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

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

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

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

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

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

---

---

引言

  本篇博客是 自动驾驶控制算法 系列的第四节，这一节是本系列课程的核心内容。内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

  在上一节得到了这样的微分方程：

  反映了前轮转角对侧向速度和横摆角的影响，可以写成如下形式：
$$\dot{x}=Ax+Bu$$  此方程是做控制的核心，因为控制的目的就是让车按照规划的轨迹形式。

---

一、微分方程与规划轨迹

  比如，在绝对坐标系上规划一条轨迹：

  在这条轨迹上取一些离散点作为参考点，运动控制的目标是通过控制方向盘，让真实车辆的位置、速度以及航向角尽可能接近规划的位置、速度、航向角。

  假设在 $t=5s$ 时，规划点包含了一系列信息 $(X_r,Y_r,v_r,{\theta }_r,a_r)$ ，意味着按照规划点，在第五秒时，车辆位置应该是$(x_r,y_r)$，速度应该是 $v_r$，航向角应该是 ${\theta }_r$，加速度应该是 $a_r$，这些 $(X_r,Y_r,v_r,{\theta }_r,a_r)$ 数据是在规划模块上已经规划好的已知数据。

1、误差定义

  但是如果车辆真实运动状态和规划点之间存在误差：

  图中绿色线为误差，在真实轨迹和规划轨迹之间，有纵向误差、横向误差、航向角误差、速度误差、加速度误差。一般定义误差为真实值减去规划值，即
$$e_{rr}=x-x_r$$其中，规划值 $x_r$已知，真实值 $x$ 满足上面所求的物理规律。

  将真实值 $x$ 用已知的规划值 $x_r$ 以及误差 $e_{rr}$ 代替，代入微分方程，得到误差微分方程：
$$\dot{x}=Ax+Bu\Rightarrow e_{rr}=\bar{A}{e}_{rr}+\bar{B}u$$误差微分方程是我们真正要的东西，它反映了误差 $e_{rr}$ 随控制量 $u$ 变化的物理规律。

2、代价函数定义

  控制目标是选择合适的 $u$，让 $x$ 尽可能接近 $x_r$ ，等价于让误差的绝对值尽可能小。

  定义代价函数为误差 $e_{rr}$ 的平方加控制量 $u$ 的平方最小。
$$\min J=e_{rr}^2+u^2$$  除了保证误差最小，还希望花费最小。误差 $e_{rr}$ 的平方和 $u$ 的平方可以加权，不一定是 $1:1$ 关系，可能是 $a$ 和 $b$ 的关系：
$$J=a{e}_{rr}^2+b{u}^2$$由于误差 $e_{rr}$ 和控制量 $u$ 一般为列向量，所以加权平方可改写成这样的形式：
$$J=e_{rr}^{T}Q{e}_{rr}+u^{T}Ru$$ 其中，$Q$、$R$ 为对角矩阵。

  这样变成 $J=e_{rr}^{T}Q{e}_{rr}+u^{T}Ru$ 在约束条件 $\dot{x}=\bar{A}{e}_{rr}+\bar{B}u$ 下取最小值的数学问题。其实就是控制里的LQR，L 就是 $\dot{x}=\bar{A}{e}_{rr}+\bar{B}u$，即线性约束，$Q$、$R$ 为权重矩阵。

---

二、误差微分方程的推导

  下面介绍怎样实现 $\dot{x}=Ax+Bu\Rightarrow e_{rr}=\bar{A}{e}_{rr}+\bar{B}u$ 的转化过程。

关于误差的线性微分方程非常重要，因为只要把它解出来，后面就是在约束条件下求小值的问题。

  注意：坐标系选择自然坐标系而不是直角坐标系，要求自然坐标系下 $e_{rr}=\bar{A}{e}_{rr}+\bar{B}u$ 的表达式。

1、误差微分方程的建立

  假设在绝对坐标系下，以已有的规划轨迹为坐标轴建立自然坐标系：

  车辆速度为 $v$，在自然坐标系下投影点的切线方向为 ${\tau }_r$，投影点的法线方向为 $n_r$，投影点速度为 $\dot{s}$，车辆到投影点的距离为 $d$，$v$ 和 $\dot{s}$ 与 $x$ 轴夹角分为 $\theta$ 和 ${\theta }_r$。

定义误差如下：

• 横向误差为 $d$
• 航向误差为 $\theta -{\theta }_r$
• 速度误差 $v-\dot{s}$

其中，速度误差属于纵向公式，本篇博客介绍横向控制，所以速度误差暂时先不管。

  注意： $v$ 和 $\dot{s}$ 是速度大小，不是速度。因为速度方向由航向误差决定，航向误差是控制速度的方向，速度误差仅仅是速度大小的误差，而不是速度矢量的误差。

2、横向误差与速度误差的关系

  在车辆真实速度处建立标架，$\tau$ 为速度方向， $n$ 为速度法线方向。

  注意：$\tau$ 和 $n$ 不是车身坐标系的 $x$ 轴和 $y$ 轴方向，因为 $v$ 是质心速度，质心和绝对坐标系 $X$ 轴的夹角为航向角 $\theta$，而车身坐标系是以横摆角 $\phi$ 为方向，即 $x$ 轴沿着横摆角方向，$y$ 轴垂直于横摆角方向，所以$\tau$ 和 $n$ 并不是车身坐标系的 $x$ 轴和 $y$ 轴方向

  过坐标原点 $O$ 做两个向量，一头指向投影点 $x_r$，一头指向真实的车辆位置 $x$。$x$ 、$x_r$ 和 $d$ 构成三角形，就有
$$x_r+d{n}_r=x$$其中，$d$ 是数，不是向量，所以要作为向量的计算法，用线段大小 $d$ 乘以的单位方向向量，即$$d=(x-x_r)\cdot n_r$$这样得到 $d$ 的具体表达式。

  但算出来 d 还不够，因为 $\dot{x}=Ax+Bu$，是以 $v_y、\dot{\phi }$ 为基本的位置量。为了和 $\dot{x}=Ax+Bu$ 联系起来，需要对 $d$ 进行求导运算。

  注意： $n_r$ 是向量不是常矢量，会随曲线投影点的变化而变化，大小是单位数量，但是方向会不断变化，它的方向变化和曲线的曲率有关，所以 $n_r$ 不是常矢量。

  对 $d$ 求导得到：
$$\dot{d}=(\dot{x}-{\dot{x}}_r)\cdot n_r+(x-x_r)\cdot {\dot{n}}_r$$

• 车辆质心的真实位矢 $x$，其导数是 $\dot{x}=|v|\tau$。
• ${\dot{x}}_r$ 是投影点的位矢，其导数为 ${\dot{x}}_r=\dot{s}{\tau }_r$。

简单证明

  有人可能对向量问题不熟悉，下面简单证明。

  比如在直角坐标系下：

  有一点沿轨迹运动，其位矢为 $r$，经过 $dt$ 的时间，运动到下一点，它的位矢变成 $r+dr$，根据向量的三角形法则，蓝色线就是 $dr$，根据复合求导
$$\frac{dr}{dt}=\frac{dr}{ds}\cdot \frac{ds}{dt}$$其中，$ds$ 就是图中红色弧线。

  当$dt\to 0$时， $dr$ 和 $ds$ 大小趋于相等，$dr$的方向趋向于 $\tau$ 的切线方向。所以$$\frac{dr}{dt}=1\cdot \tau \cdot \frac{ds}{dt}$$其中，${\tau }_r$ 是 $r$ 的切线方向，所以 ${\dot{x}}_r=\dot{s}{\tau }_r$。

---

  回到刚才所讲的内容。 $d$ 的导数：
$$\dot{d}=(|v|\tau -\dot{s}{\tau }_r)\cdot n_r+(x-x_r)\cdot \frac{d{n}_r}{dt}$$  同样 $\frac{d{n}_r}{dt}$ 可以用复合求导：
$$\frac{d{n}_r}{dt}=\frac{d{n}_r}{ds}\cdot \frac{ds}{dt}$$其中，$\frac{ds}{dt}=\dot{s}$，但 $\frac{d{n}_r}{ds}$ 等于什么呢？这里要用到数学上的向量微积分中的 Frenet 公式。

  二维曲线的 Frenet 公式：
$$\frac{d\tau }{ds}=\kappa n\frac{dn}{ds}=-\kappa \tau$$其中，$\kappa$ 是曲线的曲率。

  把 Frenet 公式带进去，得到$\frac{d{n}_r}{dt}=\dot{s}(-k{\tau }_r)$，根据向量的加减法，$d=(x-x_r)\cdot n_r$，再带到 $\dot{d}$ 里：
$$\dot{d}=(|v|z-n_r)+d{n}_r\cdot (-k\dot{s}{\tau }_r)$$  又因为 ${\tau }_r$ 和 $n_r$ 垂直，所以后面一项为 $0$，即
$$\dot{d}=|v||\tau ||n_r|\cos <\tau ,n_r>$$  $\tau$ 和 $n_r$ 间的几何关系如下：

  $\tau$和 $n_r$之间的角度等于$\frac{\pi }{2}-(\theta -\theta r)$，所以：
$$\dot{d}=|v|\cos (\frac{\pi }{2}-(\theta -\theta r))=|v|\sin (\theta -\theta r)$$  因为 $\tau$和 $n_r$ 都是单位矢量，所以它们的模都是1。

  这样就建立了 $\dot{d}$ 和 $v$、$\theta$ 、${\theta }_r$ 之间的关系。

3、投影点速度的计算

  根据几何关系 $x_r+d{n}_r=x$ 计算。

  两边求导 $x_r$ 的导数：
$${\dot{x}}_r+\dot{d}{n}_r+d{\dot{n}}_r=\dot{x}$$  把之前求得的结果带进去：
$$\dot{s}{\tau }_r+|v|\sin (\theta -{\theta }_r)n_r+d(-k\dot{s}{\tau }_r)=|v|\tau$$  等式两边同时点成 ${\tau }_r$：
$$\dot{s}+(-kd\dot{s})=|v|\tau \cdot {\tau }_r=|v|\cos (\theta -{\theta }_r)$$
则：
$$\dot{s}=\frac{|v|\cos (\theta -\theta r)}{1-\kappa d}$$  得到两个非常重要的公式：
$$\dot{d}=|v|\sin (\theta -\theta r)$$$$\dot{s}=\frac{|v|\cos (\theta -\theta r)}{1-\kappa d}$$  这两个公式可以说是一切工作的起点，后续讲无人驾驶控制算法，推导的起点就是这两个公式。

4、航向角误差与速度误差的联系

  但得到这两个公式还不够，因为还没有办法和 $v_y$、$\phi$ 联系在一起。

  将航向角 $\theta =\phi +\beta$ 代进去：
$$\begin{array}{rl}\dot{d}& =|v|\sin \left(\beta +\phi -{\theta }_r\right)\\ & =|v|\sin \beta \cos \left(\phi -{\theta }_r\right)+|v|\cos \beta \sin \left(\phi -{\theta }_r\right)\\ & =v_y\cos \left(\phi -{\theta }_r\right)+v_x\sin \left(\phi -{\theta }_r\right)\end{array}$$
  一般认为 $\phi -{\theta }_r$是小量，所以又可进一步化简为：
$$\dot{d}\approx v_y+v_x(\phi -{\theta }_r)$$  同样可以将 $\dot{s}$写成 $v_x$ 和 $v_y$ 的形式，不过到纵向控制才用得到，所以关于 $\dot{s}$ 的话题先不讲。

  求出来纵向误差 $d$，在自然坐标系里，令$e_d=d,e_{\phi }=\phi -{\theta }_r$，注意 $e_{\phi }$ 并不是航向误差。航向误差应该是航向角减去参考点的夹角 $\theta -{\theta }_r$，即 $\phi +\beta -{\theta }_r$。

  当然在不严格的理论中，可以认为航向误差是 $\phi -{\theta }_r$，因为质心侧偏角 $\beta$和横摆角 $\phi$ 相比较小，可以近似认为是航向误差，但要清楚它并不是航向误差，不能直接把它忽略掉，认为 $\beta =0$。

  在本节可近似认为 $e_{\phi }$ 就是航向误差，但仅限本节，所以要有潜意识，$e_{\phi }$ 其实并不是航向误差，具体以后再讲，目前还涉及不到。

  有了 $e_d$ 和 $e_{\phi }$，可将 $\dot{d}$ 的方程改写为：
$$\begin{array}{rl}{\dot{e}}_d& =v_x{e}_{\phi }+v_y\\ v_y& ={\dot{e}}_d-v_x{e}_{\phi }\\ {\dot{v}}_y& ={\ddot{e}}_d-v_x{\dot{e}}_{\phi }\end{array}$$  假设 $v_x$ 是常数，那么 ${\dot{v}}_x$就被忽略掉了。
$$e_{\phi }=\phi -{\theta }_r{\dot{e}}_{\phi }=\dot{\phi }-{\dot{\theta }}_r{\ddot{e}}_{\phi }=\ddot{\phi }-{\ddot{\theta }}_r\approx \ddot{\phi }$$  忽略掉 ${\theta }_r$ 二阶以上的导数。因为道路一般曲率变化比较平缓，所以 ${\theta }_r$ 考虑到一阶导数为止。
  引入 $e_d$ 和 $e_{\phi }$，可得到：
$$\{\begin{array}{l}{v}_y={\dot{e}}_d-v_x{e}_{\phi }\\ {\dot{v}}_y={\ddot{e}}_d-v_x{\dot{e}}_{\phi }\\ \dot{\phi }={\dot{e}}_{\phi }+{\dot{\theta }}_r\\ \ddot{\phi }={\ddot{e}}_{\phi }\end{array}$$  代入二自由度动力学微分方程：

得到：
$$\begin{array}{rl}{\ddot{e}}_d& =\left(\frac{C_{\alpha f}+C_{\alpha r}}{m{v}_x}\right){\dot{e}}_d+\left(-\frac{C_{\alpha f}+C_{\alpha r}}{m}\right)e_{\phi }\\ & +\left(\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}\right){\dot{e}}_{\phi }+\left(\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}-v_x\right){\dot{\theta }}_r\\ & +\left(-\frac{C_{\alpha f}}{m}\right)\delta \end{array}$$
$$\begin{array}{rl}{\ddot{e}}_{\phi }& =\left(\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I{v}_x}\right){\dot{e}}_d+\left(-\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I}\right)e_{\phi }\\ & +\left(\frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I{v}_x}\right){\dot{e}}_{\phi }+\left(\frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I{v}_x}\right){\dot{\theta }}_r\\ & +\left(-\frac{a{C}_{\alpha f}}{I}\right)\delta \end{array}$$

5、误差微分方程的线性化

  上面两个方程太长了，看起来不方便，把它简化一下：
$$\begin{array}{cc}{\ddot{e}}_d=& a_1{\dot{e}}_d+a_2{e}_{\phi }+a_3{\dot{e}}_{\phi }+b_1{\dot{\theta }}_r+c_1\delta \\ {\ddot{e}}_{\phi }=& a_4{\dot{e}}_d+a_5{e}_{\phi }+a_6{\dot{e}}_{\phi }+b_2{\dot{\theta }}_r+c_2\delta \end{array}$$  这有点线性微分方程 $\dot{X}=AX+Bu$ 意思，但很明显现在还不是线性微分方程组，将它改造：
$$\begin{array}{rl}{\ddot{e}}_d& =0\cdot e_d+a_1{\dot{e}}_d+a_2{e}_{\phi }+a_3{\dot{e}}_{\phi }+b_1{\dot{\theta }}_r+c_1\delta \\ {\ddot{e}}_{\phi }& =0\cdot e_d+a_4{\dot{e}}_d+a_5{e}_{\phi }+a_6{\dot{e}}_{\phi }+b_2{\dot{\theta }}_r+c_2\delta \\ {\dot{e}}_d& =0\cdot e_d+{\dot{e}}_d+0\cdot e_{\phi }+0\cdot {\dot{e}}_{\phi }+0\cdot {\dot{\theta }}_r+0\cdot \delta \\ {\dot{e}}_{\phi }& =0\cdot e_d+0\cdot {\dot{e}}_d+0\cdot e_{\phi }+{\dot{e}}_{\phi }+0\cdot {\theta }_r+0\cdot \delta \end{array}$$

---

三、线性误差微分方程组

  将上式变成线性微分方程组，写成矩阵形式：

  得到基于自然坐标系下的线性误差微分方程组：
$${\dot{e}}_{rr}=A{e}_{rr}+Bu+C{\dot{\theta }}_r$$  这样的方程才是我们想要的控制方程，才能将控制问题转化为求代价函数极小值的问题。

  注意：这里的A B和第三节的A B不是同一个符号

  第四节大部分任务已经完成，讲到了关于误差的线性微分方程该怎么推导。

不过本节遗留了三个问题：

1、航向误差问题

  定义 $e_{\phi }=\phi -{\theta }_r$，但实际上航向误差应该是 $\theta -{\theta }_r$，将 $\theta -{\theta }_r$ 近似认为 $\phi -{\theta }_r$ 有没有问题？

2、投影点航向角速度计算问题

  误差的线性微分方程有 ${\dot{\theta }}_r$，该怎么计算呢？

3、线性误差微分方程组的形式问题

  如果是 ${\dot{e}}_{rr}=A{e}_{rr}+Bu$ 的形式，可以用 LQR 计算。但现在形式是 ${\dot{e}}_{rr}=A{e}_{rr}+Bu+C{\dot{\theta }}_r$，后面还有个小尾巴 $C{\dot{\theta }}_r$，和 LQR 问题比较像，但不完全一致。

  其实我们能控制的只有方向盘转角 $u$，而 ${\dot{\theta }}_r$是代表道路的几何信息，是没有办法控制的，能控制的只有方向盘转角，对于 ${\dot{\theta }}_r$ 没有办法控制问题，该怎么办呢？

  在下一节博客会具体解释，欢迎关注！

---

参考资料

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

---

后记：

感谢您耐心阅读这篇关于 坐标变换与横向误差微分方程 的技术博客。

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

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

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

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