控制算法-后轮位置反馈

发表于 2020-02-26 更新于 2021-08-30 分类于控制算法，路径跟踪阅读次数：

概述

本文介绍一种使用后轮位置反馈的路径跟踪算法，使用李亚普洛夫稳定判据，推导控制率。

误差

后轮位置跟踪示意图

位置误差

关于位置误差的计算，首先搜寻参考路径与车辆后轴中心距离最近的点：

\[ s(t) = arg\min_{\gamma}\|\bigl(x_r(t),y_r(t)\bigr)-\bigl(x_{ref}(\gamma),y_{ref}(\gamma)\bigr) \| \tag{1} \]

其中，\(\bigl(x_r(t),y_r(t)\bigr)\)代表\(t\)时刻车辆后轴中心位置坐标，\(\bigl(x_{ref}(\gamma),y_{ref}(\gamma)\bigr)\)代表参考路径上离车辆后轴距离最近点的位置坐标，\(s(t)\)代表\(t\)时刻与车辆后轴中心点距离最近参考路径点的位置参数\(\gamma\)。

关于参考路径在\(s(t)\)参数处的单位切向量为

\[ \hat{t}= \frac{ \Bigl(\dfrac{\partial x_{ref}}{\partial s}\|_{s(t)},\dfrac{\partial y_{ref}}{ \partial s}\|_{s(t)} \Bigr)} {\Bigg\| \Bigl(\dfrac{ \partial x_{ref}\bigl(s(t)\bigr)}{ \partial s},\dfrac{ \partial y_{ref}\bigl(s(t)\bigr)}{ \partial s}\Bigr) \Biggr\|} =(t_x,t_y) \tag{2} \]

跟踪误差向量表示如下：

\[ \vec{d}(t)=\Bigl(x_r(t) - x_{ref}\bigl(s(t)\bigr),y_r(t) - y_{ref}\bigl(s(t)\bigr)\Bigr)=(d_x,d_y)\tag{3} \]

所以跟踪误差向量\(\vec{d}\)和参考路径上最近点的单位切向量\(\hat{t}\)的向量积为

\[ \vec{e} = \hat{t} \times \vec{d} = \left\lvert \begin{array}{cl} x & y\\ t_x & t_y\\ d_x & d_y \end{array} \right\rvert=t_xd_y - t_yd_x \tag{4} \]

角度误差

关于车辆航向向量与目标路径切向量的夹角\(\psi_e\)如下：

\[ \psi_e(t)=\psi-\arctan2(\frac{ \partial y_{ref}\bigl(s(t)\bigr)}{ \partial s},\frac{ \partial x_{ref}\bigl(s(t)\bigr)}{ \partial s}) \tag{5} \]

微分方程

参考曲线最近点

关于参考曲线与车辆后轴中心最近点处的速度\(\dot{s}(t)\)可以通过下图求得

横向误差

车辆横向移动的速度\(\dot{e}\)可以表示如下：

\[ \dot{e} = v_r \cdot \sin(\psi_e)\tag{9} \]

航向角误差

关于车辆航向角误差的变化率表示如下：

\[ \begin{array}{cl} \dot{\psi_e} &= \dot{\psi} - \dot{\psi_{ref}} \\ &= \dot{\psi} - \dot{s}\cdot k(s)\\ &=\dot{\psi} - \dfrac{v_{r}\cdot\cos(\psi_e)\cdot k(s)}{1-k(s)e} \end{array}\tag{10} \]

综述

基于上述参考曲线处的速度、横向误差变化率和航向角误差变化率，整理如下微分方程形式：

\[ \begin{array}{cl} \dot{s} &= \dfrac{v_{r}\cdot\cos(\psi_e)}{1-k(s)e}\\ \dot{e} &= v_r \cdot \sin(\psi_e)\\ \dot{\psi_e} &=\dot{\psi} - \dfrac{v_{r}\cdot\cos(\psi_e)\cdot k(s)}{1-k(s)e} \end{array}\tag{11} \]

李亚普洛夫稳定判据

Lyapunov函数

基于上述微分方程，定义李亚普洛夫函数形式如下：

\[ V(e,\psi_e) = \frac{1}{2}e^2 + \frac{1}{2k_2}\psi_e^2 \tag{12} \]

其中\(k_2 > 0\)，为了使\((e,\psi_e)\)在平衡点\((0,0)\)处稳定，根据李亚普洛夫稳定判据，必须满足一下两个条件：

\(\lim_{\rvert e,\psi_e \rvert \to \infty}V = \infty\)
\(\begin{array}{rl}\dot{V} < 0 &(e \neq 0,\psi_e \neq 0)\end{array}\)

对于条件1，等式(12)明显成立，所以李亚普洛夫稳定判据第一条满足。

对于\(\dot{V}\),结合等式(11)的微分方程，推导形式如下：

\[ \begin{array}{cl} \dot{V} &= e\dot{e} + \dfrac{1}{k_2}\psi_e\dot{\psi}_e\\ &=e \cdot v_r \cdot \sin(\psi_e) + \dfrac{1}{k_2}\psi_e\Bigl(\dot{\psi} - \dfrac{v_{r}\cdot\cos(\psi_e)\cdot k(s)}{1-k(s)e}\Bigr) \end{array}\tag{13} \]

根据等式(13)，另\(\dot{V} = 0\)，得

\[ \begin{array}{cl} v_r \cdot \sin(\psi_e)\cdot e + \dfrac{1}{k_2}\psi_e\Bigl(\dot{\psi} - \dfrac{v_{r}\cdot\cos(\psi_e)\cdot k(s)}{1-k(s)e}\Bigr)=0\\ k_2 v_r \dfrac{\sin(\psi_e)}{\psi_e}e + \dot{\psi} - \dfrac{v_{r}\cdot\cos(\psi_e)\cdot k(s)}{1-k(s)e} = 0\\ \end{array} \tag{14} \]

求出零界控制率\(\dot{\psi_e}\) 如下：

\[ \dot{\psi}^* = \dfrac{v_{r}\cdot\cos(\psi_e)\cdot k(s)}{1-k(s)e} - k_2 v_r \dfrac{\sin(\psi_e)}{\psi_e}e \tag{15} \]

为了满足第2条李亚普洛夫稳定判据，设置一个调节函数\(g(e, \psi_e, t) > 0\),可以得出基于航向角变化率的控制命令：

\[ \dot{\psi} = \dot{\psi}^* - g(e,\psi_e,t)\psi_e\tag{16} \]

设置\(g(e,{\psi}_e,t)= k_{\psi}|v_r|\) ，其中\(k_{\psi} > 0\)。

\[ \dot{\psi} = \dfrac{v_{r}\cdot\cos(\psi_e)\cdot k(s)}{1-k(s)e} - k_2 v_r \dfrac{\sin(\psi_e)}{\psi_e}e - k_{\psi}|v_r|\psi_e \tag{17} \]

将等式(17)带入等式(13)得

\[ \begin{array}{cl} \dot{V} &=v_r \cdot \sin(\psi_e) \cdot e + \dfrac{1}{k_2}\psi_e\Bigl(\dot{\psi} - \dfrac{v_{r}\cdot\cos(\psi_e)\cdot k(s)}{1-k(s)e}\Bigr)\\ &=v_r \cdot \sin(\psi_e) \cdot e +\dfrac{1}{k_2}\psi_e\bigl(- k_2 v_r \dfrac{\sin(\psi_e)}{\psi_e}e - k_{\psi}|v_r|\psi_e\bigr)\\ &=v_r \cdot \sin(\psi_e) \cdot e - v_r \cdot \sin(\psi_e) \cdot e -\dfrac{k_{\psi}}{k_2}|v_r|\psi_e^2\\ &=-\dfrac{k_{\psi}}{k_2}|v_r|\psi_e^2 \leq 0 \end{array}\tag{18} \]

所以设计得控制率满足稳定性条件。