文章目录
- 参考资料
- 1. 基本概念
- 2. 李雅普诺夫稳定判据
- 3. python代码实现
- 3.1 车辆模型
- 3.2 相关参数设置
- 3.3 生成轨迹曲线
- 3.4 角度归一化
- 3.5 后轮反馈控制算法实现
- 3.6 主函数
- 4. c++代码实现
参考资料
- 自动驾驶中的规划控制概述
- A Survey of Motion Planning and Control Techniques for Self-Driving Urban Vehicles
- https://zhuanlan.zhihu.com/p/46377932
- pythonRobotics
- Frenet坐标推导过程整理
- Frenet坐标系相关知识系统学习
- 后轮位置反馈
- 后轴反馈控制——frenet坐标的一个应用
1. 基本概念
后轮反馈控制（Rear wheel feedback）是利用后轮中心的跟踪偏差来进行转向控制量计算的方法。
如图所示，参考轨迹上与车辆后轴中心距离最近的点
(
x
r
e
f
,
y
r
e
f
)
(x_{ref},y_{ref})
(xref,yref)为：
s
(
t
)
=
arg
min
γ
∥
(
x
r
(
t
)
,
y
r
(
t
)
)
−
(
x
r
e
f
(
γ
)
,
y
r
e
f
(
γ
)
)
∥
(1)
tag{1} s(t)=arg min _{gamma}left|left(x_{r}(t), y_{r}(t)right)-left(x_{r e f}(gamma), y_{r e f}(gamma)right)right|
s(t)=argγmin∥(xr(t),yr(t))−(xref(γ),yref(γ))∥(1)
其中，
(
x
r
(
t
)
,
y
r
(
t
)
)
left(x_{r}(t), y_{r}(t)right)
(xr(t),yr(t)) 代表
t
t
t 时刻车辆后轴中心位置坐标，
(
x
r
e
f
(
γ
)
,
y
r
e
f
(
γ
)
)
left(x_{r e f}(gamma), y_{r e f}(gamma)right)
(xref(γ),yref(γ)) 代表参考路径上离车辆后 轴距离最近点的位置坐标，
s
(
t
)
s(t)
s(t) 代表
t
t
t 时刻与车辆后轴中心点距离最近参考轨迹点的位置参数
γ
gamma
γ 。
参考路径在
s
(
t
)
s(t)
s(t) 参数处的单位切向量为
t
⃗
=
(
∂
x
r
e
f
∂
s
∥
s
(
t
)
,
∂
y
r
e
f
∂
s
∥
s
(
t
)
)
∥
(
∂
x
r
e
f
(
s
(
t
)
)
∂
s
,
∂
y
r
e
f
(
s
(
t
)
)
∂
s
)
∥
=
(
t
x
,
t
y
)
(2)
tag{2} vec{t}=frac{left(frac{partial x_{r e f}}{partial s}left|_{s(t)}, frac{partial y_{r e f}}{partial s}right|_{s(t)}right)}{left|left(frac{partial x_{r e f}(s(t))}{partial s}, frac{partial y_{r e f}(s(t))}{partial s}right)right|}=left(t_{x}, t_{y}right)
t=∥∥∥(∂s∂xref(s(t)),∂s∂yref(s(t)))∥∥∥(∂s∂xref∥∥∥s(t),∂s∂yref∥∥∥s(t))=(tx,ty)(2)
跟踪误差向量表示如下:
d
⃗
(
t
)
=
(
x
r
(
t
)
−
x
r
e
f
(
s
(
t
)
)
,
y
r
(
t
)
−
y
r
e
f
(
s
(
t
)
)
)
=
(
d
x
,
d
y
)
(3)
tag{3} vec{d}(t)=left(x_{r}(t)-x_{r e f}(s(t)), y_{r}(t)-y_{r e f}(s(t))right)=left(d_{x}, d_{y}right)
d(t)=(xr(t)−xref(s(t)),yr(t)−yref(s(t)))=(dx,dy)(3)
所以跟踪误差向量
d
⃗
vec{d}
d 和参考路径上最近点的单位切向量
t
⃗
vec{t}
t 的叉积为
e
⃗
=
t
⃗
×
d
⃗
=
∣
t
x
t
y
d
x
d
y
∣
=
t
x
d
y
−
t
y
d
x
(4)
tag{4} vec{e}=vec{t} times vec{d}=left|begin{array}{cc} t_{x} & t_{y} \ d_{x} & d_{y} end{array}right|=t_{x} d_{y}-t_{y} d_{x}
e=t×d=∣∣∣∣txdxtydy∣∣∣∣=txdy−tydx(4)
关于车辆航向向量与目标路径切向量的夹角
ψ
e
psi_{e}
ψe 如下:
ψ
e
(
t
)
=
ψ
−
arctan
∂
y
r
e
f
(
s
(
t
)
)
/
∂
s
∂
x
r
e
f
(
s
(
t
)
)
/
∂
s
(5)
tag{5} psi_{e}(t)=psi-arctan frac{partial y_{r e f}(s(t))/partial s} {partial x_{r e f}(s(t))/partial s}
ψe(t)=ψ−arctan∂xref(s(t))/∂s∂yref(s(t))/∂s(5)
根据上图，
ψ
e
psi_{e}
ψe 还可以表示如下：
ψ
e
(
t
)
=
ψ
−
ψ
r
e
f
(6)
tag{6} psi_{e}(t)=psi-psi_{ref}
ψe(t)=ψ−ψref(6)
如上图所示，车辆沿目标路径顺时针运动，在参考路径左侧，此时 e > 0 e>0 e>0 。当在参考路径右侧时， e < 0 e<0 e<0.
s
˙
dot{s}
s˙ 可以表示为：
s
˙
=
v
r
⋅
cos
(
ψ
e
)
1
−
k
(
s
)
e
(8)
tag{8} dot{s}=frac{v_{r} cdot cos left(psi_{e}right)}{1-k(s) e}
s˙=1−k(s)evr⋅cos(ψe)(8)
其中
k
(
s
)
k(s)
k(s)表示曲线在参考点s处的曲率，
v
r
v_r
vr表示后轮线速度。
车辆横向移动的速度
e
˙
dot{e}
e˙ 可以表示如下:
e
˙
=
v
r
⋅
sin
(
ψ
e
)
(9)
tag{9} dot{e}=v_{r} cdot sin left(psi_{e}right)
e˙=vr⋅sin(ψe)(9)
关于车辆航向角误差的变化率表示如下:
ψ
˙
e
=
ψ
˙
−
ψ
˙
r
e
f
=
ψ
˙
−
s
˙
⋅
k
(
s
)
=
ψ
˙
−
v
r
⋅
cos
(
ψ
e
)
⋅
k
(
s
)
1
−
k
(
s
)
e
(10)
tag{10} begin{aligned} dot{psi}_{e} &=dot{psi}-dot{psi}_{r e f} \ &=dot{psi}-dot{s} cdot k(s) \ &=dot{psi}-frac{v_{r} cdot cos left(psi_{e}right) cdot k(s)}{1-k(s) e} end{aligned}
ψ˙e=ψ˙−ψ˙ref=ψ˙−s˙⋅k(s)=ψ˙−1−k(s)evr⋅cos(ψe)⋅k(s)(10)
综上所述，基于上述参考曲线处的速度、横向误差变化率和航向角误差变化率如下:
s
˙
=
v
r
⋅
cos
(
ψ
e
)
1
−
k
(
s
)
e
e
˙
=
v
r
⋅
sin
(
ψ
e
)
ψ
˙
e
=
ψ
˙
−
v
r
⋅
cos
(
ψ
e
)
⋅
k
(
s
)
1
−
k
(
s
)
e
(11)
tag{11} begin{aligned} dot{s} &=frac{v_{r} cdot cos left(psi_{e}right)}{1-k(s) e} \ dot{e} &=v_{r} cdot sin left(psi_{e}right) \ dot{psi}_{e} &=dot{psi}-frac{v_{r} cdot cos left(psi_{e}right) cdot k(s)}{1-k(s) e} end{aligned}
s˙e˙ψ˙e=1−k(s)evr⋅cos(ψe)=vr⋅sin(ψe)=ψ˙−1−k(s)evr⋅cos(ψe)⋅k(s)(11)
其实公式(11)这就是
Frenet坐标系下的公式。有关Frenet坐标系参考这篇博客。
Frenet坐标系使用道路的中心线作为参考线，使用参考线的 切线向量 和 法线向量 建立坐标系，那么基于参考线的位置就可以使用 纵向距离 和 横向距离 来描述任意位置；同时纵向和横向的速度、加速度、加加速度等信息也更便于计算
笛卡尔坐标系与Frenet坐标系对比如下：
2. 李雅普诺夫稳定判据
基于上述微分方程，采用李雅普诺夫第二方法，寻找一个状态量的方程
V
(
x
)
V(x)
V(x)，也就是
V
(
e
,
ψ
e
)
V(e,psi_e)
V(e,ψe)，定义李雅普诺夫函数形式如下:
V
(
e
,
ψ
e
)
=
1
2
e
2
+
1
2
k
2
ψ
e
2
(12)
tag{12} Vleft(e, psi_{e}right)=frac{1}{2} e^{2}+frac{1}{2 k_{2}} psi_{e}^{2}
V(e,ψe)=21e2+2k21ψe2(12)
其中 参数
k
2
>
0
k_{2}>0
k2>0 ，为了使
(
e
,
ψ
e
)
left(e, psi_{e}right)
(e,ψe) 在平衡点
(
0
,
0
)
(0,0)
(0,0) 处稳定，根据李雅普诺夫稳定判据，必须满足以下两个条 件:
- lim ∣ e , ψ e ∣ → ∞ V = ∞ lim _{left|e, psi_{e}right| rightarrow infty} V=infty lim∣e,ψe∣→∞V=∞
- V ˙ < 0 ( e ≠ 0 , ψ e ≠ 0 ) dot{V}<0 quadleft(e neq 0, psi_{e} neq 0right) V˙<0(e=0,ψe=0)
对于条件1，等式(12)明显成立，所以李雅普诺夫稳定判据第一条满足。
对于
V
˙
dot{V}
V˙，结合等式(11)的微分方程，推导形式如下:
V
˙
=
e
e
˙
+
1
k
2
ψ
e
ψ
˙
e
=
e
⋅
v
r
⋅
sin
(
ψ
e
)
+
1
k
2
ψ
e
(
ψ
˙
−
v
r
⋅
cos
(
ψ
e
)
⋅
k
(
s
)
1
−
k
(
s
)
e
)
(13)
tag{13} begin{aligned} dot{V} &=e dot{e}+frac{1}{k_{2}} psi_{e} dot{psi}_{e} \ &=e cdot v_{r} cdot sin left(psi_{e}right)+frac{1}{k_{2}} psi_{e}left(dot{psi}-frac{v_{r} cdot cos left(psi_{e}right) cdot k(s)}{1-k(s) e}right) end{aligned}
V˙=ee˙+k21ψeψ˙e=e⋅vr⋅sin(ψe)+k21ψe(ψ˙−1−k(s)evr⋅cos(ψe)⋅k(s))(13)
我们需要做的就是设计一个
ψ
˙
dot{psi}
ψ˙，使得
V
˙
<
0
dot{V}<0
V˙<0即满足第2条李雅普诺夫稳定判据。
根据等式(13)，令
V
˙
=
0
dot{V}=0
V˙=0 ，化简得
k
2
v
r
sin
(
ψ
e
)
ψ
e
e
+
ψ
˙
−
v
r
⋅
cos
(
ψ
e
)
⋅
k
(
s
)
1
−
k
(
s
)
e
=
0
(14)
tag{14} k_{2} v_{r} frac{sin left(psi_{e}right)}{psi_{e}} e+dot{psi}-frac{v_{r} cdot cos left(psi_{e}right) cdot k(s)}{1-k(s) e}=0
k2vrψesin(ψe)e+ψ˙−1−k(s)evr⋅cos(ψe)⋅k(s)=0(14)
故临界控制率
ψ
˙
∗
dot{psi}^{*}
ψ˙∗ 为:
ψ
˙
∗
=
v
r
⋅
cos
(
ψ
e
)
⋅
k
(
s
)
1
−
k
(
s
)
e
−
k
2
v
r
sin
(
ψ
e
)
ψ
e
e
(15)
tag{15} dot{psi}^{*}=frac{v_{r} cdot cos left(psi_{e}right) cdot k(s)}{1-k(s) e}-k_{2} v_{r} frac{sin left(psi_{e}right)}{psi_{e}} e
ψ˙∗=1−k(s)evr⋅cos(ψe)⋅k(s)−k2vrψesin(ψe)e(15)
为了满足第2条李雅普诺夫稳定判据，设置一个调节函数
g
(
e
,
ψ
e
,
t
)
>
0
gleft(e, psi_{e}, tright)>0
g(e,ψe,t)>0 ，可以得出基于航向角变化率的控 制命令:
ψ
˙
=
ψ
˙
∗
−
g
(
e
,
ψ
e
,
t
)
ψ
e
(16)
tag{16} dot{psi}=dot{psi}^{*}-gleft(e, psi_{e}, tright) psi_{e}
ψ˙=ψ˙∗−g(e,ψe,t)ψe(16)
设
g
(
e
,
ψ
e
,
t
)
=
k
ψ
∣
v
r
∣
gleft(e, psi_{e}, tright)=k_{psi}left|v_{r}right|
g(e,ψe,t)=kψ∣vr∣ ，其中
k
ψ
>
0
k_{psi}>0
kψ>0 。
ψ
˙
=
v
r
⋅
cos
(
ψ
e
)
⋅
k
(
s
)
1
−
k
(
s
)
e
−
k
2
v
r
sin
(
ψ
e
)
ψ
e
e
−
k
ψ
∣
v
r
∣
ψ
e
(17)
tag{17} dot{psi}=frac{v_{r} cdot cos left(psi_{e}right) cdot k(s)}{1-k(s) e}-k_{2} v_{r} frac{sin left(psi_{e}right)}{psi_{e}} e-k_{psi}left|v_{r}right| psi_{e}
ψ˙=1−k(s)evr⋅cos(ψe)⋅k(s)−k2vrψesin(ψe)e−kψ∣vr∣ψe(17)
将等式(17)带入等式(13)得
V
˙
=
v
r
⋅
sin
(
ψ
e
)
⋅
e
+
1
k
2
ψ
e
(
ψ
˙
−
v
r
⋅
cos
(
ψ
e
)
⋅
k
(
s
)
1
−
k
(
s
)
e
)
=
v
r
⋅
sin
(
ψ
e
)
⋅
e
+
1
k
2
ψ
e
(
−
k
2
v
r
sin
(
ψ
e
)
ψ
e
e
−
k
ψ
∣
v
r
∣
ψ
e
)
=
v
r
⋅
sin
(
ψ
e
)
⋅
e
−
v
r
⋅
sin
(
ψ
e
)
⋅
e
−
k
ψ
k
2
∣
v
r
∣
ψ
e
2
=
−
k
ψ
k
2
∣
v
r
∣
ψ
e
2
≤
0
(18)
tag{18} begin{aligned} dot{V} &=v_{r} cdot sin left(psi_{e}right) cdot e+frac{1}{k_{2}} psi_{e}left(dot{psi}-frac{v_{r} cdot cos left(psi_{e}right) cdot k(s)}{1-k(s) e}right) \ &=v_{r} cdot sin left(psi_{e}right) cdot e+frac{1}{k_{2}} psi_{e}left(-k_{2} v_{r} frac{sin left(psi_{e}right)}{psi_{e}} e-k_{psi}left|v_{r}right| psi_{e}right) \ &=v_{r} cdot sin left(psi_{e}right) cdot e-v_{r} cdot sin left(psi_{e}right) cdot e-frac{k_{psi}}{k_{2}}left|v_{r}right| psi_{e}^{2} \ &=-frac{k_{psi}}{k_{2}}left|v_{r}right| psi_{e}^{2} leq 0 end{aligned}
V˙=vr⋅sin(ψe)⋅e+k21ψe(ψ˙−1−k(s)evr⋅cos(ψe)⋅k(s))=vr⋅sin(ψe)⋅e+k21ψe(−k2vrψesin(ψe)e−kψ∣vr∣ψe)=vr⋅sin(ψe)⋅e−vr⋅sin(ψe)⋅e−k2kψ∣vr∣ψe2=−k2kψ∣vr∣ψe2≤0(18)
所以设计的控制率满足稳定性条件。
注意，可以选择另外的李雅普诺夫函数然后重新设计控制率，只要设计的李雅普诺夫函数能够满足稳定性判据即可。
根据车辆几何关系
tan
(
δ
)
=
L
R
(19)
tag{19} tan (delta)=frac{L}{R}
tan(δ)=RL(19)
车辆航向变化率与车速和转弯半径的关系为
ψ
˙
=
v
R
dot{psi}=frac{v}{R}
ψ˙=Rv ，结合等式
(
17
)
(17)
(17) 可得
tan
(
δ
)
=
ψ
˙
L
v
(20)
tag{20} tan (delta)=frac{dot{psi} L}{v}
tan(δ)=vψ˙L(20)
所以控制率
δ
delta
δ 为
δ
=
arctan
(
ψ
˙
L
v
r
)
(21)
tag{21} delta=arctan left(frac{dot{psi} L}{v_{r}}right)
δ=arctan(vrψ˙L)(21)
3. python代码实现
3.1 车辆模型
设车辆运动学模型为以后轴中心为车辆中心的单车运动学模型（具体可参考笔者之前的博客），即
{
x
˙
=
V
cos
(
ψ
)
y
˙
=
V
sin
(
ψ
)
ψ
˙
=
V
L
tan
δ
f
V
˙
=
a
left{begin{array}{l} dot{x}=V cos (psi) \ dot{y}=V sin (psi) \ dot{psi}=frac{V}{L}tan{delta_f}\ dot{V}=a end{array}right.
⎩⎪⎪⎨⎪⎪⎧x˙=Vcos(ψ)y˙=Vsin(ψ)ψ˙=LVtanδfV˙=a
python实现代码如下。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23import math class KinematicModel_3: """假设控制量为转向角delta_f和加速度a """ def __init__(self, x, y, psi, v, L, dt): self.x = x self.y = y self.psi = psi self.v = v self.L = L # 实现是离散的模型 self.dt = dt def update_state(self, a, delta_f): self.x = self.x+self.v*math.cos(self.psi)*self.dt self.y = self.y+self.v*math.sin(self.psi)*self.dt self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt self.v = self.v+a*self.dt def get_state(self): return self.x, self.y, self.psi, self.v
3.2 相关参数设置
1
2
3
4
5
6
7
8
9
10K_psi=1.0 K2=0.5 #李雅普诺夫的参数 dt=0.1 # 时间间隔，单位：s L=2 # 车辆轴距，单位：m v = 2 # 初始速度 x_0=0 # 初始x y_0=0 #初始y psi_0=0 # 初始航向角
3.3 生成轨迹曲线
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55class MyReferencePath: def __init__(self): # set reference trajectory # refer_path包括4维：位置x, 位置y， 轨迹点的切线方向, 曲率k self.refer_path = np.zeros((1000, 4)) self.refer_path[:,0] = np.linspace(0, 100, 1000) # x self.refer_path[:,1] = 2*np.sin(self.refer_path[:,0]/3.0)+2.5*np.cos(self.refer_path[:,0]/2.0) # y # 使用差分的方式计算路径点的一阶导和二阶导，从而得到切线方向和曲率 for i in range(len(self.refer_path)): if i == 0: dx = self.refer_path[i+1,0] - self.refer_path[i,0] dy = self.refer_path[i+1,1] - self.refer_path[i,1] ddx = self.refer_path[2,0] + self.refer_path[0,0] - 2*self.refer_path[1,0] ddy = self.refer_path[2,1] + self.refer_path[0,1] - 2*self.refer_path[1,1] elif i == (len(self.refer_path)-1): dx = self.refer_path[i,0] - self.refer_path[i-1,0] dy = self.refer_path[i,1] - self.refer_path[i-1,1] ddx = self.refer_path[i,0] + self.refer_path[i-2,0] - 2*self.refer_path[i-1,0] ddy = self.refer_path[i,1] + self.refer_path[i-2,1] - 2*self.refer_path[i-1,1] else: dx = self.refer_path[i+1,0] - self.refer_path[i,0] dy = self.refer_path[i+1,1] - self.refer_path[i,1] ddx = self.refer_path[i+1,0] + self.refer_path[i-1,0] - 2*self.refer_path[i,0] ddy = self.refer_path[i+1,1] + self.refer_path[i-1,1] - 2*self.refer_path[i,1] self.refer_path[i,2]=math.atan2(dy,dx) # yaw # 计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2). # 参考：https://blog.csdn.net/weixin_46627433/article/details/123403726 self.refer_path[i,3]=(ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) # 曲率k计算 def calc_track_error(self, x, y): """计算跟踪误差 Args: x (_type_): 当前车辆的位置x y (_type_): 当前车辆的位置y Returns: _type_: _description_ """ d_x = [self.refer_path[i,0]-x for i in range(len(self.refer_path))] d_y = [self.refer_path[i,1]-y for i in range(len(self.refer_path))] d = [np.sqrt(d_x[i]**2+d_y[i]**2) for i in range(len(d_x))] s = np.argmin(d) yaw = self.refer_path[s, 2] k = self.refer_path[s, 3] angle = normalize_angle(yaw - math.atan2(d_y[s], d_x[s])) e = d[s] # 误差 if angle < 0: e *= -1 return e, k, yaw, s
3.4 角度归一化
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17def normalize_angle(angle): """ Normalize an angle to [-pi, pi]. :param angle: (float) :return: (float) Angle in radian in [-pi, pi] copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/stanley_control/stanley_control.html """ while angle > np.pi: angle -= 2.0 * np.pi while angle < -np.pi: angle += 2.0 * np.pi return angle
3.5 后轮反馈控制算法实现
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26def rear_wheel_feedback_control(robot_state, e, k, refer_path_psi): """后轮位置反馈控制 Args: robot_state (_type_): 机器人位姿，包括x,y,yaw,v e (_type_): _description_ k (_type_): 曲率 refer_path (_type_): 参考轨迹 refer_path_psi (_type_): 参考轨迹上点的切线方向的角度 Returns: _type_: _description_ """ psi,v = robot_state[2], robot_state[3] psi_e = normalize_angle(psi - refer_path_psi) # 公式17 psi_dot = v * k * math.cos(psi_e) / (1.0 - k * e) - K2 * v * math.sin(psi_e) * e / psi_e- K_psi * abs(v) * psi_e if psi_e == 0.0 or psi_dot == 0.0: return 0.0 # 公式21 delta = math.atan2(L * psi_dot, v) return delta
3.6 主函数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51from celluloid import Camera # 保存动图时用，pip install celluloid def main_2(): print("rear wheel feedback tracking start!!") reference_path = MyReferencePath() goal = reference_path.refer_path[-1,0:2] # 运动学模型 ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt) x_ = [] y_ = [] fig = plt.figure(1) # 保存动图用 camera = Camera(fig) # plt.ylim([-3,3]) for i in range(500): robot_state = np.zeros(4) robot_state[0] = ugv.x robot_state[1] = ugv.y robot_state[2]=ugv.psi robot_state[3]=ugv.v e, k, yaw_ref, s0 = reference_path.calc_track_error(robot_state[0], robot_state[1]) delta = rear_wheel_feedback_control(robot_state, e, k, yaw_ref) ugv.update_state(0, delta) # 加速度设为0，恒速 x_.append(ugv.x) y_.append(ugv.y) # 显示动图 plt.cla() plt.plot(reference_path.refer_path[:,0], reference_path.refer_path[:,1], "-.b", linewidth=1.0, label="course") plt.plot(x_, y_, "-r", label="trajectory") plt.plot(reference_path.refer_path[s0,0], reference_path.refer_path[s0,1], "go", label="target") # plt.axis("equal") plt.grid(True) plt.pause(0.001) # camera.snap() # 判断是否到达最后一个点 if np.linalg.norm(robot_state[0:2]-goal)<=0.1: print("reach goal") break # animation = camera.animate() # animation.save('trajectory.gif') main_2()
跟踪效果如下：
可见，跟踪效果非常好。
完整python代码文件见github仓库
4. c++代码实现
由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与python代码实现类似，这边就不再做相关代码解释了。完整代码详见另一个github仓库。
最后
以上就是快乐过客最近收集整理的关于【自动驾驶】后轮位置反馈实现轨迹跟踪 | python实现 | c++实现参考资料1. 基本概念2. 李雅普诺夫稳定判据3. python代码实现4. c++代码实现的全部内容，更多相关【自动驾驶】后轮位置反馈实现轨迹跟踪内容请搜索靠谱客的其他文章。
发表评论 取消回复