Apollo代码学习—车辆运动学模型

前言
车辆模型
单车模型(Bicycle Model)
车辆运动学模型
阿克曼转向几何(Ackerman turning geometry)
小结

Apollo(阿波罗)是一个开放的、完整的、安全的自动驾驶平台，以灵活和高性能的技术架构，为全自动驾驶提供支持。

前言

要实现对车辆的控制，研究其运动学模型和动力学模型必不可少。上文对横纵向控制的构成及实现做了概述，本文将从车辆的运动学和动力学模型入手，研究如何控制车辆，以及为什么这样控制车辆。

本文的观点及图表多基于下述书籍及链接，并结合Apollo代码整理而来，由于本身并没有学过实车相关的知识，对力学的认知也不够深，如有理解偏颇或侵权之处，请指正删改。
【1】Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006.
【2】龚建伟, 姜岩, 徐威. 无人驾驶车辆模型预测控制[M]. 北京理工大学出版社, 2014.
【3】无人驾驶汽车系统入门（五）——运动学自行车模型和动力学自行车模型

提倡大家支持正版资源，本人提供文档仅限交流学习使用，侵删：
【1】Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. | CSDN资源
【2】龚建伟, 姜岩, 徐威. 无人驾驶车辆模型预测控制[M]. 北京理工大学出版社, 2014. | CSDN资源

车辆模型

车辆运动学模型与动力学模型的建立是出于车辆运动的规划与控制考虑的。自动驾驶场景下，车辆大多按照规划轨迹行驶，控制模块的作用就是控制车辆尽可能精准的按照规划轨迹行驶。这就要求规划轨迹尽可能贴近实际情况，也就是说，轨迹规划过程中应尽可能考虑车辆运动学及动力学约束，使得运动跟踪控制的性能更好。

搭建车辆模型主要是为了更好的规划和控制，因此，在分析模型时尽量以应用时所需的输入、输出对对象进行建模分析。Apollo中，计算控制命令需要车辆的定位信息、底盘信息以及规划信息等，

Status xxxController::ComputeControlCommand(const localization::LocalizationEstimate *localization, //定位信息const canbus::Chassis *chassis,  //底盘信息const planning::ADCTrajectory *planning_published_trajectory, //规划信息ControlCommand *cmd //控制命令)

其中，
定位信息(localization.proto)包含

message LocalizationEstimate {optional apollo.common.Header header = 1;optional apollo.localization.Pose pose = 2;optional Uncertainty uncertainty = 3;// The time of pose measurement, seconds since the GPS epoch (Jan 6, 1980).optional double measurement_time = 4;  // In seconds.// Future trajectory acturally driven by the driversrepeated apollo.common.TrajectoryPoint trajectory_point = 5;
}

底盘信息(chassis.proto)包含

message Chassis {enum DrivingMode {COMPLETE_MANUAL = 0;  // human driveCOMPLETE_AUTO_DRIVE = 1;AUTO_STEER_ONLY = 2;  // only steerAUTO_SPEED_ONLY = 3;  // include throttle and brake// security mode when manual intervention happens, only response statusEMERGENCY_MODE = 4;}...// Only available for Lincoln nowoptional ChassisGPS chassis_gps = 28;optional apollo.common.EngageAdvice engage_advice = 29;
}

规划信息(planning.proto)包含

message ADCTrajectoryPoint {optional double x = 1;  // in meters.optional double y = 2;  // in meters.optional double z = 3;  // height in meters.optional double speed = 6;  // speed, in meters / secondoptional double acceleration_s = 7;  // acceleration in s directionoptional double curvature = 8;  // curvature (k = 1/r), unit: (1/meters)// change of curvature in unit s (dk/ds)optional double curvature_change_rate = 9;// in seconds (relative_time = time_of_this_state - timestamp_in_header)optional double relative_time = 10;optional double theta = 11;  // relative to absolute coordinate system// calculated from the first point in this trajectoryoptional double accumulated_s = 12;// in meters, reference to route SL-coordinateoptional double s = 4 [deprecated = true];// in meters, reference to route SL-coordinateoptional double l = 5 [deprecated = true];
}

对上述元素进行精简，我们需要关注的信息主要有：

时间（ttt）
位置（x,yx, yx,y）
速度（vvv）
加速度（aaa）
角速度（ω\omegaω）
位移（sss）
航向角（ψ\psiψ）
曲率（kkk）
前轮转角（wheelwheelwheel）
方向盘转角（steersteersteer）
刹车（brakebrakebrake）
油门（throttlethrottlethrottle）
档位（geargeargear）

其中，

方向盘转角（steersteersteer）
刹车（brakebrakebrake）
油门（throttlethrottlethrottle）
档位（geargeargear）

为主要被控量。

单车模型(Bicycle Model)

建立模型时，应尽可能使模型简单易用，且能真实反映车辆特性，搭建车辆模型多基于单车模型(BicycleModel)\color{Maroon}单车模型(Bicycle\ Model)单车模型(Bicycle Model)，使用单车模型需做如下假设[^1]：

不考虑车辆在Z轴方向的运动，只考虑XY水平面的运动，如图1所示；
左右侧车轮转角一致，这样可将左右侧轮胎合并为一个轮胎，以便于搭建单车模型，如图2所示；
车辆行驶速度变化缓慢，忽略前后轴载荷的转移；
车身及悬架系统是刚性的。

图1 车辆模型

单车模型(BicycleModel)\color{Maroon}单车模型(Bicycle\ Model)单车模型(Bicycle Model)将左/右前轮合并为一个点，位于A点；将左/右后轮合并为一个点，位于B点；点C为车辆质心点。

图2 单车模型

其中，OOO为OA、OB的交点，是车辆的瞬时滚动中心，线段OA、OB分别垂直于两个滚动轮的方向；β\betaβ为滑移角（Tire Slip Angle），指车辆速度方向和车身朝向两者间所成的角度，ψ\psiψ为航向角（Heading Angle），指车身与X轴的夹角。

关于滑移角和航向角，此处有较大争议，由于此处是翻译而来的，而我不能给大家准确的信息，因此附上原文，望大家自己甄别：
原文出处：Vehicle Dynamics and Control. p20
( X , Y ) are inertial coordinates of the location of the vehicle while ψ\psiψ describes the orientation of the vehicle. The velocity of the vehicle is denoted by VVV and makes an angle β\betaβ with the longitudinal axis of the vehicle. The angle β\betaβ is called the slip angle of the vehicle.
如若有确切出处可以证明我所书写的有错，感谢指正并附上出处，感激不尽。

符号	定义	符号	定义
AAA	前轮中心	BBB	后轮中心
CCC	车辆质心	OOO	转向圆心
VVV	质心车速	RRR	转向半径
ℓr\ell_rℓr	后悬长度	ℓf\ell_fℓf	前悬长度
β\betaβ	滑移角	ψ\psiψ	航向角
δr\delta_rδr	后轮偏角	δf\delta_fδf	前轮偏角

当车辆为前轮驱动(front−wheel−only)\color{Maroon}前轮驱动(front-wheel-only)前轮驱动(front−wheel−only)时，可假设δr\delta_rδr恒为0。

车辆运动学模型

运动学是从几何学的角度研究物体的运动规律，包括物体在空间的位置、速度等随时间而产生的变化，因此，车辆运动学模型应该能反映车辆位置、速度、加速度等与时间的关系。在车辆轨迹规划过程中应用运动学模型，可以使规划出的轨迹更切合实际，满足行驶过程中的运动学几何约束，且基于运动学模型设计出的控制器也能具有更可靠的控制性能。

基于单车模型，如图2所示，搭建车辆运动学模型。由正弦法则：
sin⁡(δf−β)ℓf=sin⁡(π2−δf)R(1.1)\frac{\sin(\delta_f-\beta)}{\ell_f}=\frac{\sin(\frac{\pi}{2}-\delta_f)}{R} \tag{1.1}ℓfsin(δf−β)=Rsin(2π−δf)(1.1)
sin⁡(β−δr)ℓr=sin⁡(π2+δr)R(1.2)\frac{\sin(\beta-\delta_r)}{\ell_r}=\frac{\sin(\frac{\pi}{2}+\delta_r)}{R} \tag{1.2}ℓrsin(β−δr)=Rsin(2π+δr)(1.2)
展开公式(1.1)(1.2)(1.1)(1.2)(1.1)(1.2)可得：
sin⁡δfcos⁡β−sin⁡βcos⁡δfℓf=cos⁡δfR(1.3)\frac{\sin\delta_f\cos\beta-\sin\beta\cos\delta_f}{\ell_f}=\frac{\cos\delta_f}{R} \tag{1.3}ℓfsinδfcosβ−sinβcosδf=Rcosδf(1.3)
cos⁡δrsin⁡β−cos⁡βsin⁡δrℓr=cos⁡δrR(1.4)\frac{\cos\delta_r\sin\beta-\cos\beta\sin\delta_r}{\ell_r}=\frac{\cos\delta_r}{R} \tag{1.4}ℓrcosδrsinβ−cosβsinδr=Rcosδr(1.4)
联立公式(1.3)(1.4)(1.3)(1.4)(1.3)(1.4)可得：
(tan⁡δf−tan⁡δr)cos⁡β=ℓf+ℓrR(1.5)(\tan\delta_f-\tan\delta_r)\cos\beta=\frac{\ell_f+\ell_r}{R} \tag{1.5}(tanδf−tanδr)cosβ=Rℓf+ℓr(1.5)

低速环境下，车辆行驶路径的转弯半径变化缓慢，此时我们可以假设车辆的方向变化率等于车辆的角速度。则车辆的角速度为ψ˙=VR(1.6)\dot{\psi}=\frac{V}{R} \tag{1.6}ψ˙=RV(1.6)
联立公式(1.5)(1.6)(1.5)(1.6)(1.5)(1.6)可得：
ψ˙=Vcos⁡βℓf+ℓr(tan⁡δf−tan⁡δr)(1.7)\dot{\psi}=\frac{V\cos\beta}{\ell_f+\ell_r}(\tan\delta_f-\tan\delta_r) \tag{1.7}ψ˙=ℓf+ℓrVcosβ(tanδf−tanδr)(1.7)
则在惯性坐标系XY下，可得车辆运动学模型：

{X˙=Vcos⁡(ψ+β)Y˙=Vsin⁡(ψ+β)ψ˙=Vcos⁡βℓf+ℓr(tan⁡δf−tan⁡δr)(1.8)\begin{cases} \dot{X}=V\cos(\psi+\beta) \\ \dot{Y}=V\sin(\psi+\beta) \\ \dot{\psi}=\frac{V\cos\beta}{\ell_f+\ell_r}(\tan\delta_f-\tan\delta_r) \tag{1.8} \end{cases}⎩⎪⎨⎪⎧X˙=Vcos(ψ+β)Y˙=Vsin(ψ+β)ψ˙=ℓf+ℓrVcosβ(tanδf−tanδr)(1.8)

此模型中有三个输入：δf\delta_fδf、δr\delta_rδr、VVV。
滑移角β\betaβ可由公式(1.3)(1.4)(1.3)(1.4)(1.3)(1.4)求得：
β=tan⁡−1(ℓftan⁡δr+ℓrtan⁡δfℓf+ℓr)(1.9)\beta=\tan^{-1}(\frac{\ell_f\tan\delta_r+\ell_r\tan\delta_f}{\ell_f+\ell_r}) \tag{1.9}β=tan−1(ℓf+ℓrℓftanδr+ℓrtanδf)(1.9)

阿克曼转向几何(Ackerman turning geometry)

阿克曼转向几何(AckermanTurningGeometry)\color{Red}阿克曼转向几何(Ackerman\ Turning\ Geometry)阿克曼转向几何(Ackerman Turning Geometry)是一种为了解决交通工具转弯时，内外转向轮路径指向的圆心不同的几何学。

在单车模型中，将转向时左/右前轮偏角假设为同一角度，虽然通常两个角度大致相等，但实际并不是，通常情况下，内侧轮胎转角更大。如下图所示，δo\delta_oδo和δi\delta_iδi分别为外侧前轮和内侧前轮偏角，当车辆右转时，右前轮胎为内侧轮胎，其转角δi\delta_iδi较左前轮胎转角δo\delta_oδo更大。ℓw\ell_wℓw为轮距，LLL为轴距，后轮两轮胎转角始终为0°\color{Maroon}0\degree0°。
当以后轴中心为参考点时，转向半径RRR为下图中红线\color{Red}红线红线。

图3 阿克曼转向几何

当滑移角β\betaβ很小时，且后轮偏角为0时，公式(1.7)(1.7)(1.7)可表述为：
ψ˙V≈1R=δL(1.10)\frac{\dot{\psi}}{V}\approx\frac{1}{R}=\frac{\delta}{L} \tag{1.10}Vψ˙≈R1=Lδ(1.10)
由于内外侧轮胎的转向半径不同，因此有：
δo=LR+ℓw2(1.11)\delta_o=\frac{L}{R+\frac{\ell_w}{2}} \tag{1.11}δo=R+2ℓwL(1.11)
δi=LR−ℓw2(1.12)\delta_i=\frac{L}{R-\frac{\ell_w}{2}} \tag{1.12}δi=R−2ℓwL(1.12)
则前轮平均转角
δ=δo+δi2≅LR(1.13)\delta=\frac{\delta_o+\delta_i}{2}\cong\frac{L}{R}\tag{1.13}δ=2δo+δi≅RL(1.13)
内外转角之差
Δδ=δi−δo=LR2ℓw=δ2ℓwL(1.14)\Delta\delta=\delta_i-\delta_o=\frac{L}{R^2}\ell_w=\delta^2\frac{\ell_w}{L} \tag{1.14}Δδ=δi−δo=R2Lℓw=δ2Lℓw(1.14)
因此，两个前轮的转向角的差异Δδ\Delta\deltaΔδ与平均转向角δ\deltaδ的平方成正比。

依据阿克曼转向几何设计的车辆，沿着弯道转弯时，利用四连杆的相等曲柄使内侧轮的转向角比外侧轮大大约2~4度，使四个轮子路径的圆心大致上交会于后轴的延长线上瞬时转向中心，让车辆可以顺畅的转弯。

图4 梯形拉杆装置差动转向机构

小结

以后轴中心为参考点，以单车模型为基础建立车辆的运动学模型，(Xr,Yr)(X_r, Y_r)(Xr,Yr)为后轴中心坐标，ψ\psiψ为航向角，vrv_rvr为车速，δf\delta_fδf为前轮转角，后轮转角δr\delta_rδr恒为0°0\degree0°，www为横摆角速度，滑移角β\betaβ极小，假设为0。

当状态量为ξ=[Xr,Yr,ψ]T\xi=[X_r, Y_r, \psi]^Tξ=[Xr,Yr,ψ]T，被控量为u=[vr,δf]Tu=[v_r, \delta_f]^Tu=[vr,δf]T时，公式(1.8)(1.8)(1.8)可转换为如下形式：
[Xr˙Yr˙ψ˙]=[cos⁡ψsin⁡ψtan⁡δf/l]vr(1.15)\begin{bmatrix} \dot{X_r} \\ \dot{Y_r} \\ \dot\psi \end{bmatrix}= \begin{bmatrix} \cos\psi \\ \sin\psi \\ \tan{\delta_f}/l \end{bmatrix}v_r \tag{1.15}⎣⎡Xr˙Yr˙ψ˙⎦⎤=⎣⎡cosψsinψtanδf/l⎦⎤vr(1.15)
但在无人车控制过程中，一般控制对象u=[vr,w]Tu=[v_r, w]^Tu=[vr,w]T，则式1.15可写为：
[Xr˙Yr˙ψ˙]=[cos⁡ψsin⁡ψ0]vr+[001]w(1.16)\begin{bmatrix} \dot{X_r} \\ \dot{Y_r} \\ \dot\psi \end{bmatrix}= \begin{bmatrix} \cos\psi \\ \sin\psi \\ 0 \end{bmatrix}v_r+ \begin{bmatrix} 0\\ 0 \\ 1 \end{bmatrix}w \tag{1.16}⎣⎡Xr˙Yr˙ψ˙⎦⎤=⎣⎡cosψsinψ0⎦⎤vr+⎣⎡001⎦⎤w(1.16)
速度vrv_rvr的控制主要通过刹车（brakebrakebrake）、油门（throttlethrottlethrottle）、档位（geargeargear）等来控制，横摆角速度www主要通过转动方向盘（steersteersteer）来控制。

Apollo中的运动学约束(control_gflags.cc)主要有：

DEFINE_int32(max_localization_miss_num, 20,"Max missing number of localization before entering estop mode");
DEFINE_int32(max_chassis_miss_num, 20,"Max missing number of chassis before entering estop mode");
DEFINE_int32(max_planning_miss_num, 20,"Max missing number of planning before entering estop mode");DEFINE_double(max_acceleration_when_stopped, 0.01,"max acceleration can be observed when vehicle is stopped");
DEFINE_double(max_abs_speed_when_stopped, 0.01,"max absolute speed can be observed when vehicle is stopped");
DEFINE_double(steer_angle_rate, 100.0,"Steer angle change rate in percentage.");

车辆动力学模型请看下一篇博文：Apollo代码学习(三)—车辆动力学模型

Apollo代码学习(二)—车辆运动学模型相关推荐

Apollo代码学习(五)—横纵向控制
Apollo代码学习-横纵向控制前言纵向控制横向控制前馈控制注意反馈控制总结补充 2018.11.28 前言在我的第一篇博文:Apollo代码学习(一)-控制模块概述中,对横纵向控制 ...
Apollo学习笔记（7）车辆运动学模型
这里又整了一遍车辆运动学,之前在Autoware那会,已经在控制那一部分整过一次了,就当复习了吧. 前言车辆的控制目前用的最广的有三种方式: PID LQR MPC 其中,PID是一种对模型无要求的 ...
Apollo代码学习(六)—模型预测控制(MPC)
Apollo代码学习-模型预测控制前言模型预测控制预测模型线性化单车模型滚动优化反馈矫正总结前言非专业选手,此篇博文内容基于书本和网络资源整理,可能理解的较为狭隘,起点较低,就事论 ...
Apollo代码学习(六)—模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
Apollo代码学习(六)-模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
Apollo代码学习(七)—MPC与LQR比较
Apollo代码学习-MPC与LQR比较前言研究对象状态方程工作时域目标函数求解方法前言 Apollo中用到了PID.MPC和LQR三种控制器,其中,MPC和LQR控制器在状态方程的形式 ...
【自动驾驶】车辆运动学模型
文章目录参考资料 1. 以车辆重心为中心的单车运动学模型 1.1 参数说明 1.2 几何关系 1.2.1 偏航角ψ\psiψ的关系 1.2.1 滑移角 β\betaβ 的关系 1.2.2 运动学模型 ...
[pytorch] PyTorch Metric Learning库代码学习二 Inference
PyTorch Metric Learning库代码学习二 Inference Install the packages Import the packages Create helper funct ...
自动驾驶控制算法——车辆运动学模型
车辆运动学模型自行车模型(Bicycle Model) 自行车模型(Bicycle Model)的建立基于如下假设: 不考虑车辆在垂直方向(Z轴方向)的运动,即假设车辆的运动是一个二维平面上的运动; ...
《无人驾驶车辆模型预测控制》之车辆运动学模型
车辆运动学模型是根据车辆转向的几何学的角度关系来描述车辆的运动的.主要关注车辆的轴长.转弯半径.和前轮转角.车辆运动学只需要用较少的车辆参数就可以基本描述车辆的运动情况,但是一般只适合在低速下有一定的 ...

Apollo代码学习(二)—车辆运动学模型