本发明涉及路径预测装置,该路径预测装置使用由雷达、GPS等传感器构成的观测装置来观测飞机、船舶、车辆等移动的对象物体的位置,根据其观测值预测用于防止对象物体与位于其周围的多个物体的碰撞的路径。
背景技术:
:近年来,在车辆的驾驶辅助系统、航空管制等各种领域中要求预测避免移动体之间的碰撞的安全路径的技术。例如开发了如下的技术:在车辆的驾驶辅助系统中,通过装载于本车辆的毫米波雷达、激光雷达等的传感器获得位于本车辆周围的车辆、停止物等障碍物的位置,在根据本车辆与障碍物的相对距离、相对速度判断碰撞危险度之后,控制本车辆,防止碰撞。此外,作为更高端的技术,也开发了如下的自动驾驶技术:利用所述传感器识别周围环境,在没有驾驶员的操作的情况下自动进行方向盘操作、制动器等的操作,到达目的地。作为这种关于路径预测的现有技术,例如在专利文献1所示的装置中,预先生成多个车辆预测轨道,基于生成的预测轨道计算时间空间内的预测路径的存在概率。此外,例如在专利文献2所示的驾驶辅助装置中,计算本车辆相对于其他车辆的潜在风险映射图,使得基于风险的油门、制动器等的控制介入。另一方面,在航空管制中,相对于以往的基于三维位置导航方法,研究了在导航方法中采纳包括三维位置和时间的四维轨道(4DT:4DimensionalTrajectory)。该4DT相当于预测路径,能够通过管理4DT来预测航空交通量、空域容量,从而预计会提高航运的安全性。作为这样的路径预测的技术,例如,在专利文献3中,基于目标的当前速度和机头方向,假定为匀速直线前进而计算将来位置。此外,例如,在专利文献4所示的系统中,使用基于A*算法的最佳路径探索方法作为预测将来位置的方法。在该算法中,确定包括禁止进入区域(障碍物)的、将候选路径细分为网状的移动空间中的从出发至目标(或者通过点)止的节点。现有技术文献专利文献专利文献1:日本特开2007-233646号公报专利文献2:日本特开2012-148747号公报专利文献3:日本特开平11-120500号公报专利文献4:日本特开2009-251729号公报技术实现要素:发明要解决的课题但是,在上述专利文献1所述的现有装置中,为了计算存在概率,必须生成多个预测轨道,存在运算负荷增大的问题。此外,在专利文献2所示的装置中,存在这样的问题:风险的计算方法是不明确、且依赖参数的计算方法,不能准确地评估风险。并且,在专利文献3所述的现有技术中,存在这样的问题:在目标为了回避雷云等障碍物而变更路线的情况下,将来位置的估计精度恶化。此外,在专利文献4所述的使用A*算法的系统中,存在这样的问题:由于路径由阵点决定,未考虑移动体的运动。为了得到自然的路径,必须细分阵点的间隔,存在牺牲处理时间的问题。本发明是为了解决该问题而完成的,目的在于,提供能够减轻计算碰撞危险度低的预测路径时的运算负荷的路径预测装置。用于解决课题的手段本发明的路径预测装置具备:传感器部,其观测对象物体与位于该对象物体的周围的周围物体的位置;跟踪处理部,其根据所述对象物体与所述周围物体的位置进行跟踪处理,计算对象物体与周围物体的估计位置和估计速度;碰撞物体检测部,其根据估计位置和估计速度,检测可能与对象物体碰撞的周围物体作为目标物体;路径预测部,其基于碰撞回避模型,预测对象物体相对于目标物体的路径;碰撞危险度评估部,其与碰撞回避模型对应地计算对象物体与目标物体的碰撞危险度;碰撞判定部,其根据碰撞危险度判定有无碰撞,在判定为碰撞的情况下,向路径预测部反馈碰撞回避模型修正值;以及回避路径选择部,其从碰撞判定部判定为无碰撞的多个碰撞回避模型中选择任意碰撞回避模型,选择该碰撞回避模型的路径作为避免物体之间的碰撞的路径,路径预测部使用碰撞回避模型修正值进行新的路径预测。发明的效果本发明的路径预测装置基于碰撞回避模型,预测对象物体相对目标物体的路径,与碰撞回避模型对应地计算对象物体与目标物体的碰撞危险度,根据该碰撞危险度判定有无碰撞,选择从判定为无碰撞的多个碰撞回避模型中选择的任意一个碰撞回避模型的路径,作为避免物体之间碰撞的路径。由此,能够减轻计算碰撞危险度低的预测路径时的运算负荷。附图说明图1是示出本发明的实施方式1的路径预测装置的结构图。图2是本发明的实施方式1的路径预测装置的碰撞危险度的说明图。图3是本发明的实施方式1的路径预测装置的碰撞危险度高的情况下的说明图。图4是本发明的实施方式1的路径预测装置的碰撞危险度低的情况下的说明图。图5是示出本发明的实施方式1的路径预测装置中的转向回避时的碰撞危险度计算对象的说明图。图6是示出本发明的实施方式1的路径预测装置中的从路径预测部至碰撞判定部的处理部的动作的流程图。具体实施方式以下,为了更详细地说明本发明,基于附图对用于实施本发明的方式进行说明。实施方式1.图1是示出本实施方式的路径预测装置的结构图。如图所示,本实施方式的路径预测装置具有传感器部1、跟踪处理部2、碰撞物体检测部3、路径预测部4、碰撞危险度评估部5、碰撞判定部6以及碰撞回避路径选择部7。传感器部1是观测对象物体与位于该对象物体的周围的周围物体的相对位置的处理部,使用毫米波雷达、激光雷达、光学相机、红外相机等的传感器、或者接收周围车辆或歩行者的GPS位置的通信装置等而构成。跟踪处理部2是如下的处理部:根据由传感器部1观测到的相对位置进行跟踪处理,计算对象物体与周围物体的估计位置、估计速度、估计位置的估计误差、估计速度的估计误差。碰撞物体检测部3是如下的处理部:根据估计位置和所述估计速度,检测可能与对象物体碰撞的周围物体作为目标物体。路径预测部4是如下的处理部:分别在M个碰撞回避模型中计算对象物体相对于目标物体的提前N步的预测位置(此处,M和N是任意的整数)。碰撞危险度评估部5是如下的处理部:根据由跟踪处理部2计算出的估计位置和估计误差,按照每个碰撞回避模型计算碰撞危险度。碰撞判定部6是如下的处理部:根据由碰撞危险度评估部5计算出的碰撞危险度判定有无碰撞,在判定为碰撞的情况下向路径预测部4反馈碰撞回避模型修正值,在判定为无碰撞的情况下,将该碰撞回避模型输出至碰撞回避路径选择部7。碰撞回避路径选择部7是如下的处理部:基于规定的判定基准,针对从碰撞判定部6输出的碰撞回避模型,选择任意的碰撞回避模型,确定碰撞回避用的预测路径。另外,使用计算机构成路径预测装置,通过用CPU执行与各个处理部的功能对应的软件来实现跟踪处理部2至碰撞回避路径选择部7。或者,也可以用专用的硬件构成上述传感器部1至碰撞回避路径选择部7中的至少一个。接着,对实施方式1的路径预测装置的动作进行说明。传感器部1测定周围车辆或歩行者的位置和速度。跟踪处理部2根据位置和速度,借助跟踪处理来计算位置估计值、速度估计值、位置及速度的估计误差协方差矩阵。碰撞物体检测部3检测可能与本车辆碰撞的周围车辆。例如可以基于TTC(TimeToCollision)的思想进行检测。TTC由式(1)定义,如果TTC为阈值以下,则检测为可能碰撞的车辆。并且,将检测出的周围车辆i定义为目标车辆。TTC=(y^s,k(i)-yk(ego))(y·^s,k(i)-y·k(ego))---(1)]]>采样时刻k的周围车辆i相对于纵方向的估计位置采样时刻k的周围车辆i相对于纵方向的估计速度来样时刻k的本车辆相对于纵方向的位置采样时刻k的本车辆相对于纵方向的速度此外,作为碰撞物体检测部3的其他方法,在本车辆周围先设定规定区域,检测1至N步后的预测位置进入该规定区域的车辆,可以视作目标车辆。此处,如式(2)那样计算提前N步的N个预测位置。x^p,k+N(i)=ΦNx^s,k(i)---(2)]]>x^s,k(i)=x^s,k(i)y^s,k(i)x·^s,k(i)y·^s,k(i)T---(3)]]>x^p,k+N(i)=x^p,k+N(i)y^p,k+N(i)x·^p,k+N(i)y·^p,k+N(i)T---(4)]]>ΦN=I2×2N·ΔT·I2×20·I2×2I2×2---(5)]]>采样时刻k的周围车辆i的估计状态矢量采样时刻k的周围车辆i的提前N步预测状态矢量采样时刻k的周围车辆i相对于横向的估计位置采样时刻k的周围车辆i相对于横向的估计速度采样时刻k的周围车辆i相对于横向的提前N步预测位置采样时刻k的周围车辆i相对于横向的提前N步预测速使采样时刻k的周围车辆i相对于纵向的提前N步预测位置采样时刻k的周围车辆i相对于纵向的提前N步预测速度ΔT:步宽IL×L:L行L列的单位矩阵路径预测部4针对由碰撞物体检测部3检测出的目标车辆tgti,分别在M个碰撞回避模型中计算提前N步的预测位置。此处,作为碰撞回避模型,例如可以定义制动回避模型、左转向回避模型、右避免转向模型。假设制动回避模型为在维持车道的状态下通过制动来避免碰撞的模型,左/右转向回避模型为通过输入转向量而向左/右变更车道来避免碰撞的模型。此外,对于这些模型,假设制动量或者转向量设定成不超过规定的界限值。特别是在后述的碰撞判定部6中,在判断为不可能避免碰撞的情况下,制动量或者转向量的修正值被反馈到路径预测部4,实施不超过规定的界限值的动作。此外,在路径预测部4中需要设定碰撞回避模型的制动量或者转向量的初始值。可以按照经验设定制动及转向回避时输入的值作为初始值。此外,也可以利用学习算法,按照每个驾驶员设定不会感到不快的程度的制动量、转向量。并且,在路径预测部4中,不限于上述的模型,除此以外也可以增加与各种场景对应的碰撞回避模型。此外,也可以根据地图数据、GPS位置等,在车道数和本车辆行驶的车道为已知的情况下,丢弃不需要的碰撞回避模型,从而削减碰撞回避模型数。例如,在行车道数为2且本车辆行驶在左车道的情况下,无法进行左转向回避,因此,丢弃左转向回避模型,对剩下的碰撞回避模型进行计算。另外,例如,在途中车道数从2车道增加至3车道的地点,能够容易增加用于将车道变更为增加车道的碰撞回避模型,容易进行利用地图数据的碰撞回避模型的增加、削减。如果在地图数据以外还利用激光雷达、相机等,则能够识别外部环境,可以用来代替地图。对基于碰撞回避模型的预测位置计算方法进行说明。基于制动回避模型的制动用加速度ab如式(6)所示那样计算预测路径(提前N步的预测位置)。x^p,k+N(ego)=F(ab)=ΦNxk(ego)+0-12(N·ΔT)2·ab0-N·ΔT·ab---(6)]]>xk(ego)=xk(ego)yk(ego)x·k(ego)y·k(ego)T---(7)]]>x^p,k+N(ego)=x^p,k+N(ego)y^p,k+N(ego)x·^p,k+N(ego)y·^p,k+N(ego)T---(8)]]>ab:制动用加速度对左/右转向回避模型也能同样地计算。此处,车辆相对于转向的预测位置根据车辆重量、车体重心位置、偏航惯性力矩等车辆参数而不同,因此,在车辆参数为已知的情况下,预先设定,计算预测位置。此外,在车辆参数为未知的情况下,可以使用通过公知的学习算法等估计出的参数。碰撞危险度评估部5根据由跟踪处理部2输出的位置的估计误差协方差矩阵和位置及速度估计值来计算碰撞危险度。如式(9)所示,计算利用估计误差协方差矩阵对采样时刻k的本车辆的N步后的预测位置与目标车辆tgti的n(n=1、…、N)步骤后的预测位置之差进行归一化而得到的值即马氏距离的平方值εk+n。ϵk+n=Δx^k+nTPp,k+n(tgti)-1Δx^k+n---(9)]]>Δx^k+n=x^p,k+n(tgti)-x^p,k+n(ego)y^p,k+n(tgti)-y^p,k+n(ego)T---(10)]]>Pp,k+n(tgti)=ΦnPs,k(tgti)ΦnT---(11)]]>采样时刻k的周围车辆tgti的平滑误差协方差矩阵采样时刻k的周围车辆tgti的提前n步预测误差协方差矩阵此处,可知横位置x、纵位置y这两个变量遵循正态分布的情况下,马氏距离的平方值εk+n的概率分布为自由度2的卡方分布。利用该性质,碰撞危险度评估部5如图2所示那样将碰撞危险度定义为卡方分布的上侧概率(图2的斜线部分100)。为了直观地理解碰撞危险度,对本车辆(目标2)与目标车辆(目标1)的相对位置和碰撞危险度的对应关系进行说明。例如,如图3所示,在目标1与目标2碰撞的场景(目标1与目标2的位置相同)中,图3的斜线部分101接近1。即,计算出碰撞危险度为1(或者100%)。另一方面,如图4所示,在目标1与目标2的距离无限远的场景中,图4的斜线部分接近0。即,计算出碰撞危险度为0(0%)。因此,可知卡方分布的上侧概率直观地为与碰撞危险度相当的值。并且,能够预先计算马氏距离的平方值εk+n与卡方分布的上侧概率的对应表,因此如果先保持对应表,则无需计算就能够读取与马氏距离的平方值相应的碰撞危险度。此外,以上记述了计算本车辆与周围车辆的相对位置的碰撞危险度的方法,在此基础上说明使用目标1与目标2的绝对位置的情况下的碰撞危险度计算方法。例如,面向车辆驾驶辅助系统,可考虑在车车间通信等中获得本车辆和周围车辆的GPS位置这样的绝对值的情况。此外,在航空管制的领域中,对于多架飞机,可考虑获得基于雷达的观测位置、GPS位置并用于飞机管制的情况。在该情况下,由于各目标位置中含有位置误差,因此,使用下式(12)、(13)计算碰撞危险度的评估值,读取与评估值对应的碰撞危险度。ϵk+n=Δx^k+nT(Pp,k+n(1)+Pp,k+n(2))-1Δx^k+n---(12)]]>Δx^k+n=x^p,k+n(1)-x^p,k+n(2)y^p,k+n(1)-y^p,k+n(2)T---(13)]]>采样时刻k的目标tgti的平滑误差协方差矩阵采样时刻k的目标tgti的提前n步预测误差协方差矩阵此处,为了根据目标的误差分布的重叠计算碰撞危险度,必须进行基于误差分布的复杂的数值计算,但在本发明中不需要复杂的数值计算就能够计算碰撞危险度。此外,可以用其他的概率分布(例如正态分布)对马氏距离的平方值εk+n的概率分布进行近似。碰撞判定部6根据由碰撞危险度评估部5计算出的碰撞危险度判定碰撞,在碰撞的情况下向路径预测部4输出预测路径修正值,对预测路径进行再修正。在不碰撞的情况下向碰撞回避路径选择部7输出预测路径和碰撞危险度。在碰撞判定中,在相对于概率变量εk+n(n=1,…,N)的最小值为阈值εth以下的情况下,视作碰撞。所述的阈值εth使用自由度m的卡方分布表,如在碰撞危险度评估部5中叙述的那样,如果预先设定与碰撞危险度对应的碰撞阈值εth,则能容易地判定是否碰撞。此外,在图5所示的转向回避的的情况下,可考虑其他的周围车辆正在在本车辆200通过转向回避而变更车道后位置行驶的情形,因此,对于变更后的车道,计算最近的前方车辆201与最近的后方车辆202的碰撞危险度。并且,从目标车辆203、最近的前方车辆201以及最近的后方车辆202的碰撞危险度中选择最大值,实施碰撞判定。另外,由图中的虚线包围的区域表示预测误差。并且,碰撞判定部6将预测路径的修正值反馈至路径预测部4,由此,路径预测部4和碰撞危险度评估部5再次计算预测路径和碰撞危险度。重复这些步骤,直至超过阈值εth为止。图6示出这些路径预测部4~碰撞判定部6的处理流程。即,按照每个目标车辆,对所有的模型进行N步的路径预测(步骤ST1)、碰撞危险度的评估(步骤ST2)以及碰撞判定(步骤ST3、ST4)。此外,在步骤ST4中为碰撞阈值以下的情况下,执行模型循环直至超过碰撞阈值为止。另外,在模型循环达到预先设定的规定次数的情况下,可以停止该碰撞回避模型的计算。碰撞回避路径选择部7根据基于路径预测部4~碰撞判定部6己计算的各碰撞回避模型的预测路径,确定碰撞回避用的预测路径。对于基于各碰撞回避模型的N个预测位置,比较碰撞危险度的最大值,将具有最小值的碰撞回避模型看作最安全的回避路径,作为碰撞回避用预测路径输出。此外,也可以选择包含最小值的设定值以下的碰撞回避模型。此外,可以比较对所述N个预测位置赋予的N个碰撞危险度的总值,选择最小的路径。另外,在此也可以选择包含最小值的设定值以下的路径。此外,在制动量或者转向量超过规定的界限值的情况下,可以丢弃。此外,也可以根据驾驶员的需要,选择制动量的总值最小的路径、或者转向回避量的总值最小的路径。由此,在实施方式1中,通过限定为在现实中假定的碰撞回避模型,不必计算以往那样的无数个路径,能够减轻运算负荷。如以上说明的那样,根据实施方式1的路径预测装置,具备:传感器部,其观测对象物体与位于该对象物体的周围的周围物体的位置;跟踪处理部,其根据对象物体与周围物体的位置进行跟踪处理,计算对象物体与周围物体的估计位置和估计速度;碰撞物体检测部,其根据估计位置和估计速度,检测可能与对象物体碰撞的周围物体作为目标物体;路径预测部,其基于碰撞回避模型,预测对象物体相对于目标物体的路径;碰撞危险度评估部,其与碰撞回避模型对应地计算对象物体与目标物体的碰撞危险度;碰撞判定部,其根据碰撞危险度判定有无碰撞,在判定为碰撞的情况下,向路径预测部反馈碰撞回避模型修正值;以及回避路径选择部,其从由碰撞判定部判定为无碰撞的多个碰撞回避模型中,选择任意碰撞回避模型,选择碰撞回避模型的路径作为避免物体之间的碰撞的路径,路径预测部使用碰撞回避模型修正值,进行新的路径预测,因此,能够减轻计算碰撞危险度低的预测路径时的运算负荷。此外,根据实施方式1的路径预测装置,跟踪处理部计算估计位置的估计误差和估计速度的估计误差,碰撞危险度评估部根据用估计误差对估计位置归一化而得到的值来计算碰撞危险度,因此,不必进行复杂的数值计算就能计算碰撞危险度。此外,根据实施方式1的路径预测装置,碰撞危险度评估部从示出用估计误差对估计位置进行归一化而得到的值与碰撞危险度的对应关系的对应表,获得碰撞危险度,因此不必进行数值计算就能容易得到碰撞危险度。此外,根据实施方式1的路径预测装置,回避路径选择部对于碰撞回避模型的碰撞危险度的时间方向累计值,选择累计值在设定值以下的碰撞回避模型,因此,不必计算无数个路径,能减轻运算负荷。此外,根据实施方式1的路径预测装置,回避路径选择部以碰撞回避模型的碰撞危险度在时间方向上的最大值为代表值,选择代表值在设定值以下的碰撞回避模型,因此,不必计算无数个路径,能减轻运算负荷。此外,根据实施方式1的路径预测装置,碰撞判定部通过将碰撞危险度与设定的阈值进行比较来进行碰撞判定,因此,能够容易判定是否碰撞。另外,本发明能够在其发明范围内进行实施方式的任意构成要素的变形,或者可以省略实施方式的任意构成要素。产业上的可利用性如上所述,本发明的路径预测装置涉及这样的路径预测装置:该路径预测装置使用由雷达、GPS等传感器构成的观测装置来观测飞机、船舶、车辆等移动体的位置,根据其观测值预测用于防止移动体与位于其周围的多个移动体的碰撞的路径,适于用在车辆驾驶辅助系统、航空管制等中。标号说明1:传感器部;2:跟踪处理部;3:碰撞物体检测部;4:路径预测部;5:碰撞危险度评估部;6:碰撞判定部;7:碰撞回避路径选择部。当前第1页1 2 3