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

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

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

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

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

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

---

---

引言

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

  本节博客讲解离散轨迹的误差计算，是理论部分中的最后一节，如果看完了前七节，可以自己尝试搭建横向控制模型，因为横向控制所需要的知识和理论，以及存在的障碍，都讲明白了，大家可以自己尝试搭建相关理论模型，编写代码。

---

一、误差计算概述

  在前面六节讲到了最优的横向控制：
$$u=-k{e}_{rr}+{\delta }_f$$其中，$k$ 为 $\mathrm{lqr}(A,B,Q,R)$ 或 $\mathrm{dlqr}(\bar{A},\bar{B},Q,R)$的解。

关于这些量的计算：

• $A、B$ 的计算在第三节和第四节
• $k$ 的计算在第五节
• ${\delta }_f$ 的计算在第六节

还剩误差 $e_{rr}$ 的计算。

1、连续轨迹的横向误差

  在第四节讲过相关误差的计算，先把图画一下：

   在第四节讲到了$e_d$：
$$\begin{array}{rl}{e}_d& =\left(\vec{x}-{\vec{x}}_r\right)\cdot {\vec{n}}_r\\ {\dot{e}}_d& =|\vec{v}|\sin \left(\theta -{\theta }_r\right)\\ e_{\phi }& =\phi -{\theta }_r\\ {\dot{e}}_{\phi }& =\dot{\phi }-\kappa \dot{s}\end{array}$$其中，${\dot{\theta }}_r=\kappa \dot{s}$ 由曲率定义式推导出来。

计算误差需要 ${\vec{x}}_r$ 、${\vec{n}}_r$ 这种投影点的相关信息：

• ${\vec{x}}_r=(x_r,y_r)$ 为投影点的直角坐标
• ${\theta }_r$为投影点的速度 $\dot{s}$ 与 x 轴的夹角
• $\kappa$ 为投影点的曲率
• $\dot{s}$ 为投影点的速度
  $$\dot{s}=\frac{|\vec{v}|\cos \left(\theta -{\theta }_r\right)}{1-\kappa e_d}$$

而剩下的这几个自变量：

• $\vec{x}$ 为车的位置
• $\vec{v}$ 为车的速度
• $\dot{\phi }$ 为车的横摆角速度

都可以通过上游的定位模块以及传感器的信号采集到，视为已知。

   所以只要知道 $x_r、y_r、{\theta }_r、k、s_r$（$s_r$ 为投影点在自然坐标系下的坐标）

   如果知道这些信息，就可以把误差计算出来。

   注意：如果规划曲线为连续曲线，那么可能导致投影点不唯一。

2、连续曲线的投影问题

   举几个例子：

$A$ 的投影定义：如果 $A$ 与 $A^{\prime }$ 的连线与 $A^{\prime }$ 的切线垂直，则称 $A^{\prime }$ 是 $A$ 的投影。

• 直线
  直线的投影是最简单的， $A$ 在直线的投影就是 $A^{\prime }$

• 简单曲线
  简单曲线的投影比较简单，只要 $A$ 和 $A^{\prime }$ 的连线和 $A^{\prime }$ 的切线方向 $\vec{\tau }$ 垂直，则 $A^{\prime }$ 就是它的投影。

• 圆
  对于封闭的圆，$A$ 正好在圆心处，按照定义，$A$ 在圆上的投影有无数个，圆上每点都是它的投影。

• 椭圆
  在椭圆半长轴上， $A$ 的投影有四个。

   如果曲线连续，不仅求投影比较麻烦，而且要处理多值问题，即投影可能不止一个，这是非常麻烦的事情。

   在这里并不是说连续曲线的规划就不能用，各有各种用法，要用连续曲线做规划，在规划层面上就必须要考虑到投影的多值性问题。

3、离散轨迹点的误差

如果轨迹规划是离散的，怎么求投影相对比较容易处理？

   本篇博客讲解离散轨迹点的误差计算，应用比较广泛，且相对较简单。第四节讲过的是连续轨迹曲线的误差计算公式。

   比如，轨迹规划出一系列离散点：

   要用离散的轨迹规划做相应的控制，就要进行相应的误差计算，离散轨迹点的每一点都包含四个信息：
$$\begin{array}{c}(x_1,y_1,{\theta }_1,{\kappa }_1)\\ (x_2,y_2,{\theta }_2,{\kappa }_2)\\ ...\\ (x_n,y_n,{\theta }_n,{\kappa }_n)\end{array}$$   这仅仅是做横向控制，如果要做纵向控制的话，还需要包含离散点的在自然坐标系下的 $s_1、s_2、s_3$。不过本节只讲横向控制，只需要这 $4$ 个信息就够了。

---

二、离散轨迹规划误差计算步骤

   下面计算基于离散轨迹规划的误差 $e_d、{\dot{e}}_d、e_{\phi }、{\dot{e}}_{\phi }$。

1、匹配点

   第一步，找到离散轨迹规划点中与真实位置 $(x,y)$ 最近的点，计算规划点到真实位置之间的距离，比大小找出最短距离，最短距离对应的点就是匹配点。

2、投影点

   第二步，计算投影点。

匹配点和投影点是什么关系呢？

   比如，在直线里有三个规划点：

   图中蓝色点为匹配点，红色点为真正的投影点。投影点是不在规划轨迹点中的理想点。因此，匹配点并不等于投影点。

匹配点能否近似代替投影点？

   用最短距离的匹配点近似代替投影点，理论上可以，但有个前提，规划点要特别密集，这样匹配点和投影点的位置比较接近，越密就越接近，近似程度就越高。但是理论归理论，现实生活中这种方法依然不可行：

• 规划点不可能特别密集
• 规划点取得非常密集会增加计算负担

   在找匹配点时，是计算所有规划点和真实位置 $(x,y)$ 的距离，规划点越密集，计算量就越大。

   在自动驾驶中控制的实时性要求最高，要尽可能不惜代价提升控制代码的运行速度，所以用匹配点近似投影点在实际运用中不可行。

   可以通过匹配点近似计算出投影点。

   假设有一条连续曲线，上面有三个离散规划点：

   规划都是离散点，没有连续曲线，匹配点距离真实点最近，附带相关信息 $(x_m,y_m,{\theta }_m,{\kappa }_m)$，真实点也有信息 $(x,y,\theta ,\kappa )$，通过真实点和匹配点的信息，把投影点信息近似计算出来。

   假设从匹配点到投影点曲线的曲率不变，即近似认为匹配点的曲率等于投影点的曲率。

   黑色曲线是规划轨迹，这是真实的连续轨迹，根据连续轨迹得到离散的规划点。

   要计算的是真实点到投影点的横向距离 $e_d$，已知图中红色向量：
$$\vec{x}-{\vec{x}}_m=(x-x_m,y-y_m)$$   由匹配点的航向角 ${\theta }_m$ 可知切向向量 ${\vec{\tau }}_m$和法向向量${\vec{n}}_m$，即：
$${\vec{\tau }}_m=\left(\cos {\theta }_m,\sin {\theta }_m\right){\vec{n}}_m=\left(-\sin {\theta }_m,\cos {\theta }_m\right)$$

3、横向误差

   第三步，横向误差 $e_d$ 近似等于红色向量 在 ${\vec{n}}_m$ 方向上的投影：
$$e_d\approx \left(\vec{x}-{\vec{x}}_m\right)\cdot {\vec{n}}_m$$
   注意：$e_d$ 有正负，为正意味着真实位置在规划位置的左边，为负意味着在右边。

   若规划轨迹是直线，即曲率 $\kappa =0$ ，这种近似完全没有任何误差，$e_d=\left(\vec{x}-{\vec{x}}_m\right)\cdot {\vec{n}}_m$，这是非常好的性质，有了此性质后：

• 在曲线段，横向误差 $e_d$ 和曲率、弧长有关
• 在直线段，横向误差 $e_d$ 和曲率、弧长都没有关系，因为上式严格相等

   意味着在直线段用非常少的轨迹规划点就可以完整地进行轨迹跟踪。

   所以只需在曲线曲率比较大的位置，规划的离散轨迹密一点；在直线段或者曲率特别小的位置完全可以用几个点一笔带过，节省计算资源。

4、纵向误差

   第四步，匹配点与投影点之间的弧长 $e_s$ 等于红色向量在 ${\vec{\tau }}_m$ 上的投影：
$$e_s\approx \left(\vec{x}-{\vec{x}}_m\right)\cdot {\vec{\tau }}_m$$
   注意：弧长有正负。

   比如，有一条曲线：

   这种情况下，${\vec{\tau }}_m$ 和 $(\vec{x}-{\vec{x}}_m)$ 夹角小于 $90°$ ，点乘为正，意味着投影点在匹配点前面。

   如果是这种情况：

   其中，${\vec{\tau }}_m$和 $(\vec{x}-{\vec{x}}_m)$夹角大于 $90°$ ，点乘为负，意味着投影点在匹配点后面。

5、投影点的航向角

   第五步，也就是最关键的一步， 投影点的航向角 ${\theta }_r$：
$${\theta }_r={\theta }_m+{\kappa }_m{e}_s$$   比如，有一段圆弧：

   圆弧上有两个点，与曲线切线方向的夹角分别是 ${\theta }_1$ 和 ${\theta }_2$，根据几何关系，绿色角是 ${\theta }_1-{\theta }_2$，那么
$${\theta }_1-{\theta }_2=\frac{s}{R}=\kappa s$$    又因为 $e_s$ 近似认为是匹配点与投影点间的弧长，所以
$${\theta }_r={\theta }_m+{\kappa }_m{e}_s$$

6、四个式子

   第六步，得到四个转换关系式子：
$$\begin{array}{rl}{e}_d& =\left(\vec{x}-{\vec{x}}_m\right)\cdot {\vec{n}}_m\\ e_s& =\left(\vec{x}-{\vec{x}}_m\right)\cdot {\tau }_m\\ {\theta }_r& ={\theta }_m+k_m\cdot e_s\\ {\kappa }_r& ={\kappa }_m\end{array}$$由
$$\begin{array}{rl}\dot{s}& =\frac{|\vec{v}|\cos \left(\theta -{\theta }_r\right)}{1-\kappa e_d}\\ e_{\phi }& =\phi -{\theta }_r\\ {\dot{e}}_{\phi }& =\dot{\phi }-{\dot{\theta }}_r=\phi -{\kappa }_r\dot{s}\\ {\dot{e}}_d& =|\vec{v}|\sin \left(\theta -{\theta }_r\right)\end{array}$$   可计算出$e_d、{\dot{e}}_d、e_{\phi }、{\dot{e}}_{\phi }$。

---

三、总结

   最终，横向控制
$$u=-k{e}_{rr}+{\delta }_f$$其中，$k=\mathrm{lqr}(A,B,Q,R)$ 或 $\mathrm{dlqr}(\bar{A},\bar{B},Q,R)$的解。

关于这些量的计算：

• $AB$ 的计算在第三节和第四节
• $k$ 的计算在第五节
• ${\delta }_f$ 的计算在第六节
• 误差 $e_{rr}$ 的计算在本节。

至此，横向控制理论部分结束。

   在下一节会详细演示怎么把程序代码编写出来，将理论变成实际。第八节是横向控制的核心，将用到前七节所有知识。

   本篇博客到此结束，欢迎关注！

---

后记：

🌟 感谢您耐心阅读这篇关于 离散规划轨迹的误差计算 的技术博客。 📚

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

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

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

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