自动驾驶路径跟踪控制——车辆动力学建模基本概念

---

1. 位姿自由度

    位姿自由度——系统在空间中的位姿描述所需变量的个数。任何一个没有受约束的物体，在空间均具有6个独立的运动，即有六个自由度。
    六个自由度分别是：

• 横荡（sway）：沿图中轴左右平动；
• 纵荡（surge）：沿图中轴前后平动；
• 垂荡（heave）：沿图中轴上下平动；
• 横滚（roll）：绕图中轴转动；
• 俯仰（pitch）：绕图中轴转动；
• 偏转（yaw）：绕图中轴转动；

2. TDOFandCDOF

• TDOF（Total Degrees of Freedom）:不可控自由度(总自由度)
• CDOF(Controllable Degrees of Freedom)：可控自由度

    若TDOF = CDOF，则被认为是完整的（Holonomic）比如直升机。
    若TDOF > CDOF，则被认为是 非完整的（Nonholonomic）比如汽车。
    若CDOF > TDOF，则被认为是 冗余的(Redundant)比如机械手臂（肩关节3个自由度、腕关节3个自由度、肘关节1个自由度）。

3.运动学与动力学

    车辆的模型分为两类：运动学模型和动力学模型。运动学模型主要考虑车辆的运动状态，包括速度、加速度、方向等，不考虑车辆受力状态。动力学模型通过分析作用在车上的力和力矩构建模型。

• 运动学正问题指，从机器人的关节空间描述计算笛卡尔空间描述的机器人末端执行器的位置和姿态，该问题通常是一个几何问题，给定一组关节角度，计算末端坐标系相对于基坐标系的位置和姿态。
• 运动学逆问题指，从笛卡尔空间描述下的机器人末端执行器位置和姿态反算出机器人关节空间应该达到的关节角度组合，是实现机器人控制的一个基本问题。通常因为正运动学方程是非线性的，因此逆运动学问题较为困难，很难得到封闭解，甚至无解。而正运动学的像空间就形成了逆运动学的有解空间，称为机器人的工作空间。
  参考链接

4. 运动控制问题描述

    对于一般的运动控制问题有以下描述：
    已知机器人的运动学模型/动力学模型以及机器人轮子与地面之间的接触模型，求解所要求的运动控制目标（机器人的速度、位姿以及相应的控制律）。

5. 运动学建模

    在运动学模型中，通常需要对以下几个参量进行求解：

• 机器人速度    $\dot{\xi }={\left[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{\theta }\end{array}\right]}^T$
• 轮子转速    ${\dot{\varphi }}_i$
• 偏转角以及角速度    ${\beta }_i$    ${\dot{\beta }}_i$
• 几何参数，比如长度、宽度等

    运动学正向模型可以有以下公式描述：$$\dot{\xi }={\left[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{\theta }\end{array}\right]}^T=f({\dot{\varphi }}_1,\dots {\dot{\varphi }}_n,{\beta }_1,\dots {\beta }_m,{\dot{\beta }}_1,\dots {\dot{\beta }}_m)$$     已知轮子转速${\dot{\varphi }}_i$、偏航角以及角速度${\beta }_i$ ${\dot{\beta }}_i$，求解机器人速度$\dot{\xi }={\left[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{\theta }\end{array}\right]}^T$。
    运动学逆向模型可以有以下公式描述：$${\left[\begin{array}{ccccccccc}{\dot{\varphi }}_1& \cdots & {\dot{\varphi }}_n& {\beta }_1& \dots & {\beta }_m& {\dot{\beta }}_1& \dots & {\dot{\beta }}_m\end{array}\right]}^T=f(\dot{x},\dot{y},\dot{\theta })$$     已知机器人速度$\dot{\xi }={\left[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{\theta }\end{array}\right]}^T$，求解轮子转速${\dot{\varphi }}_i$、偏航角以及角速度${\beta }_i$ ${\dot{\beta }}_i$。
     以两轮差速小车模型为例。
    小车的速度以及角速度为：$$\{\begin{array}{c}\dot{x}=v\cos \theta \\ \dot{y}=v\sin \theta \\ \dot{\theta }=\omega \end{array}$$

    由两轮差速小车车轮转速与小车速度、角速度的关系可得，$$\{\begin{array}{c}v=\frac{{\dot{\varphi }}_r+{\dot{\varphi }}_l}{2}\\ \omega =\frac{{\dot{\varphi }}_r-{\dot{\varphi }}_l}{r}\end{array}$$    联立得到下列关系式：$$\{\begin{array}{c}\dot{x}=\frac{{\dot{\varphi }}_r+{\dot{\varphi }}_l}{2}\cos \theta \\ \dot{y}=\frac{{\dot{\varphi }}_r+{\dot{\varphi }}_l}{2}\sin \theta \\ \dot{\theta }=\frac{{\dot{\varphi }}_r-{\dot{\varphi }}_l}{r}\end{array}$$

6. 机器人位姿

    机器人的坐标系包括如下几个类别：世界坐标系（World coordinate system）、基坐标系（Base coordinate system）、工具坐标系（Tool coordinate system）和工件坐标系（Object coordinate system）。有很多大佬将这方面讲得很好，本文便不在这里多做赘述。参考链接1参考链接2

    同样以两轮差速小车为例，给定小车一个方向，进行行驶。
    设，世界坐标系: { X I , Y I } \left\{ {{X_I},{Y_I}} \right\} {XI​,YI​}；机器人坐标系: { X R , Y R } \left\{ {{X_R},{Y_R}} \right\} {XR​,YR​}；机器人位置坐标:${\xi }_I={\left[\begin{array}{ccc}x& y& \theta \end{array}\right]}^T$
    在本例中，只用考虑绕z轴方向进行旋转。其旋转矩阵R为：$$R=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$    两坐标系对应关系为：$${\xi }_R=\left[\begin{array}{cc}\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}& T\\ 0& 1\end{array}\right]\cdot {\xi }_I=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\cdot {\xi }_I$$    对时间求导，得
$${\dot{\xi }}_R=R\left(\theta \right){\dot{\xi }}_I=R\left(\theta \right)\cdot {\left[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{\theta }\end{array}\right]}^T$$

7. 跟踪误差

    如图所示，分别对机器人两个位置的坐标进行求导，得$$\begin{array}{l}\{\begin{array}{c}\dot{x}=v\cdot \cos \theta \\ \dot{y}=v\cdot \sin \theta \\ \dot{\theta }=\omega \end{array}\end{array}$$$$\{\begin{array}{c}{\dot{x}}_d=v_d\cdot \cos {\theta }_d\\ {\dot{y}}_d=v_d\cdot \sin {\theta }_d\\ {\dot{\theta }}_d={\omega }_d\end{array}$$    再求得位置偏差$x_e、y_e、{\theta }_e$$$\left[\begin{array}{c}{x}_e\\ y_e\\ {\theta }_e\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_d-x\\ y_d-y\\ {\theta }_d-\theta \end{array}\right]$$    联立求得$$\{\begin{array}{l}\begin{array}{c}{\dot{x}}_e=\omega y_e+\cos \theta (v_d\cos {\theta }_d-v\cos \theta )+\sin \theta (v_d\sin {\theta }_d-v\sin \theta )\\ =\omega y_e+v_d\cos {\theta }_e-v\end{array}\\ \begin{array}{c}{\dot{y}}_e=-\omega x_e-\sin \theta (v_d\cos {\theta }_d-v\cos \theta )+\cos \theta (v_d\sin {\theta }_d-v\sin \theta )\\ =-\omega x_e+v_d\sin {\theta }_e\end{array}\\ {\dot{\theta }}_e={\omega }_d-\omega \end{array}$$$$\{\begin{array}{l}{\dot{x}}_e=\omega \cdot y_e-v+v_d\cos {\theta }_e\\ {\dot{y}}_e=-\omega \cdot x_e+v_d\sin {\theta }_e\\ {\dot{\theta }}_e={\omega }_d-\omega \end{array}$$

8. 控制律设计

    在跟踪误差中，求得了$x_e、y_e、{\theta }_e$的导数$$\{\begin{array}{l}{\dot{x}}_e=\omega \cdot y_e-v+v_d\cos {\theta }_e\\ {\dot{y}}_e=-\omega \cdot x_e+v_d\sin {\theta }_e\\ {\dot{\theta }}_e={\omega }_d-\omega \end{array}$$    当$v_d、{\omega }_d、\frac{d{\omega }_d}{dt}、\frac{d{v}_d}{dt}$都有界时，按照一定的控制律进行设计，最终使得$$\{\begin{array}{l}{x}_e\to 0\\ y_e\to 0\\ {\theta }_e\to 0\end{array}$$

---

声明

本人所有文章仅作为自己的学习记录，若有侵权，联系立删。