# 【自动驾驶】后轮位置反馈实现轨迹跟踪

### 文章目录

• 参考资料
• 1. 基本概念
• 2. 李雅普诺夫稳定判据
• 3. python代码实现
• 3.1 车辆模型
• 3.2 相关参数设置
• 3.3 生成轨迹曲线
• 3.4 角度归一化
• 3.5 后轮反馈控制算法实现
• 3.6 主函数

# 参考资料

• 自动驾驶中的规划控制概述
• A Survey of Motion Planning and Control Techniques for Self-Driving Urban Vehicles
• https://zhuanlan.zhihu.com/p/46377932
• pythonRobotics
• Frenet坐标推导过程整理
• Frenet坐标系相关知识系统学习
• 后轮位置反馈
• 后轴反馈控制——frenet坐标的一个应用

# 1. 基本概念

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\|

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)

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)

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 ) = ψ − arctan ⁡ 2 ( ∂ y r e f ( s ( t ) ) ∂ s , ∂ x r e f ( s ( t ) ) ∂ s ) (5) \tag{5} \psi_{e}(t)=\psi-\arctan 2\left(\frac{\partial y_{r e f}(s(t))}{\partial s}, \frac{\partial x_{r e f}(s(t))}{\partial s}\right)

ψ e ( t ) = ψ − ψ r e f (6) \tag{6} \psi_{e}(t)=\psi-\psi_{ref}

s ˙ \dot{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}

e ˙ = v r ⋅ sin ⁡ ( ψ e ) (9) \tag{9} \dot{e}=v_{r} \cdot \sin \left(\psi_{e}\right)

ψ ˙ 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}

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 ) (s,e,\psi_e) 当作一个坐标系的轴向量，这就是大名鼎鼎的Frenet坐标系。有关Frenet坐标系参考这篇博客。

Frenet坐标系使用道路的中心线作为参考线，使用参考线的 切线向量 和 法线向量 建立坐标系，那么基于参考线的位置就可以使用 纵向距离 和 横向距离 来描述任意位置；同时纵向和横向的速度、加速度、加加速度等信息也更便于计算

# 2. 李雅普诺夫稳定判据

V ( e , ψ e ) = 1 2 e 2 + 1 2 k 2 ψ e 2 (12) \tag{12} V\left(e, \psi_{e}\right)=\frac{1}{2} e^{2}+\frac{1}{2 k_{2}} \psi_{e}^{2}

1. lim ⁡ ∣ e , ψ e ∣ → ∞ V = ∞ \lim _{\left|e, \psi_{e}\right| \rightarrow \infty} V=\infty
2. V ˙ < 0 ( e ≠ 0 , ψ e ≠ 0 ) \dot{V}<0 \quad\left(e \neq 0, \psi_{e} \neq 0\right)

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}

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

ψ ˙ ∗ = 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

ψ ˙ = ψ ˙ ∗ − g ( e , ψ e , t ) ψ e (16) \tag{16} \dot{\psi}=\dot{\psi}^{*}-g\left(e, \psi_{e}, t\right) \psi_{e}
g ( e , ψ e , t ) = k ψ ∣ v r ∣ g\left(e, \psi_{e}, t\right)=k_{\psi}\left|v_{r}\right| ，其中 k ψ > 0 k_{\psi}>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}

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}

tan ⁡ ( δ ) = L R (19) \tag{19} \tan (\delta)=\frac{L}{R}

tan ⁡ ( δ ) = ψ ˙ L v (20) \tag{20} \tan (\delta)=\frac{\dot{\psi} L}{v}

δ = arctan ⁡ ( ψ ˙ L v r ) (21) \tag{21} \delta=\arctan \left(\frac{\dot{\psi} L}{v_{r}}\right)

# 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.
python实现代码如下。

import 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 相关参数设置

K_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 生成轨迹曲线

class 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 角度归一化

def 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 后轮反馈控制算法实现

def 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 主函数

from 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()