CN115148049B - 轨道生成装置及轨道生成方法 - Google Patents
轨道生成装置及轨道生成方法 Download PDFInfo
- Publication number
- CN115148049B CN115148049B CN202210298509.3A CN202210298509A CN115148049B CN 115148049 B CN115148049 B CN 115148049B CN 202210298509 A CN202210298509 A CN 202210298509A CN 115148049 B CN115148049 B CN 115148049B
- Authority
- CN
- China
- Prior art keywords
- predicted
- orbit
- track
- moving body
- interference
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 36
- 238000004364 calculation method Methods 0.000 claims description 98
- 238000012937 correction Methods 0.000 claims description 86
- 238000012545 processing Methods 0.000 claims description 8
- 238000001514 detection method Methods 0.000 abstract description 32
- 230000006870 function Effects 0.000 description 161
- 238000011156 evaluation Methods 0.000 description 113
- 238000010586 diagram Methods 0.000 description 48
- 238000004422 calculation algorithm Methods 0.000 description 21
- 230000003321 amplification Effects 0.000 description 15
- 238000003199 nucleic acid amplification method Methods 0.000 description 15
- 230000006399 behavior Effects 0.000 description 6
- 238000010801 machine learning Methods 0.000 description 6
- 230000002093 peripheral effect Effects 0.000 description 6
- 238000013528 artificial neural network Methods 0.000 description 5
- 238000013527 convolutional neural network Methods 0.000 description 4
- 238000005314 correlation function Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 102100036467 Protein delta homolog 1 Human genes 0.000 description 2
- 101150102995 dlk-1 gene Proteins 0.000 description 2
- 238000012886 linear function Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
- B60W30/095—Predicting travel path or likelihood of collision
- B60W30/0956—Predicting travel path or likelihood of collision the prediction being responsive to traffic or environmental parameters
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W60/00—Drive control systems specially adapted for autonomous road vehicles
- B60W60/001—Planning or execution of driving tasks
- B60W60/0011—Planning or execution of driving tasks involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/16—Anti-collision systems
- G08G1/166—Anti-collision systems for active traffic, e.g. moving vehicles, pedestrians, bikes
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W50/0097—Predicting future conditions
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W60/00—Drive control systems specially adapted for autonomous road vehicles
- B60W60/001—Planning or execution of driving tasks
- B60W60/0027—Planning or execution of driving tasks using trajectory prediction for other traffic participants
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W60/00—Drive control systems specially adapted for autonomous road vehicles
- B60W60/001—Planning or execution of driving tasks
- B60W60/0027—Planning or execution of driving tasks using trajectory prediction for other traffic participants
- B60W60/00276—Planning or execution of driving tasks using trajectory prediction for other traffic participants for two or more other traffic participants
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B17/00—Systems involving the use of models or simulators of said systems
- G05B17/02—Systems involving the use of models or simulators of said systems electric
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/01—Detecting movement of traffic to be counted or controlled
- G08G1/0104—Measuring and analyzing of parameters relative to traffic conditions
- G08G1/0125—Traffic data processing
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/09—Arrangements for giving variable traffic instructions
- G08G1/0962—Arrangements for giving variable traffic instructions having an indicator mounted inside the vehicle, e.g. giving voice messages
- G08G1/0968—Systems involving transmission of navigation instructions to the vehicle
- G08G1/096805—Systems involving transmission of navigation instructions to the vehicle where the transmitted instructions are used to compute a route
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W2050/0001—Details of the control system
- B60W2050/0019—Control system elements or transfer functions
- B60W2050/0028—Mathematical models, e.g. for simulation
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2554/00—Input parameters relating to objects
- B60W2554/40—Dynamic objects, e.g. animals, windblown objects
- B60W2554/402—Type
- B60W2554/4029—Pedestrians
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- General Physics & Mathematics (AREA)
- Human Computer Interaction (AREA)
- Theoretical Computer Science (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- Computer Hardware Design (AREA)
- Chemical & Material Sciences (AREA)
- Analytical Chemistry (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Traffic Control Systems (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明提供能够以能躲避与干涉对象的干涉的方式适当地生成移动体的未来的轨道的轨道生成装置等。轨道生成装置(1)基于信息检测装置(4)的周边信息,使用将本车辆(3)的轨道模型化了的模型式(1)~(20),决定第一~第三预测轨道,使用第一~第三预测轨道,计算潜在风险(R_p_mi_vj_v_r),以使潜在风险(R_p_mi_vj_v_r)减少的方式,决定移动平均值(P_c1~P_c3)。在移动平均值(P_c1、P_c2)为相同符号的情况下,以反映用于修正第二预测轨道的移动平均值(P_c2)的方式决定第一预测轨道。
Description
技术领域
本发明涉及生成移动体的未来的轨道的轨道生成装置等。
背景技术
以往,作为轨道生成装置,已知记载于专利文献1的技术。该轨道生成装置使用将移动体的轨道作为圆弧进行建模而得到的模型式,以使移动体能够躲避与干涉对象的干涉的方式生成移动体的轨道。在该轨道生成装置中,基于模型式来计算评价函数,并通过极值搜索控制器以使评价函数成为极值的方式决定圆弧的曲率。然后,基于该曲率,生成移动体的未来的轨道。
在先技术文献
专利文献
专利文献1:日本特开2019-220054号公报
发明内容
发明要解决的课题
在通常的交通环境下,在移动体躲避与干涉对象的干涉时,移动体的轨道并不局限于仅向左方向或右方向弯曲的轨道,例如,有时成为向左方向弯曲之后向右方向反转地弯曲的轨道(参照后述的图24)。根据上述专利文献1的轨道生成装置,在要生成那样的轨道的情况下,需要使用多个极值搜索控制器。
在这样使用了多个极值搜索控制器的情况下,当在各极值搜索控制器中共用一个评价函数时,需要将各极值搜索控制器中的参照输入的频率设定为相互不干涉的值。在该情况下,各极值搜索控制器中的移动平均滤波器的抽头数增大,与此相伴产生响应延迟。在该情况下,移动体用于躲避与干涉对象的干涉的时间余量有可能减少。
为了避免这种情况,在多个极值搜索控制器中使用了相互不同的评价函数的情况下,产生以下所述那样的问题。即,在由各极值搜索控制器生成移动体的未来的轨道的情况下,有可能在移动体非常接近干涉对象的位置生成躲避与干涉对象的干涉的轨道(参照后述的图22)。在该情况下,移动体用于躲避与干涉对象的干涉的空间余量也有可能减少。特别是,在移动体为车辆的情况下,由于能够实现的轨道的限制一般较大,所以该问题变得显著。
本发明是为了解决上述课题而完成的,其目的在于提供能够以能躲避与干涉对象的干涉的方式适当地生成移动体的未来的轨道的轨道生成装置等。
用于解决课题的方案
为了实现上述目的,本发明是生成移动体的未来的轨道的轨道生成装置,其特征在于,轨道生成装置具备:周边状况识别部,其识别移动体的周边状况;预测轨道生成部,其基于利用周边状况识别部识别出的周边状况的识别结果,使用将移动体的轨道模型化了的多个轨道模型,生成将移动体的未来的轨道分割为多个时的多个未来的轨道的预测值即多个预测轨道;干涉程度参数计算部,其使用多个预测轨道,计算表示移动体的周边的干涉对象与移动体的干涉程度的多个干涉程度参数;以及修正量决定部,其以使多个干涉程度参数所表示的干涉程度减少的方式决定用于分别修正多个预测轨道的多个修正量,预测轨道生成部基于周边状况的识别结果,使用多个轨道模型及多个修正量,生成多个预测轨道,并且在用于修正相互相邻的两个预测轨道的两个修正量被决定为将两个预测轨道向相同的方向修正的情况下,以反映用于修正两个预测轨道中的、远离移动体的当前位置的预测轨道的修正量的方式生成两个预测轨道中的、接近移动体的当前位置的预测轨道。
根据该轨道生成装置,计算表示移动体的周边的干涉对象与移动体的干涉程度的多个干涉程度参数,以使多个干涉程度参数所表示的干涉程度减少的方式决定用于分别修正多个预测轨道的多个修正量。并且,基于周边状况的识别结果,使用多个轨道模型及多个修正量,生成多个预测轨道,因此多个预测轨道被以使干涉对象与移动体的干涉程度减少的方式生成。
此时,在用于修正相互相邻的两个预测轨道的两个修正量被决定为将两个预测轨道向相同的方向修正的情况下,两个预测轨道中的、接近移动体的当前位置的预测轨道以反映用于修正两个预测轨道中的、远离移动体的当前位置的预测轨道的修正量的方式生成。
在此,在干涉对象存在于移动体的行进方向的情况下,远离当前位置的预测轨道与接近移动体的当前位置的预测轨道相比,成为更接近干涉对象的轨道。因此,通过以反映用于修正接近干涉对象的预测轨道的修正量的方式生成远离干涉对象(即接近移动体)的预测轨道,由此在远离干涉对象的预测轨道中,能够进一步增大用于躲避与干涉对象的干涉的躲避余量。
其结果为,在由移动体能够实现的轨道被限制的情况下,例如,在移动体为车辆的情况下,也能够以能躲避与干涉对象的干涉的方式适当地生成移动体的未来的轨道。需要说明的是,本说明书中的“轨道”并不局限于曲线状,也包括组合了线段的轨道。另外,本说明书中的“生成未来的轨道”包括运算未来的轨道、决定未来的轨道及预测未来的轨道。而且,本说明书中的“识别周边状况”除了识别周边状况之外,还包括取得周边状况及检测周边状况。
在本发明的轨道生成装置中,优选的是,预测轨道生成部在两个修正量被决定为将两个预测轨道向相互不同的方向修正的情况下,与用于修正远离移动体的当前位置的预测轨道的修正量无关地生成接近移动体的当前位置的预测轨道。
在该情况下,在两个修正量被决定为将两个预测轨道向相互不同的方向修正的情况下,两个预测轨道为了躲避干涉对象而被向相互不同的方向修正。因此,在那样的条件下,在将用于修正远离移动体的当前位置的预测轨道的修正量反映到接近当前位置的预测轨道时,在接近当前位置的预测轨道中,会使针对干涉对象的躲避余量减少。
与此相对地,根据该轨道生成装置,在两个预测轨道为了躲避干涉对象而被向相互不同的方向修正时,与用于修正远离移动体的当前位置的预测轨道的修正量无关地生成接近移动体的当前位置的预测轨道。由此,在接近当前位置的预测轨道中,能够适当地确保针对干涉对象的躲避余量。
在本发明的轨道生成装置中,优选的是,接近移动体的当前位置的预测轨道以被分割为比远离移动体的当前位置的预测轨道短的多个预测轨道的状态生成。
在接近移动体的当前位置的预测轨道的情况下,在该预测轨道的附近存在干涉对象时,需要以能够更可靠地躲避因与移动体的距离短而引起的与干涉对象的干涉的方式生成预测轨道。与此相对地,根据该轨道生成装置,由于接近移动体的当前位置的预测轨道以被分割为比远离移动体的当前位置的预测轨道短的多个预测轨道的状态生成,因此能够以能更可靠地躲避与干涉对象的干涉的方式生成这些多个预测轨道。
在本发明的轨道生成装置中,优选的是,移动体具备周边状况识别部、预测轨道生成部、干涉程度参数计算部及多个修正量决定部。
根据该轨道生成装置,在干涉对象存在于移动体的行进方向的情况下,在移动体的远离干涉对象的预测轨道中,能够增大针对干涉对象的躲避余量。由此,即使在能够实现的轨道的限制一般较大的移动体(例如,车辆等不能向正侧面移动的移动体)中,也能够以能躲避与干涉对象的干涉的方式适当地生成移动体的未来的轨道。
在本发明的轨道生成装置中,优选的是,移动体是具备周边状况识别部、预测轨道生成部、干涉程度参数计算部及多个修正量决定部的第一移动体以外的第二移动体,预测轨道生成部使用第二移动体的过去的位置、多个轨道模型及多个修正量,生成多个预测轨道。
根据该轨道生成装置,能够以反映第一移动体以外的第二移动体的过去的轨道的方式适当地生成第二移动体的多个预测轨道。由此,在控制第一移动体的移动状态时,能够将第一移动体控制为以躲避与第二移动体的干涉的方式移动。
在本发明的轨道生成装置中,优选的是,预测轨道生成部以使第二移动体的过去的位置的时间序列与将接近第二移动体的当前位置的预测轨道沿着预测轨道向第二移动体的过去的位置侧延伸了时的轨道上的多个推定位置的时间序列之间的差量减少的方式,生成接近移动体的当前位置的预测轨道。
根据该轨道生成装置,接近第二移动体的当前位置的预测轨道被以使第二移动体的过去的位置的时间序列与多个推定位置的时间序列之间的差量减少的方式生成。即,能够考虑对第二移动体的过去的位置的跟随性地生成接近第二移动体的当前位置的预测轨道,能够提高其生成精度。
另一本发明是通过运算处理装置生成移动体的未来的轨道的轨道生成方法,其特征在于,运算处理装置执行如下步骤:周边状况识别步骤,在周边状况识别步骤中,识别移动体的周边状况;预测轨道生成步骤,在预测轨道生成步骤中,基于利用周边状况识别步骤识别出的周边状况的识别结果,使用将移动体的轨道模型化了的多个轨道模型,生成将移动体的未来的轨道分割为多个时的多个未来的轨道的预测值即多个预测轨道;干涉程度参数计算步骤,在干涉程度参数计算步骤中,使用多个预测轨道,计算表示移动体的周边的干涉对象与移动体的干涉程度的多个干涉程度参数;修正量决定步骤,在修正量决定步骤中,以使多个干涉程度参数所表示的干涉程度减少的方式决定用于分别修正多个预测轨道的多个修正量,在预测轨道生成步骤中,基于周边状况的识别结果,使用多个轨道模型及多个修正量,生成多个预测轨道,并且在用于修正相互相邻的两个预测轨道的两个修正量被决定为将两个预测轨道向相同的方向修正的情况下,以反映用于修正两个预测轨道中的、远离移动体的当前位置的预测轨道的修正量的方式生成两个预测轨道中的、接近移动体的当前位置的预测轨道。
附图说明
图1是示意地表示具备本发明的第一实施方式的轨道生成装置的车辆的构成的图。
图2是表示第一实施方式的轨道生成装置的功能性的构成的框图。
图3是用于说明本车辆的坐标系与交通参加者的坐标系的图。
图4是表示当本车辆躲避交通参加者时的轨道的一例的图。
图5是表示当本车辆躲避交通参加者时的轨道的另一例的图。
图6是用于说明本车辆的第一~第三预测轨道的图。
图7是表示x方向潜在风险的计算中使用的映射的一例的图。
图8是表示y方向潜在风险的计算中使用的映射的一例的图。
图9是用于说明定义了本车辆的行驶路的右侧边界线及左侧边界线的函数的图。
图10是表示右侧行驶路边界潜在风险与右侧行驶路边界函数的关系的图。
图11是表示左侧行驶路边界潜在风险与左侧行驶路边界函数的关系的图。
图12是表示轨道决定部的构成的框图。
图13是表示极值搜索控制器的构成的框图。
图14是表示施加搜索输入与评价函数的关系的图。
图15是表示移动平均值与施加搜索输入的关系的图。
图16是表示增益K_bp_21的计算中使用的映射的图。
图17是表示增益K_bp_21的计算中使用的映射的图。
图18是表示增益K_bp_31的计算中使用的映射的图。
图19是表示增益K_bp_31的计算中使用的映射的图。
图20是表示增益K_bp_32的计算中使用的映射的图。
图21是表示增益K_bp_32的计算中使用的映射的图。
图22是表示在本车辆的右前方存在干涉对象的情况下的第一~第三预测轨道的决定结果的一例的图。
图23是表示本车辆的第一~第三预测轨道的决定结果的一例的图。
图24是表示本车辆的第一~第三预测轨道的决定结果的另一例的图。
图25是表示移动平均值P_c1~P_c3与预测曲率ρ_m1的运算值的关系的图。
图26是表示移动平均值P_c2~P_c3与预测曲率ρ_m2的运算值的关系的图。
图27是表示第二实施方式的轨道生成装置的功能性的构成的框图。
图28是用于说明行人M1的坐标系与另一交通参加者TP4的坐标系的图。
图29是用于说明行人M1的第一~第三预测轨道的图。
图30是表示x方向潜在风险的计算中使用的映射的一例的图。
图31是表示y方向潜在风险的计算中使用的映射的一例的图。
图32是用于说明定义了行人移动的人行道的右侧边界线及左侧边界线的函数的图。
图33是表示右侧行驶路边界潜在风险与右侧行驶路边界函数的关系的图。
图34是表示左侧行驶路边界潜在风险与左侧行驶路边界函数的关系的图。
图35是表示横穿意图参数的计算中使用的映射的图。
图36是表示横穿意图参数的计算中使用的映射的图。
图37是表示在不使用横穿意图参数的情况下的行人M1的预测轨道的图。
图38是表示在使用了横穿意图参数的情况下的行人M1的预测轨道的图。
图39是用于说明跟踪位势的计算方法的图。
图40是表示第二实施方式的轨道决定部的构成的框图。
图41是表示增益K2_bp_21的计算中使用的映射的图。
图42是表示增益K2_bp_21的计算中使用的映射的图。
图43是表示增益K2_bp_31的计算中使用的映射的图。
图44是表示增益K2_bp_31的计算中使用的映射的图。
图45是表示增益K2_bp_32的计算中使用的映射的图。
图46是表示增益K2_bp_32的计算中使用的映射的图。
图47是表示在行人M1的右前方存在干涉对象的情况下的第一~第三预测轨道的决定结果的一例的图。
图48是表示行人M1的第一~第三预测轨道的决定结果的一例的图。
图49是表示行人M1的第一~第三预测轨道的决定结果的另一例的图。
图50是用于说明移动平均值P2_c1~P2_c3与预测曲率ρ2_m1的运算值的关系的图。
图51是用于说明移动平均值P2_c2~P2_c3与预测曲率ρ2_m2的运算值的关系的图。
附图标记说明
1 轨道生成装置
2 ECU2(周边状况识别部、预测轨道生成部、干涉程度参数计算部、修正量决定部、运算处理装置)
3 本车辆(移动体、第一移动体)
4 信息检测装置(周边状况识别部)
21 第一评价函数计算部(干涉程度参数计算部)
22 第二评价函数计算部(干涉程度参数计算部)
23 第三评价函数计算部(干涉程度参数计算部)
30 轨道决定部(预测轨道生成部、修正量决定部)
TP1 交通参加者(干涉对象)
R_p_mi_vj_v_r潜在风险(干涉程度参数)
R_p_rbr_mi_r右侧行驶路边界潜在风险(干涉程度参数)
R_p_rbl_mi_r左侧行驶路边界潜在风险(干涉程度参数)
P_c1移动平均值(修正量)
P_c2移动平均值(修正量)
P_c3移动平均值(修正量)
100 轨道生成装置
121 第一评价函数计算部(干涉程度参数计算部)
122 第二评价函数计算部(干涉程度参数计算部)
123 第三评价函数计算部(干涉程度参数计算部)
200 轨道决定部(预测轨道生成部、修正量决定部)
R2_p_mi_vj_v_r 潜在风险(干涉程度参数)
R2_p_rbr_mi_r 右侧行驶路边界潜在风险(干涉程度参数)
R2_p_rbl_mi_r 左侧行驶路边界潜在风险(干涉程度参数)
P2_c1 移动平均值(修正量)
P2_c2 移动平均值(修正量)
P2_c3 移动平均值(修正量)
M1 行人(移动体,第二移动体)
TP4 交通参加者(干涉对象)。
具体实施方式
以下,参照附图对本发明的第一实施方式的轨道生成装置进行说明。如图1所示,该轨道生成装置1应用于能够自动驾驶的四轮车辆3(以下称为“本车辆3”)。需要说明的是,在本实施方式中,本车辆3相当于移动体。在该轨道生成装置1中,为了执行本车辆3的自动驾驶控制,通过后述的算法,决定本车辆3的未来的行驶轨道。
该轨道生成装置1具备ECU2,并且信息检测装置4、原动机5及致动器6与该ECU2电连接。该信息检测装置4由相机、毫米波雷达、LIDAR、激光雷达、声纳、GPS、各种传感器及来自被称为ITS、VTS的基础设施的信息接收装置等构成。
信息检测装置4检测本车辆3的位置及本车辆3的行进方向的周边信息而向ECU2输出。在该情况下,作为周边信息,包括车道、人行道、人行横道、信号机、车道与人行道的边界及交通参加者(行人、其他车辆、障碍物)的信息等。需要说明的是,在本实施方式中,信息检测装置4相当于周边状况识别部。
原动机5例如由电动马达等构成,当决定了本车辆3的未来的行驶轨道时,由ECU2控制原动机5的输出,以使本车辆3在该行驶轨道上行驶。
另外,致动器6由制动用致动器及转向用致动器等构成,当决定了本车辆3的未来的行驶轨道时,由ECU2控制致动器6的动作,以使本车辆3在该行驶轨道上行驶。
另一方面,ECU2由微型计算机构成,该微型计算机由CPU、RAM、ROM、I/O接口及各种电路(均未图示)等构成。ECU2如后述那样,基于来自上述的信息检测装置4的周边信息,预测本车辆3以外的交通参加者(例如,后述的交通参加者TP1)的行动,并且基于交通参加者的行动的预测结果、本车辆3的位置及本车辆3的周边的交通环境等,决定本车辆3的未来的行驶轨道。
需要说明的是,在本实施方式中,ECU2相当于周边状况识别部、预测轨道生成部、干涉程度参数计算部、修正量决定部及运算处理装置。
接下来,参照图2对本实施方式的轨道生成装置1的功能性的构成进行说明。该轨道生成装置1为了执行本车辆3的自动驾驶控制,通过以下叙述的算法,来决定本车辆3的行驶轨道。
另外,在以下的说明中,将对本车辆3的行驶进行干涉的对象称为“干涉对象”。在该情况下,如后述那样,本车辆3以外的交通参加者及交通环境(道路环境、信号机的状态等)相当于干涉对象。
如图2所示,轨道生成装置1具备交通参加者检测部10、行动预测部11、第一~第三评价函数计算部21~23及轨道决定部30。这些要素10~11、21~23、30具体由ECU2构成,这些要素10~11、21~23、30中的运算处理以规定的控制周期ΔT(例如5msec)执行。
需要说明的是,在本实施方式中,第一~第三评价函数计算部21~23相当于干涉程度参数计算部,轨道决定部30相当于预测轨道生成部及修正量决定部。
首先,在交通参加者检测部10中,基于来自信息检测装置4的周边信息,使用规定的机器学习算法(例如,应用了DNN及CNN等神经网络的算法),检测/计算交通参加者的当前检测位置W_vj(k)。
在该情况下,当前检测位置W_vj(k)中的符号“(k)”表示控制时刻,在以下的说明中,适当省略该符号“(k)”等。另外,当前检测位置W_vj(k)中的尾标“j”表示存在的交通参加者的编号/数目。即,在交通参加者为一个的情况下,j=1,在交通参加者为n(n为多个)个的情况下,j=1~n。
以下,如图3所示,以仅作为其他车辆的交通参加者TP1存在于本车辆3的前方的情况(即j=1的情况下)为例进行说明。在此,本车辆3的坐标系如后述那样被规定为这样的正交二维坐标系,即,将本车辆3的当前位置的中心设为原点,且将本车辆3的行进方向及左方向设为X轴及Y轴。另外,图中的Z_m11、Z_m12、Z_m13是本车辆3的后述的预测位置。
另一方面,交通参加者TP1的坐标系被规定为这样的正交二维坐标系,即,将交通参加者TP1的中心设为原点、将交通参加者TP1的行进方向及横向设为X_v轴及Y_v轴且将X_v轴相对于X轴的倾斜度设为θ_v1。
在该正交二维坐标系中,在将交通参加者TP1的X_v坐标值设为x_v1且将Y_v坐标值设为y_v1的情况下,交通参加者TP1的当前检测位置W_v1(k)被计算为以三个值[x_v1(k)、y_v1(k)、θ_v1(k)]为要素的值(其中,x_v1(k)=y_v1(k)=0)。另外,图3中的交通参加者TP1的周围的由点划线所示的线表示后述的潜在风险。
在行动预测部11中,基于由交通参加者检测部10检测出的当前检测位置W_vj(k)的检测结果,使用规定的机器学习算法(例如,应用了DNN及CNN等神经网络的算法),计算交通参加者TP1的当前位置W_v1(k:k)以及5个预测位置W_v1(k:k+km11)、W_v1(k:k+km12)、W_v1(k:k+km13)、W_v1(k:k+km2)、W_v1(k:k+km3)。
在该情况下,预测位置W_v1(k:k+km11)的括弧内中的“:”的左侧的值k表示当前的控制时刻,“:”的右侧的值k+km11表示预测时刻。另外,当前的控制时刻k与预测时刻k+km13的间隔、预测时刻k+km13与预测时刻k+km2的间隔及预测时刻k+km2与预测时刻k+km3的间隔设定为彼此相同。
而且,当前的控制时刻k与预测时刻k+km11的间隔、预测时刻k+km11与预测时刻k+km12的间隔及预测时刻k+km12与预测时刻k+km13的间隔设定为彼此相同。即,两个预测时刻k+km11、k+km12相当于将当前的控制时刻k与预测时刻k+km13之间3等分时的、当前的控制时刻k与预测时刻k+km13之间的两个时刻。另外,当前位置W_v1(k:k)被设定为当前检测位置W_v1(k)。
如以上那样,在行动预测部11中,在交通参加者为一个的情况下,计算交通参加者的6个位置(当前位置及5个预测位置),并且在交通参加者为n个的情况下,计算n×6个位置。这一点在前述的各要素11、21~23、30的运算中也同样。以下,以交通参加者为一个的情况为例进行说明。
需要说明的是,在行动预测部11中,也可以使用后述的第二实施方式的方法来代替规定的机器学习算法,从而计算5个预测位置W_v1(k:k+km11)、W_v1(k:k+km12)、W_v1(k:k+km13)、W_v1(k:k+km2)、W_v1(k:k+km3)。另外,在行动预测部11中,也可以构成为计算4个以下或6个以上的预测位置。
接下来,对本实施方式的轨道生成装置1中的本车辆3的未来的行驶轨道(以下称为“本车轨道”)的定义及其决定原理进行说明。
例如,如图4所示,在本车辆3在左车道行驶中、作为行人的交通参加者TP2存在于左前方、该交通参加者TP2朝向左车道侧步行的情况下,为了躲避与交通参加者TP2的干涉,需要以使本车辆3向右车道移动的方式决定本车轨道。与此同时,需要以使本车辆3不从右车道向右外侧伸出的方式决定本车轨道。
另外,例如,如图5所示,在本车辆3在左车道行驶中、除了与图4相同的交通参加者TP2之外、作为行人的交通参加者TP3存在于右车道内的情况下,在本车辆3为了躲避与交通参加者TP2的干涉而移动到右车道侧之后,为了躲避与交通参加者TP3的干涉,需要以使本车辆3向左车道侧移动的方式决定本车轨道。
为了满足以上的条件,在本实施方式中,如图6所示,本车轨道被决定为将三个圆弧状的第一~第三预测轨道组合而成的轨道。最初,对第一预测轨道进行说明。首先,定义将本车辆3的当前位置的中心设为原点、将本车辆3的前方设为X轴且将本车辆3的左方设为Y轴的X-Y坐标系。
然后,将从该X-Y坐标系中的原点起经过第一预测时间T_m1后的位置设为第一预测位置Z_m1,并将从原点到第一预测位置Z_m1为止的本车轨道定义为圆弧状的第一预测轨道。在此,在将本车辆3的当前速度设为V_ego的情况下,第一预测轨道的距离L_m1由下式(1)那样定义。
[数1]
L_m1(k)=V_ego(k)·T_m1…(1)
而且,在将第一预测轨道的曲率半径设为R_m1的情况下,第一预测轨道的旋转角度θ_m1由下式(2)那样定义。该旋转角度θ_m1相当于Y轴与后述的Y’轴之间的角度。
[数2]
θ_m1(k)=L_m1(k)/R_m1(k)…(2)
另外,将对从原点到第一预测位置Z_m1为止的本车轨道进行3等分时的预测位置从原点朝向第一预测位置Z_m1依次设为三个预测位置Z_m11、Z_m12、Z_m13(=Z_m1)。在该情况下,预测位置Z_m11的X坐标值x_m11及Y坐标值y_m11如下式(3)、(4)那样定义。
[数3]
x_m11(k)=R_m1(k)·sin(θ_m1(k)/3)…(3)
[数4]
y_m11(k)=R_m1(k)·(1-cos(θ_m1(k)/3)…(4)
另外,预测位置Z_m12的X坐标值x_m12及Y坐标值y_m12如下式(5)、(6)那样定义。
[数5]
x_m12(k)=R_m1(k)·sin(2θ_m1(k)/3)…(5)
[数6]
y_m12(k)=R_m1(k)·(1-cos(2θ_m1(k)/3)…(6)
而且,预测位置Z_m13的X坐标值x_m13及Y坐标值y_m13如下式(7)、(8)那样定义。
[数7]
x_m13(k)=R_m1(k)·sin(θ_m1(k))…(7)
[数8]
y_m13(k)=R_m1(k)·(1-cos(θ_m1(k))…(8)
以上的式(1)~(8)相当于第一预测轨道的模型式(模型轨道)。
接下来,对第二预测轨道进行说明。首先,定义将第一预测位置Z_m1(=预测位置Z_m13)设为原点、将第一预测轨道的第一预测位置Z_m1处的切线设为X’轴且将与该切线正交的线设为Y’轴的X’-Y’坐标系。
而且,将从该X’-Y’坐标系中的原点即第一预测位置Z_m1起经过第二预测时间T_m2后的位置设为第二预测位置Z_m2,并将从第一预测位置Z_m1到第二预测位置Z_m2为止的本车轨道定义为圆弧状的第二预测轨道。在该情况下,第二预测轨道的距离L_m2如下式(9)那样定义。
[数9]
L_m2(k)=V_ego(k)·T_m2…(9)
而且,在将第二预测轨道的曲率半径设为R_m2的情况下,第二预测轨道的旋转角度θ_m2如下式(10)那样定义。该旋转角度θ_m2相当于Y’轴与后述的Y”轴之间的角度。
[数10]
θ_m2(k)=L_m2(k)/R_m2(k)…(10)
并且,第二预测位置Z_m2的X’坐标值x’_m2及Y’坐标值y’_m2如下式(11)、(12)那样定义。
x′_m2(k)=R_m2(k)·sin(θ_m2(k))…(11)
[数12]
y′_m2(k)=R_m2(k)·(1-cos(θ_m2(k))…(12)
而且,第二预测位置Z_m2的X坐标值x_m2及Y坐标值y_m2如下式(13)、(14)那样定义。
[数13]
x_m2(k)=x′_m2(k)·cos(θ_m1(k))-y′_m2(k)·sin(θ_m1(k))+x_m13(k)…(13)
[数14]
y_m2(k)=x′_m2(k)·sin(θ_m1(k))+y′_m2(k)·cos(θ_m1(k))+y_m13(k)…(14)
以上的式(9)~(14)相当于第二预测轨道的模型式(模型轨道)。
接下来,对第三预测轨道进行说明。首先,定义将第二预测位置Z_m2设为原点、将第二预测轨道的第二预测位置Z_m2处的切线设为X”轴且将与该切线正交的线设为Y”轴的X”-Y”坐标系。
而且,将从该X”-Y”坐标系中的原点(即第二预测位置Z_m2)起经过第三预测时间T_m3后的位置设为第三预测位置Z_m3,并将从第二预测位置Z_m2到第三预测位置Z_m3为止的本车轨道定义为圆弧状的第三预测轨道。在该情况下,第三预测轨道的距离L_m3如下式(15)那样定义。
[数15]
L_m3(k)=V_eqo(k)·T_m3…(15)
而且,在将第三预测轨道的曲率半径设为R_m3的情况下,第三预测轨道的旋转角度θ_m3如下式(16)那样定义。
[数16]
θ_m3(k)=L_m3(k)/R_m3(k)…(16)
并且,第三预测位置Z_m3的X”坐标值x”_m3及Y”坐标值y”_m3如下式(17)、(18)那样定义。
[数17]
x″_m3(k)=R_m3(k)·sin(θ_m3(k))…(17)
[数18]
y″_m3(k)=R_m3(k)·(1-cos(θ_m3(k))…(18)
而且,第三预测位置Z_m3的X坐标值x_m3及Y坐标值y_m3如下式(19)、(20)那样定义。
[数19]
x_m3(k)=x″_m3(k)·cos(θ_m1(k)+θ_m2(k))-y″_m2(k)·sin(θ_m1(k)+θ_m2(k))+x_m2(k)…(19)
[数20]
y_m3(k)=x″_m3(k)·sin(θ_m1(k)+θ_m2(k))+y″_m3(k)·cos(θ_m1(k)+θ_m2(k))+y_m2(k)…(20)
以上的式(15)~(20)相当于第三预测轨道的模型式(模型轨道)。
如以上那样,第一预测轨道被决定为将比第二预测轨道及第三预测轨道短的三个预测轨道组合而成的轨道。具体而言,第一预测轨道被决定为,将从当前位置到预测位置Z_m11为止的轨道、从预测位置Z_m11到预测位置Z_m12为止的轨道、从预测位置Z_m12到预测位置Z_m13为止的轨道组合而成的轨道。
这是因为,第一预测轨道与第二预测轨道及第三预测轨道相比,由于与本车辆3的距离近,所以需要以能够更可靠地躲避与干涉对象的干涉的方式生成预测轨道,以实现该目的。需要说明的是,也可以将第一预测轨道决定为组合了2个或4个以上的预测轨道而成的轨道。
接下来,对前述的第一~第三评价函数计算部21~23进行说明。在这些第一~第三评价函数计算部21~23中,使用如以上那样被决定的5个预测位置Z_m11、Z_m12、Z_m13、Z_m2、Z_m3,分别计算第一评价函数J_1、第二评价函数J_2及第三评价函数J_3,来作为表示本车辆3与干涉对象的干涉程度的函数。
在此,第一评价函数J1、第二评价函数J_2及第三评价函数J_3如后述那样,在5个预测位置Z_m11、Z_m12、Z_m13、Z_m2、Z_m3的计算中使用,因此基于5个预测位置Z_m11、Z_m12、Z_m13、Z_m2、Z_m3的上次值,计算第一评价函数J1、第二评价函数J_2及第三评价函数J_3的本次值。
需要说明的是,在以下的说明中,将5个预测位置Z_m11、Z_m12、Z_m13、Z_m2、Z_m3统称为“预测位置Z_mi”。即,“预测位置Z_mi”的尾标i表示i=11~13、2、3。
以下,在第一~第三评价函数计算部21~23中,对第一~第三评价函数J_1~J_3的计算中使用的方法及其原理进行说明。
首先,对潜在风险R_p_mi_vj_v的计算方法进行说明。该潜在风险R_p_mi_vj_v表示在假设本车辆3位于预测位置Z_mi且交通参加者等干涉对象位于预测位置W_vj的情况下的、本车辆3与干涉对象的干涉程度。
首先,x坐标偏差dx_mi_vj及y坐标偏差dy_mi_vj由下式(21)、(22)计算。
[数21]
dx_mi_vj(k)=x_mi(k)-x_vj(k)…(21)
[数22]
dy_mi_vj(k)=y_mi(k)-y_vj(k)…(22)
这些偏差dx_mi_vj、dy_mi_vj表示本车辆3的预测位置Z_mi与干涉对象的预测位置W_vj的偏差。在该情况下,例如,在本车辆3的预测位置为Z_mi=Z_m11的情况下,计算其与干涉对象的预测位置W_vj(k:k+km11)的偏差。
接着,由下式(23)、(24)计算变换x坐标偏差dx_mi_vj_v及变换y坐标偏差dy_mi_vj_v。这些偏差dx_mi_vj_v、dy_mi_vj_v是将上述两个偏差dx_mi_vj、dy_mi_vj变换到干涉对象的坐标系的偏差。
[数23]
dx_mi_vj_v(k)=dx_mi_vj(k)·cos(θ_vj(k))+dy_mi_vj(k)·sin(θ_vj(k))…(23)
[数24]
dy_mi_vj_v(k)=dy_mi_vj(k)·cos(θ_vj(k))-dx_mi_vj(k)·sin(θ_vj(k))…(24)
接下来,根据变换x坐标偏差dx_mi_vj_v,检索图7所示的映射,从而计算x方向潜在风险R_px_mi_vj_v。该x方向潜在风险R_px_mi_vj_v的映射值的横向宽度及形状等根据干涉对象的种类(例如,行人、自行车、四轮车辆、摩托车等)来设定。
而且,根据变换y坐标偏差dy_mi_vj_v,检索图8所示的映射,从而计算y方向潜在风险R_py_mi_vj_v。该y方向潜在风险R_py_mi_vj_v的映射值的横向宽度及形状等也与x方向潜在风险R_px_mi_vj_v的映射值同样,根据干涉对象的种类(例如,行人、自行车、四轮车辆、摩托车等)来设定。
并且,如下式(25)所示,作为x方向潜在风险R_px_mi_vj_v与y方向潜在风险R_py_mi_vj_v的积而计算出潜在风险R_p_mi_vj_v。
[数25]
R_p_mi_vj_v(k)=R_px_mi_vj_v(k)·R_py_mi_vj_v(k)…(25)
通过以上的方法,本车辆3与干涉对象的干涉程度越大,则计算出的潜在风险R_p_mi_vj_v的值越大。
接下来,对右侧行驶路边界潜在风险R_p_rbr_mi及左侧行驶路边界潜在风险R_p_rbl_mi的计算方法进行说明。这两个潜在风险R_p_rbr_mi、R_p_rbl_mi是表示假设本车辆3位于预测位置Z_mi的情况下的、本车辆3发生行驶路脱离的可能性的值。
首先,在本车辆3的行驶路的右侧边界线LR及左侧边界线LL成为例如如图9所示的情况下,这些边界线LR、LL能够定义为下式(26)、(27)所示的两个一次函数。
[数26]
y(k)=a_rbr(k)·x(k)+b_rbr(k)…(26)
[数27]
y(k)=a_rbl(k)·x(k)+b_rbl(k)…(27)
上式(26)中的a_rbr、b_rbr是规定右侧边界线LR的参数,上式(27)中的a_rbl、b_rbl是规定左侧边界线LL的参数。这4个参数a_rbr、b_rbr、a_rbl、b_rbl基于前述的信息检测装置4的检测结果,被实时地计算。
接下来,将右侧行驶路边界函数σ_r及左侧行驶路边界函数σ_l如下式(28)、(29)所示那样定义。
[数28]
σ_r(k)=y(k)-a_rbr(k)·x(k)-b_rbr(k)…(28)
[数29]
σ_l(k)=y(k)-a_rbl(k)·x(k)-b_rbl(k)…(29)
这些式(28)、(29)相当于将上式(26)、(27)的左边移项到右边。因此,在右侧行驶路边界函数σ_r为正值的情况下,本车辆3的中心存在于比右侧边界线LR靠左侧的行驶路内的区域,在右侧行驶路边界函数σ_r为负值的情况下,本车辆3的中心存在于比右侧边界线LR靠右侧的行驶路外的区域。
与此相同地,在左侧行驶路边界函数σ_l为正值的情况下,本车辆3的中心存在于比左侧边界线LL靠左侧的行驶路外的区域,在左侧行驶路边界函数σ_l为负值的情况下,本车辆3的中心存在于比左侧边界线LL靠右侧的行驶路内的区域。
基于以上的原理,右侧行驶路边界潜在风险R_p_rbr由下式(30)、(31)计算,左侧行驶路边界潜在风险R_p_rbl由下式(32)、(33)计算。
[数30]
·σ_r(k)≤σ_mrg时
R_p_rbr(k)=|σ_r(k)-σ_mrg|…(30)
[数31]
·σ_mrg<o_r(k)时
R_p_rbr(k)=0…(31)
[数32]
·-σ_mrg≤σ_l(k)时
R_p_rbl(k)=|σ_l(k)+σ_mrg|…(32)
[数33]
·σ_l(k)<-σ_mrg时
R_p_rbl(k)=0…(33)
以上的式(30)~(33)中的σ_mrg是用于使本车辆3能够可靠地避免行驶路脱离的余量(正的规定值)。在此,在将右侧行驶路边界潜在风险R_p_rbr与右侧行驶路边界函数σ_r的关系及左侧行驶路边界潜在风险R_p_rbl与左侧行驶路边界函数σ_l的关系图式化了的情况下,图10、11示出了上述关系。
接下来,通过与以上相同的原理,将本车辆3的预测位置Z_mi处的右侧行驶路边界函数σ_r_mi及左侧行驶路边界函数σ_l_mi如下式(34)、(35)所示那样定义。
[数34]
σ_r_mi(k)=y_mi(k)-a_rbr(k)·x_mi(k)-b_rbr(k)…(34)
[数35]
σ_l_mi(k)=y_mi(k)-a_rbl(k)·x_mi(k)-b_rbl(k)…(35)
而且,通过前述的原理,本车辆3的预测位置Z_mi处的右侧行驶路边界潜在风险R_p_rbr_mi由下式(36)、(37)计算,本车辆3的预测位置Z_mi处的左侧行驶路边界潜在风险R_p_rbl_mi由下式(38)、(39)计算。
[数36]
·σ_r_mi(k)≤σ_mrg(k)时
R_p_rbr_mi(k)=|σ_r_mi(k)-σ_mrg|…(36)
[数37]
·σ_mrg<σ_r_mi(k)时
R_ρ_rbr_mi(k)=0…(37)
[数38]
·-σ_mrg≤σ_l_mi(k)时
R_p_rbl_mi(k)=|σ_l_mi(k)+σ_mrg|…(38)
[数39]
·σ_l_mi(k)<-σ_mrg时
R_p_rbl_mi(k)=0…(39)
需要说明的是,在本车辆3在存在中央隔离带或中心线的道路上行驶中的情况下,通过与以上同样的方法,能够计算相对于中央隔离带或中心线的行驶路边界潜在风险。
接下来,对前述的第一评价函数计算部21中的第一评价函数J_1的计算方法进行说明。该第一评价函数计算部21的计算中使用的各种参数的尾标“i”表示i=11~13。
在第一评价函数计算部21中,基于前述的潜在风险R_p_mi_vj_v、右侧行驶路边界潜在风险R_p_rbr_mi及左侧行驶路边界潜在风险R_p_rbl_mi的计算方法及计算原理,如以下所述那样,使用来自轨道决定部30的三个施加预测位置Z_mi_r,计算第一评价函数J_1。这些施加预测位置Z_mi_r如后述那样是对前述的三个预测位置Z_mi施加了后述的参照输入r而成的位置。
在第一评价函数计算部21中,由于使用对预测位置Z_mi施加了参照输入r的施加预测位置Z_mi_r,因此,在以下的说明中,与施加预测位置Z_mi_r同样,使用在前述的各种参数的最后附加了“_r”的参数。这一点在第二评价函数计算部22及第三评价函数计算部23中也同样。
在第一评价函数计算部21中,首先,根据下式(40)、(41),计算当前的控制时刻下的x坐标偏差dx_mi_vj_r(k:k)及y坐标偏差dy_mi_vj_r(k:k)。
[数40]
dx_mi_vj_r(k:k)=x_mi_r(k)-x_vj(k:k)…(40)
[数41]
dy_mi_vj_r(k:k)=y_mi_r(k)-y_vj(k:k)…(41)
上式(40)的x_mi_r是施加预测位置Z_mi_r的X坐标值,x_vj(k:k)是交通参加者等干涉对象的当前位置W_vj(k:k)的X坐标值。另外,上式(41)的y_mi_r是施加预测位置Z_mi_r的Y坐标值,y_vj(k:k)是干涉对象的当前位置W_vj(k:k)的Y坐标值。
接着,根据下式(42)、(43),计算预测时刻k+kmi下的x坐标偏差dx_mi_vj_r(k:k+kmi)及y坐标偏差dy_mi_vj_r(k:k+kmi)。
[数42]
dx_mi_vj_r(k:k+kmi)=x_mi_r(k)-x_vj(k:k+kmi)…(42)
[数43]
dy_mi_vj_r(k:k+kmi)=y_mi_r(k)-y_vj(k:k+kmi)…(43)
上式(42)的x_vj(k:k+kmi)是干涉对象的预测位置W_vj(k:k+kmi)的X坐标值,上式(43)的y_vj(k:k+kmi)是干涉对象的预测位置W_vj(k:k+kmi)的Y坐标值。
而且,根据下式(44)、(45),计算当前的控制时刻下的变换x坐标偏差dx_mi_vj_v_r(k:k)及变换y坐标偏差dy_mi_vj_v_r(k:k)。
[数44]
dx_mi_vj_v_r(k:k)=dx_mi_vj_r(k:k)·cos(θ_vj(k:k))+dy_mi_vj_r(k:k)·sin(θ_vj(k:k))…(44)
[数45]
dy_mi_vj_v_r(k:k)=dy_mi_vj_r(k:k)·cos(θ_vj(k:k))-dx_mi_vj_r(k:k)·sin(θ_vj(k:k))…(45)
这些偏差dx_mi_vj_v_r、dy_mi_vj_v_r相当于将上述的本车辆3的坐标系中的偏差dx_mi_vj_r、dy_mi_vj_r变换为干涉对象的坐标系的值。
接下来,根据下式(46)、(47),计算预测时刻k+kmi下的变换x坐标偏差dx_mi_vj_v_r(k:k+kmi)及变换y坐标偏差dy_mi_vj_v_r(k:k+kmi)。
[数46]
dx_mi_vj_v_r(k:k+kmi)=dx_mi_vj_r(k:k+kmi)·cos(θ_vj(k:k+kmi))+dy_mi_vj_r(k:k+kmi)·sin(θ_vj(k:k+kmi))…(46)
[数47]
dy_mi_vj_v_r(k:k+kmi)=dy_mi_vj_r(k:k+kmi)·cos(θ_vj(k:k+kmi))-dx_mi_vj_r(k:k+kmi)·sin(θ_vj(k:k+kmi))…(47)
这些偏差dx_mi_vj_v_r(k:k+kmi)、dy_mi_vj_v_r(k:k+kmi)相当于将上述的本车辆3的坐标系中的偏差dx_mi_vj_r(k:k+kmi)、dy_mi_vj_r(k:k+kmi)变换为干涉对象的坐标系的值。
而且,根据前述的变换x坐标偏差dx_mi_vj_v_r(k:k),检索未图示的映射,从而计算x方向潜在风险R_px_mi_vj_v_r(k:k)。在该情况下,作为映射,使用与前述的图7同样的映射。
另外,根据变换y坐标偏差dy_mi_vj_v_r(k:k),检索未图示的映射,从而计算y方向潜在风险R_py_mi_vj_v_r(k:k)。在该情况下,作为映射,使用与前述的图8同样的映射。
接着,根据下式(48),计算当前的控制时刻k下的潜在风险R_p_mi_vj_v_r(k:k)。
[数48]
R_p_mi_vj_v_r(k:k)=R_px_mi_vj_v_r(k:k)·R_py_mi_vj_v_r(k:k)…(48)
而且,根据前述的变换x坐标偏差dx_mi_vj_v_r(k:k+kmi),检索在x方向潜在风险R_px_mi_vj_v_r(k:k)的计算中使用的映射,从而计算x方向潜在风险R_px_mi_vj_v_r(k:k+kmi)。
另外,根据前述的变换y坐标偏差dy_mi_vj_v_r(k:k+kmi),检索在y方向潜在风险R_py_mi_vj_v_r(k:k)的计算中使用的映射,从而计算y方向潜在风险R_py_mi_vj_v_r(k:k+kmi)。
接着,根据下式(49),计算预测时刻k+kmi下的潜在风险R_p_mi_vj_v_r(k:k+kmi)。需要说明的是,在本实施方式中,潜在风险R_p_mi_vj_v_r(k:k)、R_p_mi_vj_v_r(k:k+kmi)相当于干涉程度参数。
[数49]
R_p_mi_vj_v_r(k:k+kmi)=R_px_mi_vj_v_r(k:k+kmi)·R_py_mi_vj_v_r(k:k+kmi)…(49)
另外,本车辆3的预测位置Z_mi处的右侧行驶路边界函数σ_r_mi_r及左侧行驶路边界函数σ_l_mi_r如下式(50)、(51)所示那样定义。
[数50]
σ_r_mi_r(k)=y_mi_r(k)-a_rbr(k)·x_mi_r(k)-b_rbr(k)…(50)
[数51]
σ_l_mi_r(k)=y_mi_r(k)-a_rbl(k)·x_mi_r(k)-b_rbl(k)…(51)
而且,右侧行驶路边界潜在风险R_p_rbr_mi_r由下式(52)、(53)计算,左侧行驶路边界潜在风险R_p_rbl_mi_r由下式(54)、(55)计算。需要说明的是,在本实施方式中,右侧及左侧行驶路边界潜在风险R_p_rbr_mi_r、R_p_rbl_mi_r相当于干涉程度参数。
[数52]
·σ_r_mi_r(k)≤σ_mrg时
R_p_rbr_mi_r(k)=|σ_r_mi_r(k)-σ_mrg|…(52)
[数53]
·σ_mrg<σ_r_mi_r(k)时
R_p_rbr_mi_r(k)=0…(53)
[数54]
·-σ_mrg≤σ_l_mi_r(k)时
R_p_rbl_mi_r(k)=|σ_l_mi_r(k)+σ_mrg|…(54)
[数55]
·σ_l_mi_r(k)<-σ_mrg(k)时
R_p_rbl_mi_r(k)=0…(55)
然后,最终,第一评价函数J1由下式(56)计算。
[数56]
在上式(56)中,n表示位于本车辆3的前方的交通参加者等干涉对象的数量。如该式(56)所示,第一评价函数J1被计算为当前的控制时刻k及预测时刻k+kmi(i=11~13)的、三个预测位置Z_mi(i=11~13)的潜在风险R_p_mi_vj_v_r的总和与右侧及左侧行驶路边界潜在风险R_p_rbr_mi_r、R_p_rbl_mi_r之间的和。
接下来,对前述的第二评价函数计算部22中的第二评价函数J_2的计算方法进行说明。在该第二评价函数计算部22中,如以下所述那样,使用来自轨道决定部30的施加第二预测位置Z_m2_r,通过与第一评价函数计算部21同样的方法,计算第二评价函数J_2。该施加预测位置Z_m2_r是对第二预测位置Z_m2施加了参照输入r的位置。
在第二评价函数计算部22中,首先,根据下式(57)、(58),计算预测时刻k+km2下的x坐标偏差dx_m2_vj_r(k:k+km2)及y坐标偏差dy_m2_vj_r(k:k+km2)。
[数57]
dx_m2_vj_r(k:k+km2)=x_m2_r(k)-x_vj(k:k+km2)…(57)
[数58]
dy_m2_vj_r(k:k+km2)=y_m2_r(k)-y_vj(k:k+km2)…(58)
上式(57)的x_vj(k:k+km2)是干涉对象的预测位置W_vj(k:k+km2)的X坐标值,上式(58)的y_vj(k:k+km2)是干涉对象的预测位置W_vj(k:k+km2)的Y坐标值。
而且,根据下式(59)、(60),计算预测时刻k+km2下的变换x坐标偏差dx_m2_vj_v_r(k:k+km2)及变换y坐标偏差dy_m2_vj_v_r(k:k+km2)。
[数59]
dx_m2_vj_v_r(k:k+km2)=dx_m2_vj_r(k:k+km2)·cos(θ_vj(k:k+km2))+dy_m2_vj_r(k:k+km2)·sin(θ_vj(k:k+km2))…(59)
[数60]
dy_m2_vj_v_r(k:k+km2)=dy_m2_vj_r(k:k+km2)·cos(θ_vj(k:k+km2))-dx_m2_vj_r(k:k+km2)·sin(θ_vj(k:k+km2))…(60)
接下来,根据前述的变换x坐标偏差dx_m2_vj_v_r(k:k+km2),检索未图示的映射,从而计算x方向潜在风险R_px_m2_vj_v_r(k:k+km2)。在该情况下,作为映射,使用与前述的图7同样的映射。
另外,根据前述的变换y坐标偏差dy_m2_vj_v_r(k:k+km2),检索未图示的映射,从而计算y方向潜在风险R_py_m2_vj_v_r(k:k+km2)。在该情况下,作为映射,使用与前述的图8同样的映射。
接着,根据下式(61),计算预测时刻k+km2下的潜在风险R_p_m2_vj_v_r(k:k+km2)。
[数61]
R_p_m2_vj_v_r(k:k+km2)=R_px_m2_vj_v_r(k:k+km2)·R_py_m2_vj_v_r(k:k+km2)…(61)
另外,本车辆3的第二预测位置Z_m2处的右侧行驶路边界函数σ_r_m2_r及左侧行驶路边界函数σ_l_m2_r如下式(62)、(63)所示那样定义。
[数62]
σ_r_m2_r(k)=y_m2_r(k)-a_rbr(k)·x_m2_r(k)-b_rbr(k)…(62)
[数63]
σ_l_m2_r(k)=y_m2_r(k)-a_rbl(k)·x_m2_r(k)-b_rbl(k)…(63)
而且,右侧行驶路边界潜在风险R_p_rbr_m2_r由下式(64)、(65)计算,左侧行驶路边界潜在风险R_p_rbl_m2_r由下式(66)、(67)计算。
[数64]
·σ_r_m2_r(k)≤σ_mrg时
R_p_rbr_m2_r(k)=|σ_r_m2_r(k)-σ_mrg|…(64)
[数65]
·σ_mrg<σ_r_m2_r(k)时
R_p_rbr_m2_r(k)=0…(65)
[数66]
·-σ_mrg≤σ_l_m2_r(k)时
R_p_rbl_m2_r(k)=|σ_l_m2_r(k)+σ_mrg|…(66)
[数67]
·σ_l_m2_r(k)<-σ_mrg时
R_p_rbl_m2_r(k)=0…(67)
然后,最终,第二评价函数J_2由下式(68)计算。
[数68]
如该式(68)所示,第二评价函数J_2被计算为第二预测位置Z_m2处的潜在风险R_p_m2_vj_v_r的总和与右侧及左侧行驶路边界潜在风险R_p_rbr_m2_r、R_p_rbl_m2_r之间的和。
接下来,对前述的第三评价函数计算部23中的第三评价函数J_3的计算方法进行说明。在该第三评价函数计算部23中,如以下所述那样,使用来自轨道决定部30的施加第三预测位置Z_m3_r,通过与第一及第二评价函数计算部21、22同样的方法,计算第三评价函数J_3。该施加预测位置Z_m3_r是对第三预测位置Z_m3施加了参照输入r的位置。
在第三评价函数计算部23中,首先,根据下式(69)、(70),计算预测时刻k+km3下的x坐标偏差dx_m3_vj_r(k:k+km3)及y坐标偏差dy_m3_vj_r(k:k+km3)。
[数69]
dx_m3_vj_r(k:k+km3)=x_m3_r(k)-x_vj(k:k+km3)…(69)
[数70]
dy_m3_vj_r(k:k+km3)=y_m3_r(k)-y_vj(k:k+km3)…(70)
上式(69)的x_vj(k:k+km3)是干涉对象的预测位置W_vj(k:k+km3)的X坐标值,上式(70)的y_vj(k:k+km3)是干涉对象的预测位置W_vj(k:k+km3)的Y坐标值。
而且,根据下式(71)、(72),计算预测时刻k+km3下的变换x坐标偏差dx_m3_vj_v_r(k:k+km3)及变换y坐标偏差dy_m3_vj_v_r(k:k+km3)。
[数71]
dx_m3_vj_v_r(k:k+km3)=dx_m3_vj_r(k:k+km3)·cos(θ_vj(k:k+km3))+dy_m3_vj_r(k:k+km3)·sin(θ_vj(k:k+km3))…(71)
[数72]
dy_m3_vj_v_r(k:k+km3)=dy_m3_vj_r(k:k+km3)·cos(θ_vj(k:k+km3))-dx_m3_vj_r(k:k+km3)·sin(θ_vj(k:k+km3))…(72)
接下来,根据前述的变换x坐标偏差dx_m3_vj_v_r(k:k+km3),检索未图示的映射,从而计算x方向潜在风险R_px_m3_vj_v_r(k:k+km3)。在该情况下,作为映射,使用与前述的图7同样的映射。
另外,根据前述的变换y坐标偏差dy_m3_vj_v_r(k:k+km3),检索未图示的映射,从而计算y方向潜在风险R_py_m3_vj_v_r(k:k+km3)。在该情况下,作为映射,使用与前述的图8同样的映射。
接着,根据下式(73),计算预测时刻k+km3下的潜在风险R_p_m3_vj_v_r(k:k+km3)。
[数73]
R_p_m3_vj_v_r(k:k+km3)=R_px_m3_vj_v_r(k:k+km3)·R_py_m3_vj_v_r(k:k+km3)…(73)
另外,本车辆3的第三预测位置Z_m3处的右侧行驶路边界函数σ_r_m3_r及左侧行驶路边界函数σ_l_m3_r如下式(74)、(75)所示那样定义。
[数74]
σ_r_m3_r(k)=y_m3_r(k)-a_rbr(k)·x_m3_r(k)-b_rbr(k)…(74)
[数75]
σ_l_m3_r(k)=y_m3_r(k)-a_rbl(k)·x_m3_r(k)-b_rbl(k)…(75)
而且,右侧行驶路边界潜在风险R_p_rbr_m3_r由下式(76)、(77)计算,左侧行驶路边界潜在风险R_p_rbl_m3_r由下式(78)、(79)计算。
[数76]
·σ_r_m3_r(k)≤σ_mrg时
R_p_rbr_m3_r(k)=|σ_r_m3_r(k)-σ_mrg|…(76)
[数77]
·σ_mrg<σ_r_m3_r(k)时
R_p_rbr_m3_r(k)=0…(77)
[数78]
·-σ_mrg≤σ_l_m3_r(k)时
R_p_rbl_m3_r(k)=|σ_l_m3_r(k)+σ_mrg|…(78)
[数79]
·σ_l_m3_r(k)<-σ_mrg时
R_p_rbl_m3_r(k)=0…(79)
然后,最终,第三评价函数J_3由下式(80)计算。
[数80]
如该式(80)所示,第三评价函数J_3被计算为第三预测位置Z_m3处的潜在风险R_p_m3_vj_v_r的总和与右侧及左侧行驶路边界潜在风险R_p_rbr_m3_r、R_p_rbl_m3_r之间的和。
接下来,对前述的轨道决定部30进行说明。如图12所示,轨道决定部30具备第一~第三极值搜索控制器31~33及第一~第三预测轨道计算部34~36。在这些第一~第三极值搜索控制器31~33中,通过后述的方法,计算作为第一~第三预测轨道的曲率的预测曲率ρ_m1~ρ_m3。这些预测曲率ρ_m1~ρ_m3相当于前述的图6中的曲率半径R_m1~R_m3的倒数。
以下,对第一~第三极值搜索控制器31~33的计算方法及其原理,以图13所示的极值搜索控制器50为例进行说明。如该图所示,该极值搜索控制器50是这样的控制器,在输入了施加搜索输入u_sk_r时输出评价函数J的系统是搜索对象70的情况下,使用评价函数J,计算搜索输入u_sk及施加搜索输入u_sk_r。
该极值搜索控制器50具备高通滤波器51、参照输入产生器52、两个延迟要素54、58、乘法器53、移动平均滤波器55、放大要素56、以及两个加法要素57、59。
在该高通滤波器51中,根据下式(81),计算滤波器值h。
[数81]
h(k)=J(k)-J(k-1)…(81)
如上式(81)所示,滤波器值h被计算为评价函数J的本次值J(k)与上次值J(k-1)之间的差量。另外,高通滤波器51用于使由包含于评价函数值J中的、后述的参照输入r所引起的频率分量通过。在该情况下,代替上式(81),也可以构成为使用使后述的参照输入r的频率分量通过的滤波器算法(巴特沃斯高通滤波器算法或带通滤波器算法),来计算滤波器值h。
另外,参照输入产生器52产生参照输入r。需要说明的是,作为参照输入r的波形,例如,使用正弦波、余弦波、三角波、梯形波及矩形波等。
而且,该参照输入r经由延迟要素54被输入到乘法器53,在乘法器53中,根据下式(82),计算中间值P_hr。
[数82]
P_hr(k)=h(k)·r(k-1)…(82)
另外,在移动平均滤波器55中,根据下式(83),计算移动平均值P_c。
[数83]
在该式(83)中,为了从移动平均值P_c去除参照输入r的频率分量,移动平均值P_c的抽头数n_r+1被设定为,抽头数n_r+1与控制周期ΔT的积ΔT·(n_r+1)成为与参照输入r的周期Tr相等的值或周期Tr的整数倍。
接着,移动平均值P_c在由放大要素56放大了规定增益K_sk的量的状态下被输入到加法要素57,并且搜索输入u_sk经由延迟要素58被输入到加法要素57。
并且,在加法要素57中,根据下式(84)所示的积分控制算法,计算搜索输入u_sk。
[数84]
u_sk(k)=u_sk(k-1)+K_sk·P_c(k)…(84)
而且,在加法要素59中,根据下式(85),计算施加搜索输入u_sk_r。
[数85]
u_sk_r(k)=u_sk(k)+r(k)…(85)
接下来,使用以上的计算算法,对计算出搜索输入u_sk及施加搜索输入u_sk_r的理由及其原理进行说明。在本实施方式的情况下,如前述那样,以包含各种潜在风险的总和的方式计算评价函数J,因此通过以评价函数J成为极小值的方式计算,能够减少潜在风险的总和。
由于该理由,在本实施方式中,为了以使评价函数J成为极小值的方式计算搜索输入u_sk,使用以下的原理。首先,评价函数J由于使用施加搜索输入u_sk_r计算出,因此因施加搜索输入u_sk_r所含的参照输入r的特性(周期函数)而呈现规定振幅的振动性的举动。
在此,在假设施加搜索输入u_sk_r与评价函数J的关系表现为例如图14所示的曲线的情况下,由参照输入r引起的评价函数J的振动性的举动成为如图中的箭头所示那样具有某倾斜度的状态。需要说明的是,图14的u_sk_op是施加搜索输入u_sk_r的规定值。
另一方面,前述的移动平均值P_c是评价函数J的滤波器值h与参照输入r的乘积值的移动平均值,因此能够视为相当于评价函数J与参照输入r的相关函数的疑似相关函数。
因此,若被视为疑似相关函数的移动平均值P_c是正值,则评价函数J的倾斜度表示正值,若移动平均值P_c是负值,则评价函数J的倾斜度表示负值。除此之外,移动平均值P_c利用前述的式(83)进行计算,由此在除去了参照输入r的频率分量的状态下进行计算。由于以上的理由,移动平均值P_c与施加搜索输入u_sk_r的关系能够表示为例如图15所示的单调增加的函数。即,移动平均值P_c表示在变更了施加搜索输入u_sk_r时评价函数J变化的方向。
因此,在以使评价函数J成为极小值的方式计算施加搜索输入u_sk_r时,以使图15所示的函数的倾斜度成为值0的方式计算移动平均值P_c即可。即,以使移动平均值P_c收敛于值0的方式使用反馈控制算法来计算施加搜索输入u_sk_r、换言之搜索输入u_sk即可。
由于以上的原理,在极值搜索控制器50中,使用式(84)的积分控制算法,作为使评价函数J成为极小值那样的解而计算出搜索输入u_sk。
在本实施方式的第一~第三极值搜索控制器31~33的情况下,基于与以上的极值搜索控制器50相同的原理,计算作为搜索输入u_sk的预测曲率ρ_m1~ρ_m3。
首先,对第一极值搜索控制器31进行说明。第一极值搜索控制器31通过下述方法,计算预测曲率ρ_m1,该预测曲率ρ_m1相当于前述的第一预测轨道的曲率半径R_m1的倒数。
如图12所示,第一极值搜索控制器31具备高通滤波器31a、参照输入产生器31b、两个延迟要素31d、31h、乘法器31c、移动平均滤波器31e、三个放大要素31f、31j、31k及两个加法要素31g、31i。
在该高通滤波器31a中,根据下式(86),计算滤波器值h_1。
[数86]
h_1(k)=J_1(k)-J_1(k-1)…(86)
另外,参照输入产生器31b产生前述的参照输入r。该参照输入r经由延迟要素31d输入到乘法器31c,在乘法器31c中,根据下式(87),计算中间值P_hr1。
[数87]
P_hr1(k)=h_1(k)·r(k-1)…(87)
另外,在移动平均滤波器31e中,根据下式(88),计算移动平均值P_c1。需要说明的是,在本实施方式中,移动平均值P_c1相当于修正量。
[数88]
接着,移动平均值P_c1在由放大要素31f放大了规定增益K_sk1的量的状态下被输入到加法要素31g,后述的移动平均值P_c2在由放大要素31k放大了增益K_sk1·K_bp_21的量的状态下被输入到加法要素31g,并且后述的移动平均值P_c3在由放大要素31j放大了增益K_sk1·K_bp_32的量的状态下被输入到加法要素31g。
在此,在P_c2≥0时,根据移动平均值P_c1,检索图16所示的映射,从而计算增益K_bp_21。图16的Pref1是正的规定值。在该映射中,增益K_bp_21在0<P_c1的范围内被设定为值1,在P_c1<-Pref1的范围内值被设定为0,并且在-Pref1≤P_c1≤0的范围内,被设定为移动平均值P_c1越大,则越接近值1。
另外,在P_c2<0时,根据移动平均值P_c1,检索图17所示的映射,从而计算增益K_bp_21。图17的Pref2是正的规定值,Pref1和Pref2可以是相同的值,也可以是相互不同的值。在该映射中,增益K_bp_21在P_c1<0的范围内被设定为值1,在Pref2<P_c1的范围内被设定为值0,并且在0≤P_c1≤Pref2的范围内,被设定为移动平均值P_c1越大,则越接近值0。
而且,在P_c2≥0&P_c3≥0时,根据移动平均值P_c1,检索图18所示的映射,从而计算增益K_bp_31。在该映射中,增益K_bp_31在0<P_c1的范围内被设定为值1,在P_c1<-Pref1的范围内被设定为值0,并且在-Pref1≤P_c1≤0的范围内,被设定为移动平均值P_c1越大,则越接近值1。
另外,在P_c2<0&P_c3<0时,根据移动平均值P_c1,检索图19所示的映射,从而计算增益K_bp_31。在该映射中,增益K_bp_31在P_c1<0的范围内被设定为值1,在Pref2<P_c1的范围内被设定为值0,并且在0≤P_c1≤Pref2的范围内,被设定为移动平均值P_c1越大,则越接近值0。
而且,在两个移动平均值P_c2、P_c3的符号不同时,增益K_bp_31被计算为值0。
关于两个增益K_bp_21、K_bp_31如以上那样被计算的理由、值P_c2·K_sk1·K_bp_21从第二极值搜索控制器32被输入到第一极值搜索控制器31的理由、值P_c3·K_sk1·K_bp_31从第三极值搜索控制器33被输入到第一极值搜索控制器31的理由,见后述。
而且,预测曲率ρ_m1经由延迟要素31h而被输入到加法要素31g。并且,在加法要素31g中,根据下式(89),计算预测曲率ρ_m1。
[数89]
ρ_m1(k)=ρ_m1(k-1)+K_sk1·(P_c1(k)+K_bp_21(k)·P_c2(k)+K_bp_31(k)·P_c3(k))…(89)
如上式(89)所示,预测曲率ρ_m1通过将值P_c1·K_sk1、值P_c2·K_sk1·K_bp_21、值P_c3·K_sk1·K_bp_31与其上次值ρ_m1(k-1)相加而计算出,因此能通过这些移动平均值P_c1~P_c3而被修正(更新)。即,三个移动平均值P_c1~P_c3作为修正预测曲率ρ_m1的修正量而发挥作用。
而且,在加法要素31i中,根据下式(90),计算施加预测曲率ρ_m1_r。
[数90]
ρ_m1_r(k)=ρ_m1(k)+r(k)…(90)
并且,如以上那样计算出的预测曲率ρ_m1及施加预测曲率ρ_m1_r从第一极值搜索控制器31被输入到第一预测轨道计算部34。
接下来,对第二极值搜索控制器32进行说明。第二极值搜索控制器32通过以下叙述的方法,计算预测曲率ρ_m2及施加预测曲率ρ_m2_r,该预测曲率ρ_m2相当于前述的第二预测轨道的曲率半径R_m2的倒数。
如图12所示,第二极值搜索控制器32具备高通滤波器32a、参照输入产生器32b、两个延迟要素32d、32h、乘法器32c、移动平均滤波器32e、两个放大要素32f、32j及两个加法要素32g、32i。
在该高通滤波器32a中,根据下式(91),计算滤波器值h_2。
[数91]
h_2(k)=J_2(k)-J_2(k-1)…(91)
另外,参照输入产生器32b产生前述的参照输入r。该参照输入r经由延迟要素32d而被输入到乘法器32c,在乘法器32c中,根据下式(92),计算中间值P_hr2。
[数92]
P_hr2(k)=h_2(k)·r(k-1)…(92)
另外,在移动平均滤波器32e中,根据下式(93),计算移动平均值P_c2。需要说明的是,在本实施方式中,移动平均值P_c2相当于修正量。
[数93]
接着,移动平均值P_c2在由放大要素32f放大了规定增益K_sk2的量的状态下被输入到加法要素32g,后述的移动平均值P_c3在由放大要素32j放大了增益K_sk2·K_bp_32的量的状态下被输入到加法要素32g。
在此,在P_c3≥0时,根据移动平均值P_c2,检索图20所示的映射,从而计算增益K_bp_32。图20的Pref3是正的规定值。在该映射中,增益K_bp_32在0<P_c2的范围内被设定为值1,在P_c2<-Pref3的范围内被设定为值0,并且在-Pref3≤P_c2≤0的范围内,被设定为移动平均值P_c2越大,则越接近值1。
另外,在P_c3<0时,根据移动平均值P_c2,检索图21所示的映射,从而计算增益K_bp_32。图21的Pref4是正的规定值。在此,Pref3与Pref4可以是相同的值,也可以是相互不同的值。
在该映射中,增益K_bp_32在P_c2<0的范围内被设定为值1,在Pref4<P_c2的范围内被设定为值0,并且在0≤P_c2≤Pref4的范围内,被设定为移动平均值P_c2越大,则越接近值0。
关于增益K_bp_32如以上那样被计算的理由、值P_c3·K_sk2·K_bp_32从第三极值搜索控制器33向第二极值搜索控制器32输入的理由,见后述。
而且,预测曲率ρ_m2经由延迟要素32h而被输入到加法要素32g。并且,在加法要素32g中,根据下式(94),计算预测曲率ρ_m2。
[数94]
ρ_m2(k)=ρ_m2(k-1)+K_sk2·(P_c2(k)+K_bp_32(k)·P_c3(k))…(94)
如上式(94)所示,预测曲率ρ_m2通过将值P_c2·K_sk2、值P_c3·K_sk2·K_bp_32与其上次值ρ_m2(k-1)相加而计算出,因此能通过这些移动平均值P_c2、P_c3而被修正(更新)。即,两个移动平均值P_c2、P_c3作为修正预测曲率ρ_m2的修正量而发挥作用。
而且,在加法要素32i中,根据下式(95),计算施加预测曲率ρ_m2_r。
[数95]
ρ_m2_r(k)=ρ_m2(k)+r(k)…(95)
并且,如以上那样计算出的预测曲率ρ_m2及施加预测曲率ρ_m2_r从第二极值搜索控制器32被输入到第二预测轨道计算部35。
接下来,对第三极值搜索控制器33进行说明。第三极值搜索控制器33通过以下叙述的方法,计算预测曲率ρ_m3及施加预测曲率ρ_m3_r,该预测曲率ρ_m3相当于前述的第三预测轨道的曲率半径R_m3的倒数。
如图12所示,第三极值搜索控制器33具备高通滤波器33a、参照输入产生器33b、两个延迟要素33d、33h、乘法器33c、移动平均滤波器33e、放大要素33f及两个加法要素33g、33i。
在该高通滤波器33a中,根据下式(96),计算滤波器值h_3。
[数96]
h_3(k)=J_3(k)-J_3(k-1)…(96)
另外,参照输入产生器33b产生前述的参照输入r。该参照输入r经由延迟要素33d而被输入到乘法器33c,在乘法器33c中,根据下式(97),计算中间值P_hr3。
[数97]
P_hr3(k)=h_3(k)·r(k-1)…(97)
另外,在移动平均滤波器33e中,根据下式(98),计算移动平均值P_c3。需要说明的是,在本实施方式中,移动平均值P_c3相当于修正量。
[数98]
接着,移动平均值P_c3在由放大要素33f放大了规定增益K_sk3的量的状态下被输入到加法要素33g,预测曲率ρ_m3经由延迟要素33h被输入到加法要素33g。
并且,在加法要素33g中,根据下式(99),计算预测曲率ρ_m3。
[数99]
ρ_m3(k)=ρ_m3(k-1)+K_sk3·P_c3(k)…(99)
而且,在加法要素33i中,根据下式(100),计算施加预测曲率ρ_m3_r。
[数100]
ρ_m3_r(k)=ρ_m3(k)+r(k)…(100)
并且,如以上那样计算出的预测曲率p_m3及施加预测曲率ρ_m3_r从第三极值搜索控制器33向第三预测轨道计算部36输入。
接下来,对前述的第一预测轨道计算部34进行说明。在该第一预测轨道计算部34中,使用来自第一极值搜索控制器31的预测曲率ρ_m1及施加预测曲率ρ_m1_r,基于前述的原理,计算三个预测位置Z_m11~Z_m13、三个施加预测位置Z_m11_r~Z_m13_r。
首先,根据前述的式(1)~(8),计算预测位置Z_m11的X坐标值x_m11及Y坐标值y_m11、预测位置Z_m12的X坐标值x_m12及Y坐标值y_m12、预测位置Z_m13的X坐标值x_m13及Y坐标值y_m13。
接着,根据下式(101)、(102),计算第一预测轨道的曲率半径R_m1_r。
[数101]
[数102]
·|ρ_m1|≤ρ_min时
R_m1_r(k)=Sign(ρ_m1_r(k))/ρ_min…(102)
上式(101)、(102)的ρ_min是规定的最小曲率(正值)。另外,式(102)的Sign是在G≥0的情况下成为Sign(G)=1且在G<0的情况下成为Sign(G)=-1的符号函数。这一点在其他的数式的符号函数Sign中也同样。
接下来,根据下式(103),计算第一预测轨道的旋转角度θ_m1_r。
[数103]
θ_m1_r(k)=L_m1(k)/R_m1_r(k)…(103)
而且,根据下式(104)、(105),计算施加预测位置Z_m11_r的X坐标值x_m11_r及Y坐标值y_m11_r。
[数104]
x_m11_r(k)=R_m1_r(k)·sin(θ_m1_r(k)/3)…(104)
[数105]
y_m11_r(k)=R_m1_r(k)·(1-cos(θ_m1_r(k)/3)…(105)
接着,根据下式(106)、(107),计算施加预测位置Z_m12_r的X坐标值x_m12_r及Y坐标值y_m12_r。
[数106]
x_m12_r(k)=R_m1_r(k)·sin(2θ_m1_r(k)/3)…(106)
[数107]
y_m12_r(k)=R_m1_r(k)·(1-cos(2θ_m1_r(k)/3)…(107)
接着,根据下式(108)、(109),计算施加预测位置Z_m13_r的X坐标值x_m13_r及Y坐标值y_m13_r。
[数108]
x_m13_r(k)=R_m1_r(k)·sin(θ_m1_r(k))…(108)
[数109]
y_m13_r(k)=R_m1_r(k)·(1-cos(θ_m1_r(k))…(109)
接下来,对前述的第二预测轨道计算部35进行说明。在该第二预测轨道计算部35中,使用来自第二极值搜索控制器32的预测曲率ρ_m2及施加预测曲率ρ_m2_r,基于前述的原理,计算第二预测位置Z_m2及施加第二预测位置Z_m2_r。
首先,根据前述的式(9)~(14),计算第二预测位置Z_m2的X坐标值x_m2及Y坐标值y_m2。
接着,根据下式(110)、(111),计算第二预测轨道的曲率半径R_m2_r。
[数110]
[数111]
·|ρ_m2|≤ρ_min时
R_m2_r(k)=Sign(ρ_m2_r(k))/ρ_min…(111)
接下来,根据下式(112),计算第二预测轨道的旋转角度θ_m2_r。
[数112]
θ_m2_r(k)=L_m2(k)/R_m2_r(k)…(112)
而且,根据下式(113)、(114),计算前述的X’-Y’坐标系(参照图6)中的施加第二预测位置Z_m2_r的X’坐标值x’_m2_r及Y’坐标值y’_m2_r。
[数113]
x′_m2_r(k)=R_m2_r(k)·sin(θ_m2_r(k))…(113)
[数114]
y′_m2_r(k)=R_m2_r(k)·(1-cos(θ_m2_r(k))…(114)
接着,根据下式(115)、(116),计算X-Y坐标系(参照图6)中的施加第二预测位置Z_m2_r的X坐标值x_m2_r及Y坐标值y_m2_r。
[数115]
x_m2_r(k)=x′_m2_r(k)·cos(θ_m1(k))-y′_m2_r(k)·sin(θ_m1(k))+x_m13(k)…(115)
[数116]
y_m2_r(k)=x′_m2_r(k)·sin(θ_m1(k))+y′_m2_r(k)·cos(θ_m1(k))+y_m13(k)…(116)
接下来,对前述的第三预测轨道计算部36进行说明。在该第三预测轨道计算部36中,使用来自第三极值搜索控制器33的预测曲率ρ_m3及施加预测曲率ρ_m3_r,基于前述的原理,计算第三预测位置Z_m3及施加第三预测位置Z_m3_r。
首先,根据前述的式(15)~(20),计算第三预测位置Z_m3的X坐标值x_m3及Y坐标值y_m3。
接着,根据下式(117)、(118),计算第三预测轨道的曲率半径R_m3_r。
[数117]
·|ρ_m3|>ρ_min时
R_m3_r(k)=1/ρ_m3_r(k)…(117)
[数118]
·|ρ_m3|≤ρ_min时
R_m3_r(k)=Sign(ρ_m3_r(k))/ρ_min…(118)
接下来,根据下式(119),计算第三预测轨道的旋转角度θ_m3_r。
[数119]
θ_m3_r(k)=L_m3(k)/R_m3_r(k)…(119)
而且,根据下式(120)、(121),计算前述的X”-Y”坐标系(参照图6)中的施加第三预测位置Z_m3_r的X”坐标值x”_m3_r及Y”坐标值y”_m3_r。
[数120]
x′_m3_r(k)=R_m3_r(k)·sin(θ_m3_r(k))…(120)
[数121]
y′_m3_r(k)=R_m3_r(k)·(1-cos(θ_m3_r(k))…(121)
接着,根据下式(122)、(123),计算X-Y坐标系(参照图6)中的施加第三预测位置Z_m3_r的X坐标值x_m3_r及Y坐标值y_m3_r。
[数122]
x_m3_r(k)=x′_m3_r(k)·cos(θ_m1(k)+θ_m2(k))-y′_m3_r(k)·sin(θ_m1(k)+θ_m2(k))+x_m2(k)…(122)
[数123]
y_m3_r(k)=x′_m2_r(k)·cos(θ_m1(k)+θ_m2(k))+y′_m2_r(k)·cos(θ_m1(k)+θ_m2(k))+y_m2(k)…(123)
如以上那样,在轨道决定部30中,通过第一~第三极值搜索控制器31~33,计算第一~第三预测轨道的预测曲率ρ_m1~ρ_m3,基于这些预测曲率ρ_m1~ρ_m3,计算前述的5个预测位置Z_mi(i=11~13、2、3)及5个施加预测位置Z_mi_r。
接下来,对前述的值P_c2·K_sk1·K_bp_21及值P_c3·K_sk1·K_bp_31被输入到第一极值搜索控制器31的理由、值P_c3·K_sk2·K_bp_32被输入到第二极值搜索控制器32的理由、三个增益K_bp_21、K_bp_31、K_bp_32如前述那样设定的理由进行说明。
例如,在第一~第三极值搜索控制器31~33中,在相互无关地计算出预测曲率ρ_m1~ρ_m3的情况下,有时会产生如下所述那样的状态。
即,如图22所示,在本车辆3的右前方存在干涉对象的情况下,第一预测轨道(当前位置~预测位置Z_m13)、第二预测轨道(预测位置Z_m13~第二预测位置Z_m2)被决定为直线状,而另一方面,从第二预测位置Z_m2到第三预测位置Z_m3为止的第三预测轨道被决定为圆弧状。
在这样决定了预测轨道的情况下,由于本车辆3在达到第二预测位置Z_m2的时刻成为急剧地向左方转弯的状态,因此由于横加速度急速增加,而成为不稳定的行驶状态。除此之外,根据本车辆3的运动性能,不能沿着第三预测轨道转弯,用于躲避与干涉对象之间的干涉的余量有可能减少。
为了消除以上的问题,在轨道决定部30中,例如,如图23所示,在第一预测轨道、第二预测轨道及第三预测轨道被决定为相同的转弯方向的情况下,即,在三个移动平均值P_c1~P_c3的符号为彼此相同的情况下,需要以反映两个移动平均值P_c2、P_c3的方式计算预测曲率ρ_m1,并且以反映移动平均值P_c3的方式计算预测曲率ρ_m2。
另一方面,例如,如图24所示,在第一预测轨道和第二预测轨道被决定相反的转弯方向,第二预测轨道和第三预测轨道被决定为相反的转弯方向的情况下,即,移动平均值P_c1、P_c2的符号的正负不同,并且移动平均值P_c2、P_c3的符号的正负不同的情况下,在以反映两个移动平均值P_c2、P_c3的方式计算预测曲率ρ_m1时,会减少针对干涉对象的躲避余量。而且,在以反映移动平均值P_c3的方式计算预测曲率ρ_m2的情况下,也会产生相同的问题。因此,需要以不反映两个移动平均值P_c2、P_c3的方式计算预测曲率ρ_m1,并且以不反映移动平均值P_c3的方式计算预测曲率ρ_m2。
由于以上的理由,构成为两个增益K_bp_21、K_bp_31如前述那样设定,并且前述的值P_c2·K_sk1·K_bp_21及值P_c3·K_sk1·K_bp_31被输入到第一极值搜索控制器31。
由于相同的理由,构成为增益K_bp_32如前述那样设定,并且值P_c3·K_sk2·K_bp_32被输入到第二极值搜索控制器32。
根据以上的结构,三个移动平均值P_c1~P_c3的符号、两个值P_c2·K_bp_21、P_c3·K_bp_31、相对于预测曲率ρ_m1的移动平均值P_c2、P_c3的反映状态之间的关系成为图25所示的12个模式A1~A12。在该情况下,在前述的图23的状态中,成为模式A1(或A2),在前述的图24的状态中,成为模式A9~A12中的任意一个。
另外,两个移动平均值P_c2、P_c3的符号、值P_c3·K_bp_32、相对于预测曲率ρ_m2的移动平均值P_c3的反映状态之间的关系为图26所示的6个模式B1~B6。
如以上那样,在本实施方式的轨道决定部30中,计算5个预测位置Z_m11~Z_m13、Z_m2、Z_m3之后,这些值被输入到车辆控制部40。
在车辆控制部40中,根据5个预测位置Z_m11~Z_m13、Z_m2、Z_m3及当前速度V_ego,控制原动机5及致动器6。由此,本车辆3沿着由5个预测位置Z_m11~Z_m13、Z_m2、Z_m3决定的第一~第三预测轨道行驶。
如以上那样,根据第一实施方式的轨道生成装置1,在第一~第三预测轨道的每一个中,计算表示与交通参加者等干涉对象的干涉程度的各种潜在风险R_p_mi_vj_v_r、R_p_rbr_mi_r、R_p_rbl_mi_r,以包含这些潜在风险R_p_mi_vj_v_r、R_p_rbr_mi_r、R_p_rbl_mi_r的总和的方式计算第一~第三评价函数J_1~J_3。
并且,以使这些第一~第三评价函数J_1~J_3成为极小值的方式计算预测曲率ρ_m1~ρ_m3,基于这些预测曲率ρ_m1~ρ_m3,决定第一~第三预测轨道。即,5个预测位置Z_mi(i=11~13、2、3)被以使第一~第三评价函数J_1~J_3成为极小值的方式决定。
在该情况下,如上述那样,第一~第三评价函数J_1~J_3被以包含表示与干涉对象的干涉程度的潜在风险R_p_mi_vj_v_r的总和的方式计算出,因此那样的第一~第三评价函数J_1~J_3被决定为成为极小值,由此能够将第一~第三预测轨道决定为减少与干涉对象的干涉程度。
而且,预测曲率ρ_m1在第一极值搜索控制器31中的移动平均值P_c1与第二极值搜索控制器32中的移动平均值P_c2为相同符号的情况下,以反映移动平均值P_c2的方式决定(参照图23及图25)。
另外,预测曲率ρ_m2在第二极值搜索控制器32中的移动平均值P_c2与第三极值搜索控制器33中的移动平均值P_c3为相同符号的情况下,以反映移动平均值P_c3的方式决定(参照图23及图26)。
在该情况下,三个移动平均值P_c1~P_c3被计算为用于分别修正三个预测曲率ρ_m1~ρ_m3的修正量,即用于修正第一~第三预测轨道的修正量。因此,在第一预测轨道及第二预测轨道的修正量的符号相同的情况下,第一预测轨道被向与比其远离本车辆3的当前位置的第二预测轨道的修正方向相同的方向进一步修正。
与此相同地,在第二预测轨道及第三预测轨道的修正量的符号相同的情况下,第二预测轨道被向与比其远离本车辆3的当前位置的第三预测轨道的修正方向相同的方向进一步修正。
在此,在干涉对象存在于本车辆3的行进方向的情况下,远离当前位置的预测轨道与接近本车辆3的当前位置的预测轨道相比,成为更靠近干涉对象的轨道。因此,以反映用于修正接近干涉对象的预测轨道的修正量的方式生成远离干涉对象(即接近本车辆3)的预测轨道,由此能够在接近本车辆3的预测轨道中,进一步使用于躲避与干涉对象的干涉的躲避余量增大。其结果为,即使在能够实现的轨道受到限制的本车辆3中,也能够以能充分地确保用于躲避与干涉对象的干涉的余量的方式适当地生成本车辆3的未来的轨道。
另一方面,在相邻的两个预测轨道的修正量为不同符号的情况下,两个预测轨道为了躲避干涉对象而被向相互不同的方向修正。因此,在那样的条件下,在将用于修正远离本车辆3的当前位置的预测轨道的修正量反映到接近当前位置的预测轨道时,在接近当前位置的预测轨道中,会减少针对干涉对象的躲避余量。
与此相对地,在本实施方式的情况下,在移动平均值P_c1与移动平均值P_c2为不同符号的情况下,在不反映移动平均值P_c2的情况下决定预测曲率ρ_m1(参照图24及图25)。与此相同地,在移动平均值P_c2与移动平均值P_c3为不同符号的情况下,在不反映移动平均值P_c3的情况下决定预测曲率ρ_m2(参照图24及图26)。由此,即使在接近当前位置的预测轨道中,也能够充分地确保用于躲避与干涉对象的干涉的余量。
另外,第一预测轨道被决定为将比第二预测轨道及第三预测轨道短的三个预测轨道组合而成。由此,即使在与本车辆3的距离比第二预测轨道及第三预测轨道近的第一预测轨道中,也能够以能更可靠地躲避与干涉对象的干涉的方式生成第一预测轨道。
需要说明的是,第一实施方式是将本车辆3作为移动体的例子,但也可以取而代之,将二轮车辆、机器人、航空器或船舶等作为移动体。
而且,第一实施方式是将本发明的轨道生成装置1搭载在能够自动驾驶的四轮车辆3的例子,但也可以取而代之,将轨道生成装置搭载在四轮车辆以外的移动体(例如,二轮车辆、机器人、航空器或船舶等),也可以单独使用轨道生成装置。
另外,第一实施方式是在交通参加者检测部10中使用规定的机器学习算法(例如,使用了DNN及CNN等神经网络的算法)计算了交通参加者的当前检测位置W_vj(k)的例子,但取而代之,也可以使用图像识别算法等,来计算交通参加者的当前检测位置W_vj(k)。
而且,第一实施方式是在行动预测部11中使用规定的机器学习算法(例如,使用了DNN及CNN等神经网络的算法)计算出5个预测位置W_v1(k:k+km11)、W_v1(k:k+km12)、W_v1(k:k+km13)、W_v1(k:k+km2)、W_v1(k:k+km3)的例子,但取而代之,也可以使用神经网络以外的机器学习算法,计算5个预测位置W_v1(k:k+km11)、W_v1(k:k+km12)、W_v1(k:k+km13)、W_v1(k:k+km2)、W_v1(k:k+km3)。
另外,第一实施方式是使用了各种潜在风险作为干涉程度参数的例子,但本发明的干涉程度参数并不局限于此,只要是表示本车辆3的周边的干涉对象与本车辆3的干涉程度的参数即可。
第一实施方式是在轨道决定部30中计算预测曲率ρ_mi,并基于该预测曲率ρ_mi来计算出预测位置Z_mi的例子,但也可以构成为取代预测曲率ρ_mi而计算专利第6580087号公报的图18记载那样的线段与角度的组合的预测值,并基于这些预测值来计算预测位置Z_mi。
接下来,对第二实施方式的轨道生成装置100(参照图27)进行说明。本实施方式的轨道生成装置100为了执行本车辆3的自动驾驶控制,通过下述的算法,决定存在于本车辆3的前方的交通参加者的预测轨道。需要说明的是,在本实施方式中,本车辆3相当于第一移动体,交通参加者相当于第二移动体。
在此,作为交通参加者,相当于行人、自行车、四轮车及二轮摩托车等能够移动的物体,以下,以交通参加者为行人M1(参照图29)的情况为例进行说明,并且将对行人M1的移动进行干涉的对象称为“干涉对象”。在该情况下,行人M1以外的交通参加者及交通环境(道路环境、信号机的状态等)相当于干涉对象。
如图27所示,轨道生成装置100具备交通参加者检测部110、行动预测部111、第一~第三评价函数计算部121~123及轨道决定部200。这些要素100~111、121~123、200具体而言由ECU2构成,这些要素100~111、121~123、200中的运算处理以前述的规定的控制周期ΔT执行。
需要说明的是,在本实施方式中,第一~第三评价函数计算部121~123相当于干涉程度参数计算部,轨道决定部200相当于预测轨道生成部及修正量决定部。
首先,在交通参加者检测部110中,以与前述的第一实施方式的交通参加者检测部10同样的方法,检测/计算行人M1的当前检测位置、行人M1以外的交通参加者的当前检测位置W_vj(k)。首先,将以仅一个行人的交通参加者TP4存在于行人M1的前方的情况(即j=1的情况下)为例进行说明。
在本实施方式的情况下,行人M1的坐标系如图28所示,将行人M1的中心设为原点,并将行人M1的行进方向及横向定义为X轴及Y轴。需要说明的是,图中的Z2_m1h0是行人M1的当前位置,Z2_m11、Z2_m12、Z2_m13是行人M1的后述的预测位置。
另外,交通参加者TP4的坐标系被定义为将交通参加者TP4的行进方向及横向设为X_v轴及Y_v轴且将X_v轴相对于X轴的倾斜度设为θ2_v1的正交二维坐标系。
在该正交二维坐标系中,在将X_v坐标值设为x2_v1且将Y_v坐标值设为y2_v1的情况下,交通参加者TP4的当前检测位置W2_v1(k)被计算为以三个值[x2_v1(k),y2_v1(k),θ2_v1(k)]为要素的值。另外,图28中的交通参加者TP4的周围中的点划线所示的线模拟地表示后述的潜在风险。
在交通参加者检测部111中,通过与前述的第一实施方式的交通参加者检测部10同样的方法,计算交通参加者TP4的当前位置W2_v1(k:k)、5个预测位置W2_v1(k:k+km11)、W2_v1(k:k+km12)、W2_v1(k:k+km13)、W2_v1(k:k+km2)、W2_v1(k:k+km3)。
如以上那样,在交通参加者检测部111中,在交通参加者为一个(1人)的情况下,计算交通参加者的6个位置(当前位置及5个预测位置),在交通参加者为n个的情况下,计算n×6个位置。这一点在前述的各要素121~123、200的运算中也同样。以下,以交通参加者为一个的情况为例进行说明。
接下来,对本实施方式的轨道生成装置100中的行人M1的未来的预测轨道的定义及其决定原理进行说明。如图29所示,在本实施方式的轨道生成装置100中,与第一实施方式的轨道生成装置1同样地,行人M1的未来的预测轨道被决定为将三个圆弧状的第一~第三预测轨道组合而成的轨道。
最初,对第一预测轨道进行说明。首先,如前述那样,定义将行人M1的当前位置的中心设为原点、将行人M1的行进方向设为X轴且将行人M1的横向设为Y轴的X-Y坐标系。
并且,将从该X-Y坐标系中的原点起经过第一预测时间T2_m1后的位置设为第一预测位置Z2_m1,并将从原点到第一预测位置Z2_m1为止的预测轨道定义为圆弧状的第一预测轨道。在此,在将行人M1的当前速度设为V_trgt的情况下,第一预测轨道的距离L2_m1如下式(124)那样定义。
[数124]
L2_m1(k)=V_trgt(k)·T2_m1…(124)
而且,在将第一预测轨道的曲率半径设为R2_m1的情况下,第一预测轨道的旋转角度θ2_m1如下式(125)那样定义。该旋转角度θ2_m1相当于Y轴与后述的Y’轴之间的角度。
[数125]
θ2_m1(k)=L2_m1(k)/R2_m1(k)…(125)
另外,行人M1在规定时刻Th1前(例如,一个控制周期前)的推定移动距离L2_mh1如下式(126)那样定义。该推定移动距离L2_mh1是推定为行人M1在从规定时刻Th1前的控制时刻到本次的控制时刻为止的期间沿着将第一预测轨道延长而得到的圆弧轨道移动时的移动距离。
[数126]
L2_m1h1(k)=V_trgt(k)·ΔT…(126)
而且,行人M1的规定时刻Th1前的推定位置Z2_m1h1处的旋转角度θ2_mh1如下式(127)那样定义。该规定时刻Th1前的推定位置Z2_m1h1是推定为行人M1在从规定时刻Th1前的控制时刻到本次的控制时刻为止的期间沿着将第一预测轨道延长而得到的圆弧轨道移动时的规定时刻Th1前的控制时刻下的位置。
[数127]
θ2_mh1(k)=-L2_m1h1(k)/R2_m1(k)…(127)
而且,行人M1的规定时刻Th2前(例如,两个控制周期前)的推定移动距离L2_mh2如下式(128)那样定义。该推定移动距离L2_mh2是推定为行人M1在从规定时刻Th2前的控制时刻到本次的控制时刻为止的期间沿着将第一预测轨道延长而得到的圆弧轨道移动时的移动距离。
[数128]
L2_m1h2(k)=V_trgt(k)·2ΔT…(128)
而且,行人M1的规定时刻Th2前的推定位置Z2_m1h2处的旋转角度θ2_mh2如下式(129)那样定义。规定时刻Th2前的推定位置Z2_m1h2是推定为行人M1在从规定时刻Th2前的控制时刻到本次的控制时刻为止的期间沿着将第一预测轨道延长而得到的圆弧轨道移动时的规定时刻Th2前的控制时刻下的位置。
[数129]
θ2_mh2(k)=-L2_m1h2(k)/R2_m1(k)…(129)
另一方面,将对从原点到第一预测位置Z2_m1为止的预测轨道进行3等分时的预测位置从原点朝向第一预测位置Z2_m1依次设为三个预测位置Z2_m11、Z2_m12、Z2_m13(=Z2_m1)。在该情况下,预测位置Z2_m11的X坐标值x2_m11及Y坐标值y2_m11如下式(130)、(131)那样定义。
[数130]
x2_m11(k)=R2_m1(k)·sin(θ2_m1(k)/3)…(130)
[数131]
y2_m11(k)=R2_m1(k)·(1-cos(θ2_m1(k)/3)…(131)
另外,预测位置Z2_m12的X坐标值x2_m12及Y坐标值y2_m12如下式(132)、(133)那样定义。
[数132]
x2_m12(k)=R2_m1(k)·sin(2θ2_m1(k)/3)…(132)
[数133]
y2_m12(k)=R2_m1(k)·(1-cos(2θ2_m1(k)/3)…(133)
而且,预测位置Z2_m13的X坐标值x2_m13及Y坐标值y2_m13如下式(134)、(135)那样定义。
[数134]
x2_m13(k)=R2_m1(k)·sin(θ2_m1(k))…(134)
[数135]
y2_m13(k)=R2_m1(k)·(1-cos(θ2_m1(k))…(135)
另一方面,当前位置Z2_m1h0的X坐标值x2_m1h0及Y坐标值y2_m1h0如下式(136)、(137)那样定义。
[数136]
x2_m1h0(k)=0…(136)
[数137]
y2_m1h0(k)=0…(137)
另外,行人M1的规定时刻Th1前的推定位置Z2_m1h1的X坐标值x2_m1h1及Y坐标值y2_m1h1如下式(138)、(139)那样定义。
[数138]
x2_m1h1(k)=R2_m1(k)·sin(θ2_mh1(k))…(138)
[数139]
y2_m1h1(k)=R2_m1(k)·(1-cos(θ2_mh1(k))…(139)
而且,行人M1的规定时刻Th2前的推定位置Z2_m1h2的X坐标值x2_m1h2及Y坐标值y2_m1h2如下式(140)、(141)那样定义。
[数140]
x2_m1h2(k)=R2_m1(k)·sin(θ2_mh2(k))…(140)
[数141]
y2_m1h2(k)=R2_m1(k)·(1-cos(θ2_mh2(k))…(141)
以上的式(124)~(125)、(130)~(135)相当于第一预测轨道的模型式(模型轨道)。
接下来,对第二预测轨道进行说明。首先,定义将第一预测位置Z2_m1(=预测位置Z2_m13)设为原点、将第一预测轨道的第一预测位置Z2_m1处的切线设为X’轴且将与该切线正交的线设为Y’轴的X’-Y’坐标系。
而且,将从该X’-Y’坐标系中的原点即第一预测位置Z2_m1起经过第二预测时间T2_m2后的位置设为第二预测位置Z2_m2,并将从第一预测位置Z2_m1到第二预测位置Z2_m2为止的预测轨道定义为圆弧状的第二预测轨道。在该情况下,第二预测轨道的距离L2_m2如下式(142)那样定义。
[数142]
L2_m2(k)=V_trgt(k)·T2_m2…(142)
而且,在将第二预测轨道的曲率半径设为R2_m2的情况下,第二预测轨道的旋转角度θ2_m2如下式(143)那样定义。该旋转角度θ2_m2相当于Y’轴与后述的Y”轴之间的角度。
[数143]
θ2_m2(k)=L2_m2(k)/R2_m2(k)…(143)
并且,第二预测位置Z2_m2的X’坐标值x2’_m2及Y’坐标值y2’_m2如下式(144)、(145)那样定义。
[数144]
x2′_m2(k)=R2_m2(k)·sin(θ2_m2(k))…(144)
[数145]
y2′_m2(k)=R2_m2(k)·(1-cos(θ2_m2(k))…(145)
而且,第二预测位置Z2_m2的X坐标值x2_m2及Y坐标值y2_m2如下式(146)、(147)那样定义。
[数146]
x2_m2(k)=x2′_m2(k)·cos(θ2_m1(k))-y2′_m2(k)·sin(θ2_m1(k))+x2_m13(k)…(146)
[数147]
y2_m2(k)=x2′_m2(k)·sin(θ2_m1(k))+y2′_m2(k)·cos(θ2_m1(k))+y2_m13(k)…(147)
以上的式(142)~(147)相当于第二预测轨道的模型式(模型轨道)。
接下来,对第三预测轨道进行说明。首先,定义将第二预测位置Z2_m2设为原点、将第二预测轨道的第二预测位置Z2_m2处的切线设为X”轴且将与该切线正交的线设为Y”轴的X”-Y”坐标系。
而且,将从该X”-Y”坐标系中的原点即第二预测位置Z2_m2起经过第三预测时间T2_m3后的位置设为第三预测位置Z2_m3,将从第二预测位置Z2_m2到第三预测位置Z2_m3为止的预测轨道定义为圆弧状的第三预测轨道。在该情况下,第三预测轨道的距离L2_m3如下式(148)那样定义。
[数148]
L2_m3(k)=V_trgt(k)·T2_m3…(148)
而且,在将第三预测轨道的曲率半径设为R2_m3的情况下,第三预测轨道的旋转角度θ2_m3如下式(149)那样定义。
[数149]
θ2_m3(k)=L2_m3(k)/R2_m3(k)…(149)
并且,第三预测位置Z2_m3的X”坐标值x2”_m3及Y”坐标值y2”_m3如下式(150)、(151)那样定义。
[数150]
x2″_m3(k)=R2_m3(k)·sin(θ2_m3(k))…(150)
[数151]
y2″_m3(k)=R2_m3(k)·(1-cos(θ2_m3(k))…(151)
而且,第三预测位置Z2_m3的X坐标值x2_m3及Y坐标值y2_m3如下式(152)、(153)那样定义。
[数152]
x2_m3(k)=x2″_m3(k)·cos(θ2_m1(k)+θ2_m2(k))-y2″_m2(k)·sin(θ2_m1(k)+θ2_m2(k))+x2_m2(k)…(152)
[数153]
y2_m3(k)=x2″_m3(k)·sin(θ2_m1(k)+θ2_m2(k))+y2″_m3(k)·cos(θ2_m1(k)+θ2_m2(k))+y2_m2(k)…(153)
以上的式(148)~(153)相当于第三预测轨道的模型式(模型轨道)。
接下来,对前述的第一~第三评价函数计算部121~123进行说明。在这些第一~第三评价函数计算部121~123中,使用以上的5个预测位置Z2_m11、Z2_m12、Z2_m13、Z2_m2、Z2_m3、当前位置Z2_m1h0、两个推定位置Z2_m1h1、Z2_m1h2,分别计算第一评价函数J2_1、第二评价函数J2_2及第三评价函数J2_3,来作为表示行人M1与干涉对象的干涉程度的函数。
在此,第一评价函数J2_1、第二评价函数J2_2及第三评价函数J2_3如后述那样,在5个预测位置Z2_m11、Z2_m12、Z2_m13、Z2_m2、Z2_m3的计算中使用,因此第一评价函数J2_1、第二评价函数J2_2及第三评价函数J2_3的本次值被基于5个预测位置Z2_m11、Z2_m12、Z2_m13、Z2_m2、Z2_m3的上次值而计算出。
需要说明的是,在以下的说明中,将5个预测位置Z2_m11、Z2_m12、Z2_m13、Z2_m2、Z2_m3统称为“预测位置Z2_mi”。即,“预测位置Z2_mi”的尾标i表示i=11~13、2、3。
以下,在第一~第三评价函数计算部121~123中,对第一~第三评价函数J2_1~J2_3的计算中使用的方法及其原理进行说明。
首先,对潜在风险R2_p_mi_vj_v的计算方法进行说明。该潜在风险R2_p_mi_vj_v表示在假设行人M1位于预测位置Z2_mi且交通参加者位于预测位置W2_vj的情况下的、行人M1与交通参加者的干涉程度。
首先,x坐标偏差dx2_mi_vj及y坐标偏差dy2_mi_vj由下式(154)、(155)计算。
[数154]
dx2_mi_vj(k)=x2_mi(k)-x2_vj(k)…(154)
[数155]
dy2_mi_vj(k)=y2_mi(k)-y2_vj(k)…(155)
这些偏差dx2_mi_vj、dy2_mi_vj表示行人M1的预测位置Z2_mi与交通参加者的预测位置W2_vj的偏差。
接着,变换x坐标偏差dx2_mi_vj_v及变换y坐标偏差dy2_mi_vj_v由下式(156)、(157)计算。这些偏差dx2_mi_vj_v、dy2_mi_vj_v是将上述两个偏差dx2_mi_vj、dy2_mi_vj变换到交通参加者的坐标系的偏差。
[数156]
dx2_mi_vj_v(k)=dx2_mi_vj(k)·cos(θ2_vj(k))+dy2_mi_vj(k)·sin(θ2_vj(k))…(156)
[数157]
dy2_mi_vj_v(k)=dy2_mi_vj(k)·cos(θ2_vj(k))-dx2_mi_vj(k)·sin(θ2_vj(k))…(157)
接下来,根据变换x坐标偏差dx2_mi_vj_v,检索图30所示的映射,从而计算x方向潜在风险R2_px2_mi_vj_v。该x方向潜在风险R2_px2_mi_vj_v的映射值的横向宽度及形状等根据干涉对象的种类(例如,行人、自行车、四轮车辆、摩托车等)来设定。
而且,根据变换y坐标偏差dy2_mi_vj_v,检索图31所示的映射,从而计算y方向潜在风险R2_py2_mi_vj_v。该y方向潜在风险R2_py2_mi_vj_v的映射值的横向宽度及形状等也与x方向潜在风险R2_px2_mi_vj_v的映射值同样,根据干涉对象的种类(例如,行人、自行车、四轮车辆、摩托车等)来设定。
并且,如下式(158)所示,作为x方向潜在风险R2_px2_mi_vj_v与y方向潜在风险R2_py2_mi_vj_v的积而计算出潜在风险R2_p_mi_vj_v。
[数158]
R2_p_mi_vj_v(k)=R2_px_mi_vj_v(k)·R2_py_mi_vj_v(k)…(158)
通过以上的方法,行人M1与交通参加者的干涉程度越大,则计算出的潜在风险R2_p_mi_vj_v的值越大。
接下来,对右侧行驶路边界潜在风险R_p_rbr_mi及左侧行驶路边界潜在风险R_p_rbl_mi的计算方法进行说明。需要说明的是,在本实施方式的说明中,将行人M1移动的人行道等路径称为“行驶路”。这两个潜在风险R2_p_rbr_mi、R2_p_rbl_mi是表示假设行人M1位于预测位置Z2_mi的情况下的、行人M1发生行驶路脱离的可能性的值。
首先,在行人M1的行驶路的右侧边界线LR2及左侧边界线LL2为例如如图32所示的的情况下,这些边界线LR2、LL2能够定义为下式(159)、(160)所示的两个一次函数。
[数159]
y(k)=a2_rbr(k)·x(k)+b2_rbr(k)…(159)
[数160]
y(k)=a2_rbl(k)·x(k)+b2_rbl(k)…(160)
上式(159)中的a2_rbr、b2_rbr是规定右侧边界线LR2的参数,上式(160)中的a2_rbl、b2_rbl是规定左侧边界线LL2的参数。这4个参数a2_rbr、b2_rbr、a2_rbl、b2_rbl基于前述的信息检测装置4的检测结果,被实时地计算。
接下来,将右侧行驶路边界函数σ2_r及左侧行驶路边界函数σ2_l如下式(161)、(162)所示那样定义。
[数161]
σ2_r(k)=y(k)-a2_rbr(k)·x(k)-b2_rbr(k)…(161)
[数162]
σ2_l(k)=y(k)-a2_rbl(k)·x(k)-b2_rbl(k)…(162)
这些式(161)、(162)相当于将上式(159)、(160)的左边移项到右边。因此,在右侧行驶路边界函数σ2_r为正值的情况下,行人M1的中心存在于比右侧边界线LR靠左侧的行驶路内的区域,在右侧行驶路边界函数σ2_r为负值的情况下,行人M1的中心存在于比右侧边界线LR靠右侧的行驶路外的区域。
与此相同地,在左侧行驶路边界函数σ2_l为正值的情况下,行人M1的中心存在于比左侧边界线LL靠左侧的行驶路外的区域,在左侧行驶路边界函数σ2_l为负值的情况下,行人M1的中心存在于比左侧边界线LL靠右侧的行驶路内的区域。
基于以上的原理,右侧行驶路边界潜在风险R2_p_rbr由下式(163)、(164)计算,左侧行驶路边界潜在风险R2_p_rbl由下式(165)、(166)计算。
[数163]
·σ2_r(k)≤σ2_mrg时
R2_p_rbr(k)=Ω_cross(a_rbr(k))·|σ2_r(k)-σ2_mrg|…(163)
[数164]
·σ2_mrg<σ2_r(k)时
R2_p_rbr(k)=0…(164)
[数165]
·-σ2_mrg≤σ2_l(k)时
R2_p_rbl(k)=Ω_cross(a2_rbl(k))·|σ2_l(k)+σ2_mrg|…(165)
[数166]
·σ2_l(k)<-σ2_mrg时
R2_p_rbl(k)=0…(166)
以上的式(163)~(166)中的σ2_mrg是用于使行人M1能够可靠地避免行驶路脱离的余量(正的规定值)。在此,在将右侧行驶路边界潜在风险R2_p_rbr与右侧行驶路边界函数σ2_r的关系及左侧行驶路边界潜在风险R2_p_rbl与左侧行驶路边界函数σ2_l的关系图式化了的情况下,图33、34示出了上述关系。
另外,上式(163)的Ω_cross(a2_rbr(k))是横穿意图参数,并且根据规定前述的右侧边界线LR2的参数a2_rbr,检索图35所示的映射来计算。在该图中,ax1、ax2是0<ax1<ax2成立的参数a2_rbr的规定值,Ω_1是横穿意图参数Ω_cross的正的规定值。
如图35所示,在该映射中,横穿意图参数Ω_cross在参数a2_rbr位于-ax1≤a2_rb≤≤ax1的范围内的情况下,设定为规定值Ω_1,在参数a2_rbr位于a2_rbr≤-ax2、ax2≤a2_rbr的范围内的情况下,设定为值0。而且,横穿意图参数Ω_cross在参数a2_rbr位于-ax2<a2_rbr<-ax1的范围内的情况下,被设定为参数a2_rbr越大,则越向规定值Ω_1增大,在参数a2_rbr位于ax1<a2_rbr<ax2的范围内情况下,被设定为参数a2_rbr越大,则其值越向0减少。
而且,上式(165)的Ω_cross(a2_rbl(k))也是横穿意图参数,并且根据规定前述的左侧边界线LL2的参数a2_rbl,检索图36所示的映射来计算。在该映射中,横穿意图参数Ω_cross相对于参数a2_rbl的值与图35同样地设定。
在上式(163)、(165)中使用这两个值Ω_cross(rbr(k))、Ω_cross(rbl(k))是基于以下理由。例如,如图37所示,在行人M1从人行道朝向人行横道行进时,在使用省略了上式(163)、(165)的值Ω_cross(rbr(k))、Ω_cross(rbl(k))的式子的情况下,由于两个行驶路边界函数σ2_r、σ2_l的效果,导致预测轨道被决定为留在人行道内。
因此,在本实施方式的情况下,避免图37所示那样的现象,如图38所示,在行人M1从人行道朝向人行横道行进时,使用两个值Ω_cross(rbr(k))、Ω_cross(rbl(k))以朝向人行横道侧的状态决定预测轨道。
接下来,根据与以上相同的原理,将行人M1的预测位置Z2_mi中的右侧行驶路边界函数σ2_r_mi及左侧行驶路边界函数σ2_l_mi如下式(167)、(168)所示那样定义。
[数167]
σ2_r_mi(k)=y2_mi(k)-a2_rbr(k)·x2_mi(k)-b2_rbr(k)…(167)
[数168]
σ2_l_mi(k)=y2_mi(k)-a2_rbl(k)·x2_mi(k)-b2_rbl(k)…(168)
而且,通过前述的原理,行人M1的预测位置Z2_mi处的右侧行驶路边界潜在风险R2_p_rbr_mi由下式(169)、(170)计算,行人M1的预测位置Z2_mi处的左侧行驶路边界潜在风险R2_p_rbl_mi由下式(171)、(172)计算。
[数169]
·σ2_r_mi(k)≤σ2_mrg时
R2_p_rbr_mi(k)=Ω_cross(a2_rbr(k))·|σ2_r_mi(k)-σ2_mrg|…(169)
[数170]
·σ2_mrg<σ2_r_mi(k)时
R2_p_rbr_mi(k)=0…(170)
[数171]
·-σ2_mrg≤σ2_l_mi(k)时
R2_p_rbl_mi(k)=Ω_cross(a2_rbl(k))·|σ2_l_mi(k)+σ2_mrg|…(171)
[数172]
·σ2_l_mi(k)<-σ2_mrg时
R2_p_rbl_mi(k)=0…(172)
接下来,参照图39对跟踪位势(trace potential)Tr的计算方法进行说明。在该图中,Z2_h1是规定时刻Th1前的控制时刻的行人M1的实测位置,Z2_h2是规定时刻Th2前的控制时刻的行人M1的实测位置。
另外,如前述那样,图中的规定时刻Th1前的推定位置Z2_m1h1是推定为行人M1在从规定时刻Th1前到本次的控制时刻为止的期间沿着将第一预测轨道延长而得到的圆弧轨道移动时的规定时刻Th1前的控制时刻下的位置,规定时刻Th2前的推定位置Z2_m1h2是推定为行人M1在从规定时刻Th2前到本次的控制时刻为止的期间沿着将第一预测轨道延长而得到的圆弧轨道移动时的规定时刻Th2前的控制时刻下的位置。
在本实施方式中,在第一预测轨道的计算中,为了确保第一预测轨道对行人M1的两个实测位置Z2_h1、Z2_h2的跟随性,通过下述的方法计算跟踪位势Tr。
首先,根据下式(173)、(174),计算规定时刻Th1前的x坐标偏差δxh1及规定时刻Th1前的y坐标偏差δyh1。
[数173]
δxh1(k)=x2_m1h1(k)-x2_h1(k)…(173)
[数174]
δyh1(k)=y2_m1h1(k)-y2_h1(k)…(174)
接着,根据下式(175)、(176),计算规定时刻Th2前的x坐标偏差δxh2及规定时刻Th2前的y坐标偏差δyh2。
[数175]
δxh2(k)=x2_m1h2(k)-x2_h2(k)…(175)
[数176]
δyh2(k)=y2_m1h2(k)-y2_h2(k)…(176)
并且,根据下式(177),计算跟踪位势Tr。
[数177]
如上式(177)所示,跟踪位势Tr被计算为相当于两个推定位置Z2_m1h1、Z2_m1h2、两个实测位置Z2_h1、Z2_h2的平方和误差的值。因此,跟踪位势Tr越小,则两个推定位置Z2_m1h1、Z2_m1h2与两个实测位置Z2_h1、Z2_h2之间的差量越小。
需要说明的是,也可以如以下所述那样计算跟踪位势Tr。例如,也可以为,除了规定时刻Th1前及规定时刻Th2前的推定位置Z2_m1h1、Z2_m1h2,还计算比规定时刻Th2靠前的时刻即规定时刻Th3前的推定值,除了规定时刻Th1前及规定时刻Th2前的行人M1的实测位置Z2_h1、Z2_h2,还取得规定时刻Th3前的行人M1的实测位置,将三个以上的推定值的每一个与三个以上的实测位置的每一个的偏差的平方和用作跟踪位势Tr。
接下来,对前述的第一评价函数计算部121中的第一评价函数J2_1的计算方法进行说明。该第一评价函数计算部121的计算中使用的各种参数的尾标“i”表示i=11~13。
在第一评价函数计算部121中,基于前述的潜在风险R2_p_mi_vj_v、右侧行驶路边界潜在风险R2_p_rbr_mi及左侧行驶路边界潜在风险R2_p_rbl_mi的计算方法及计算原理,如以下所述那样,使用来自轨道决定部200的三个施加预测位置Z2_mi_r,计算第一评价函数J2_1。这些施加预测位置Z2_mi_r如后述那样是对前述的三个预测位置Z2_mi施加了参照输入r而成的位置。
在第一评价函数计算部121中,由于使用对预测位置Z2_mi施加了参照输入r的施加预测位置Z2_mi_r,因此,在以下的说明中,与施加预测位置Z2_mi_r同样,使用在前述的各种参数的最后附加了“_r”的参数。这一点在第二评价函数计算部122及第三评价函数计算部123中也同样。
在第一评价函数计算部121中,首先,根据下式(178)、(179),计算当前的控制时刻下的x坐标偏差dx2_mi_vj_r(k:k)及y坐标偏差dy2_mi_vj_r(k:k)。
[数178]
dx2_mi_vj_r(k:k)=x2_mi_r(k)-x2_vj(k:k)…(178)
[数179]
dy2_mi_vj_r(k:k)=y2_mi_r(k)-y2_vj(k:k)…(179)
上式(178)的x2_mi_r是施加预测位置Z2_mi_r的X坐标值,x2_vj(k:k)是交通参加者的当前位置W2_vj(k:k)的X坐标值。另外,上式(179)的y2_mi_r是施加预测位置Z2_mi_r的Y坐标值,y2_vj(k:k)是交通参加者的当前位置W2_vj(k:k)的Y坐标值。
接着,根据下式(180)、(181),计算预测时刻k+kmi下的x坐标偏差dx2_mi_vj_r(k:k+kmi)及y坐标偏差dy2_mi_vj_r(k:k+kmi)。
[数180]
dx2_mi_vj_r(k:k+kmi)=x2_mi_r(k)-x2_vj(k:k+kmi)…(180)
[数181]
dy2_mi_vj_r(k:k+kmi)=y2_mi_r(k)-y2_vj(k:k+kmi)…(181)
上式(180)的x2_vj(k:k+kmi)是交通参加者的预测位置W2_vj(k:k+kmi)的X坐标值,上式(181)的y2_vj(k:k+kmi)是交通参加者的预测位置W2_vj(k:k+kmi)的Y坐标值。
而且,根据下式(182)、(183),计算当前的控制时刻下的变换x坐标偏差dx2_mi_vj_v_r(k:k)及变换y坐标偏差dy2_mi_vj_v_r(k:k)。
[数182]
dx2_mi_vj_v_r(k:k)=dx2_mi_vj_r(k:k)·cos(θ2_vj(k:k))+dy2_mi_vj_r(k:k)·sin(θ2_vj(k:k))…(182)
[数183]
dy2_mi_vj_v_r(k:k)=dy2_mi_vj_r(k:k)·cos(θ2_vj(k:k))-dx2_mi_vj_r(k:k)·sin(θ2_vj(k:k))…(183)
这些偏差dx2_mi_vj_v_r、dy2_mi_vj_v_r相当于将上述的行人M1的坐标系中的偏差dx2_mi_vj_r、dy2_mi_vj_r变换为交通参加者的坐标系的值。
接下来,根据下式(184)、(185),计算预测时刻k+kmi下的变换x坐标偏差dx2_mi_vj_v_r(k:k+kmi)及变换y坐标偏差dy2_mi_vj_v_r(k:k+kmi)。
[数184]
dx2_mi_vj_v_r(k:k+kmi)=dx2_mi_vj_r(k:k+kmi)·cos(θ2_vj(k:k+kmi))+dy2_mi_vj_r(k:k+kmi)·sin(θ2_vj(k:k+kmi))···(184)
[数185]
dy2_mi_vj_v_r(k:k+kmi)=dy2_mi_vj_r(k:k+kmi)·cos(θ2_vj(k:k+kmi))-dx2_mi_vj_r(k:k+kmi)·sin(θ2_vj(k:k+kmi))···(185)
这些偏差dx2_mi_vj_v_r(k:k+kmi)、dy2_mi_vj_v_r(k:k+kmi)相当于将上述的行人M1的坐标系中的偏差dx2_mi_vj_r(k:k+kmi)、dy2_mi_vj_r(k:k+kmi)变换为交通参加者的坐标系的值。
而且,根据前述的变换x坐标偏差dx2_mi_vj_v_r(k:k),检索未图示的映射,从而计算x方向潜在风险R2_px2_mi_vj_v_r(k:k)。在该情况下,作为映射,使用与前述的图30同样的映射。
另外,根据变换y坐标偏差dy2_mi_vj_v_r(k:k),检索未图示的映射,从而计算y方向潜在风险R2_py2_mi_vj_v_r(k:k)。在该情况下,作为映射,使用与前述的图31同样的映射。
接着,根据下式(186),计算当前的控制时刻k下的潜在风险R2_p_mi_vj_v_r(k:k)。
[数186]
Re_p_mi_vj_v_r(k:k)=R2_px_mi_vj_v_r(k:k)·R2_py_mi_vj_v_r(k:k)…(186)
而且,根据前述的变换x坐标偏差dx2_mi_vj_v_r(k:k+kmi),检索在x方向潜在风险R2_px2_mi_vj_v_r(k:k)的计算中使用的映射,从而计算x方向潜在风险R2_px2_mi_vj_v_r(k:k+kmi)。
另外,根据前述的变换y坐标偏差dy2_mi_vj_v_r(k:k+kmi),检索在y方向潜在风险R2_py2_mi_vj_v_r(k:k)的计算中使用的映射,从而计算y方向潜在风险R2_py2_mi_vj_v_r(k:k+kmi)。
接着,根据下式(187),计算预测时刻k+kmi下的潜在风险R2_p_mi_vj_v_r(k:k+kmi)。需要说明的是,在本实施方式中,潜在风险R2_p_mi_vj_v_r(k:k)、R2_p_mi_vj_v_r(k:k+kmi)相当于干涉程度参数。
[数187]
R2_p_mi_vj_v_r(k:k+kmi)=R2_px_mi_vj_v_r(k;k+kmi)·R2_py_mi_vj_v_r(k:k+kmi)…(187)
另外,行人M1的预测位置Z2_mi处的右侧行驶路边界函数σ2_r_mi_r及左侧行驶路边界函数σ2_l_mi_r如下式(188)、(189)所示那样定义。
[数188]
σ2_r_mi_r(k)=y2_mi_r(k)-a2_rbr(k)·x2_mi_r(k)-b2_rbr(k)…(188)
[数189]
σ2_l_mi_r(k)=y2_mi_r(k)-a2_rbl(k)·x2_mi_r(k)-b2_rbl(k)…(189)
而且,右侧行驶路边界潜在风险R2_p_rbr_mi_r由下式(190)、(191)计算,左侧行驶路边界潜在风险R2_p_rbl_mi_r由下式(192)、(193)计算。需要说明的是,在本实施方式中,右侧及左侧行驶路边界潜在风险R2_p_rbr_mi_r、R2_p_rbl_mi_r相当于干涉程度参数。
[数190]
·σ2_r_mi_r(k)≤σ2_mrg时
R2_p_rbr_mi_r(k)=Ω_cross(a2_rbr(k))·|σ2_r(k)-σ2_mrg|…(190)
[数191]
·σ2_mrg<σ2_r_mi_r(k)时
R2_p_rbr_mi_r(k)=0…(191)
[数192]
·-σ2_mrg≤σ2_l_mi_r(k)时
R2_p_rbl_mi_r(k)=Ω_cross(a2_rbl(k))·|σ2_l(k)+σ2_mrg|…(192)
[数193]
·σ2_l_mi_r(k)<-σ2_mrg时
R2_p_rbl_mi_r(k)=0…(193)
接着,根据下式(194)、(195),计算规定时刻Th1前的x坐标偏差δxh1_r及规定时刻Th1前的y坐标偏差δyh1_r。
[数194]
δxh1_r(k)=x2_m1h1_r(k)-x2_h1(k)…(194)
[数195]
δyh1_r(k)=y2_m1h1_r(k)-y2_h1(k)…(195)
而且,根据下式(196)、(197),计算规定时刻Th2前的x坐标偏差δxh2_r及规定时刻Th2前的y坐标偏差δyh2_r。
[数196]
δxh2_r(k)=x2_m1h2_r(k)-x2_h2(k)…(196)
[数197]
δyh2_r(k)=y2_m1h2_r(k)-y2_h2(k)…(197)
另外,根据下式(198),计算跟踪位势Tr_r。
[数198]
如以上的式(194)~(198)所示,跟踪位势Tr_r被计算为相当于两个推定位置Z2_m1h1_r、Z2_m1h2_r、两个实测位置Z2_h1、Z2_h2的平方和误差的值。因此,跟踪位势Tr越小,则两个推定位置Z2_m1h1_r、Z2_m1h2_r与两个实测位置Z2_h1、Z2_h2之间的差量越小。
并且,最终,第一评价函数J2_1由下式(199)计算。
[数199]
在上式(199)中,ω_tr是规定的权重系数,n表示位于行人M1的前方的干涉对象的数量。如该式(199)所示,第一评价函数J2_1被计算为当前的控制时刻k及预测时刻k+kmi(i=11~13)的、三个预测位置Z2_mi(i=11~13)的潜在风险R2_p_mi_vj_v_r的总和与右侧及左侧行驶路边界潜在风险R2_p_rbr_mi_r、R2_p_rbl_mi_r、权重系数与跟踪位势的积ω_tr·Tr_r之间的和。
接下来,对前述的第二评价函数计算部122中的第二评价函数J2_2的计算方法进行说明。在该第二评价函数计算部122中,如以下所述那样,使用来自轨道决定部200的施加第二预测位置Z2_m2_r,通过与第一评价函数计算部121大致同样的方法,计算第二评价函数J2_2。该施加预测位置Z2_m2_r是对第二预测位置Z2_m2施加了参照输入r的位置。
在第二评价函数计算部122中,首先,根据下式(200)、(201),计算预测时刻k+km2下的x坐标偏差dx2_m2_vj_r(k:k+km2)及y坐标偏差dy2_m2_vj_r(k:k+km2)。
[数200]
dx2_m2_vj_r(k:k+km2)=x2_m2_r(k)-x2_vj(k:k+km2)…(200)
[数201]
dy2_m2_vj_r(k:k+km2)=y2_m2_r(k)-y2_vj(k:k+km2)…(201)
上式(200)的x2_vj(k:k+km2)是交通参加者的预测位置W2_vj(k:k+km2)的X坐标值,上式(201)的y2_vj(k:k+km2)是交通参加者的预测位置W2_vj(k:k+km2)的Y坐标值。
而且,根据下式(202)、(203),计算预测时刻k+km2下的变换x坐标偏差dx2_m2_vj_v_r(k:k+km2)及变换y坐标偏差dy2_m2_vj_v_r(k:k+km2)。
[数202]
dx2_m2_vj_v_r(k:k+km2)=dx2_m2_vj_r(k:k+km2)·cos(θ2_vj(k:k+km2))+dy2_m2_vj_r(k:k+km2)·sin(θ2_vj(k:k+km2))…(202)
[数203]
dy2_m2_vj_v_r(k:k+km2)=dy2_m2_vj_r(k:k+km2)·cos(θ2_vj(k:k+km2))-dx2_m2_vj_r(k:k+km2)·sin(θ2_vj(k:k+km2))…(203)
接下来,根据前述的变换x坐标偏差dx2_m2_vj_v_r(k:k+km2),检索未图示的映射,从而计算x方向潜在风险R2_px2_m2_vj_v_r(k:k+km2)。在该情况下,作为映射,使用与前述的图30同样的映射。
另外,根据前述的变换y坐标偏差dy2_m2_vj_v_r(k:k+km2),检索未图示的映射,从而计算y方向潜在风险R2_py2_m2_vj_v_r(k:k+km2)。在该情况下,作为映射,使用与前述的图31同样的映射。
接着,根据下式(204),计算预测时刻k+km2下的潜在风险R2_p_m2_vj_v_r(k:k+km2)。
[数204]
R2_p_m2_vj_v_r(k:k+km2)=R2_px_m2_vj_v_r(k:k+km2)·R2_py_m2_vj_v_r(k:k+km2)…(204)
另外,行人M1的第二预测位置Z2_m2处的右侧行驶路边界函数σ2_r_m2_r及左侧行驶路边界函数σ2_l_m2_r如下式(205)、(206)所示那样定义。
[数205]
σ2_r_m2_r(k)=y2_m2_r(k)-a2_rbr(k)·x2_m2_r(k)-b2_rbr(k)…(205)
[数206]
σ2_l_m2_r(k)=y2_m2_r(k)-a2_rbl(k)·x2_m2_r(k)-b2_rbl(k)…(206)
而且,右侧行驶路边界潜在风险R2_p_rbr_m2_r由下式(207)、(208)计算,左侧行驶路边界潜在风险R2_p_rbl_m2_r由下式(209)、(210)计算。
[数207]
·σ2_r_m2_r(k)≤σ2_mrg时
R2_p_rbr_m2_r(k)=|σ2_r_m2_r(k)-σ2_mrg|…(207)
[数208]
·σ2_mrg<σ2_r_m2_r(k)时
R2_p_rbr_m2_r(k)=0…(208)
[数209]
·-σ2_mrg≤σ2_l_m2_r(k)时
R2_p_rbl_m2_r(k)=|σ2_l_m2_r(k)+σ2_mrg|…(209)
[数210]
·σ2_l_m2_r(k)<-σ2_mrg时
R2_p_rbl_m2_r(k)=0…(210)
然而,最终,第二评价函数J2_2由下式(211)计算。
[数211]
如该式(211)所示,第二评价函数J2_2被计算为第二预测位置Z2_m2处的潜在风险R2_p_m2_vj_v_r的总和与右侧及左侧行驶路边界潜在风险R2_p_rbr_m2_r、R2_p_rbl_m2_r之间的和。
接下来,对前述的第三评价函数计算部123中的第三评价函数J2_3的计算方法进行说明。在该第三评价函数计算部123中,如以下所述那样,使用来自轨道决定部200的施加第三预测位置Z2_m3_r,通过与第二评价函数计算部122同样的方法,计算第三评价函数J2_3。该施加预测位置Z2_m3_r是对第三预测位置Z2_m3施加了参照输入r的位置。
在第三评价函数计算部123中,首先,根据下式(212)、(213),计算预测时刻k+km3下的x坐标偏差dx2_m3_vj_r(k:k+km3)及y坐标偏差dy2_m3_vj_r(k:k+km3)。
[数212]
dx2_m3_vj_r(k:k+km3)=x2_m3_r(k)-x2_vj(k:k+km3)…(212)
[数213]
dy2_m3_vj_r(k:k+km3)=y2_m3_r(k)-y2_vj(k:k+km3)…(213)
上式(212)的x2_vj(k:k+km3)是交通参加者的预测位置W2_vj(k:k+km3)的X坐标值,上式(213)的y2_vj(k:k+km3)是交通参加者的预测位置W2_vj(k:k+km3)的Y坐标值。
而且,根据下式(214)、(215),计算预测时刻k+km3下的变换x坐标偏差dx2_m3_vj_v_r(k:k+km3)及变换y坐标偏差dy2_m3_vj_v_r(k:k+km3)。
[数214]
dx2_m3_vj_v_r(k:k+km3)=dx2_m3_vj_r(k:k+km3)·cos(θ2_vj(k:k+km3))+dy2_m3_vj_r(k:k+km3)·sin(θ2_vj(k:k+km3))…(214)
[数215]
dy2_m3_vj_v_r(k:k+km3)=dy2_m3_vj_r(k:k+krn3)·cos(θ2_vj(k:k+km3))-dx2_m3_vj_r(k:k+km3)·sin(θ2_vj(k:k+km3))…(215)
接下来,根据前述的变换x坐标偏差dx2_m3_vj_v_r(k:k+km3),检索未图示的映射,从而计算x方向潜在风险R2_px2_m3_vj_v_r(k:k+km3)。在该情况下,作为映射,使用与前述的图30同样的映射。
另外,根据前述的变换y坐标偏差dy2_m3_vj_v_r(k:k+km3),检索未图示的映射,从而计算y方向潜在风险R2_py2_m3_vj_v_r(k:k+km3)。在该情况下,作为映射,使用与前述的图31同样的映射。
接着,根据下式(216),计算预测时刻k+km3下的潜在风险R2_p_m3_vj_v_r(k:k+km3)。
[数216]
R2_p_m3_vj_v_r(k:k+km3)=R2_px_m3_vj_v_r(k:k+km3)·R2_py_m3_vj_v_r(k:k+km3)…(216)
另外,行人M1的第三预测位置Z2_m3处的右侧行驶路边界函数σ2_r_m3_r及左侧行驶路边界函数σ2_l_m3_r如下式(217)、(218)所示那样定义。
[数217]
σ2_r_m3_r(k)=y2_m3_r(k)-a2_rbr(k)·x2_m3_r(k)-b2_rbr(k)…(217)
[数218]
σ2_l_m3_r(k)=y2_m3_r(k)-a2_rbl(k)·x2_m3_r(k)-b2_rbl(k)…(218)
而且,右侧行驶路边界潜在风险R2_p_rbr_m3_r由下式(219)、(220)计算,左侧行驶路边界潜在风险R2_p_rbl_m3_r由下式(221)、(222)计算。
[数219]
·σ2_r_m3_r(k)≤σ2_mrg时
R2_p_rbr_m3_r(k)=|σ2_r_m3_r(k)-σ2_mrg|…(219)
[数220]
·σ2_mrg<σ2_r_m3_r(k)时
R2_p_rbr_m3_r(k)=0…(220)
[数221]
·-σ2_mrg≤σ2_l_m3_r(k)时
R2_p_rbl_m3_r(k)=|σ2_l_m3_r(k)+σ2_mrg|…(221)
[数222]
·σ2_l_m3_r(k)<-σ2_mrg时
R2_p_rbl_m3_r(k)=0…(222)
并且,最终,第三评价函数J2_3由下式(223)计算。
[数223]
如该式(223)所示,第三评价函数J2_3被计算为第三预测位置Z2_m3处的潜在风险R2_p_m3_vj_v_r的总和与右侧及左侧行驶路边界潜在风险R2_p_rbr_m3_r、R2_p_rbl_m3_r之间的和。
接下来,对前述的轨道决定部200进行说明。如图40所示,轨道决定部200具备第一~第三极值搜索控制器210~230及第一~第三预测轨道计算部241~243。
首先,对第一极值搜索控制器210进行说明。该第一极值搜索控制器210如以下所述那样,通过与第一实施方式的第一极值搜索控制器31同样的方法,计算预测曲率ρ2_m1及施加预测曲率ρ2_m1_r。该预测曲率ρ2_m1相当于前述的图29中的第一预测轨道的曲率半径R2_m1的倒数。
如图40所示,第一极值搜索控制器210具备高通滤波器211、参照输入产生器212、两个延迟要素214、218、乘法器213、移动平均滤波器215、三个放大要素216a、216b、216c及两个加法要素217、219。
在该高通滤波器211中,根据下式(224),计算滤波器值h2_1。
[数224]
h2_1(k)=J2_1(k)-J2_1(k-1)…(224)
另外,参照输入产生器212产生前述的参照输入r。该参照输入r经由延迟要素214而被输入到乘法器213,在乘法器213中,根据下式(225),计算中间值P2_hr1。
[数225]
P2_hr1(k)=h2_1(k)·r21(k-1)…(225)
另外,在移动平均滤波器215中,根据下式(226),计算移动平均值P2_c1。需要说明的是,在本实施方式中,移动平均值P2_c1相当于修正量。
[数226]
接着,移动平均值P2_c1在由放大要素216a放大了规定增益K2_sk1的量的状态下被输入到加法要素217,后述的移动平均值P2_c2在由放大要素216c放大了增益K2_sk1·K2_bp_21的量的状态下被输入到加法要素217,并且后述的移动平均值P2_c3在由放大要素216b放大了增益K2_sk1·K2_bp_32的量的状态下被输入到加法要素217。
在此,在P2_c2≥0时,根据移动平均值P2_c1,检索图41所示的映射,从而计算增益K2_bp_21。图41的Pref5是正的规定值。在该映射中,增益K2_bp_21在0<P2_c1的范围内被设定为值1,在P2_c1<-Pref5的范围内被设定为值0,并且在-Pref5≤P2_c1≤0的范围内,被设定为移动平均值P2_c1越大,则越接近值1。
另外,在P2_c2<0时,根据移动平均值P2_c1,检索图42所示的映射,从而计算增益K2_bp2_21。图42的Pref6是正的规定值,Pref5和Pref6可以是相同的值,也可以是相互不同的值。在该映射中,增益K2_bp2_21在P2_c1<0的范围内被设定为值1,在Pref6<P2_c1的范围内被设定为值0,并且在0≤P2_c1≤Pref6的范围内,被设定为移动平均值P2_c1越大,则越接近值0。
而且,在P2_c2≥0&P2_c3≥0时,根据移动平均值P2_c1,检索图43所示的映射,从而计算增益K2_bp2_31。在该映射中,增益K2_bp2_31在0<P2_c1的范围内被设定为值1,在P2_c1<-Pref5的范围内被设定为值0,并且在-Pref5≤P2_c1≤0的范围内,被设定为移动平均值P2_c1越大,则越接近值1。
另外,在P2_c2<0&P2_c3<0时,根据移动平均值P2_c1,检索图44所示的映射,从而计算增益K2_bp2_31。在该映射中,增益K2_bp2_31在P2_c1<0的范围内被设定为值1,在Pref6<P2_c1的范围内被设定为值0,并且在0≤P2_c1≤Pref6的范围内,被设定为移动平均值P2_c1越大,则越接近值0。
而且,在两个移动平均值P2_c2、P2_c3的符号不同时,增益K2_bp2_31被计算为值0。
关于两个增益K2_bp_21、K2_bp_31如以上那样被计算的理由、值P2_c2·K2_sk1·K2_bp_21从第二极值搜索控制器220被输入到第一极值搜索控制器210输入的理由、值P2_c3·K2_sk1·K2_bp_31从第三极值搜索控制器230被输入到第一极值搜索控制器210的理由,见后述。
而且,预测曲率ρ2_m1经由延迟要素218而被输入到加法要素217。并且,在加法要素217中,根据下式(227),计算预测曲率ρ2_m1。
[数227]
ρ2_m1(k)=ρ2_m1(k-1)+K2_sk1·(P2_c1(k)+K2_bp_21(k)·P2_c2(k)+K2_bp_31(k)·P2_c3(k))…(227)
而且,在加法要素219中,根据下式(228),计算施加预测曲率ρ2_m1_r。
[数228]
ρ2_m1_r(k)=ρ2_m1(k)+r(k)…(228)
并且,如以上那样计算出的预测曲率ρ2_m1及施加预测曲率ρ2_m1_r从第一极值搜索控制器210被输入到第一预测轨道计算部241。
接下来,对第二极值搜索控制器220进行说明。第二极值搜索控制器220通过与第一实施方式的前述的第二极值搜索控制器32同样的方法,计算预测曲率p2_m2及施加预测曲率ρ2_m2_r。该预测曲率ρ2_m2相当于前述的第二预测轨道的曲率半径R2_m2的倒数。
如图40所示,第二极值搜索控制器220具备高通滤波器221、参照输入产生器222、两个延迟要素224、228、乘法器223、移动平均滤波器225、两个放大要素226a、226b及两个加法要素227、229。
在该高通滤波器221中,根据下式(229),计算滤波器值h2_2。
[数229]
h2_2(k)=J2_2(k)-J2_2(k-1)…(229)
另外,参照输入产生器222产生参照输入r。该参照输入r经由延迟要素224而被输入到乘法器223,在乘法器223中,根据下式(230),计算中间值P2_hr2。
[数230]
P2_hr2(k)=h2_2(k)·r2_2(k-1)…(230)
另外,在移动平均滤波器225中,根据下式(231),计算移动平均值P2_c2。需要说明的是,在本实施方式中,移动平均值P2_c2相当于修正量。
[数231]
接着,移动平均值P2_c2在由放大要素226a放大了规定增益K2_sk2的量的状态下被输入到加法要素227,后述的移动平均值P2_c3在由放大要素226b放大了增益K2_sk2·K2_bp_32的量的状态下被输入到加法要素227。
在此,在P2_c3≥0时,根据移动平均值P2_c2,检索图45所示的映射,从而计算增益K2_bp_32。图45的Preff是正的规定值。在该映射中,增益K2_bp_32在0<P2_c2的范围内被设定为值1,在P2_c2<-Pref7的范围内被设定为值0,并且在-Pref7≤P2_c2≤0的范围内,被设定为移动平均值P2_c2越大,则越接近值1。
另外,在P2_c3<0时,根据移动平均值P2_c2,检索图46所示的映射,从而计算增益K2_bp_32。图46的Pref8是正的规定值。在此,Pref7和Pref8可以是相同的值,也可以是相互不同的值。
在该映射中,增益K2_bp_32在P2_c2<0的范围内被设定为值1,在Pref8<P2_c2的范围内被设定为值0,并且在0≤P2_c2≤Pref8的范围内,被设定为移动平均值P2_c2越大,则越接近值0。
关于增益K2_bp_32如以上那样被计算的理由、值P2_c3·K2_sk2·K2_bp_32从第三极值搜索控制器230被输入到第二极值搜索控制器220的理由,见后述。
而且,预测曲率ρ2_m2经由延迟要素228而被输入到加法要素227。并且,在加法要素227中,根据下式(232),计算预测曲率ρ2_m2。
[数232]
ρ2_m2(k)=λ_fgt·ρ2_m2(k-1)+K2_sk2·(P2_c2(k)+K2_bp_32(k)·P2_c3(k))…(232)
上式(232)的λ_fgt是遗忘系数,并且以0<λ_fgt<1成立的方式设定。该遗忘系数λ_fgt是用于在潜在风险为零时使第二预测轨道返回直线状(即,用于使预测曲率ρ2_m2接近值0)的值。
而且,在加法要素229中,根据下式(233),计算施加预测曲率ρ2_m2_r。
[数233]
ρ2_m2_r(k)=ρ2_m2(k)+r(k)…(233)
并且,如以上那样计算出的预测曲率ρ2_m2及施加预测曲率ρ2_m2_r从第二极值搜索控制器220被输入到第二预测轨道计算部242。
接下来,对第三极值搜索控制器230进行说明。第三极值搜索控制器230通过与第一实施方式的前述的第三极值搜索控制器33同样的方法,计算预测曲率ρ2_m3及施加预测曲率ρ2_m3_r。该预测曲率ρ2_m3相当于前述的第三预测轨道的曲率半径R2_m3的倒数。
如图40所示,第三极值搜索控制器230具备高通滤波器231、参照输入产生器232、两个延迟要素234、238、乘法器233、移动平均滤波器235、放大要素236及两个加法要素237、239。
在该高通滤波器231中,根据下式(234),计算滤波器值h2_3。
[数234]
h2_3(k)=J2_3(k)-J2_3(k-1)…(234)
另外,参照输入产生器232产生参照输入r。该参照输入r经由延迟要素234而被输入到乘法器233,在乘法器233中,根据下式(235),计算中间值P2_hr3。
[数235]
P2_hr3(k)=h2_3(k)·r2_3(k-1)…(235)
另外,在移动平均滤波器235中,根据下式(236),计算移动平均值P2_c3。需要说明的是,在本实施方式中,移动平均值P2_c3相当于修正量。
[数236]
接着,移动平均值P2_c3在由放大要素236放大了规定增益K2_sk3的量的状态下被输入到加法要素237,预测曲率ρ2_m3经由延迟要素238被输入到加法要素237。
并且,在加法要素237中,根据下式(237),计算预测曲率ρ2_m3。
[数237]
ρ2_m3(k)=λ_fgt·ρ2_m3(k-1)+K2_sk3·P2_c3(k)…(237)
上式(237)的遗忘系数λ_fgt是用于在潜在风险为零时使第三预测轨道返回直线状(即,用于使预测曲率ρ2_m3接近值0)的值。
而且,在加法要素239中,根据下式(238),计算施加预测曲率ρ2_m3_r。
[数238]
ρ2_m3_r(k)=ρ2_m3(k)+r(k)…(238)
并且,如以上那样计算出的预测曲率ρ2_m3及施加预测曲率ρ2_m3_r从第三极值搜索控制器230被输入到第三预测轨道计算部243。
接下来,对前述的第一预测轨道计算部241进行说明。在该第一预测轨道计算部241中,使用来自第一极值搜索控制器210的预测曲率ρ2_m1及施加预测曲率ρ2_m1_r,基于前述的原理,计算三个预测位置Z2_m11~Z2_m13、三个施加预测位置Z2_m11_r~Z2_m13_r。
首先,根据前述的式(124)~(125)、(130)~(135),计算预测位置Z2_m11的X坐标值x2_m11及Y坐标值y2_m11、预测位置Z2_m12的X坐标值x2_m12及Y坐标值y2_m12、预测位置Z2_m13的X坐标值x2_m13及Y坐标值y2_m13。
接着,根据下式(239)、(240),计算第一预测轨道的曲率半径R2_m1_r。
[数239]
·|ρ2_m1|>ρ2_min时
R2_m1_r(k)=1/ρ2_m1_r(k)…(239)
[数240]
·|ρ2_m1|≤ρ2_min时
R2_m1_r(k)=Sign(ρ2_m1_r(k))/ρ2_min…(240)
上式(239)、(240)的ρ2_min是规定的最小曲率(正值)。
接下来,根据下式(241),计算第一预测轨道的旋转角度θ2_m1_r。
[数241]
θ2_m1_r(k)=L2_m1(k)/R2_m1_r(k)…(241)
而且,根据下式(242)、(243),计算施加预测位置Z2_m11_r的X坐标值x2_m11_r及Y坐标值y2_m11_r。
[数242]
x2_m11_r(k)=R2_m1_r(k)·sin(θ2_m1_r(k)/3)…(242)
[数243]
y2_m11_r(k)=R2_m1_r(k)·(1-cos(θ2_m1_r(k)/3)…(243)
接着,根据下式(244)、(245),计算施加预测位置Z2_m12_r的X坐标值x2_m12_r及Y坐标值y2_m12_r。
[数244]
x2_m12_r(k)=R2_m1_r(k)·sin(2θ2_m1_r(k)/3)…(244)
[数245]
y2_m12_r(k)=R2_m1_r(k)·(1-cos(2θ2_m1_r(k)/3)…(245)
另外,根据下式(246)、(247),计算施加预测位置Z2_m13_r的X坐标值x2_m13_r及Y坐标值y2_m13_r。
[数246]
x2_m13_r(k)=R2_m1_r(k)·sin(θ2_m1_r(k))…(246)
[数247]
y2_m13_r(k)=R2_m1_r(k)·(1-cos(θ2_m1_r(k))…(247)
而且,根据下式(248)、(249),计算规定时刻Th1前的推定位置Z2_m1h1_r的X坐标值x2_m1h1_r及Y坐标值y2_m1h1_r。
[数248]
x2_m1h1_r(k)=R2_m1_r(k)·sin(θ2_mh1_r(k))…(248)
[数249]
y2_m1h1_r(k)=R2_m1_r(k)·(1-cos(θ2_mh1_r(k))…(249)
接下来,根据下式(250)、(251),计算规定时刻Th2前的推定位置Z2_m1h2_r的X坐标值x2_m1h2_r及Y坐标值y2_m1h2_r。
[数250]
x2_m1h2_r(k)=R2_m1_r(k)·sin(θ2_mh2_r(k))…(250)
[数251]
y2_m1h2_r(k)=R2_m1_r(k)·(1-cos(θ2_mh2_r(k))…(251)
接下来,对前述的第二预测轨道计算部242进行说明。在该第二预测轨道计算部242中,使用来自第二极值搜索控制器220的预测曲率ρ2_m2及施加预测曲率ρ2_m2_r,基于前述的原理,计算第二预测位置Z2_m2及施加第二预测位置Z2_m2_r。
首先,根据前述的式(142)~(147),计算第二预测位置Z2_m2的X坐标值x2_m2及Y坐标值y2_m2。
接着,根据下式(252)、(253),计算第二预测轨道的曲率半径R2_m2_r。
[数252]
[数253]
·|ρ2_m2|≤ρ2_min时
R2_m2_r(k)=Sign(ρ2_m2_r(k))/ρ2_min…(253)
接下来,根据下式(254),计算第二预测轨道的旋转角度θ2_m2_r。
[数254]
θ2_m2_r(k)=L2_m2(k)/R2_m2_r(k)…(254)
而且,根据下式(255)、(256),计算前述的X’-Y’坐标系(参照图29)中的施加第二预测位置Z2_m2_r的X’坐标值x’_m2_r及Y’坐标值y’_m2_r。
[数255]
x2′_m2_r(k)=R2_m2_r(k)·sin(θ2_m2_r(k))…(255)
[数256]
y2′_m2_r(k)=R2_m2_r(k)·(1-cos(θ2_m2_r(k))…(256)
接着,根据下式(257)、(258),计算X-Y坐标系(参照图29)中的施加第二预测位置Z2_m2_r的X坐标值x2_m2_r及Y坐标值y2_m2_r。
[数257]
x2_m2_r(k)=x2′_m2_r(k)·cos(θ2_m1(k))-y2′_m2_r(k)·sin(θ2_m1(k))+x2_m13(k)…(257)
[数258]
y2_m2_r(k)=x2′_m2_r(k)·sin(θ2_m1(k))+y2′_m2_r(k)·cos(θ2_m1(k))+y2_m13(k)…(258)
接下来,对前述的第三预测轨道计算部243进行说明。在该第三预测轨道计算部243中,使用来自第三极值搜索控制器230的预测曲率ρ2_m3及施加预测曲率ρ2_m3_r,基于前述的原理,计算第三预测位置Z2_m3及施加第三预测位置Z2_m3_r。
首先,根据前述的式(148)~(153),计算第三预测位置Z2_m3的X坐标值x2_m3及Y坐标值y2_m3。
接着,根据下式(259)、(260),计算第三预测轨道的曲率半径R2_m3_r。
[数259]
·|ρ2_m3|>ρ2_min时
R2_m3_r(k)=1/ρ2_m3_r(k)…(259)
[数260]
·|ρ2_m3|≤ρ2_min时
R2_m3_r(k)=Sign(ρ2_m3_r(k))/ρ2_min…(260)
接下来,根据下式(261),计算第三预测轨道的旋转角度θ2_m3_r。
[数261]
θ2_m3_r(k)=L2_m3(k)/R2_m3_r(k)…(261)
而且,根据下式(262)、(263),计算前述的X”-Y”坐标系(参照图29)中的施加第三预测位置Z2_m3_r的X”坐标值x”m3_r及Y”坐标值y”_m3_r。
[数262]
x2″_m3_r(k)=R2_m3_r(k)·sin(θ2_m3_r(k))…(262)
[数263]
y2″_m3_r(k)=R2_m3_r(k)·(1-cos(θ2_m3_r(k))…(263)
接着,根据下式(264)、(265),计算X-Y坐标系(参照图29)中的施加第三预测位置Z2_m3_r的X坐标值x2_m3_r及Y坐标值y2_m3_r。
[数264]
x2_m3_r(k)=x2″_m3_r(k)·cos(θ2_m1(k)+θ2_m2(k))-y2″_m3_r(k)·sin(θ2_m1(k)+θ2_m2(k))+x2_m2(k)…(264)
[数265]
如以上那样,在轨道决定部200中,通过第一~第三极值搜索控制器210~230,计算第一~第三预测轨道的预测曲率ρ2_m1~ρ2_m3,基于这些预测曲率ρ2_m1~ρ2_m3,计算前述的5个预测位置Z2_mi(i=11~13、2、3)、5个施加预测位置Z2_mi_r(i=11~13、2、3)、规定时刻Th1前及规定时刻Th2前的推定位置Z2_m1h1、Z2_m1h2。
接下来,对前述的值P2_c2·K2_sk1·K2_bp_21及值P2_c3·K2_sk1·K2_bp_31被输入到第一极值搜索控制器210的理由、值P2_c3·K2_sk2·K2_bp_32被输入到第二极值搜索控制器220的理由、三个增益K2_bp_21、K2_bp_31、K2_bp_32如前述那样设定的理由进行说明。
例如,在第一~第三极值搜索控制器210~230中,在相互无关地计算出预测曲率ρ2_m1~ρ2_m3的情况下,有时会产生如下所述那样的状态。
即,如图47所示,在行人M1的右前方存在干涉对象的情况下,第一预测轨道(当前位置~预测位置Z2_m13)、第二预测轨道(预测位置Z2_m13~第二预测位置Z2_m2)被决定为直线状,而另一方面,从第二预测位置Z2_m2到第三预测位置Z2_m3为止的第三预测轨道被决定为圆弧状。
与此相对地,在通常的情况下,行人M1通常不在图47所示那样的轨道上移动,而是在到达第二预测位置Z2_m2之前开始预先转弯地移动,以能够躲避与干涉对象的干涉。
因此,在轨道决定部200中,需要预测并决定那样的行人M1的移动轨道。例如,如图48所示,在第一预测轨道、第二预测轨道及第三预测轨道被决定为相同的转弯方向的情况下,即,在三个移动平均值P2_c1~P2_c3的符号为彼此相同的情况下,需要以反映两个移动平均值P2_c2、P2_c3的方式计算预测曲率ρ2_m1,并且以反映移动平均值P2_c3的方式计算预测曲率ρ2_m2。
另一方面,例如,如图49所示,在第一预测轨道与第二预测轨道被决定为相反的转弯方向、第二预测轨道与第三预测轨道被决定为相反的转弯方向的情况下,即,移动平均值P2_c1、P2_c2的符号的正负不同,并且移动平均值P2_c2、P2_c3的符号的正负不同的情况下,在以反映两个移动平均值P2_c2、P2_c3的方式计算预测曲率ρ2_m1时,会减少针对干涉对象的躲避余量。而且,在以反映移动平均值P2_c3的方式计算预测曲率ρ2_m2的情况下,也会产生相同的问题。因此,需要以不反映两个移动平均值P2_c2、P2_c3的方式计算预测曲率ρ2_m1,并且以不反映移动平均值P2_c3的方式计算预测曲率ρ2_m2。
由于以上的理由,两个增益K2_bp_21、K2_bp_31如前述那样设定,并且前述的值P2_c2·K2_sk1·K2_bp_21及值P2_c3·K2_sk1·K2_bp_31构成为被输入到第一极值搜索控制器210。
由于相同的理由,构成为增益K2_bp_32如前述那样设定,并且值P2_c3·K2_sk2·K2_bp_32被输入到第二极值搜索控制器220。
根据以上的结构,三个移动平均值P2_c1~P2_c3的符号、两个值P2_c2·K2_bp_21、P2_c3·K2_bp_31、移动平均值P2_c2、P2_c3相对于预测曲率ρ2_m1的的反映状态之间的关系成为图50所示的12个模式C1~C12。在该情况下,在前述的图48的状态下,成为模式C1(或C2),在前述的图49的状态下,成为模式C9~C12中的任意一个。
另外,两个移动平均值P2_c2、P2_c3的符号、值P2_c3·K2_bp_32、移动平均值P2_c3相对于预测曲率ρ2_m2的的反映状态之间的关系成为图51所示的6个模式D1~D6。
如以上那样,在本实施方式的轨道决定部200中,在计算5个预测位置Z2_m11、Z2_m12、Z2_m13、Z2_m2、Z2_m3之后,这些值被输入到车辆控制部40。
在车辆控制部40中,根据5个预测位置Z2_m11、Z2_m12、Z2_m13、Z2_m2、Z2_m3,控制原动机5及致动器6。由此,本车辆3能够基于行人M1的第一~第三预测轨道以躲避行人M1的干涉的方式进行行驶。
如以上那样,根据第二实施方式的轨道生成装置100,在第一~第三预测轨道的每一个中,计算表示与干涉对象的干涉程度的潜在风险R2_p_mi_vj_v_r、R2_p_rbr_mi_r、R2_p_rbl_mi_r,以包含这些潜在风险R2_p_mi_vj_v_r、R2_p_rbr_mi_r、R2_p_rbl_mi_r的总和的方式计算第一~第三评价函数J2_1~J2_3。
并且,以使这些第一~第三评价函数J2_1~J2_3成为极小值的方式计算预测曲率ρ2_m1~ρ2_m3,基于这些预测曲率ρ2_m1~ρ2_m3,决定第一~第三预测轨道。即,5个预测位置Z2_mi(i=11、12、13、2、3)被以使第一~第三评价函数J2_1~J2_3成为极小值的方式决定。
在该情况下,如上述那样,第一评价函数J2_1被计算为表示行人M1与干涉对象的干涉程度的潜在风险的总和、以及权重系数与跟踪位势的积ω_tr·Tr_r之间的总和,第二及第三评价函数J2_2、J2_3被计算为表示行人M1与干涉对象的干涉程度的潜在风险的总和。因此,通过那样的第一~第三评价函数J2_1~J2_3被以成为极小值的方式决定,从而第一~第三预测轨道被决定为减少与干涉对象的干涉程度。
而且,在第一极值搜索控制器31中的移动平均值P2_c1与第二极值搜索控制器32中的移动平均值P2_c2为相同符号的情况下,以反映移动平均值P2_c2的方式决定预测曲率ρ2_m1(参照图48及图50)。
另外,在第二极值搜索控制器32中的移动平均值P2_c2与第三极值搜索控制器33中的移动平均值P2_c3为相同符号的情况下,以反映移动平均值P2_c3的方式决定预测曲率ρ2_m2(参照图48及图51)。
在该情况下,三个移动平均值P2_c1~P2_c3被计算为用于分别修正三个预测曲率ρ2_m1~ρ2_m3的修正量,即用于修正第一~第三预测轨道的修正量。因此,在第一预测轨道及第二预测轨道的修正量的符号相同的情况下,第一预测轨道被向与比其远离行人M1的当前位置的第二预测轨道的修正方向相同的方向进一步修正。
与此相同地,在第二预测轨道及第三预测轨道的修正量的符号相同的情况下,第二预测轨道被向与比其远离行人M1的当前位置的第三预测轨道的修正方向相同的方向进一步修正。
在此,在干涉对象存在于行人M1的行进方向的情况下,远离当前位置的预测轨道与接近行人M1的当前位置的预测轨道相比,成为更靠近干涉对象的轨道。因此,通过以反映用于修正接近干涉对象的预测轨道的修正量的方式生成远离干涉对象(即接近行人M1)的预测轨道,从而能够在远离干涉对象的预测轨道中,进一步增大用于躲避与干涉对象的干涉的躲避余量。这样,能够更早地预测行人M1相对于干涉对象的躲避行动,由此在基于行人M1的预测行动来决定本车辆3的行驶轨道时,在能够实现的行驶轨道受到限制的本车辆3中也能够以能躲避行人M1的干涉的方式适当地决定车辆3的未来的行驶轨道。
另一方面,在相邻的两个预测轨道的修正量为不同符号的情况下,两个预测轨道为了躲避干涉对象而被向相互不同的方向修正。因此,在那样的条件下,在将用于修正远离行人M1的当前位置的预测轨道的修正量反映到接近当前位置的预测轨道时,在接近当前位置的预测轨道中,会减少针对干涉对象的躲避余量。其结果为,行人M1相对于干涉对象的躲避行动的变化被预测为很小,在基于行人M1的预测行动来决定本车辆3的行驶轨道时,在其能够实现的行驶轨道受到限制的本车辆3中,难以确保能够躲避与行人M1的干涉足够的余量,不能适当地决定本车辆3的未来的行驶轨道。
与此相对地,在本实施方式的情况下,在移动平均值P2_c1与移动平均值P2_c2为不同符号的情况下,在不反映移动平均值P2_c2的情况下决定预测曲率ρ2_m1(参照图49及图50)。与此相同地,在移动平均值P2_c2与移动平均值P2_c3为不同符号的情况下,在不反映移动平均值P2_c3的情况下决定预测曲率ρ2_m2(参照图49及图51)。由此,在接近当前位置的预测轨道中,能够适当地确保针对干涉对象的躲避余量。
另外,第一预测轨道被决定为将比第二预测轨道及第三预测轨道短的三个预测轨道组合而成。由此,即使在与行人M1的距离比第二预测轨道及第三预测轨道比近的第一预测轨道中,也能够适当地确保用于躲避与干涉对象的干涉的躲避余量。
而且,第一评价函数J2_1如前述的式(199)所示,以除了潜在风险的总和之外还包含权重系数与跟踪位势的积ω_tr·Tr_r的方式计算出。该跟踪位势Tr_r被作为相当于两个推定位置Z2_m1h1_r、Z2_m1h2_r与两个实测位置Z2_h1、Z2_h2的平方和误差的值计算出。
因此,通过以使第一评价函数J2_1成为极小值的方式决定第一预测轨道,由此能够以使两个推定位置Z2_m1h1_r、Z2_m1h2_r与两个实测位置Z2_h1、Z2_h2之间的差量变小的方式决定第一预测轨道。由此,能够考虑对行人M1的过去的轨道的跟随性地决定第一预测轨道。
需要说明的是,第二实施方式是将本车辆3作为第一移动体的例子,但也可以取而代之,将二轮车辆、机器人、航空器或船舶等作为第一移动体。
另外,第二实施方式是将本发明的轨道生成装置100搭载在能够自动驾驶的四轮车辆3的例子,但也可以取而代之,将轨道生成装置搭载在四轮车辆以外的移动体(例如,二轮车辆、机器人、航空器或船舶等),也可以单独使用轨道生成装置。
另外,第二实施方式是将行人M1作为第二移动体的例子,但也可以取而代之,将二轮车辆、机器人、航空器或船舶等作为第二移动体。
另外,第二实施方式是将交通参加者作为干涉对象的例子,但也可以取而代之,将二轮车辆、机器人、航空器或船舶等作为干涉对象。
Claims (17)
1.一种轨道生成装置,其是生成移动体的未来的轨道的轨道生成装置,其特征在于,
所述轨道生成装置具备:
周边状况识别部,其识别所述移动体的周边状况;
预测轨道生成部,其基于利用该周边状况识别部识别出的所述周边状况的识别结果,使用将所述移动体的轨道模型化了的多个轨道模型,生成将所述移动体的所述未来的轨道分割为多个时的多个未来的轨道的预测值即多个预测轨道;
干涉程度参数计算部,其使用该多个预测轨道,计算表示所述移动体的周边的干涉对象与该移动体的干涉程度的多个干涉程度参数;以及
修正量决定部,其以使该多个干涉程度参数所表示的干涉程度减少的方式决定用于分别修正所述多个预测轨道的多个修正量,
所述预测轨道生成部基于所述周边状况的识别结果,使用所述多个轨道模型及所述多个修正量,生成所述多个预测轨道,并且在用于修正相互相邻的两个所述预测轨道的两个所述修正量被决定为将该两个预测轨道向相同的方向修正的情况下,以反映用于修正该两个预测轨道中的、远离所述移动体的当前位置的所述预测轨道的所述修正量的方式生成该两个预测轨道中的、接近所述移动体的该当前位置的所述预测轨道。
2.根据权利要求1所述的轨道生成装置,其特征在于,
所述预测轨道生成部在所述两个所述修正量被决定为将所述两个预测轨道向相互不同的方向修正的情况下,与用于修正远离所述移动体的所述当前位置的所述预测轨道的所述修正量无关地生成接近所述移动体的所述当前位置的所述预测轨道。
3.根据权利要求1所述的轨道生成装置,其特征在于,
接近所述移动体的所述当前位置的所述预测轨道以被分割为比远离所述移动体的所述当前位置的所述预测轨道短的多个预测轨道的状态生成。
4.根据权利要求2所述的轨道生成装置,其特征在于,
接近所述移动体的所述当前位置的所述预测轨道以被分割为比远离所述移动体的所述当前位置的所述预测轨道短的多个预测轨道的状态生成。
5.根据权利要求1所述的轨道生成装置,其特征在于,
所述移动体具备所述周边状况识别部、所述预测轨道生成部、所述干涉程度参数计算部及所述修正量决定部。
6.根据权利要求2所述的轨道生成装置,其特征在于,
所述移动体具备所述周边状况识别部、所述预测轨道生成部、所述干涉程度参数计算部及所述修正量决定部。
7.根据权利要求3所述的轨道生成装置,其特征在于,
所述移动体具备所述周边状况识别部、所述预测轨道生成部、所述干涉程度参数计算部及所述修正量决定部。
8.根据权利要求4所述的轨道生成装置,其特征在于,
所述移动体具备所述周边状况识别部、所述预测轨道生成部、所述干涉程度参数计算部及所述修正量决定部。
9.根据权利要求1所述的轨道生成装置,其特征在于,
所述移动体是具备所述周边状况识别部、所述预测轨道生成部、所述干涉程度参数计算部及所述修正量决定部的第一移动体以外的第二移动体,
所述预测轨道生成部使用所述第二移动体的过去的位置、所述多个轨道模型及所述多个修正量,生成所述多个预测轨道。
10.根据权利要求2所述的轨道生成装置,其特征在于,
所述移动体是具备所述周边状况识别部、所述预测轨道生成部、所述干涉程度参数计算部及所述修正量决定部的第一移动体以外的第二移动体,
所述预测轨道生成部使用所述第二移动体的过去的位置、所述多个轨道模型及所述多个修正量,生成所述多个预测轨道。
11.根据权利要求3所述的轨道生成装置,其特征在于,
所述移动体是具备所述周边状况识别部、所述预测轨道生成部、所述干涉程度参数计算部及所述修正量决定部的第一移动体以外的第二移动体,
所述预测轨道生成部使用所述第二移动体的过去的位置、所述多个轨道模型及所述多个修正量,生成所述多个预测轨道。
12.根据权利要求4所述的轨道生成装置,其特征在于,
所述移动体是具备所述周边状况识别部、所述预测轨道生成部、所述干涉程度参数计算部及所述修正量决定部的第一移动体以外的第二移动体,
所述预测轨道生成部使用所述第二移动体的过去的位置、所述多个轨道模型及所述多个修正量,生成所述多个预测轨道。
13.根据权利要求9所述的轨道生成装置,其特征在于,
所述预测轨道生成部以使所述移动体的过去的位置的时间序列与将接近所述移动体的所述当前位置的所述预测轨道沿着该预测轨道向该移动体的过去的位置侧延伸了时的轨道上的多个推定位置的时间序列之间的差量减少的方式,生成接近所述移动体的所述当前位置的所述预测轨道。
14.根据权利要求10所述的轨道生成装置,其特征在于,
所述预测轨道生成部以使所述移动体的过去的位置的时间序列与将接近所述移动体的所述当前位置的所述预测轨道沿着该预测轨道向该移动体的过去的位置侧延伸了时的轨道上的多个推定位置的时间序列之间的差量减少的方式,生成接近所述移动体的所述当前位置的所述预测轨道。
15.根据权利要求11所述的轨道生成装置,其特征在于,
所述预测轨道生成部以使所述移动体的过去的位置的时间序列与将接近所述移动体的所述当前位置的所述预测轨道沿着该预测轨道向该移动体的过去的位置侧延伸了时的轨道上的多个推定位置的时间序列之间的差量减少的方式,生成接近所述移动体的所述当前位置的所述预测轨道。
16.根据权利要求12所述的轨道生成装置,其特征在于,
所述预测轨道生成部以使所述移动体的过去的位置的时间序列与将接近所述移动体的所述当前位置的所述预测轨道沿着该预测轨道向该移动体的过去的位置侧延伸了时的轨道上的多个推定位置的时间序列之间的差量减少的方式,生成接近所述移动体的所述当前位置的所述预测轨道。
17.一种轨道生成方法,其是通过运算处理装置生成移动体的未来的轨道的轨道生成方法,其特征在于,
所述运算处理装置执行如下步骤:
周边状况识别步骤,在所述周边状况识别步骤中,识别所述移动体的周边状况;
预测轨道生成步骤,在所述预测轨道生成步骤中,基于利用该周边状况识别步骤识别出的所述周边状况的识别结果,使用将所述移动体的轨道模型化了的多个轨道模型,生成将所述移动体的所述未来的轨道分割为多个时的多个未来的轨道的预测值即多个预测轨道;
干涉程度参数计算步骤,在所述干涉程度参数计算步骤中,使用该多个预测轨道,计算表示所述移动体的周边的干涉对象与该移动体的干涉程度的多个干涉程度参数;
修正量决定步骤,在所述修正量决定步骤中,以使该多个干涉程度参数所表示的干涉程度减少的方式决定用于分别修正所述多个预测轨道的所述多个修正量,
在所述预测轨道生成步骤中,基于所述周边状况的识别结果,使用所述多个轨道模型及所述多个修正量,生成所述多个预测轨道,并且在用于修正相互相邻的两个所述预测轨道的两个所述修正量被决定为将该两个预测轨道向相同的方向修正的情况下,以反映用于修正该两个预测轨道中的、远离所述移动体的当前位置的所述预测轨道的所述修正量的方式生成该两个预测轨道中的、接近所述移动体的该当前位置的所述预测轨道。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2021-056089 | 2021-03-29 | ||
JP2021056089A JP7496797B2 (ja) | 2021-03-29 | 2021-03-29 | 軌道生成装置及び軌道生成方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115148049A CN115148049A (zh) | 2022-10-04 |
CN115148049B true CN115148049B (zh) | 2023-08-22 |
Family
ID=83406009
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210298509.3A Active CN115148049B (zh) | 2021-03-29 | 2022-03-24 | 轨道生成装置及轨道生成方法 |
Country Status (3)
Country | Link |
---|---|
US (1) | US20220314981A1 (zh) |
JP (1) | JP7496797B2 (zh) |
CN (1) | CN115148049B (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2002131432A (ja) * | 2000-10-24 | 2002-05-09 | Honda Motor Co Ltd | 車両用進行軌跡予測装置 |
JP2007317018A (ja) * | 2006-05-26 | 2007-12-06 | Toyota Motor Corp | 衝突判定装置 |
CN108973997A (zh) * | 2017-06-02 | 2018-12-11 | 本田技研工业株式会社 | 行驶轨道决定装置以及自动驾驶装置 |
CN109421712A (zh) * | 2017-08-30 | 2019-03-05 | 本田技研工业株式会社 | 车辆控制装置、车辆、车辆控制方法以及存储介质 |
JP2019220054A (ja) * | 2018-06-22 | 2019-12-26 | 本田技研工業株式会社 | 行動予測装置及び自動運転装置 |
CN111009153A (zh) * | 2019-12-04 | 2020-04-14 | 珠海深圳清华大学研究院创新中心 | 一种轨迹预测模型的训练方法、装置和设备 |
CN111738323A (zh) * | 2020-06-12 | 2020-10-02 | 上海应用技术大学 | 基于灰色马尔科夫模型的混合增强智能轨迹预测方法和装置 |
Family Cites Families (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10089537B2 (en) * | 2012-05-18 | 2018-10-02 | Magna Electronics Inc. | Vehicle vision system with front and rear camera integration |
US9045144B2 (en) * | 2013-05-09 | 2015-06-02 | Robert Bosch Gmbh | Third-order polynomial-based course prediction for driver assistance functions |
US9988047B2 (en) * | 2013-12-12 | 2018-06-05 | Magna Electronics Inc. | Vehicle control system with traffic driving control |
US11004000B1 (en) * | 2017-01-30 | 2021-05-11 | Waymo Llc | Predicting trajectory intersection by another road user |
JP6724820B2 (ja) | 2017-02-28 | 2020-07-15 | 株式会社デンソー | 走行軌道生成装置 |
JP6503585B2 (ja) | 2017-09-01 | 2019-04-24 | 本田技研工業株式会社 | 車両制御システム、車両制御方法、及びプログラム |
DE102017215592A1 (de) | 2017-09-05 | 2019-03-07 | Volkswagen Aktiengesellschaft | Fahrassistenzvorrichtung eingerichtet zur teilautonomen und vollautonomen Führung eines Kraftfahrzeugs, Verfahren und Kraftfahrzeug |
JP6634100B2 (ja) * | 2018-01-12 | 2020-01-22 | 本田技研工業株式会社 | 走行軌道決定装置及び自動運転装置 |
US11126186B2 (en) * | 2018-09-15 | 2021-09-21 | Toyota Research Institute, Inc. | Systems and methods for predicting the trajectory of a road agent external to a vehicle |
JP7115270B2 (ja) * | 2018-12-07 | 2022-08-09 | トヨタ自動車株式会社 | 自動運転システム |
US20200278686A1 (en) | 2019-02-28 | 2020-09-03 | University Of South Carolina | Iterative Feedback Motion Planning |
JP7295012B2 (ja) * | 2019-12-24 | 2023-06-20 | 日立Astemo株式会社 | 車両制御システム、および、車両制御方法 |
-
2021
- 2021-03-29 JP JP2021056089A patent/JP7496797B2/ja active Active
-
2022
- 2022-03-24 CN CN202210298509.3A patent/CN115148049B/zh active Active
- 2022-03-26 US US17/705,290 patent/US20220314981A1/en active Pending
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2002131432A (ja) * | 2000-10-24 | 2002-05-09 | Honda Motor Co Ltd | 車両用進行軌跡予測装置 |
JP2007317018A (ja) * | 2006-05-26 | 2007-12-06 | Toyota Motor Corp | 衝突判定装置 |
CN108973997A (zh) * | 2017-06-02 | 2018-12-11 | 本田技研工业株式会社 | 行驶轨道决定装置以及自动驾驶装置 |
CN109421712A (zh) * | 2017-08-30 | 2019-03-05 | 本田技研工业株式会社 | 车辆控制装置、车辆、车辆控制方法以及存储介质 |
JP2019220054A (ja) * | 2018-06-22 | 2019-12-26 | 本田技研工業株式会社 | 行動予測装置及び自動運転装置 |
CN111009153A (zh) * | 2019-12-04 | 2020-04-14 | 珠海深圳清华大学研究院创新中心 | 一种轨迹预测模型的训练方法、装置和设备 |
CN111738323A (zh) * | 2020-06-12 | 2020-10-02 | 上海应用技术大学 | 基于灰色马尔科夫模型的混合增强智能轨迹预测方法和装置 |
Also Published As
Publication number | Publication date |
---|---|
US20220314981A1 (en) | 2022-10-06 |
CN115148049A (zh) | 2022-10-04 |
JP2022153059A (ja) | 2022-10-12 |
JP7496797B2 (ja) | 2024-06-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11131993B2 (en) | Methods and systems for trajectory forecasting with recurrent neural networks using inertial behavioral rollout | |
Stahl et al. | Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios | |
CN112673234B (zh) | 路径规划方法和路径规划装置 | |
Amditis et al. | A situation-adaptive lane-keeping support system: Overview of the safelane approach | |
US20210171025A1 (en) | Moving body behavior prediction device and moving body behavior prediction method | |
Khodayari et al. | A historical review on lateral and longitudinal control of autonomous vehicle motions | |
Petrich et al. | Map-based long term motion prediction for vehicles in traffic environments | |
JP6634100B2 (ja) | 走行軌道決定装置及び自動運転装置 | |
CN112539755B (zh) | 用于自主车辆的基于admm算法的更新点云配准管线 | |
Chae et al. | Virtual target-based overtaking decision, motion planning, and control of autonomous vehicles | |
CN110352450A (zh) | 驾驶辅助方法及驾驶辅助装置 | |
CN110632916B (zh) | 行动预测装置以及自动驾驶装置 | |
Bonnin et al. | A generic concept of a system for predicting driving behaviors | |
US11400942B2 (en) | Vehicle lane trajectory probability prediction utilizing kalman filtering with neural network derived noise | |
Thyri et al. | A domain-based and reactive COLAV method with a partially COLREGs-compliant domain for ASVs operating in confined waters. | |
JP7119839B2 (ja) | 物体検出装置、物体検出方法及び車両制御装置 | |
Xia et al. | Parameterized Decision-making with Multi-modal Perception for Autonomous Driving | |
CN115148049B (zh) | 轨道生成装置及轨道生成方法 | |
Park et al. | Path generation algorithm based on crash point prediction for lane changing of autonomous vehicles | |
JP7277215B2 (ja) | 挙動予測方法及び挙動予測装置並びに車両制御装置 | |
Milanés et al. | An approach to driverless vehicles in highways | |
KR20200133122A (ko) | 차량 충돌 방지 장치 및 방법 | |
CN114003027B (zh) | 移动体控制装置、移动体、移动体控制方法及存储介质 | |
CN113654569A (zh) | 一种路径规划方法、系统及存储介质 | |
Nantabut et al. | A*-Guided Scenario-based Stochastic Model Predictive Control for an Autonomous Vehicle in Simulated Pedestrian Environments |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |