无人驾驶运动控制----pure pursuit算法实践和理解

最新推荐文章于 2024-04-10 23:21:27 发布

liuzengyuan10

最新推荐文章于 2024-04-10 23:21:27 发布

阅读量8.4k

点赞数 16

文章标签：自动驾驶

本文链接：https://blog.csdn.net/Quentinmen/article/details/104359557

版权

0.导论

pure pursuit 方法是基于几何追踪的路径追踪方法，基于几何的控制方法较为简单和直接，不用考虑车辆的运动学模型和动力学模型，控制时使用的参数少，能够较好的运用到实践使用中。最常用的两种方法是pure pursuit方法和stanly方法。这里主要介绍pure pursuit 方法

1.算法原理和思想

算法的基本原理，AdamShan已经在他的博客《无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪》中讲的很清晰了，这里只是复述一下加深自己的理解。

pure pursuit建立在两个模型上，阿克曼转向几何模型和二维自行车模型。

参数说明：

δ：车辆的转向角；

L : 为车轴长度

R：转弯半径

K：是计算出来的圆弧的曲率

ld : 预瞄距离

α：目标点方向与当前航向的夹角；

(gx , gy )：目标点；

根据阿克曼转向几何关系，可以建立车辆前轮转向角和后轮遵循的曲率之间的关系，轮的偏向角δ，与后轮划过的圆弧的半径有以下关系：

根据二维自行车模型的几何关系推导：

由此，建立了转向角δ与前视距离ld ，车轴长度L，目标点方向与当前航向角度α之间的关系。

pure pursuit算法的基本思想是：参考人类驾驶员的行为，以车的后轴为基点，通过控制前轮的偏向角δ，使车辆沿一条经过预瞄点的圆弧行驶。

pure pursuit算法主要参数是前视距离ld，对算法的性能影响很大。现在采取的通用方法是令前视距离ld是与速度有关的参数，不同的速度对用不同的前视距离。

2.算法步骤

查看论文《implement of the pure pursuit path tracking algorithm》，论文给出了pure pursuit算法的的算法步骤是：

确定车辆的当前位置；
在规划出的路径中，找到离车最近的点；
找到目标点；
换算目标点坐标为车辆坐标系；
计算转向角δ并操纵车辆转向运动；
更新车辆状态

原文如下：

对各步骤的理解：

确定车辆的位置：通过gps，惯导等设备，确定车辆的位置，航向角等实时状态信息；
找到离车最近的位置：在前视距离范围内，可能会有多个数据目标路径的点，应当选取一个距离起点最接近前视距离的那个点，为了找到最满足要求的这个点，首先可以选取一个在路径上里此刻最近的点，以确定自己此刻在规划出的路径中的位置。
找到目标点：利用上一步获取的位置点，采取一定的计算方法，获取规划出的路径中，距离此点距离最接近前视距离的点，把这个点设为目标点。
利用pure pursuit算法的计算公式，计算出到达目标点所需的转向角δ。
根据单位时间内车辆的运动更新车辆的状态。

3.基于python的代码实现

参考大佬AdamShan的博客，用python对代码做客复现，加入了自己对代码各个部分的思考。

这里不考虑纵向的运动控制，沿用算法中的p控制器，实验中设置为匀速运动。

首先设定参数

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

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

最小前视距离设置为0.1，前视距离关于车速的系数k设置为0.1 ，速度P控制器的比例系数Kp设置为1.0，时间间隔为0.1 秒，车的轴距我们定为0.6米，符合我们实验用的小车情况。

定义车辆状态类，在简单的自行车模型中，我们只考虑车辆的当前位置 (x,y)(x,y) ,车辆的偏航角度yaw以及车辆的速度v，为了在软件上模拟，我们定义车辆的状态更新函数来模拟真实车辆的状态更新：

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.v = state.v + a * dt

    return state

定义函数用于搜索最临近的路点：

这个函数主要用在pure pursuit控制器中，

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))     # 找出最小的d所在的位置
    J = 0.0
    Lf = k * state.v + Lfc
    '''
    首先从目标点中找到一个离当前点最近的点
    然后计算离这个点距离满足前视距离的下一个点
    当两点之间的距离小于前视距离，需要累加几个点直至距离超过前视距离
      
    
    # search look ahead target point index
    # 解读：从path point 接下来中找到 离当前点最接近于 前视距离的一个点
    # 当路径中的下一个点离当前很远时，这里保证了目标点至少下移一个点，不会停留在原地  
    '''
    while Lf > J and (ind + 1) < len(cx):    
        dx = cx[ind + 1] - cx[ind]
        dy = cy[ind + 1] - cy[ind]
        J += math.sqrt(dx ** 2 + dy ** 2)   
        ind += 1
    return ind

定义纯追踪控制器：

def pure_pursuit_control(state, cx, cy, pind):   
    ind = calc_target_index(state, cx, cy)        #搜寻最近的点 ind
    '''
    # 每次运行 pure pursuit control时
    # 首选从路径中选择一个最近的点
    # 再从这个点展开，寻找一个长度为前视距离的点；
    # 然后计算 转向角δ；
    '''
    
    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)      #计算转向角
    return delta, ind

主函数：

与原文基本一致，修改了路径和速度，预设路径是一条螺旋线，速度设为2m/s为定值。

def main():
    #  设置目标路点
    cx = [0.76, 1.52, 2.28, 3.04, 3.81, 4.57, 5.33, 5.44, 5.64, 5.66, 5.33, 5.30, 4.39, 4.55, 5.33, 6.33, 6.90, 6.55, 5.33, 3.89, 3.13, 3.66, 5.33, 7.22, 8.16, 7.44, 5.33, 3.00]
    cy = [0.52, 1.04, 1.57, 2.09, 2.61, 3.14, 3.66, 3.77, 3.66, 3.33, 3.03, 3.03, 3.66, 4.44, 4.92, 4.66, 3.66, 2.44, 1.78, 2.22, 3.66, 5.33, 6.18, 5.55, 3.66, 1.55, 0.52, 1.33]
    

    target_speed = 2.0 / 3.6  # [m/s]

    T = 100.0  # 最大模拟时间

    # 设置车辆的出事状态
    state = VehicleState(x=-0.0, y=-3.0, yaw=0.0, v=2.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[km/h]:" + str(state.v * 3.6)[:4])
        plt.pause(0.001)

if __name__ == '__main__':
    main()

实验结果：红点表示事先规划好的路点，蓝线则表示我们的车辆实际运行的轨迹

结果分析：

可以看出，pure pursuit 算法对直线追踪效果还可以。在直线路径上，一般经过1-2个点的调整，就可以很好的跟踪直线。

在曲线路径，算法的原因，车在接近目标点时，哪怕以很慢的速度，但距离只要在预瞄距离以内，小车就更新预瞄点为下一个，这样导致的情况是，小车还没到达预瞄点（距离大概在预瞄距离处时，小车便更换了预瞄点，导致对曲线路径没法做到100%的跟踪效果。

（粗略）查看了一些论文后，大致归纳了算法的改进方向（待后续补充修改）：

1.在车辆运动过程中，动态地调整前视距离；

2.修改对转向角的计算方法（考虑道路曲率等因素在内，加强对曲线路径的追踪效果）。

4.参考资料：

博客：https://blog.csdn.net/AdamShan/article/details/80555174

GitHub代码库：https://github.com/gameinskysky/PythonRobotics/tree/master/PathTracking/pure_pursuit

参考论文：《implement of the pure pursuit path tracking algorithm》地址：http://www.doc88.com/p-4085136872925.html

liuzengyuan10

关注

16
点赞
踩
120

收藏

觉得还不错? 一键收藏
6
评论
无人驾驶运动控制----pure pursuit算法实践和理解

0.导论pure pursuit 方法是基于几何追踪的路径追踪方法，基于几何的控制方法较为简单和直接，不用考虑车辆的运动学模型和动力学模型，控制时使用的参数少，能够较好的运用到实践使用中。最常用的两种方法是pure pursuit方法和stanly方法。这里主要介绍pure pursuit 方法1.算法原理和思想算法的基本原理，AdamShan已经在他的博客《无人驾驶汽车系统入门...
复制链接

扫一扫