CN114371712B - 一种具有不停车绕障功能的泊车轨迹重规划方法 - Google Patents

一种具有不停车绕障功能的泊车轨迹重规划方法 Download PDF

Info

Publication number
CN114371712B
CN114371712B CN202210024530.4A CN202210024530A CN114371712B CN 114371712 B CN114371712 B CN 114371712B CN 202210024530 A CN202210024530 A CN 202210024530A CN 114371712 B CN114371712 B CN 114371712B
Authority
CN
China
Prior art keywords
vehicle
track
time
proposition
trajectory
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
Application number
CN202210024530.4A
Other languages
English (en)
Other versions
CN114371712A (zh
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.)
Hunan University
Original Assignee
Hunan University
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 Hunan University filed Critical Hunan University
Priority to CN202210024530.4A priority Critical patent/CN114371712B/zh
Publication of CN114371712A publication Critical patent/CN114371712A/zh
Application granted granted Critical
Publication of CN114371712B publication Critical patent/CN114371712B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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

本发明公开了一种具有不停车绕障功能的泊车轨迹重规划方法，包括假设在t＝t0时刻，车辆发现了前方突然出现的障碍物石块，车辆自动驾驶系统立刻执行模块A；同时在t＝t0时刻车辆自动驾驶系统开启一个异步的行程，并在该行程中执行模块B；要求车辆从t＝t0+TB时刻到t＝t0+TB+T时刻均沿着原始既定轨迹的路径行驶；在t＝t0+TB时刻执行模块C，推演车辆在t＝t0+TB+T时刻应该处于的位置信息，在t＝t0+TB时刻执行完模块C，立刻执行模块D生成一条trajectorynew；最后检查trajectorytransfer段是否能够避障。本具有不停车绕障功能的泊车轨迹重规划方法，能够大幅度降低车辆应对随时出现突发障碍物的反应时间，并且车辆不再需要降低车速驻车而损耗刹车片或浪费燃油。

Description

一种具有不停车绕障功能的泊车轨迹重规划方法
技术领域
本发明涉及泊车轨迹重规划技术领域，尤其涉及一种具有不停车绕障功能的泊车轨迹重规划方法。
背景技术
在矿上，给定矿用自动驾驶卡车的行车起点和终点位置姿态(以下简称位姿)之后，车辆将按照泊车轨迹规划技术生成一条理想的轨迹，随后矿用卡车将基于闭环控制技术跟踪该轨迹而行驶起来。但是，矿用卡车在矿区场景行驶的过程中，矿区有很多大型石块会从山坡滚落，或者从其他运载车辆上滚落，如果这些石块落在车辆的泊车轨迹上，车辆如果继续沿着原始泊车轨迹行驶，势必会撞上这种突然出现的障碍物(也就是石块)，因此车辆需要进行在线重规划。在线轨迹重规划是指在车辆沿着既定轨迹规划结果进行跟踪行驶过程中，重新生成从车辆当前位置到既定终止位姿的一段局部轨迹。这段重新生成的局部轨迹段，应该具备避让突然出现的新障碍物(石块)的性质。
目前学术界和产业界现有技术往往要求车辆在沿着既定轨迹行驶过程中遭遇突然出现的新障碍物时，紧急停车。在车辆完全停止下来之后，以静止状态所处的位置姿态为新的起始位姿，按照新位置的技术方案去规划一段新的泊车轨迹。然而这种完全停车绕障技术会导致矿用卡车完成装在卸载停泊的行车耗时时长大幅度提升，运载效率大幅度下降，综合经济效益远比人类驾驶员差。
综上所述，需要提出一种具有不停车绕障功能的泊车轨迹重规划方法，以解决上述问题。
发明内容
本发明所要解决的技术问题在于，针对现有矿上自动驾驶卡车在遇到障碍物时，需要停车重新规划行驶轨迹，导致行车耗时时长大幅度提升，运载效率大幅度下降，综合经济效益差的问题，提出了一种具有不停车绕障功能的泊车轨迹重规划方法。
为了解决上述技术问题，本发明实施例提供了一种具有不停车绕障功能的泊车轨迹重规划方法，包括：
步骤一、在车辆沿着既定的轨迹跟踪行驶的过程中，既定的轨迹记为trajectoryold，假设在t＝t0时刻，车辆发现了前方突然出现的障碍物石块，车辆自动驾驶系统立刻执行模块A，模块A提供的技术包括计算刹车减速度abrake，使得车辆从t＝t0时刻开始实施刹车制动；
步骤二、同时在t＝t0时刻，车辆自动驾驶系统开启一个异步的行程，并在该行程中执行模块B，模块B负责生成一条从车辆在t＝t0时刻所处位置姿态到终止位姿的行车轨迹，记为trajectorynew；
步骤三、在t＝t0+TB时刻得到可以绕过障碍物的行车轨迹trajectorynew之后，指定一个参数T>0，要求车辆从t＝t0+TB时刻到t＝t0+TB+T时刻均沿着原始既定轨迹的路径行驶，并且刹车加速度仍旧为abrake；在t＝t0+TB时刻，按照模块C提供的技术去推演车辆在t＝t0+TB+T时刻的位置坐标值、姿态角度、速率、前轮角度信息，并将这些信息记为X，在t＝t0+TB时刻执行完模块C而确定X之后，立刻执行模块D生成一条以X为起始位姿快速向trajectorynew进行衔接的平滑过渡局部轨迹，将这条局部轨迹记录为trajectorytransfer；
步骤四、最后检查trajectorytransfer段是否能够避障。
其中，步骤一中，模块A计算刹车减速度abrake的技术方案为：当感知到突然出现的障碍物时的车辆位置与障碍物之间的距离为S，则要求车辆最迟也能够在0.8*S里程之处停止下来，假设在故障发生时刻车辆的速度为v0，则可以按照以下公式反推出abrake：
进而有：
其中，步骤二中模块B采用计算最优控制方法规划行车轨迹trajectorynew，计算最优控制方法的主要步骤包括：1)构建最优控制命题，2)将其离散化为非线性规划命题，以及3)利用梯度优化方法求解该非线性规划命题；
构建最优控制命题：
一个完整的面向轨迹规划任务的最优控制命题包括车辆运动学约束、两点边值约束、碰撞躲避约束以及一个代价函数；
车辆运动学约束：
车辆运动学约束用于描述车辆在二维平面上的运动能力，在车速不高的情况下，二自由度模型已能够符合实际需求，二自由度模型将车辆的两只前轮及两只后轮分别向车体纵轴方向合并为虚拟单轮，通过确定虚拟前轮的转动角速度以及虚拟后轮的加速度，可确定车辆的前轮转角、行驶速度；根据二自由度模型，车辆在惯性坐标系X-Y中的运动过程受到以下微分方程组的制约：
t∈[0，tf]；
其中终止时刻tf是待定变量；(x(t)，y(t))代表车辆后轮轴中点坐标；v(t)及a(t)分别代表沿车体纵轴方向的速度及加速度，以使车辆前进的方向为正；φ(t)为车辆前轮偏转角，以左偏为正；ω(t)为前轮偏转角速度；θ(t)代表车辆在坐标系中的姿态角，即从X轴正方向到车体纵轴正方向的角度，以逆时针转向为正；此外，还定义了车辆的几何尺寸相关参数：Lw代表前后轮轴距，Lf代表车辆前悬距离，Lr代表后悬距离，Lb代表车宽，车辆内在的机械特性对应着状态/控制变量的容许作用区间，一般包括：
Φmax≤φ(t)≤Φmax，
vmin≤v(t)≤vmax，
amin≤a(t)≤amax，
-Ωmax≤ω(t)≤Ωmax，t∈[0，tf]；
被微分的变量包括x(t)、y(t)、φ(t)、θ(t)以及v(t)，可被微分的变量一定对自变量t连续，将其称为状态变量；未被微分的变量包括a(t)和ω(t)，这两个变量不一定对时间t连续，将其称为控制变量；
两点边值约束：
在车辆运动的起始时刻t＝0，应显式地指定车辆所处的运动状态，即
[v(0)，φ(0)，a(0)，ω(0)，x(0)，y(0)，θ(0)]＝[p1，p2，p3，p4，p5，p6，p7]，其中[p1，p2，p3，p4，p5，p6，p7]对应着由车载传感器记录/推算的当前时刻(t＝0)的运动状态客观实际情况；在车辆运动的终止时刻tf，也需对车辆运动状态进行限制：
[v(tf)，φ(tf)，a(tf)，ω(tf)，x(tf)，y(tf)，θ(tf)]＝[p8，p9，p10，p11，p12，p13，p14]；
碰撞躲避约束：
碰撞躲避约束要求车辆在整个运动时域[0，tf]上避免与环境中的任何静止、移动障碍物发生碰撞；假设X-Y坐标系中存在NOBS个静止的凸多边形障碍物，其中第j个障碍物包含NVj个顶点针对车身与凸多边形障碍物j之间的碰撞躲避约束进行建模，在二维平面上，两个凸多边形从尚未发生碰撞到发生碰撞的整个过程中，碰撞发生的瞬间一定出现了某一凸多边形顶点撞入了另一凸多边形中的事件；将点P与凸多边形每两个相邻顶点分别组成三角形，并将这些三角形的面积累加，如果面积之和大于凸多边形，则点P处于凸多边形外部，否则点P处在多边形的轮廓上或内部，据此，点P处于凸多边形Q1～Qn外部的约束条件可正式建立为：
其中SΔ代表相应三角形面积，代表凸多边形面积，SΔ应基于三角形顶点坐标书写，以/>为例，假设P＝(x，y)、Qk＝(xQk，yQk)、Qk+1＝(xQ(k+1)，yQ(k+1))，则有：
其中是常值；
将点P处于凸多边形Q1～Qn外部的一般性约束条件简记为PointOutOfPolygon(P，Q1...Qn)，据此可建立第j个障碍物与车身矩形A(t)B(t)C(t)D(t)的碰撞躲避约束条件，完整的碰撞躲避约束条件可写为：
j＝1，...，NOBS，t∈[0，tf]；
进一步简写为：
代价函数：
期待泊车运动过程尽早完成，因此设置：J＝tf；
完整的最优控制问题：将以上的约束条件和代价函数汇总起来即得到面向泊车轨迹规划任务的最优控制问题。
将其离散化为非线性规划命题：
最优控制问题可简记为以下形式：
min tf，
G(x(t)，u(t))≤0，t∈[0，tf]；
其中x(t)代表状态变量，u(t)代表控制变量，求解命题即确定符合约束条件的控制变量u(t)以及时域长度tf，使得末值型代价函数tf得以最小化；
首先，定义(Nfe+1)个采样时刻{tk|k＝0，...，Nfe}，要求这些采样时刻均匀分布于时域[0，tf]上，即：
0＝t0<t1<t2<...<tNfe＝tf，
第二步，引入一系列变量{uk|k＝0，...，Nfe}、{zk|k＝0，...，Nfe}来分别表征u(t)、x(t)，求解u(t)、x(t)这一原始任务转化为求解它们在一系列采样时刻的取值，即求解变量{uk}、{xk}的取值，代价函数变为代数等式/不等式G(x(t)，u(t))≤0，t∈[0，tf]变为G(uk，xk)≤0，k＝0，...，Nfe，微分等式的变化方式稍微复杂，将dx(t)/dt＝F(x(t)，u(t))写以下等价形式：
其中无论变量a如何选取，上式一定成立，现将式中的t取为tk，并令a＝tk-1，则有：
为去除复杂的积分运算，可将被积函数整体近似为一个常值Const，即：
由于函数F(·，·)本身不可能是常值函数，上式的成立意味着x(t)、u(t)在子区间t∈(tk-1，tk)上取值恒定，即：
其中Const1、Const2为常值，即：
x(tk)＝x(tk-1)+Const·(tk-tk-1)，
进而：
xk＝xk-1+Const·hk；
选取子区间端点处的变量值，即：
或者：
则：
xk＝xk-1+F(xk-1，uk-1)·hk；
在所有子区间(tk-1，tk)上重复上述转化，则可形成一个完整的非线性规划命题：
s.t.xk＝xk-1+F(xk-1，uk-1)·(tf/Nfe)，k＝1，...，Nfe；
G(uk，xk)≤0，k＝0，...，Nfe；
利用梯度优化方法求解该非线性规划命题：
首先，待求解的非线性规划命题可精炼地描述为：
其中代表由优化变量组成的向量，即解向量，通过引入松弛向量s>0，可将不等式约束/>转化为等式约束，即：
此时，如将命题中唯一的不等式约束条件s>0转化为内罚函数项补入代价函数J，则可构造仅包含等式约束的标准形式非线性规划命题：
其中，代表包含内罚函数项的代价函数；μIPM>0是障碍因子，其取值越趋于0+则通过内罚函数μIPM·ln(s)描述不等式s>0的精准程度越高。
其中，步骤三中模块C负责确定车辆在t＝t0+TB+T时刻的位置坐标值、姿态角度、速率、前轮角度等状态/控制变量的取值，车辆在t＝t0+TB+T时刻较之于t＝t0时刻向前继续行驶了以下公式所示的长度：
v0·(TB+T)+0.5·abrake·(TB+T)2；
在轨迹上找到相应点位，提取该点的位置坐标、姿态角、前轮转角角度、车速等信息即可。
其中，步骤三中模块D负责生成一条以X为起始位姿快速向trajectorynew进行衔接的平滑过渡局部轨迹trajectorytransfer，这条轨迹的生成方式仍旧是以计算最优控制方法求解一个最优控制命题，开设N个并行化的线程，分别将trajectorynew未来的N个可能的点位设置为衔接点Y，随后利用并行计算架构同时计算从X到每一个Y的备选行车轨迹，将所有N条备选的行车轨迹中最短的那一条记为trajectorytransfer；这N条衔接轨迹的计算，可按照模块B介绍的方法生成从X到Y的一条轨迹，每一条备用轨迹的计算过程仍旧包含三个步骤，1)构建最优控制命题，2)将其离散化为非线性规划命题，3)利用梯度优化方法求解该非线性规划命题；
将轨迹规划任务构建为最优控制命题：
一个面向轨迹规划任务的最优控制命题，在模块D之中，仅仅包括车辆运动学约束、两点边值约束以及一个代价函数，不再有碰撞躲避约束；
车辆运动学约束：
t∈[0，tf]；
以及：
-Φmax≤φ(t)≤Φmax，
vmin≤v(t)≤vmax，
amin≤a(t)≤amax，
-Ωmax≤ω(t)≤Ωmax，t∈[0，tf]；
两点边值约束：将最优控制问题中提及的t＝0视为X点位的状态，即：
[v(0)，φ(0)，a(0)，ω(0)，x(0)，y(0)，θ(0)]＝[p1，p2，p3，p4，p5，p6，p7]，
其中[p1，p2，p3，p4，p5，p6，p7]对应着X点位运动状态，t＝tf时刻末端对应着Y状态：
[v(tf)，φ(tf)，a(tf)，ω(tf)，x(tf)，y(tf)，θ(tf)]＝[p8，p9，p10，P11，p12，p13，p14]；
代价函数：期待泊车迁移运动过程尽早完成，因此设置：
J＝tf；
完整的最优控制问题：将以上约束条件和代价函数汇总起来即得到面向局部迁移轨迹规划任务的最优控制问题。
将最优控制命题离散化为非线性规划命题：离散化过程同模块B；
利用梯度优化方法求解该非线性规划命题：求解过程同模块B。
其中，步骤四中检查trajectorytransfer段是否能够避障的具体方法为：如果该段轨迹未与环境中任何新/旧障碍物相撞，则认为该段轨迹是合法的，立刻构成最终的不停车绕障轨迹如下：在车辆于t＝t0+TB+T时刻抵达X之前，仍旧沿着旧轨迹的路径部分进行减速行驶，在抵达X之后则沿着trajectorytransfer行驶，直至车辆行驶到trajectorynew轨迹线上为止，随后继续沿着trajectorynew剩余部分继续行驶直至抵达终止位姿为止；如果检查trajectorytransfer段发生了碰撞，则车辆在抵达X之后继续沿着trajectoryold做减速行驶，按照模块C提供的技术去推演车辆在t＝t0+TB+2T时刻的位置坐标值、姿态角度、速率、前轮角度等信息，仍旧将这些信息记为X，在确定X之后，立刻执行模块D生成一条以X为起始位姿向trajectorynew进行衔接的平滑过渡局部轨迹trajectorytransfer，对其进行碰撞检查，如果发生碰撞则继续重复上述步骤直至车辆彻底停下为止；如果未发生碰撞则在X点位处衔接过渡段轨迹，随后拼接一段trajectorynew轨迹。
实施本发明实施例，具有如下有益效果：
本具有不停车绕障功能的泊车轨迹重规划方法，能够大幅度降低车辆应对随时出现突发障碍物的反应时间，并且车辆不再需要降低车速驻车而损耗刹车片或浪费燃油，综合经济效益较高。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为二自由度车辆运动学模型(自行车模型)；
图2为描述点P位于凸多边形Q1Q2Q3Q4Q5外部的三角形面积判据；
图3为不停车绕障轨迹重规划方案示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本具有不停车绕障功能的泊车轨迹重规划方法的构建方法包括：
步骤一、在车辆沿着既定的轨迹跟踪行驶的过程中，既定的轨迹记为trajectoryold，假设在t＝t0时刻，车辆发现了前方突然出现的障碍物石块，车辆自动驾驶系统立刻执行模块A，模块A提供的技术包括计算刹车减速度abrake，使得车辆从t＝t0时刻开始实施刹车制动。需要说明的是，车辆从t＝t0时刻开始随后仅仅是降低行车速率行驶，但是车辆仍旧沿着既定路径行驶。
模块A计算刹车减速度abrake的技术方案为：当感知到突然出现的障碍物时的车辆位置与障碍物之间的距离为S，这意味着如果车辆沿着既定轨迹trajectoryold继续行驶一段长度为S的里程则会与障碍物相撞。则要求车辆最迟也能够在0.8*S里程之处停止下来，假设在故障发生时刻车辆的速度为v0，则可以按照以下公式反推出abrake：
进而有：
车辆发现前方突然出现的障碍物石块的方法为现有技术：车辆通过感知系统实时监测车辆周围环境中的障碍物石块。具体而言，感知系统包括硬件传感器部分以及软件算法部分，硬件传感器包括视觉传感器，即摄像头，以及激光雷达。视觉传感器负责记录地表的图像，可以发现石块纹理与普通地表的差异；激光雷达利用了雷达回波原理，可探测凸出地表的障碍物精细的轮廓以及距离等。软件算法包括激光雷达点云聚类算法、点云与视觉数据融合技术(即融合感知技术)以及基于深度学习的物体识别(识别障碍物是石块而不是铁块)。此外，也可采用其他的车辆感知系统，只要能使车辆实时感知到前方是否有障碍物即可。
步骤二、同时在t＝t0时刻，车辆自动驾驶系统开启一个异步的行程，并在该行程中执行模块B，模块B负责生成一条从车辆在t＝t0时刻所处位置姿态到终止位姿的行车轨迹，记为trajectorynew。
模块B采用计算最优控制方法规划行车轨迹trajectorynew，计算最优控制方法的主要步骤包括：1)构建最优控制命题，2)将其离散化为非线性规划命题，3)利用梯度优化方法求解该非线性规划命题。
一、将轨迹规划任务构建为最优控制命题：
一个完整的面向轨迹规划任务的最优控制命题包括车辆运动学约束、两点边值约束、碰撞躲避约束以及一个代价函数；
车辆运动学约束：
车辆运动学约束用于描述车辆在二维平面上的运动能力，在车速不高的情况下，二自由度模型已能够符合实际需求，二自由度模型将车辆的两只前轮及两只后轮分别向车体纵轴方向合并为虚拟单轮，请参见图1，图1为二自由度车辆运动学模型(自行车模型)。通过确定虚拟前轮的转动角速度以及虚拟后轮的加速度，可确定车辆的前轮转角、行驶速度等。两只虚拟单轮的存在使得车辆从形态上类似于自行车，因此二自由度模型也称自行车模型。根据二自由度模型，车辆在惯性坐标系X-Y中的运动过程受到以下微分方程组的制约：
t∈[0,tf]；
其中终止时刻tf是待定变量；(x(t),y(t))代表图1中车辆后轮轴中点坐标；v(t)及a(t)分别代表沿车体纵轴方向的速度及加速度，以使车辆前进的方向为正；φ(t)为车辆前轮偏转角，以左偏为正；ω(t)为前轮偏转角速度；θ(t)代表车辆在坐标系中的姿态角，即从X轴正方向到车体纵轴正方向的角度，以逆时针转向为正；此外，图1中还定义了车辆的几何尺寸相关参数：Lw代表前后轮轴距，Lf代表车辆前悬距离，Lr代表后悬距离，Lb代表车宽，车辆内在的机械特性对应着状态/控制变量的容许作用区间，一般包括：
Φmax≤φ(t)≤Φmax，
vmin≤v(t)≤vmax，
amin≤a(t)≤amax，
-Ωmax≤ω(t)≤Ωmax，t∈[0，tf]；
被微分的变量包括x(t)、y(t)、φ(t)、θ(t)以及v(t)，可被微分的变量一定对自变量t连续，将其称为状态变量；未被微分的变量包括a(t)和ω(t)，这两个变量不一定对时间t连续，将其称为控制变量；之所以将其称为控制变量，是因为通过确定tf以及t∈[0，tf]上的a(t)、ω(t)可通过积分唯一确定所有剩余状态变量，a(t)和ω(t)体现掌控车辆运动规律的意味。轨迹规划任务的本质是寻找一个非线性系统的某一组控制变量u(t)＝[a(t)，ω(t)]，使得系统在受限条件下实现状态的最优迁移——最优控制问题正是精准、直接描述此类任务的方式。
两点边值约束：
在车辆运动的起始时刻t＝0，应显式地指定车辆所处的运动状态，即
[v(0)，φ(0)，a(0)，ω(0)，x(0)，y(0)，θ(0)]＝[p1，p2，p3，p4，p5，p6，p7]，
其中[p1，p2，p3，p4，p5，p6，p7]对应着由车载传感器记录/推算的当前时刻(t＝0)的运动状态客观实际情况；类似地，在车辆运动的终止时刻tf，也需对车辆运动状态进行限制：
[v(tf)，φ(tf)，a(tf)，ω(tf)，x(tf)，y(tf)，θ(tf)]＝[p8，p9，p10，p11，p12，p13，p14]；
碰撞躲避约束：
碰撞躲避约束要求车辆在整个运动时域[0，tf]上避免与环境中的任何静止、移动障碍物发生碰撞；在建立碰撞躲避约束时，首先应确定障碍物的表达形式，最常见的平面障碍物描述方式是凸多边形，即激光雷达反射点云的最小凸包。假设X-Y坐标系中存在NOBS个静止的凸多边形障碍物，其中第j个障碍物包含NVj个顶点不失一般性地，针对车身与凸多边形障碍物j之间的碰撞躲避约束进行建模，在二维平面上，两个凸多边形从尚未发生碰撞到发生碰撞的整个过程中，碰撞发生的瞬间一定出现了某一凸多边形顶点撞入了另一凸多边形中的事件；因此，如果能够在每一时刻都禁止车身矩形顶点落入障碍物内部，并且禁止障碍物顶点落入车辆内部，则碰撞一定不会萌生。障碍物顶点/>车身顶点A(t)～D(t)所在位置均可解析表示，另外还需要一个能够描述“某点处于某一凸多边形外部”的约束条件建模方式。现将这一问题抽象出来，专门考虑如何描述点P＝(x，y)位于具有n个顶点的凸多边形Q1～Qn外部。请参见图2，图2为描述点P位于凸多边形Q1Q2Q3Q4Q5外部的三角形面积判据，将点P与凸多边形每两个相邻顶点分别组成三角形，并将这些三角形的面积累加，如果面积之和大于凸多边形，则点P处于凸多边形外部，否则点P处在多边形的轮廓上或内部，据此，点P处于凸多边形Q1～Qn外部的约束条件可正式建立为：
其中SΔ代表相应三角形面积，代表凸多边形面积，SΔ应基于三角形顶点坐标书写，以/>为例，假设P＝(x，y)、Qk＝(xQk，yQk)、Qk+1＝(xQ(k+1)，yQ(k+1))，则有：
其中是常值；
将点P处于凸多边形Q1～Qn外部的一般性约束条件简记为PointOutOfPolygon(P，Q1...Qn)，据此可建立第j个障碍物与车身矩形A(t)B(t)C(t)D(t)的碰撞躲避约束条件，完整的碰撞躲避约束条件可写为：
j＝1，...，NOBS，t∈[0，tf]；
进一步简写为：
代价函数：
期待泊车运动过程尽早完成，因此设置：J＝tf；
完整的最优控制问题：将以上的约束条件和代价函数汇总起来即得到面向泊车轨迹规划任务的最优控制问题。
二、将其离散化为非线性规划命题：
最优控制问题描述了车辆在泊车场景中的轨迹规划任务，求解即确定终止时刻tf以及控制变量a(t)、ω(t)，在满足全部约束条件的前提下，使得代价函数J取最小值。最优控制问题可简记为以下形式：
min tf，
G(x(t)，u(t))≤0，t∈[0，tf]；
其中x(t)代表状态变量，u(t)代表控制变量。求解命题即确定符合约束条件的控制变量u(t)以及时域长度tf，使得末值型代价函数tf得以最小化；
首先，定义(Nfe+1)个采样时刻{tk|k＝0，...，Nfe}，要求这些采样时刻均匀分布于时域[0，tf]上，即：
0＝t0<t1<t2<...<tNfe＝tf，
第二步，引入一系列变量{uk|k＝0，...，Nfe}、{zk|k＝0，...，Nfe}来分别表征u(t)、x(t)，求解u(t)、x(t)这一原始任务转化为求解它们在一系列采样时刻的取值，即求解变量{uk}、{xk}的取值，由于在离散化意义下已不存在连续时间变量t，因此在原始命题(12)中与t相关的部分均需相应改造。代价函数变为代数等式/不等式G(x(t)，u(t))≤0，t∈[0，tf]变为G(uk，xk)≤0，k＝0，...，Nfe，微分等式的变化方式稍微复杂，将dx(t)/dt＝F(x(t)，u(t))写以下等价形式：
其中无论变量a如何选取，上式一定成立，现将式中的t取为tk，并令a＝tk-1，则有：
为去除复杂的积分运算，可将被积函数整体近似为一个常值Const，即：
由于函数F(·，·)本身不可能是常值函数，上式的成立意味着x(t)、u(t)在子区间t∈(tk-1，tk)上取值恒定，即：
其中Const1、Const2为常值，即：
x(tk)＝x(tk-1)+Const·(tk-tk-1)，
进而：
xk＝xk-1+Const·hk；
上式已经成功去除了积分运算以及连续变量t，最后还需明确常值Const究竟应该如何选取，这相当于明确x(t)、u(t)在子区间t∈(tk-1，tk)上取什么样的常值，最常见的取值方式是选取子区间端点处的变量值，即：
或者：
上两式分别代表在子区间左右两端取值，从中选定一种取值方式即可，以选用第一式为例，则：
xk＝xk-1+F(xk-1，uk-1)·hk；
在所有子区间(tk-1，tk)上重复上述转化，则可形成一个完整的非线性规划命题：
s.t.xk＝xk-1+F(xk-1uk-1)·(tf/Nfe)，k＝1，...，Nfe；
G(uk，xk)≤0，k＝0，...，Nfe；
至此，将在时间维度上连续的最优控制问题离散化为一个非线性规划命题。求解该非线性规划命题，即求解各采样时刻上的配置点xk、uk以及tf，在约束条件严格满足的前提下将代价函数最小化。需要强调的是，在非线性规划命题的求解过程中，xk、uk以及tf被视为地位相等的决策变量，它们将被同时求解出来。
三、利用梯度优化方法求解该非线性规划命题：
首先，待求解的非线性规划命题可精炼地描述为：
其中代表由优化变量组成的向量，即解向量，通过引入松弛向量s>0，可将不等式约束/>转化为等式约束，即：
此时，如将命题中唯一的不等式约束条件s>0转化为内罚函数项补入代价函数J，则可构造仅包含等式约束的标准形式非线性规划命题：
其中，代表包含内罚函数项的代价函数；μIPM>0是障碍因子，其取值越趋于0+则通过内罚函数μIPM·ln(s)描述不等式s>0的精准程度越高。
综上所述，求解命题的方式实质上是序贯求解μIPM取值逐步趋于0+所对应的一系列形如上式的子问题。随着μIPM趋于0+，子问题的极值将趋于原命题的极值。
步骤三、假设模块B需要耗时TB秒钟完成，从t＝t0时刻起，一直到t＝t0+TB时刻为止，车辆仍旧沿着原始的既定路径行驶，并且行车速率均匀减小，刹车的减速度为前面所述的abrake。凭借经验与业界常识，TB一般小于1.0秒钟，做出如下假设：车辆从t＝t0时刻到t＝t0+TB时刻的刹车行驶过程中不会出现与障碍物碰撞的情况。
在t＝t0+TB时刻得到可以绕过障碍物的行车轨迹trajectorynew之后，车辆已经不再处于t＝t0时刻曾经在的位置姿态，这是因为车辆从t＝t0时刻到t＝t0+TB时刻之间始终处于运动状态而不是静止状态。因此，直接在t＝t0+TB时刻将车辆的行驶轨迹从trajectoryold切换为trajectorynew是不恰当的，这会导致车辆控制跟踪环节出现不稳定现象。因此设计方案：指定一个参数T>0，要求车辆从t＝t0+TB时刻到t＝t0+TB+T时刻均沿着原始既定轨迹的路径行驶，并且刹车加速度仍旧为abrake；在t＝t0+TB时刻，按照模块C提供的技术去推演车辆在t＝t0+TB+T时刻的位置坐标值、姿态角度、速率、前轮角度信息，并将这些信息记为X，在t＝t0+TB时刻执行完模块C而确定X之后，立刻执行模块D生成一条以X为起始位姿快速向trajectorynew进行衔接的平滑过渡局部轨迹，将这条局部轨迹记录为trajectorytransfer；模块D能够在极快时间内完成，其耗时远远短于T，因此在t＝t0+TB+T时刻到来前，模块D一定能够完成。
其中，模块C负责确定车辆在t＝t0+TB+T时刻的位置坐标值、姿态角度、速率、前轮角度等状态/控制变量的取值。在t＝t0时刻车辆的速度是v0，并且从t＝t0到t＝t0+TB+T这段时间里车辆沿着原始轨迹trajectoryold进行减速度为abrake的匀变速运动，因此可以确定在从t＝t0到t＝t0+TB+T这段长度为TB+T的时间里车辆向前移动至轨迹trajectoryold上的何处位置。具体而言，车辆在t＝t0+TB+T时刻较之于t＝t0时刻向前继续行驶了以下公式所示的长度：
v0·(TB+T)+0.5·abrake·(TB+T)2；
在轨迹上找到相应点位，提取该点的位置坐标、姿态角、前轮转角角度、车速等信息即可。本技术方案中假设完整的轨迹信息应包含上述状态/控制变量信息。
模块D负责生成一条以X为起始位姿快速向trajectorynew进行衔接的平滑过渡局部轨迹trajectorytransfer，这条轨迹的生成方式仍旧是以计算最优控制方法求解一个最优控制命题，与模块B的区别在于，目前并不明确知道这条衔接的过渡曲线应该在哪个具体点位衔接到trajectorynew上面去。为此，先开设N个并行化的线程，分别将trajectorynew未来的N个可能的点位设置为衔接点Y，随后利用并行计算架构同时计算从X到每一个Y的备选行车轨迹，将所有N条备选的行车轨迹中最短的那一条记为trajectorytransfer；这N条衔接轨迹的计算，可按照模块B介绍的方法生成从X到Y的一条轨迹，注意在这里所有的避障约束全部去掉(因为在总体架构中还会事后检验trajectorytransfer是否合法)，这导致模块D的计算可在极快时间内完成，一般是50～100ms以内，是模块B耗时的十分之一左右。每一条备用轨迹的计算过程仍旧包含三个步骤，1)构建最优控制命题，2)将其离散化为非线性规划命题，3)利用梯度优化方法求解该非线性规划命题。
1)将轨迹规划任务构建为最优控制命题：
一个面向轨迹规划任务的最优控制命题，在模块D之中，仅仅包括车辆运动学约束、两点边值约束以及一个代价函数，不再有碰撞躲避约束；
车辆运动学约束：
t∈[0，tf]；
以及：
-Φmax≤φ(t)≤Φmax，
vmin≤v(t)≤vmax，
amin≤a(t)≤amax，
-Ωmax≤ω(t)≤Ωmax，t∈[0，tf]；
两点边值约束：将最优控制问题中提及的t＝0视为X点位的状态，即：
[v(0)，φ(0)，a(0)，ω(0)，x(0)，y(0)，θ(0)]＝[p1，p2，p3，p4，p5，p6，p7]，
其中[p1，p2，p3，p4，p5，p6，p7]对应着X点位运动状态，t＝tf时刻末端对应着Y状态：
[v(tf)，φ(tf)，a(tf)，ω(tf)，x(tf)，y(tf)，θ(tf)]＝[p8，p9，p10，p11，p12，p13，p14]；
代价函数：期待泊车迁移运动过程尽早完成，因此设置：
J＝tf；
完整的最优控制问题：将以上约束条件和代价函数汇总起来即得到面向局部迁移轨迹规划任务的最优控制问题。
2)将最优控制命题离散化为非线性规划命题：离散化过程同模块B，请参考前述步骤。
3)利用梯度优化方法求解该非线性规划命题：求解过程同模块B，请参考前述步骤。
步骤四、最后检查trajectorytransfer段是否能够避障。
具体方法为：如果该段轨迹未与环境中任何新/旧障碍物相撞，则认为该段轨迹是合法的，立刻构成最终的不停车绕障轨迹如下：在车辆于t＝t0+TB+T时刻抵达X之前，仍旧沿着旧轨迹的路径部分进行减速行驶，在抵达X之后则沿着trajectorytransfer行驶，直至车辆行驶到trajectorynew轨迹线上为止，随后继续沿着trajectorynew剩余部分继续行驶直至抵达终止位姿为止；如果检查trajectorytransfer段发生了碰撞，则车辆在抵达X之后继续沿着trajectoryold做减速行驶，按照模块C提供的技术去推演车辆在t＝t0+TB+2T时刻的位置坐标值、姿态角度、速率、前轮角度等信息，仍旧将这些信息记为X，在确定X之后，立刻执行模块D生成一条以X为起始位姿向trajectorynew进行衔接的平滑过渡局部轨迹trajectorytransfer，对其进行碰撞检查，如果发生碰撞则继续重复上述步骤直至车辆彻底停下为止(此时视为不得不进行停车绕障)；如果未发生碰撞则在X点位处衔接过渡段轨迹，随后拼接一段trajectorynew轨迹。
请参见图3，图3为不停车绕障轨迹重规划方案示意图。图3中的图a：车辆生成泊车轨迹trajectoryold后，利用闭环控制技术沿着该轨迹行驶。图b：在t＝t0时刻，车辆恰好行驶至图中第一点位，车辆此时感知到了前方突然出现的障碍物石块。图c：在t＝t0+TB时刻，车辆规划出了一条衔接第一点位与终止位姿的新轨迹trajectorynew，但是在t＝t0+TB时刻车辆已不再处于第一点位，而是处于第二点位，第二点位所代表的位姿、速度等状态量并不在trajectorynew上，因此难以直接将trajectorynew裁切一段而直接用作新的行驶轨迹段。图d：车辆在t＝t0+TB时刻后继续匀减速地沿着rajectoryold行驶至t＝t0+TB+T时刻，此时提前预知了车辆在t＝t0+TB+T时刻即将处于rajectoryold的位置，即图中的第三点位(即技术方案中的X点位)；随后在t＝t0+TB时刻基于最优控制技术快速计算一条从第三点位平滑地迁移到trajectorynew平滑轨迹之上某点(图中第四点位)的局部轨迹trajectorytransfer。图e：最终完整的不停车绕障行车轨迹。
我们在实车实验中确实验证了该方法的有效性，能够大幅度降低车辆应对随时出现突发障碍物的反应时间，并且车辆不再需要降低车速驻车而损耗刹车片或浪费燃油，大大提高了综合经济效益。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种具有不停车绕障功能的泊车轨迹重规划方法，其特征在于，包括以下步骤：
步骤一、在车辆沿着既定的轨迹跟踪行驶的过程中，既定的轨迹记为trajectoryold，假设在t＝t0时刻，车辆发现了前方突然出现的障碍物石块，车辆自动驾驶系统立刻执行模块A，模块A提供的技术包括计算刹车减速度abrake，使得车辆从t＝t0时刻开始实施刹车制动；
模块A计算刹车减速度abrake的技术方案为：当感知到突然出现的障碍物时的车辆位置与障碍物之间的距离为S，则要求车辆最迟也能够在0.8*S里程之处停止下来，假设在故障发生时刻车辆的速度为v0，则可以按照以下公式反推出abrake：
进而有：
步骤二、同时在t＝t0时刻，车辆自动驾驶系统开启一个异步的行程，并在该行程中执行模块B，模块B负责生成一条从车辆在t＝t0时刻所处位置姿态到终止位姿的行车轨迹，记为trajectorynew；
模块B采用计算最优控制方法规划行车轨迹trajectorynew，计算最优控制方法的主要步骤包括：1)构建最优控制命题，2)将其离散化为非线性规划命题，3)利用梯度优化方法求解该非线性规划命题；
1)构建最优控制命题：
一个完整的面向轨迹规划任务的最优控制命题包括车辆运动学约束、两点边值约束、碰撞躲避约束以及一个代价函数；
车辆运动学约束：
车辆运动学约束用于描述车辆在二维平面上的运动能力，在车速不高的情况下，二自由度模型已能够符合实际需求，二自由度模型将车辆的两只前轮及两只后轮分别向车体纵轴方向合并为虚拟单轮，通过确定虚拟前轮的转动角速度以及虚拟后轮的加速度，可确定车辆的前轮转角、行驶速度；根据二自由度模型，车辆在惯性坐标系X-Y中的运动过程受到以下微分方程组的制约：
其中终止时刻tf是待定变量；(x(t),y(t))代表车辆后轮轴中点坐标；v(t)及a(t)分别代表沿车体纵轴方向的速度及加速度，以使车辆前进的方向为正；φ(t)为车辆前轮偏转角，以左偏为正；ω(t)为前轮偏转角速度；θ(t)代表车辆在坐标系中的姿态角，即从X轴正方向到车体纵轴正方向的角度，以逆时针转向为正；此外，还定义了车辆的几何尺寸相关参数：Lw代表前后轮轴距，车辆内在的机械特性对应着状态/控制变量的容许作用区间，一般包括：
被微分的变量包括x(t)、y(t)、φ(t)、θ(t)以及v(t)，可被微分的变量一定对自变量t连续，将其称为状态变量；未被微分的变量包括a(t)和ω(t)，这两个变量不一定对时间t连续，将其称为控制变量；
两点边值约束：
在车辆运动的起始时刻t＝0，应显式地指定车辆所处的运动状态，即
[v(0),φ(0),a(0),ω(0),x(0),y(0),θ(0)]＝[p1,p2,p3,p4,p5,p6,p7],
其中[p1,p2,p3,p4,p5,p6,p7]对应着由车载传感器记录/推算的当前时刻(t＝0)的运动状态客观实际情况；在车辆运动的终止时刻tf，也需对车辆运动状态进行限制:
[v(tf)，φ(tf),a(tf)，ω(tf)，x(tf),y(tf),θ(tf)]＝[P8，P9，P10，P11，P12，P13，P14]；
碰撞躲避约束：
碰撞躲避约束要求车辆在整个运动时域[0,tf]上避免与环境中的任何静止、移动障碍物发生碰撞；假设X-Y坐标系中存在NOBS个静止的凸多边形障碍物，其中第j个障碍物包含NVj个顶点针对车身与凸多边形障碍物j之间的碰撞躲避约束进行建模，在二维平面上，两个凸多边形从尚未发生碰撞到发生碰撞的整个过程中，碰撞发生的瞬间一定出现了某一凸多边形顶点撞入了另一凸多边形中的事件；将点P与凸多边形每两个相邻顶点分别组成三角形，并将这些三角形的面积累加，如果面积之和大于凸多边形，则点P处于凸多边形外部，否则点P处在多边形的轮廓上或内部，据此，点P处于凸多边形Q1～Qn外部的约束条件可正式建立为：
其中SΔ代表相应三角形面积，代表凸多边形面积，SΔ应基于三角形顶点坐标书写，以为例，假设P＝(x,y)、Qk＝(xQk,yQk)、Qk+1＝(xQ(k+1),yQ(k+1))，则有：
其中是常值；
将点P处于凸多边形Q1～Qn外部的一般性约束条件简记为PointOutOfPolygon(P,Q1...Qn)，据此可建立第j个障碍物与车身矩形A(t)B(t)C(t)D(t)的碰撞躲避约束条件，完整的碰撞躲避约束条件可写为：
进一步简写为：
代价函数：
期待泊车运动过程尽早完成，因此设置：J＝tf；
完整的最优控制问题：将以上的约束条件和代价函数汇总起来即得到面向泊车轨迹规划任务的最优控制问题；
2)将其离散化为非线性规划命题：
最优控制问题可简记为以下形式：
其中x(t)代表状态变量，u(t)代表控制变量，求解命题即确定符合约束条件的控制变量u(t)以及时域长度tf，使得末值型代价函数tf得以最小化；
首先，定义(Nfe+1)个采样时刻{tk|k＝0,...,Nfe}，要求这些采样时刻均匀分布于时域[0,tf]上，即：
0＝t0＜t1＜t2＜…＜tNfe＝tf，
第二步，引入一系列变量{uk|k＝0,...,Nfe}、{zk|k＝0,...,Nfe}来分别表征u(t)、x(t)，求解u(t)、x(t)这一原始任务转化为求解它们在一系列采样时刻的取值，即求解变量{uk}、{xk}的取值，代价函数变为代数等式/不等式G(x(t),u(t))≤0,t∈[0,tf]变为G(uk,xk)≤0，k＝0,...,Nfe，微分等式的变化方式稍微复杂，将dx(t)/dt＝F(x(t),u(t))写以下等价形式：
其中无论变量a如何选取，上式一定成立，现将式中的t取为tk，并令a＝tk-1，则有：
为去除复杂的积分运算，可将被积函数整体近似为一个常值Const，即：
由于函数F(·,·)本身不可能是常值函数，上式的成立意味着x(t)、u(t)在子区间t∈(tk-1,tk)上取值恒定，即：
其中Const1、Const2为常值，即：
x(tk)＝x(tk-1)+Const·(tk-tk-1),
进而：
xk＝xk-1+Const·hk；
选取子区间端点处的变量值，即：
或者：
则：
xk＝xk-1+F(xk-1，uk-1)·hk；
在所有子区间(tk-1,tk)上重复上述转化，则可形成一个完整的非线性规划命题：
3)利用梯度优化方法求解该非线性规划命题：
首先，待求解的非线性规划命题可精炼地描述为：
其中代表由优化变量组成的向量，即解向量，通过引入松弛向量s＞0，可将不等式约束/>转化为等式约束，即：
此时，如将命题中唯一的不等式约束条件s＞0转化为内罚函数项补入代价函数J，则可构造仅包含等式约束的标准形式非线性规划命题：
其中，代表包含内罚函数项的代价函数；μIPM＞0是障碍因子，其取值越趋于0+则通过内罚函数μIPM·ln(s)描述不等式s＞0的精准程度越高；
步骤三、在t＝t0+TB时刻得到可以绕过障碍物的行车轨迹trajectorynew之后，指定一个参数T>0，要求车辆从t＝t0+TB时刻到t＝t0+TB+T时刻均沿着原始既定轨迹的路径行驶，并且刹车加速度仍旧为abrake；在t＝t0+TB时刻，按照模块C提供的技术去推演车辆在t＝t0+TB+T时刻的位置坐标值、姿态角度、速率、前轮角度信息，并将这些信息记为X，在t＝t0+TB时刻执行完模块C而确定X之后，立刻执行模块D生成一条以X为起始位姿快速向trajectorynew进行衔接的平滑过渡局部轨迹，将这条局部轨迹记录为trajectorytransfer；
模块C负责确定车辆在t＝t0+TB+T时刻的位置坐标值、姿态角度、速率、前轮角度等状态/控制变量的取值，车辆在t＝t0+TB+T时刻较之于t＝t0时刻向前继续行驶了以下公式所示的长度：
V0·(TB+T)+0.5·abrake·(TB+T)2；
在轨迹上找到相应点位，提取该点位的位置坐标、姿态角、前轮转角角度、车速等信息即可；
模块D负责生成一条以X为起始位姿快速向trajectorynew进行衔接的平滑过渡局部轨迹trajectorytransfer，这条轨迹的生成方式仍旧是以计算最优控制方法求解一个最优控制命题，开设N个并行化的线程，分别将trajectorynew未来的N个可能的点位设置为衔接点Y，随后利用并行计算架构同时计算从X到每一个Y的备选行车轨迹，将所有N条备选的行车轨迹中最短的那一条记为trajectorytransfer；这N条衔接轨迹的计算，可按照模块B介绍的方法生成从X到Y的一条轨迹，每一条备用轨迹的计算过程仍旧包含三个步骤，1)构建最优控制命题，2)将其离散化为非线性规划命题，3)利用梯度优化方法求解该非线性规划命题；
1)将轨迹规划任务构建为最优控制命题：
一个面向轨迹规划任务的最优控制命题，在模块D之中，仅仅包括车辆运动学约束、两点边值约束以及一个代价函数，不再有碰撞躲避约束；
车辆运动学约束：
以及：
两点边值约束：将最优控制问题中提及的t＝0视为X点位的状态，即：
[v(0),φ(0),a(0),ω(0),x(0),y(0),θ(0)]＝[p1,p2,p3,p4,p5,p6,p7],
其中[p1,p2,p3,p4,p5,p6,p7]对应着X点位运动状态，t＝tf时刻末端对应着Y状态：
[v(tf)，φ(tf),a(tf)，ω(tf),x(tf)，y(tf)，θ(tf)]＝[p8，p9，p10，p11，p12，p13，p14]；
代价函数：期待泊车迁移运动过程尽早完成，因此设置：
J＝tf；
完整的最优控制问题：将以上约束条件和代价函数汇总起来即得到面向局部迁移轨迹规划任务的最优控制问题；
2)将最优控制命题离散化为非线性规划命题：离散化过程同模块B；
3)利用梯度优化方法求解该非线性规划命题：求解过程同模块B；
步骤四、最后检查trajectorytransfer段是否能够避障；
检查trajectorytransfer段是否能够避障的具体方法为：如果该段轨迹未与环境中任何新/旧障碍物相撞，则认为该段轨迹是合法的，立刻构成最终的不停车绕障轨迹如下：在车辆于t＝t0+TB+T时刻抵达X之前，仍旧沿着旧轨迹的路径部分进行减速行驶，在抵达X之后则沿着trajectorytransfer行驶，直至车辆行驶到trajectorynew轨迹线上为止，随后继续沿着trajectorynew剩余部分继续行驶直至抵达终止位姿为止；如果检查trajectorytransfer段发生了碰撞，则车辆在抵达X之后继续沿着trajectoryold做减速行驶，按照模块C提供的技术去推演车辆在t＝t0+TB+2T时刻的位置坐标值、姿态角度、速率、前轮角度等信息，仍旧将这些信息记为X，在确定X之后，立刻执行模块D生成一条以X为起始位姿向trajectorynew进行衔接的平滑过渡局部轨迹trajectorytransfer，对其进行碰撞检查，如果发生碰撞则继续重复上述步骤直至车辆彻底停下为止；如果未发生碰撞则在X点位处衔接过渡段轨迹，随后拼接一段trajectorynew轨迹。
CN202210024530.4A 2022-01-11 2022-01-11 一种具有不停车绕障功能的泊车轨迹重规划方法 Active CN114371712B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210024530.4A CN114371712B (zh) 2022-01-11 2022-01-11 一种具有不停车绕障功能的泊车轨迹重规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210024530.4A CN114371712B (zh) 2022-01-11 2022-01-11 一种具有不停车绕障功能的泊车轨迹重规划方法

Publications (2)

Publication Number Publication Date
CN114371712A CN114371712A (zh) 2022-04-19
CN114371712B true CN114371712B (zh) 2024-06-25

Family

ID=81143263

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210024530.4A Active CN114371712B (zh) 2022-01-11 2022-01-11 一种具有不停车绕障功能的泊车轨迹重规划方法

Country Status (1)

Country Link
CN (1) CN114371712B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117970933B (zh) * 2024-04-02 2024-05-31 无锡车联天下信息技术有限公司 一种用于低速泊车直线行驶场景下的车辆自定位修正方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112802346A (zh) * 2020-12-28 2021-05-14 苏州易航远智智能科技有限公司 基于云端共享和地图融合的自主泊车系统和方法
CN113885518A (zh) * 2021-10-27 2022-01-04 湖南大学 基于信任域的井下矿用铰接车轨迹规划方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2987018B1 (fr) * 2012-02-16 2014-03-28 Peugeot Citroen Automobiles Sa Procede de freinage automatique pour vehicule automobile.
CN110550029B (zh) * 2019-08-12 2021-02-09 华为技术有限公司 障碍物避让方法及装置
CN110687908B (zh) * 2019-09-25 2023-01-13 南京理工大学 基于ros的园区无人车轨迹生成系统及其控制方法
CN113671941A (zh) * 2020-05-15 2021-11-19 北京京东乾石科技有限公司 一种轨迹规划方法、装置、设备和存储介质
CN113156966A (zh) * 2021-04-30 2021-07-23 湖南大学 基于自适应尺度约束优化的多非完整机器人车辆协同轨迹规划方法、装置及介质
CN113419534B (zh) * 2021-07-01 2022-03-08 湖南大学 一种基于贝塞尔曲线的转向路段路径规划方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112802346A (zh) * 2020-12-28 2021-05-14 苏州易航远智智能科技有限公司 基于云端共享和地图融合的自主泊车系统和方法
CN113885518A (zh) * 2021-10-27 2022-01-04 湖南大学 基于信任域的井下矿用铰接车轨迹规划方法

Also Published As

Publication number Publication date
CN114371712A (zh) 2022-04-19

Similar Documents

Publication Publication Date Title
CN110187639B (zh) 一种基于参数决策框架的轨迹规划控制方法
Li et al. Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles
CN112389427B (zh) 车辆轨迹优化方法、装置、电子设备和存储介质
Kammel et al. Team AnnieWAY's autonomous system for the 2007 DARPA Urban Challenge
CN108106623B (zh) 一种基于流场的无人车路径规划方法
Laugier et al. Sensor-based control architecture for a car-like vehicle
CN111089594A (zh) 一种适用于多场景的自主泊车轨迹规划方法
JPH0781604A (ja) 自動走行車両
CN113895463B (zh) 一种适用于自动驾驶车辆掉头的路径规划方法
WO2023273067A1 (zh) 一种行驶规划方法、装置、计算机设备及存储介质
CN114435470A (zh) 自动倒车控制方法、装置、车辆和存储介质
CN114371712B (zh) 一种具有不停车绕障功能的泊车轨迹重规划方法
Canudas de Wit Trends in mobile robot and vehicle control
Lee et al. Autonomous-driving vehicle control with composite velocity profile planning
CN114372375A (zh) 一种越野车安全行驶区域求解方法及系统
CN114228690A (zh) 一种基于ddpg和迭代控制的自动驾驶车辆侧倾控制方法
Zhang et al. Structured road-oriented motion planning and tracking framework for active collision avoidance of autonomous vehicles
Hu et al. Path planning and tracking for autonomous vehicle collision avoidance with consideration of tire-road friction coefficient
Wang et al. Automatic lane change control for intelligent connected vehicles
Lin et al. Waypoint tracking for collision avoidance using artificial potential field
CN113885518A (zh) 基于信任域的井下矿用铰接车轨迹规划方法
Hu et al. Decision-making system based on finite state machine for low-speed autonomous vehicles in the park
Chandru et al. Motion planning for autonomous lane change manoeuvre with abort ability
Ľuboš et al. Virtual simulation of overtaking maneuver of autonomous vehicle
Han et al. Path planning and trajectory tracking strategy of autonomous vehicles

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