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

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝

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

---

文章目录

• 引言
• 一、误差计算概述

• 1、连续轨迹的横向误差
• 2、连续曲线的投影问题
• 3、离散轨迹点的误差

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

• 1、匹配点
• 2、投影点
• 3、横向误差
• 4、纵向误差
• 5、投影点的航向角
• 6、四个式子

• 三、总结

---

引言

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

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

---

一、误差计算概述

  在前面六节讲到了最优的横向控制：
$u=-ke_{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{aligned} 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_{\varphi}&=\varphi -\theta _r\\ \dot{e}_{\varphi}&=\dot{\varphi}-\kappa \dot{s}\\ \end{aligned}$其中，$\dot{\theta}_r=\kappa \dot{s}$

计算误差需要 $\vec{x}_r$ 、$\vec{n}_r$

• $\vec{x}_r=(x_r,y_r)$
• $\theta _r$为投影点的速度 $\dot s$
• $\kappa$
• $\dot{s}$ 为投影点的速度
  $\dot{s}=\frac{|\vec{v}|\cos \left( \theta -\theta _r \right)}{1-\kappa e_d}$

而剩下的这几个自变量：

• $\vec{x}$
• $\vec v$
• $\dot \varphi$

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

$x_r、y_r、\theta_r 、k、s_r$（$s_r$ 为投影点在自然坐标系下的坐标）

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

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

2、连续曲线的投影问题

   举几个例子：

$A$ 的投影定义：如果 $A$ 与 $A$ 的连线与 $A$ 的切线垂直，则称 $A$ 是 $A$

• 直线
  直线的投影是最简单的， $A$ 在直线的投影就是 $A$
• 简单曲线
  简单曲线的投影比较简单，只要 $A$ 和 $A$ 的连线和 $A$ 的切线方向 $\vec \tau$ 垂直，则 $A$
• 圆
  对于封闭的圆，$A$ 正好在圆心处，按照定义，$A$
• 椭圆
  在椭圆半长轴上， $A$

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

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

3、离散轨迹点的误差

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

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

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

   要用离散的轨迹规划做相应的控制，就要进行相应的误差计算，离散轨迹点的每一点都包含四个信息：
$\begin{gathered} (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{gathered}$   这仅仅是做横向控制，如果要做纵向控制的话，还需要包含离散点的在自然坐标系下的 $s_1、s_2、s_3$。不过本节只讲横向控制，只需要这 $4$

---

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

$e_d、\dot e_d、e_\varphi、\dot e_\varphi$。

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) \quad \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 _me_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 _me_s$

6、四个式子

   第六步，得到四个转换关系式子：
$\begin{aligned} 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{aligned}$由
$\begin{aligned} \dot{s}&=\frac{|\vec{v}|\cos \left( \theta -\theta _r \right)}{1-\kappa e_d}\\ e_{\varphi}&=\varphi -\theta _r\\ \dot{e}_{\varphi}&=\dot{\varphi}-\dot{\theta}_r=\varphi -\kappa _r\dot{s}\\ \dot{e}_d&=|\vec{v}|\sin \left( \theta -\theta _r \right)\\ \end{aligned}$   可计算出$e_d、\dot{e}_d、e_{\varphi}、\dot{e}_{\varphi}$。

---

三、总结

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

关于这些量的计算：

• $AB$
• $k$
• $\delta_f$
• 误差 $e_{rr}$

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

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

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

---

后记：

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

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

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

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

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