【技术实现步骤摘要】
本专利技术涉及自动驾驶领域，尤其涉及一种在厂区等多种场景下运行的牵引车-挂车系统在存在不规则障碍物的狭窄通道内安全通行的轨迹规划方法。
技术介绍
1、随着科学技术的发展，部分人力劳动正在逐渐被自动化、智能化技术替代。以厂区内货物运输为例，自动驾驶技术的应用逐渐替代了人类驾驶员。厂区内货物运输为了满足运输量要求，一般通过多挂车园区拖挂运输货物，大多数挂车由一个小型车辆作为牵引车，然后在车辆尾部拖曳1-5个挂车(货箱)。当拖曳挂车数目过多时，由于车身长自由度多，对自动驾驶技术的轨迹规划能力提出了较高的要求。
2、现有技术中，常用基于采样和搜索的轨迹规划方法来规划自动驾驶汽车的运动轨迹。多挂车车辆由于车身过长，牵引车与挂车和挂车与挂车之间通过铰接进行连接，整个车身自由度过多，运动的灵活度大，轨迹规划中的各类约束条件过于复杂。基于采样与搜索的轨迹规划方法难以保证在满足复杂约束条件下规划出可行的安全轨迹。同时，这两种方法导出的轨迹并不光滑，即使使用数学方法对轨迹进行平滑处理，也不能保证拟合轨迹满足车辆的运动学原理，同时难以保证得到的轨迹为近似最优轨迹。
技术实现思路
1、针对自动驾驶牵引车-挂车系统在厂区等存在障碍物的狭窄通道内通行时，安全轨迹难以规划的问题，本专利技术提供一种自动驾驶牵引车-挂车系统的安全轨迹规划方法。通过该方法能在给定的狭小空间内规划出满足所有约束条件的近似最优轨迹，提高牵引车-挂车系统在狭窄通路内的行驶安全性与通行效率。
2、为此，本专利技术采用
3、一种狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，包括以下步骤：
4、s1，探测车辆周围环境信息，包括牵引车的状态量[x0，y0，θ0，v0，δ0]、挂车的状态量[xi，yi，θi，vi]和障碍物的位置；所述障碍物的位置包括自车周围一系列静态障碍物的外轮框位置坐标(x，y)与动态障碍物的位置外轮框坐标(x′，y′，t)，由自车的状态量与自车周围静态、动态障碍物的位置点集形成自车与周围障碍的瞬时位姿映射关系；
5、其中，(x0，y0)为在笛卡尔坐标系下牵引车后轴中心的位置坐标、θ0为牵引车横摆角，v0为牵引车后轴中点的速度，δ0为牵引车前轮转角；(xi，yi)为挂车车轴中点mi的坐标，θi为挂车横摆角，vi为挂车车轴中点的速度；
6、s2，确定约束条件，包括车辆运动学约束、两点边界约束、相邻两车的横摆角约束、控制量约束和避障约束；
7、s3，构建代价函数：
8、
9、该代价函数相当于偏移量与控制量的加权和，其中，x表示实际的状态量，xref表示参考的状态量，u表示实际的控制量，uref表示参考的控制量s4，
10、s4，利用cilqr算法确定狭窄通道自动驾驶牵引车-挂车系统的行车轨迹规划，包括：
11、s41，确定最优控制问题：
12、所述最优控制问题表示为以下形式：
13、
14、式中，j(x(t)，u(t))为代价函数，是优化求解的目标；x(t)表示状态量，包括牵引车的[x(t)、y(t)、θ(t)、v(t)、a(t)、δf(t)]与挂车的[x(t)、y(t)、θ(t)]；u(t)表示控制量，包括牵引车的[jerk(t)、ω(t)]；xk+1＝f(x(k)，x(k))为车辆状态转移方程；x0＝xinitial、u0＝uinitial与xtaim＝xaim，utaim＝uaim分别表示初始时刻与目标时刻的状态量与控制量；hbox(.)≤0表示状态量、控制量、相邻两车的夹角的不等式约束；lbarrier(x(t))≤0表示避障约束；
15、s42，将s41的最优控制问题中的不等式约束转化为s3中代价函数的一部分，构造内点惩罚函数，利用惩罚函数表示约束函数；
16、s43，将s42得到的惩罚函数二次展开，转化为代价函数的一部分，将s41的车辆状态转移方程沿规划的轨迹在等时间间隔的一系列连续的轨迹点泰勒一阶展开，将车辆状态转移方程线性化；最终得到一系列的状态量，所述一系列的状态量构成一条优化后的轨迹；
17、s5：对s43优化后的轨迹再次进行cilqr求解，直到代价函数收敛到某一小值或迭代到最大迭代次数，则停止迭代，得到最优轨迹。
18、上述的s2中，所述车辆运动学约束具体如下：
19、假设牵引车轴距为la0，后轴中点到牵引车铰接点h0的距离为lb0、前轮转角的角速度为ω0，则牵引车的车辆运动学方程为：
20、
21、其中，a0为后轴中点的加速度，(x0，y0)为牵引车后轴中心m0的位置坐标、v0为牵引车后轴中点的速度，θ0为牵引车横摆角；
22、第i辆挂车的横摆角速度为：
23、
24、式中，θi(t)为t时刻的第i个挂车的横摆角，θi-1(t)为t时刻的第i-1个挂车的横摆角。
25、第i辆挂车的速度为：
26、
27、s2中所述的避障约束为：
28、将车身形心与碰撞多边形的轮廓折点相连，判断所有连线的夹角和是否等于360°，若等于360°则证明形心在碰撞多边形内部，不满足碰撞约束条件；若夹角和不等于360°则证明形心在碰撞多边形外部，满足碰撞约束条件。
29、s2中所述的相邻两车的横摆角约束为：
30、对牵引车与挂车和挂车与挂车间相邻两个车的横摆角约束：
31、|θi(t)-θi-1(t)|≤θerr，i＝1，2，3…，n
32、其中，θi(t)与θi-1(t)分别表示相邻两车的横摆角，θerr表示相邻两车间最大横摆角之差。
33、s2中所述的控制量约束为：
34、
35、式中，a为牵引车的加速度，jerk为牵引车加速度的导数，v0为牵引车后轴中点速度，δf为牵引车前轮转角，ω为牵引车前轮转角的角速度。
36、s2中所述的两点边界约束为：
37、识别初始时刻t＝0时牵引车-挂车系统的状态量与控制量，分别有：x(0)＝xinitial，u(0)＝uinitial，反应初始时刻的真实值；
38、给定目标时刻t＝t时牵引车-挂车系统的状态量与控制量，分别有：x(t)＝xaim，u(t)＝uaim；
39、初始时刻的状态量与控制量一起构成两点边界约束。
40、上述的步骤s43包括以下分步骤：
41、s431，确定一条初始轨迹，用样条曲线对初始轨迹进行平滑处理；
42、由基于搜索的轨迹规划算法a*生成连接车辆起点与目标点的初始轨迹，平滑处理为在笛卡尔坐标系下求初始轨迹的斜率k。由路径斜率求得牵引车前轮转角为：
43、δr＝arctan(k)
44、求得轨迹点上的参考前轮转角，设该参考线上的参考速度方向沿参考线切线方向，为定值vf；初始本文档来自技高网...
【技术保护点】
1.一种狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，S2中所述车辆运动学约束具体如下：
3.根据权利要求1所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，S2中所述避障约束为：
4.根据权利要求1所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，S2中所述相邻两车的横摆角约束为：
5.根据权利要求1所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，S2中所述控制量约束为：
6.根据权利要求1所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，S2中所述两点边界约束为：
7.根据权利要求1所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，S43包括以下分步骤：
8.根据权利要求7所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，S432包括以下分步骤：
...【技术特征摘要】
1.一种狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，s2中所述车辆运动学约束具体如下：
3.根据权利要求1所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，s2中所述避障约束为：
4.根据权利要求1所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨迹规划方法，其特征在于，s2中所述相邻两车的横摆角约束为：
5.根据权利要求1所述的狭窄通道内自动驾驶牵引车-挂车系统行车轨...
【专利技术属性】
技术研发人员：张祖锋，殷嘉伦，张雪峰，卢孟轩，
申请(专利权)人：畅加风行苏州智能科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。