1. 简介

车辆运动规划与控制需要通过对车辆运动学或者动力学系统的控制来实现
如果规划阶段能够考虑车辆 运动学和动力学约束，那么运动跟踪控制性能会更好

车辆在地面运动的动力学过程是非常复杂的，为了尽量准确描述车辆运动，需要建立复杂的微分方程组，并用多个状态变量来描述其运动

用于模型预测控制的模型只要能够表现出车辆运动学与动力学约束，就可以使模型预测控制器实现预定控制目的
特别是在规划阶段，为了保证规划算法的实时性，约束简化和近似就是一种非常重要的手段，比如 轮胎摩擦圆约束 和 点质量模型

因此，过于复杂的模型并不是研究的重点，实际性价比不大
从无人驾驶车辆 路径重规划 和 道路跟踪控制 的角度对车辆系统进行建模,建立能够尽量准确反映车辆运动特性,并且有利于模型预测控制器设计的简化车辆运动学模型和动力学模型

2. 车辆运动学

车辆运动学模型从几何学的角度研究车辆的运动规律
包括车辆的空间位姿、速度等随时间的变化

当车辆在良好路面上低速行驶时，一般不需要考虑车辆的操纵稳定性等动力学问题
此时基于运动学模型设计的路径跟踪控制器具备可靠的控制性能

3. 运动模型

先看阿克曼转向车辆的运动模型：
[Xr˙Yr˙φ˙]=[cos⁡φsin⁡φ0]vr+[001]w\left[ \begin{matrix} \dot{X_r} \\ \dot{Y_r}\\ \dot{\varphi} \end{matrix} \right]= \left[ \begin{matrix} \cos \varphi \\ \sin \varphi\\ 0 \end{matrix} \right]v_r + \left[ \begin{matrix} 0 \\ 0 \\ 1 \end{matrix} \right]w ⎣⎡Xr˙Yr˙φ˙⎦⎤=⎣⎡cosφsinφ0⎦⎤vr+⎣⎡001⎦⎤w

符号	含义	符号	含义	符号	含义
Xr˙\dot{X_r}Xr˙	车辆后轴中心 X坐标位置的变化量	Yr˙\dot{Y_r}Yr˙	车辆后轴中心 Y坐标位置的变化量	φ˙\dot{\varphi}φ˙	车辆的横摆角（航向角）的变化量
φ\varphiφ	车辆的横摆角（航向角）	vrv_rvr	车辆后轴中心的速度	www	车辆横摆角的角速度

4. 推导

大概从上面公式也能知道其含义，接下来看图推导：

符号	含义	符号	含义	符号	含义
(Xf˙\dot{X_f}Xf˙, Yf˙\dot{Y_f}Yf˙)	车辆前轴中心的坐标	(Xr˙\dot{X_r}Xr˙, Yr˙\dot{Y_r}Yr˙)	车辆后轴中心的坐标	φ\varphiφ	车辆的横摆角（航向角）
vfv_fvf	车辆后轴中心的速度	vrv_rvr	车辆后轴中心的速度	δf\delta_fδf	前轮偏角
lll	轴距	NNN	车辆前轴中心	MMM	车辆后轴中心
RRR	后轮转向半径	PPP	车辆瞬时转动中心	www	车辆横摆角的角速度

此处假设转向过程中车辆质心侧偏角保持不变，即车辆瞬时转向半径与道路曲率半径相同

在后轴行驶轴心(Xr,Yr)(X_r, Y_r)(Xr,Yr)处速度为：vr=Xr˙cos⁡φ+Yr˙sin⁡φ(1)v_r =\dot{X_r}\cos\varphi + \dot{Y_r}\sin\varphi \tag{1}vr=Xr˙cosφ+Yr˙sinφ(1)

前后轴的运动学约束 为：
{Xf˙sin⁡(φ+δf)−Yf˙cos⁡(φ+δf)=0Xr˙sin⁡φ−Yr˙cos⁡φ=0(2)\begin{cases} \dot{X_f}\sin(\varphi + \delta_f) - \dot{Y_f}\cos(\varphi + \delta_f) = 0\\ \dot{X_r}\sin\varphi - \dot{Y_r}\cos\varphi = 0 \end{cases}\tag{2} {Xf˙sin(φ+δf)−Yf˙cos(φ+δf)=0Xr˙sinφ−Yr˙cosφ=0(2)

垂直于运动方向上的变化相互抵消

可得：
{Xr˙=vrcos⁡φYr˙=vrsin⁡φ(3)\begin{cases} \dot{X_r} = v_r\cos\varphi\\ \dot{Y_r} = v_r\sin\varphi \end{cases} \tag{3} {Xr˙=vrcosφYr˙=vrsinφ(3)

根据前后轮的几何关系可得：
{Xf=Xr+lcos⁡φYf=Yr+lsin⁡φ(4)\begin{cases} X_f = X_r + l\cos\varphi\\ Y_f = Y_r + l\sin\varphi \end{cases} \tag{4} {Xf=Xr+lcosφYf=Yr+lsinφ(4)
将公式3 和 4 代入公式2
可解得横摆角速度为：w=vrltan⁡δf(5)w =\frac{v_r}{l}\tan\delta_f\tag{5}w=lvrtanδf(5)

其实可以从图中的几何关系推导得出，假设横摆的线速度为 vwv_wvw，那么有 tan⁡δf=vw/vr\tan\delta_f= v_w / v_rtanδf=vw/vr，而角速度公式 w=vw/lw = v_w / lw=vw/l

不过推导一下也不是很难，恩，相对后续的内容，这是最简单的了
先对公式4求导
{Xf˙=Xr˙+l(−sin⁡φ)⋅φ˙Yf˙=Yr˙+lcos⁡φ⋅φ˙\begin{cases} \dot{X_f} = \dot{X_r} + l(-\sin\varphi) \cdot \dot{\varphi} \\ \dot{Y_f} = \dot{Y_r} + l\cos\varphi \cdot \dot{\varphi} \end{cases} {Xf˙=Xr˙+l(−sinφ)⋅φ˙Yf˙=Yr˙+lcosφ⋅φ˙
把公式3代入
{Xf˙=vrcos⁡φ+l(−sin⁡φ)⋅φ˙Yf˙=vrsin⁡φ+lcos⁡φ⋅φ˙\begin{cases} \dot{X_f} = v_r\cos\varphi+ l(-\sin\varphi) \cdot \dot{\varphi} \\ \dot{Y_f} = v_r\sin\varphi + l\cos\varphi \cdot \dot{\varphi} \end{cases} {Xf˙=vrcosφ+l(−sinφ)⋅φ˙Yf˙=vrsinφ+lcosφ⋅φ˙
再代入公式2的第一个式子
(vrcos⁡φ+l(−sin⁡φ)⋅φ˙)⋅sin⁡(φ+δf)−(vrsin⁡φ+lcos⁡φ⋅φ˙)⋅cos(φ+δf)=0(v_r\cos\varphi+ l(-\sin\varphi) \cdot \dot{\varphi}) \cdot \sin(\varphi + \delta_f) - (v_r\sin\varphi + l\cos\varphi \cdot \dot{\varphi}) \cdot cos(\varphi + \delta_f) = 0 (vrcosφ+l(−sinφ)⋅φ˙)⋅sin(φ+δf)−(vrsinφ+lcosφ⋅φ˙)⋅cos(φ+δf)=0
展开
vrcos⁡φsin⁡(φ+δf)−l⋅φ˙⋅sin⁡φsin⁡(φ+δf)−vrsin⁡φcos⁡(φ+δf)+l⋅φ˙⋅cos⁡φcos⁡(φ+δf)=0v_r\cos\varphi \sin(\varphi + \delta_f) - l \cdot \dot{\varphi} \cdot\sin\varphi \sin(\varphi + \delta_f) - v_r\sin\varphi \cos(\varphi + \delta_f) + l\cdot \dot{\varphi}\cdot\cos\varphi \cos(\varphi + \delta_f) = 0 vrcosφsin(φ+δf)−l⋅φ˙⋅sinφsin(φ+δf)−vrsinφcos(φ+δf)+l⋅φ˙⋅cosφcos(φ+δf)=0
梳理一下
vrcos⁡φsin⁡(φ+δf)−vrsin⁡φcos⁡(φ+δf)=l⋅φ˙⋅sin⁡φsin⁡(φ+δf)−l⋅φ˙⋅cos⁡φcos⁡(φ+δf)vr(cos⁡φsin⁡(φ+δf)−sin⁡φcos⁡(φ+δf))=l⋅φ˙⋅(sin⁡φsin⁡(φ+δf)−cos⁡φcos⁡(φ+δf))vrsin⁡δf=l⋅φ˙⋅cos⁡δfw=φ˙=vrltan⁡δfv_r\cos\varphi \sin(\varphi + \delta_f) - v_r\sin\varphi \cos(\varphi + \delta_f) = l \cdot \dot{\varphi} \cdot\sin\varphi \sin(\varphi + \delta_f) - l \cdot \dot{\varphi}\cdot\cos\varphi \cos(\varphi + \delta_f)\\ v_r(\cos\varphi \sin(\varphi + \delta_f) - \sin\varphi \cos(\varphi + \delta_f) ) = l \cdot \dot{\varphi} \cdot (\sin\varphi \sin(\varphi + \delta_f) - \cos\varphi \cos(\varphi + \delta_f))\\ v_r \sin\delta_f = l \cdot \dot{\varphi} \cdot\cos\delta_f\\ w =\dot{\varphi} =\frac{v_r}{l}\tan\delta_f vrcosφsin(φ+δf)−vrsinφcos(φ+δf)=l⋅φ˙⋅sinφsin(φ+δf)−l⋅φ˙⋅cosφcos(φ+δf)vr(cosφsin(φ+δf)−sinφcos(φ+δf))=l⋅φ˙⋅(sinφsin(φ+δf)−cosφcos(φ+δf))vrsinδf=l⋅φ˙⋅cosδfw=φ˙=lvrtanδf

同时根据横摆角速度www 和车速 vrv_rvr 可以得到转向半径RRR 和前轮偏角 δf\delta_fδf
{R=vr/wδf=arctan⁡(l/R)(6)\begin{cases} R = v_r / w\\ \delta_f = \arctan(l/R) \end{cases}\tag{6} {R=vr/wδf=arctan(l/R)(6)

由公式3 和 5，可得到车辆运动学模型为：
[Xr˙Yr˙φ˙]=[cos⁡φsin⁡φtan⁡δf/l]vr(7)\left[ \begin{matrix} \dot{X_r} \\ \dot{Y_r}\\ \dot{\varphi} \end{matrix} \right]= \left[ \begin{matrix} \cos \varphi \\ \sin \varphi\\ \tan\delta_f / l \end{matrix} \right]v_r\tag{7} ⎣⎡Xr˙Yr˙φ˙⎦⎤=⎣⎡cosφsinφtanδf/l⎦⎤vr(7)

该模型可被进一步表示为更为一般的形式：ξ˙kin=fkin(ξkin,ukin)\dot{\xi}_{kin} = f_{kin}(\xi_{kin}, u_{kin})ξ˙kin=fkin(ξkin,ukin)

状态量 ξkin=[Xr,Yr,φ]T\xi_{kin} = [X_r, Y_r, \varphi]^Tξkin=[Xr,Yr,φ]T，即位置和横摆角
控制量 ukin=[vr,δf]Tu_{kin} = [v_r, \delta_f]^Tukin=[vr,δf]T，即速度和前轮偏角，可以简单理解为油门和方向盘角度

在路径跟踪控制过程中，往往采用速度和横摆角速度作为控制量，将公式5 代入上运动学模型转换成如下：

[Xr˙Yr˙φ˙]=[cos⁡φsin⁡φ0]vr+[001]w(8)\left[ \begin{matrix} \dot{X_r} \\ \dot{Y_r}\\ \dot{\varphi} \end{matrix} \right]= \left[ \begin{matrix} \cos \varphi \\ \sin \varphi\\ 0 \end{matrix} \right]v_r + \left[ \begin{matrix} 0 \\ 0 \\ 1 \end{matrix} \right]w\tag{8} ⎣⎡Xr˙Yr˙φ˙⎦⎤=⎣⎡cosφsinφ0⎦⎤vr+⎣⎡001⎦⎤w(8)

5. 车辆跟踪误差模型

跟踪误差模型是车辆路径跟踪控制中常用的车辆运动模型之一

如图所示，

符号	含义	符号	含义	符号	含义
ede_ded	车辆后轴中心与其在道路中心线上的投影点之间的距离（路径跟踪的距离偏差）	eφe_\varphieφ	路径跟踪的航向偏差	MMM	道路车辆后轴中心
φroad\varphi_{road}φroad	道路中心线切向与惯性坐标系X轴的夹角	φ\varphiφ	车辆的横摆角	κref\kappa_{ref}κref	P1P_1P1点处的曲率
P0P_{0}P0	道路中心线上的某个参考点	P1P_{1}P1	车辆后轴中心在道路中心线上的投影	sss	P1P_{1}P1到P0P_{0}P0的弧长
vxv_xvx	车辆坐标系xxx速度	vyv_yvy	车辆坐标系yyy速度	s˙\dot{s}s˙	P1P_{1}P1点沿道路中心线的移动速度

其中 P1P_1P1 点处的曲率 κref=1/R\kappa_{ref}=1/Rκref=1/R
角速度相同的条件下，可以得到关于投影沿道路中心线的移动速度s˙\dot{s}s˙ 的以下关系：
s˙/R=[vxcos⁡(eφ)−vysin⁡(eφ)]/(R−ed)\dot{s}/R=[v_x\cos(e_\varphi) - v_y\sin(e_\varphi)]/(R-e_d)s˙/R=[vxcos(eφ)−vysin(eφ)]/(R−ed)

那么车辆后轴中心在道路中心线上的投影沿道路中心线的移动速度 s˙\dot{s}s˙ 可以
把 κref=1/R\kappa_{ref}=1/Rκref=1/R 代入，即
s˙=11−κrefed[vxcos⁡(eφ)−vysin⁡(eφ)](9)\dot{s}=\frac{1}{1-\kappa_{ref}e_d}[v_x\cos(e_\varphi) - v_y\sin(e_\varphi)]\tag{9}s˙=1−κrefed1[vxcos(eφ)−vysin(eφ)](9)

车辆跟踪误差方程可以表示为：
{eφ˙=φ˙−κrefs˙ed˙=vxsin⁡(eφ)+vycos⁡(eφ)(10)\begin{cases} \dot{e_\varphi}=\dot\varphi-\kappa_{ref}\dot{s}\\ \dot{e_d}=v_x\sin(e_\varphi) + v_y\cos(e_\varphi) \end{cases}\tag{10}{eφ˙=φ˙−κrefs˙ed˙=vxsin(eφ)+vycos(eφ)(10)

对航向偏差 eφe_\varphieφ 采取小角度假设 sin(eφ)≈0，cos(eφ)≈1sin(e_\varphi)≈0，cos(e_\varphi)≈1sin(eφ)≈0，cos(eφ)≈1
令 κrefed≈0\kappa_{ref}e_d≈0κrefed≈0，则公式10可以简化为：
{eφ˙=φ˙−κrefvx1−κrefed≈φ˙−κrefvxed˙=vxeφ+vy(11)\begin{cases} \dot{e_\varphi}=\dot\varphi-\frac{\kappa_{ref}v_x}{1-\kappa_{ref}e_d}≈\dot\varphi-\kappa_{ref}v_x\\ \dot{e_d}=v_xe_\varphi+ v_y \end{cases}\tag{11}{eφ˙=φ˙−1−κrefedκrefvx≈φ˙−κrefvxed˙=vxeφ+vy(11)

忽略车辆的横向速度 vyv_yvy，则可以写为：
[eφ˙ed˙]=[00vx0][eφed]+[vxl0]tan⁡(δf)+[−vx0]κref(12)\begin{bmatrix} \dot{e_\varphi} \\ \dot{e_d} \end{bmatrix}= \begin{bmatrix} 0 &0 \\v_x &0 \end{bmatrix} \begin{bmatrix} e_\varphi \\e_d \end{bmatrix} + \begin{bmatrix} \frac{v_x}{l} \\0 \end{bmatrix}\tan( \delta_f) + \begin{bmatrix} -v_x \\0 \end{bmatrix}\kappa_{ref} \tag{12} [eφ˙ed˙]=[0vx00][eφed]+[lvx0]tan(δf)+[−vx0]κref(12)
根据几何关系或者是公式5和6，也可以得出 κref=tan⁡(δref)/l\kappa_{ref}=\tan(\delta_{ref})/lκref=tan(δref)/l
δref\delta_{ref}δref表示由参考路径获取的前馈控制量

再令 u1=tan⁡(δf)≈δf，u2=tan⁡(δref)≈δref，ξ=[eφed]Tu_1=\tan(\delta_f)≈\delta_f， u_2=\tan(\delta_{ref})≈\delta_{ref}， \xi =[e_\varphi \ \ e_d]^Tu1=tan(δf)≈δf，u2=tan(δref)≈δref，ξ=[eφ ed]T，可以再简化为：

ξ˙=Aξ+B1u1+B2u2(13)\dot{\xi} = A\xi+B_1u_1+B_2u_2\tag{13}ξ˙=Aξ+B1u1+B2u2(13)

其中，A=[00vx0]，B1=[vxl0]T，B2=[−vxl0]TA= \begin{bmatrix} 0 &0 \\v_x &0 \end{bmatrix}， B_1= \begin{bmatrix} \frac{v_x}{l} \ \ 0 \end{bmatrix}^T， B_2= \begin{bmatrix} -\frac{v_x}{l} \ \ 0 \end{bmatrix}^TA=[0vx00]，B1=[lvx 0]T，B2=[−lvx 0]T

基于跟踪误差模型设计的模型预测控制权可以方便的对跟踪过程中的航向偏差和距离偏差施加约束
同时可以考虑道路曲率对跟踪效果的影响，有利于提升弯道跟踪的效果

谢谢

车辆运动控制（1）车辆运动学建模相关推荐

车辆运动控制（4）考虑道路曲率和轮胎滑移
车辆运动控制(4)考虑道路曲率和轮胎滑移 1. 车辆横摆动力学模型简化 1.1 小角度假设 1.2 轮胎垂向载荷 1.3 简化 2. 考虑道路曲率 3. 考虑轮胎滑移 1. 车辆横摆动力学模型简化车 ...
车辆运动控制（3）轮胎模型
车辆运动控制(3)轮胎模型 1. 简介 2. 轮胎模型 3. Pacejka 轮胎模型 4. 轮胎侧向力 4.1. 根据魔术公式计算 4.2. 根据刷子轮胎模型计算 5. 线性化 1. 简介在 &l ...
车辆运动控制（6）考虑侧倾约束
车辆运动控制(6)考虑侧倾约束 1. 简介 2. 模型受力分析 3. 简化 1. 简介前两节介绍的车辆模型未能考虑车辆的操纵稳定性,或者说只能体现车辆横摆稳定性鉴于无人驾驶车辆的操纵稳定性需要综 ...
自动驾驶车辆运动控制过程中，对PID控制参数的优化和整定。传统PID控制算法
自动驾驶车辆运动控制过程中,对PID控制参数的优化和整定. 传统PID控制算法,由于参数固定,在道路曲率较大和车速变化情况下难以满足控制需求,采用RL强化学习算法(DDPG算法,Actor-Criti ...
车辆运动控制（5）点质量车辆动力学模型
车辆运动控制(5)点质量车辆动力学模型 1. 进一步简化 2. 摩擦圆约束 1. 进一步简化点质量车辆动力学模型忽略了车辆的尺寸信息以及由横.纵向加速度引起的载荷转移用一个带质量的点来描述车辆运动 ...
车辆运动控制（7）考虑道路倾角和曲率
车辆运动控制(7)考虑道路倾角和曲率 1. 考虑道路倾角及曲率影响 2. 综合等效约束车辆动力学模型 1. 考虑道路倾角及曲率影响道路倾角与曲率对无人驾驶车辆的操纵稳定性有着重要影响直接关系到车辆 ...
麦克纳姆小车运动学建模翻译
基于麦克纳姆轮的移动平台运动学和动力学建模作者:Nkgatho Tlale Mark de Villiers 摘要: 全方位移动平台具有从任意布局状态迅速的任意方向进行移动的能力,因此,对于建立全向 ...
六自由度机器人（机械臂）运动学建模及运动规划系列（二）——运动学分析
本篇主要介绍六轴机械臂的运动学分析. 运动学分析是工业机器人研究和应用的重要内容,是运动控制的基础,主要研究机器人末端坐标系与基坐标系的转换关系,分为正运动学和逆运动学分析两部分. 另外,对于刚刚学习 ...
机器人学回炉重造（1-2）：各种典型机械臂的正运动学建模（标准D-H法）
文章目录写在前面三连杆平面机械臂平行四边形操作臂闭链结构例:平行四边形操作臂球形臂拟人臂球腕斯坦福机械臂带球形手腕的拟人化机械臂 DLR机械臂参考文献写在前面本文所有机械臂均 ...
走好每一步，基于C实现机器人运动学建模与标定、运动规划、轨迹规划算法
走好每一步,基于C实现机器人运动学建模与标定.运动规划.轨迹规划算法废话综述一:C部分初始C语言 Chapter2-4:基本数据类型 Chapter5-7:运算符.表达式.循环.分支与跳转 C ...

车辆运动控制（1）车辆运动学建模