无人驾驶-控制-纯跟踪_纯跟踪控制器c++

无人驾驶-控制-纯跟踪（Pure Pursuit）

一、模型详解

1.1 简介

（1）对于无人车来说，规划好的路径通常由一系列路径点构成，这些点包含空间位置信息、姿态信息、速度、加速度等。路径与轨迹区别在于，轨迹包含了时间信息；按照我的理解，路径跟踪只是跟踪一系列路径点，时间长短都没关系，只要跟踪上即可；轨迹跟踪同时包含速度跟踪，与时间序列有关。路径的生成可以通过RTK或IMU采集点，RTK采集的经纬度（经纬度再转XYZ）与IMU采集的IMU坐标系下的XYZ值，然后对这条由点组成的路径进行跟踪。

（2）目前主流的轨迹跟踪分为两类：

• 基于几何追踪的算法：纯跟踪(Pursuit)、预瞄点(Stanley)

• 基于模型预测的算法：LQR、MPC

1.2 自行车模型

在本节我们重点介绍一种广泛使用的基于几何追踪的方法——纯追踪法（Pure Pursuit），这种方法作为最基础的算法，广泛应用。

在学习纯跟踪算法前，先了解自行车模型，并掌握自行车模型下前轮转角与转弯半径的推导公式：

tan(δ) = L / R，其中δ为前轮转角，L为前后轮轴距，R为转弯半径

自行车模型详解: 自行车模型-阿克曼模型.

二、公式推导

2.1 车辆跟踪模型

（1）从自行车模型出发，纯跟踪算法以车后轴为切点, 车辆纵向车身为切线, 通过控制前轮转角，使车辆可以沿着一条经过目标路点的圆弧行驶，如下图所示：

纯跟踪算法要求： 控制车辆的后轴中心点过要追踪的点（gx,gy）

2.2 参数介绍

• R：车辆转弯半径

• α：后轴中心点与跟踪点连线与车辆纵轴夹角，当路点在车的左边时，α > 0 ；当路
  点在车的右边时，α<0；

• ld：后轴中心点与跟踪点的直线距离，也叫前视距离

• el：车辆当前姿态和目标路点在横向上的误差

• k：车辆曲率，1 / R

2.3 公式推导

（1）根据正弦定理

后轴中心点与跟踪点连线与车辆纵轴夹角为α，跟踪点与圆弧圆点与后轴中心点与圆弧圆点夹角为2α，可构成等腰三角形，另外两角为（PI / 2 - α），根据边角关系知：

（2）前轮转角δ

结合以上推导，得出纯追踪控制算法控制量表达式：

tan(δ) = L / R
R = Id / 2sin(α) -------------------->δ(t) = arctan(2Lsin(α) / ld)

（3）圆弧曲率

圆弧曲率可由下式推导：

k = 1 / R =
sin(α) = el / ld = ld / 2R ------------------------->1 / R = 2*el / ld * ld

（4）前视距离与速度相关

考虑到本质是横向上的CTE，由上式可知纯追踪控制器其实是一个横向转角的P控制器，这个P控制器受到前视距离的影响很大，如何调整前视距离变成纯追踪算法的关键，通常来说，被认为前视距离是车速的函数，在不同的车速下需要选择不同的前视距离。一种最常见的调整前视距离的方法就是将前视距离表示成车辆纵向速度的线形函数，即 l=kv，那么前轮的转角公式就变成：

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

（5）引入横向误差el

这里将时间考虑进来，在知道t时刻车身和目标路点的夹角 α(t)和距离目标路点的前视距离 ld的情况下，由于车辆轴距 L固定，我们可以利用上式估计出应该作出的前轮转角 δ ，为了更好的理解纯追踪控制器的原理，我们定义一个新的量：el 车辆当前姿态和目标路点在横向上的误差，由此可得夹角正弦：

将sin(α) = el / ld 代入到 δ = arctan( 2Lsin(α) / ld )

则可以推导出下式：

δ(k) = arctan( 2L*el(k) / ld(k) / ld(k) )

在pure pursuit方法中，前视距离表示成无人车纵向线速度的形式，即ld = λ vx + c ，c为一常值，最终的控制器如下：

利用小角度近似，δ == tan(δ)，则可将δ近似为：

我们把2L / ld^2 看作比例控制器的参数，ey作为系统误差，那么这就相当于一个以横向跟踪误差CTE作为系统误差的比例控制器，其中ld = λ*vx + c，在纯跟踪算法中需要调整 λ，c；

无人车模型的两个控制输入：无人车前轮转向角(δ)，有时候也用方向盘转角来表示；无人车线速度v，前者是影响无人车横向运动的主要控制输入，因此，设计的轨迹跟踪控制器，都是为了得到合适的δ。

横向跟踪误差: 横向跟踪误差定义.

三、代码分析

3.1 参数定义

在这个实践中，我们纯追踪控制控制转向角度，使用一个简单的P控制器控制速度，首先我们定义参数数值如下：

import numpy as np
import math
import matplotlib.pyplot as plt
k = 0.1  # 前视距离系数
Lfc = 2.0  # 前视距离
Kp = 1.0  # 速度P控制器系数
dt = 0.1  # 时间间隔，单位：s
L = 2.9  # 车辆轴距，单位：m
1
2
3
4
5
6
7
8

在这里我们将最小前视距离设置为2，前视距离关于车速的系数k设置为0.1 ，速度P控制器的比例系数Kp设置为1.0，时间间隔为0.1 秒，车的轴距我们定为2.9米。

3.2 定义车辆状态

定义车辆状态类，在简单的自行车模型中，我们只考虑车辆的当前位置(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
1
2
3
4
5
6
7
8
9
10
11
12

3.3 控制器定义

在这个实践中，我们纵向控制使用一个简单的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)
    if pind >= ind:
        ind = pind
    if ind < len(cx):
        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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

3.4 最近点搜索

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14

3.5 主函数

def main():
    #  设置目标路点
    cx = np.arange(0, 50, 1)
    cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]
    target_speed = 10.0 / 3.6  # [m/s]
    T = 100.0  # 最大模拟时间
    # 设置车辆的出事状态
    state = VehicleState(x=-0.0, y=-3.0, yaw=0.0, 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[km/h]:" + str(state.v * 3.6)[:4])
        plt.pause(0.001)
if __name__ == '__main__':
    main()
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

3.6 效果呈现

参考链接

1. https://blog.csdn.net/weixin_39549161/article/details/88712443
2. https://blog.csdn.net/weixin_47012067/article/details/121137189