【自动驾驶】PurePursuit实现轨迹跟踪 |python实现 |c++实现_c++实现pid智能汽车轨迹跟踪控制

参考资料

• 轨迹跟踪Pure Pursuit方法
• 使用pure pursuit实现无人车轨迹追踪
• 论文

1. 几何车辆模型

在前文中讲解了PID实现轨迹跟踪，这篇来讲解纯追踪法。

使用的车辆模型这里依旧采取以后轴中心为车辆中心的单车运动学模型

其满足
$$\tan {\delta }_f=\frac{L}{R}\text{\hspace{1em}(1)}$$

2. Pure Pursuit（纯追踪）算法

2.1 算法思想

纯追踪算法的原理很简单，就是单车模型通过调整前轮转向$\delta$运动，使得车辆后轴中心刚好可以经过当前规划的路点。换句话说，此时的后轴中心为圆弧切点，车辆纵向车身为切线。通过控制前轮转角$\delta$， 使车辆可以沿着一条经过目标路点（goal point）（或者叫预瞄点）的圆弧行驶。

该算法会根据机器人的当前位置在路径上移动预瞄点，直到路径的终点。可以想象成机器人不断追逐它前面的一个点。

总结如下：

基于当前车辆后轴中心位置,在参考路径上向$l_d$ (自定义，称为前视距离)的距离匹配一个预瞄点。假设车辆后轴中心点可以按照一定的转弯半径$R$行驶抵达该预瞄点,然后根据预瞄距离$l_d$、转弯半径$R$、车辆坐标系下预瞄点的朝向角$2\alpha$之间的几何关系确定前轮转角。

2.2 算法推导

$\delta$即为${\delta }_f$。
α为路点与车后轴中心连成的向量的角度与车辆偏航角的差值，当路点在车的左边时， $\alpha >0$ ，反之则 $\alpha <0$ ； $l_d$为车后轴离前视路点的距离，又被称为前视距离，它决定了将预瞄路点放置多远。

根据上图，由正弦定理得

$$\frac{l_d}{\sin (2\alpha )}=\frac{R}{\sin \left(\frac{\pi }{2}-\alpha \right)}\text{\hspace{1em}(2)}$$
根据三角函数性质，对等式(2)化简得
$$\frac{l_d}{2\sin (\alpha )\cos (\alpha )}=\frac{R}{\cos (\alpha )}\text{\hspace{1em}(3)}$$
由于 $\cos (\alpha )\ne 0$ ，对等式(3)进一步化简得
$$R=\frac{l_d}{2\sin (\alpha )}\text{\hspace{1em}(4)}$$
故圆弧的曲率表示为
$$k=\frac{2\sin (\alpha )}{l_d}\text{\hspace{1em}(5)}$$
根据等式(1)所示的车辆几何关系得
$$\delta =\arctan (\frac{L}{R})=\arctan (k\cdot L)\text{\hspace{1em}(6)}$$
将等式(5)带入等式(6)得纯追踪算法控制量的的最终表达式：
$$\delta =\arctan \left(\frac{2L\sin \alpha }{l_d}\right)\text{\hspace{1em}(7)}$$

当然，通过上图可知
$$\sin \alpha =\frac{e_y}{l_d}\text{\hspace{1em}(8)}$$
将式(8)代入式(7)得:
$$\delta =\arctan \left(\frac{2L{e}_y}{l_d^2}\right)\text{\hspace{1em}(9)}$$

分析式(9)，利用小角度近似，我们有:
$$\delta \approx \frac{2L}{l_d^2}{e}_y$$

把 $\frac{2L}{l_d^2}$​看作比例控制器的参数，$e_y$作为系统误差，那么这就相当于一个以横向跟踪误差CTE作为系统误差的比例控制器。

在pure pursuit方法中，$l_d$表示成无人车纵向线速度的形式，即 $l_d=\lambda v_x+c，c$ 为常数。

2.3 小结

1. 算法伪代码

算法伪代码可以概括如下：

• 输入：当前车辆位置、纵向速度$v_x$和当前目标路点
• 计算：
  • $l_d=\lambda v_x+c$
  • 计算$\alpha$（根据几何关系计算）
  • $\delta =\arctan \left(\frac{2L\sin \alpha }{l_d}\right)$
• 输出：车辆转角$\delta$

2. 模型适配

纯跟踪算法是把车看成一个阿克曼模型进行几何解算的，本质是不适合差速车或者麦轮车的

3. 前视距离调整

前视距离的调整需要根据机器人运行的实际情况来进行调整。

前视距离是整个Pure Pursuit控制器的重要参数。往前看的距离是机器人从当前位置应沿着路径观察的距离，以计算转角控制命令。

在低速的情况下合理调整前视距离可以实现较好的路径跟踪效果，较小的前视距离能使机器人更加精确地追踪路径，但可能会引起机器人控制的不稳定甚至震荡；

较大的前视距离可以使机器人跟踪路径更加平滑，但不能精确地跟踪原始的路径，在大转角处会出现曲率大、转向不足的情况。

3. python代码实现

该部分有参考python robotics代码

以后轴中心为车辆中心的单车运动学模型为（具体可参考笔者之前的博客）
{ 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(ψ)ψ˙​=LV​tanδf​V˙=a​
单车运动学模型实现如下。

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

相关参数：

L=2 # 车辆轴距，单位：m
v = 2 # 初始速度
x_0=0 # 初始x
y_0=-1 #初始y
psi_0=0 # 初始航向角
dt=0.1 # 时间间隔，单位：s
lam = 0.1 # 前视距离系数
c=2 # 前视距离
1
2
3
4
5
6
7
8

计算参考轨迹上车辆当前位置的前视目标点：

def cal_target_index(robot_state, refer_path, l_d):
    """得到前视目标点
    Args:
        robot_state (_type_): 当前车辆位置
        refer_path (_type_): 参考轨迹（数组）
        l_d：前视距离
    Returns:
        _type_: 前视目标点的索引
    """
    dists = []
    for xy in refer_path:
        dis = np.linalg.norm(robot_state-xy)
        dists.append(dis)

    min_index = np.argmin(dists)

    delta_l = np.linalg.norm(refer_path[min_index]-robot_state)
    # 搜索前视目标点
    while l_d > delta_l and (min_index+1) < len(refer_path):
        delta_l = np.linalg.norm(refer_path[min_index+1]-robot_state)
        min_index += 1
    return min_index
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

Pure pursuit算法：

def pure_pursuit_control(robot_state,current_ref_point,l_d):
    """pure pursuit

    Args:
        robot_state (_type_): 车辆位置
        current_ref_point (_type_): 当前参考路点
        l_d：前视距离
    return:返回前轮转向角delta
    """
    alpha = math.atan2(current_ref_point[1]-robot_state[1], current_ref_point[0]-robot_state[0])-ugv.psi
    delta = math.atan2(2*L*np.sin(alpha),l_d)
    return delta
    
1
2
3
4
5
6
7
8
9
10
11
12
13

主函数

from celluloid import Camera  # 保存动图时用，pip install celluloid


# set reference trajectory
refer_path = np.zeros((1000, 2))
refer_path[:,0] = np.linspace(0, 100, 1000) # 直线
refer_path[:,1] = 2*np.sin(refer_path[:,0]/3.0)+2.5*np.cos(refer_path[:,0]/2.0) # 生成正弦轨迹



ugv = KinematicModel_3(x_0,y_0,psi_0,v,L,dt)

x_ = []
y_ = []
fig = plt.figure(1)
# 保存动图用
camera = Camera(fig)
for i in range(600):
    robot_state = np.zeros(2)
    robot_state[0] = ugv.x
    robot_state[1] = ugv.y
    
    l_d = lam*ugv.v+c # 注意，这里的运动学模型使用的速度v就是车身纵向速度vx
    ind = cal_target_index(robot_state,refer_path,l_d)  # 搜索前视路点


    delta = pure_pursuit_control(robot_state,refer_path[ind],l_d)

    ugv.update_state(0,delta) # 加速度设为0，恒速

    x_.append(ugv.x)
    y_.append(ugv.y)

    # 显示动图
    plt.cla()
    plt.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0)
    plt.plot(x_, y_, "-r", label="trajectory")
    plt.plot(refer_path[ind, 0], refer_path[ind, 1], "go", label="target")
    # plt.axis("equal")
    plt.grid(True)
    plt.pause(0.001)
#     camera.snap()
# animation = camera.animate()
# animation.save('trajectory.gif')

plt.figure(2)
plt.plot(refer_path[:,0], refer_path[:,1], '-.b', linewidth=1.0)
plt.plot(x_,y_,'r')
plt.show()

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

结果如下：

完整代码见github仓库

4. c++代码实现

由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与python代码实现类似，这边不做相关代码解释。完整代码详见另一个github仓库。