一种自动驾驶半挂汽车列车弯道行驶小曲率轨迹规划方法与流程

本发明涉及自动驾驶，尤其涉及一种在山路等多弯道路下行驶的自动驾驶半挂汽车列车弯道行驶小曲率轨迹规划方法。

背景技术：

1、随着科学技术的发展，部分人力劳动正在逐渐被自动化、智能化技术替代。以干线物流运输为例，自动驾驶技术的应用逐渐替代了驾驶员。半挂汽车列车因其车身长、货物运输量大、货物装卸方便、运输成本低等成为公路干线物流运输的首选。在物流运输中，半挂汽车列车难免行驶在多弯的山区道路。半挂汽车列车高速行驶时容易导致地面给车辆的侧向力不足，发生折叠、推头、甩尾等危险工况。因山区道路多弯、狭窄且多为单行道，给自动驾驶汽车的轨迹规划增加了不小的难度。

2、现有技术中，常用基于采样和搜索的轨迹规划方法来规划自动驾驶汽车的运动轨迹。半挂汽车列车车身过长，牵引车与拖车之间通过铰接进行连接，车身自由度大，运动的灵活度大，轨迹规划中的各类约束条件过于复杂。基于采样与搜索的轨迹规划方法难以保证在满足复杂约束条件下规划出可行的安全轨迹。同时，这两种方法导出的轨迹并不光滑，即使使用数学方法对轨迹进行平滑处理，也不能保证拟合轨迹满足车辆的运动学原理，同时难以保证得到的轨迹为最优轨迹。

技术实现思路

1、针对自动驾驶半挂汽车列车在多弯的山区道路行驶时容易发生折叠、甩尾、推头的问题，本发明提供一种基于最优控制理论的自动驾驶半挂汽车列车的安全轨迹规划方法，以提高半挂汽车列车在山区多弯道路行驶的安全性。

2、为此，本发明采用以下技术方案：

3、一种自动驾驶半挂汽车列车弯道行驶小曲率轨迹规划方法，包括以下步骤：

4、s1，在t∈[k,k+δk]时段，利用车载传感器得到自车的状态量：

5、[xr,yr,θ1,θ2,v1,δf]，

6、其中，(xr，yr)为在笛卡尔坐标系下半挂汽车列车牵引车后轴的位置坐标；θ1为牵引车航向角；θ2为半挂航向角；v1为牵引车后轴中点的速度；δf为牵引车前轮转角；

7、获取自车周围的点云数据，经由感知预测模块得到自车周围静态障碍物的外轮廓位置坐标(x，y)和动态障碍物的外轮廓位置坐标(x′，y′，t)，由所述自车的状态量与自车周围静态、动态障碍物的位置点集形成自车与周围障碍的瞬时位姿映射外轮廓；

8、s2，用圆和椭圆分别覆盖半挂汽车列车的牵引车与半挂车的外轮廓，得到圆和椭圆的映射外轮廓，确定牵引车前轴与半挂车轴的中点位置坐标和所述圆与椭圆的圆心；

9、s3，由s2得到的圆和椭圆的映射外轮廓与s1得到的所述瞬时位姿映射外轮廓，求所述圆与所述静态、动态障碍物的改进minkowski sum；

10、将圆心/椭圆心与碰撞多边形圆弧与直线的切点相连，判断所有连线的夹角和是否等于360°，若等于360°则证明圆心在碰撞多边形内部，不满足碰撞约束条件；若夹角和不等于360°则满足碰撞约束条件；

11、s4，确认约束条件：包括车辆运动学约束，半挂汽车列车铰接角约束，控制量约束；两点边界约束和避障约束；

12、s5，确定代价函数：

13、

14、式中，(x-xref)2为偏移量，(u-uref)2为控制量，w1与w2分别为偏移量与控制量的权重系数；x表示实际的状态量，xref表示参考的状态量；u表示实际的控制量，uref表示参考的控制量；

15、所述代价函数与s4中的车辆运动学约束、半挂汽车列车铰接角约束、控制量约束、两点边界约束和避障约束构成最优控制问题；

16、s6，由基于搜索的轨迹规划算法a*生成连接车辆起点与目标点的初始轨迹，用样条曲线对所述初始轨迹进行平滑处理；

17、s7，通过将所述最优控制问题中的不等式约束转化为代价函数的一部分，构造内点惩罚函数；通过cilqr算法求解复杂约束下的非线性规划问题；

18、s8，用动态规划法求解lqr，得到优化后的轨迹；

19、s9，针对s8优化后的轨迹再次进行cilqr迭代求解，直到代价函数收敛到某一小值或者达到最大迭代次数，则停止迭代，得到最优轨迹。

20、s3中所的避障约束条件简写为：

21、lbarrier(xc(t))≤0，

22、式中，xc(t)表示t时刻的车辆状态量；若满足上式，则不会发生碰撞。

23、s4中，假设车辆行驶道路平坦、路面各处附着系数相同；车辆在转弯时行驶速度小、车身无侧倾和俯仰运动；轮胎侧偏角小，视为所有车轮均不发生侧向滑动；

24、假设铰接点位于牵引车后轴中点处，牵引车前轴、后轴、半挂车轴与铰接点在大地坐标系下简化为三个点，分别记为of(xf,yf)，or(xr,yr)，ol(xl,yl)，om(xm,ym)；假设牵引车前轮转角为δf、横摆角为θ1、轴距为l1、后轴中点的车速为v1，拖箱横摆角为θ2，铰接点到拖箱后轴中点的距离为l2；不考虑侧向滑动，牵引车与半挂车所有车轴中点轴向速度为零，满足下式：

25、

26、式中，与分别为牵引车前轴中点的纵向与横向速度；与分别为牵引车后轴中点的纵向与横向速度；与分别为挂车后轴中点的纵向与横向速度；字母上方加点表示求导；

27、根据几何位置关系，假设建立牵引车后轴中点or与牵引车前轴中点of之间的关系如下式：

28、

29、式中，or(xr，yr)为牵引车后轴中点，of(xf，yf)为牵引车前轴中点，l1为牵引车轴距，θ1为牵引车横摆角；

30、根据几何位置关系，建立牵引车后轴中点or与半挂轴中点ol之间的关系如下式：

31、

32、式中，ol(xl，yl)为半挂车轴中点，l2为铰接点到半挂车车轴之间的距离，θ2为半挂车横摆角；

33、牵引车后轴中点在大地坐标系下的横纵向车速表示为：

34、

35、牵引车前轴中点的速度表示为：

36、

37、式中，表示牵引车的横摆角速度；

38、挂车车轴中点的速度表示为：

39、

40、式中，表示半挂车的横摆角速度；

41、由以上公式求得铰接点位于牵引车后轴中点的车辆的运动学方程为：

42、

43、车辆运动学约束简写为：

44、xk+1＝f(x(k),x(k))。

45、s4中，将半挂汽车列车的铰接角定义为牵引车与挂车航向角的夹角，将铰接角阈值设定为85°，有：

46、0°≤|θ1-θ2|≤85°

47、铰接角约束简写为：hbox(θ1(t)-θ2(t))≤0。

48、s4中，控制量约束为：

49、

50、在上述不等式中，a为牵引车的加速度，jerk为牵引车加速度的导数(加加速度)，ω为牵引车前轮转角的角速度；车辆控制量约束简写为：

51、hbox(u(t))≤0。

52、s4中，给定目标时刻t＝t的状态量与控制量：x(t)＝xaim，u(t)＝uaim，与初始时刻的状态量与控制量一起构成两点边界约束；车辆两点边界约束简写为：

53、x0＝xinitial，u0＝uinitial；

54、xt＝xaim，ut＝uaim。

55、s4中，假设牵引车车头到半挂车前端的距离为s，牵引车宽为宽为w，拖箱的长为l、宽为w；根据牵引车的几何尺寸与圆最大内接矩形的定义，得到圆心位于笛卡尔坐标系原点的圆方程为：

56、

57、根据半挂车的几何尺寸与椭圆最大内接矩形的定义，得到圆心位于笛卡尔坐标系原点的椭圆方程为：

58、

59、避障约束简写为：

60、lbarrier(xc(t))≤0，

61、式中，xc(t)表示t时刻的车辆状态量，若满足避障约束条件(满足上式)则不会发生碰撞。

62、s5中所述最优控制问题表示为以下形式：

63、minimize＝j(x(t),u(t))

64、subject to

65、xk+1＝f(x(k),x(k))

66、x0＝xinitial，u0＝uinitial；xt＝xaim，ut＝uaim

67、hbox(x(t))≤0，hbox(u(t))≤0

68、hbox(θ1(t)-θ2(t))≤0

69、lbarrier(x(t))≤0

70、上式中：

71、j(x(t),u(t))为代价函数，是优化求解的目标；

72、x(t)表示状态量，包括[x(t)、y(t)、θ(t)、v(t)、a(t)、δf(t)]；

73、u(t)表示控制量，包括[jerk(t)、ω(t)]；

74、xk+1＝f(x(k),x(k))为状态转移方程；x0＝xinitial、u0＝uinitial与xt＝xaim，ut＝uaim分别表示初始时刻与目标时刻的状态量与控制量；hbox(.)≤0表示状态量、控制量、牵引车与挂车夹角的不等式约束；lbarrier(x(t))≤0表示避障约束。

75、s6中，在笛卡尔坐标系下求所述初始轨迹的斜率k，由公式：

76、

77、求得轨迹点上的参考前轮转角设该参考线上的参考速度方向沿参考线切线方向，为定值νf；有了参考前轮转角δr与参考牵引车后轴速度vr，就得到所述初始轨迹。

78、s8包括以下分步骤：

79、s81，定义初始轨迹，将代价函数在离散点处泰勒二次展开，车辆状态转移方程在离散点处泰勒一次展开，化为标准形式：

80、沿初始轨迹划分一系列连续时段，在每个时段内将状态转移方程进行泰勒展开，泰勒展开后的状态转移方程与代价函数为：

81、

82、

83、引入q函数，描述在xt状态下采取动作ut的代价：

84、

85、引入v函数，描述状态本身的代价，这里定义状态代价等于可以使得后续状态代价最优的q代价：

86、

87、将代价矩阵重写成如下形式：

88、

89、s82，执行反向传递过程，求每一个时间间隔的最优控制策略：

90、backward过程：

91、对q函数求导可得令其等于零:

92、

93、解得：

94、

95、令：

96、得到最优控制律为：

97、ut＝ktxt+kt

98、根据上述推导结果，有：

99、

100、得：

101、

102、式中：

103、

104、将t时刻向前回溯到t-1时刻，由于xt-1不是末端节点状态，因此扩展q函数定义成：

105、

106、由公式：

107、与

108、重写v函数，有：

109、

110、得到t-1时刻的q函数为：

111、

112、式中：

113、再次对q函数求导，令其导数等于零：

114、

115、得到t-1时刻的最优控制律：

116、ut-1＝kt-1xt-1+kt-1，

117、通过多次向前回溯，得到初状态：

118、u1＝k1x1+k1；

119、s83，执行正向传递过程，求优化后的轨迹：

120、将控制量从前往后依次带入到车辆状态转移方程，得到一条优化后的轨迹：

121、fort＝1tot：

122、ut＝ktxt+kt

123、xt+1＝f(xt,ut)。

124、本发明针对在多弯道的山区道路行驶的半挂汽车列车车速较高时导致地面给车辆的侧向力不足而发生甩尾、侧滑等危险工况的问题，提出了一种基于最优控制理论的轨迹规划方法。

125、与现有技术相比，本发明具有以下有益效果：

126、1.本发明运用最优控制理论直接生成车辆行驶轨迹，能在带有连续弯道的狭窄道路内规划出曲率小，满足车辆行驶的各类约束条件的最优轨迹，所述最优轨迹为一条在保证车辆无碰撞行驶的条件下曲率最小的轨迹，能够保证车辆行驶时车轮与地面之间有足够大的侧向力，从而防止车辆发生侧滑与甩尾，切实保证了半挂汽车列车在多弯的山区道路行驶的安全性，对于提高半挂汽车列车的行车安全性具有重要意义。

127、2.本发明采用cilqr算法求解复杂约束下的非线性规划问题，利用惩罚函数表示约束函数，转化为代价函数的一部分。使用动态规划法求解lqr问题，通过不断迭代得到最优轨迹。与其它求解最优控制问题的方法相比，该方法约束处理巧妙，迭代优化求解能得到最优轨迹且求解速度快。

128、3.本发明在描述避障约束时分别用圆和椭圆描述半挂汽车列车牵引车与半挂车的外轮廓。引入碰撞多边形概念，所述碰撞多边形定义为某时刻覆盖车体的圆与障碍物的改进minkowski sum。碰撞多边形在生成时考虑了车辆的位姿，与其他生成障碍区域的方法相比，碰撞多边形面积更小，相应的可通行区域更大，因此更容易得到最优轨迹。