【自动驾驶】控制算法（十二）横纵向综合控制 | 从理论到实战全面解析

写在前面：

🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落 。📝
个人主页 ：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。

👍 点赞、评论、收藏 ，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货 ，还有思维的火花！

📚 系列专栏：【运动控制】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！

🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

文章目录

引言
一、规划接口
- [1.1 轨迹规划的定义与重要性](#1.1 轨迹规划的定义与重要性)
- [1.2 不同场景的轨迹规划](#1.2 不同场景的轨迹规划)
- [1.3 轨迹规划的数学模型](#1.3 轨迹规划的数学模型)
- [1.4 考虑车辆运动特性的轨迹规划](#1.4 考虑车辆运动特性的轨迹规划)
二、轨迹生成
- [2.1 五次多项式在轨迹规划中的应用](#2.1 五次多项式在轨迹规划中的应用)
- [2.2 解多项式系数](#2.2 解多项式系数)
三、横向控制的规划接口
- [3.1 匹配点的更新](#3.1 匹配点的更新)
- [3.2 规划点与时间的关系](#3.2 规划点与时间的关系)
四、纵向控制的规划接口
- [4.1 横向误差的计算与理解](#4.1 横向误差的计算与理解)
- [4.2 速度误差与期望加速度](#4.2 速度误差与期望加速度)
- [4.3 横向控制的实现与调整](#4.3 横向控制的实现与调整)
- [4.4 纵向控制与规划接口的整合](#4.4 纵向控制与规划接口的整合)
五、代码实现与模型调整
- [5.1 代码下载与准备工作](#5.1 代码下载与准备工作)
- [5.2 Matlab 环境配置](#5.2 Matlab 环境配置)
- [5.3 Carsim 标定过程](#5.3 Carsim 标定过程)
- [5.4 Simlink 模型建立](#5.4 Simlink 模型建立)
- [5.5 纵向控制模型整合与参数优化](#5.5 纵向控制模型整合与参数优化)
- [5.6 横向控制的集成与误差计算模块的修改](#5.6 横向控制的集成与误差计算模块的修改)
六、横向误差大的原因
- [6.1 横向误差的原因分析](#6.1 横向误差的原因分析)
- [6.2 仿真与实车的差别](#6.2 仿真与实车的差别)
七、解决横向误差的办法
八、转向不足及过度转向
- [8.1 转向不足与过度转向的原因](#8.1 转向不足与过度转向的原因)
- [8.2 力矩平衡与转向特性](#8.2 力矩平衡与转向特性)
- [8.3 车辆设计与转向特性的关系](#8.3 车辆设计与转向特性的关系)
- [8.4 使用 PID 控制器处理转向不足](#8.4 使用 PID 控制器处理转向不足)
九、总结
参考资料

引言

各位小伙伴们大家好，欢迎来到自动驾驶控制算法第十二节，内容整理自 B 站知名 up 主忠厚老实的老王的视频，作为博主的学习笔记，分享给大家共同学习。

本节是超级大综合，将用到前十一节所有的知识，根据本篇博客的内容实现横纵向控制。

简单回顾一下本系列所讲的内容：

第一到第二节：开篇以及运动学方程
第三到第八节 ：横向控制以及 LQR
第九到第十一节 ：纵向控制，也就是双 PID 算法
第十二节：横纵向综合控制、规划接口，因为控制的上游是规划，必须要有规划的输入才能做控制

所以不仅要把横纵向控制所有的模型都做搭好，还要写接口，接口就是在规划里面规划一条轨迹，然后输入到控制里，让控制根据规划的轨迹去跑，所以还要写接口。

回顾整个系列，会发现运动学方程几乎就没怎么讲，可能只用到了 tan ⁡ δ = L R \tan \delta =\frac{L}{R} tanδ=RL 公式。但运动学方程实际上是非常有用的，只是本系列教程将淡化了。

运动学方程适用于低速，但是有很好的特点，就是大小转角均可，而 LQR 只适用于小转角，但不限于速度，高速、低速都可以控制。所以如果想做停车、掉头之类的控制，一般用运动学方程，可以适用于大转角，即方向盘转角可以打得很大，但 LQR 不行， LQR 转角必须就是比较小，大家可以翻一翻第四节。

【自动驾驶】控制算法（四）坐标变换与横向误差微分方程

第四节推导二自由度动力学方程时，就假设前轮转角较小，前轮转角较大的话 LQR 也可以，但控制效果没有运动学方程那么好。

在这里就简单提一下怎么用运动学方程做控制。

首先是非线性方程要线性化，做一阶的泰勒展开线性化后，就变成如下形式：
e ˙ = A e ˙ + B u \dot{e}=A\dot{e}+Bu e˙=Ae˙+Bu 这样就和 LQR 的处理方式基本一样了。

一、规划接口

1.1 轨迹规划的定义与重要性

首先要讲的就是规划接口，因为控制模块的功能就是接收规划轨迹，让车按照规划轨迹运动。控制模块功能实际上就暗含两个事情：

接受一条规划轨迹
那首先得有一条规划的轨迹才行，所以那就是要有规划。
让车辆按照规划轨迹运动
就是横纵向控制

所以必须要讲一下怎么规划一条轨迹。

注意：轨迹规划不是路径规划。

在自动驾驶领域里，路径规划和轨迹规划不一样，这两个是分开的。路径规划比如导航，想要去哪，手机搜一下，就给一条路径，此路径是大概的方向性的东西，只会告诉该走哪条路，该往什么方向走，但是不会告诉速度是多少，加速度是多少，因此

路径规划 ：时间无关，只告诉该往什么地方走
轨迹规划 ：包含时间，所以轨迹规划不仅包含位置，还包含速度、加速度以及道路曲率等。

所以轨迹规划和路径规划不一样，要给出 s ( t ) s(t) s(t) 以及 d ( t ) d(t) d(t)。但在这里就不讲 s ( t ) s(t) s(t) 以及 d ( t ) d(t) d(t)了，讲 x ( t ) , y ( t ) x (t), y (t) x(t),y(t)，即不涉及曲线坐标系、自然坐标系，只在直角坐标系下去做规划，这也是由易到难的过程，先做简单的，再做难的。

1.2 不同场景的轨迹规划

至于在自然坐标系下的轨迹规划，在进阶的课程再讲。本篇博客就讲在直角坐标系下的规划。比如这里有一辆车：

在坐标圆点上，想移动到右上角的红色区域，坐标为 ( 100 , 10 ) (100,10) (100,10)，边界条件为：

坐标	x x x	x ˙ \dot x x˙	x ¨ \ddot x x¨	y y y	y ˙ \dot y y˙	y ¨ \ddot y y¨
初始	0 0 0	0 0 0	0 0 0	0 0 0	0 0 0	0 0 0
终点	100 100 100	0 0 0	0 0 0	10 10 10	0 0 0	0 0 0

用时 t = 20 s t=20s t=20s，此场景实际上对应的就是停车。

当然，终点也可以改，比如 x = 100 x =100 x=100, x ˙ = 20 \dot x=20 x˙=20，即车辆到达终点时有速度，其他都不变。此场景实际上对应的是驶入，比如在辅路上，旁边有花坛，想行驶到主路上融入交通流的场景。

或者可以把起点也改了，比如起点 x = 0 x=0 x=0，但速度 x ˙ = 10 \dot x=10 x˙=10，加速度 x ¨ = 0 \ddot x=0 x¨=0，其他不变；终点处 x = 100 x =100 x=100， x ˙ = 20 \dot x=20 x˙=20， x ¨ = 0 \ddot x=0 x¨=0，这其实对应就是换道和超车场景。

给定起点和终点的位置、速度、加速度信息的规划，可以做很多场景：停车、驶入、变道。所以本篇博客就研究给定起点和终点的位置、速度、加速度信息的规划，这样的规划最简单也最常用，而且能覆盖大部分直线行驶的场景，但不能做转弯，也不能做调头，因为转化和调头必须要用到自然坐标系才可以。

关于在自然坐标系下的规划，到后面进阶的课程再讲，本篇博客主要讲简单的规划，主要是给控制服务的，就是给控制提供规划接口。

1.3 轨迹规划的数学模型

规划问题会转化成数学问题，就是设计一条合适的 x ( t ) , y ( t ) x(t),y(t) x(t),y(t)，满足始末的边界条件，也就是给出初始端的位置、速度、加速度，终点的位置、速度、加速度以及耗费的时间 T T T：

起点	x ( 0 ) x(0) x(0)	x ˙ ( 0 ) \dot x(0) x˙(0)	x ¨ ( 0 ) \ddot x(0) x¨(0)	y ( 0 ) y(0) y(0)	y ˙ ( 0 ) \dot y(0) y˙(0)	y ¨ ( 0 ) \ddot y(0) y¨(0)
终点	x ( T ) x(T) x(T)	x ˙ ( T ) \dot x(T) x˙(T)	x ¨ ( T ) \ddot x(T) x¨(T)	y ( T ) y(T) y(T)	y ˙ ( T ) \dot y(T) y˙(T)	y ¨ ( T ) \ddot y(T) y¨(T)

根据上述条件，设计出一条合理的轨迹。

这是经典的规划问题，很常见，而且不仅仅在无人驾驶上用到，在机器人规划上也有类似的算法，给定始末点的位置、速度、加速度，然后算轨迹。

1.4 考虑车辆运动特性的轨迹规划

但无人驾驶车辆在算法应用上不能直接照搬机器人领域的算法，这是因为它们在运动特性上存在显著差异。具体来说，无人驾驶车辆无法像机器人那样独立进行横向运动。在车辆行驶中，横向运动通常是由纵向运动引起的，即车辆不能像螃蟹那样仅依靠横向移动。而机器人则具备这种横向移动的能力，它们既可以横向移动，也可以纵向移动。因此，无人驾驶车辆的运动规划需要考虑额外的限制。

车辆规划轨迹与机器人轨迹的不同之处在于，车辆轨迹必须受到切线曲率、加速度和速度的限制。例如，在确定起点和终点后，不能简单地绘制一条直线作为行驶路径。这是因为车辆的实际行驶路径需要遵循物理学和动力学的基本原则，以及道路和交通规则的限制。因此，无人驾驶车辆的路径规划必须更加复杂和精确，以确保行驶的安全性和效率。

举个例子，比如给定起点和终点，能直接从起点到终点拉一条直线吗？显然不可以。

合适规划轨迹应该长这样才符合汽车运动的规律：

因为汽车不能平白无故的就有横向速度，横向运动必须要由纵向运动诱发。如果在起点建立直角坐标系，就是对曲线切线斜率 d y d x \frac{dy}{dx} dxdy 也有要求。

所以汽车的轨迹规划边界条件要做相应的修改：

起点	x ( 0 ) x(0) x(0)	x ˙ ( 0 ) \dot x(0) x˙(0)	x ¨ ( 0 ) \ddot x(0) x¨(0)	y ( 0 ) y(0) y(0)	y ′ ( 0 ) y'(0) y′(0)	y ′ ′ ( 0 ) y''(0) y′′(0)
终点	x ( T ) x(T) x(T)	x ˙ ( T ) \dot x(T) x˙(T)	x ¨ ( T ) \ddot x(T) x¨(T)	y ( x e n d ) y(x_{end}) y(xend)	y ′ ( x e n d ) y'(x_{end}) y′(xend)	y ′ ′ ( x e n d ) y''(x_{end}) y′′(xend)

变量符号上的点代表对时间求导 d d t \frac{d}{dt} dtd，撇代表对坐标求导 d d x \frac{d}{dx} dxd。如果是自然坐标系，撇就等于 d d s \frac{d}{ds} dsd，这里是直角坐标系，所以是 d d x \frac{d}{dx} dxd，车辆规划和机器人规划还是有很大不同的。

机器人就是 x x x 和 y y y，都是对时间的导数，因为机器人可以做纵向运动，也可以做横向运动，但仅用 y ′ y' y′ 做规划还是不够，因为真正输入到控制模块里能用的轨迹必须要是 ( x ( t ) , y ( t ) ) (x(t),y(t)) (x(t),y(t))，都是和时间相关的，控制就按照目标点去跑。

所以关于车辆轨迹规划，车辆纵向的 x x x 方向可以是关于时间的导数，因为 x x x 直接和时间相关，但车辆横向的 y y y 方向不行，横向 y y y 和纵向 x x x 相关。所以还要写 y ˙ \dot y y˙ 和 y ′ y' y′ 间的转化，即 d d x \frac{d}{dx} dxd 和 d d t \frac{d}{dt} dtd 间的转化。

这两者之间的转换也非常简单，因为 y ( x ) y(x) y(x) 中的 x x x 和 t t t 有关 x = x ( t ) x=x(t) x=x(t)，所以 y ( x ) y(x) y(x) 和 y ( t ) y(t) y(t) 之间的关系就是
y ( t ) = y ( x ( t ) ) y(t)=y(x(t)) y(t)=y(x(t)) 那么 y ′ y' y′ 和 y ˙ \dot y y˙ 之间的关系也很好计算，只要复合求导即可。
y ˙ ( t ) = d y d x ⋅ d x d t = y ′ ⋅ x ˙ \dot{y}\left( t \right) =\frac{dy}{dx}\cdot \frac{dx}{dt}=y'\cdot \dot{x} y˙(t)=dxdy⋅dtdx=y′⋅x˙ y ¨ \ddot y y¨ 和 y ′ ′ y'' y′′ 之间的关系还是比较复杂的，要使用稍微复杂点的复合求导，最终结果是
y ¨ ( t ) = d d t ( d y d t ) = d d t ( d y d x ⋅ d x d t ) = d ( d y d x ) d t d x d t + d y d x ⋅ d 2 x d t 2 = d d x ( d y d x ) ⋅ d x d t ⋅ d x d t + y ′ ⋅ x ¨ = y ′ ′ ⋅ x ˙ 2 + y ′ ⋅ x ¨ \begin{aligned} \ddot{y}\left( t \right) &=\frac{d}{dt}\left( \frac{dy}{dt} \right) =\frac{d}{dt}\left( \frac{dy}{dx}\cdot \frac{dx}{dt} \right) \\ &=\frac{d\left( \frac{dy}{dx} \right)}{dt}\frac{dx}{dt}+\frac{dy}{dx}\cdot \frac{d^2x}{dt^2}\\ &=\frac{d}{dx}\left( \frac{dy}{dx} \right) \cdot \frac{dx}{dt}\cdot \frac{dx}{dt}+y'\cdot \ddot{x}\\ &=y''\cdot \dot{x}^2+y'\cdot \ddot{x}\\ \end{aligned} y¨(t)=dtd(dtdy)=dtd(dxdy⋅dtdx)=dtd(dxdy)dtdx+dxdy⋅dt2d2x=dxd(dxdy)⋅dtdx⋅dtdx+y′⋅x¨=y′′⋅x˙2+y′⋅x¨ 这样问题就非常清晰了，给这么多边界条件：

x ( 0 ) x\left( 0 \right) x(0)	x ˙ ( 0 ) \dot{x}\left( 0 \right) x˙(0)	x ¨ ( 0 ) \ddot{x}\left( 0 \right) x¨(0)	x ( T ) x\left( T \right) x(T)	x ˙ ( T ) \dot{x}\left( T \right) x˙(T)	x ¨ ( T ) \ddot{x}\left( T \right) x¨(T)
y ( 0 ) y\left( 0 \right) y(0)	y ′ ( 0 ) y'\left( 0 \right) y′(0)	y ′ ′ ( 0 ) y''\left( 0 \right) y′′(0)	y ( x e n d ) y\left( x_{end} \right) y(xend)	y ′ ( x e n d ) y'\left( x_{end} \right) y′(xend)	y ′ ′ ( x e n d ) y''\left( x_{end} \right) y′′(xend)

x x x 共有 6 6 6 个， y y y 也有 6 6 6 个，再加上耗费的时间 T T T。求轨迹满足边界条件，生成包含 x ( t ) , y ( x ) x(t),y(x) x(t),y(x) 的轨迹，先算出 y y y 和 x x x 的关系，再通过转化成 y y y 和 t t t 之间的关系，这样就生成了一条规划轨迹。

二、轨迹生成

2.1 五次多项式在轨迹规划中的应用

对于 x ( t ) x(t) x(t)，很容易想到的就是用五次多项式：
x ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 x(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5 x(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5 因为五次多项式有 6 6 6 个系数，正好对应这 6 6 6 个边界条件。

对于 y ( x ) y(x) y(x) 也一样， y y y 也用五次多项式：
y ( x ) = b 0 + b 1 x + b 2 x 2 + b 3 x 3 + b 4 x 4 + b 5 x 5 y(x)=b_0+b_1x+b_2x^2+b_3x^3+b_4x^4+b_5x^5 y(x)=b0+b1x+b2x2+b3x3+b4x4+b5x5 因为 y y y 也对应着 6 6 6 个边界条件，五次多项式正好有 6 6 6 个未知数，正好就可以对应这 6 6 6 个边界条件。

2.2 解多项式系数

通过这 12 12 12 个边界条件（ x x x 方向 6 6 6 个， y y y 方向也是 6 6 6 个）解出 a 0 a_0 a0 到 a 5 a_5 a5 以及 b 0 b_0 b0 到 b 5 b_5 b5 出来。 y y y 还要转化一下，通过以下关系式：
y ( t ) = y ( x ( t ) ) y ˙ ( t ) = y ′ [ x ( t ) ] ⋅ x ˙ ( t ) y ¨ ( t ) = y ′ ′ [ x ( t ) ] ⋅ x ˙ ( t ) 2 + y ′ [ x ( t ) ] ⋅ x ¨ ( t ) \begin{aligned} y\left( t \right) &=y\left( x\left( t \right) \right)\\ \dot{y}\left( t \right) &=y'\left[ x\left( t \right) \right] \cdot \dot{x}\left( t \right)\\ \ddot{y}\left( t \right) &=y''\left[ x\left( t \right) \right] \cdot \dot{x}\left( t \right) ^2+y'\left[ x\left( t \right) \right] \cdot \ddot{x}\left( t \right)\\ \end{aligned} y(t)y˙(t)y¨(t)=y(x(t))=y′[x(t)]⋅x˙(t)=y′′[x(t)]⋅x˙(t)2+y′[x(t)]⋅x¨(t) 解出 y ( t ) , y ˙ ( t ) , y ¨ ( t ) y(t),\dot{y}(t),\ddot{y}(t) y(t),y˙(t),y¨(t)，可以得到一条 x x x 和 t t t 的关系，以及 y y y 和 t t t 的关系，以及他们的导数和 t t t 的关系，这样就是一条完整的轨迹规划。

三、横向控制的规划接口

得到这条轨迹规划后，该怎样才能运用于横纵向控制？

参考以第八节博客：

【自动驾驶】控制算法（八）横向控制Ⅰ | 算法与流程

在第八节中用到了规划中的 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr,yr,θr,κr) 四个量。其中， θ r \theta_r θr 是轨迹切线方向与 x x x 轴的夹角， κ r \kappa_r κr 代表轨迹曲率。

3.1 匹配点的更新

匹配点由时间给出，比如时间是 1 1 1 秒，规划器给出匹配点坐标 ( x r , y r ) (x_r,y_r) (xr,yr)，即匹配点 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr,yr,θr,κr) 和时间相关，每过一段时间规划器就会发出匹配点坐标，控制器就按照匹配点控制。

3.2 规划点与时间的关系

( x r , y r ) (x_r,y_r) (xr,yr) 其实就是规划器 ( x ( t ) , y ( t ) ) (x(t),y(t)) (x(t),y(t))，和时间有直接关系， ( θ r , κ r ) (\theta_r,\kappa_r) (θr,κr) 和时间 t t t 的关系如下：
θ r ( t ) = arctan ⁡ { y ′ [ x ( t ) ] } \theta _r\left( t \right) =\arctan\text{\{}y'\left[ x\left( t \right) \right] \} θr(t)=arctan{y′[x(t)]} κ r ( t ) = y ′ ′ [ x ( t ) ] ( 1 + y ′ [ x ( t ) ] 2 ) 3 2 \kappa _r\left( t \right) =\frac{y''\left[ x\left( t \right) \right]}{\left( 1+y'\left[ x\left( t \right) \right] ^2 \right) ^{\frac{3}{2}}} κr(t)=(1+y′[x(t)]2)23y′′[x(t)] 这样横向控制和规划接口就讲完了，接下来讲纵向控制和规划接口

四、纵向控制的规划接口

4.1 横向误差的计算与理解

在横向公式里面讲过了横向误差 e s e_s es，如果各位如果不记得的话，要翻一下第七节和第八节：

【自动驾驶】控制算法（七）离散规划轨迹的误差计算

【自动驾驶】控制算法（八）横向控制Ⅰ | 算法与流程

e s e_s es 在横向控制里有，所以横向控制可以直接输出 e s e_s es，就是车辆当前位置与匹配点的距离，而且此距离是基于自然坐标系下的距离，不是基于直角坐标系下的距离。

这里可能会有很多人会有疑惑，明明规划都是直角坐标系，怎么突然就来了自然坐标系？

这是因为直角坐标系只用于规划，当生成了规划轨迹 ( x ( t ) , y ( t ) ) (x(t),y(t)) (x(t),y(t)) 后，直角坐标系使命就完成了，控制就是基于生成的曲线作为自然坐标系的坐标轴的，所以 e s e_s es 才是位置误差。

注意：横向控制里面 e s e_s es 还不算是真正的位置误差。

下面看一下 e s e_s es 到底是怎么算出来的，比如这里有一条轨迹：

目标点被视为匹配点，在此基础上，首先从车辆位置到目标点绘制一个向量，接着在目标点的切线方向上再绘制一个向量。将这两个向量进行点乘运算，其结果即为 e s e_s es ，即红色向量在蓝色向量上的投影就是 e s e_s es 。如果各位对此概念不太熟悉，可以多参考第七节的内容，那里有更详细的解释。

【自动驾驶】控制算法（七）离散规划轨迹的误差计算

关于 e s e_s es 的正负问题，可以这样思考：如果场景如上图所示， e s e_s es 是否为负值？由于两个向量之间的夹角为钝角，因此 e s e_s es 应为负值。但误差的定义是目标点与当前位置之间的距离。在所描述的场景中，目标点位于车辆当前位置的前方，因此 e s e_s es 按理来说应该是正值。因此，在实际的控制作用中，为了使 e s e_s es 与位置误差的实际意义相匹配，需要对 e s e_s es 加上负号。

4.2 速度误差与期望加速度

速度误差为 v p − s ˙ v_p-\dot{s} vp−s˙， p p p ( planning ) 是规划速度， s ˙ \dot s s˙ 在横向控制里面有。则规划速度 v p v_p vp 为
v p = x ˙ r ( t ) 2 + y ˙ r ( t ) 2 v_p=\sqrt{\dot{x}_r(t)^2+\dot{y}_r(t)^2} vp=x˙r(t)2+y˙r(t)2 期望加速度 a p a_p ap 为
a p = x ¨ r ( t ) 2 + y ¨ r ( t ) 2 a_p=\sqrt{\ddot{x}_r(t)^2+\ddot{y}_r(t)^2} ap=x¨r(t)2+y¨r(t)2 这里混用了 ( x ( t ) , y ( t ) ) (x(t),y(t)) (x(t),y(t)) 和 ( x r ( t ) , y r ( t ) ) (x_r(t),y_r(t)) (xr(t),yr(t))，他们其实是一回事，在规划模块里规划轨迹是 ( x ( t ) , y ( t ) ) (x(t),y(t)) (x(t),y(t))，到控制模块时轨迹就变成了 ( x r ( t ) , y r ( t ) ) (x_r(t),y_r(t)) (xr(t),yr(t))，其实是一回事儿。这样从规划到控制的理论部分就讲完了。

把整个规划和控制逻辑梳理一下，首先通过规划得到 x ( t ) , y ( x ) x(t),y(x) x(t),y(x)，然后通过这两个可以得到 x ( t ) , y ( t ) , y ( x ) x(t),y(t),y(x) x(t),y(t),y(x)，得到这三个的同时也可以得到各阶导数 x ˙ ( t ) , y ˙ ( t ) , y ′ ( x ) \dot x(t),\dot y(t),y'(x) x˙(t),y˙(t),y′(x)， x ¨ ( t ) , y ¨ ( t ) , y ′ ′ ( x ) \ddot x(t),\ddot y(t),y''(x) x¨(t),y¨(t),y′′(x)。

4.3 横向控制的实现与调整

由此可得横向控制公式：
x r ( t ) = x ( t ) y r ( t ) = y ( t ) θ r ( t ) = arctan ⁡ [ y ′ [ x ( t ) ] ] κ r ( t ) = y ′ ′ [ x ( t ) ] ( 1 + y ′ [ x ( t ) ] 2 ) 3 2 \begin{aligned} x_r\left( t \right) &=x\left( t \right)\\ y_r\left( t \right) &=y\left( t \right)\\ \theta _r\left( t \right) &=\arctan \left[ y'\left[ x\left( t \right) \right] \right]\\ \kappa _r\left( t \right) &=\frac{y''\left[ x\left( t \right) \right]}{\left( 1+y'\left[ x\left( t \right) \right] ^2 \right) ^{\frac{3}{2}}}\\ \end{aligned} xr(t)yr(t)θr(t)κr(t)=x(t)=y(t)=arctan[y′[x(t)]]=(1+y′[x(t)]2)23y′′[x(t)]

4.4 纵向控制与规划接口的整合

以及纵向控制公式：
e s = d ⃗ e r r ⋅ τ ⃗ s ˙ = 1 1 − κ r e d ( v x cos ⁡ e φ − v y sin ⁡ e φ ) v p = x ˙ ( t ) 2 + y ˙ ( t ) 2 e v = v p − s ˙ a p = x ¨ ( t ) 2 + y ¨ ( t ) 2 \begin{aligned} e_s&=\vec{d}e_{rr}\cdot \vec{\tau}\\ \dot{s}&=\frac{1}{1-\kappa re_d}\left( v_x\cos e{\varphi}-v_y\sin e_{\varphi} \right)\\ v_p&=\sqrt{\dot{x}\left( t \right) ^2+\dot{y}\left( t \right) ^2}\\ e_v&=v_p-\dot{s}\\ a_p&=\sqrt{\ddot{x}\left( t \right) ^2+\ddot{y}\left( t \right) ^2}\\ \end{aligned} ess˙vpevap=d err⋅τ =1−κred1(vxcoseφ−vysineφ)=x˙(t)2+y˙(t)2 =vp−s˙=x¨(t)2+y¨(t)2 其中，纵向误差 e s e_s es 输入至纵向控制要加负号， e v e_v ev 为速度误差， a p a_p ap 为期望加速度。

纵向控制中的 e s e_s es 、 s ˙ \dot s s˙ 是通过横向控制计算出来的，所以纵向控制需要输入的只有三个。

五、代码实现与模型调整

5.1 代码下载与准备工作

这样理论部分就讲完了，接下来就是重头戏代码环节。

首先把横向控制和纵向控制的代码以及模型给下载下来，链接如下：

自动驾驶横纵向控制代码和模型

5.2 Matlab 环境配置

把代码和模型全部都复制到 Casim 工作目录上，然后打开 Matlab ， Matlab 共有四个 . m m m 文件，前三个是标定用的，. m m m 文件是第十一节里的，最后是第八节的 LQR 。可以注意到无论是 Q Q Q 还是 C f , C r C_f,C_r Cf,Cr 都变了，不是第八讲 LQR 了，这里要改一下，因为横纵向控制的 LQR 容易出现很大偏差，所以需要更精确的 LQR ，不能就是像第八节那样随便混一下。

侧偏刚度需要精确解，必须要算出每个轮子精确的垂向力，通过垂向力查表，根据曲线大概估算出侧偏刚度。

重新设置新的模型 planning_control，就不要用以前的模型了，在新的模型里，输入就是油门、刹车以及四轮的转角，输出就是 x , y , φ , v x , v y , φ ˙ x,y,\varphi,v_x,v_y,\dot \varphi x,y,φ,vx,vy,φ˙ 以及发动机转速。

5.3 Carsim 标定过程

首先做标定，把那三个标定代码跑起来，得到标定表。先不急着把横向控制模型复制进来，如果拿到代码，应该没有油门刹车标定表，所以要先标定一下，而且标定时要把 Carsim 当时的输出 x , y , φ , v x , v y , φ ˙ x,y,\varphi,v_x,v_y,\dot \varphi x,y,φ,vx,vy,φ˙ 改为标定时的输出，只有 v x , a x v_x,a_x vx,ax 以及发动机转速，并且要先从初始条件 0 0 0 开始标定油门，然后把初速度改成 180 k m / h 180km/h 180km/h，再标定刹车。标完后再把 Carsim 的输出改成 x , y , φ , v x , v y , φ ˙ x,y,\varphi,v_x,v_y,\dot \varphi x,y,φ,vx,vy,φ˙ 以及发动机转速。

5.4 Simlink 模型建立

再建立新的 Simlink 空模型，用 Carsim 把空模型关联上去，最后把仿真的时间给改一下，改成 40 40 40 秒，因为希望仿真时间长一点。然后把纵向控制的模型（就是第十一节的模型）打开，把里面所有的东西都复制到新的空模型里。

5.5 纵向控制模型整合与参数优化

先不急着把横向控制模型复制进来，应该先做标定，先把那三份标定代码给跑起来，可以得到标定表。标定时要把 Carsim 的输入和输出，改为标定时的输入和输出：

输入为油门、刹车
输出为 v x v_x vx、 a x a_x ax 以及发动机转速

并且要先从初始速度为 0 0 0 开始标定油门，然后再改为初速度 180 k m / h 180km/h 180km/h，再标定刹车。标完之后再把 Carsim 的输出改为 x , y , φ , v x , v y , φ ˙ x,y,\varphi,v_x,v_y,\dot \varphi x,y,φ,vx,vy,φ˙ 以及发动机转速。

得到标定表后，把注释取消掉，把纵向双 PID 弄好。

可以把位置 PID 的输入删掉，因为输入 e s e_s es 在横向控制里直接给，所以不需要用加减计算出来。

把纵向控制所需要的输入先断开，用红色线起个名字再打包，这样看起来不那么混乱，因为模型很复杂，所以能打包的尽量就打包。

纵向控制需要车的当前速度作为电机模型的输入，所以把速度写一下，写个函数：

matlab 复制代码

function v = fcn(vx,vy)
    v=sqrt(vx^2+vy^2);

写好后连起来，规划函数如下：

MATLAB Function：planning

matlab 复制代码

function [vp,ap,xr,yr,thetar,kr] = fcn(t)
    dx=100;%30 秒，要向前移动 100 米，然后向左移动 10 米。
    dy=10;
    T=30;
    xstart=[0,0,0];%起点到终点的位置、速度、加速度
    xend=[dx,0,0];
    ystart=[0,0,0];
    yend=[dy,0,0];
    a=zeros(1,6);%无论是 x 和y，都是五次多项式，有 6 个系数
    b=zeros(1,6);
    
    a(1)=xstart(1);
    a(2)=xstart(2);
    a(3)=xstart(3)/2;
    A1=[T^3,T^4,T^5;%解三元一次方程组
        3*T^2,4*T^3,5*T^4;
        6*T,12*T^2,20*T^3];%注意在推导时，系数是从 A0 到A5，但是从代码的编写写的是 A1 到A6
        %因为 Matlab 数组的下标是从一开始的，没有 A0 
    B1=[xend(1)-a(1)-a(2)*T-a(3)*T^2;
        xend(2)-a(2)-2*a(3)*T;
        xend(3)-2*a(3)];
    xs=inv(A1)*B1;
    a(4)=xs(1);
    a(5)=xs(2);
    a(6)=xs(3);
    b(1)=ystart(1);
    b(2)=ystart(2);
    b(3)=ystart(3)/2;
    A2=[dx^3,dx^4,dx^5;%方程组的t变成dx 
        3*dx^2,4*dx^3,5*dx^4;
        6*dx,12*dx^2,20*dx^3];
    B2=[yend(1)-b(1)-b(2)*dx-b(3)*dx^2;
        yend(2)-b(2)-2*b(3)*dx;
        yend(3)-2*b(3)];
    ys=inv(A2)*B2;
    b(4)=ys(1);
    b(5)=ys(2);
    b(6)=ys(3);
    xr=a(1)+a(2)*t+a(3)*t^2+a(4)*t^3+a(5)*t^4+a(6)*t^5;
    yr=b(1)+b(2)*xr+b(3)*xr^2+b(4)*xr^3+b(5)*xr^4+b(6)*xr^5;
    xr_dot=a(2)+2*a(3)*t+3*a(4)*t^2+4*a(5)*t^3+5*a(6)*t^4;
    yr_dx=b(2)+2*b(3)*xr+3*b(4)*xr^2+4*b(5)*xr^3+5*b(6)*xr^4;
    yr_dot=yr_dx*xr_dot;
    thetar=atan(yr_dx);
    xr_dot2=2*a(3)+6*a(4)*t+12*a(5)*t^2+20*a(6)*t^3;
    yr_dx2=2*b(3)+6*b(4)*xr+12*b(5)*xr^2+20*b(6)*xr^3;
    yr_dot2=yr_dx2*xr_dot^2+yr_dx*xr_dot2;
    kr=yr_dx2/((1+yr_dx^2)^1.5);%曲率
    vp=sqrt(xr_dot^2+yr_dot^2);
    if xr_dot2>=0
        ap=sqrt(xr_dot2^2+yr_dot2^2);
    else
        ap=-sqrt(xr_dot2^2+yr_dot2^2);
end

5.6 横向控制的集成与误差计算模块的修改

下面加入横向控制，把横向控制的核心代码拷进来。把单位换算去掉，因为在单位换算在外面已经做过了，所以就不需要再做了。把 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr,yr,θr,κr) 全都删掉，然后加接口进去，让 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr,yr,θr,κr) 从外面输入。

修改误差计算模块的代码，首先把 e s e_s es 和 s ˙ \dot s s˙ 输出出去，把 d m i n d_{min} dmin 去掉，因为 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr,yr,θr,κr) 由外面的规划模块给，不是直接给定轨迹，找最距离最短的匹配点。误差计算模块修改如下：

matlab 复制代码

function [kr,err,es,s_dot] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)
    tor=[cos(thetar);sin(thetar)];
    nor=[-sin(thetar);cos(thetar)];
    d_err=[x-xr;y-yr];
    ed=nor'*d_err;
    es=tor'*d_err;
    %projection_point_thetar=thetar(dmin);%apollo
    projection_point_thetar=thetar+kappar*es;
    ed_dot=vy*cos(phi-projection_point_thetar)+vx*sin(phi-projection_point_thetar);
    %%%%%%%%%
    ephi=sin(phi-projection_point_thetar);
    %%%%%%%%%
    ss_dot=vx*cos(phi-projection_point_thetar)-vy*sin(phi-projection_point_thetar);%两步算出来s_dot
    s_dot=ss_dot/(1-kappar*ed);
    ephi_dot=phi_dot-kappar*s_dot;
    kr=kappar;
    err=[ed;ed_dot;ephi;ephi_dot];
end

这样横向控制就做好了，连起来就可以了。转角算出来之后要做单元换算，把弧度转换为角度，后轮角度给写成 0 0 0 就行了，因为 LQR 是小角度，所以要加 − 1 -1 −1 到 1 1 1 的限制，最后别忘了 e s e_s es 要乘 − 1 -1 −1，因为 e s e_s es 输入到纵向控制里要加负号。在纵向控制里再给加个 10 10 10 到 10 10 10 的限制，因为 e s e_s es 也不希望过大。

一般实车调试的误差要小于 0.1 0.1 0.1 米，而仿真至少要达到厘米级，就是零点零几米

如果控制效果不好，一般就是加速度超了，所以首先要检查的就是加速度。

LQR 对低高速没有限制，但是必须要小转角，第一是小转角，第二就是加速度不能超了。一般加速度 2 2 2 到 3 3 3 ，特别是高速情况下，想再有 4 4 4 到 5 5 5 的加速几乎不可能，因为在高速情况下，电机的加速能力就很差了。

六、横向误差大的原因

6.1 横向误差的原因分析

纵向误差一般不会特别大，如果大的话调 PID 参数就可以了，有问题的一般是横向误差，有以下几点原因：

侧偏刚度估算太准

而且在跑时，前轮和后轮的垂向力不一样，因为加速度会导致轴转移，垂向力不一样就会导致侧偏刚度变化。
LQR 模型的简化

是基于二自由度自行车模型，本身就有一定简化，所以并不能完全的模拟车辆的横向运动，本身就有一定误差，所以导致横向跟踪也会有一定的误差。
汽车转向不足的固有特性

转向不足导致横向控制存在一定误差。一般来说，在仿真误差达到厘米级就认为可以接受了。如果是实车，大概是 0.1 0.1 0.1 米左右也可以接受，因为仿真没有噪声，是理想情况，所以要对仿真的要求更严格一点。

6.2 仿真与实车的差别

但是实车情况不太可能像仿真那样，实际的汽车有以下几个特点：

汽车不是自行车模型
汽车运行时的侧偏刚度会变化
汽车都会有存在转向不足的问题

实车肯定要考虑到转向不足，所以如果自己搭建模型，也有可能会遇到横向误差太大的问题。

七、解决横向误差的办法

横向误差太大，该如何避免？

有以下三种方法：

调节LQR参数
把侧偏刚度估计准一点。
调整LQR的Q矩阵
如果横向误差太大，就需要调 Q Q Q 矩阵，给横向误差 e d e_d ed 更大的惩罚值，如果 e d e_d ed过大，就会给很大的惩罚值，这样就尽可能的让 e d e_d ed收敛到 0 0 0 。 e φ e_\varphi eφ 是不太可能收敛到 0 0 0 的，因为 e φ e_\varphi eφ 稳态误差就是 − β -\beta −β。所以只要把 e d e_d ed 的权重改大，但不能太大，否则导致超调。
处理转向不足
因为转向不足而导致的横向误差太大

八、转向不足及过度转向

下面讲一下转向不足以及过度转向，比如这里有辆车，直接用自行车模型表示：

如果给这样的前轮转角，理论上应该按照红色弧线行驶，但可能实际上并不是按红色轨迹跑，可能出现过度转向或转向不足，这是汽车本身的特性。

8.1 转向不足与过度转向的原因

为什么会发生转向不足或者过度转向呢？

这里简单解释一下，比如这里有一辆车：

要往左转，那么自然就会受到侧向力的作用，假设为 F y 1 F_{y 1} Fy1 和 F y 2 F_ {y2} Fy2。如果 F y 1 F_{y 1} Fy1 和 F y 2 F_ {y2} Fy2 之间不相匹配，就会导致在车的质心处有力矩存在。

8.2 力矩平衡与转向特性

什么叫相匹配？什么叫不相匹配？

简单举个例子，比如这样的车：

前轮和后轮都受到力的作用，到质心的距离分别是 a a a 和 b b b 。

若 F 1 ⋅ a = F 2 ⋅ b F_1\cdot a=F_2\cdot b F1⋅a=F2⋅b，则力矩平衡，导致中心转向。
若 F 1 ⋅ a > F 2 ⋅ b F_1\cdot a>F_2\cdot b F1⋅a>F2⋅b，则质心有正力矩，导致过度转向。
若 F 1 ⋅ a < F 2 ⋅ b F_1\cdot a<F_2\cdot b F1⋅a<F2⋅b，则质心有负力矩，导致转向不足。

8.3 车辆设计与转向特性的关系

一般市面上买到的车基本上都是转向不足的，这是为了安全考虑。但是如果是赛车，一般会调教成中心转向，因为赛车需要更灵敏的转向，所以要调成中心转向。

为什么不调成过度转向呢？

因为车天生就有过度转向的趋势，即在高速情况下， F 1 ⋅ a > F 2 ⋅ b F_1\cdot a>F_2\cdot b F1⋅a>F2⋅b，这是轮胎的特性。

所以一般来说：

如果是转向不足，在高速下可能会变成中心转向
如果是中心转向，在高速下可能会变成过度转向

所以就没有必要调整成过度转向的，即使是赛车，也是中心转向为主。实车调试都有转向不足的问题，就会导致横向误差比较大。

8.4 使用 PID 控制器处理转向不足

那怎么去处理这件事情？

一般在模型里用 PID ，但只用 PID 的积分模块，其他项都是 0 0 0 ，放到横向误差处做积分，因为有转向不足就会有误差，就对误差做积分，然后把误差积分和得到的转角相减，再输入到前轮转角，即将误差先做积分，再补偿到前轮转角中。

很容易理解，因为 e d e_d ed 向左误差为正，那么角度也是向左为正，所以一旦 e d e_d ed 为正，就意味着方向盘往左打多了，所以要减掉多打的角度。

注意弧度到角度的单位换算，应该是先做减法，再做单位换算。增益模块放到后面，对于实车来说，如果加了的话，提升会比较大。

因为转向不足做了只用积分的 PID ，积分参数可以自己调节。

九、总结

这样本系列就基本结束了，全部都讲完了。如果各位一直做到现在，可以发现，如果纯用 Matlab 确实不行，因为太慢了，特别是第 12 12 12 节的模型，实际上跑起来比较费时间，特别是要调的话，每调一次 PID 参数都要跑一次，都要费很多长时间。再加上只是纯控制模型，还没有把规划加进去。规划只是接口，给定起点、终点，然后算法自动规划一条曲线，车就按照规划路径跑，但这只是接口而已，还没有做真正的轨迹规划。

一旦把规划集成进去，运行会更慢，以后的进阶课程可能就不再以纯的 Matlab 为主了，可能只是用 Matlab 做简单的算法学习，真正要写能用的代码的话，肯定是要上 C++ 的，以及用 Linux 的，这也是没办法的事情，因为快。

本系列博客正式结束了，感谢大家的阅读，谢谢大家，下个系列再见。

参考资料

【基础】自动驾驶控制算法第十二讲横纵向综合控制(完结)

后记：

🌟 感谢您耐心阅读这篇关于 自动驾驶控制算法横纵向综合控制 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客 ，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人 的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀