参考：chatGPT
目录
- 为什么要学这套链条
- 理想阿克曼转向模型：所有后续公式的几何起点
- 车辆运动学模型：从阿克曼几何到自行车模型
- 为什么不用全局位置直接控，而要引入 Frenet 误差
- Frenet 坐标系：每个量到底是什么意思
- Frenet 误差模型的完整推导
- 小误差近似：工程模型为什么会简化成那样
- 转向执行器动态： τ \tau τ 从哪里来，为什么公式要这样写
- 非线性误差模型如何线性化成 x ˙ = A x + B u + w \dot x = A x + B u + w x˙=Ax+Bu+w
- 连续时间模型如何离散化成 x k + 1 = A d x k + B d u k + W d x_{k+1}=A_d x_k+B_d u_k+W_d xk+1=Adxk+Bduk+Wd
- 单步模型为什么还不够：MPC 为什么必须做未来时域展开
- 从单步离散模型到批量预测方程
- 代价函数是怎么设计出来的，为什么是二次型
- 把预测方程代入代价函数：如何变成只关于输入的优化问题
- 输入约束和输入变化率约束如何写成 QP 形式
- QP 求解
- 整套链条的统一大图景
- 常见误区与统一澄清
1. 为什么要学这套链条
自动驾驶横向控制的目标，说得最朴素一点，就是：
让车沿着一条给定的参考轨迹平稳地走。
如果只从这个目标出发，你很容易以为问题只是：
- 当前车的位置是多少
- 当前车头朝向是多少
- 方向盘该打多少
但真正做控制时，会发现单独看世界坐标下的位置和姿态是不够的。控制器真正关心的是：
- 车偏离参考轨迹多少
- 偏离的方向是什么
- 车头和参考轨迹方向差了多少
- 如果我现在和未来几步分别打多少方向，未来误差会怎样演化
于是就形成了一整条经典建模链：
text
车辆几何
→ 车辆运动学
→ 相对轨迹误差建模
→ 执行器建模
→ 线性化
→ 离散化
→ 未来时域展开
→ 代价函数设计
→ QP 求解这篇文章就是把这条链从头到尾讲清楚。
2. 理想阿克曼转向模型：所有后续公式的几何起点
2.1 阿克曼转向到底要解决什么
先想一个非常基础的问题：四轮车转弯时，为什么左右前轮角度不能一样？
如果假设车辆低速转弯、轮胎纯滚动且无侧滑，那么每个车轮都必须满足：
- 轮心速度方向与车轮平面方向一致
- 车轮不能横着滑
而刚体在平面中的瞬时运动，总可以看成绕某个点做旋转。这个点叫做：
- 瞬时旋转中心
- Instantaneous Center of Rotation, ICR
要让所有车轮都不侧滑，那么每个车轮都必须"服从同一个 ICR"。这就意味着：
每个车轮所在平面的法线，必须都通过同一个瞬时旋转中心。
如果左右前轮打同样的角度，这个条件一般无法同时满足，于是轮胎会产生侧滑。阿克曼转向几何的作用就是让左右前轮角不同，以实现共同 ICR。
2.2 最基础的单轨几何关系
在控制和建模中，我们通常把真实四轮车等效成一个"单轨模型"或"自行车模型"：
- 左右前轮合并成一个等效前轮
- 左右后轮合并成一个等效后轮
设：
- 轴距： L L L
- 车辆等效前轮转角： δ \delta δ
- 后轴中心到 ICR 的距离： R R R
那么几何上会形成一个直角三角形，最终得到：
tan δ = L R \tan\delta = \frac{L}{R} tanδ=RL
这是整套模型最核心的几何起点之一。
2.3 为什么是 tan δ = L R \tan\delta = \frac{L}{R} tanδ=RL，而不是 R L \frac{R}{L} LR
关键在于： δ \delta δ 不是随便定义的三角形角，而是车轮方向相对于车身纵向方向的夹角。
在单轨模型里：
- 后轴中心到前轴中心的方向是车身纵向
- ICR 位于后轴延长线上
- 前轮方向必须与其所走圆轨迹的切线一致
由这个几何关系建立三角形时，最终得到的正切关系恰好是：
tan δ = L R \tan\delta = \frac{L}{R} tanδ=RL
可以用直觉检验：
- R R R 越大，弯越缓， δ \delta δ 应该越小
- L L L 越大，想走同样半径的弯，需要更大的 δ \delta δ
这都与 tan δ = L R \tan\delta = \frac{L}{R} tanδ=RL 完全一致。
2.4 曲率的引入
定义曲率：
κ = 1 R \kappa = \frac{1}{R} κ=R1
于是：
tan δ = L κ \tan\delta = L\kappa tanδ=Lκ
从而：
δ = arctan ( L κ ) \delta = \arctan(L\kappa) δ=arctan(Lκ)
这条公式在后面会反复出现，因为：
- 路径规划器通常更自然给出曲率 κ \kappa κ
- 车辆执行器需要的是转向角 δ \delta δ
所以这条公式本质上是在做"路径几何量 → \to → 控制输入量"的映射。
2.5 更完整的左右前轮阿克曼关系
如果不做单轨近似，而是看真实左右前轮：
设：
- 轴距： L L L
- 轮距： W W W
- 后轴中心到 ICR 的距离： R R R
- 内侧前轮转角： δ l \delta_l δl
- 外侧前轮转角： δ r \delta_r δr
则有：
tan δ l = L R − W 2 \tan\delta_l = \frac{L}{R-\frac{W}{2}} tanδl=R−2WL
tan δ r = L R + W 2 \tan\delta_r = \frac{L}{R+\frac{W}{2}} tanδr=R+2WL
从这两式可以进一步推出经典阿克曼约束：
cot δ r − cot δ l = W L \cot\delta_r - \cot\delta_l = \frac{W}{L} cotδr−cotδl=LW
这意味着：
- 左右前轮转角不是独立量
- 内侧轮角更大，外侧轮角更小
- 这正是为了让四轮对应同一个瞬时旋转中心
2.6 那个"三角形"的三个点到底是谁
对单轨模型的三角形
点分别是：
- 瞬时旋转中心 C C C
- 后轴中心 O r O_r Or
- 前轴中心 O f O_f Of
这时：
- C O r = R CO_r = R COr=R
- O r O f = L O_rO_f = L OrOf=L
从而得到：
tan δ = L R \tan\delta = \frac{L}{R} tanδ=RL
对左前轮的三角形
点分别是：
- 瞬时旋转中心 C C C
- 左后轮轮心 R l R_l Rl
- 左前轮轮心 F l F_l Fl
于是：
- R l F l = L R_lF_l = L RlFl=L
- C R l = R − W 2 CR_l = R-\frac{W}{2} CRl=R−2W
得到：
tan δ l = L R − W 2 \tan\delta_l = \frac{L}{R-\frac{W}{2}} tanδl=R−2WL
对右前轮的三角形
点分别是：
- 瞬时旋转中心 C C C
- 右后轮轮心 R r R_r Rr
- 右前轮轮心 F r F_r Fr
于是：
tan δ r = L R + W 2 \tan\delta_r = \frac{L}{R+\frac{W}{2}} tanδr=R+2WL
3. 车辆运动学模型：从阿克曼几何到自行车模型
3.1 世界坐标中的状态定义
设车辆后轴中心在世界坐标下的位置为： ( x , y ) (x,y) (x,y)，车辆航向角为： ψ \psi ψ，车辆速度为： v v v，等效前轮转角为： δ \delta δ
3.2 世界坐标下的理想运动学模型
理想自行车模型写成：
x ˙ = v cos ψ \dot x = v\cos\psi x˙=vcosψ
y ˙ = v sin ψ \dot y = v\sin\psi y˙=vsinψ
ψ ˙ = v L tan δ \dot\psi = \frac{v}{L}\tan\delta ψ˙=Lvtanδ
这三式分别表示：
- （1）（2）车在世界坐标中沿当前航向前进
- （3）航向变化率由速度、轴距、转角决定
3.3 航向变化率为什么是 v L tan δ \frac{v}{L}\tan\delta Lvtanδ
由圆周运动关系（ ω t = 2 π , v t = 2 π R \omega t=2 \pi, vt = 2 \pi R ωt=2π,vt=2πR）：
ω = v R \omega = \frac{v}{R} ω=Rv
这里 ω \omega ω 就是航向变化率 ψ ˙ \dot\psi ψ˙。
而阿克曼几何给出：
R = L tan δ R = \frac{L}{\tan\delta} R=tanδL
代入：
ψ ˙ = v R = v L / tan δ = v L tan δ \dot\psi = \frac{v}{R} = \frac{v}{L/\tan\delta} = \frac{v}{L}\tan\delta ψ˙=Rv=L/tanδv=Lvtanδ
3.4 曲率写法
由于：
κ = 1 R = tan δ L \kappa = \frac{1}{R} = \frac{\tan\delta}{L} κ=R1=Ltanδ
所以：
ψ ˙ = v κ \dot\psi = v\kappa ψ˙=vκ
这说明：
- 速度控制"前进快慢"
- 曲率控制"路径弯曲程度"
4. 为什么不用全局位置直接控，而要引入 Frenet 误差
如果你只在世界坐标下写：
- 我现在在 ( x , y ) (x,y) (x,y)
- 轨迹上的点在 ( x r , y r ) (x_r,y_r) (xr,yr)
看起来好像就能直接做控制，但这在轨迹跟踪里其实并不自然。
原因是：
- 轨迹本身可能是弯的
- 只比较欧氏位置差，无法直接体现"我相对轨迹偏到哪边"
- 车头方向和轨迹切线方向的关系也非常关键
所以我们希望定义一种"沿轨迹"和"垂直轨迹"的局部坐标描述。
这就是 Frenet 坐标系。
5. Frenet 坐标系：每个量到底是什么意思
5.1 参考轨迹的弧长参数化
用弧长 s s s 参数化参考轨迹：
r ( s ) = [ x r ( s ) y r ( s ) ] \mathbf r(s)= \begin{bmatrix} x_r(s)\\ y_r(s) \end{bmatrix} r(s)=[xr(s)yr(s)]
这里的意思是：
沿参考轨迹走了 s s s 米时，对应点的世界坐标是多少。
用弧长参数化有一个重要性质：
∣ d r d s ∣ = 1 \left|\frac{d\mathbf r}{ds}\right| = 1 dsdr =1
也就是说，轨迹对弧长求导，天然就是单位切向量（回忆高数对弧长的求导）。
5.2 切向量
参考轨迹在 s s s 处的切向方向角记为 ψ r ( s ) \psi_r(s) ψr(s)，则切向单位向量为：
t ( s ) = [ cos ψ r sin ψ r ] \mathbf t(s)= \begin{bmatrix} \cos\psi_r\\ \sin\psi_r \end{bmatrix} t(s)=[cosψrsinψr]
5.3 法向量
左法向单位向量为：
n ( s ) = [ − sin ψ r cos ψ r ] \mathbf n(s)= \begin{bmatrix} -\sin\psi_r\\ \cos\psi_r \end{bmatrix} n(s)=[−sinψrcosψr]
你可以验证：
t T n = 0 \mathbf t^T\mathbf n = 0 tTn=0
即两者正交。
5.4 横向误差 e e e 的定义
设车辆当前位置为：
p = [ x y ] \mathbf p= \begin{bmatrix} x\\ y \end{bmatrix} p=[xy]
定义 e e e 为：从参考轨迹点 r ( s ) \mathbf r(s) r(s) 出发，沿法向 n ( s ) \mathbf n(s) n(s) 走 e e e 的距离，刚好到达车辆位置 p \mathbf p p。
于是：
p = r ( s ) + e n ( s ) \mathbf p = \mathbf r(s) + e \, \mathbf n(s) p=r(s)+en(s)
这不是某种"推导出来的复杂结论"，它本身就是横向误差的定义。
展开后：
x = x r − e sin ψ r x = x_r - e\sin\psi_r x=xr−esinψr
y = y r + e cos ψ r y = y_r + e\cos\psi_r y=yr+ecosψr
5.5 航向误差 θ e \theta_e θe
定义：
θ e = ψ − ψ r \theta_e = \psi - \psi_r θe=ψ−ψr
表示车辆自身朝向和参考轨迹切线方向的差。
因此：
ψ = ψ r + θ e \psi = \psi_r + \theta_e ψ=ψr+θe
5.6 参考曲率 κ r \kappa_r κr
曲率定义为：
κ r = d ψ r d s \kappa_r = \frac{d\psi_r}{ds} κr=dsdψr
表示"沿轨迹走单位弧长，轨迹方向变化了多少"。
对于圆：
κ = 1 R \kappa = \frac{1}{R} κ=R1
这与常见几何定义完全一致。
6. Frenet 误差模型的完整推导
这是整套横向控制建模中最关键的一步。
6.1 从位置定义开始
已经有：
p = r ( s ) + e n ( s ) \mathbf p = \mathbf r(s) + e \,\mathbf n(s) p=r(s)+en(s)
由于：
- s = s ( t ) s=s(t) s=s(t)
- e = e ( t ) e=e(t) e=e(t)
- r , n \mathbf r,\mathbf n r,n 都是 s s s 的函数
6.2 对时间求导
p ˙ = d d t r ( s ) + d d t ( e , n ( s ) ) \dot{\mathbf p}= \frac{d}{dt}\mathbf r(s)+ \frac{d}{dt}\big(e,\mathbf n(s)\big) p˙=dtdr(s)+dtd(e,n(s))
第一项用链式法则：
d d t r ( s ) = d r d s s ˙ \frac{d}{dt}\mathbf r(s)= \frac{d\mathbf r}{ds}\dot s dtdr(s)=dsdrs˙
第二项用乘法法则：
d d t ( e , n ) = e ˙ , n + e , d n d t \frac{d}{dt}(e,\mathbf n)= \dot e,\mathbf n + e,\frac{d\mathbf n}{dt} dtd(e,n)=e˙,n+e,dtdn
而：
d n d t = d n d s s ˙ \frac{d\mathbf n}{dt}= \frac{d\mathbf n}{ds}\dot s dtdn=dsdns˙
所以：
p ˙ = d r d s s ˙ + e ˙ n + e d n d s s ˙ \dot{\mathbf p}= \frac{d\mathbf r}{ds}\dot s+ \dot e \, \mathbf n+ e \, \frac{d\mathbf n}{ds}\dot s p˙=dsdrs˙+e˙n+edsdns˙
6.3 Frenet 公式
由于弧长参数化：
d r d s = t \frac{d\mathbf r}{ds} = \mathbf t dsdr=t
下面求 d n d s \frac{d\mathbf n}{ds} dsdn。
由：
n = [ − sin ψ r cos ψ r ] \mathbf n= \begin{bmatrix} -\sin\psi_r\\ \cos\psi_r \end{bmatrix} n=[−sinψrcosψr]
对 s s s 求导：
d n d s = [ − cos ψ r d ψ r d s − sin ψ r d ψ r d s ] \frac{d\mathbf n}{ds}= \begin{bmatrix} -\cos\psi_r \frac{d\psi_r}{ds}\\ -\sin\psi_r \frac{d\psi_r}{ds} \end{bmatrix} dsdn=[−cosψrdsdψr−sinψrdsdψr]
而：
d ψ r d s = κ r \frac{d\psi_r}{ds} = \kappa_r dsdψr=κr
所以：
d n d s = − κ r [ cos ψ r sin ψ r ] = − κ r t \frac{d\mathbf n}{ds}= -\kappa_r \begin{bmatrix} \cos\psi_r\\ \sin\psi_r \end{bmatrix}= -\kappa_r \mathbf t dsdn=−κr[cosψrsinψr]=−κrt
6.4 代回位置导数
因此：
p ˙ = t s ˙ + e ˙ , n + e ( − κ r t ) s ˙ \dot{\mathbf p}= \mathbf t \dot s+ \dot e,\mathbf n+ e(-\kappa_r\mathbf t)\dot s p˙=ts˙+e˙,n+e(−κrt)s˙
整理：
p ˙ = ( 1 − e κ r ) s ˙ , t + e ˙ , n \dot{\mathbf p}= (1-e\kappa_r)\dot s,\mathbf t+ \dot e,\mathbf n p˙=(1−eκr)s˙,t+e˙,n
这就是 Frenet 基底下的速度表达。
6.5 车辆真实速度表达
另一方面，车辆真实速度沿自身航向方向：
p ˙ = v [ cos ψ sin ψ ] \dot{\mathbf p}=v \begin{bmatrix} \cos\psi\\ \sin\psi \end{bmatrix} p˙=v[cosψsinψ]
由于 θ e = ψ − ψ r \theta_e = \psi - \psi_r θe=ψ−ψr，把该向量在 Frenet 基底 ( t , n ) (\mathbf t,\mathbf n) (t,n) 下分解得：
p ˙ = v cos θ e , t + v sin θ e , n \dot{\mathbf p}= v\cos\theta_e,\mathbf t+ v\sin\theta_e,\mathbf n p˙=vcosθe,t+vsinθe,n
6.6 对比两种表达
我们有：
( 1 − e κ r ) s ˙ t + e ˙ n = v cos θ e t + v sin θ e n (1-e\kappa_r)\dot s \,\mathbf t + \dot e \, \mathbf n= v \cos\theta_e \, \mathbf t + v\sin\theta_e \, \mathbf n (1−eκr)s˙t+e˙n=vcosθet+vsinθen
由于 t , n \mathbf t,\mathbf n t,n 独立正交，对应系数相等。
法向方向：
e ˙ = v sin θ e \dot e = v\sin\theta_e e˙=vsinθe
切向方向：
( 1 − e κ r ) s ˙ = v cos θ e (1-e\kappa_r)\dot s = v\cos\theta_e (1−eκr)s˙=vcosθe
所以：
s ˙ = v cos θ e 1 − e κ r \dot s = \frac{v\cos\theta_e}{1-e\kappa_r} s˙=1−eκrvcosθe
6.7 航向误差动力学
由：
θ e = ψ − ψ r \theta_e = \psi - \psi_r θe=ψ−ψr
得：
θ ˙ e = ψ ˙ − ψ ˙ r \dot\theta_e = \dot\psi - \dot\psi_r θ˙e=ψ˙−ψ˙r
车辆真实航向变化率：
ψ ˙ = v L tan δ \dot\psi = \frac{v}{L}\tan\delta ψ˙=Lvtanδ
参考轨迹航向变化率：
ψ ˙ r = d ψ r d s s ˙ = κ r s ˙ \dot\psi_r = \frac{d\psi_r}{ds}\dot s = \kappa_r \dot s ψ˙r=dsdψrs˙=κrs˙
所以：
θ ˙ e = v L tan δ − κ r s ˙ \dot\theta_e= \frac{v}{L}\tan\delta - \kappa_r\dot s θ˙e=Lvtanδ−κrs˙
代入 s ˙ \dot s s˙：
θ ˙ e = v L tan δ − κ r v cos θ e 1 − e κ r \dot\theta_e= \frac{v}{L}\tan\delta - \kappa_r\frac{v\cos\theta_e}{1-e\kappa_r} θ˙e=Lvtanδ−κr1−eκrvcosθe
6.8 得到完整非线性 Frenet 误差模型
因此：
e ˙ = v sin θ e \dot e = v\sin\theta_e e˙=vsinθe
s ˙ = v cos θ e 1 − e κ r \dot s = \frac{v\cos\theta_e}{1-e\kappa_r} s˙=1−eκrvcosθe
θ ˙ e = v L tan δ − κ r v cos θ e 1 − e κ r \dot\theta_e = \frac{v}{L}\tan\delta- \kappa_r\frac{v\cos\theta_e}{1-e\kappa_r} θ˙e=Lvtanδ−κr1−eκrvcosθe
这就是严格的 Frenet 误差动力学。
7. 小误差近似：工程模型
完整非线性模型准确，但直接用于线性 MPC 太复杂，所以工程中通常做小误差近似。
7.1 假设
假设车辆跟踪还不错：
e ≈ 0 , θ e ≈ 0 e \approx 0,\qquad \theta_e \approx 0 e≈0,θe≈0
于是：
sin θ e ≈ θ e \sin\theta_e \approx \theta_e sinθe≈θe
cos θ e ≈ 1 \cos\theta_e \approx 1 cosθe≈1
1 − e κ r ≈ 1 1-e\kappa_r \approx 1 1−eκr≈1
7.2 简化 e ˙ \dot e e˙
原式：
e ˙ = v sin θ e \dot e = v\sin\theta_e e˙=vsinθe
变成：
e ˙ ≈ v θ e \dot e \approx v\theta_e e˙≈vθe
7.3 简化 s ˙ \dot s s˙
s ˙ = v cos θ e 1 − e κ r ≈ v \dot s = \frac{v\cos\theta_e}{1-e\kappa_r} \approx v s˙=1−eκrvcosθe≈v
7.4 简化 θ ˙ e \dot\theta_e θ˙e
原式：
θ ˙ e = v L tan δ − κ r s ˙ \dot\theta_e = \frac{v}{L}\tan\delta - \kappa_r \dot s θ˙e=Lvtanδ−κrs˙
近似后：
θ ˙ e ≈ v L tan δ − v κ r \dot\theta_e \approx \frac{v}{L}\tan\delta - v\kappa_r θ˙e≈Lvtanδ−vκr
7.5 工程中常用的简化误差模型
因此得到：
e ˙ = v θ e \dot e = v\theta_e e˙=vθe
θ ˙ e = v L tan δ − v κ r \dot\theta_e = \frac{v}{L}\tan\delta - v\kappa_r θ˙e=Lvtanδ−vκr
这就是后面线性化前的"简化非线性模型"。
8. 转向执行器动态： τ \tau τ
8.1 为什么不能直接写 δ = u \delta=u δ=u
如果写：
δ = u \delta = u δ=u
意思就是：
控制器命令一发出，轮子瞬间到位。
这对真实车辆几乎从不成立。真实转向机构有：
- 电机或液压动态
- 机械惯性
- 速率限制
- 控制延迟
所以需要一个更真实的执行器动态模型。
8.2 最简单的一阶跟踪模型
定义：
- u u u：控制器下发的目标转角
- δ \delta δ：实际轮角
希望实际轮角向目标靠拢。最简单的假设是：
实际转角变化速度与误差 u − δ u-\delta u−δ 成正比。
即：
δ ˙ = k ( u − δ ) \dot\delta = k(u-\delta) δ˙=k(u−δ)
令：
k = 1 τ k=\frac{1}{\tau} k=τ1
得到：
δ ˙ = 1 τ ( u − δ ) \dot\delta = \frac{1}{\tau}(u-\delta) δ˙=τ1(u−δ)
展开，得到常用的控制公式：
δ ˙ = − 1 τ δ + 1 τ u \dot\delta = -\frac{1}{\tau}\delta + \frac{1}{\tau}u δ˙=−τ1δ+τ1u
8.3 τ \tau τ 的物理意义
把它写成：
τ δ ˙ + δ = u \tau\dot\delta + \delta = u τδ˙+δ=u
这是标准一阶系统。若 u = u 0 u=u_0 u=u0 为常值，则：
δ ( t ) = u 0 + ( δ ( 0 ) − u 0 ) e − t / τ \delta(t)=u_0+(\delta(0)-u_0)e^{-t/\tau} δ(t)=u0+(δ(0)−u0)e−t/τ
所以：
- τ \tau τ 小，响应快
- τ \tau τ 大，响应慢
它叫时间常数，不是阿克曼几何推出来的，而是执行器动态参数。