双舵轮AGV轨迹跟踪Pure Pursuit算法模型分析、python代码实现

一、Pure Pursuit算法模型图解和公式推导

下图分析的双舵轮AGV的运动学模型，是按照前后舵轮均安装在车体中心线上的车型分析的，当然其它结构的车型也可以按照这样的分析方法画出如下图解。

符号	物理量
r(m)	车体中心（小圆）转弯半径
R(m)	车轮（大圆）转弯半径
L(m)	轴距
δ(rad)	前轮转角
α(rad)	车身与预瞄点夹角
Ld(m)	预瞄距离
e(m)	与预瞄点的横向偏差
Xr(m)	预瞄点横坐标
Yr(m)	预瞄点纵坐标

通过正弦定理推导：

即

则可推导出 转弯圆弧的曲率k ：

还可得到：

即可推导出：（重要）

由上式可知，本控制器的本质是对 转角进行控制 ，以减少横向误差为目标的横向控制器。其中，
可视为控制器的 P参数 。 L 为车辆的 轴距 ， Ld 为设定的 预瞄距离 。 本控制器的控制效果主要取决于预瞄距离的选取，一般来说预瞄距离越长，控制效果会越平滑，预瞄距离越短，控制效果会越精确（同时也会带来一定的震荡）。 预瞄距离的选取也和当前车速有关 。

通常来说ld 被认为是车速的函数，在不同的车速下需要选择不同的前视距离。

一种最常见的调整前视距离的方法就是将前视距离表示成 车辆纵向速度 的线形函数，即 Ld=k*Vx ，那么前轮的转角公式就变成了：

于是纯追踪控制器的参数调整就变成了调整 前视系数k 。常来说，会使 用最大、最小前视距离 来 约束前视距离 ，越大的前视距离意味着轨迹的追踪越平滑，小的前视距离会使得追踪更加精确（当然也会带来控制的震荡）。

而两轮转角对应的角速度W，根据当前控制速度V可得：

• 非线性的预瞄距离选取方式

在有些文献中， 预瞄距离的选取方式 表现为如下形式（二次方程）：

上式中，对于常数A：

amax为 最大制动加速度 。 Av2表示 最短车辆制动距离 。 B表示车辆 遇到异常时需要的反应时间 ， Bv则为 对应的反应距离 ，C表示 车辆的最小转弯半径 。

• 实际应用总结

在实际应用中，Pure Pursuit算法通常 不要求跟踪的目标点到本车中心的距离切实等于预瞄距离。 而是会选择 采样好的一系列目标点中到中心距离最接近预瞄距离的那个点来 近似跟踪 。这样做的好处是可以 不需要目标轨迹的函数方程来求解真实预瞄距离坐标 ，极大地提升了算法的效率。

实践中， 纵向控制Vx 通常使用一个简单的 P控制器 ， 横向控制 （即转角控制）我们使用 纯追踪控制器 。

---

二、python实现三阶贝塞尔曲线跟踪

三阶 贝塞尔曲线路径生成以及轨迹跟踪仿真：

import numpy as np
import math
import matplotlib.pyplot as plt

k = 0.1  # 前视距离系数
Lfc = 0.5  # 前视距离
Kp = 1.0  # 速度P控制器系数
dt = 0.1  # 时间间隔，单位：s
L = 0.68  # 车辆轴距，单位：m

class VehicleState:

    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v

# 双舵轮车型的状态更新函数（模拟里程计）
def update(state, a, delta):
##
    state.x = state.x + state.v * math.cos(state.yaw) * dt
    state.y = state.y + state.v * math.sin(state.yaw) * dt
#    state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
    state.yaw = state.yaw + state.v / (L*0.5) * math.tan(delta) * dt    #TODO:车体中心为运动半径, v + w*dt
    state.v = state.v + a * dt
    
    return state
	
#p速度控制器，平滑小车前向速度
def PControl(target, current):
    a = Kp * (target - current)

    return a


# 纯跟踪算法控制器，输出为角速度（双舵轮的两个舵向角）、路点索引值
def pure_pursuit_control(state, cx, cy, pind):

    ind = calc_target_index(state, cx, cy)
	
	# pind：上一个路点索引值
    if pind >= ind:
        ind = pind

    if ind < len(cx):
    # tx、ty：车子所在（近似）路径上的点
        tx = cx[ind]
        ty = cy[ind]
    else:
        tx = cx[-1]
        ty = cy[-1]
        ind = len(cx) - 1

    alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw  #车身与预瞄点夹角

    if state.v < 0:  # back
        alpha = math.pi - alpha

    Lf = k * state.v + Lfc  # 前视距离*时间系数

#    delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
    delta = math.atan2(L * math.sin(alpha) / (Lf*Lf), 1.0)    #TODO:双舵轮转角进行控制

    return delta, ind
	
   # 搜索最临近车体的路点
def calc_target_index(state, cx, cy):

    dx = [state.x - icx for icx in cx]
    dy = [state.y - icy for icy in cy]
    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
    ind = d.index(min(d))
    L = 0.0

    Lf = k * state.v + Lfc

    while Lf > L and (ind + 1) < len(cx):
        dx = cx[ind + 1] - cx[ind]
        dy = cx[ind + 1] - cx[ind]
        L += math.sqrt(dx ** 2 + dy ** 2)
        ind += 1

    return ind
	
"""
三阶贝塞尔曲线
"""
def bezier_get(start_p, control1_p, control2_p, end_p, t_in):
# t为微分系数，0.01表示
    temp = 1.0 - t_in
    t = t_in
    cx = []
    cy = []
    
    while(t < 1.0):
        t = t_in + t
        temp = 1.0 - t
        pot_x = start_p[0]*temp*temp*temp + 3*control1_p[0]*t*temp*temp + 3*control2_p[0]*t*t*temp + end_p[0]*t*t*t
        cx.append(pot_x)
        
        pot_y = start_p[1]*temp*temp*temp + 3*control1_p[1]*t*temp*temp + 3*control2_p[1]*t*t*temp + end_p[1]*t*t*t
        cy.append(pot_y)

    return cx, cy
	
def main():
# n bezier path
    start_p = [1.0, 5.0]
    control1_p = [4.0, 8.0]
    control2_p = [7.0, 5.0]
    end_p = [11.0, 10.0]

    cx, cy = bezier_get(start_p, control1_p, control2_p, end_p, 0.01)

########### bezier ####################   
     
    target_speed = 1.5   # [m/s]

    T = 10.0  # 最大模拟时间[s]

    # 设置车辆的init状态
    state = VehicleState(x = start_p[0], y = start_p[1], yaw=0.5, v=0.0)

    lastIndex = len(cx) - 1
    time = 0.0
    x = [state.x]
    y = [state.y]
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]
    target_ind = calc_target_index(state, cx, cy)



    while T >= time and lastIndex > target_ind:
        ai = PControl(target_speed, state.v)
        di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
        state = update(state, ai, di)

        time = time + dt

        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)
        v.append(state.v)
        t.append(time)

        plt.cla()
        plt.plot(cx, cy, ".r", label="course")
        plt.plot(x, y, "-b", label="trajectory")
        plt.plot(cx[target_ind], cy[target_ind], "go", label="target")
        plt.axis("equal")
        plt.grid(True)
        plt.title("Speed[m/s]:" + str(state.v)[:4])
        plt.pause(0.001)    # odom 1000Hz
        
    plt.show()
    
    
if __name__ == '__main__':
    main()

注意点：
需要pip安装 numpy 、 matplotlib 等 ，可以直接按照我的另一篇博文推介的开源项目安装（强烈安利这个开源项目，机器人各方面的算法都有涉及）。

• 顺便放一个成品效果演示，工程化实现，用任务管理方式实现了直线、贝塞尔曲线、旋转、自然导航等子任务的顺序执行，任务指令使用json方式下达（代码不开放，有问题可以问）。
  

  双舵轮AGV纯跟踪算法仿真演示

• 参考链接：

1）无人驾驶之车辆控制（1）-- 纯跟踪（Pure Pursuit）算法与Stanley算法：
https://blog.csdn.net/zxxxxxxy/article/details/103665245
2）无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪：
https://blog.csdn.net/AdamShan/article/details/80555174
3）LaTex在线公式编辑器：
https://private.codecogs.com/eqnedit.php