写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_CSDN博客,期待与您一同探索 移动机器人 领域的无限可能。
🔍 本文系 清流君 原创之作,荣幸在CSDN首发🐒
若您觉得内容有价值,还请评论告知一声,以便更多人受益。
转载请注明出处,尊重原创,从我做起。
👍 点赞、评论、收藏,三连走一波,让我们一起养成好习惯😜
在这里,您将收获的不只是技术干货,还有思维的火花!
📚 系列专栏:【运动控制】系列,带您深入浅出,领略控制之美。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!
🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~
文章目录
前言
本篇博客是 自动驾驶控制算法 系列的第八节第Ⅰ部分。内容整理自 B站知名up主 忠厚老实的老王 的视频,作为博主的学习笔记,分享给大家共同学习。
本节内容是整个横向控制的核心,要把前七节所有内容都用在第八节上。第八节分四小节,本小节讲解具体算法,下一小节讲解模型搭建和代码编程,再下一小节讲解代码优化、注意事项以及仿真结果。
算法这一讲可以说是整个第八节的核心所在。写代码、做仿真的来源是算法,有了具体算法后,无论用什么平台仿真,用什么语言编写程序,都是水到渠成的事情。只要照着算法编程逻辑是十分清晰的,因为算法已经把逻辑梳理好了,只要照着算法编就可以了。
相反如果没有写过算法,然后就直接就去编程也可以,但只针对较简单的项目、简单的算法。对于复杂算法,建议各位先把算法写好,把逻辑理清楚,然后再编程。
本篇博客讲解横向控制算法,前面讲到的控制律:
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 A、B A、B 的计算在第三节和第四节
- k k k 的计算在第五节
- e r r e_{rr} err 的计算在第四节和第七节
- δ f \delta_f δf 的计算在第六节
第四节讲的是连续规划轨迹的误差计算,第七节是讲的是离散规划轨迹的误差计算。
将前七节所有的知识把综合起来,就变成了第八节,讲解横向控制的完整算法。横向控制很复杂,模块非常多,所以必须要把算法梳理一遍,不然很容易出错。
一、算法输入与输出
1、算法输入
根据前七讲,算法输入分为三类:
-
整车参数
轴距L、质心到前后轮的距离 a 、 b a、b a、b 、侧偏刚度 C α C_{\alpha} Cα、质量 m m m、转动惯量 I I I。 -
车辆位置与状态
包括车辆的 ( x , y , φ ) (x,y,\varphi) (x,y,φ) 以及它的速度 v x 、 v y v_x、v_y vx、vy ,横摆角速度 φ ˙ \dot \varphi φ˙。 -
规划轨迹点
轨迹点为四维数组,包含坐标 x r , y r x_r,y_r xr,yr 以及轨迹点的切线方向与 x 轴夹角 θ r \theta_r θr 以及轨迹点的曲率 κ r \kappa_r κr。
( x r y r θ r κ r ) = ( x 1 x 2 ⋯ y 1 y 2 ⋯ θ 1 θ 2 ⋯ κ 1 κ 2 ⋯ ) \left. \left( \begin{array}{c} x_r\\ y_r\\ \theta _r\\ \kappa _r\\ \end{array} \right. \right) =\left( \begin{matrix} x_1& x_2& \cdots\\ y_1& y_2& \cdots\\ \theta _1& \theta _2& \cdots\\ \kappa _1& \kappa _2& \cdots\\ \end{matrix} \right) xryrθrκr = x1y1θ1κ1x2y2θ2κ2⋯⋯⋯⋯
2、算法输出
算法输出为
u
=
−
k
e
r
r
+
δ
f
u=-ke_{rr}+\delta _f
u=−kerr+δf
二、算法流程
1、算法过程概述
第一步:通过整车参数加上 v x v_x vx,可以算出 A 、 B A、B A、B,再设置权重矩阵 Q 、 R Q、R Q、R,通过 LQR 可得到反馈矩阵 k k k。
第二步:车辆的位置状态,加上规划轨迹点,可以算出误差 e r r e_{rr} err 以及投影曲率 κ \kappa κ ,即真实位置在轨迹点的投影曲率。
第三步:第一步得到反馈矩阵 k k k,以及第二步所得到的投影的曲率 κ \kappa κ,加上整车参数,再加 v x v_x vx,可得到前馈控制 δ f \delta_f δf。
第四步:第一步得到了 k k k 加上第二步得到的误差 e r r e_{rr} err,再加上第三步得到 δ f \delta_f δf,可以得到 u = − k e r r + δ f u=-ke_{rr}+\delta _f u=−kerr+δf
2、算法流程图
流程图可以直观地反映算法的结构,基本上流程画出来,算法逻辑顺序就理清楚了,流程图如下:
-
A 、 B A、B A、B 计算模块
首先是整车参数和 v x v_x vx 作为输入模块,输入进 A 、 B A、B A、B 计算模块,可算出 A 、 B A、B A、B -
LQR模块
把 A 、 B A、B A、B 算出来后,再输入 Q Q Q 和 R R R 进入 LQR 模块,就可算出最终的反馈矩阵 k k k。 -
误差曲率计算模块
接下来是车辆状态及速度,加上规划轨迹点以及 v x v_x vx 作为输入,这里把 v x v_x vx 单独拿出来,因为 v x v_x vx 在其他模块上也有用。作为输入之后输入进误差和 k k k 的计算模块,得到误差以及曲率 κ \kappa κ。 -
前馈控制计算模块
整车参数再加上 v x v_x vx 再加上 k k k 及 κ \kappa κ 输入前馈控制计算模块,可算出前馈控制 δ f \delta_f δf。 -
最终控制计算模块
最后将反馈矩阵 k k k 、前馈 δ f \delta_f δf 以及误差代入最终控制计算模块,可得到最后的控制量 u u u,
其中, A 、 B A、B A、B 计算模块、误差 e r r e_{rr} err、反馈矩阵 k k k 计算模块、前馈控制 δ f \delta_f δf 计算模块都用到 v x v_x vx ,将 v x v_x vx 连在一起,这样得到完整算法的流程图。
流程图包含了从第三节到第七节的所有内容,这就是后续编程的理论依据,以后写代码就是按照流程图来写。
可以看一下整个算法分为五个模块,写算法具体就是写这五个模块,模块写出来了,算法也就编写出来了。
三、具体算法
下面看一下这五个模块的算法到底该如何编写。
1、AB 计算模块
本模块最简单,代公式即可,因为
A
、
B
A、B
A、B 的具体表达式的在前面推导过:
A
=
(
0
1
0
0
0
C
α
f
+
C
α
r
m
v
x
−
C
α
f
+
C
α
r
m
a
C
α
f
−
b
C
α
r
m
v
x
0
0
0
1
0
a
C
α
f
−
b
C
α
r
I
v
x
−
a
C
α
f
−
b
C
α
r
I
a
2
C
α
f
+
b
2
C
α
r
I
v
x
)
A=\left( \begin{matrix} 0& 1& 0& 0\\ 0& \frac{C_{\alpha f}+C_{\alpha r}}{mv_x}& -\frac{C_{\alpha f}+C_{\alpha r}}{m}& \frac{aC_{\alpha f}-bC_{\alpha r}}{mv_x}\\ 0& 0& 0& 1\\ 0& \frac{aC_{\alpha f}-bC_{\alpha r}}{Iv_x}& -\frac{aC_{\alpha f}-bC_{\alpha r}}{I}& \frac{a^2C_{\alpha f}+b^2C_{\alpha r}}{Iv_x}\\ \end{matrix} \right)
A=
00001mvxCαf+Cαr0IvxaCαf−bCαr0−mCαf+Cαr0−IaCαf−bCαr0mvxaCαf−bCαr1Ivxa2Cαf+b2Cαr
B
=
(
0
−
C
α
f
m
0
−
a
C
α
f
I
)
B=\left( \begin{array}{c} 0\\ -\frac{C_{\alpha f}}{m}\\ 0\\ -\frac{aC_{\alpha f}}{I}\\ \end{array} \right)
B=
0−mCαf0−IaCαf
上式和车辆参数、
v
x
v_x
vx 有关的矩阵,把车辆参数及
v
x
v_x
vx 代入计算即可。
注意:要考虑当 v x = 0 v_x=0 vx=0 时的奇异性, A A A 中有无穷大出现,所以在编程时要考虑奇异性。
2、LQR模块
LQR 模块是最费时间的模块,需要解黎卡提方程,需要矩阵迭代。但是有小技巧可以加快 LQR 模块的计算。因为 LQR 只与 A 、 B 、 Q 、 R A、B、Q、R A、B、Q、R 有关,而 A 、 B A、B A、B 只与整车参数和 v x v_x vx 有关,整车参数可近似认为不变。
注意:整车参数只是近似认为不变,并不是一成不变。在某些情况下就是整车参数不能认为近似不变,必须要考虑整车参数变化所带来对 k k k 的影响。
在什么情况下不可近似处理呢?
(1)急加速
如果车辆在疯狂加速,即加速度非常大、非常猛时:
根据达朗贝尔原理,车辆有很大的惯性力,导致前后轮的垂向力发生变化。虽然前后轮的垂向力加起来等于车重,但分配不一样,不是轮子分别近似分配 1 / 4 1/ 4 1/4 ,可能前轮垂向力减小的非常多,而后轮垂向力会增大的非常多,垂向力的变化直接导致轮胎侧偏刚度发生变化,所以在加速特别猛时,不能把侧偏刚度看成常数,要考虑侧偏刚度的变化。
(2)急速过弯
非常急速的过弯时,整个车都倾斜起来:
此时车左右轮明显不对称,因为右轮几乎承受了车的全部重量,而左轮几乎没有承受任何力,导致自行车模型不再适用。
所有的理论都基于自行车模型,即把左右两个轮认为是一个轮,等于是把车压扁了,意味着自行车模型只适用于左右对称或是近似对称的情况。而在极速过弯情况下,不能认为左右轮对称。
在前急加速模型仍然可用自行车模型,因为左右两个轮对称,仅需考虑侧偏刚度变化带来对 k k k 的影响,而后面急速过弯就完全不可以用自行车模型,所以在路径规划时,要极力避免急速过弯的情况出现。因为急加速还可以调整,但如果是急速过弯的话,就完全没办法调整。
在汽车界一般认为侧向加速度就是超过 0.4 0.4 0.4 倍的重力加速度,即 a y > 0.4 g a_y>0.4g ay>0.4g,就属于急速过弯。
回到模块上来,如果认为整车参数近似不变,那么 A 、 B A、B A、B 只与 v x v_x vx 有关。
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),这样每个 v x v_x vx 都有唯一的 k k k 与之对应。
因此,可离线算出 v x v_x vx 与 k k k 的对应表,实际应用中不需要求解黎卡提方程,直接查表即可。
比如有如下形式的对应表:
v x v_x vx | k k k |
---|---|
0.01 | k 1 k_1 k1 |
0.02 | k 2 k_2 k2 |
0.03 | k 3 k_3 k3 |
… | … |
50 | k n k_n kn |
反映了 v x v_x vx 与 k k k 的对应关系。这样测出 v x v_x vx 后,直接查表查出 k k k,这就是用离线查表法计算 LQR。
查表法计算 LQR 的优缺点如下:
- 优点
大大加快计算速度 - 缺点
耗费存储空间
这是典型为了加快程序运算速度而采用空间换时间的算法,通过查表法算, LQR 可以把速度提升好几个数量级,特别是在控制中,对实时性的要求是最高的,这就意味着必须要尽一切可能加快程序运算速度。
一般在实际应用中的 LQR 不是通过调包计算的,而是查表查出来的。而且查表法算LQR 对高速运动的控制也有效果,因为高速和低速的区别就在于侧偏刚度变化,其他变化不大。
在这种情况下, k k k 就和 v x v_x vx 以及侧偏刚度 C α C_\alpha Cα 有关,就是从一维表格变成三维表格。可以把速度以及侧偏刚度输进去,查 k k k 出来,一样可以达到控制效果,而且高速对控制性的实时性要求更高,因为低速控制实时性不高的话,危险性不大。但高速情况下的控制,若实时性不够会很容易发生事故。
3、误差与曲率计算模块
该模块是整个算法的核心。
(1)规划点
首先要有规划轨迹,就是一系列的四维数组点:
(
x
r
y
r
θ
r
κ
r
)
=
(
x
1
x
2
⋯
y
1
y
2
⋯
θ
1
θ
2
⋯
κ
1
κ
2
⋯
)
\left. \left( \begin{array}{c} x_r\\ y_r\\ \theta _r\\ \kappa _r\\ \end{array} \right. \right) =\left( \begin{matrix} x_1& x_2& \cdots\\ y_1& y_2& \cdots\\ \theta _1& \theta _2& \cdots\\ \kappa _1& \kappa _2& \cdots\\ \end{matrix} \right)
xryrθrκr
=
x1y1θ1κ1x2y2θ2κ2⋯⋯⋯⋯
(2)匹配点
遍历 ( x R , y r ) (x_R,y_r) (xR,yr) 找到与 ( x , y ) (x,y) (x,y) 最近的规划点的序列号,记为 d m i n d_{min} dmin。
【序列号】是距离最短的点的下标序列,遍历所有规划点,比如找到第四个规划点距离车辆当前位置 ( x , y ) (x,y) (x,y) 最近,那么 d m i n = 4 d_{min}=4 dmin=4。最短距离的点叫做匹配点, d m i n d_{min} dmin 就是匹配点的序列号。
(3)方向向量
写出匹配点的切线方向量
τ
⃗
\vec{\tau}
τ 与法线方向量
n
⃗
\vec{n}
n:
τ
⃗
=
(
cos
(
θ
d
min
)
sin
(
θ
d
min
)
)
n
⃗
=
(
−
sin
(
θ
d
min
)
cos
(
θ
d
min
)
)
\vec{\tau}=\left( \begin{array}{c} \cos \left( \theta _{d_{\min}} \right)\\ \sin \left( \theta _{d_{\min}} \right)\\ \end{array} \right) \quad \vec{n}=\left( \begin{array}{c} -\sin \left( \theta _{d_{\min}} \right)\\ \cos \left( \theta _{d_{\min}} \right)\\ \end{array} \right)
τ=(cos(θdmin)sin(θdmin))n=(−sin(θdmin)cos(θdmin))
(4)误差的距离的向量
误差的距离的向量
d
−
e
⃗
r
r
d_-\vec{e}_{rr}
d−err:
d
−
e
⃗
r
r
=
(
x
−
x
d
m
i
n
y
−
y
d
m
i
n
)
d_-\vec{e}_{rr}=\left( \begin{array}{c} x-x_{d_{min}}\\ y-y_{d_{min}}\\ \end{array} \right)
d−err=(x−xdminy−ydmin) 实际上就是两个向量的减法,第一个向量是车辆真实位矢向量,第二个向量是匹配点的位矢向量,这两个相减就是误差的距离向量,对应的就是第七节的
x
⃗
−
x
⃗
m
\vec{x}-\vec{x}_m
x−xm。
(5)横向误差
e d = n ⃗ T ⋅ d − e ⃗ r r e_d=\vec{n}^{T}\cdot d_{-}\vec{e}_{\mathrm{rr}} ed=nT⋅d−err
(6)纵向误差
e s = τ ⃗ T ⋅ d − e ⃗ r r e_s=\vec{\tau}^T\cdot d_-\vec{e}_{rr} es=τT⋅d−err
(7)投影点切线方向与x轴夹角
计算
θ
r
\theta_r
θr,即投影点切线方向与
x
x
x 轴夹角,因为匹配点不等于投影点,所以还要进行相应的运算:
θ
r
=
θ
d
min
+
κ
d
min
⋅
e
s
\theta _r=\theta _{d_{\min}}+\kappa_{d_{\min}}\cdot e_s
θr=θdmin+κdmin⋅es
(8)横向误差导数
e ˙ d = ∣ v ⃗ ∣ sin ( θ − θ r ) = v y cos ( φ − θ r ) + v x sin ( φ − θ r ) \dot{e}_d=|\vec{v}|\sin \left( \theta -\theta _r \right) =v_y\cos \left( \varphi -\theta _r \right) +v_x\sin \left( \varphi -\theta _r \right) e˙d=∣v∣sin(θ−θr)=vycos(φ−θr)+vxsin(φ−θr)
(9)航向角误差
e φ = φ − θ r e_{\varphi}=\varphi-\theta_r eφ=φ−θr
(10)弧速度
s ˙ = v cos ( θ − θ r ) 1 − κ d m i n ⋅ e d = v x cos ( φ − θ r ) − v y sin ( φ − θ r ) 1 − κ d min e d \dot{s}=\frac{v\cos \left( \theta -\theta _r \right)}{1-\kappa _{d_{min}}\cdot e_d}=\frac{v_x\cos \left( \varphi -\theta _r \right) -v_y\sin \left( \varphi -\theta _r \right)}{1-\kappa_{d_{\min}}e_d} s˙=1−κdmin⋅edvcos(θ−θr)=1−κdminedvxcos(φ−θr)−vysin(φ−θr)
(11)横摆角速度误差
e ˙ φ = φ ˙ − κ d min ⋅ s ˙ \dot{e}_{\varphi}=\dot{\varphi}-\kappa_{\text{d}_{\min}}\cdot \dot{s} e˙φ=φ˙−κdmin⋅s˙
(12)投影点曲率
κ r = κ d min \kappa _r=\kappa _{\text{d}_{\min}} κr=κdmin
(13)输出
第三个模块的算法是整个控制算法中最复杂的,这一块涉及到的公式很多,而且很容易出问题。
为了得到 LQR 的 A A A 和 B B B,做了一系列假设,其中假设 e φ e_\varphi eφ 比较小,所以 sin e φ = e φ \sin e_\varphi=e_\varphi sineφ=eφ,以及 cos e φ \cos e_\varphi coseφ近似认为是1,然后才能得到 A A A 和 B B B 的具体的表达式。
但第九步 e φ = φ − θ r e_{\varphi}=\varphi-\theta_r eφ=φ−θr 有问题,角度具有多值性,即 φ + 2 π \varphi + 2\pi φ+2π,还是原来角度,但从 e φ e_\varphi eφ 的角度上来说, φ + 2 π \varphi + 2\pi φ+2π 就不是小角度了,包括 θ r \theta_r θr 也一样, θ r = 0 \theta_r=0 θr=0 和 θ r = 2 π \theta_r=2 \pi θr=2π 实际上是同一个角度,但如果用 e φ = φ − θ r e_{\varphi}=\varphi-\theta_r eφ=φ−θr,可能导致 e φ e_{\varphi} eφ 很大。
最终控制 u = − k e r r + δ f u=-ke_{rr}+\delta _f u=−kerr+δf ,误差 e r r = ( e d , e ˙ d , e φ , e ˙ φ ) T e_{rr}=\left( e_d,\dot{e}_d,e_{\varphi},\dot{e}_{\varphi} \right) ^T err=(ed,e˙d,eφ,e˙φ)T ,最终控制中的 u u u 和 e φ e_{\varphi} eφ 有关系,这样可能导致 e φ e_{\varphi} eφ 在短时间内突变成多 2 π 2\pi 2π,导致控制量 u u u 在短时间内突变,进而导致控制失效。
所以第九步最容易出错,在具体写代码时注意避免。
4、前馈控制计算模块
k 3 = k ( 3 ) k_{3}=k(3) k3=k(3) δ f = κ [ a + b − b k 3 − m v x 2 a + b ( b C α f + a C α r k 3 − a C α r ) ] \delta _f=\kappa\left[ a+b-bk_3-\frac{mv_{x}^{2}}{a+b}\left( \frac{b}{C_{\alpha f}}+\frac{a}{C_{\alpha r}}k_3-\frac{a}{C_{\alpha r}} \right) \right] δf=κ[a+b−bk3−a+bmvx2(Cαfb+Cαrak3−Cαra)]
5、最终控制计算模块
δ = − k e r r + δ f \delta =-ke_{rr}+\delta _f δ=−kerr+δf 这样关于横线控制五个模块的算法都已经讲完了。
只要写好这五个模块,通过流程图把这五个模块拼起来,就可以得到最终控制量 u u u。
四、预测模块
但这样还不够,因为这五个模块只是前七节讲到的内容,为了更好地体现算法性能,还要再加预测模块。
1、预测模块的重要性
为什么要加预测模块?
首先因为车本身具有惯性,导致控制天生有滞后性。为了解决这个问题,要进行提前控制,加入预测模块。
这里具体解释一下。比如有一段规划轨迹:
如果是人开车,看到这样一条路径规划,会不会去打方向盘?应该不会,因为人具有预见性,能看到未来路径到底长什么样。虽然现在的状态和规划路径有误差,但是如果保持当前状态行驶,在未来一段时间内,车和规划路径就没有误差了。所以人不会打方向盘,因为人能知道未来的路径规划是什么。
但算法不行,算法开车只会机械地寻找与真实位置最接近的规划轨迹点。如果算出误差不等于0,那么算法就会打方向盘。
下面的例子也说明了同样的道理:
人开车的话,知道未来的路径规划不是直线,所以即使现在的运动状态和规划的轨迹完全贴合,没有误差,人也会提前动方向盘。
但算法开车只看此时误差为 0 0 0,算法不会动方向盘,只在匀速直线运行到下一时刻时,发现此时误差不等于 0 0 0,才会动方向盘。所以算法控制具有一定的滞后性,看不到未来的路径规划,只关心哪个轨迹点与真实位置最近。
所以要加上预测模块,为了更好地控制,算法需要具有一定的预见性。
预测就是在车辆当前状态下,假设车辆在一段时间内做匀速直线运动,运动后的点就是预测点。比如说下面这样:
红色点为预测点,如果没有预测,算法很明显和轨迹有误差 d d d,所以 d d d 不等于 0 0 0, u u u 就不等于 0 0 0,算法就会打方向盘。
但是如果做了预测,其实就是求预测点和轨迹之间的误差。预测点和轨迹贴在一起,没有误差,就意味着 x p r e x_{pre} xpre 和轨迹重合, u = 0 u=0 u=0,所以就不打方向盘,这时算法就具有预见性。
对于上面提到的第二种情况也一样,比如预测点在这本来用原本真实点计算误差,那就是 0 0 0,现在用预测点和规划点计算误差,此时误差不等于 0 0 0,所以打方向盘。
2、预测模块算法
下面编写具体的预测算法,比如下图的直角坐标系:
车辆速度为 v v v,与 x x x 轴夹角为 θ \theta θ,预测时间记为 t s t_s ts,假设在预测时间内,车辆做匀速直线运动,那么前视距离为 v t s vt_s vts。若有加速度信号,则前视距离为 v t s + 1 2 a t s 2 vt_s+\frac{1}{2}at_{s}^{2} vts+21ats2。
预测点的信息记为 ( x p r e , y p r e , φ p r e , v x p r e , v y p r e , φ ˙ p r e ) ( x_{pre},y_{pre},\varphi _{pre},v_{xpre},v_{ypre},\dot{\varphi}_{pre}) (xpre,ypre,φpre,vxpre,vypre,φ˙pre)
根据几何关系有:
x
p
r
e
=
x
+
v
t
s
cos
θ
=
x
+
v
t
s
cos
(
β
+
φ
)
=
x
+
v
x
t
s
cos
φ
+
v
y
t
s
sin
φ
y
p
r
e
=
y
+
v
t
s
sin
θ
=
y
+
v
y
t
s
cos
φ
+
v
x
t
s
sin
φ
φ
p
r
e
=
φ
+
φ
˙
t
s
v
x
p
r
e
=
v
x
v
y
pre
=
v
y
φ
˙
pre
=
φ
˙
\begin{aligned} x_{pre}&=x+vt_s\cos \theta =x+vt_s\cos \left( \beta +\varphi \right) =x+v_xt_s\cos \varphi +v_yt_s\sin \varphi\\ y_{pre}&=y+vt_s\sin \theta =y+v_yt_s\cos \varphi +v_xt_s\sin \varphi\\ \varphi _{pre}&=\varphi +\dot{\varphi}t_s\\ v_{xpre}&=v_x\\ v_{y\text{pre}}&=v_y\\ \dot{\varphi}_{\text{pre}}&=\dot{\varphi}\\ \end{aligned}
xpreypreφprevxprevypreφ˙pre=x+vtscosθ=x+vtscos(β+φ)=x+vxtscosφ+vytssinφ=y+vtssinθ=y+vytscosφ+vxtssinφ=φ+φ˙ts=vx=vy=φ˙ 这就是预测模块的完整算法。
五、完整算法流程图
加入预测模块后的算法流程图:
六、总结
本来是真实的 ( x , y , φ , v x , v y , φ ˙ ) (x,y,\varphi ,v_x,v_y,\dot{\varphi}) (x,y,φ,vx,vy,φ˙) 和规划轨迹 ( x r , y r , θ r , κ r ) \left( x_r,y_r,\theta _r,\kappa _r \right) (xr,yr,θr,κr) 做误差计算,现在加入预测模块,就用预测点的 ( x p r e , y p r e , φ p r e , v x p r e , v y p r e , φ ˙ p r e ) \left( x_{pre},y_{pre},\varphi _{pre},v_{xpre},v_{ypre},\dot{\varphi}_{pre} \right) (xpre,ypre,φpre,vxpre,vypre,φ˙pre) 和规划轨迹做预测,这样算法具有一定的预见性。
这才是想要的完整算法,后续编程就按照算法流程图来编写,一共六大模块,已经全部讲完。
本篇博客就写到这里,但第八节还没有完,下一小节讲解具体的代码编写以及模型搭建。欢迎关注!
参考资料
后记:
🌟 感谢您耐心阅读这篇关于 横向控制Ⅰ | 算法与流程 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀