Stanley横向控制与算法仿真实现

1. Stanley

Stanley横向控制就是我们常说的也叫做前轮反馈控制(Front wheel feedback)，是一种基于横向跟踪误差的非线性反馈控制算法，其核心思想是根据车辆位姿与给定路径的相对几何关系来控制车辆方向盘转角。具体来说，Stanley横向控制算法将车辆的横向跟踪误差和航向跟踪误差作为反馈信号，通过非线性比例函数计算出前轮转向角，以减小横向跟踪误差并提高车辆的横向跟踪性能。

2. 算法原理

Stanley算法原理如上图所示，其中

• $P$：当前距离车辆最近的路经点
• $C$：前轮朝向与$P$点切线交点
• $e_y$：$P$点与车辆前轮中心点的横向偏差
• $l_d$：点$C$到$P$的距离
• ${\delta }_{\theta }$：切线$PC$与车轴朝向的夹角
• ${\delta }_e$：切线$PC$与前轮朝向的夹角
• $\phi$：车辆的航向角
• $L$：轴距
• ${\delta }_f$：前轮转角
• $\alpha$：切线$PC$与$X$轴的夹角

由上图的几何关系我们很容易得到前轮转角的${\delta }_f$计算公式
$${\delta }_f={\delta }_e+{\delta }_{\theta }\text{\hspace{1em}(1)}$$
这里${\delta }_e$可以理解为为了消除车辆航向与参考航向之间得角度差所需得前轮转角，而${\delta }_{\theta }$是为了消除横向距离误差$e_y$需要得前轮转角。

对于${\delta }_{\theta }$的计算，由几何关系很容易得到
$${\delta }_{\theta }=\alpha ?\phi \text{\hspace{1em}(2)}$$
对于${\delta }_e$，由几何关系可得
$${\delta }_e=arctan\frac{e_y}{l_d}\text{\hspace{1em}(3)}$$
当$e_y$在一定范围内时，如果将$l_d$设置为常量的话，若$l_d$设置的越大，转向角就越小，控制会更平滑，但车子就越慢接近参考路径，$l_d$越小，转向角就越大，车子就越快接近参考路径，但有时候也会引起震荡。与纯跟踪的预瞄距离类似，我们希望$l_d$的大小能根据车辆的速度来进行动态的调整，因此，在实际应用中，我们一般会根据车辆速度$v$来给定$l_d$一个值，一般情况下他们的关系为
$$l_d=\frac{v}{k}\text{\hspace{1em}(4)}$$
由公式(3)和公式(4)可得
$${\delta }_e=arctan\frac{k{e}_y}{v}\text{\hspace{1em}(5)}$$
由公式(1)、(2)和(5)可得
$${\delta }_f=\alpha ?\phi +arctan\frac{k{e}_y}{v}\text{\hspace{1em}(6)}$$

3. 算法和仿真实现

stanley.py

import numpy as np
import math

k = 0.5  # 比例系数


def stanley_control(vehicle, cx, cy, cyaw, last_target_idx):

    current_target_idx, error_front_axle = calc_target_index(vehicle, cx, cy)

    if last_target_idx >= current_target_idx:
        current_target_idx = last_target_idx

    # 车辆航向与参考航向之间得角度差
    theta_f = normalize_angle(cyaw[current_target_idx] - vehicle.yaw)

    # 横向距离误差
    theta_e = np.arctan2(k * error_front_axle, vehicle.v)
    delta = normalize_angle(theta_e + theta_f)

    return delta, current_target_idx


def normalize_angle(angle):
    a = math.fmod(angle + np.pi, 2 * np.pi)
    if a < 0.0:
        a += (2.0 * np.pi)
    return a - np.pi


def calc_target_index(vehicle, cx, cy):

    # 计算前轮位置
    fx = vehicle.x + vehicle.L * np.cos(vehicle.yaw)
    fy = vehicle.y + vehicle.L * np.sin(vehicle.yaw)

    # 寻找距离前轮最近的轨迹点
    dx = [fx - icx for icx in cx]
    dy = [fy - icy for icy in cy]
    d = np.hypot(dx, dy)
    target_idx = np.argmin(d)

    front_axle_vec_rot_90 = np.array([math.cos(vehicle.yaw - math.pi / 2.0),
                                      math.sin(vehicle.yaw - math.pi / 2.0)])
    error_front_axle = np.dot([dx[target_idx], dy[target_idx]], front_axle_vec_rot_90)

    return target_idx, error_front_axle

bicycle_model.py

import math
import numpy as np

class Vehicle:
    def __init__(self,
                 x=0.0,
                 y=0.0,
                 yaw=0.0,
                 v=0.0,
                 dt=0.1,
                 l=3.0):
        self.steer = 0
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.dt = dt
        self.L = l  # 轴距
        self.x_front = x + l * math.cos(yaw)
        self.y_front = y + l * math.sin(yaw)

    def update(self, a, delta, max_steer=np.pi):
        delta = np.clip(delta, -max_steer, max_steer)
        self.steer = delta

        self.x = self.x + self.v * math.cos(self.yaw) * self.dt
        self.y = self.y + self.v * math.sin(self.yaw) * self.dt
        self.yaw = self.yaw + self.v / self.L * math.tan(delta) * self.dt

        self.v = self.v + a * self.dt
        self.x_front = self.x + self.L * math.cos(self.yaw)
        self.y_front = self.y + self.L * math.sin(self.yaw)


class VehicleInfo:
    # Vehicle parameter
    L = 3.0  #轴距
    W = 2.0  #宽度
    LF = 3.8  #后轴中心到车头距离
    LB = 0.8  #后轴中心到车尾距离
    MAX_STEER = 0.6  # 最大前轮转角
    TR = 0.5  # 轮子半径
    TW = 0.5  # 轮子宽度
    WD = W  #轮距
    LENGTH = LB + LF  # 车辆长度

def draw_trailer(x, y, yaw, steer, ax, vehicle_info=VehicleInfo, color='black'):
    vehicle_outline = np.array(
        [[-vehicle_info.LB, vehicle_info.LF, vehicle_info.LF, -vehicle_info.LB, -vehicle_info.LB],
         [vehicle_info.W / 2, vehicle_info.W / 2, -vehicle_info.W / 2, -vehicle_info.W / 2, vehicle_info.W / 2]])

    wheel = np.array([[-vehicle_info.TR, vehicle_info.TR, vehicle_info.TR, -vehicle_info.TR, -vehicle_info.TR],
                      [vehicle_info.TW / 2, vehicle_info.TW / 2, -vehicle_info.TW / 2, -vehicle_info.TW / 2, vehicle_info.TW / 2]])

    rr_wheel = wheel.copy() #右后轮
    rl_wheel = wheel.copy() #左后轮
    fr_wheel = wheel.copy() #右前轮
    fl_wheel = wheel.copy() #左前轮
    rr_wheel[1,:] += vehicle_info.WD/2
    rl_wheel[1,:] -= vehicle_info.WD/2

    #方向盘旋转
    rot1 = np.array([[np.cos(steer), -np.sin(steer)],
                     [np.sin(steer), np.cos(steer)]])
    #yaw旋转矩阵
    rot2 = np.array([[np.cos(yaw), -np.sin(yaw)],
                     [np.sin(yaw), np.cos(yaw)]])
    fr_wheel = np.dot(rot1, fr_wheel)
    fl_wheel = np.dot(rot1, fl_wheel)
    fr_wheel += np.array([[vehicle_info.L], [-vehicle_info.WD / 2]])
    fl_wheel += np.array([[vehicle_info.L], [vehicle_info.WD / 2]])

    fr_wheel = np.dot(rot2, fr_wheel)
    fr_wheel[0, :] += x
    fr_wheel[1, :] += y
    fl_wheel = np.dot(rot2, fl_wheel)
    fl_wheel[0, :] += x
    fl_wheel[1, :] += y
    rr_wheel = np.dot(rot2, rr_wheel)
    rr_wheel[0, :] += x
    rr_wheel[1, :] += y
    rl_wheel = np.dot(rot2, rl_wheel)
    rl_wheel[0, :] += x
    rl_wheel[1, :] += y
    vehicle_outline = np.dot(rot2, vehicle_outline)
    vehicle_outline[0, :] += x
    vehicle_outline[1, :] += y

    ax.plot(fr_wheel[0, :], fr_wheel[1, :], color)
    ax.plot(rr_wheel[0, :], rr_wheel[1, :], color)
    ax.plot(fl_wheel[0, :], fl_wheel[1, :], color)
    ax.plot(rl_wheel[0, :], rl_wheel[1, :], color)

    ax.plot(vehicle_outline[0, :], vehicle_outline[1, :], color)
    ax.axis('equal')

main.py

from bicycle_model import Vehicle, VehicleInfo, draw_trailer
from stanley import stanley_control, calc_target_index, normalize_angle
import numpy as np
import matplotlib.pyplot as plt
import math
import imageio

MAX_SIMULATION_TIME = 200.0  # 程序最大运行时间200*dt


def cal_phi(ref_path, i):
    if i == len(ref_path) - 1:
        return math.atan2(ref_path[i, 1] - ref_path[i - 1, 1], ref_path[i, 0] - ref_path[i - 1, 0])
    else:
        return math.atan2(ref_path[i + 1, 1] - ref_path[i, 1], ref_path[i + 1, 0] - ref_path[i, 0])


def main():
    # 设置跟踪轨迹
    ref_path = np.zeros((1000, 3))
    ref_path[:, 0] = np.linspace(0, 50, 1000)  # x坐标
    ref_path[:, 1] = 10 * np.sin(ref_path[:, 0] / 20.0)  # y坐标
    ref_path[:, 2] = [cal_phi(ref_path, i) for i in range(len(ref_path))]  # 参考轨迹上点的切线方向的角度,近似计算

    # 假设车辆初始位置为（0，0），航向角yaw=pi/2，速度为2m/s，时间周期dt为0.1秒
    vehicle = Vehicle(x=0.0,
                      y=0.0,
                      yaw=np.pi / 2,
                      v=2.0,
                      dt=0.1,
                      l=VehicleInfo.L)

    target_ind, error_front_axle = calc_target_index(vehicle, ref_path[:, 0], ref_path[:, 1])
    time = 0.0  # 初始时间

    # 记录车辆轨迹
    trajectory_x = []
    trajectory_y = []
    lat_err = []  # 记录横向误差

    i = 0
    image_list = []  # 存储图片
    plt.figure(1)

    last_idx = ref_path.shape[0] - 2  # 跟踪轨迹的最后一个点的索引
    while MAX_SIMULATION_TIME >= time and last_idx > target_ind:
        time += vehicle.dt  # 累加一次时间周期

        # stanley
        delta_f, target_ind = stanley_control(vehicle, ref_path[:, 0], ref_path[:, 1], ref_path[:, 2], target_ind)

        # 横向误差计算
        nearest_index, e_y = calc_target_index(vehicle, ref_path[:, 0], ref_path[:, 1])
        lat_err.append(e_y)

        # 更新车辆状态
        vehicle.update(0.0, delta_f, np.pi / 10)  # 由于假设纵向匀速运动，所以加速度a=0.0
        trajectory_x.append(vehicle.x + vehicle.L * np.cos(vehicle.yaw))
        trajectory_y.append(vehicle.y + vehicle.L * np.sin(vehicle.yaw))

        # 显示动图
        plt.cla()
        plt.plot(ref_path[:, 0], ref_path[:, 1], '-.b', linewidth=1.0)
        draw_trailer(vehicle.x, vehicle.y, vehicle.yaw, vehicle.steer, plt)

        plt.plot(trajectory_x, trajectory_y, "-r", label="trajectory")
        plt.plot(ref_path[target_ind, 0], ref_path[target_ind, 1], "go", label="target")
        plt.axis("equal")
        plt.grid(True)
        plt.pause(0.001)
        plt.savefig("temp.png")
        i += 1
    #     if (i % 50) > 0:
    #         image_list.append(imageio.imread("temp.png"))
    #
    # imageio.mimsave("display.gif", image_list, duration=0.1)

    plt.figure(2)
    plt.subplot(2, 1, 1)
    plt.plot(ref_path[:, 0], ref_path[:, 1], '-.b', linewidth=1.0)
    plt.plot(trajectory_x, trajectory_y, 'r')
    plt.title("actual tracking effect")

    plt.subplot(2, 1, 2)
    plt.plot(lat_err)
    plt.title("lateral error")
    plt.show()


if __name__ == '__main__':
    main()

运行效果：

控制效果和横向误差：

文章首发公众号：iDoitnow如果喜欢话，可以关注一下