CN115061478A - 自动驾驶车辆局部避障及路径跟踪方法、系统及存储介质 - Google Patents

自动驾驶车辆局部避障及路径跟踪方法、系统及存储介质 Download PDF

Info

Publication number
CN115061478A
CN115061478A CN202210832265.2A CN202210832265A CN115061478A CN 115061478 A CN115061478 A CN 115061478A CN 202210832265 A CN202210832265 A CN 202210832265A CN 115061478 A CN115061478 A CN 115061478A
Authority
CN
China
Prior art keywords
obstacle
target point
path
potential field
sub
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.)
Pending
Application number
CN202210832265.2A
Other languages
English (en)
Inventor
李淑静
陈栋磊
周杰
李卫华
李国庆
雷钰龙
王一鹏
王剑锋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
State Grid Smart Energy Traffic Technology Innovation Center Suzhou Co ltd
Harbin Institute of Technology Weihai
Original Assignee
State Grid Smart Energy Traffic Technology Innovation Center Suzhou Co ltd
Harbin Institute of Technology Weihai
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by State Grid Smart Energy Traffic Technology Innovation Center Suzhou Co ltd, Harbin Institute of Technology Weihai filed Critical State Grid Smart Energy Traffic Technology Innovation Center Suzhou Co ltd
Priority to CN202210832265.2A priority Critical patent/CN115061478A/zh
Publication of CN115061478A publication Critical patent/CN115061478A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种自动驾驶车辆局部避障及路径跟踪方法、系统及存储介质，通过建立自动驾驶车辆模型和自动驾驶车辆差速转向模型，利用全局路径并减少避障过程的计算量，基于全局路径建立路径引力势场使自动驾驶车辆对全局路径进行跟踪，同时在避障完成后快速返回全局路径；针对局部最优解问题，提出了由障碍物和全局路径信息选取子目标点的策略，建立子目标点引力场使自动驾驶车辆快速避开障碍物返回全局路径；为了提升动态避障能力，由自动驾驶车辆与障碍物的相对速度建立速度斥力势场，本发明实现了路径跟踪和局部避障的协同控制。

Description

自动驾驶车辆局部避障及路径跟踪方法、系统及存储介质
技术领域
本发明涉及路径规划领域，具体为一种自动驾驶车辆局部避障及路径跟踪方法、系统及存储介质。
背景技术
为了在各种情况下安全操作，自动驾驶车辆必须通过雷达、摄像头和其他传感器收集环境数据。车辆跟随决策模块进行决策，绘制避免碰撞的路线，并控制到达目标点，也就是路径跟踪。
经典人工势场算法在自动驾驶车辆的行驶环境中建立了虚拟力场，包括引力势场和斥力势场。这两个势场的共同作用使自动车避开障碍物，朝着联合势场下降梯度最陡的方向移动。经典人工势场算法具有数学原理简单、计算成本低、实时性好等优点。因此，它适用于局部路径规划，规划的路径为光滑的。
但是经典人工势场算法有以下缺点：
1.假设自动驾驶车辆行驶在无限的环境中，但实际道路是有限的；规划的路径与实际道路环境不符，不适合全局路径规划；
2.在避障方面，斥力势场只考虑自动驾驶车辆与障碍物之间的距离，对动态障碍物的适应性较差；
3.如果自动驾驶车辆受势场的合力为零或接近零，则局部最优解会导致自动驾驶车辆停止或振荡，路径规划失败。
发明内容
为克服上述背景技术中对于自动驾驶车辆局部最优解和动态避障能力不足以及很少考虑避障后自动驾驶车辆返回全局路径方式的问题，本发明的目的在于提供一种自动驾驶车辆局部避障及路径跟踪方法缺点，
为了实现上述目的，本发明采用以下技术方案：
本发明的第一方面，提供自动驾驶车辆局部避障及路径跟踪方法，其特征在于，包括：
S1:建立动力学模型:
建立自动驾驶车辆模型和自动驾驶车辆差速转向模型；
S2:建立路径引力势场:
根据全局路径建立路径引力势场，对自动驾驶车辆施加虚拟重力，使其沿着预定路径行驶，保证自动驾驶车辆在避开障碍物后能够按时返回到预定路径；
S3:建立速度斥力势场:
根据自动驾驶车辆与障碍物的相对速度建立速度斥力势场，提升自动驾驶车辆动态避障能力；
S4:建立子目标点引力势场:根据障碍物的影响范围，选择子目标点，根据所述子目标点建立子目标点引力势场，使自动驾驶车辆快速避开障碍物返回全局路径。
在一些可能的实施方式中，假设所述自动驾驶车辆是刚体，建立一个线性二自由度自动驾驶车辆模型为：
Figure BDA0003748910860000021
其中，[x y ψ]T为自动驾驶车辆中心的位置和航向角。
在一些可能的实施方式中，所述自动驾驶车辆差速转向模型为:
Figure BDA0003748910860000022
其中，[υ ω]T为自动驾驶车辆的速度和角速度，vr、vl分别为自动驾驶车辆左右轮线速度；L是自动驾驶车辆的轴距。
在一些可能的实施方式中，所述“建立路径引力势场”包括：
建立横向引力势场；
建立纵向引力势场；
对横向引力势场和纵向引力势场求和得到路径引力势场。
在一些可能的实施方式中，所述横向引力势场和横向引力的函数公式分别为：
Upath＝k1e2
Fpath＝2k1e；
其中，e为横向偏差，即车辆质心和全局路径之间的最短距离，k1为势场函数公式增益。
在一些可能的实施方式中，所述横向引力方向垂直于全局路径。
在一些可能的实施方式中，所述纵向引力势场和纵向引力的函数公式分别为：
Figure BDA0003748910860000031
Figure BDA0003748910860000032
其中，L＇(x，y)为剩余的路径曲线；
Figure BDA0003748910860000033
为航向角的一阶导数；k2为势场函数公式增益；c为常数,c保证当自动驾驶车辆直线移动时，Fg的最大值为
Figure BDA0003748910860000034
而不是无穷大。
在一些可能的实施方式中，所述纵向引力方向与全局路径中心线平行。
在一些可能的实施方式中，所述纵向引力势场的速度公式为：
v(t)＝k3Fg；
其中，k3为增益系数；t为时间。
在一些可能的实施方式中，所述自动驾驶车辆最大加速度阈值设定为amax，如果：[v(t)-v(t-Δt)]/Δt＞amax，那么：v(t)＝v(t-Δt)+amaxΔt；如果：[v(t)-v(t-Δt)]/Δt＜-amax，那么：v(t)＝v(t-Δt)-amaxΔt。
在一些可能的实施方式中，所述速度斥力势场和速度斥力的函数公式分别为：
Figure BDA0003748910860000041
Figure BDA0003748910860000042
其中，α是相对速度vor的方向与自动驾驶车辆和障碍物所在位置点组成的向量
Figure BDA0003748910860000043
之间的夹角；ρ0是障碍物排斥的影响半径，该半径与障碍物的大小和自动驾驶车辆的速度成比例；Kv是增益系数，X是自动驾驶车辆的位置坐标，Xobs是障碍物的位置坐标，ρ(X，Xobs)是障碍物与车辆之间的距离。
在一些可能的实施方式中，所述“建立子目标点引力势场”具体为：当自动驾驶车辆行驶到障碍物的影响范围时，选择子目标点，所述子目标点包括纵向子目标点和横向子目标点；根据选定的纵向子目标点和横向子目标点分别建立纵向子目标点引力场和横向子目标点引力场；所述横向子目标点引力场使自动驾驶车辆转向障碍物一侧，以避免共线斥力和引力引起的局部最优解；所述纵向子目标点引力场使自动驾驶车辆以平滑的轨迹避开障碍物，返回全局路径。
在一些可能的实施方式中，所述子目标点的选择方法包括三种情况：
情况一：自动驾驶车辆在行驶过程中仅受一个障碍物影响；
情况二：自动驾驶车辆在行驶过程中同时受若干个障碍物影响；
情况三：自动驾驶车辆在行驶过程中间接受若干个障碍物影响。
在一些可能的实施方式中，情况一中，所述“自动驾驶车辆在行驶过程中仅受一个障碍物影响”时，子目标点的选择方法为：
设障碍物A的中心坐标为(xobsA，yobsA)，预定路径和障碍物A的影响范围的交点为R和O，其中R的坐标为XR＝(xR,yR)T，O的坐标为XO＝(xO,yO)T；
选择O点作为纵向子目标点，确定横向子目标点，将点R绕障碍物中心A旋转一个角度θ到所选侧，得到横向子目标点M，所述横向子目标点M的坐标XM＝(xM,yM)T的函数公式为：
xM＝(xR-xobsA)cosθ-(yR-yobsA)sinθ+xobsA
yM＝(xR-xobsA)sinθ+(yR-yobsA)cosθ+yobsA；
在子目标点的合力的作用下，即所述纵向子目标点O和所述横向子目标点M的引力作用下，自动驾驶车辆能够有效地克服局部最优解，顺利避障，避障结束后快速返回全局路径，所述横向子目标点M和纵向子目标点O的引力函数公式分别为：
FattM＝-kρ(X，XM)
FattO＝-kρ(X，XO)；
其中，k是路径引力势场的增益系数；ρ(X,XM)是自动驾驶车辆与横向目标点之间的距离；ρ(X,XO)是自动驾驶车辆与纵向目标点之间的距离。
在一些可能的实施方式中，所述“确定横向子目标点”具体为：横向子目标点M选择在障碍物A的左侧或右侧，所述横向子目标点M在选择障碍物A的左侧或右侧的过程中应满足避障过程中最大横向偏差e尽可能小的原则。
在一些可能的实施方式中，当f(xobsA，yobsA)＝0时，可以确定障碍物A的中心位于预定路径上，横向子目标点M应选择在避障距离较短的一侧，避障距离可通过中心角∠RAO来衡量。在∠RAO<π的一侧，避障距离明显更短，因此在这一侧选择横向子目标点。当∠RAO＝π,需要选择预定路径左侧的横向子目标点，确定所选方向后，将点R旋转一个角度θ绕障碍物中心A到所选侧得到横向子目标点M；
在避障开始时，纵向子目标点的引力FattO大于横向子目标点引力FattM，这将驱使自动驾驶车辆接近障碍物，增加碰撞风险；当
Figure BDA0003748910860000061
时，横向子目标点引力FattM在合力中的比重应先大后小，纵向子目标点引力FattO比例应先小后大，这样可以使自动驾驶车辆顺利绕过障碍物；当
Figure BDA0003748910860000062
时，局部最优解不会出现在下一个避障过程中；因此，子目标点的合力仅包含纵向子目标点引力FattO，从而使自动驾驶车辆返回全局路径。
在一些可能的实施方式中，在保证自动驾驶车辆运行轨迹平稳的同时，用权重系数w来调整子目标点的合力，所述权重系数w的函数公式为：
Figure BDA0003748910860000063
其中，||XC||是自动驾驶车辆到障碍物中心A和横向目标点M连线的最短距离；||RB||是R点到障碍物中心A和横向目标点M连线的最短距离。
在一些可能的实施方式中，情况二中，所述“自动驾驶车辆在行驶过程中同时受若干个障碍物影响”时，这种情况满足N个障碍物的影响范围相交，并且
Figure BDA0003748910860000064
其中
Figure BDA0003748910860000065
为空集，子目标点的选择方法为：
当自动驾驶车辆刚进入障碍物A1的影响范围时，根据情况一中子目标点的选择方法确定避开障碍物A1的纵向子目标点O1和横向子目标点M1；
自动驾驶车辆进入新障碍物A2的影响范围后，此时
Figure BDA0003748910860000066
自动驾驶车辆同时受到两个障碍物的影响；横向子目标点的选择应保证自动驾驶车辆从一侧避开两个障碍物时的最大横向偏差e尽可能小；
假设障碍物中心到全局路径的距离为d，在避开不同障碍物时，影响半径ρ0减去距离d得到S，以衡量横向偏差e；
S＝ρ0-d
Smax＝{S1，S2，…，SN}max；
根据最大值S2对应的障碍物A2，根据情况一中子目标点的选择方法确定纵向子目标点O2和和横向子目标点M2；所述横向子目标点M2的坐标(xM2,yM2)T的函数公式为：
xM2＝(xR2-xobsA2)cosθ-(yR2-yobsA2)sinθ+xobsA2
yM2＝(xR2-xobsA2)sinθ+(yR2-yobsA2)cosθ+yobsA2
其中，障碍物A2的中心坐标为(xobsA2，yobsA2)，预定路径和障碍物A2的影响范围的交点为R2和O2，R2的坐标为XR2＝(xR2,yR2)T，O2的坐标为XO2＝(xO2,yO2)T；
当自动驾驶车辆刚进入障碍物A1的影响范围时，
Figure BDA0003748910860000071
Figure BDA0003748910860000072
基于障碍物A1建立子目标点引力势场；自动驾驶车辆进入障碍物A2的影响范围后X∈Circle A2，基于障碍物A2建立子目标点引力势场；子目标点的合力函数公式为：
Figure BDA0003748910860000073
其中，wA1是为障碍物A1确定的权重系数；wA2是基于障碍物A2确定的权重系数；FattO1表示障碍物A1纵向子目标点的引力函数；FattM1表示障碍物A1横向子目标点的引力函数；FattO2表示障碍物A2纵向子目标点的引力函数；FattM2表示障碍物A2横向子目标点的引力函数。
在一些可能的实施方式中，情况三中，所述“自动驾驶车辆在行驶过程中间接受多个障碍物影响”时，这种情况满足N个障碍物的影响范围相交，并且
Figure BDA0003748910860000081
其中
Figure BDA0003748910860000082
为空集，子目标点的选择方法为：
根据情况一中子目标点的选择方法确定确定避开障碍物A1的纵向子目标点O1和横向子目标点M1；当即将避开障碍物A1时，自动驾驶车辆进入新障碍物A2的影响范围，此时，
Figure BDA0003748910860000083
舍弃子目标点M1和O1，并根据障碍物A2选择子目标点；
确定了全局路径的交点和障碍物A2的影响范围为R2和O2，R2和O2的坐标分别为(xR2,yR2)T和(xO2,yO2)T；点O2被选择为纵向子目标点；由于自动驾驶车辆未返回全局路径，因此继续在当前侧避开障碍物A2会更安全；因此，点R2绕障碍物A2旋转角度θ至自动驾驶车辆所在侧，以获得横向子目标点M2，其坐标(xM2,yM2)T的函数公式为：
xM2＝(xR-xobsA2)cosθ-(yR-yobsA2)sinθ+xobsA2
yM2＝(xR-xobsA2)sinθ+(yR-yobsA2)cosθ+yobsA2；
当自动驾驶车辆仅在障碍物A1的影响范围内时，
Figure BDA0003748910860000084
Figure BDA0003748910860000085
基于障碍物A1建立子目标点引力势场；自动驾驶车辆进入障碍物A2的影响范围后时，X∈Circle A2，障碍物A2的影响是主要的，基于障碍物A2建立子目标点的引力势场；子目标点的合力的函数公式为：
Figure BDA0003748910860000086
在一些可能的实施方式中，基于所述权重系数w，建立自动驾驶车辆的合势场，所述自动驾驶车辆的合势场Utotal和势场力Ftotal的函数公式为：
Figure BDA0003748910860000091
Figure BDA0003748910860000092
其中，Upath是横向路径引力势场，Ug是纵向路径引力势场，Uatt是子目标点的引力势场，Urepv是速度斥力势场，Urep是障碍物斥力势场，Fpath是横向路径引力势场的势场力，Fg是纵向路径引力势场的势场力，Fatt是子目标点的引力势场的势场力，Frepv是速度斥力势场的势场力，Frep是障碍物斥力势场的势场力；
当ρ(X,Xobs)＞ρ0时，即自动驾驶车辆与障碍物的距离大于障碍物的影响半径，自动驾驶车辆处于路径跟踪状态，所以只有路径引力势场是起作用的；
当ρ(X,Xobs)≦ρ0∩∠RAX≦θ时，自动驾驶车辆进入障碍物的影响范围，路径引力势场阻碍自动驾驶车辆转向避障，路径引力势场Upath+Ug再转向开始时为0然后逐渐增加；
当ρ(X,Xobs)≦ρ0∩∠RAX＞θ时，在路径重力势场和纵向子目标重力场的共同作用下，自自动驾驶车辆快速返回全局路径。
在一些可能的实施方式中，所述自动驾驶车辆的移动方向是势场合力的方向，自动驾驶车辆的航向角为：
Figure BDA0003748910860000093
其中，Ftotalx为势场合力在X方向的分量，Ftotaly为势场合力在Y方向的分量；所述航向角一阶导数阈值设为
Figure BDA0003748910860000101
如果：
Figure BDA0003748910860000102
那么：
Figure BDA0003748910860000103
如果：
Figure BDA0003748910860000104
那么：
Figure BDA0003748910860000105
本发明的第二方面，提供自动驾驶车辆局部避障及路径跟踪系统，所述系统在运行时实现上述自动驾驶车辆局部避障及路径跟踪方法的步骤。
本发明的第三方面，一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述自动驾驶车辆局部避障及路径跟踪方法的步骤或执行上述自动驾驶车辆局部避障及路径跟踪系统。
本发明的第四方面，提供一种车辆，所述车辆包括控制器，所述控制器实现上述自动驾驶车辆局部避障及路径跟踪方法或者上述自动驾驶车辆局部避障及路径跟踪系统。
本发明的有益效果在于：本发明利用全局路径并减少避障过程的计算量，基于全局路径建立路径引力势场使自动驾驶车辆对全局路径进行跟踪，同时在避障完成后快速返回全局路径；针对局部最优解问题，提出了由障碍物和全局路径信息选取子目标点的策略，建立子目标点引力场使自动驾驶车辆快速避开障碍物返回全局路径；为了提升动态避障能力，由自动驾驶车辆与障碍物的相对速度建立速度斥力势场。
附图说明
图1为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中车辆运动学模型图；
图2为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中路径势场作用位置图；
图3为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中路径引力场的示意图；
图4为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中相对速度影响示意图；
图5为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中自动驾驶车辆仅受一个障碍物影响示意图；
图6为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中权重系数w计算示意图；
图7为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中避障过程中的势场力作用示意图；
图8为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中自动驾驶车辆同时受多个障碍物影响示意图；
图9为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中自动驾驶车辆间接受多个障碍物影响示意图；
图10为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中合势场的三维模型图；
图11为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中局部避障与路径跟踪策略图；
图12为本发明实施例自动驾驶车辆局部避障及路径跟踪方法中自动驾驶车辆局部避障及路径跟踪方法整体步骤流程图。
具体实施方式
下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征能更易于被本领域技术人员理解，从而对本发明的保护范围做出更为清楚明确的界定。
为了使本发明的目的、技术方案更加清晰，下面结合附图对本发明实施方案进一步详细描述。
本实施例提供自动驾驶车辆局部避障及路径跟踪方法，参照附图12所示，具体包括以下步骤：
S1:建立动力学模型:假设自动驾驶车辆是刚体，参照附图1所示，建立一个线性二自由度自动驾驶车辆模型为：
Figure BDA0003748910860000121
其中，[x y ψ]T为自动驾驶车辆中心的位置和航向角。
建立自动驾驶车辆差速转向模型为:
Figure BDA0003748910860000122
其中，[υ ω]T为自动驾驶车辆的速度和角速度，vr、vl分别为自动驾驶车辆左右轮线速度；L是自动驾驶车辆的轴距。
S2:根据全局路径建立路径引力势场，对自动驾驶车辆施加虚拟重力，使其沿着预定路径行驶，使自动驾驶车辆在避开障碍物后能够按时返回到预定路径。全局路径方向在本申请中是假设的已知信息，参照附图2所示，建立路径引力势场具体包括以下步骤：
S21:：建立横向引力势场的函数，所述横向引力势场和横向引力的函数公式分别为：
Upath＝k1e2，Fpath＝2k1e；
其中，e为横向偏差，即车辆质心和全局路径之间的最短距离，k1为势场函数公式增益，Xsp为期望路径上最接近自动驾驶车辆的位置。所述横向引力方向垂直于全局路径，可以防止自动驾驶车辆偏离全局路径，并在偏离后快速返回全局路径。
S22：为保证自动驾驶车辆在没有障碍物干扰的情况下遵循预定轨迹，并控制其速度建立纵向引力势场的函数，所述纵向引力势场和纵向引力的函数公式分别为：
Figure BDA0003748910860000131
其中，L＇(x，y)为剩余的路径曲线；
Figure BDA0003748910860000132
为航向角的一阶导数；k2为势场函数公式增益；c为常数,c保证当自动驾驶车辆直线移动时，Fg的最大值为
Figure BDA0003748910860000133
而不是无穷大。
所述纵向引力势场随自动驾驶车辆行驶状态的变化而动态变化；纵向引力势场的势能根据自动驾驶车辆当前的横摆角速度来计算；纵向引力势场用于控制自动驾驶车辆在路径跟踪过程中沿全局路径方向行驶，在转向过程中控制自动驾驶车辆减速以保证横向稳定性；自动驾驶车辆在直线行驶时，纵向重力达到最大值，控制自动驾驶车辆达到最大速度。
所述纵向引力方向与全局路径中心线平行，使自动驾驶车辆沿预定路径方向移动，并控制无人车在转向过程中减速，以确保横向稳定性。直线行驶时，纵向引力达到最大值，并控制自动驾驶车辆达到最大速度。
S23：对横向引力势场和纵向引力势场求和得到路径引力势场。
参照附图3所示，图3为路径引力场的示意图，该路径引力场是由纵向引力势场和横向引力势场求和得到的。
根据纵向势场力大小控制移动速度时，能满足自动驾驶车辆直线移动时高速要求和转向时减速到合理速度的要求。建立了基于纵向势场的速度公式，所述纵向引力势场的速度公式为：
v(t)＝k3Fg；
其中，k3为增益系数；t为时间。
为了保证自动驾驶车辆加减速过程的平顺性，所述自动驾驶车辆最大加速度阈值设定为amax，
如果：[v(t)-v(t-Δt)]/Δt＞amax，那么：v(t)＝v(t-Δt)+amaxΔt；
如果：[v(t)-v(t-Δt)]/Δt＜-amax，那么：v(t)＝v(t-Δt)-amaxΔt。
S3:考虑到障碍物的速度，准确地描述驾驶环境，提高动态避障能力，根据自动驾驶车辆与障碍物的相对速度建立速度斥力势场，参照附图4所示，建立速度斥力势场，所述速度斥力势场和速度斥力的函数公式分别为：
Figure BDA0003748910860000141
Figure BDA0003748910860000142
其中，α是相对速度vor的方向与自动驾驶车辆和障碍物所在位置点组成的向量
Figure BDA0003748910860000143
之间的夹角，当
Figure BDA0003748910860000144
时，即自动驾驶车辆远离障碍物或当自动驾驶车辆不在速度斥力势场范围内时，不考虑速度斥力势场作用，速度斥力作用方向与相对速度方向相反，迫使自动驾驶车辆远离障碍物。ρ0是障碍物排斥的影响半径，该半径与障碍物的大小和自动驾驶车辆的速度成比例。Kv是增益系数，X是自动驾驶车辆的位置坐标，Xobs是障碍物的位置坐标，ρ(X，Xobs)是障碍物与自动驾驶车辆之间的距离。相对速度vor是自动驾驶车辆相对于动态障碍物的速度，如附图4所示，用自动驾驶车辆的速度v减动态障碍物的速度v0就是相对速度vor。
S4:根据障碍物的影响范围，选择子目标点，根据所述子目标点建立子目标点引力势场，使自动驾驶车辆快速避开障碍物返回全局路径。“建立子目标点引力势场”具体为：当自动驾驶车辆行驶到障碍物的影响范围时，选择子目标点，所述子目标点包括纵向子目标点和横向子目标点，根据选定的纵向子目标点和横向子目标点分别建立纵向子目标点引力场和横向子目标点引力场；所述横向子目标点引力场使自动驾驶车辆转向障碍物一侧，以避免引起局部最优解；所述纵向子目标点引力场使自动驾驶车辆以平滑的轨迹避开障碍物，返回全局路径。
子目标点的选择方法包括三种情况：
情况一：自动驾驶车辆在行驶过程中仅受一个障碍物影响时；子目标点的选择方法为：
参照附图5所示，障碍物A的中心坐标为(xobsA，yobsA)，预定路径的方程设为f(x,y)＝0，预定路径和障碍物A的影响范围的交点为R和O，其中R的坐标为XR＝(xR,yR)T，O的坐标为XO＝(xO,yO)T。
选择O点作为纵向子目标点，横向子目标点M选择在障碍物A的左侧或右侧，所述横向子目标点M在选择障碍物A的左侧或右侧的过程中应满足避障过程中最大横向偏差e尽可能小的原则，保证e尽可能小，找到Xsp即可保证最小，这样可以保证自动驾驶车辆在避障时接近预定路径，避免遇到其他障碍物，提高安全性。
当f(xobsA，yobsA)＝0时，可以确定障碍物A的中心位于预定路径上，横向子目标点M应选择在避障距离较短的一侧，避障距离可通过中心角∠RAO来衡量。在∠RAO<π的一侧，避障距离明显更短，因此在这一侧选择横向子目标点。当∠RAO＝π,需要选择预定路径左侧的横向子目标点，确定所选方向后，将点R旋转一个角度θ绕障碍物中心A到所选侧得到横向子目标点M。
在避障开始时，纵向子目标点的引力FattO大于横向子目标点引力FattM，这将驱使自动驾驶车辆接近障碍物，增加碰撞风险。为了使子目标点引力更为合理，设计如下原则对不同避障阶段的子目标点引力进行调节。当X∈扇形RAM时，横向子目标点引力FattM在合力中的比重应先大后小，纵向子目标点引力FattO比例应先小后大，这样可以使自动驾驶车辆顺利绕过障碍物；当X∈扇形MAO时，局部最优解不会出现在下一个避障过程中；因此，子目标点的合力仅包含纵向子目标点引力FattO，从而使自动驾驶车辆返回全局路径。RAM与MAO是对应三个点形成的扇形区域。
确定横向子目标点后，将点R绕障碍物中心A旋转一个角度θ到所选侧，得到横向子目标点M，所述横向子目标点M的坐标XM＝(xM,yM)T的函数公式为：xM＝(xR-xobsA)cosθ-(yR-yobsA)sinθ+xobsA；
yM＝(xR-xobsA)sinθ+(yR-yobsA)cosθ+yobsA；
在子目标点的合力的作用下，即所述纵向子目标点O和所述横向子目标点M的引力作用下，自动驾驶车辆能够有效地克服局部最优解，顺利避障，避障结束后快速返回全局路径，所述横向子目标点M和纵向子目标点O的引力函数公式分别为：
FattM＝-kρ(X，XM)；
FattO＝-kρ(X，XO)；
其中，k是路径引力势场的增益系数；ρ(X,XM)是自动驾驶车辆与横向目标点之间的距离；ρ(X,XO)是自动驾驶车辆与纵向目标点之间的距离。
在保证自动驾驶车辆运行轨迹平稳的同时，用权重系数w来调整子目标点的合力，所述权重系数w的函数公式为：
Figure BDA0003748910860000171
其中，||XC||是自动驾驶车辆到障碍物中心A和横向目标点M连线的最短距离；||RB||是R点到障碍物中心A和横向目标点M连线的最短距离，一般来说，||RB||是一个常数，
Figure BDA0003748910860000172
其中ρ0为障碍物的影响半径。因此，权重系数w取决于从自动驾驶车辆到连接横向子目标点和障碍物的线的距离||XC||，参照附图6所示，考虑权重系数w，子目标点势场合力表示为：
Fatt＝(1-w)FattO+wFattM；
在避障过程中，势场力作用参照附图7所示。
情况二：自动驾驶车辆在行驶过程中同时受若干个障碍物影响时，这种情况满足N个障碍物的影响范围相交，并且
Figure BDA0003748910860000173
其中
Figure BDA0003748910860000174
为空集。参照附图8所示，子目标点的选择方法为：
当自动驾驶车辆刚进入障碍物A1的影响范围时，根据情况一中子目标点的选择方法确定避开障碍物A1的纵向子目标点O1和横向子目标点M1；自动驾驶车辆进入新障碍物A2的影响范围后，此时
Figure BDA0003748910860000175
自动驾驶车辆同时受到两个障碍物的影响；横向子目标点的选择应保证自动驾驶车辆从一侧避开两个障碍物时的最大横向偏差e尽可能小，保证e尽可能小，找到Xsp即可保证最小，这是因为由于障碍物的大小不同，仅考虑障碍物的中心位置并不能准确计算出避障时的横向偏差e。
参照附图8，根据“假设障碍物中心到全局路径的距离为d，在避开不同障碍物时，影响半径ρ0减去距离d得到S”，S为用来衡量横向偏差e的值。
S＝ρ0-d；
Smax＝{S1，S2，...，SN}max；
根据最大值S2对应的障碍物A2，根据情况一中子目标点的选择方法确定纵向子目标点O2和和横向子目标点M2，所述横向子目标点M2的坐标(xM2,yM2)T的函数公式为：
xM2＝(xR2-xobsA2)cosθ+(yR2-yobsA2)sinθ+xobsA2；
yM2＝(xR2-xobsA2)sinθ+(yR2-yobsA2)cosθ+yobsA2；
其中，障碍物A2的中心坐标为(xobsA2，yobsA2)，预定路径和障碍物A2的影响范围的交点为R2和O2，R2的坐标为XR2＝(xR2,yR2)T，O2的坐标为XO2＝(xO2,yO2)T。
当自动驾驶车辆刚进入障碍物A1的影响范围时，
Figure BDA0003748910860000181
Figure BDA0003748910860000182
基于障碍物A1建立子目标点引力势场；自动驾驶车辆进入障碍物A2的影响范围后X∈Circle A2，基于障碍物A2建立子目标点引力势场；子目标点的合力函数公式为：
Figure BDA0003748910860000183
其中，wA1是为障碍物A1确定的权重系数；wA2是基于障碍物A2确定的权重系数；FattO1表示障碍物A1纵向子目标点的引力函数；FattM1表示障碍物A1横向子目标点的引力函数；FattO2表示障碍物A2纵向子目标点的引力函数；FattM2表示障碍物A2横向子目标点的引力函数。
情况三：自动驾驶车辆在行驶过程中间接受若干个障碍物影响时，这种情况满足N个障碍物的影响范围相交，并且
Figure BDA0003748910860000191
其中
Figure BDA0003748910860000192
为空集。参照附图9所示，子目标点的选择方法为：
根据情况一中仅受一个障碍物时子目标点的选择方法，确定避开障碍物A1的纵向子目标点O1和横向子目标点M1；当即将避开障碍物A1时，自动驾驶车辆进入新障碍物A2的影响范围，此时，
Figure BDA0003748910860000193
舍弃子目标点M1和O1，并根据障碍物A2选择子目标点。
确定预定路径和障碍物A2的影响范围的交点为R2和O2，R2和O2的坐标分别为(xR2,yR2)T和(xO2,yO2)T，点O2被选择为纵向子目标点，由于自动驾驶车辆未返回预定路径，因此继续在当前侧避开障碍物A2会更安全；因此，点R2绕障碍物A2旋转角度θ至自动驾驶车辆所在侧，以获得横向子目标点M2，其坐标(xM2,yM2)T的函数公式为：
xM2＝(xR-xobsA2)cosθ-(yR-yobsA2)sinθ+xobsA2；
yM2＝(xR-xobsA2)sinθ+(yR-yobsA2)cosθ+yobsA2；
当自动驾驶车辆仅在障碍物A1的影响范围内时，
Figure BDA0003748910860000194
Figure BDA0003748910860000195
基于障碍物A1建立子目标点引力势场。自动驾驶车辆进入障碍物A2的影响范围后时，X∈Circle A2，障碍物A2的影响是主要的，基于障碍物A2建立子目标点的引力势场。子目标点的合力的函数公式为：
Figure BDA0003748910860000196
参照附图10所示，自动驾驶车辆在A点进入障碍物的影响范围。此时，路径势场的横向引力阻碍转弯，纵向引力迫使自动驾驶车辆向障碍物行驶，都会增加碰撞风险。因此，障碍物影响范围内的合势场力不能是简单的累加。
基于所述权重系数w，参照附图4所示，建立自动驾驶车辆的合势场，所述自动驾驶车辆的合势场Utotal和势场力Ftotal的函数公式为：
Figure BDA0003748910860000201
Figure BDA0003748910860000202
其中，Upath是横向路径引力势场，Ug是纵向路径引力势场，Uatt是子目标点的引力势场，Urepv是速度斥力势场，Urep是障碍物斥力势场，Fpath是横向路径引力势场的势场力，Fg是纵向路径引力势场的势场力，Fatt是子目标点的引力势场的势场力，Frepv是速度斥力势场的势场力，Frep是障碍物斥力势场的势场力。
参照附图11所示，局部避障和路径跟踪策略如下：
当ρ(X,Xobs)＞ρ0时，即自动驾驶车辆与障碍物的距离大于障碍物的影响半径，自动驾驶车辆处于路径跟踪状态，所以只有路径引力势场是起作用的。
当ρ(X,Xobs)≤ρ0∩∠RAX≤θ时，自动驾驶车辆进入障碍物的影响范围，路径引力势场阻碍自动驾驶车辆转向避障，路径引力势场Upath+Ug再转向开始时为0然后逐渐增加。
当ρ(X,Xobs)≤ρ0∩∠RAX＞θ时，在路径重力势场和纵向子目标重力场的共同作用下，自动驾驶车辆快速返回全局路径。
所述自动驾驶车辆的移动方向是势场合力的方向，自动驾驶车辆的航向角为：
Figure BDA0003748910860000211
其中，Ftotalx为势场合力在X方向的分量，Ftotaly为势场合力在Y方向的分量；所述航向角一阶导数阈值设为
Figure BDA0003748910860000212
本发明利用全局路径并减少避障过程的计算量，基于全局路径建立路径引力势场使自动驾驶车辆对全局路径进行跟踪，同时在避障完成后快速返回全局路径。针对局部最优解问题，提出了由障碍物和全局路径信息选取子目标点的策略，建立子目标点引力场使自动驾驶车辆快速避开障碍物返回全局路径。为了提升动态避障能力，由自动驾驶车辆与障碍物的相对速度建立速度斥力势场。
本实施例还提供自动驾驶车辆局部避障及路径跟踪系统，所述系统在运行时实现上述自动驾驶车辆局部避障及路径跟踪方法的步骤。
本实施例又提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述自动驾驶车辆局部避障及路径跟踪方法的步骤或执行上述自动驾驶车辆局部避障及路径跟踪系统。
本实施例又提供一种车辆，所述车辆包括控制器，所述控制器实现上述自动驾驶车辆局部避障及路径跟踪方法或者上述自动驾驶车辆局部避障及路径跟踪系统。
其中，处理器还可以称为CPU(Central Processing Unit，中央处理单元)。处理器可能是一种集成电路芯片，具有信号的处理能力。处理器可以是：
DSP(Digital Signal Processor，数字信号处理器，数字信号处理器是由大规模或超大规模集成电路芯片组成的用来完成某种信号处理任务的处理器。它是为适应高速实时信号处理任务的需要而逐渐发展起来的。随着集成电路技术和数字信号处理算法的发展，数字信号处理器的实现方法也在不断变化，处理功能不断提高和扩大。)
ASIC(Application Specific Integrated Circuit，专用集成电路，即专用集成电路，是指应特定用户要求和特定电子系统的需要而设计、制造的集成电路。
FPGA(现场可编程门阵列，Field Programmable Gate Array)是在PAL(Programmable Array Logic，可编程阵列逻辑)、GAL(generic array logic，通用阵列逻辑)等可编程器件的基础上进一步发展的产物。它是作为专用集成电路(ASIC)领域中的一种半定制电路而出现的，既解决了定制电路的不足，又克服了原有可编程器件门电路数有限的缺点。
通用处理器，所述通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
其他可编程逻辑器件、分立门或者晶体管逻辑器件和分立硬件组件等。
存储介质存储有能够实现上述所有方法的程序指令，其中，该程序指令可以以软件产品的形式存储在上述存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本发明各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质，或者是计算机、服务器、手机、平板等终端设备。
以上实施方式只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人了解本发明的内容并加以实施，并不能以此限制本发明的保护范围，凡根据本发明精神实质所做的等效变化或修饰，都应涵盖在本发明的保护范围内。

Claims (29)

1.自动驾驶车辆避障及路径跟踪方法，其特征在于，包括：
S1:建立动力学模型:
建立自动驾驶车辆模型和自动驾驶车辆差速转向模型；
S2:建立路径引力势场:
根据全局路径建立路径引力势场，对自动驾驶车辆施加虚拟重力，使其沿着预定路径行驶，保证自动驾驶车辆在避开障碍物后能够按时返回到预定路径；
S3:建立速度斥力势场:
根据自动驾驶车辆与障碍物的相对速度建立速度斥力势场，提升自动驾驶车辆动态避障能力；
S4:建立子目标点引力势场:
根据障碍物的影响范围，选择子目标点，根据所述子目标点建立子目标点引力势场，使自动驾驶车辆快速避开障碍物返回全局路径。
2.根据权利要求1所述的自动驾驶车辆避障及路径跟踪方法，其特征在于，假设所述自动驾驶车辆是刚体，建立一个线性二自由度自动驾驶车辆模型为：
Figure FDA0003748910850000011
其中，[x y ψ]T为自动驾驶车辆中心的位置和航向角。
3.根据权利要求1所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述自动驾驶车辆差速转向模型为:
Figure FDA0003748910850000012
其中，[υ ω]T为自动驾驶车辆的速度和角速度，vr、vl分别为自动驾驶车辆左右轮线速度；L是自动驾驶车辆的轴距。
4.根据权利要求1所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述“建立路径引力势场”包括：
建立横向引力势场；
建立纵向引力势场；
对横向引力势场和纵向引力势场求和得到路径引力势场。
5.根据权利要求4所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述横向引力势场和横向引力的函数公式分别为：
Upath＝k1e2；
Fpath＝2k1e；
其中，e为横向偏差，即车辆质心和全局路径之间的最短距离，k1为势场函数公式增益。
6.根据权利要求5所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述横向引力方向垂直于全局路径。
7.根据权利要求4所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述纵向引力势场和纵向引力的函数公式分别为：
Figure FDA0003748910850000021
Figure FDA0003748910850000022
其中，L＇(x，y)为剩余的路径曲线；
Figure FDA0003748910850000023
为航向角的一阶导数；k2为势场函数公式增益；c为常数,c保证当自动驾驶车辆直线移动时，Fg的最大值为
Figure FDA0003748910850000024
而不是无穷大。
8.根据权利要求7所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述纵向引力方向与全局路径中心线平行。
9.根据权利要求7所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述纵向引力势场的速度公式为：
v(t)＝k3Fg；
其中，k3为增益系数；t为时间。
10.根据权利要求9所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述自动驾驶车辆最大加速度阈值设定为amax，
如果：[v(t)-v(t-Δt)]/Δt＞amax，那么：v(t)＝v(t-Δt)+amaxΔt；
如果：[v(t)-v(t-Δt)]/Δt＜-amax，那么：v(t)＝v(t-Δt)-amaxΔt。
11.根据权利要求1所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述速度斥力势场和速度斥力的函数公式分别为：
Figure FDA0003748910850000031
Figure FDA0003748910850000032
其中，α是相对速度vor的方向与自动驾驶车辆和障碍物所在位置点组成的向量
Figure FDA0003748910850000033
之间的夹角；ρ0是障碍物排斥的影响半径，该半径与障碍物的大小和自动驾驶车辆的速度成比例；Kv是增益系数，X是自动驾驶车辆的位置坐标，Xobs是障碍物的位置坐标，ρ(X，Xobs)是障碍物与车辆之间的距离。
12.根据权利要求11所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，当
Figure FDA0003748910850000034
时，即自动驾驶车辆远离障碍物或当自动驾驶车辆不在速度斥力势场范围内时，不考虑速度斥力势场作用，速度斥力作用方向与相对速度方向相反，迫使自动驾驶车辆远离障碍物。
13.根据权利要求1所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述建立子目标点引力势场具体为：当自动驾驶车辆行驶到障碍物的影响范围时，选择子目标点，所述子目标点包括纵向子目标点和横向子目标点；根据选定的纵向子目标点和横向子目标点分别建立纵向子目标点引力场和横向子目标点引力场。
14.根据权利要求1所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述横向子目标点引力场使自动驾驶车辆转向障碍物一侧，以避免共线斥力和引力引起的局部最优解；所述纵向子目标点引力场使自动驾驶车辆以平滑的轨迹避开障碍物，返回全局路径。
15.根据权利要求13所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述子目标点的选择方法包括三种情况：
情况一：自动驾驶车辆在行驶过程中仅受一个障碍物影响；
情况二：自动驾驶车辆在行驶过程中同时受若干个障碍物影响；
情况三：自动驾驶车辆在行驶过程中间接受若干个障碍物影响。
16.根据权利要求15所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，情况一，所述“自动驾驶车辆在行驶过程中仅受一个障碍物影响”时，子目标点的选择方法为：
设障碍物A的中心坐标为(xobsA，yobsA)，预定路径和障碍物A的影响范围的交点为R和O，其中R的坐标为
Figure FDA0003748910850000041
O的坐标为
Figure FDA0003748910850000042
选择O点作为纵向子目标点，确定横向子目标点，将点R绕障碍物中心A旋转一个角度θ到所选侧，得到横向子目标点M，所述横向子目标点M的坐标
Figure FDA0003748910850000043
的函数公式为：
xM＝(xR-xobsA)cosθ-(yR-yobsA)sinθ+xobsA；
yM＝(xR-xobsA)sinθ+(yR-yobsA)cosθ+yobsA；
在子目标点的合力的作用下，即所述纵向子目标点O和所述横向子目标点M的引力作用下，自动驾驶车辆能够有效地克服局部最优解，顺利避障，避障结束后快速返回全局路径，所述横向子目标点M和纵向子目标点O的引力函数公式分别为：
FattM＝-kρ(X，XM)；
FattO＝-kρ(X，XO)；
其中，k是路径引力势场的增益系数；ρ(X，XM)是自动驾驶车辆与横向目标点之间的距离；ρ(X，XO)是自动驾驶车辆与纵向目标点之间的距离。
17.根据权利要求16所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述“确定横向子目标点”具体为：横向子目标点M选择在障碍物A的左侧或右侧，所述横向子目标点M在选择障碍物A的左侧或右侧的过程中应满足避障过程中最大横向偏差e尽可能小的原则。
18.根据权利要求17所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，当f(xobsA，yobsA)＝0时，可以确定障碍物A的中心位于预定路径上，横向子目标点M应选择在避障距离较短的一侧，避障距离可通过中心角∠RAO来衡量；在∠RAO＜π的一侧，避障距离明显更短，因此在这一侧选择横向子目标点；当∠RAO＝π，需要选择预定路径左侧的横向子目标点，确定所选方向后，将点R旋转一个角度θ绕障碍物中心A到所选侧得到横向子目标点M。
19.根据权利要求18所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，在避障开始时，纵向子目标点的引力FattO大于横向子目标点引力FattM，这将驱使自动驾驶车辆接近障碍物，增加碰撞风险；当
Figure FDA0003748910850000051
时，横向子目标点引力FattM在合力中的比重应先大后小，纵向子目标点引力FattO比例应先小后大，这样可以使自动驾驶车辆顺利绕过障碍物；当
Figure FDA0003748910850000061
时，局部最优解不会出现在下一个避障过程中；因此，子目标点的合力仅包含纵向子目标点引力FattO，从而使自动驾驶车辆返回全局路径。
20.根据权利要求19所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，在保证自动驾驶车辆运行轨迹平稳的同时，用权重系数w来调整子目标点的合力，子目标点势场合力表示为：
Fatt＝(1-w)FattO+wFattM；
所述权重系数w的函数公式为：
Figure FDA0003748910850000062
其中，||XC||是自动驾驶车辆到障碍物中心A和横向目标点M连线的最短距离；||RB||是R点到障碍物中心A和横向目标点M连线的最短距离。
21.根据权利要求20所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，||RB||是一个常数，
Figure FDA0003748910850000063
22.根据权利要求15所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，情况二中，所述“自动驾驶车辆在行驶过程中同时受若干个障碍物影响”时，这种情况满足N个障碍物的影响范围相交，并且
Figure FDA0003748910850000064
其中
Figure FDA0003748910850000065
为空集，子目标点的选择方法为：
当自动驾驶车辆刚进入障碍物A1的影响范围时，根据情况一中子目标点的选择方法确定避开障碍物A1的纵向子目标点O1和横向子目标点M1；
自动驾驶车辆进入新障碍物A2的影响范围后，此时
Figure FDA0003748910850000066
自动驾驶车辆同时受到两个障碍物的影响；横向子目标点的选择应保证自动驾驶车辆从一侧避开两个障碍物时的最大横向偏差e尽可能小；
假设障碍物中心到全局路径的距离为d，在避开不同障碍物时，影响半径ρ0减去距离d得到S，以衡量横向偏差e；
S＝ρ0-d；
S max＝{S1，S2，...，SN}max；
根据最大值S2对应的障碍物A2，根据情况一中子目标点的选择方法确定纵向子目标点O2和和横向子目标点M2；所述横向子目标点M2的坐标
Figure FDA0003748910850000071
的函数公式为：
xM2＝(xR2-xobsA2)cosθ-(yR2-yobsA2)sinθ+xobsA2；
yM2＝(xR2-xobsA2)sinθ+(yR2-yobsA2)cosθ+yobsA2；
其中，障碍物A2的中心坐标为(xobsA2，yobsA2)，预定路径和障碍物A2的影响范围的交点为R2和O2，R2的坐标为
Figure FDA0003748910850000072
O2的坐标为
Figure FDA0003748910850000073
当自动驾驶车辆刚进入障碍物A1的影响范围时，
Figure FDA0003748910850000074
Figure FDA0003748910850000075
基于障碍物A1建立子目标点引力势场；自动驾驶车辆进入障碍物A2的影响范围后X∈Circle A2，基于障碍物A2建立子目标点引力势场；子目标点的合力函数公式为：
Figure FDA0003748910850000076
其中，wA1是为障碍物A1确定的权重系数；wA2是基于障碍物A2确定的权重系数；FattO1表示障碍物A1纵向子目标点的引力函数；FattM1表示障碍物A1横向子目标点的引力函数；FattO2表示障碍物A2纵向子目标点的引力函数；FattM2表示障碍物A2横向子目标点的引力函数。
23.根据权利要求15所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，情况三中，所述“自动驾驶车辆在行驶过程中间接受多个障碍物影响”时，这种情况满足N个障碍物的影响范围相交，并且
Figure FDA0003748910850000081
其中
Figure FDA0003748910850000082
为空集，子目标点的选择方法为：
根据情况一中子目标点的选择方法确定确定避开障碍物A1的纵向子目标点O1和横向子目标点M1；当即将避开障碍物A1时，自动驾驶车辆进入新障碍物A2的影响范围，此时，
Figure FDA0003748910850000083
舍弃子目标点M1和O1，并根据障碍物A2选择子目标点；
确定预定路径和障碍物A2的交点为R2和O2，R2和O2的坐标分别为
Figure FDA0003748910850000084
和
Figure FDA0003748910850000085
点O2被选择为纵向子目标点；由于自动驾驶车辆未返回全局路径，因此继续在当前侧避开障碍物A2会更安全；因此，点R2绕障碍物A2旋转角度θ至自动驾驶车辆所在侧，以获得横向子目标点M2，其坐标
Figure FDA0003748910850000086
的函数公式为：
xM2＝(xR-xobsA2)cosθ-(yR-yobsA2)sinθ+xobsA2；
yM2＝(xR-xobsA2)sinθ+(yR-yobsA2)cosθ+yobsA2；
当自动驾驶车辆仅在障碍物A1的影响范围内时，
Figure FDA0003748910850000087
Figure FDA0003748910850000088
基于障碍物A1建立子目标点引力势场；自动驾驶车辆进入障碍物A2的影响范围后时，X∈Circle A2，障碍物A2的影响是主要的，基于障碍物A2建立子目标点的引力势场；子目标点的合力的函数公式为：
Figure FDA0003748910850000089
24.根据权利要求20所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，基于所述权重系数w，建立自动驾驶车辆的合势场，所述自动驾驶车辆的合势场Utotal和势场力Ftotal的函数公式为：
Figure FDA0003748910850000091
Figure FDA0003748910850000092
其中，Upath是横向路径引力势场，Ug是纵向路径引力势场，Uatt是子目标点的引力势场，Urepv是速度斥力势场，Urep是障碍物斥力势场，Fpath是横向路径引力势场的势场力，Fg是纵向路径引力势场的势场力，Fatt是子目标点的引力势场的势场力，Frepv是速度斥力势场的势场力，Frep是障碍物斥力势场的势场力。
25.根据权利要求24所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，当p(X，Xobs)＞ρ0时，即自动驾驶车辆与障碍物的距离大于障碍物的影响半径，自动驾驶车辆处于路径跟踪状态，所以只有路径引力势场是起作用的；
当ρ(X，Xobs)≤ρ0∩∠RAX≤θ时，自动驾驶车辆进入障碍物的影响范围，路径引力势场阻碍自动驾驶车辆转向避障，路径引力势场Upath+Ug再转向开始时为0然后逐渐增加；
当ρ(X，Xobs)≤ρ0∩∠RAX＞θ时，在路径重力势场和纵向子目标重力场的共同作用下，自自动驾驶车辆快速返回全局路径。
26.根据权利要求24所述的自动驾驶车辆局部避障及路径跟踪方法，其特征在于，所述自动驾驶车辆的移动方向是势场合力的方向，自动驾驶车辆的航向角为：
Figure FDA0003748910850000101
其中，Ftotalx为势场合力在X方向的分量，Ftotaly为势场合力在Y方向的分量；所述航向角一阶导数阈值设为
Figure FDA0003748910850000102
如果：
Figure FDA0003748910850000103
那么：
Figure FDA0003748910850000104
如果：
Figure FDA0003748910850000105
那么：
Figure FDA0003748910850000106
27.自动驾驶车辆局部避障及路径跟踪系统，其特征在于，所述系统在运行时实现权利要求1-26中任一项所述的自动驾驶车辆局部避障及路径跟踪方法的步骤。
28.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1-26任一项所述的自动驾驶车辆局部避障及路径跟踪方法的步骤或执行权利要求27所述的自动驾驶车辆局部避障及路径跟踪系统。
29.一种车辆，其特征在于，所述车辆包括控制器，所述控制器实现权利要求1-26任一项所述的自动驾驶车辆局部避障及路径跟踪方法或者权利要求27所述自动驾驶车辆局部避障及路径跟踪系统。
CN202210832265.2A 2022-07-15 2022-07-15 自动驾驶车辆局部避障及路径跟踪方法、系统及存储介质 Pending CN115061478A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210832265.2A CN115061478A (zh) 2022-07-15 2022-07-15 自动驾驶车辆局部避障及路径跟踪方法、系统及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210832265.2A CN115061478A (zh) 2022-07-15 2022-07-15 自动驾驶车辆局部避障及路径跟踪方法、系统及存储介质

Publications (1)

Publication Number Publication Date
CN115061478A true CN115061478A (zh) 2022-09-16

Family

ID=83206777

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210832265.2A Pending CN115061478A (zh) 2022-07-15 2022-07-15 自动驾驶车辆局部避障及路径跟踪方法、系统及存储介质

Country Status (1)

Country Link
CN (1) CN115061478A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117584952A (zh) * 2024-01-16 2024-02-23 北京理工大学 一种越野环境的动态人工势场构建方法、系统及电子设备

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117584952A (zh) * 2024-01-16 2024-02-23 北京理工大学 一种越野环境的动态人工势场构建方法、系统及电子设备

Similar Documents

Publication Publication Date Title
GB2570887A (en) A system for a vehicle
JP2769052B2 (ja) 自律移動機械、移動機械の制御装置及び方法
CN112810630B (zh) 一种自动驾驶车辆轨迹规划方法及系统
JP2669074B2 (ja) 自律走行車両の車速感応操舵制御装置
CN111289978A (zh) 一种车辆无人驾驶行为决策的方法和系统
JP2003241836A (ja) 自走移動体の制御方法および装置
CN110877610A (zh) 碰撞回避装置
WO2023010854A1 (zh) 路径跟踪方法、装置、车辆及存储介质
JP4670807B2 (ja) 移動経路作成方法、自律移動体および自律移動体制御システム
Gehrig et al. A trajectory-based approach for the lateral control of car following systems
KR101278708B1 (ko) 수중로봇과 그 제어방법
US11712802B2 (en) Construction constrained motion primitives from robot maps
Zhuge et al. A novel dynamic obstacle avoidance algorithm based on collision time histogram
CN112577506A (zh) 一种自动驾驶局部路径规划方法和系统
CN115061478A (zh) 自动驾驶车辆局部避障及路径跟踪方法、系统及存储介质
Osman et al. Vision based lane reference detection and tracking control of an automated guided vehicle
CN114791734A (zh) 一种基于语义信息串联的履带车辆的控制方法及系统
CN113359740A (zh) 一种轮式移动机器人及其控制方法、控制系统、存储介质
Kim et al. A heuristic obstacle avoidance algorithm using vanishing point and obstacle angle
WO2021138475A1 (en) Vehicle control to join and depart a route
Wang et al. Towards robust autonomous coverage navigation for carlike robots
JP7300068B2 (ja) 車両制御装置、車両制御方法、及び車両制御システム
Yang et al. ESO-based lateral control for electrical vehicles with unmodeled tire dynamics on uneven road
CN117950395A (zh) 轨迹规划方法及装置、移动工具、存储介质
Filatov et al. Autonomous parking control system of four-wheeled vehicle

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