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

 

0.导论

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

1.算法原理和思想

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

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

参数说明:

δ: 车辆的转向角;

 L :   为车轴长度

 R:  转弯半径

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

ld :   预瞄距离

α:  目标点方向与当前航向的夹角;

(gx , gy ):目标点;

无人驾驶运动控制----pure pursuit算法实践和理解_第1张图片 阿克曼转向模型

 

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

无人驾驶运动控制----pure pursuit算法实践和理解_第2张图片

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

无人驾驶运动控制----pure pursuit算法实践和理解_第3张图片 二维运动学模型

 

无人驾驶运动控制----pure pursuit算法实践和理解_第4张图片

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

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

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

2.算法步骤

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

  1. 确定车辆的当前位置;
  2. 在规划出的路径中,找到离车最近的点;
  3. 找到目标点;
  4. 换算目标点坐标为车辆坐标系;
  5. 计算转向角δ并操纵车辆转向运动;
  6. 更新车辆状态

原文如下:

无人驾驶运动控制----pure pursuit算法实践和理解_第5张图片

对各步骤的理解:

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

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算法实践和理解_第6张图片

结果分析:

可以看出,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

你可能感兴趣的:(无人驾驶运动控制----pure pursuit算法实践和理解)