轨道计划装置
文献发布时间：2024-04-18 20:01:30
技术领域
本公开涉及一种轨道计划装置，特别涉及一种计划用于实现车辆等的自动驾驶的动作的轨道计划装置。
背景技术
近年来，汽车的自动驾驶以及搬运台车等的自主移动系统的开发正在发展。在自主移动系统中，生成由移动体应行驶的轨迹和速度构成的轨道，控制成使移动体沿着所生成的轨道行驶。对于轨道计划，在很多场景中制定沿着道路的中央和磁标记等感应体那样的轨道计划。但是，根据情况的不同，在没有道路的白线的收费站附近、未铺设路面上的行驶场景以及不使用感应体的自主搬运台车向目的地移动的场景中，不能使用这样的信息。在这样的场景中，需要能够在没有应行驶的标记信息的空间上避开障碍物的同时到达目的地的轨道，例如，如专利文献1所公开的那样，开发了即使没有应行驶的标记信息也实现轨道计划的技术。
现有技术文献
专利文献
专利文献1：日本特开2012-145998号公报
发明内容
发明要解决的问题
在专利文献1中，采用了如下方法：基于由通过多个圆的中心点的线构成的沿路方向来生成行驶路径，其中，所述多个圆内切于不存在障碍物的可行驶区域。在该情况下，例如在如机场那样的非常宽广的可行驶区域中，无法决定内切的圆，因此，不能生成行驶路径而不能到达目的地。另外，在如收费站附近那样的形状复杂且横向宽度的变化大的可行驶区域中，不能运算正确的沿路方向而不能到达目的地。
本公开是为了解决上述那样的问题而完成的，其目的在于提供一种即使在如机场和收费站附近等那样可行驶区域复杂的情况下也能够通过可行驶区域到达目的地的轨道计划装置。
用于解决问题的方案
本公开的轨道计划装置是计划移动体的轨道的轨道计划装置，其中，具备：可行驶区域运算部，其基于所述移动体的周边信息，运算所述移动体的可行驶区域；目标状态运算部，其运算至少包括所述移动体的目标位置的目标状态量；状态预测部，其通过至少预测所述移动体的当前的状态量以及所述移动体的当前位置与所述目标位置之间的一个以上的位置处的所述移动体的状态量，生成一个以上的轨道候选；预测状态评价部，其基于所述目标状态量和所述可行驶区域，评价所述一个以上的轨道候选并输出评价结果；以及轨道生成部，其基于所述评价结果，从所述一个以上的轨道候选生成所述轨道，向基于所述轨道来控制所述移动体的运动控制部输出所述轨道。
发明的效果
根据本公开的轨道计划装置，基于包括移动体的目标位置的目标状态量和可行驶区域来评价直到目标状态量为止的轨道，并基于评价结果来生成轨道，因此，即使在可行驶区域复杂的情况下，也能够通过可行驶区域到达目的地。
附图说明
图1是表示搭载了实施方式1的轨道计划装置的移动体的概略结构的一例的框图。
图2是表示实施方式1中的可行驶区域的一例的图。
图3是表示实施方式1中的目标状态量的一例的图。
图4是表示在实施方式1的轨道计划装置中轨道点运算部所运算的轨道点的一例的图。
图5是表示在实施方式1的轨道计划装置中轨道生成部所生成的轨道的一例的图。
图6是说明实施方式1的轨道计划装置的动作的流程图。
图7是表示在实施方式1的轨道计划装置中从信息获取部获取的信息的一例的图。
图8是表示在实施方式1的轨道计划装置中将从信息获取部获取的信息转换成移动体坐标系的一例的图。
图9是表示实施方式1的轨道计划装置中的可行驶区域的预测的一例的图。
图10是表示实施方式1的轨道计划装置中的可行驶区域的预测的另一例的图。
图11是表示实施方式1的轨道计划装置中的可行驶区域的一例的图。
图12是表示实施方式1的轨道计划装置中的可行驶区域的一例的图。
图13是表示实施方式1的轨道计划装置中的可行驶区域的一例的图。
图14是表示实施方式1的轨道计划装置中的目标状态量的一例的图。
图15是表示实施方式1的轨道计划装置中的重新设定的目标状态量的一例的图。
图16是表示实施方式1的轨道计划装置中的设定目标状态量中的速度的上限值的一例的图。
图17是表示实施方式1的轨道计划装置中的设定目标状态量中的速度的上限值的一例的图。
图18是表示在实施方式1的轨道计划装置中通过粒子滤波预测的状态量的一例的图。
图19是表示在实施方式1的轨道计划装置中根据可行驶区域的形状而设定的输入值的一例的图。
图20是表示在实施方式1的轨道计划装置中根据可行驶区域的形状而设定的输入值的一例的图。
图21是表示在实施方式1的轨道计划装置中移动体的当前的状态量与目标状态量的偏离大的情况下的输入值的设定的一例的图。
图22是表示实施方式1的轨道计划装置中的观测变量的一例的图。
图23是表示实施方式1的轨道计划装置中的可行驶区域外的粒子的加权的一例的图。
图24是表示实施方式1的轨道计划装置中的可行驶区域内的粒子的加权的一例的图。
图25是表示实施方式1的轨道计划装置中的可行驶区域内的粒子的加权的一例的图。
图26是表示实施方式1的轨道计划装置中的可行驶区域内的粒子的加权的一例的图。
图27是表示实施方式1中的预测的可行驶区域内的粒子的加权的一例的图。
图28是表示实施方式1的轨道计划装置中的评价的权重的设定的一例的图。
图29是表示在实施方式1的轨道计划装置中得到到达目标状态量的多个轨道点的处理的一例的图。
图30是表示在实施方式1的轨道计划装置中到达目标状态量为止的轨道生成的一例的图。
图31是表示在实施方式1的轨道计划装置中到达目标状态量为止的轨道生成的一例的图。
图32是表示搭载了实施方式2的轨道计划装置的移动体的概略结构的一例的框图。
图33是说明在实施方式2的轨道计划装置中将移动体与目标位置连结的多项式的导出方法的图。
图34是说明在实施方式2的轨道计划装置中将移动体与目标位置连结的多项式的导出方法的图。
图35是说明在实施方式2的轨道计划装置中将移动体与目标位置连结的多项式的导出方法的图。
图36是说明预测状态量与当前的移动体的状态量大幅偏离的情况下的加权的图。
图37是说明预测状态量与上次运算出的轨道点的状态量大幅偏离的情况下的加权的图。
图38是表示实现实施方式1和2的轨道计划装置的硬件结构的图。
图39是表示实现实施方式1和2的轨道计划装置的硬件结构的图。
具体实施方式
<实施方式1>
图1是表示搭载了实施方式1的轨道计划装置的移动体1的概略结构的一例的框图。移动体1具备：轨道计划装置200，其基于移动体1应到达的目的地信息、移动体1的周边环境信息、以及从获取移动体1的自身状态的信息获取部100得到的信息，生成移动体1应通过的轨道；以及运动控制部300，其基于由轨道计划装置200生成的轨道，控制移动体1的运动。
信息获取部100具有目标值获取部110、自身状态获取部120以及周边环境获取部130。
目标值获取部110获取移动体应到达的目标位置、目标速度、目标方位角等信息。目标值获取部110从例如从管制接收到的基础设施信息、用户预先指定的信息、移动体所具有的地图信息的规定的位置等中获取信息。在此，与高精地图不同，移动体所具有的地图信息是指汽车导航地图、通过SLAM(Simultaneous Localization and Mapping：同步定位与地图构建)等生成的点云地图等。
目标位置例如可列举出收费站处的门的入口或横杆的位置、高速公路上的退避位置、对于牵引车的飞机的前轮部、用户指定的移动体1的位置等。目标速度例如可列举出法定速度、用户预先设定的指定速度等。目标方位角是通过目标位置时的目标的角度，例如可列举出通过门时的相对于门的垂直方向的朝向等。
自身状态获取部120获取移动体自身的当前的状态。自身状态获取部120例如可列举出速度传感器、加速度传感器、惯性测量装置、转向角传感器、转向转矩传感器、横摆角速度传感器(yaw rate sensor)以及全球卫星定位系统(GNSS：Global NavigationSatellite System)传感器等。在此，以下将惯性测量装置称为IMU(Inertial MeasurementUnit：惯性测量单元)传感器。
周边环境获取部130获取移动体周边的墙壁、移动障碍物的位置及速度、方位角、无障碍物的能够行驶的空间信息。周边环境获取部130例如可列举出毫米波雷达、相机、LiDAR(Light Detection and Ranging：光探测与测距)、声纳、车辆间通信装置以及道路车辆间通信装置等。
轨道计划装置200具有可行驶区域运算部210、目标状态运算部220、状态预测部230、预测状态评价部240、轨道点运算部250以及轨道生成部260。
可行驶区域运算部210基于从所述周边环境获取部130获取的移动体1的周边信息，运算不存在障碍物的、移动体1能够行驶的可行驶区域。图2表示可行驶区域的一例。在图2中，在移动体1的行进方向左侧存在静止障碍物SOB，移动障碍物MOB要从行进方向右侧进入由左右的车道边界LB规定的行驶车道，用粗线示出了不存在静止障碍物SOB以及移动障碍物MOB的可行驶区域TA。如图2所示，可行驶区域TA不一定限定于由道路的白线等车道边界LB规定的行驶车道。
目标状态运算部220基于来自目标值获取部110的信息，运算移动体1应到达的目的地处的目标状态量。该目标状态量至少包括移动体1的目标位置。图3表示目标状态量的一例。
图3表示如ETC门的近前那样的没有白线的道路，在此的车道边界LB不是白线而是墙壁或护栏等。在图3中，目标状态量TG包括在时刻t时的目标位置的坐标(x
状态预测部230通过至少预测移动体1的当前的状态量以及移动体1的当前位置与目标位置之间的一个以上的位置处的移动体1的状态量，生成一个以上的轨道候选。为此，例如通过使用移动体的运动模型的状态估计运算，将规定的多个输入输入到所述移动体的运动模型，预测与多个输入相应的往前至少一步、即往前控制周期中的一个采样时间的状态量，由此生成一个以上的轨道候选。在本实施方式中，作为状态估计方法的一例，使用粒子滤波(particle filter)。
粒子滤波是指利用概率密度分布的时间序列数据的预测方法，有时也被称为序贯蒙特卡罗法。另外，作为状态估计运算的粒子滤波是通过多个粒子来对状态的概率密度分布进行近似，例如，若具有某状态量的粒子较多，则通过使用粒子滤波作为其状态估计运算，能够估计整体的概率密度分布，因此，能够减小输出局部最优解的频率。
预测状态评价部240通过对各预测状态量即各粒子进行加权而得到加权后的轨道候选，基于权重来评价加权后的轨道候选，并输出评价结果。此时，基于在目标状态运算部220中运算出的目标状态量和在可行驶区域运算部210中运算出的可行驶区域进行加权。对于该权重，在后述的轨道点运算部250中运算轨道点时，能够基于各预测状态量的权重值而对在状态预测部230中预测出的多个状态量进行加权平均，由此运算合理的状态量。
例如，若将可行驶区域外的状态量的加权系数设为0，则在加权平均的运算时，将该状态量乘以加权系数0，能够防止成为可行驶区域外的轨道点，能够生成保证是可行驶区域TA内的轨道的轨道。
轨道点运算部250根据轨道候选运算轨道点。具体而言，轨道点运算部250进行如下运算：根据由预测状态评价部240赋予的权重对由状态预测部230预测出的预测状态量进行加权平均，并将加权平均后的状态量设为轨道点。图4表示该运算的概念图。
如图4所示，在可行驶区域TA及其附近存在加权系数为0的状态量的粒子组G0、加权系数低的状态量的粒子组GL以及加权系数高的状态量的粒子组GH，将加权系数高的状态量的粒子组GH的加权平均后的状态量设为轨道点TP。另外，也能够将由预测状态评价部240赋予的权重最高的状态量设为轨道点。
轨道生成部260基于评价结果而从轨道候选生成轨道，并向基于所生成的轨道来控制移动体1的运动控制部300输出轨道。具体而言，轨道生成部260将由轨道点运算部250运算出的按各个离散时间的轨道点构成的点列作为生成轨道向运动控制部300输出。图5表示生成轨道的概念图。
如图5所示，在可行驶区域TA示出了离散时间t
运动控制部300具有控制量运算部310和致动器控制部320。
控制量运算部310将由轨道生成部260生成的轨道作为目标轨道，运算对用于沿着目标轨道行驶的移动体1的目标控制值，并输出到致动器控制部320。
致动器控制部320是搭载于移动体1的控制器，使致动器工作，以使移动体追随由控制量运算部310运算出的目标控制值。作为致动器，例如可列举出转向装置、驱动用马达以及制动器。
下面，使用图6所示的流程图说明实施方式1的轨道计划装置200的动作的一例。以下，对使用粒子滤波的情况进行说明。另外，以下“一步”是指控制周期中的一个采样时间。
首先，作为轨道计划装置200的输入信息，从目标值获取部110获取目标位置、目标速度、目标方位角等目标值，从自身状态获取部120获取移动体的位置、速度、方位角等自身状态，从周边环境获取部130获取可行驶区域的各顶点坐标、移动障碍物的位置、速度等周边环境信息(步骤S101)。图7表示此时的输入信息的概念图。
在图7中，对于规定可行驶区域TA的多个顶点VTA的坐标，x坐标用x
目标状态量TG包括目标位置的坐标(x
另外，对于各移动障碍物MOB的坐标，x坐标用x
在图7中，由于障碍物的存在而缺少可行驶区域TA的一部分，沿着障碍物的轮廓示出了多个顶点VTA。
此外，在本实施方式中，提取可行驶区域TA的各顶点作为可行驶区域TA的信息，但也能够使用圆或椭圆之类的线的信息。
另外，通过使用从自身状态获取部120得到的自身状态，如图8所示，也能够使用变更成以移动体1的位置为原点、以移动体1的朝向为x轴、以与移动体的朝向垂直的方向为y轴的移动体坐标系的值。以下，使用该移动体坐标系的值。
如图8所示，在移动体坐标系中，对于多个顶点VTA的坐标，X坐标用X
目标状态量TG包括目标位置的坐标(X
另外，对于各移动障碍物MOB的坐标，X坐标用X
在与目标值信息相关的各值的坐标变换中，使用以下的数学式(1)。
[数学式1]
在与移动障碍物信息相关的各值的坐标变换中，使用以下的数学式(2)。
[数学式2]
在与可行驶区域TA的各顶点相关的各值的坐标变换中，使用以下的数学式(3)。
[数学式3]
在此，返回图6的流程图的说明。在步骤S101中获取了周边环境信息之后，在可行驶区域运算部210中，基于从周边环境获取部130获取的信息，运算不存在障碍物而移动体1能够行驶的可行驶区域TA(步骤S102)。在本实施方式中，将图8所示的移动体坐标系中的与可行驶区域TA的各顶点VTA相关的X坐标X
此外，代替将与可行驶区域TA的各顶点VTA相关的X坐标X
图9是表示基于可行驶区域TA的形状的时间序列变化来预测可行驶区域的方法的概念图。在图9中，作为时刻T而将当前的时刻设为t，将该时刻的控制周期中的一个采样时间前的时刻设为t-1，将再一个采样时间前的时刻设为t-2，将时刻t的一个采样时间后的时刻设为t+1。
在图9中，移动体1沿箭头方向前进，可行驶区域TA由于前方的障碍物的存在而局部缺少。根据时刻t-2的过去的可行驶区域TA、时刻t-1的过去的可行驶区域TA以及时刻t的当前的可行驶区域TA的经时变化，可知存在有在近前方向上变化了与移动体1的前进相应的量的部分NP1以及即使本车辆前进也不变化或变化少的部分NP2。部分NP1为静止障碍物的可能性高，部分NP2为移动障碍物的可能性高。根据这样的信息，当预测时刻t+1的未来的可行驶区域时，图9的右端的图中的带阴影的区域成为预测可行驶区域ETA，通过将时刻t的当前的可行驶区域TA与预测可行驶区域ETA合并，能够延长当前的可行驶区域TA。
通过使用预测可行驶区域ETA，能够将轨道生成至比当前能够识别的由外界传感器得到的可行驶区域远的区域。该区域有望在未来成为可行驶区域。
另外，也能够基于可行驶区域TA外的障碍物的种类来运算预测可行驶区域，并将其用于轨道生成。
图10是表示基于障碍物的种类来预测可行驶区域的方法的概念图。此外，障碍物的种类例如可列举出墙壁、停止的其他车辆以及移动的其他车辆等。在障碍物是移动的其他车辆的情况下，周边环境获取部130不仅获取移动的其他车辆的位置，还获取移动的其他车辆的速度等。而且，可行驶区域运算部210基于移动的其他车辆的位置及速度等来运算预测可行驶区域。因此，预测可行驶区域也包括未进入到当前运算出的可行驶区域中的区域。这意味着，即使是当前被判定为不能行驶的区域，该区域也在将来被判定为能够行驶。
在图10中，将速度v
在图10的左图中，可行驶区域TA的最大识别距离为L
在图10的右图中，前方存在静止障碍物SOB的地点是未来也不能行驶的区域R1，对于前方存在移动障碍物MOB的地点，假定在未来可行驶区域在移动障碍物MOB的行进方向上延伸，延长可行驶区域TA而设为延长区域R2。延长的长度为(v
通过使用预测可行驶区域TAX，能够将轨道生成至比当前能够识别的由外界传感器得到的可行驶区域远的位置。由于基于实际的障碍物的运动来预测可行驶区域，因此可靠度提高。另外，能够不仅考虑实际的障碍物的运动，还如图9所示考虑可行驶区域TA的形状的时间序列变化来预测可行驶区域TA。由此，可靠度进一步提高。
另外，在步骤S102的可行驶区域TA的运算中，在存在即使能够行驶也可能成为死路的区域的情况下，根据需要进行从可行驶区域TA去除该区域的处理。图11是说明该处理的概念图。
在图11中，示出了在移动体1的前方存在ETC门等狭窄的通路的场景，目标位置TGP位于能够通过的门内。其他门不能行驶，成为将来不能行驶的死路DE，作为去除区域AR而从可行驶区域TA中去除。
对于可能成为死路的区域，例如，虽然x轴方向的值是与目标位置TGP大致相同的值、但y轴方向的值存在偏离的地方设为死路。或者，在得到如图11所示的俯视图、航空照片等的情况下，通过图像处理技术检测在与目标位置TGP之间存在墙壁的区域、目标位置TGP被墙壁包围的区域等。在利用基础设施信息得到正在施工中、不能使用的ETC门等的位置信息的情况下，将不能通行的区域与可行驶区域TA进行对照，将可行驶区域TA中的移动体1将来不能行驶的区域设为可能成为死路的区域。
能够生成避免即使在可行驶区域TA内也在到达目标位置TGP之前被死路阻挡而无法活动的轨道。
另外，在步骤S102的可行驶区域TA的运算中，在存在虽然能够行驶但禁止进入的区域的情况下，根据需要进行从可行驶区域TA中去除该区域的处理。图12是说明该处理的概念图。
在图12中，示出了在移动体1的前方设置有禁止进入区IPA的场景。作为禁止进入区IPA，可列举出正在施工中等的区域中未被围栏等障碍物包围的区域。将可行驶区域TA与禁止进入区IPA重叠的区域作为去除区域AR而从可行驶区域TA中去除。
对于禁止进入区IPA，在得到如图12那样的俯视图、航空照片等的情况下，通过图像处理技术检测正在施工中等的区域等，使用安装在移动体1上的前方相机，通过图像处理技术检测正在施工中之类的无法进入的区域。在利用基础设施信息得到正在施工等的位置信息的情况下，将正在施工中的区域等与可行驶区域进行对照，将可行驶区域TA中的正在施工中等的区域设为禁止进入区IPA。
另外，在步骤S102的可行驶区域TA的运算中，在存在禁止越线行驶的越线禁止线的情况下，进行仅将可行驶区域TA与越线禁止线重叠的区域设为能够行驶的处理。图13是说明该处理的概念图。
在图13中，示出了在移动体1的行进方向的左右设置有越线禁止线NSL的场景。作为越线禁止线NSL，可列举出设置在路面上的禁止超出的白色实线、禁止为了超车而超出的黄色实线。另外，还包括在交叉路口附近30m内不能变更车道、在ETC门正前方不能变更车道等规则。而且，由可行驶区域TA和越线禁止线NSL包围的区域作为去除区域AR而从可行驶区域TA中去除，仅可行驶区域TA和越线禁止线重叠的区域能够行驶。
如使用图11～图13说明的那样，可行驶区域运算部210基于移动体1的周边环境信息，预测移动体1将来不能行驶的区域，将除了该区域之外的区域运算为可行驶区域TA。此外，将来不能行驶的区域不限定于如图11～图13所示的区域。
在此，返回图6的流程图的说明。在步骤S102中运算了可行驶区域TA之后，在目标状态运算部220中，基于由目标值获取部110获取的目标值，运算移动体1应到达的目标状态量(步骤S103)。图14表示本实施方式中的目标状态量的概要图。
如图14所示，除了使用目标位置(X
[数学式4]
P
通过使目标状态量包括目标速度V
通过使目标状态量包括目标方位角Θ
通过使目标状态量包括目标横向位置Y
图14所示的危险区域D定义为：与动态的前车及静止障碍物之间应确保的安全的车间距离；以及在移动体1正在运动时由于在移动体1的附近有人和其他车辆而碰撞的可能性高，另外，若发生碰撞则损害很大且危险，因此人和其他车辆不能接近的距离。在图14中，在危险区域D内存在移动障碍物MOB，与移动障碍物MOB之间的距离D
另外，目标状态量P
[数学式5]
P
另外，目标状态量P
另外，目标状态运算部220在目标状态量处于可行驶区域TA外的情况下，也能够进行处理以将最接近目标状态量的可行驶区域TA内的状态量设定为目标状态量。图15是说明该处理的概念图。
在图15中，示出了作为目标状态量而使用目标位置(X
通过进行这样的处理，与目标位置TGP处于可行驶区域TA外的情况相比，生成轨道容易设定在可行驶区域TA内，因此能够实现安全的行驶。
另外，目标状态运算部220也能够根据在可行驶区域运算部210中运算出的可行驶区域TA的形状，对目标状态量中的至少与速度相关的目标状态量设定上限值。例如，可列举出在无法高速行驶那样的可行驶区域TA狭窄的情况和可行驶区域TA的识别距离短的情况下，对与速度相关的目标状态量设定上限值。
图16是表示可行驶区域TA狭窄的场景的概念图。如图16所示，沿着移动体1的行进方向，在移动体1的左右的近距离存在静止障碍物SOB。可行驶区域TA的最大识别距离为L
在这样的情况下，通过对与目标位置或目标方位角相关的目标状态量设定上限值，生成轨道容易设定在可行驶区域TA内，因此能够实现不给移动体1的乘员带来精神负担的行驶。
图17是表示可行驶区域TA的识别距离短的场景的概念图。如图17所示，移动体1的行进方向前方的可行驶区域TA的识别距离L
这样的场景设想为移动体1是牵引车且在前方有牵引对象的飞机那样的情况、在前方的较近的位置有墙壁那样的情况、安装在移动体1上的相机传感器的前方的检测距离短的情况。
在这样的情况下，通过对与速度相关的目标状态量设定上限值，生成轨道容易设定在可行驶区域TA内，因此能够实现不给移动体1的乘员带来精神负担的行驶。
在此，返回图6的流程图的说明。在步骤S103中运算了目标状态量之后，在状态预测部230中，基于移动体的当前的状态来定义N
[数学式6]
P＝[X
其中，二维位置X
[数学式7]
W＝1/Np…(7)
此外，粒子数也能够根据可行驶区域TA的形状、面积而可变，还能够根据与目标状态量的偏离度而可变。
在步骤S104中定义了粒子之后，在状态预测部230中，对各粒子赋予与使用了均匀随机数的随机的粒子数相应的输入，由此预测离散时间宽度Td后的状态量(步骤S105)。以下，说明粒子的状态预测的方法。
粒子的状态量预测使用系统模型来进行，以下说明在本实施方式中使用的模型。系统模型的状态变量为粒子的二维位置X
[数学式8]
P
另外，向系统模型的输入值P
[数学式9]
P
另外，移动体1的侧滑角β用以下的数学式(10)来表示。
[数学式10]
β＝tan
在此，系统模型使用移动体1的轴距L，作为微分方程用以下的数学式(11)来表示。
[数学式11]
此外，上述的系统模型可以说是将四轮近似于两轮且不考虑力学的运动学模型，但也能够使用将四轮近似于两轮的动力学模型即两轮模型等其他车辆运动模型。
对于向系统模型的输入变量中的加速度a，对于预先设定的任意的上限值a
[数学式12]
a
另外，关于向系统模型的输入变量中的转向角速度u，对于预先设定的上限值u
[数学式13]
u
另外，对于与转向角相关的上限值δ
[数学式14]
δ
离散时间宽度Td后的转向角δ’用以下的数学式(15)来表示。
[数学式15]
δ’＝δ+u·Td…(15)
因此，第二约束条件用以下的数学式(16)来表示。
[数学式16]
(δ
对于向系统模型的输入值P
如上所述，使用基于作为制约的转向角上限值δ
粒子的状态量使用预测状态量P
[数学式17]
P＝[P
图18表示将时刻T
此外，在状态预测部230中的运算中，为了赋予随机的输入而使用按照均匀分布的均匀随机数来决定输入，但也可以使用按照正态分布的正态随机数或按照其他分布的随机的输入。
另外，在状态预测部230中的运算中，采用不会超过预先设定的任意的上限值的输入，但该输入也能够设为根据可行驶区域TA的形状而可变的值。
图19是说明与可行驶区域TA狭长的形状相匹配地设定输入的情况的概念图。如图19所示，若可行驶区域TA为纵长的形状，则设定使预测状态量Px’不会处于图示那样的可行驶区域TA外的转向角速度u的输入的上限值，或者使转向角速度u的输入的分布变窄。通过进行这样的处理，容易在可行驶区域TA内生成预测状态量。
另外，在可行驶区域TA为狭长的形状的情况下，通过减少输入的数量，能够减少运算量。
图20是说明与可行驶区域TA横长的形状相匹配地设定输入的情况的概念图。如图20所示，若可行驶区域TA为横长的形状，则设定使预测状态量P
另外，在可行驶区域TA为横长的形状的情况下，通过增多输入的数量，容易得到更合适的轨道。
此外，状态预测部230也能够在移动体1的当前的状态量与目标状态量的偏离大的情况下，增多向移动体1的运动模型的输入值的数量，在偏离度小的情况下，减少输入值的数量。图21是说明当前的状态量与目标状态量的偏离大的情况下的输入值的设定的概念图。
如图21所示，在方位角的目标状态量Θ
在该情况下，将转向角的分布取得较宽。即，扩宽转向角的上下限值的范围。另外，通过增多输入值的数量，容易得到更合适的轨道。由此，即使在移动体1的当前的状态量与目标状态量的偏离大的情况下，也容易到达目标位置。另一方面，在偏离小的情况下，通过减少输入值的数量，运算量变少，能够降低运算负荷。这样，通过根据当前的状态量与目标状态量的偏离的程度来改变输入值的数量，能够实现移动体1的平滑的轨道生成和运算负荷的降低。
在此，返回图6的流程图的说明。在步骤S105中预测了各粒子的状态量之后，在预测状态评价部240中，根据各粒子的更新后的状态量求出观测值(步骤S106)。观测变量基于由目标状态运算部220运算出的目标状态量来定义。在本实施方式中，以到达目标横向位置、维持目标速度、到达目标方位角以及保持与移动障碍物的安全距离为目标，其中，该目标横向位置是与目标位置处的目标方位角方向垂直的方向上的位置。基于这些目标，将粒子与目标横向位置Y
[数学式18]
P
图22是表示各观测变量的图。在图22中，示意性地示出了相对于当前的移动体1具有时刻T
在此，危险区域D是使长边的朝向在粒子P
[数学式19]
D
另外，距离D
[数学式20]
D
在步骤S106中求出了各粒子的观测值之后，在预测状态评价部240中，根据各粒子的观测值P
[数学式21]
P
另外，在粒子的位置在步骤S102中运算出的可行驶区域TA外的情况下，将各粒子的权重设为0，或者设为比处于可行驶区域TA内的粒子低的值。可行驶区域TA的内外判定例如通过在将可行驶区域TA的各顶点与移动体1连结的多边形区域内是否存在粒子的二维位置X
在此，在上述中，可行驶区域TA外的粒子的权重设为0，但对于可行驶区域TA外的粒子，也能够根据从可行驶区域TA脱离的脱离度而将赋予给粒子的权重设为可变。
图23是将赋予给可行驶区域TA外的粒子的权重设为可变的处理的概念图。在图23中，处于移动体1的前方的可行驶区域TA内的粒子PW
另外，在赋予权重W
这样，通过不删除可行驶区域TA的外侧的粒子(预测点)而根据距可行驶区域TA的距离来进行预测状态量的评价，从而对于由于外界传感器的可识别范围的界限等而即使实际上是可行驶区域TA但在外界传感器上被识别为可行驶区域TA外的区域，也能够作为轨道候选保留，对于保留的轨道候选，能够进行与距可行驶区域TA的距离即可靠度相应的预测点的评价，轨道计划容易成功。另外，通过使得越远离可行驶区域TA的粒子则所赋予的权重越轻，能够降低基于该粒子生成轨道的可能性，能够提高所生成的轨道的安全性。
另外，在图23中赋予给粒子的权重为不连续的值，但也能够设为根据与可行驶区域TA的距离而赋予的权重连续变化的值。
另外，对于可行驶区域TA内的粒子，也能够根据距规定可行驶区域TA的边界的距离而将赋予给粒子的权重设为可变。
图24是将赋予给可行驶区域TA内的粒子的权重设为可变的处理的概念图。在图24中，在移动体1的前方的可行驶区域TA内，从规定可行驶区域TA的边界侧朝向内侧依次设定有赋予权重W
这样，在可行驶区域TA内的预测状态量中，以随着接近规定可行驶区域TA的边界而减小的方式赋予权重，从而能够实现使得位于可行驶区域TA的中央的预测状态量的权重比位于可行驶区域TA的边界附近的预测状态量的权重高、尽量避开可行驶区域TA的边界的轨道计划，能够提高所生成的轨道的安全性。
另外，在图24中赋予给粒子的权重为不连续的值，但也能够设为根据距规定可行驶区域TA的边界的距离而赋予的权重连续变化的值。
另外，对于可行驶区域TA内的粒子，也能够进行处理，以便对于预测为即使在可行驶区域TA内但若行进则可能成为死路的地点的粒子，使权重为0或赋予较小的权重。图25是说明该处理的概念图。
在图25中，示出了在移动体1的前方存在ETC门等狭窄的通路的场景，目标位置TGP位于能够通过的门内。其他的门不能行驶，成为死路DE，成为若行进则移动体1将来不能行驶的区域。在该区域中预测的粒子PW的权重为0(W＝0)。
这样，能够生成避免即使在可行驶区域TA内也在到达目标位置TGP之前被死路阻挡而无法活动的轨道。
此外，对于是否可能成为死路的判定，例如，若是与图25中的目标位置TGP在x方向上的偏差小且在y方向上的偏差大的粒子、即相对于目标位置TGP在水平方向(y方向)上偏移的粒子，则会被目标位置的近前的障碍物阻挡。因此，能够根据是否与目标位置TGP在x方向上的偏差小且在y方向上的偏差大来判定是否为被预测为可能成为死路的地点的粒子。
另外，如使用图11说明的那样，也能够预先从可行驶区域TA中排除死路的部位，将被预测为可能成为死路的地点的粒子作为处于可行驶区域TA外的粒子来赋予权重W。
另外，对于可行驶区域内的粒子，在可行驶区域内存在越线禁止线的情况下，在前一步由轨道点运算部250生成的轨道点或相对于移动体1而跨越越线禁止线的位置存在步骤S105中运算出的粒子的情况下，也能够将该粒子的权重W设为0，或者赋予较小的权重。图26是说明该处理的概念图。
在图26中，示出了在移动体1的行进方向的左右设置有越线禁止线NSL的场景，在由左右的越线禁止线NSL规定的行驶车道内设定有由多个轨道点TP构成的生成轨道GT，但当前在预测状态评价部240中正在评价中的一些粒子成为被预测为相对于前一步由轨道点运算部250生成的轨道点TP跨越越线禁止线NSL的位置的粒子PW，权重为0(W＝0)。
这样，通过将被预测为相对于轨道点TP跨越越线禁止线NSL的位置的粒子的权重设为0，能够防止轨道被设定在移动体1将来不能行驶的区域，能够提高所生成的轨道的安全性。
另外，如使用图13说明的那样，也能够将由可行驶区域TA和越线禁止线NSL包围的区域作为去除区域AR而从可行驶区域TA中排除，并将在该地点预测出的粒子作为处于可行驶区域TA外的粒子来赋予权重W。
另外，对于当前能够识别的可行驶区域之外的粒子，在存在预测出的可行驶区域的情况下，若该粒子位于预测可行驶区域中，则也能够进行处理以使该粒子的权重W不为0，也能够赋予比当前能够识别的可行驶区域内的粒子的权重小的权重。图27是说明处于预测可行驶区域内的粒子的加权处理的概念图。
在图27中，在当前能够识别的可行驶区域TA的前方设置有预测可行驶区域ETA，对处于预测可行驶区域ETA内的粒子PW
通过使用预测可行驶区域ETA，能够将轨道生成至比当前能够识别的由外界传感器得到的可行驶区域TA远的位置。另外，由于预测可行驶区域ETA的可靠度不高，因此，通过相对地增大当前能够识别的由外界传感器得到的可行驶区域TA内的预测状态量的权重，能够生成可靠度高的轨道。
在此，对于在步骤S105中预测了离散时间宽度Td后的状态量的粒子，使用从周边环境获取部130得到的移动障碍物的预测轨道，判断在各粒子周边的区域是否存在通过在相同时刻即在与粒子的预测时间相同的时间使用预测轨道而得到的预测障碍物，在存在的情况下也能够进行将该粒子的权重W设为0的处理。
将各粒子的更新前的权重W重新定义为W
[数学式22]
在此，似然度LLV使用与预先设定的粒子的状态量P
[数学式23]
在此，矩阵Π用以下的数学式(24)来表示。
[数学式24]
Π＝HQH
其中，在设为P
[数学式25]
P
另外，测定函数h是根据状态量P
[数学式26]
对于基于各粒子的观测值与理想观测值之差的各粒子的权重更新，也能够基于移动体1与目标状态量之差或应用场景而设定以何种程度重点评价哪个项目之类的评价的权重。图28是说明评价的权重的设定处理的概念图。
在图28中，移动体1处于远离目标横向位置Y
通过这样的设定，例如，在通过收费站门的场景中，在目标位置是想要通过的门时，在需要与门垂直地通过的场景中，通过相比于与同纵向的位置相关的目标状态量之差而重视与同横向的位置相关的目标状态量之差，能够在纵向的位置对准之前先使横向的位置对准，能够生成可提前到达能够与门垂直地进入的位置的轨道。
另外，在图28中，处于移动体1的方位角Θ
通过这样的设定，例如，在通过收费站门的场景中，在目标位置是想要通过的门时，在距目标位置的距离较远的地点降低与方位角相关的状态量的评价的权重，在距离较近的地点增大与方位角相关的状态量的评价的权重，由此能够生成在远离目标位置的地点与轨道的方位角相关的状态量的自由度高、随着接近目标位置而使与方位角相关的状态量趋于一致的轨道。
在此，返回图6的流程图的说明。在步骤S107中更新了各粒子的权重之后，在预测状态评价部240中，基于各粒子的权重来进行粒子的重采样(步骤S108)。但是，为了防止粒子数的大幅减少，仅在有效粒子数Neff为阈值Nthr以上的情况下进行重采样，在除此以外的情况下，在该步骤中不进行任何处理。
在此，有效粒子数Neff用以下的数学式(27)来表示。
[数学式27]
在重采样中，与通常的粒子滤波同样，根据经验分布函数而等间隔地进行采样。在进行了重采样的情况下，基于以下的数学式(28)，各粒子的权重设为相等并进行权重的重置。
[数学式28]
W＝1/Np…(28)
在步骤S108中进行了粒子的重采样之后，在轨道点运算部250中，对于粒子的位置和速度，基于在预测状态评价部240中运算出的权重，在轨道生成部260中运算加权平均值，将进行了该加权平均后的至少由位置数据和速度数据构成的点设为轨道点，存储在例如轨道生成部260内的存储器(未图示)中(步骤S109)。
另外，关于轨道点，也能够基于由预测状态评价部240运算出的权重，将权重最大即加权系数最大的粒子设为轨道点。
另外，轨道点不限于位置数据和速度数据，也能够包含方位角数据、转向角数据等而构成。
另外，轨道点也能够将仅位置数据的点设为轨道点。
在步骤S109中存储了轨道点之后，判断时刻T是否达到计划周期(horizon)Tthr(步骤S110)。在时刻小于计划周期的情况下(否的情况)，反复进行步骤SS104以下的处理。在时刻T为计划周期Tthr以上的情况下(是的情况)，将轨道生成部260作为轨道点存储的由按各离散时间的位置数据和速度数据构成的轨道点的点列作为生成轨道输出到运动控制部300。
图29表示反复进行从步骤S104到步骤S110的运算直到时刻达到计划周期Tthr以上而得到多个轨道点的处理的概念图。在图29中，将粒子数设为四个，另外，图中的粒子示出了在重采样之后的状态。
如图29所示，在可行驶区域TA内，存在时刻T＝T
另外，通过将粒子组G1～G3各自的权重最大、即加权系数最大的粒子设为轨道点，也能够得到与上述同样的效果。
使用图30和图31说明以上说明的使用本实施方式的轨道计划装置200到达目标状态量为止的轨道生成的一例。图30和图31是表示轨道生成的一例的示意图，在图30的左图中示意性地示出了移动体1存在于距目标状态量TG远的位置时的生成轨道GT1。在图30的右图中示出了使用左图的生成轨道GT1，沿着生成轨道GT1向前方前进，在比左图时稍微前进的位置处的生成轨道GT2。在图31的左图中示出了使用图30的右图的生成轨道GT1，沿着生成轨道GT1向前方前进，在比图30的右图时稍微前进的位置处的生成轨道GT3。在图31的右图中示出了使用左图的生成轨道GT3，沿着生成轨道GT3向前方前进，到达目标状态量TG后的生成轨道GT4。
如以上说明的那样，根据实施方式1的轨道计划装置200，基于包括移动体1的位置的状态量的目标状态量和可行驶区域来评价直到目标状态量为止的轨道，基于评价结果来生成轨道，因此，即使在可行驶区域复杂的情况下，也能够通过可行驶区域到达目的地。
＜实施方式2＞
图32是表示搭载了实施方式2的轨道计划装置的移动体1的概略结构的一例的框图。此外，在图32中，对与使用图1说明的移动体1相同的结构标注相同的附图标记，省略重复的说明。
在图32所示的移动体1中，生成移动体1应通过的轨道的轨道计划装置200A的结构与实施方式1的轨道计划装置200不同。即，实施方式2的轨道计划装置200A通过由状态预测部230运算通过移动体1的当前的位置和目标位置的多项式来生成轨道，来代替在状态估计中使用粒子滤波，因此，在不具有轨道点运算部250这一点上与轨道计划装置200不同。
对于运算将移动体1与目标状态量TG连结的多项式的轨道的方法，以下，将目标状态量TG设为目标位置(x
如图33所示，在将移动体1的位置设为原点、将移动体1的朝向设为x轴、将与移动体的朝向垂直的方向设为y轴的情况下，将移动体1与目标位置TG连结的多项式y＝f(x)为以下的数学式(29)。
[数学式29]
y＝f(x)＝∑C
而且，该多项式附加如下的制约条件：通过移动体1的位置(原点)和目标位置TG，在移动体1的位置地点处方向必须朝向x轴(角度为0度)，另外在目标位置地点处必须朝向规定的方向(角度)。
若将其用数学式来表示，则通过求解以下的边界条件、即移动体位置地点和目标位置地点的条件的联立方程，能够求出各系数c
[数学式30]
f(x
[数学式31]
f’(x
下面，对将多项式设为以下的数学式(32)的情况进行说明。
[数学式32]
f(x)＝C
例如，当给出在移动体位置地点的函数值(f(x
在图34中示出了在存在多个障碍物OB的情况下，用于得到避开多个障碍物OB而连结移动体1与目标位置TG的多项式轨道的三种模式的多项式轨道PT1、PT2以及PT3。
给出多项式轨道PT1、PT2以及PT3的多项式分别用以下的数学式(33)、(34)以及(35)来表示。
[数学式33]
f(x)＝C’
[数学式34]
f(X)＝C”
[数学式35]
f(x]＝C”’
根据上述多个多项式轨道，进行向目标位置TG的到达值以及多项式轨道是否离开可行驶区域TA的边界而处于可行驶区域TA内等的评价，将评价最高的多项式轨道设为生成轨道。若以图34为例，则多项式轨道PT3成为生成轨道。
另外，对于将多项式设为上述的数学式(32)的情况，也能够通过以下的方法来得到多项式轨道。
例如，当给出在移动体位置地点的函数值(f(x
图35示出了在存在多个障碍物OB的情况下，用于得到避开多个障碍物OB而连结移动体1与目标位置TG的多项式轨道的三种模式的多项式轨道PT1、PT2以及PT3。
给出多项式轨道PT1、PT2以及PT3的多项式分别用以下的数学式(36)、(37)以及(38)来表示。
[数学式36]
f(x)＝C’
[数学式37]
f(x)＝C”
[数学式38]
f(x)＝C”’
根据上述多个多项式轨道，进行向目标位置TG的到达值以及多项式轨道是否离开可行驶区域TA的边界而处于可行驶区域TA内等的评价，将评价最高的多项式轨道设为生成轨道。若以图35为例，则多项式轨道PT1成为生成轨道。
这样，实施方式2的轨道计划装置200A通过运算通过移动体1的当前的位置和目标位置的多项式来得到生成轨道，来代替在状态估计中使用粒子滤波，因此，不运算轨道点，不具有轨道点运算部250。
在此，作为采用通过运算多项式来得到生成轨道的方法的情况下的优点，可列举出运算负荷低。即，仅通过求解上述说明的联立方程就能够运算一个轨道候选，但在使用粒子滤波的情况下，需要将运算反复进行与想要生成大量粒子的状态转移的轨道的长度相应的量，因此运算负荷大。
另一方面，作为采用使用粒子滤波来得到生成轨道的方法的情况下的优点，可列举出能够生成保证是可行驶区域TA内的轨道。即，粒子滤波是使用大量预测点(预测状态量)来求出一个轨道点的方法，但能够根据各粒子的点与可行驶区域TA的位置关系来进行轨道点是在可行驶区域TA外还是在可行驶区域TA内的评价，因此，能够生成能够保证是可行驶区域TA内的轨道。另一方面，在通过运算多项式来得到生成轨道的方法中，虽然能够生成将移动体1与目标位置TG连结的轨道，但无法将“在可行驶区域TA内”以及“避开障碍物”加入到多项式的数学式中或者无法加以考虑，因此所生成的轨道有可能处于可行驶区域TA外。
另外，作为采用使用粒子滤波来得到生成轨道的方法的情况下的优点，可列举出轨道的搜索范围大。即，在通过运算多项式来得到生成轨道的方法中，仅能够得到能够由多项式表达的轨道，因此，搜索范围小，但在使用粒子滤波的情况下，以状态的预测点为基础，即由点来表达，因此搜索范围大。另外，还能够得到非线性的轨道，也能够生成不能由多项式表达的轨道。
＜加权的变形例＞
以上说明的实施方式1的预测状态评价部240通过对各预测状态量即各粒子进行加权来得到加权后的轨道候选。此时，在预测状态量与当前的移动体1的状态量大幅偏离的情况下、或者与上次运算出的轨道点的状态量大幅偏离的情况下，若使用基于这样的预测状态量运算出的轨道点来生成轨道，则轨道会急剧变化，移动体1的乘坐舒适性变差。
因此，在预测状态评价部240中，在预测状态量与当前的移动体1的状态量大幅偏离的情况下、以及与上次运算出的轨道点的状态量大幅偏离的情况下，通过减小加权系数，能够抑制生成急剧变化的轨道，改善移动体1的乘坐舒适性。
图36表示预测状态量与当前的移动体1的状态量大幅偏离的情况下的加权的概念图。如图36所示，在存在相对于当前的移动体1的状态量(x
在此，对于当前的移动体1的状态量与预测状态量(粒子)的偏离的程度，两状态量之差能够基于预先确定的阈值来判断，若大于阈值，则能够判断为偏离大，若为阈值以下，则能够判断为偏离小。
另外，相对于粒子的加权也能够设为基于当前的移动体1的状态量与预测状态量(粒子)之差的绝对值来改变加权系数。例如，也能够基于两状态量之差的绝对值而预先设定阶段性变化的加权系数，如果两状态量之差变大，则阶段性地减小加权系数，如果两状态量之差变大，则阶段性地增大加权系数。
图37表示预测状态量与上次运算出的轨道点的状态量大幅偏离的情况下的加权的概念图。如图37所示，在存在相对于上次运算出的轨道点TP2的状态量大幅偏离的粒子组GS、以及相对于上次运算出的轨道点TP2的状态量偏离不那么大的粒子组GM的情况下，预测状态评价部240减小粒子组GS的粒子的加权系数。另一方面，预测状态评价部240不减小粒子组GM的粒子的加权系数。
在此，对于上次运算出的轨道点TP2的状态量与预测状态量(粒子)的偏离的程度，两状态量之差能够基于预先确定的阈值来判断，若大于阈值，则能够判断为偏离大，若为阈值以下，则能够判断为偏离小。
另外，相对于粒子的加权也能够设为基于上次运算出的轨道点TP2的状态量与预测状态量(粒子)之差的绝对值来改变加权系数。例如，也能够基于两状态量之差的绝对值而预先设定阶段性变化的加权系数，如果两状态量之差变大，则阶段性地减小加权系数，如果两状态量之差变大，则阶段性地增大加权系数。
此外，以上说明的实施方式1和2的轨道计划装置200以及200A的各构成要素能够使用计算机来构成，通过使计算机执行程序来实现。即，轨道计划装置200以及200A例如通过图38所示的处理电路50来实现。作为处理电路50，应用CPU、DSP(Digital SignalProcessor：数字信号处理器)等处理器，通过执行存储在存储装置中的程序来实现各部分的功能。
此外，作为处理电路50，也可以应用专用的硬件。在处理电路50为专用的硬件的情况下，处理电路50例如为单一电路、复合电路、程序化的处理器、并行程序化的处理器、ASIC(Application Specific Integrated Circuit：专用集成电路)、FPGA(Field-Programmable Gate Array：现场可编程门阵列)、或它们的组合等。
轨道计划装置200和200A的构成要素的各自的功能可以由单独的处理电路实现，也可以将这些功能集中由一个处理电路实现。
另外，图39示出了在处理电路50使用处理器构成的情况下的硬件结构。在该情况下，轨道计划装置200和200A的各部分的功能通过软件等(软件、固件、或者软件与固件)的组合来实现。软件等被记述为程序，并存储在存储器52中。作为处理电路50发挥功能的处理器51通过读取并执行存储在存储器52(存储装置)中的程序来实现各部分的功能。即，该程序可以说是使计算机执行轨道计划装置200和200A的构成要素的动作的顺序以及方法的程序。
在此，存储器52例如可以是RAM、ROM、闪存、EPROM(Erasable Programmable ReadOnly Memory：可擦编程只读存储器)、EEPROM(Electrically Erasable ProgrammableRead Only Memory：电可擦可编程序只读存储器)等非易失性或易失性半导体存储器、HDD(Hard Disk Drive：硬盘驱动器)、磁盘、软盘、光盘、压缩光盘、迷你盘、DVD(Digitalversatile Disc：数字多功能光盘)及其驱动装置等、或者今后使用的所有的存储介质。
以上，说明了轨道计划装置200和200A的各构成要素的功能通过硬件以及软件等中的任一方来实现的结构。但是不限于此，也可以为通过专用的硬件来实现轨道计划装置200和200A的一部分构成要素并通过软件等来实现另一部分构成要素的结构。例如，对于一部分构成要素，能够通过作为专用硬件的处理电路50来实现其功能，对于其他一部分构成要素，能够通过作为处理器51的处理电路50读取并执行存储在存储器52中的程序来实现其功能。
如上所述，轨道计划装置200和200A能够通过硬件、软件等或者它们的组合来实现上述各功能。
虽然详细说明了本公开，但上述的说明在所有方面都是示例性的，本公开不限定于此。可理解在不脱离本公开的范围的情况下能够设想未例示的无数变形例。
此外，本公开在其公开的范围内能够自由地组合各实施方式，或者能够对各实施方式适当地进行变形、省略。