后轮位置反馈控制与算法仿真实现

艰默

发布于 2024-03-04 14:37:25

1330

发布于 2024-03-04 14:37:25

文章被收录于专栏：iDoitnowiDoitnow

1. 后轮反馈控制

后轮反馈控制（Rear wheel feedback）算法是利用后轮中心的跟踪偏差来进行转向控制量计算的方法，属于Frenet坐标系的一个应用。通过选择合适的李雅普诺夫函数设计控制率，利用后轮中心的跟踪偏差来进行转向控制量计算的方法。

2. 算法原理

后轮反馈控制算法原理如上图所示，其中

：当前距离车辆最近的路经点；

e_y

：

点与车辆后轮中心点的横向偏差

，实际上对应的就是frenet坐标下的

；

\varphi

：车辆朝向与

轴正方向的夹角，即航向角；

\varphi_{r}

：

点切线与

轴正方向的夹角；

\varphi_e

：车辆航向角误差，即

\varphi-\varphi_{r}

；

\vec{n_\tau}

：

点法线的单位向量；

\vec{\tau_r}

：

点切线的单位向量；

：轴距

\delta_f

：前轮转角

：车辆的速度

由前面的文章frenet坐标与cartesian坐标相互转换与代码实现和上图的几何关系可得车辆在参考轨迹上的投影点

处的线速度

\dot{s}

和横向误差

e_y

（即对应frenet坐标系下的

\dot{l}

）的表达式为

\dot{s}=\frac{\vec{v} \vec{\tau_r}}{1-k_rl} = \frac{|\vec{v}| |\vec{\tau_r}|cos{\varphi_e}}{1-k_re_y}=\frac{|\vec{v}|cos{\varphi_e}}{1-k_re_y} \tag{1}

\dot{e_y}=\dot{l}= \vec{v} \vec{n_r}= |\vec{v}| |\vec{n_r}| cos({\frac{\pi}{2}-\varphi_e}) = |\vec{v}| sin{\varphi_e} \tag{2}

由图中几何关系可得航向误差为

\varphi_e = \varphi-\varphi_{r} \tag{3}

则航向误差变化率为

\dot{\varphi_e} = \dot{\varphi}-\dot{\varphi_{r}} = \dot{\varphi}-\frac{\dot{s}}{R_r }= \dot{\varphi}-\dot{s}k_r \tag{4}

注：一个刚体的角速度 = 线速度/线速度到速度瞬心的距离，上式中

R_r

为点

处的瞬时圆心半径。

将(1)代入(4)可得

\dot{\varphi_e} = \dot{\varphi}-\dot{s}k_r = \dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}\tag{4}

综上可得后轮反馈控制算法对应模型的微分方程为

\begin{cases} \dot{s}&=\quad \frac{|\vec{v}|cos{\varphi_e}}{1-k_re_y} \\ \dot{e_y}&=\quad |\vec{v}| sin{\varphi_e}\\ \dot{\varphi_e} &=\quad \dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y} \end{cases} \tag{5}

有了模型的微分方程，我们可以根据李雅普诺夫第二方法设计该模型的车辆航向变化率

\dot{\varphi}

。

李雅普诺夫第二方法：稳定的系统能量总是不断被耗散的，李雅普诺夫通过定义一个标量函数

V(x)

（通常能代表广义能量）来分析稳定性。这种方法的避免了直接求解方程，也没有进行近似线性化，所以也一般称之为直接法。

V(x)

需满足：

V(x)=0

当且仅当

x =0

V(x)>0

当且仅当

x \neq0

\dot{V}(x) \leq0

当

x \neq0

则称系统在李雅普诺夫意义下是稳定的，特别的，若

x \neq0

时，有

\dot{V}(x)<0

，则系统是渐进稳定的。

这里我们比较关心的就是横向误差

e_y

和

\varphi_e

，期望他们随着时间的增长逐渐收敛于0，我们可以定义李雅普诺夫函数形式如下：

V(e_y,\varphi_e)=\frac{1}{2}e_y^2+\frac{1}{2k_e}\varphi_e^2 \tag{6}

其中

k_e>0

。

为了使

(e_y,\varphi_e)

在平衡点

(0,0)

处稳定，根据李雅普诺夫第二方法的稳定判据，

V(x)

需满足：

V(e_y,\varphi_e)=0

当且仅当

e_y=0,\varphi_e =0

V(e_y,\varphi_e)>0

当且仅当

e_y\neq0,\varphi_e\neq0

\dot{V}(e_y,\varphi_e) \leq0

当

e_y\neq0,\varphi_e\neq0

对于1、2两条，我们选取的李亚普函数

V(e_y,\varphi_e)

显然满足。所以满足李雅普诺夫第二方法的前两条。

对于

\dot{V}(e_y,\varphi_e)

我们结合(5)可得

\begin{aligned} \dot{V}(e_y,\varphi_e) &=e_y\dot{e_y}+\frac{1}{k_e}\varphi_e \dot{\varphi_e}\\ &=e_y|\vec{v}| sin{\varphi_e}+ \frac{1}{k_e}\varphi_e(\dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}) \end{aligned} \tag{7}

令

\dot{V}(e_y,\varphi_e)=0

，结合(7)式可得

\begin{aligned} e_y|\vec{v}| sin{\varphi_e}+ \frac{1}{k_e}\varphi_e(\dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y})&=0\\ k_ee_y|\vec{v}| \frac{sin{\varphi_e}}{\varphi_e}+ \dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}&=0 \end{aligned} \tag{8}

由上式可以得到零界航向变化率

\dot{\varphi_0}

等于

\dot{\varphi_0}=\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}-k_ee_y|\vec{v}| \frac{sin{\varphi_e}}{\varphi_e} \tag{9}

为了使

\dot{V}(e_y,\varphi_e)

满足要求3，我们可以设计一个调节函数

g(e_y,\varphi_e) = k_\varphi |\vec{v}|\varphi_e>0

，其中

k_{\varphi}>0

，我们可以将车辆航向的变化率

\dot{\varphi}

设置为

\dot{\varphi}=\dot{\varphi_0}-g(e_y,\varphi_e)=\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}-k_ee_y|\vec{v}| \frac{sin{\varphi_e}}{\varphi_e}-k_\varphi |\vec{v}|\varphi_e \tag{10}

将(10)代入(7)得

\begin{aligned} \dot{V}(e_y,\varphi_e) &=e_y|\vec{v}| sin{\varphi_e}+ \frac{1}{k_e}\varphi_e(\dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y})\\ &=e_y|\vec{v}| sin{\varphi_e}+ \frac{1}{k_e}\varphi_e(\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}-k_ee_y|\vec{v}| \frac{sin{\varphi_e}}{\varphi_e}-k_\varphi |\vec{v}|\varphi_e-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}) \\ &=e_y|\vec{v}| sin{\varphi_e}+\frac{1}{k_e}\varphi_e(-k_ee_y|\vec{v}| \frac{sin{\varphi_e}}{\varphi_e}-k_\varphi |\vec{v}|\varphi_e) \\ &=-\frac{k_\varphi}{k_e} |\vec{v}|\varphi_e^2 < 0 \end{aligned} \tag{11}

因此我们设计的公式(10)即车辆航向的变化率

\dot{\varphi}

满足李雅普诺夫的稳定性条件。

根据前面文章介绍过的车辆运动学模型下的几何关系

tan\delta_f=\frac{L}{R} \tag{12}

车辆的航向变化率与车速的转弯半径关系

\dot{\varphi}=v/R

，结合上式可得

\delta_f=arctan\frac{\dot{\varphi}L}{v} \tag{13}

3. 算法和仿真实现

rear_wheel_feedback.py

import math
import numpy as np

class C:
    # System config
    K_theta = 1.0
    K_e = 0.5


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 rear_wheel_feedback_control(vehicle, ref_path):

    theta_e, er, k, ind = calc_preparation(vehicle, ref_path)
    vr = vehicle.v

    dot_phi = vr * k * math.cos(theta_e) / (1.0 - k * er) - \
              C.K_theta * abs(vr) * theta_e - C.K_e * vr * math.sin(theta_e) * er / (theta_e + 1e-19)

    delta = math.atan2(vehicle.L * dot_phi, vr)

    return delta, ind, er



def calc_preparation(vehicle, ref_path):
    """
    计算角度误差theta_e、横向误差er、曲率rk和索引index
    """

    rx, ry, ref_yaw, ref_kappa = ref_path[:, 0], ref_path[:, 1], ref_path[:, 2], ref_path[:, 4]
    dx = [vehicle.x - icx for icx in rx]
    dy = [vehicle.y - icy for icy in ry]
    d = np.hypot(dx, dy)
    index = np.argmin(d)
    rk = ref_kappa[index]
    ryaw = ref_yaw[index]

    vec_nr = np.array([math.cos(ryaw + math.pi / 2.0),
                       math.sin(ryaw + math.pi / 2.0)])

    vec_target_2_rear = np.array([vehicle.x - rx[index],
                                  vehicle.y - ry[index]])

    er = np.dot(vec_target_2_rear, vec_nr)
    theta_e = normalize_angle(vehicle.yaw - ryaw)

    return theta_e, er, rk, index

kinematic_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')

path_generator.py

"""
路径轨迹生成器
"""

import math
import numpy as np

class Path:
    def __init__(self):
        self.ref_line = self.design_reference_line()
        self.ref_yaw = self.cal_yaw()
        self.ref_s = self.cal_accumulated_s()
        self.ref_kappa = self.cal_kappa()

    def design_reference_line(self):
        rx, ry = [], []
        step_curve = 0.005 * math.pi
        step_line = 0.25

        for ix in np.arange(5, 80, step_line):
            rx.append(ix)
            ry.append(60)
        cx, cy, cr = 80, 45, 15
        theta = np.arange(math.pi/2, -math.pi/2, -step_curve)
        for itheta in theta:
            rx.append(cx + cr * math.cos(itheta))
            ry.append(cy + cr * math.sin(itheta))

        for ix in np.arange(80, 15, -step_line):
            rx.append(ix)
            ry.append(30)

        cx, cy, cr = 15, 15, 15
        theta = np.arange(math.pi/2, math.pi * 1.5, step_curve)
        for itheta in theta:
            rx.append(cx + cr * math.cos(itheta))
            ry.append(cy + cr * math.sin(itheta))

        for ix in np.arange(15, 90, step_line):
            rx.append(ix)
            ry.append(0)

        return np.column_stack((rx, ry))

    def cal_yaw(self):
        yaw = []
        for i in range(len(self.ref_line)):
            if i == 0:
                yaw.append(math.atan2(self.ref_line[i + 1, 1] - self.ref_line[i, 1],
                                 self.ref_line[i + 1, 0] - self.ref_line[i, 0]))
            elif i == len(self.ref_line) - 1:
                yaw.append(math.atan2(self.ref_line[i, 1] - self.ref_line[i - 1, 1],
                                 self.ref_line[i, 0] - self.ref_line[i - 1, 0]))
            else:
                yaw.append(math.atan2(self.ref_line[i + 1, 1] - self.ref_line[i -1, 1],
                                  self.ref_line[i + 1, 0] - self.ref_line[i - 1, 0]))
        return yaw

    def cal_accumulated_s(self):
        s = []
        for i in range(len(self.ref_line)):
            if i == 0:
                s.append(0.0)
            else:
                s.append(math.sqrt((self.ref_line[i, 0] - self.ref_line[i-1, 0]) ** 2
                                 + (self.ref_line[i, 1] - self.ref_line[i-1, 1]) ** 2))
        return s

    def cal_kappa(self):
        # 计算曲线各点的切向量
        dp = np.gradient(self.ref_line.T, axis=1)
        # 计算曲线各点的二阶导数
        d2p = np.gradient(dp, axis=1)
        # 计算曲率
        kappa = (d2p[0] * dp[1] - d2p[1] * dp[0]) / ((dp[0] ** 2 + dp[1] ** 2) ** (3 / 2))

        return kappa

    def get_ref_line_info(self):
        return self.ref_line[:, 0], self.ref_line[:, 1], self.ref_yaw, self.ref_s, self.ref_kappa

main.py

from kinematic_bicycle_model import Vehicle, VehicleInfo, draw_trailer
from rear_wheel_feedback import rear_wheel_feedback_control
from path_generator import Path
import numpy as np
import matplotlib.pyplot as plt
import imageio.v2 as imageio

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

def main():
    # 设置跟踪轨迹
    rx, ry, ref_yaw, ref_s, ref_kappa = Path().get_ref_line_info()
    ref_path = np.column_stack((rx, ry, ref_yaw, ref_s, ref_kappa))
    # 假设车辆初始位置为（5，55），航向角yaw=pi/6，速度为2m/s，时间周期dt为0.1秒
    vehicle = Vehicle(x=5.0,
                      y=55.0,
                      yaw=np.pi/6,
                      v=2.0,
                      dt=0.1,
                      l=VehicleInfo.L)

    time = 0.0  # 初始时间
    target_ind = 0
    # 记录车辆轨迹
    trajectory_x = []
    trajectory_y = []
    lat_err = []  # 记录横向误差

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

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

        # rear_wheel_feedback
        delta_f, target_ind, e_y = rear_wheel_feedback_control(vehicle, ref_path)

        # 横向误差
        lat_err.append(e_y)

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

        # 显示动图
        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 % 2) == 0:
    #         image_list.append(imageio.imread("temp.png"))
    #
    # imageio.mimsave("display.gif", image_list, duration=0.01)

    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()

运行效果：