基于运动学和动力学模型的无人驾驶路径跟踪控制器设计

王思山，周海鹰，毕栋，龚家元，兰建平，周奎

（湖北汽车工业学院，湖北 十堰442002）

自动驾驶技术目的是提高驾驶安全性和舒适性并提高交通效率，备受学术界和工业界的关注。文中主要讨论无人驾驶软件构架中控制层软硬件的主要实现方法。纯追踪方法相对易于实现，并且对大干扰具有鲁棒性，但由于超前距离随车速增加而增加，此方法存在高速切角的问题。另外，高速发生稳态误差。大多数驾驶环境下，Stanley方法具有比纯追踪方法更好的跟踪性能。由于使用从车辆前轴到路径的横向偏移，Stanley方法不会产生拐角问题，但会导致转弯过冲，受到各种干扰的影响［1-2］。几何路径跟踪方法则是继纯跟踪和Stanley方法之后受欢迎的方法之一。文中采用前馈和反馈模型对自动驾驶车辆进行路径跟踪，此方法易于实现，并且对系统和道路干扰具有一定的鲁棒性。

1 车辆动力学建模

无人驾驶路径跟踪控制器通过车辆姿态和地图信息产生转向指令驱动转向输入来控制车辆行走［3］。如图1 所示，前馈转向控制器用于预测，反馈转向控制器用于跟踪误差调整。文献［4］中讨论了地图匹配算法的计算方法，使用当前车辆位置和路径信息计算车辆横向误差e和头指向误差Δφ。

图1 路径跟踪控制器结构

1.1 基于动力学的车辆模型

基于动力学的前馈转向模型主要用于预测转向输入，最大程度地减小横向跟踪误差，因此增加前馈转向模型可改善系统性能，前馈转向可在车辆偏离路径之前预测车辆转向。通行做法中有几种可根据横向加速度和路径信息来确定前馈转向的方法，前馈转向输入根据带有4个非线性轮胎的复杂车辆模型计算得出。为简化研究、减少控制器的计算量，使用平面自行车模型（图2）。此模型假设轮胎是线性的，轮胎建模带来的误差由反馈转向消除。在小角度假设的条件下，得到前馈转向模型：

式中：L为车辆轴距；r为车辆的偏航率；Ux为纵向车速；αf为前轮转向角；αr为后轮转向角。在小角度转向和无侧滑的假设下，通过力矩平衡得到前馈转向公式：

图2 基于动力学的平面自行车模型

前馈转向输入通过局部路径规划中产生的基于多项式的地图信息计算得到。测试中前馈转向输入会突然变化，是使用基于多项式的地图时二阶导数值的不均匀性引起的。这种现象会导致车辆沿地图轨迹振动，产生偏航和不必要的前轮胎打滑，在车辆转弯时成为系统稳定的关键。为平滑路径，一般采用实时三次样条插值来处理地图数据。

1.2 基于运动学的车辆模型

在前馈转向模型中，由于使用了线性轮胎假设，车辆运行环境干扰或者车辆的简化建模导致系统运行中跟踪误差的出现，为此添加基于运动学的转向反馈模型。基于车道保持的转向反馈提供路径跟踪能力，并为路径跟踪控制器增加鲁棒性。

根据车辆运动学模型（图3），路径跟踪控制反馈产生方向盘反馈控制量δfb，为简化反馈控制参数标定，δfb为前视误差ela的比例控制：

式中：kp为车道保持系数增益。前视误差ela包括横向距离误差e和头指向误差Δφ：

式中：xla为预瞄距离。将式（4）代入式（3）得：

前馈与反馈控制方向盘输入量为

图3 基于运动学的路径跟踪模型

为保持系统的稳定性需要选择合适的kp，xla的调整也需要考虑车辆的运行状况。如式（5）所示，随着xla的增加，车道保持系统对Δφ变得更加敏感，可对车辆的偏航运动快速响应，从而提高航向保持稳定性。但如果xla过高，车道保持转向命令，对车辆的偏航运动变得过于敏感，并在系统中引起偏航振荡。因此xla的选择需要在轮胎饱和期间的偏航稳定性和偏航振动之间权衡，文中控制器设计了随车速变化的预瞄距离动态调整策略，以提高控制器的鲁棒性。

2 硬件系统设计

2.1 无人驾驶测试车辆

使用运动型多功能车进行测试，如图4 所示，测试车辆的电子控制单元（ECU）具有线控功能，路径跟踪控制所需的电动助力转向（EPS）系统通过CAN BUS 接口进行控制，车辆动态状态和位置信息从惯性导航系统（INS）收集。测试车辆参数见表1，主要应用于计算轨迹控制器的前馈和反馈量。

图4 无人驾驶测试车

表1 车辆参数

2.2 控制器硬件结构

控制器硬件结构由传感器单元、网络和数据处理单元组成，转向控制系统结构如图5 所示。INS数据和CAN BUS 信息由基于LPC1768微控制器的Cortex-M3 处理。INS 与通过4G 链接GPS 差分站（RTK），INS 和LPC176 使用波特率为115 200 baud的串行端口相连，同时LPC1768的CAN接口与测试车辆的车载网络相连，CAN通信速率为500 kb·s-1。LPC1768负责收集数据，然后通过串行端口将其发送到计算单元。计算单元是轨迹控制器的大脑，人机界面（HMI）等其他设备也可以读取车辆状态并进一步处理。

图5 控制器硬件结构

3 软件设计

轨迹控制器是一种路径跟踪系统，基于当前车辆位置和来自路径规划器的参考路径来控制转向执行器。转向控制系统由3个模块组成，即地图匹配模块、前馈预测模块和非线性反馈模块。在地图匹配模块匹配到参考路径上的目标点之后，控制器软件会生成所需的转向角。由于测试速度为恒速控制，文中不涉及油门和制动控制器。轨迹控制器软件结构如图6所示。

图6 轨迹控制器软件结构

3.1 软件中关系因素的设计

1）数字地图的样条插值 数字地图中的全球定位系统（GPS）路径信息从INS 获取，来自INS 的位置数据坐标系是世界大地坐标系（WGS84），因此地理坐标（纬度，经度）需要转换为带有参考原点的平面直角坐标。局部路径规划模块生成包含经纬度信息的数字地图。数字地图的参考点是离散的，在实际计算中需要计算道路曲率，需要对路径进行平滑处理，因此对离散点使用三次样条插值处理。路径的三次样条插值结果存储于向量中的多项式，通过三次样条插值函数生成基于多项式描述的新路径，其插值结果如图7所示。

图7 样条插值结果

2）地图匹配 地图匹配是在自动驾驶中找到当前位置和地图之间的最近点［5］。图3中参数e被描述为车辆重心与最近路径点之间的距离，因此须从局部路径规划器输出的整个路径中搜索离质心最接近的点。地图匹配模块搜索步骤如下：先进行粗略搜索，再进行精细搜索，通过距离计算来找出最接近匹配点。

3）预瞄距离设置 预瞄距离根据车辆速度动态调整。较小的预瞄距离迫使系统更准确地跟踪路径，但当车辆沿行进路线行驶时会导致方向盘的输入震荡，因此车辆可能变得不稳定；较大的预瞄距离可减少在跟踪路径时的振荡，并允许车辆在到达弯道之前开始转弯，从而在急剧弯曲的路径周围产生更平滑的轨迹。预瞄距离是阻尼因子，因此较大的幅度会减少过冲，但如果太大，跟踪性能将变差，出现拐角等现象［6］。速度为10～60 km·h-1时，将预瞄距离设置为5～25 m。预瞄距离xla和车速Vcar的关系如式（7）所示：

4）前馈计算 式（2）中Rs是道路半径，将Ks设置为道路曲率，则

为考虑地图的准确性，局部路径规划生成的控制点是均匀的，间距设置为0.1 m。当前的Ks根据距离该点之前和之后1 m 的控制点来计算。当前控制点的信息（包括前馈转向和头指向）被发送到控制器中计算e和Δφ，代入式（2）中计算后得到前馈转向输入值δff。

5）非线性反馈计算kp主要用于保持车辆的稳定性［7］，在标定测试中设定为16 000 N·m-1。车速超过30 km·h-1时，转向不再稳定。减小kp，转向系统在高速下表现良好；但kp小于3 500 N·m-1时，转向不受控制系统的控制。不同曲率路径测试数据表明：kp越大，e越小。道路半径与车道保持系数增益之间的关系通过实际道路测试进行标定。最后通过Matlab软件进行曲线拟合，以获得用于非线性反馈计算的kp曲线，如图8所示。

6）转向限定 转向由线控EPS 控制器控制，根据转向结构特性，转向角须限制在-540～540°。为提高自动驾驶汽车的行驶性能和安全性，将转向速度作为EPS的关键参数限制为0.5（°）·ms-1。

图8 车道保持系数增益与半径的关系

3.2 软件框架的实现

ROS作为无人驾驶系统的常用软件框架，其节点结构、消息传递、传感器驱动和可视化工具可加速系统的开发，因此使用ROS作为基础软件框架。路径跟踪控制软件结构分为4个部分，包括MBED平台、rosserial_python 桥接、ROS 节点和ROS 计算服务，如图9所示。

图9 基于ROS平台的软件结构

为了连接计算单元ROS 应用程序和LPC1768的嵌入式软件，使用rosserial 进行标准ROS消息序列化，并通过串行设备上连接多个主题（topic）和服务（service）。Mbed 是基于32 位ARM Cortex-M 微控制器设备的操作系统，通过串行接口可以连接ROS节点，直接发布和订阅ROS消息，发布TF转换并获得ROS系统时间。

INS信息由串行端口的中断处理程序接收，并处理INS数据以满足转向控制器的需求，并由ROS进行主题发布。车辆动力传动系统和车身的车辆状态信息由LPC1768的2个CAN控制器收集，并使用ROS消息发布到ROS处理节点。

在计算单元方面，使用可用于Ubuntu 16.04（Xenial）版本的ROS Kinetic 作为软件的中间件。rosserial_python自动处理、发布和订阅已连接的rosserial设备，主机端rosserial连接使用python实现，订阅来自嵌入式平台的主题，并传递控制命令到CAN BUS。无人驾驶汽车的运动学和动力学模型计算是在转向控制器服务中实现的，该服务由ROS节点调用，主要进行地图数据读取、地图匹配和消息转换。

4 试验验证

为验证文中无人驾驶轨迹控制器系统，选择典型城市道路，由直线和环岛道路组成，最大道路曲率为0.02，测试场景见图10，定速和变速运动测试结果见图11。为找到车道保持测试的横向误差，将自动驾驶汽车的速度设置为恒定速度5 m·s-1，试验数据表明：较大的横向距离误差在环岛处产生，横向距离误差约0.3～0.4 m；直道中横向距离误差小于0.1 m。直道变速测试中，纵向速度从5 m·s-1变为7 m·s-1，整个过程横向距离误差小于0.1 m。图12为某次测试数据的误差统计图，结果表明：实验中最大横向误差0.4 m，大多数时间误差为±0.1 m。文中控制器在直线道路上表现出较好的性能，而在环岛道路上横向距离误差比直线道路大。

图10 测试环境

图11 测试结果图

图12 误差统计图

5 总结

介绍了路径跟踪控制器的结构，将基于动力学的平面自行车模型用于计算前馈转向，将非线性反馈模型用于计算反馈转向；设计了基于ROS平台的软件系统和运行ROS的嵌入式系统组成的轨迹控制软硬件，并对轨迹控制器的性能进行了测试。虽然平面自行车模型近似于车辆的横向动力学模型，但系统并未考虑轮胎尤其是在弯道中的非线性表现，对车辆动力学建模中的小角度转向也有一定的假设要求，不能完全描述车辆运动学。同时将基于运动学的反馈模型进行比例控制器建模，虽然应用了非线性增益调节，但比例控制器不能较好地跟踪与收敛，因此系统在直线上表现出较好的性能，在弯道上效果不如直道；使用模型预测方法（MPC）［8］并添加非线性轮胎模型来处理路径干扰和加大前馈作用，可提高路径跟踪性能。