大地坐标系中的平面运动学描述框架

一、平面运动学的基本假设

平面运动学描述框架建立在以下核心假设之上：

假设	数学表述	物理意义
刚体假设	车辆为刚性 body	忽略车身弹性变形
平面约束	$Z=\text{const}$, $\dot{Z}=0$	车辆在水平面内运动
地球曲率忽略	$R_{\text{earth}}\to \infty$	局部大地为平面
科里奥利力忽略	${\mathbf{\omega }}_e\times \mathbf{v}\approx 0$	地球自转效应可忽略

在上述假设下，车辆的三维空间运动可投影至二维水平平面，自由度从 6 个缩减为 3 个（平面位置 $X,Y$ 和航向角 $\psi$）。

二、位姿的数学描述

2.1 位置描述

车辆质心在大地坐标系中的位置用二维矢量表示：

$${\mathbf{r}}_{\text{CG}}=\left[\begin{array}{c}X\\ Y\end{array}\right]$$

其中 $(X,Y)$ 为质心坐标，下标 $\text{CG}$ 表示质心（Center of Gravity）。

2.2 姿态描述

车辆在平面内的姿态用横摆角（Yaw Angle）$\psi$ 描述，定义为：

$$\psi =\angle ({\vec{e}}_X,{\vec{b}}_X)$$

其中 ${\vec{e}}_X$ 为大地坐标系 $X$ 轴单位向量，${\vec{b}}_X$ 为车身坐标系 $X$ 轴单位向量在水平面的投影。

横摆角约定

• 正方向：逆时针为正
• 取值范围：$\psi \in (-\pi ,\pi ]$ 或 $\psi \in [0,2\pi )$
• 零位：车身纵轴与大地 $X$ 轴重合

2.3 位姿矢量

综合位置和姿态，车辆在平面内的**位姿（Pose）**可表示为：

$$\mathbf{q}=\left[\begin{array}{c}X\\ Y\\ \psi \end{array}\right]\in {\mathbb{R}}^2\times {\mathbb{S}}^1$$

其中 ${\mathbb{S}}^1$ 表示单位圆流形（角度空间）。

三、平面运动学微分方程

3.1 速度关系

车辆在大地坐标系中的运动学关系由位姿对时间的导数描述：

$$\dot{\mathbf{q}}=\left[\begin{array}{c}\dot{X}\\ \dot{Y}\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}{v}_X^{\text{earth}}\\ v_Y^{\text{earth}}\\ r\end{array}\right]$$

其中：

• $v_X^{\text{earth}},v_Y^{\text{earth}}$：质心在大地系中的速度分量
• $r=\dot{\psi }$：横摆角速度（Yaw Rate）

3.2 车身速度与大地速度的映射

设车辆在车身坐标系中的速度为 $(v_x,v_y)$，横摆角为 $\psi$，则大地坐标系中的速度为：

$$\left[\begin{array}{c}\dot{X}\\ \dot{Y}\end{array}\right]=\left[\begin{array}{cc}\cos \psi & -\sin \psi \\ \sin \psi & \cos \psi \end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]$$

即：

$$\begin{array}{rl}\dot{X}& =v_x\cos \psi -v_y\sin \psi \\ \dot{Y}& =v_x\sin \psi +v_y\cos \psi \end{array}$$

坐标系变换的核心公式

上述变换是平面旋转矩阵的直接应用，将在 01-平面旋转矩阵与方向余弦的数学构建 中详细推导。

3.3 轨迹积分

给定初始位姿 $\mathbf{q}(t_0)$ 和速度输入 $(v_x(t),v_y(t),r(t))$，车辆轨迹可通过积分获得：

$$\mathbf{q}(t)=\mathbf{q}(t_0)+{\int }_{t_0}^t\left[\begin{array}{c}{v}_x(\tau )\cos \psi (\tau )-v_y(\tau )\sin \psi (\tau )\\ v_x(\tau )\sin \psi (\tau )+v_y(\tau )\cos \psi (\tau )\\ r(\tau )\end{array}\right]d\tau$$

四、曲率与路径几何

4.1 路径曲率

车辆运动轨迹的曲率$\kappa$ 定义为：

$$\kappa =\frac{1}{R}=\frac{d\psi }{ds}$$

其中 $R$ 为瞬时转弯半径，$s$ 为弧长。

曲率与速度的关系：

$$\kappa =\frac{r}{v}=\frac{\dot{\psi }}{\sqrt{v_x^2+v_y^2}}$$

4.2 曲率的运动学意义

曲率值	运动特征
$\kappa =0$	直线运动
$\kappa =\text{const}>0$	匀速圆周运动
$\kappa =\kappa (s)$	一般曲线运动

五、平面运动学的状态空间形式

将位姿作为状态变量，速度作为输入，可得平面运动学的状态空间表达：

状态变量：

$$\mathbf{x}=\left[\begin{array}{c}X\\ Y\\ \psi \end{array}\right]$$

输入变量：

$$\mathbf{u}=\left[\begin{array}{c}{v}_x\\ v_y\\ r\end{array}\right]$$

状态方程：

$$\dot{\mathbf{x}}=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]\mathbf{u}$$

非线性特性

尽管形式上为线性矩阵乘法，但由于 $\psi$ 是状态变量，$\cos \psi$ 和 $\sin \psi$ 使系统本质为非线性。

六、应用示例：轨迹规划框架

在轨迹规划中，大地坐标系提供以下功能：

graph LR
    A[参考路径] --> B[期望位姿 q_d]
    B --> C[轨迹生成器]
    C --> D[速度/加速度剖面]
    D --> E[大地坐标轨迹 X_d, Y_d, ψ_d]
    E --> F[跟踪控制器]

七、相关内容

• 01-大地坐标系的定义与惯性基准建立
• 01-平面旋转矩阵与方向余弦的数学构建
• 02-车身姿态角与平面运动状态变量的对应关系