基于跟踪误差模型的无人驾驶车辆预测控制方法

李培新 姜小燕 魏燕定 周晓军

(1.浙江大学浙江省先进制造技术重点实验室，杭州 310027； 2.中国航天科技集团公司第一研究院，北京 100076)

基于跟踪误差模型的无人驾驶车辆预测控制方法

李培新1姜小燕2魏燕定1周晓军1

(1.浙江大学浙江省先进制造技术重点实验室，杭州 310027； 2.中国航天科技集团公司第一研究院，北京 100076)

针对无人驾驶车辆的轨迹跟踪问题，在分析车辆运动学模型的基础上，设计了一种基于模型预测控制理论的轨迹跟踪控制方法。首先，将车辆运动学模型进行线性化处理，得到车辆运动学线性跟踪误差模型，该模型可以用来预测车辆的未来行为。其次，利用此跟踪误差模型作为预测模型，应用线性模型预测控制方法，通过优化得到使性能指标最小的控制序列，将控制序列的第一步作用于系统。最后，建立了3种典型的道路试验曲线，并且在基于实时多体动力学软件Vortex搭建的虚拟仿真平台中对轨迹跟踪控制器进行了仿真。仿真结果表明，该控制器可以保证无人驾驶车辆快速且稳定地跟踪参考轨迹，距离偏差和方位偏差都在合理的范围内，且实时性可以达到要求。

无人驾驶车辆； 跟踪误差； 预测控制

引言

车辆的导航控制技术是实现无人驾驶车辆的基础，而其中轨迹跟踪问题是最基本的问题。轨迹跟踪问题一直是车辆控制中的难点，近年来已经有不少学者进行了研究。文献[1]采用PID控制方法设计了控制器，它具有较好的跟踪能力，但是缺乏对复杂工况的适应性。文献[2]通过车载传感器实时获取行驶轨迹相对于最优路径轨迹的偏差信息，采用可变参数的PID控制，实现铲运车的无人驾驶。文献[3]采用了预瞄方法对车辆轨迹跟踪问题进行了研究，仿真结果表明它对大的偏差以及不连续的轨迹有较强的鲁棒性，但是预瞄距离的选取比较麻烦。文献[4-6]针对车辆轨迹跟踪问题设计了二次型最优控制器，仿真结果表明，在曲率变化大的轨迹中表现较差，降低了跟踪过程中的全局稳定性。文献[7]基于针对车辆换道及超车的工况，利用BackStepping控制算法选取Lyapunov函数设计了轨迹跟踪控制器。近年来，模型预测控制理论[8-11]引起了人们的关注。模型预测控制汲取了优化控制的思想，利用滚动的有限时段优化取代了一成不变的全局优化。文献[12]设计了一种应用于两轮移动机器人的非线性模型预测控制算法，但由于计算量太大，导致其实时性较差，对偏差的收敛速度慢，应用范围有限。为了提高计算实时性，文献[13-14]利用两轮机器人的运动学模型作为预测模型，文献[15-17]利用车辆的运动学或者动力学模型作为预测模型，对这些模型进行了线性化处理，应用线性模型预测控制方法，通过适当的处理将其转换为二次规划(QP)问题，可以提高计算速度。

本文采用车辆运动学的跟踪误差模型作为预测模型设计模型预测控制器，在基于实时多体动力学软件Vortex搭建的无人驾驶车辆虚拟仿真平台中对所设计的轨迹跟踪控制器进行仿真。

1 基于运动学的数学模型

1.1 车辆运动学模型

在笛卡尔世界坐标系中的二维平面上定义一个车辆模型，如图1所示，为了简化控制器的设计，本文假设车轮与地面为点接触，并且接触点只有纯滚动而不发生相对滑动，这种理想约束本质上是一种非完整约束。

图1 车辆广义坐标示意图Fig.1 Generalized coordinates of vehicle

假设车辆仅在平面上作运动，前后轮的非完整约束方程为

(1)

(2)

式中x——车辆后轮中心横坐标

y——车辆后轮中心纵坐标

xf——车辆前轮中心横坐标

yf——车辆前轮中心纵坐标

θ——车辆横摆角

δ——车辆前轮转向角

许多类型的轮式移动机器人运动学模型都可以转换为Unicycle模型，无人驾驶车辆是一种典型的轮式移动机器人，以车辆后轮中心为参考点，可将车辆运动学模型写为

(3)

式中l——前轮中心和后轮中心的距离

v——车辆后轮中心的速度

车辆输入变量定义为u=[vδ]T，车辆当前坐标定义为x=[xyθ]T。

式(3)也可写成

(4)

1.2 车辆运动学误差模型

一般而言，在轨迹跟踪问题中，一般采用跟踪参考车辆的方法，参考轨迹假定由一个虚拟的参考车辆生成，用r表示参考量，因此车辆的参考轨迹可以写成

(5)

其中

xr=[xryrθr]Tur=[vrδr]T

将式(4)的右边围绕点(xr,ur)进行泰勒展开，除去高阶部分，可得

也可以写成

(6)

式中fx,r——f相对于x的雅可比矩阵

fu,r——f相对于u的雅可比矩阵

结合式(6)与式(5)，可得车辆运动学误差模型

(7)

为了设计控制器，应用欧拉方法将式(7)进行离散化，得到车辆运动学的离散线性时变模型为

(8)

其中

式中k——采样时间T——采样周期

2 车辆轨迹跟踪控制器设计

2.1 车辆轨迹跟踪问题描述

无人驾驶车辆的轨迹跟踪问题是指在惯性坐标系中，根据某种控制理论，为车辆设计一个控制器，确定车辆的控制输入，使得车辆从给定的初始状态出发，到达并跟随给定的参考轨迹。车辆轨迹跟踪示意图如图2所示，参考轨迹由一个虚拟的参考车辆生成，实际车辆在跟踪参考轨迹的时会有相应的误差ex和ey，轨迹跟踪控制算法设计的目的就是使得实际车辆能够精确地跟随参考轨迹，消除跟踪误差。

图2 车辆轨迹跟踪示意图Fig.2 Sketch of virtual simulation platform

模型预测控制是一种优化控制算法，它的基本思想是通过在每一采样时刻k求取某性能指标的最优来确定未来的控制输入，它与最优控制的最大区别在于每一时刻都要在线求取目标函数的最小值，以此得到最优的控制输入，所以它具有良好的全局适应性。

由于本文将车辆运动学的跟踪误差模型作为预测模型，因此在车辆轨迹跟踪控制中，将k时刻的优化性能指标定义为

(9)

Q——误差权矩阵，Q∈Rn×Rn

R——控制权矩阵，R∈Rm×Rm

h——预测时域

图3 车辆轨迹跟踪控制原理图Fig.3 Schematic of vehicle trajectory tracking control

2.2 轨迹跟踪控制算法设计

定义整个预测时域h内的车辆的轨迹跟踪误差和预测时域h内的控制变量误差为

根据1.2小节的离散线性时变车辆运动学模型，可以推导出h时刻的预测输出为

(10)

(11)

其中

Λ(k,i)定义为

由于模型预测控制的目标是使得无人驾驶车辆与参考车辆的轨迹跟踪误差越来越小，因此可以定义车辆的参考轨迹跟踪误差模型为

(12)

可以定义矩阵Ar的值使得在预测时域h内，车辆的参考轨迹跟踪误差越来越小。定义整个预测时域h内的车辆参考轨迹跟踪误差为

(13)

根据推导，性能指标式(9)也可写成

(14)

通过求取该性能指标的最小值可得输入变量为

(15)

其中

(Q∈Rnh×Rnh，R∈Rmh×Rmh)

3 仿真分析

3.1 虚拟仿真平台介绍

为了有效地验证本文提出的控制算法，本文的仿真在实验室搭建的虚拟仿真平台中进行，虚拟仿真平台示意图如图4所示。其中动力学解算模块采用实时多体动力学软件Vortex。Vortex是用于车辆、机械、机器人等多体系统动力学建模的实时交互式动力学仿真引擎，其采用了碰撞检测算法和动力学计算方法，强调准确性与实时性并重。

为了保证仿真过程的实时性，采用2台计算机分别进行动力学解算和视景渲染，Vortex只负责动力学解算，Vega Prime进行视景图像渲染，利用高层体系结构HLA实现Vortex和Vega Prime仿真例程的同步和数据交互。整个仿真过程通过设定管理者联邦成员，并通过高精度的软件定时器实现仿真步长的推进控制，这样可以保证仿真过程的同步性[18]。算法中包含了矩阵的相关计算，由于整个仿真环境是建立在Visual C++基础上，故需要采用实时矩阵计算工具，本文采用了开源矩阵计算工具包Eigen进行矩阵的实时计算。

图4 虚拟仿真平台示意图Fig.4 Sketches of virtual simulation platform

3.2 参考轨迹生成

轨迹生成的方法有很多，由于车辆是非完整系统，需要生成曲率连续的轨迹。

KANAYAMA等[19]采用回旋曲线的方法生成曲率连续的轨迹，其缺点是缺少车辆坐标的闭式表达式。本文采用PINCHARD等[20]推导的车辆轨迹生成方法，示意图如图5所示，该方法根据车辆的位置、速度及曲率等约束条件，推导轨迹的五次多项式，它的一般形式是

图5 轨迹生成示意图Fig.5 Path generation

(16)

(17)

其中

式中k——曲率r——极半径

φ——极角ai——系数，i=0,1,2,…,5

该轨迹起点约束条件为φ=0，r=R1，r′=0，k=0，终点约束条件为φ=Φ，r=R2，r′=0，k=0。Φ为车辆转过的角度，R1为起点半径，R2为终点半径，v1为起点速度，v2为终点速度。

根据以上约束条件，可以得到多项式系数的一般形式为

(18)

3.3 仿真结果及分析

为了检验控制器对初始偏差的响应能力，在仿真过程中取初始横向偏差为-0.5 m，车速为20 km/h，虚拟仿真平台联合仿真的刷新频率为30 Hz，前轮转向角为-36°≤δ≤36°，预测时域为Hp=5，控制时域为Hc=5，加权矩阵为R=0.05I2×2，Ar=0.5I3×3。

3.3.1换道曲线试验

车辆换道曲线试验通常用来测试车辆的操纵性能，它能检验车辆的直线跟踪性能以及对位置及曲率变化的快速响应能力。车辆换道试验曲线如图6所示。

图6 车辆换道试验曲线Fig.6 Test curve of vehicle lane change

车辆在换道曲线的轨迹跟踪仿真结果如图7所示，由图7可以看出，车辆在跟踪换道曲线时，该控制器克服了初始偏差的影响，具有较快的收敛速度，车辆运行稳定后距离偏差在-0.1～0.1 m的范围内，方位偏差在-1°～1°的范围内。曲线试验的误差都在合理的范围内，表明该控制器可以准确稳定地跟踪直线和曲线，且对曲率变化有较快的响应能力。

3.3.2“8”字形曲线试验

“8”字形曲线在日常驾驶中并不常见，但它可以提供对车辆操控情况的直观感受，其在2个圆弧连接处的曲率是不连续的，所以它还可以用来检验轨迹跟踪控制算法的稳定性。“8”字形试验曲线如图8所示。

图7 换道曲线试验仿真结果Fig.7 Simulation results of lane change curve

图8 “8”字形试验曲线Fig.8 Eight-shaped test curve

车辆在“8”字形曲线轨迹跟踪仿真结果如图9所示，由图9可以看出，车辆在跟踪“8”字形曲线时，控制器克服了初始横向偏差的影响，具有较快的收敛速度，车辆运行稳定后距离偏差虽然有一定的稳态误差，但是距离偏差控制在-0.1～0.1 m的范围内，且距离偏差曲线变化比较柔和，稳态误差越来越小，方位偏差控制在-1°～1°的范围内。曲线试验的误差都在合理的范围内，表明该控制器能够快速准确地跟踪预定曲线，而且可以保证运行过程中的平稳性。

3.3.3道路曲线试验

为了更好地检验所设计的车辆轨迹跟踪控制器在复杂路况下的跟踪效果，设计了道路试验曲线，如图10所示，道路试验曲线由一系列真实的驾驶场景组合而成，它可以用来测试控制算法在真实路面的性能。

图9 “8”字形曲线试验仿真结果Fig.9 Simulation results of eight-shaped curves test

图10 道路试验曲线Fig.10 Road test curve

相比于前面的2种参考曲线，道路曲线的曲率变化更快，车辆跟踪曲线所处的工况环境更加恶劣。车辆在道路曲线的轨迹跟踪仿真结果如图11所示，由图11可以看出，车辆在跟踪预定轨迹时，控制器克服了初始偏差的影响，可以快速收敛，稳定后道路曲线试验的距离偏差控制在-0.1～0.1 m的范围内，方位偏差控制在-5°～5°的范围内。曲线试验的误差都在合理的范围内，表明该控制器可以快速且稳定地跟踪复杂的参考轨迹，可以将其应用于真实的驾驶场景中。

通过以上3种不同曲线的仿真结果表明，该控制器克服了初始轨迹跟踪误差的影响，能够准确快速地跟踪不同场景下的参考轨迹，跟踪效果理想，且具有良好的全局稳定性，能够满足无人驾驶车辆轨迹跟踪控制的要求。

图11 道路曲线试验仿真结果Fig.11 Simulation results of road curves test

通过分析对比动力学软件Vortex和视景显示软件Vega Prime的刷新频率，稳定在30 Hz左右，误差在±0.05 Hz以内，控制器达到了实时性的要求。

4 结论

(1)将车辆非线性运动学模型进行线性化处理，得到车辆运动学的线性跟踪误差模型，以此模型作为预测模型，可以将线性模型预测控制方法应用于无人驾驶车辆轨迹跟踪问题中，大幅减少了运算量，满足了系统实时性的要求。

(2)在3种典型道路曲线中的仿真结果表明，该控制器可以使无人驾驶车辆快速且稳定地跟踪参考轨迹，距离与方位偏差都在合理的范围之内，跟踪效果理想，可满足无人驾驶车辆的轨迹跟踪要求。

(3)在基于实时多体动力学Vortex的虚拟仿真平台中进行了联合仿真实验，仿真结果表明该控制器的实时性可以得到保证。

1 NETTO M, BLOSSEVILLE J M, LUSETTI B, et al.A new robust control system with optimized use of the lane detection data for vehicle full lateral control under strong curvatures[C]∥IEEE Intelligent Transportation Systems Conference, ITSC’06, IEEE, 2006: 1382-1387.

2 李建国, 战凯, 石峰, 等.基于最优轨迹跟踪的地下铲运机无人驾驶技术[J/OL].农业机械学报, 2015, 46(12): 323-328.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20151244&journal_id=jcsam.DOI：10.6041/j.issn.1000-1298.2015.12.044.

LI Jianguo, ZHAN Kai, SHI Feng, et al.Auto-driving technology for underground scraper based on optimal trajectory tracking[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2015, 46(12): 323-328.(in Chinese)

3 CAMPBELLl S F.Steering control of an autonomous ground vehicle with application to the DARPA urban challenge[D].Cambridge:Massachusetts Institute of Technology, 2007.

4 SNIDER J M.Automatic steering methods for autonomous automobile path tracking[R].Robotics Institute, Pittsburgh, PA, Tech.Rep.CMU-RITR-09-08, 2009.

5 PENG H, TOMIZUKA M.Optimal preview control for vehicle lateral guidance[R].California Partners for Advanced Transit and Highways (PATH), 1991.

6 SHARP R S.Driver steering control and a new perspective on car handling qualities[J].Proc.IMech E,Part C: Journal of Mechanical Engineering Science, 2005, 219(10): 1041-1051.

7 游峰，王荣本,，张荣辉, 等.智能车辆换道与超车轨迹跟踪控制[J].农业机械学报, 2008, 39(6): 42-45.

YOU Feng, WANG Rongben, ZHANG Ronghui, et al.Lane changing and overtaking control method for intelligent vehicle based on backstepping algorithm[J].Transactions of the Chinese Society for Agricultural Machinery, 2008, 39(6): 42-45.(in Chinese)

8 RAWLINGS J B.Tutorial overview of model predictive control[J].Control Systems, IEEE, 2000, 20(3): 38-52.

9 席裕庚.预测控制[M].北京：国防工业出版社, 1993.

10 MACIEJOWSKI J M.Predictive control: with constraints[M].New York:Pearson Education, 2002.

11 KÜNHE F, GOMES J, FETTER W.Mobile robot trajectory tracking using model predictive control[C]∥IEEE Latin-American Robotics Symposium, 2005.

12 LAGES W F, ALVES J A V.Real-time control of a mobile robot using linearized model predictive control[J].IFAC Proceedings Volumes, 2006, 39(16): 968-973.

13 KÜNHE F, LAGES W F, DA SILVA JR J M G.Model predictive control of a mobile robot using linearization[C]∥Proceedings of Mechatronics and Robotics, 2004: 525-530.

14 FALCONE P, BORRELLI F, ASGARI J, et al.Predictive active steering control for autonomous vehicle systems[J].IEEE Transactions on Control Systems Technology, 2007, 15(3): 566-580.

15 CARVALHO A, LEFÉVRE S, SCHILDBACH G, et al.Automated driving: the role of forecasts and uncertainty—a control perspective[J].European Journal of Control, 2015, 24: 14-32.

16 龚建伟，姜岩，徐威.无人驾驶车辆模型预测控制[M].北京：北京理工大学出版社, 2014.

17 孙银健.基于模型预测控制的无人驾驶车辆轨迹跟踪控制算法研究[D].北京：北京理工大学, 2015.

18 余小勇, 魏燕定, 黄茫茫,等.基于HLA的车辆半实物仿真中精确定时方法[J].浙江大学学报:工学版, 2012, 46(7):1195-1200.

YU Xiaoyong, WEI Yanding, HUANG Mangmang, et al.Precise timing of HLA-based hardware-in-loop simulation for vehicles[J].Journal of Zhejiang University:Engineering Science, 2012, 46(7):1195-1200.(in Chinese)

19 KANAYAMA Y, MIYAKE N.Trajectory generation for mobile robots[C]∥Proceedings of the International Symposium on Robotics Research,1985: 16-23.

20 PINCHARD O, LIEGEOIS A, POUGNET F.Generalized polar polynomials for vehicle path generation with dynamic constraints[C]∥1996 IEEE International Conference on Robotics and Automation, 1996, 1: 915-920.

PredictiveControlMethodofAutonomousVehicleBasedonTracking-errorModel

LI Peixin1JIANG Xiaoyan2WEI Yanding1ZHOU Xiaojun1

(1.KeyLaboratoryofAdvancedManufacturingTechnologyofZhejiangProvince,ZhejiangUniversity,Hangzhou310027,China2.TheFirstInstituteofChinaAerospaceScienceandTechnologyCorporation,Beijing100076,China)

For the trajectory tracking problem of autonomous vehicle, on the basis of analysis of kinematic model of vehicle, a model based predictive control method for autonomous vehicle trajectory tracking was designed.Firstly, a linear error model of vehicle kinematics was obtained by using a successive linearization approach, and it was used to predict the future behavior of the vehicle.Secondly, based on this model, it was possible to get a sequence of optimal control by using the linear MPC method and minimizing the objective function, and the first element of this sequence was applied to the system.Lastly, three typical test trajectories (lane change course, figure eight course and road course) were designed and the tracking controller was tested in the virtual simulation platform.The platform was set up on real-time multi-body dynamics software Vortex and visual rendering software Vega Prime.In order to meet the real-time requirements of the platform, two computers were used for dynamic resolving and visual rendering respectively, and the high level architecture (HLA) was adopted to realize the synchronization and data interaction between Vortex and Vega Prime.Simulation results showed that this controller can track the reference trajectory quickly and stably, the distance error and heading error were in a reasonable range.The refresh rate of Vortex and Vega Prime was stabilized at about 30 Hz, the error was within ±0.05 Hz, indicating that the controller can meet the real-time requirements of the system.

autonomous vehicle; tracking error; predictive control

10.6041/j.issn.1000-1298.2017.10.045

TP242.6； U461.1

A

1000-1298(2017)10-0351-07

2017-01-17

2017-03-06

航天支撑技术基金项目(E20130116)

李培新(1986—)，男，博士生，主要从事车辆半实物虚拟仿真和无人驾驶车辆仿真研究，E-mail：beyondlpx@163.com

魏燕定(1970—)，男，教授，博士生导师，主要从事机械动力学和车辆半实物虚拟仿真研究，E-mail：weiyd@zju.edu.cn