写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_CSDN博客,期待与您一同探索 移动机器人 领域的无限可能。
🔍 本文系 清流君 原创之作,荣幸在CSDN首发🐒
若您觉得内容有价值,还请评论告知一声,以便更多人受益。
转载请注明出处,尊重原创,从我做起。
👍 点赞、评论、收藏,三连走一波,让我们一起养成好习惯😜
在这里,您将收获的不只是技术干货,还有思维的火花!
📚 系列专栏:【运动控制】系列,带您深入浅出,领略控制之美。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!
🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~
文章目录
- 引言
- 一、误差计算概述
- 1、连续轨迹的横向误差
- 2、连续曲线的投影问题
- 3、离散轨迹点的误差
- 二、离散轨迹规划误差计算步骤
- 1、匹配点
- 2、投影点
- 3、横向误差
- 4、纵向误差
- 5、投影点的航向角
- 6、四个式子
- 三、总结
引言
本篇博客是 自动驾驶控制算法 系列的第七节。内容整理自 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−xr)⋅nr=∣v∣sin(θ−θr)=φ−θr=φ˙−κs˙其中,
θ
˙
r
=
κ
s
˙
\dot{\theta}_r=\kappa \dot{s}
θ˙r=κs˙ 由曲率定义式推导出来。
计算误差需要 x ⃗ r \vec{x}_r xr 、 n ⃗ r \vec{n}_r nr 这种投影点的相关信息:
- x ⃗ r = ( x r , y r ) \vec{x}_r=(x_r,y_r) xr=(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−xm=(x−xm,y−ym) 由匹配点的航向角
θ
m
\theta_m
θm 可知切向向量
τ
⃗
m
\vec \tau_m
τm和法向向量
n
⃗
m
\vec n_m
nm,即:
τ
⃗
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)nm=(−sinθm,cosθm)
3、横向误差
第三步,横向误差
e
d
e_d
ed 近似等于红色向量 在
n
⃗
m
\vec n_m
nm 方向上的投影:
e
d
≈
(
x
⃗
−
x
⃗
m
)
⋅
n
⃗
m
e_d\approx \left( \vec{x}-\vec{x}_m \right) \cdot \vec{n}_m
ed≈(x−xm)⋅nm
注意:
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−xm)⋅nm,这是非常好的性质,有了此性质后:
- 在曲线段,横向误差 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−xm)⋅τm
注意:弧长有正负。
比如,有一条曲线:
这种情况下, τ ⃗ m \vec{\tau}_m τm 和 ( x ⃗ − x ⃗ m ) (\vec x-\vec x_m) (x−xm) 夹角小于 90 ° 90° 90° ,点乘为正,意味着投影点在匹配点前面。
如果是这种情况:
其中, τ ⃗ m \vec{\tau}_m τm和 ( x ⃗ − x ⃗ m ) (\vec x-\vec x_m) (x−xm)夹角大于 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−xm)⋅nm=(x−xm)⋅τ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 的计算在本节。
至此,横向控制理论部分结束。
在下一节会详细演示怎么把程序代码编写出来,将理论变成实际。第八节是横向控制的核心,将用到前七节所有知识。
本篇博客到此结束,欢迎关注!
后记:
🌟 感谢您耐心阅读这篇关于 离散规划轨迹的误差计算 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀