汽车实验用转向机器人轨迹规划

赵书尚,余 欢,李阁强

(河南科技大学 机电工程学院,河南 洛阳 471000)

轨迹规划是实现汽车智能化的关键技术之一,是汽车具备自主行驶能力的基础[1].对于蛇行试验而言,生成一条从初始状态到目标状态的轨迹是转向机器人进行实验的基础[2].轨迹规划的主要任务是找到一条从初始点到目标点的安全轨迹,并满足相关约束条件[3].汽车实验用转向机器人的轨迹规划主要存在以下两个问题:① 随机性.汽车在行驶过程中,只能利用多种传感器来获取汽车自身的状态信息,试验时无法保证人为控制的行驶速度恒定不变,增加了路径跟踪的难度.② 约束多.转向机器人的轨迹规划问题不仅要考虑车辆运动存在的几何约束,还需要考虑生成轨迹曲率边界连续有界,以及车辆的动力学约束对轨迹规划的影响.

近年来,国内外学者对智能汽车轨迹规划问题进行了大量的研究.Kelly等[4]提出基于模型预测的无人车轨迹规划方法,该方法不断调整多项式参数,使轨迹的末端状态不断靠近目标状态,最终优化得出一组参数,使智能汽车从初始状态到目标状态.此外,还有人工势场法、模拟退火算法、模糊逻辑算法、神经网络算法、遗传算法、栅格算法等[5].

为解决多约束条件下汽车实验用转向机器人生成满足蛇行试验条件轨迹的问题,本文采用序列二次规划方法,考虑车辆运动学约束,设计车速与前轮转角自适应控制器,规划生成最优的轨迹曲线,最后仿真验证了其有效性.

1 问题描述

对于汽车转向机器人来说,轨迹生成问题,主要研究如何生成一系列动作[6],使得汽车由初始状态到达目标状态.为了便于描述,建立了汽车运动学模型,如图1所示,以车辆后轴中点为原点O,建立直角坐标系,模型可表示为

(1)

式中:(x,y)为车辆后轴中点坐标;α为车身倾角;θ为前轮转角;v为汽车车速;ω为前轮转角速度.

图1 汽车运动学模型Fig.1 Definition of tractor coordinate system

由式(1)可得转弯半径r、曲率k、前后轮轴距L以及前轮转向角θ之间的关系为

(2)

因此,汽车的轨迹生成问题就可以总结规划为一系列动作,使得汽车从初始状态Y0=[0,0,a0,θ0],到达目标状态Yf=[xf,yf,af,θ0],其中(xf,yf)为汽车目标状态时后轴中点坐标.

通过轨迹曲线对车辆轨迹进行建模,鉴于多项式在求导和计算上的便利性[7],本文将轨迹曲线表示为轨迹上任一点的纵坐标y关于横坐标x的τ次多项式函数:

(3)

式中,a=[a0a1…aτ]为路径曲线参数向量.

曲线在任一点的曲率为

(4)

2 轨迹生成算法

假设汽车行驶距离为s,则对汽车走过的距离s进行积分,汽车的状态可以表征为

(5)

由式(1)和式(5)可得,汽车的轨迹形状、汽车在各点的航向角与汽车的移动距离、不同距离的曲率及行驶速度,在不考虑汽车侧向滑动的情况下,汽车的速度影响前轮转角的变化和完成轨迹所需要的时间.汽车的轨迹规划问题可总结为生成适当的曲率,同时控制前轮转角与车速之间的关系,从而规划出满足初始状态Y0到Yf的轨迹.

为了保证汽车整条轨迹的可行性,除了生成连续有界的曲率满足汽车运动学之外,需要人为控制合理的速度保证汽车不会发生侧向滑动,同时，应满足道路约束以及汽车动力学约束.

3 轨迹规划约束

对于曲率约束,在上一节引入的多项式函数轨迹曲线表示方法已确保了轨迹曲线光滑以及曲率连续有界,同时由于速度采用人为控制,利用路径曲线参数向量a即可确定一条轨迹.

由于汽车系统是一个受非完整性约束的非线性系统[8],因此,汽车轨迹规划在满足曲率连续有界约束的同时还应满足汽车动力学约束.汽车实际运动过程中,在不考虑汽车产生的侧向滑动等因素的限制,只考虑其受汽车转向系统的限制,可将轨迹规划中的汽车动力学约束转化为汽车转向角约束.前轮转角连续变化且范围有限,但后轴中心的线速度始终与汽车后轴垂直,故可将转角约束等同转化为几何约束进行处理,将转向角约束转化为曲率半径约束,相应的数学表达式如下:

式中:kmax为最大曲率;rmin为最小转弯半径;θmax为前轮最大转向角.

4 轨迹优化

由于采用多项式曲线进行轨迹规划,在人为控制车速的情况下,只需要通过确定路径曲线参数向量aτ，即可确定满足汽车运动学约束以及曲率连续有界约束.为了保证轨迹曲线的可行性,轨迹曲线的曲率必须有界,因此本文采用最优化方法求解满足曲率边界条件约束的参数:

(11)

式中:λ1为使得轨迹曲线曲率最大;λ2为使得轨迹曲线曲率最小;aτ为路径曲线参数向量(该参数使得汽车车体的最大前轮转向角与最小前轮转向角之差最小,从而得到较为平滑的轨迹).因此,轨迹规划问题转化为轨形规划问题后，最终可以表征为有约束条件的非线性优化问题:

采用序列二次规划方法求解优化问题,在每次优化迭代中,可将初始值简单的设成(0.1,0.1,0.1,0.1xf),通过对曲率k的极值进行求解，可得最大以及最小曲率.

将轨形曲线等长度地划分为N个轨迹片段,汽车从起始点到最后目标点的过程中，需要一次通过轨迹片段上的目标点,为了不影响汽车到最终目标点的可达性,需要对片段的每一个目标点进行曲率约束.因此,可通过设定期望目标点的曲率来解决此问题.首先,通过圆弧连接当前目标点以及下一个目标点，求解到达当前目标点的期望曲率,再将该期望目标状态曲率加上一定的阈值作为约束条件加入到优化过程中.通过这种方法,可以保证片段上每一个目标点是可达的.

5 车速与前轮转角自适应控制器设计

汽车转向机器人在模拟驾驶员进行蛇行试验时,需不断检测对规划好的路径进行预瞄,根据前方规划路径的曲率以及当前车辆的车速决定汽车前轮转角,使得无人驾驶汽车尽量逼近前方的道路[9-10].在理想状况下,汽车可无偏差地行驶在规划路径上.但在实际实验中,车速是人为控制的,车速与方向盘转角是实时变化的自适应关系,汽车转向机器人需要根据当前车速以及期望路径的曲率持续调整前轮转角来减小偏差以达到补偿的目的.因此,需建立路径跟踪控制器，如图2所示.

图2 预瞄补偿控制框图Fig.2 Preview compensation control diagram

如图3所示,建立汽车预瞄控制2自由度模型,P点预瞄点,S点为汽车的质心,预瞄距离为l1+l3.

图3 预瞄控制模型Fig.3 Preview control model

预瞄点P的曲率为

(12)

进一步可得预瞄点侧偏角βp为

(13)

式中:l1为汽车质心到前轮轴心的距离;l3为汽车前轮轴心到预瞄点的距离;β为质心处侧偏角.

线性化处理后可得

(14)

进一步可得质心S处的侧偏角β为

(15)

建立汽车2自由度微分方程并与式(15)联立:

(16)

式中:K1为前轮侧偏刚度系数;K2为后轮侧偏刚度系数;θ为前轮转角;l2为汽车质心到后轮轴心的距离.

将式(16)联立后进行拉氏变换可得预瞄控制的传递函数:

(17)

综上所述,根据轨迹曲率，通过预瞄控制传递函数，可以得到预瞄控制所需要的前轮转角.

由于在进行蛇行试验过程中,车速为人为控制,无法保持恒定,受到车速不稳定的干扰,车速与前轮转角实时变化,汽车实际路径与期望路径之间可能存在偏差.因此，除预瞄控制外,为了更好地跟踪期望路径，还需要对车辆进行补偿控制.汽车与期望路径的偏差,可通过横向偏差以及航向角偏差两个量进行表示[11].

假定预瞄点位姿为(Xp,Yp,δp),期望轨迹点位姿为(Xd,Yd,δd).通过几何关系可得汽车与期望路径的横向偏差为

(18)

亦可得汽车于期望路径的航向角偏差为

Δδ=δd-δp

(19)

在蛇行试验过程中,由于人为控制的车速无法保证恒定不变,因此,在实际路径与期望路径的补偿控制中采用模糊自整定PID控制,对预瞄控制的补偿量Δθ如下:

(20)

式中:kYp为横向偏差比例系数;kYi为横向偏差积分系数;kYd为横向偏差微分系数;kδp为航向角偏差比例系数;kδi为航向角偏差积分系数;kδd为航向角偏差微分系数.

如图4所示,在传统PID的基础上,采用模糊控制策略设置模糊PID控制器,以偏差和偏差变化率为输入,建立PID参数和输入量之间的关系,从而实时对PID参数进行整定,使其具有一定的自适应能力.

图4 模糊自适应控制器Fig.4 Fuzzy PID self-adapt controller

6 实验

通过Simulink-Carsim软件搭建联合仿真平台,在Carsim中设置曲率约束、汽车行驶速度、汽车转向角约束,利用C语言编写轨迹规划模块,搭建最优预瞄驾驶员模型.采用Matlab中的非线性规划求解器对其进行求解.

轨迹规划模块可通过序列二次规划方法得到最优可行轨迹,汽车转向机器人根据期望值决策出方向盘转角输入给Carsim模型.

在蛇行轨道中验证算法的可行性,令汽车的起始状态为Y0=[0, 0, 0, 0],Yf=[255, 0,0, 0],车辆参数如表1所示.

如图5所示,汽车以36 km/h的行驶速度进行蛇行试验,轨迹规划算法能够生成满足车辆运动连续性以及曲率连续有界要求的平滑轨迹,如图6所示,轨形跟踪的最大误差为0.25 m.如图7所示,汽车前轮转角仅在速度不稳定时产生波动,轨形跟踪效果较好.

表1 汽车参数列表Tab.1 Vehicle parameters

图5 路径跟踪Fig.5 Path tracking

图6 路径跟踪局部放大图Fig.6 Path tracking partial amplification diagram

图7 前轮转角变化曲线Fig.7 Change curve of front wheel rotation angle

7 结论

(1) 本文在汽车行驶速度不变的前提下,将轨迹规划问题转化为轨形规划问题.采用序列二次规划方法,根据实验条件考虑车辆运动学约束,规划生成最优的蛇行轨迹曲线.

(2) 建立预瞄控制模型,采用模糊控制策略对PID参数进行自整定,设计模糊PID控制器,使其具有一定的自适应能力.

(3) 为了验证该方法的有效性和实时性,本文对路径跟踪进行实验仿真.仿真实验结果表明,采用序列二次规划方法能够生成满足车辆运动连续性要求的平滑轨迹,误差较小,并且避免了传统规划方法的“振荡”现象.随着车速的波动,前轮转角发生相应的变化,呈自适应关系.本研究适用于在模拟场地进行蛇行试验的车辆.

参考文献:

[1] 李文学,饶运清,戚得众.全向轮机器人路径规划与导航系统设计[J].机械设计与制造,2014(12):18-22.

LI W X,RAO Y Q,QI D Z.Design of path planing and navigation for omnidirectional wheel robot[J].Machinery Design & Manufacture,2014(12):18-22.

[2] ONIEVA E,NARANIO J E,ALONSO J.Automatic lateral control for unmanned vehicles via genetic algorithms [J].Applied Soft Computing,2011,11(1):1303-1309.

[3] 孙浩,张素民.基于车辆稳态动力学特性的汽车动态轨迹规划[J].国防科技大学学报,2013(6):171-176.

SUN H,ZHANG S M.Dynamic trajectory planning based on vehicle steady dynamics[J].Journal of National University of Defense Technology,2013(6):171-176.

[4] KELLY A,NAGY B.Reactive nonholonomic trajectory gen-eration via parametric optimal control[J].The International Journal of Robotics Research,2003,22(7-8):583-601.

[5] 胡玉兰,朱立忠.基于遗传算法的机器人运动路径规划的应用研究[J].机械设计与制造,2002(5):109-111.

HU Y L,ZHU L Z.The application research of genetic algorithm for the robot shortest path planning[J].Machinery Design & Manufacture,2002(5):109-111.

[6] 陈成,何玉庆,卜春光,等.基于四阶贝塞尔曲线的无人车可行轨迹规划[J].自动化学报,2015,41(3):486-496.

CHEN C,HE Y Q,BU C G,et al.Feasible trajectory generation for autonomous vehicles based on quartic bezier curve[J].Acta Automatica Sinica,2015,41(3):486-496.

[7] 付骁鑫,江永亨,黄德先.一种新的实时智能汽车轨迹规划方法[J].控制与决策,2017(10):1751-1758.

FU X X,JIANG Y H,HUANG D X.A novel real-time trajectory planning algorithm for intelligent vehicle[J].Control and Decision,2017,(10):1751-1758.

[8] 单恩忠.基于微分平坦与样条理论的自主泊车轨迹规划[D].长沙:国防科学技术大学,2009.

SHAN E Z.Research on trajectory planning of auto-parking based on differential flatness and spline theory[D].Changsha:Nation University of Defense Techonlogy,2009.

[9] 修彩靖,陈慧.无人驾驶车路径跟踪控制研究[J].计算机工程,2012,38(10):128-130.

XIU C J,CHEN H.Research on path tracking control of autonomous vehicle[J].Computer Engineering,2012,38(10):128-130.

[10] 杨钢.转向机器人模块化设计的研究[D].镇江:江苏大学,2013.

YANG G.The research on modular design of steering robot[D].Zhenjiang:Jiangsu University,2013.

[11] KIM K,HWANG K,KIM H.Study of an adaptive fuzzy algorithm to control a algorithm to control a rectangular-shaped unmanned surveillance flying car [J].Journal of Mechanical Science and Technology,2013,27(8):2477-2486.