写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落 。📝
个人主页 ：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。👍 点赞、评论、收藏 ，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货 ，还有思维的火花！📚 系列专栏：【运动控制】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~
文章目录
引言
本篇博客是 自动驾驶控制算法 系列的第七节。内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。
本节博客讲解离散轨迹的误差计算，是理论部分中的最后一节，如果看完了前七节，可以自己尝试搭建横向控制模型，因为横向控制所需要的知识和理论，以及存在的障碍，都讲明白了，大家可以自己尝试搭建相关理论模型，编写代码。
一、误差计算概述
在前面六节讲到了最优的横向控制：
u = − k e r r + δ f u=-ke_{rr}+\delta _f u=−kerr+δf其中， k {k} k 为 l q r ( A , B , Q , R ) \mathrm{lqr}(A,B,Q,R) lqr(A,B,Q,R) 或 d l q r ( A ˉ , B ˉ , Q , R ) {{\mathrm{dlqr}(\bar{A},\bar{B},Q,R)}} dlqr(Aˉ,Bˉ,Q,R)的解。
关于这些量的计算：
- A 、 B A、B A、B 的计算在第三节和第四节
- k k k 的计算在第五节
- δ f \delta_f δf 的计算在第六节
还剩误差 e r r e_{rr} err 的计算。
1、连续轨迹的横向误差
在第四节讲过相关误差的计算，先把图画一下：
在第四节讲到了 e d e_d ed：
e d = ( x ⃗ − x ⃗ r ) ⋅ n ⃗ r e ˙ d = ∣ v ⃗ ∣ sin ( θ − θ r ) e φ = φ − θ r e ˙ φ = φ ˙ − κ s ˙ \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} ede˙deφe˙φ=(x −x r)⋅n r=∣v ∣sin(θ−θr)=φ−θr=φ˙−κs˙其中， θ ˙ r = κ s ˙ \dot{\theta}_r=\kappa \dot{s} θ˙r=κs˙ 由曲率定义式推导出来。
计算误差需要 x ⃗ r \vec{x}_r x r 、 n ⃗ r \vec{n}_r n r 这种投影点的相关信息：
- x ⃗ r = ( x r , y r ) \vec{x}_r=(x_r,y_r) x r=(xr,yr) 为投影点的直角坐标
- θ r \theta _r θr为投影点的速度 s ˙ \dot s s˙ 与 x 轴的夹角
- κ \kappa κ 为投影点的曲率
- s ˙ \dot{s} s˙ 为投影点的速度
s ˙ = ∣ v ⃗ ∣ cos ( θ − θ r ) 1 − κ e d \dot{s}=\frac{|\vec{v}|\cos \left( \theta -\theta _r \right)}{1-\kappa e_d} s˙=1−κed∣v ∣cos(θ−θr)
而剩下的这几个自变量：
- x ⃗ \vec{x} x 为车的位置
- v ⃗ \vec v v 为车的速度
- φ ˙ \dot \varphi φ˙ 为车的横摆角速度
都可以通过上游的定位模块以及传感器的信号采集到，视为已知。
所以只要知道 x r 、 y r 、 θ r 、 k 、 s r x_r、y_r、\theta_r 、k、s_r xr、yr、θr、k、sr（ s r s_r sr 为投影点在自然坐标系下的坐标）
如果知道这些信息，就可以把误差计算出来。
注意：如果规划曲线为连续曲线，那么可能导致投影点不唯一。
2、连续曲线的投影问题
举几个例子：
A A A 的投影定义：如果 A A A 与 A ′ A' A′ 的连线与 A ′ A' A′ 的切线垂直，则称 A ′ A' A′ 是 A A A 的投影。
-
直线
直线的投影是最简单的， A A A 在直线的投影就是 A ′ A' A′
-
简单曲线
简单曲线的投影比较简单，只要 A A A 和 A ′ A' A′ 的连线和 A ′ A' A′ 的切线方向 τ ⃗ \vec \tau τ 垂直，则 A ′ A' A′ 就是它的投影。
-
圆
对于封闭的圆， A A A 正好在圆心处，按照定义， A A A 在圆上的投影有无数个，圆上每点都是它的投影。
-
椭圆
在椭圆半长轴上， A A A 的投影有四个。
如果曲线连续，不仅求投影比较麻烦，而且要处理多值问题，即投影可能不止一个，这是非常麻烦的事情。
在这里并不是说连续曲线的规划就不能用，各有各种用法，要用连续曲线做规划，在规划层面上就必须要考虑到投影的多值性问题。
3、离散轨迹点的误差
如果轨迹规划是离散的，怎么求投影相对比较容易处理？
本篇博客讲解离散轨迹点的误差计算，应用比较广泛，且相对较简单。第四节讲过的是连续轨迹曲线的误差计算公式。
比如，轨迹规划出一系列离散点：
要用离散的轨迹规划做相应的控制，就要进行相应的误差计算，离散轨迹点的每一点都包含四个信息：
( x 1 , y 1 , θ 1 , κ 1 ) ( x 2 , y 2 , θ 2 , κ 2 ) . . . ( x n , y n , θ n , κ n ) \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} (x1,y1,θ1,κ1)(x2,y2,θ2,κ2)...(xn,yn,θn,κn) 这仅仅是做横向控制，如果要做纵向控制的话，还需要包含离散点的在自然坐标系下的 s 1 、 s 2 、 s 3 s_1、s_2、s_3 s1、s2、s3。不过本节只讲横向控制，只需要这 4 4 4 个信息就够了。
二、离散轨迹规划误差计算步骤
下面计算基于离散轨迹规划的误差 e d 、 e ˙ d 、 e φ 、 e ˙ φ e_d、\dot e_d、e_\varphi、\dot e_\varphi ed、e˙d、eφ、e˙φ。
1、匹配点
第一步，找到离散轨迹规划点中与真实位置 ( x , y ) (x,y) (x,y) 最近的点，计算规划点到真实位置之间的距离，比大小找出最短距离，最短距离对应的点就是匹配点。
2、投影点
第二步，计算投影点。
匹配点和投影点是什么关系呢？
比如，在直线里有三个规划点：
图中蓝色点为匹配点，红色点为真正的投影点。投影点是不在规划轨迹点中的理想点。因此，匹配点并不等于投影点。
匹配点能否近似代替投影点？
用最短距离的匹配点近似代替投影点，理论上可以，但有个前提，规划点要特别密集，这样匹配点和投影点的位置比较接近，越密就越接近，近似程度就越高。但是理论归理论，现实生活中这种方法依然不可行：
- 规划点不可能特别密集
- 规划点取得非常密集会增加计算负担
在找匹配点时，是计算所有规划点和真实位置 ( x , y ) (x,y) (x,y) 的距离，规划点越密集，计算量就越大。
在自动驾驶中控制的实时性要求最高，要尽可能不惜代价提升控制代码的运行速度，所以用匹配点近似投影点在实际运用中不可行。
可以通过匹配点近似计算出投影点。
假设有一条连续曲线，上面有三个离散规划点：
规划都是离散点，没有连续曲线，匹配点距离真实点最近，附带相关信息 ( x m , y m , θ m , κ m ) (x_{m},y_{m},\theta_{m},\kappa_{m}) (xm,ym,θm,κm)，真实点也有信息 ( x , y , θ , κ ) (x,y,\theta,\kappa) (x,y,θ,κ)，通过真实点和匹配点的信息，把投影点信息近似计算出来。
假设从匹配点到投影点曲线的曲率不变，即近似认为匹配点的曲率等于投影点的曲率。
黑色曲线是规划轨迹，这是真实的连续轨迹，根据连续轨迹得到离散的规划点。
要计算的是真实点到投影点的横向距离 e d e_d ed，已知图中红色向量：
x ⃗ − x ⃗ m = ( x − x m , y − y m ) \vec{x}-\vec{x}_m=(x-x_m,y-y_m) x −x m=(x−xm,y−ym) 由匹配点的航向角 θ m \theta_m θm 可知切向向量 τ ⃗ m \vec \tau_m τ m和法向向量 n ⃗ m \vec n_m n m，即：
τ ⃗ m = ( cos θ m , sin θ m ) n ⃗ m = ( − sin θ m , cos θ m ) \vec{\tau}_m=\left( \cos \theta _m,\sin \theta _m \right) \quad \vec{n}_m=\left( -\sin \theta _m,\cos \theta _m \right) τ m=(cosθm,sinθm)n m=(−sinθm,cosθm)
3、横向误差
第三步，横向误差 e d e_d ed 近似等于红色向量 在 n ⃗ m \vec n_m n m 方向上的投影：
e d ≈ ( x ⃗ − x ⃗ m ) ⋅ n ⃗ m e_d\approx \left( \vec{x}-\vec{x}_m \right) \cdot \vec{n}_m ed≈(x −x m)⋅n m
注意 ： e d e_d ed 有正负，为正意味着真实位置在规划位置的左边，为负意味着在右边。
若规划轨迹是直线，即曲率 κ = 0 \kappa=0 κ=0 ，这种近似完全没有任何误差， e d = ( x ⃗ − x ⃗ m ) ⋅ n ⃗ m e_d= \left( \vec{x}-\vec{x}_m \right) \cdot \vec{n}_m ed=(x −x m)⋅n m，这是非常好的性质，有了此性质后：
- 在曲线段，横向误差 e d e_d ed 和曲率、弧长有关
- 在直线段，横向误差 e d e_d ed 和曲率、弧长都没有关系，因为上式严格相等
意味着在直线段用非常少的轨迹规划点就可以完整地进行轨迹跟踪。
所以只需在曲线曲率比较大的位置，规划的离散轨迹密一点；在直线段或者曲率特别小的位置完全可以用几个点一笔带过，节省计算资源。
4、纵向误差
第四步，匹配点与投影点之间的弧长 e s e_s es 等于红色向量在 τ ⃗ m \vec{\tau}_m τ m 上的投影：
e s ≈ ( x ⃗ − x ⃗ m ) ⋅ τ ⃗ m e_s\approx \left( \vec{x}-\vec{x}_m \right) \cdot \vec{\tau}_m es≈(x −x m)⋅τ m
注意：弧长有正负。
比如，有一条曲线：
这种情况下， τ ⃗ m \vec{\tau}_m τ m 和 ( x ⃗ − x ⃗ m ) (\vec x-\vec x_m) (x −x m) 夹角小于 90 ° 90° 90° ，点乘为正，意味着投影点在匹配点前面。
如果是这种情况：
其中， τ ⃗ m \vec{\tau}_m τ m和 ( x ⃗ − x ⃗ m ) (\vec x-\vec x_m) (x −x m)夹角大于 90 ° 90° 90° ，点乘为负，意味着投影点在匹配点后面。
5、投影点的航向角
第五步，也就是最关键的一步， 投影点的航向角 θ r \theta _r θr：
θ r = θ m + κ m e s \theta _r=\theta _m+\kappa _me_s θr=θm+κmes 比如，有一段圆弧：
圆弧上有两个点，与曲线切线方向的夹角分别是 θ 1 \theta_1 θ1 和 θ 2 \theta_2 θ2，根据几何关系，绿色角是 θ 1 − θ 2 \theta_1-\theta_2 θ1−θ2，那么
θ 1 − θ 2 = s R = κ s \theta_{1}-\theta_{2}=\frac{s}{R}=\kappa s θ1−θ2=Rs=κs 又因为 e s e_s es 近似认为是匹配点与投影点间的弧长，所以
θ r = θ m + κ m e s \theta _r=\theta _m+\kappa _me_s θr=θm+κmes
6、四个式子
第六步，得到四个转换关系式子：
e d = ( x ⃗ − x ⃗ m ) ⋅ n ⃗ m e s = ( x ⃗ − x ⃗ m ) ⋅ τ m θ r = θ m + k m ⋅ e s κ r = κ m \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} edesθrκr=(x −x m)⋅n m=(x −x m)⋅τm=θm+km⋅es=κm由
s ˙ = ∣ v ⃗ ∣ cos ( θ − θ r ) 1 − κ e d e φ = φ − θ r e ˙ φ = φ ˙ − θ ˙ r = φ − κ r s ˙ e ˙ d = ∣ v ⃗ ∣ sin ( θ − θ r ) \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} s˙eφe˙φe˙d=1−κed∣v ∣cos(θ−θr)=φ−θr=φ˙−θ˙r=φ−κrs˙=∣v ∣sin(θ−θr) 可计算出 e d 、 e ˙ d 、 e φ 、 e ˙ φ e_d、\dot{e}d、e{\varphi}、\dot{e}{\varphi} ed、e˙d、eφ、e˙φ。
三、总结
最终，横向控制
u = − k e r r + δ f u=-ke_{rr}+\delta _f u=−kerr+δf其中， k = l q r ( A , B , Q , R ) {{k}=\mathrm{lqr}(A,B,Q,R)} k=lqr(A,B,Q,R) 或 d l q r ( A ˉ , B ˉ , Q , R ) {{\mathrm{dlqr}(\bar{A},\bar{B},Q,R)}} dlqr(Aˉ,Bˉ,Q,R)的解。
关于这些量的计算：
- A B AB AB 的计算在第三节和第四节
- k k k 的计算在第五节
- δ f \delta_f δf 的计算在第六节
- 误差 e r r e_{rr} err 的计算在本节。
至此，横向控制理论部分结束。
在下一节会详细演示怎么把程序代码编写出来，将理论变成实际。第八节是横向控制的核心，将用到前七节所有知识。
本篇博客到此结束，欢迎关注！
后记：
🌟 感谢您耐心阅读这篇关于 离散规划轨迹的误差计算 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客 ，以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享，共同探索移动机器人 的无限可能！💡
🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀