后轴反馈控制——frenet坐标的一个应用

0. 参考文献

前置
【1】https://blog.csdn.net/l898985121/article/details/109048855
这个是之前写的一篇文章,讲了frenet坐标的推导过程
【2】https://blog.csdn.net/l898985121/article/details/115702160
前面写的关于李雅普诺夫稳定的一篇转载文章
参考
【3】https://zhuanlan.zhihu.com/p/46377932
【3.1】https://arxiv.org/abs/1604.07446
这个是横向控制综述,看里面的后轴反馈方法
【4】https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
这个是后轴反馈控制的python代码

1. 简单推导

1.1 frenet 坐标

长成这个样子:
在这里插入图片描述
从这个当中,我们选择后面两行,构成一个非线性的系统方程,系统的状态量是横向偏差 e e e和航向角偏差 ψ e \psi_e ψe。我们可以控制的输入是 ω \omega ω
当然也可以选择其他的状态量当做系统方程,这个在后文答案二中讨论

1.2 笔者的思路

然后,采用李雅普诺夫第二方法,寻找一个状态量的方程 V ( x ) V(\pmb{x}) V(xxx),也就是 V ( e , ψ e ) V(e,\psi_e) V(e,ψe),满足三个条件,就能得到一个渐进稳定的系统,为了满足这三个条件,我们需要设计 ω \omega ω

我们选择的一个李雅普诺夫函数如下:
V ( e , ψ e ) = e 2 + ψ e 2 V(e,\psi_e) = e^2 + \psi_e^2 V(e,ψe)=e2+ψe2
求导得到:
V ˙ = 2 e e ˙ + 2 ψ e ψ ˙ e \dot{V}=2e\dot{e}+2 \psi_e \dot{\psi}_e V˙=2ee˙+2ψeψ˙e
将上面的状态方程带入,得到:
V ˙ = 2 e v r sin ⁡ ( ψ e ) + 2 ψ e ( ω − v r κ cos ⁡ ( ψ e ) 1 − κ e ) \dot{V}=2 e v_r \sin (\psi_e) + 2 \psi_e (\omega - \frac{v_r \kappa \cos (\psi_e)}{1- \kappa e}) V˙=2evrsin(ψe)+2ψe(ω1κevrκcos(ψe))
观察上面这个式子,如何设计一个 ω \omega ω,使得这个式子小于0(系统渐进稳定)。

后续:这里其实确实可以瞎J设计,只要稳定就行,然后通过添加奇怪的系数k,可以进一步控制系统收敛的过程。

下面让我们先看几个答案

1.3 答案一

来自《A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles》这篇文章
选择李雅普诺夫函数为:
V ( e , ψ e ) = e 2 + ψ e 2 k e V(e,\psi_e) = e^2 + \frac{\psi_e^2}{k_e} V(e,ψe)=e2+keψe2
求导得到:
V ˙ = 2 e e ˙ + 2 ψ e ψ ˙ e k e \dot{V}=2e\dot{e}+\frac{2 \psi_e \dot{\psi}_e}{k_e} V˙=2ee˙+ke2ψeψ˙e
将frenet坐标代入得到:
V ˙ = 2 e v r sin ⁡ ( ψ e ) + 2 ψ e k e ( ω − v r κ cos ⁡ ( ψ e ) 1 − κ e ) \dot{V}=2 e v_r \sin (\psi_e) + \frac{2 \psi_e}{k_e} (\omega - \frac{v_r \kappa \cos (\psi_e)}{1- \kappa e}) V˙=2evrsin(ψe)+ke2ψe(ω1κevrκcos(ψe))
同样思路,设计一个 ω \omega ω,使得上面的 V ˙ \dot{V} V˙小于0。

论文中设计好了一个,我们看一下:
ω = v r κ cos ⁡ ( ψ e ) 1 − κ e − ( k t ∥ v r ∥ ) ψ e − ( k e v r sin ⁡ ( ψ e ) ψ e ) e \omega = \frac{v_r \kappa \cos (\psi_e)}{1- \kappa e} - (k_t\|v_r\|)\psi_e-(k_ev_r\frac{\sin(\psi_e)}{\psi_e})e ω=1κevrκcos(ψe)(ktvr)ψe(kevrψesin(ψe))e
将这个 ω \omega ω代入到上面的 V ˙ \dot{V} V˙,消去分数项,有:
V ˙ = 2 e v r sin ⁡ ( ψ e ) + 2 ψ e k e ( − ( k t ∥ v r ∥ ) ψ e − ( k e v r sin ⁡ ( ψ e ) ψ e ) e ) \dot{V}=2 e v_r \sin (\psi_e) +\frac{2 \psi_e}{k_e}(- (k_t\|v_r\|)\psi_e-(k_ev_r\frac{\sin(\psi_e)}{\psi_e})e) V˙=2evrsin(ψe)+ke2ψe((ktvr)ψe(kevrψesin(ψe))e)
化简可得:
V ˙ = − k θ ∥ v r ∥ ψ e 2 \dot{V}=-k_\theta \|v_r\| \psi_e^2 V˙=kθvrψe2
这个在有偏差时,小于0。
说明选择的 ω \omega ω能让系统李雅普诺夫稳定

1.4 答案二

我们看frenet坐标的第二个式子
e ˙ = v r sin ⁡ ( ψ e ) \dot{e}=v_r \sin( \psi_e) e˙=vrsin(ψe)
求导:
e ¨ = v r cos ⁡ ( ψ e ) ψ e ˙ \ddot{e}=v_r \cos(\psi_e) \dot{\psi_e} e¨=vrcos(ψe)ψe˙
将frenet坐标第三行代入上式,有:
e ¨ = v r cos ⁡ ( ψ e ) ( ω − v r κ cos ⁡ ( ψ e ) 1 − κ e ) \ddot{e}=v_r \cos(\psi_e)( \omega - \frac{v_r \kappa \cos (\psi_e)}{1- \kappa e}) e¨=vrcos(ψe)(ω1κevrκcos(ψe))
我们分析如下系统方程:
e ˙ = v r sin ⁡ ( ψ e ) e ¨ = v r cos ⁡ ( ψ e ) ( ω − v r κ cos ⁡ ( ψ e ) 1 − κ e ) \begin{aligned} \dot{e} &=v_r \sin( \psi_e) \\ \ddot{e} &=v_r \cos(\psi_e) ( \omega - \frac{v_r \kappa \cos (\psi_e)}{1- \kappa e}) \end{aligned} e˙e¨=vrsin(ψe)=vrcos(ψe)(ω1κevrκcos(ψe))
构造李雅普诺夫函数:
V = k 0 e 2 + e ˙ 2 V=k_0 e^2+ \dot{e}^2 V=k0e2+e˙2
求导:
V ˙ = 2 k 0 e e ˙ + 2 e ˙ e ¨ \dot{V}= 2k_0e \dot{e} + 2 \dot{e} \ddot{e} V˙=2k0ee˙+2e˙e¨
将状态方程代入,得到:
V ˙ = 2 k 0 e v r sin ⁡ ( ψ e ) + 2 v r sin ⁡ ( ψ e ) v r cos ⁡ ( ψ e ) ( ω − v r κ cos ⁡ ( ψ e ) 1 − κ e ) \dot{V} = 2 k_0 ev_r\sin(\psi_e) + 2v_r\sin(\psi_e) v_r \cos(\psi_e) (\omega - \frac{v_r \kappa \cos (\psi_e)}{1- \kappa e}) V˙=2k0evrsin(ψe)+2vrsin(ψe)vrcos(ψe)(ω1κevrκcos(ψe))
设计一个 ω \omega ω使得 V ˙ \dot{V} V˙小于0,

我们先来看一个神奇设计方法,设计 ω \omega ω如下。
ω = η v r cos ⁡ ( ψ e ) + v r κ cos ⁡ ( ψ e ) 1 − κ e = − k 0 e − k 1 e ˙ v r cos ⁡ ( ψ e ) + v r κ cos ⁡ ( ψ e ) 1 − κ e \begin{aligned} \omega &= \frac{\eta}{v_r\cos(\psi_e)} +\frac{v_r \kappa \cos (\psi_e)}{1- \kappa e} \\ &= \frac{-k_0e - k_1 \dot{e}}{v_r\cos(\psi_e)}+\frac{v_r \kappa \cos (\psi_e)}{1- \kappa e} \end{aligned} ω=vrcos(ψe)η+1κevrκcos(ψe)=vrcos(ψe)k0ek1e˙+1κevrκcos(ψe)

把设计好的 ω \omega ω代入上式 V ˙ \dot{V} V˙,可得:
V ˙ = 2 v r sin ⁡ ( ψ e ) ( − k 1 e ˙ ) \dot{V}=2v_r\sin(\psi_e)(-k_1 \dot{e}) V˙=2vrsin(ψe)(k1e˙)
下面这段注意分析错了,上面这个公式还差最后一步化简
不一定<0,说明不是这个李雅普诺夫函数下的稳定,但不表示不稳定。
V ˙ = 2 v r sin ⁡ ( ψ e ) ( − k 1 e ˙ ) = 2 v r sin ⁡ ( ψ e ) ( − k 1 v r sin ⁡ ( ψ e ) ) \dot{V}=2v_r\sin(\psi_e)(-k_1 \dot{e})=2v_r\sin(\psi_e)(-k_1v_r\sin(\psi_e)) V˙=2vrsin(ψe)(k1e˙)=2vrsin(ψe)(k1vrsin(ψe))
也是小于零的,所以李雅普诺夫稳定。

1.5 总结

从上面两个答案可以看出:
(1)选择不同的系统方程,和最终不同的 V ˙ \dot{V} V˙的形式,可以得到不同的 ω \omega ω设计形式,这些不同的形式都可以使系统李雅普诺夫稳定。
(2)得到 ω d \omega_d ωd后,再考虑如何通过车辆方向盘转角进行控制,使得车辆实际的 ω \omega ω趋近于 ω d \omega_d ωd,可以选择前馈加反馈的形式,前馈就是那个artctan(wL/R)之类的式子,反馈就是pid就行。如下图:
在这里插入图片描述

(3)可以看出上面的式子是运动学下的模型,在高速下,理论上也应该能成立,也就是说,在高速下可以用同样的方式求出 ω d \omega_d ωd,并且如果车辆能以 ω d \omega_d ωd进行运动,一定可以按照设计的方式进行收敛;
但通常情况下,高速会出现误差,也就是说是车辆的实际 ω \omega ω控制的有问题,可以从后面开始考虑。

2. 代码注释

这里看的python robotics中的rear wheel feedback control部分的代码

"""
Path tracking simulation with rear wheel feedback steering control and PID speed control.
author: Atsushi Sakai(@Atsushi_twi)
"""
# 这里是import一些库
import matplotlib.pyplot as plt
import math
import numpy as np

from scipy import interpolate
from scipy import optimize

Kp = 1.0  # speed propotional gain
# steering control parameter
KTH = 1.0 # 这个就是k_theta
KE = 0.5 # 这个就是k_e

dt = 0.1  # [s]
L = 2.9  # [m]

show_animation = True

class State:
    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, direction=1):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.direction = direction

    def update(self, a, delta, dt):
        self.x   = self.x + self.v * math.cos(self.yaw) * dt
        self.y   = self.y + self.v * math.sin(self.yaw) * dt
        self.yaw = self.yaw + self.v / L * math.tan(delta) * dt
        self.v   = self.v + a * dt

class CubicSplinePath:
    def __init__(self, x, y):
        x, y = map(np.asarray, (x, y))
        s = np.append([0],(np.cumsum(np.diff(x)**2) + np.cumsum(np.diff(y)**2))**0.5)

        self.X = interpolate.CubicSpline(s, x)
        self.Y = interpolate.CubicSpline(s, y)

        self.dX = self.X.derivative(1)
        self.ddX = self.X.derivative(2)

        self.dY = self.Y.derivative(1)
        self.ddY = self.Y.derivative(2)

        self.length = s[-1]
    
    def calc_yaw(self, s):
        dx, dy = self.dX(s), self.dY(s)
        return np.arctan2(dy, dx)
    
    def calc_curvature(self, s):
        dx, dy   = self.dX(s), self.dY(s)
        ddx, ddy   = self.ddX(s), self.ddY(s)
        return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
    
    def __find_nearest_point(self, s0, x, y):
        def calc_distance(_s, *args):
            _x, _y= self.X(_s), self.Y(_s)
            return (_x - args[0])**2 + (_y - args[1])**2
        
        def calc_distance_jacobian(_s, *args):
            _x, _y = self.X(_s), self.Y(_s)
            _dx, _dy = self.dX(_s), self.dY(_s)
            return 2*_dx*(_x - args[0])+2*_dy*(_y-args[1])

        minimum = optimize.fmin_cg(calc_distance, s0, calc_distance_jacobian, args=(x, y), full_output=True, disp=False)
        return minimum

	# 这个是计算跟踪的误差
    def calc_track_error(self, x, y, s0): # 输入是车辆的坐标和航向角
    	# 先找到最近点
        ret = self.__find_nearest_point(s0, x, y)
        
        s = ret[0][0]
        e = ret[1]

        k   = self.calc_curvature(s)
        yaw = self.calc_yaw(s)

        dxl = self.X(s) - x
        dyl = self.Y(s) - y
        angle = pi_2_pi(yaw - math.atan2(dyl, dxl))
        if angle < 0:
            e*= -1

        return e, k, yaw, s
# 简单的pid控制
def pid_control(target, current):
    a = Kp * (target - current)
    return a

def pi_2_pi(angle):
    while(angle > math.pi):
        angle = angle - 2.0 * math.pi

    while(angle < -math.pi):
        angle = angle + 2.0 * math.pi

    return angle

def rear_wheel_feedback_control(state, e, k, yaw_ref):
    v = state.v
    th_e = pi_2_pi(state.yaw - yaw_ref)

	# 就是这儿,直接按照公式进行计算,原封不动的计算,算出omega就行
    omega = v * k * math.cos(th_e) / (1.0 - k * e) - \
        KTH * abs(v) * th_e - KE * v * math.sin(th_e) * e / th_e

    if th_e == 0.0 or omega == 0.0:
        return 0.0

    delta = math.atan2(L * omega / v, 1.0)

    return delta

# 这里是仿真函数
def simulate(path_ref, goal):
    T = 500.0  # max simulation time 最大仿真时间
    goal_dis = 0.3 # 这个是啥

    state = State(x=-0.0, y=-0.0, yaw=0.0, v=0.0) # 这个应该是个初始状态
	
	# 初始时间是零
    time = 0.0
    x = [state.x] # 初始的x
    y = [state.y] # 初始的y
    yaw = [state.yaw] # 初始的航向角,沿着x轴方向
    v = [state.v] # 初始的车速 = 0
    t = [0.0] # 构造一个列表
    goal_flag = False # 是否到达终点?

    s = np.arange(0, path_ref.length, 0.1) # 用0.1的间隔进行离散,在main里面离散过了,但那个是用来显示
    # 这个用来计算初始情况下的一个error
    e, k, yaw_ref, s0 = path_ref.calc_track_error(state.x, state.y, 0.0)

	# 开始循环
    while T >= time:
    
     	# 在每个循环的开始处计算一下error
        e, k, yaw_ref, s0 = path_ref.calc_track_error(state.x, state.y, s0)

		# 然后计算一下控制量?这里是横向
        di = rear_wheel_feedback_control(state, e, k, yaw_ref)

		# 计算一下参考速度
        speed_ref = calc_target_speed(state, yaw_ref)
        # 这里是纵向
        ai = pid_control(speed_ref, state.v)
        # 更新一下状态,这个位置其实就是仿真的车辆模型,用了一个dr进行积分
        state.update(ai, di, dt)
		# 每个时间间隔进行更新
        time = time + dt

        # check goal 检查是否到达目标位置
        dx = state.x - goal[0]
        dy = state.y - goal[1]
        # 如果到达了就退出,并把flag 设置一下
        if math.hypot(dx, dy) <= goal_dis:
            print("Goal")
            goal_flag = True
            break
		
		# 进行一下记录
        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)
        v.append(state.v)
        t.append(time)
		
		# 显示
        if show_animation:
            plt.cla()
            # for stopping simulation with the esc key.
            plt.gcf().canvas.mpl_connect('key_release_event',
                    lambda event: [exit(0) if event.key == 'escape' else None])
            plt.plot(path_ref.X(s), path_ref.Y(s), "-r", label="course")
            plt.plot(x, y, "ob", label="trajectory")
            plt.plot(path_ref.X(s0), path_ref.Y(s0), "xg", label="target")
            plt.axis("equal")
            plt.grid(True)
            plt.title("speed[km/h]:{:.2f}, target s-param:{:.2f}".format(round(state.v * 3.6, 2), s0))
            plt.pause(0.0001)

    return t, x, y, yaw, v, goal_flag

def calc_target_speed(state, yaw_ref):
    target_speed = 10.0 / 3.6

    dyaw = yaw_ref - state.yaw
    switch = math.pi / 4.0 <= dyaw < math.pi / 2.0

    if switch:
        state.direction *= -1
        return 0.0
    
    if state.direction != 1:
        return -target_speed

    return target_speed

def main():
    print("rear wheel feedback tracking start!!")
    # 这个是样条曲线的一些控制点,ax是横坐标,ay是纵坐标
    # 目标点就是ax和ay的最后一个点。
    ax = [0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0]
    ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0]
    goal = [ax[-1], ay[-1]]

    # 这里是计算参考线三次样条曲线
    reference_path = CubicSplinePath(ax, ay)
    # 这里对曲线长度进行等分,每一份长度0.1
    s = np.arange(0, reference_path.length, 0.1)
	# 这里进行仿真
    t, x, y, yaw, v, goal_flag = simulate(reference_path, goal)

    # Test
    assert goal_flag, "Cannot goal"
	# 这里是一些显示
    if show_animation:  # pragma: no cover
        plt.close()
        plt.subplots(1)
        plt.plot(ax, ay, "xb", label="input")
        plt.plot(reference_path.X(s), reference_path.Y(s), "-r", label="spline")
        plt.plot(x, y, "-g", label="tracking")
        plt.grid(True)
        plt.axis("equal")
        plt.xlabel("x[m]")
        plt.ylabel("y[m]")
        plt.legend()

        plt.subplots(1)
        plt.plot(s, np.rad2deg(reference_path.calc_yaw(s)), "-r", label="yaw")
        plt.grid(True)
        plt.legend()
        plt.xlabel("line length[m]")
        plt.ylabel("yaw angle[deg]")

        plt.subplots(1)
        plt.plot(s, reference_path.calc_curvature(s), "-r", label="curvature")
        plt.grid(True)
        plt.legend()
        plt.xlabel("line length[m]")
        plt.ylabel("curvature [1/m]")

        plt.show()

if __name__ == '__main__':
    main()
  • 5
    点赞
  • 11
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值