连续时间位姿状态空间的严格构建

一、状态空间的定义

1.1 状态变量的选择

单轨运动学模型的状态变量描述车辆的位姿（Pose）：

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

其中：

• $(x,y)\in {\mathbb{R}}^2$：后轴中心在大地坐标系的位置
• $\psi \in {\mathbb{S}}^1$：车辆横摆角（单位圆流形）

1.2 输入变量的选择

控制输入为：

$$\mathbf{u}=\left[\begin{array}{c}v\\ \delta \end{array}\right]\in \mathbb{R}\times [-{\delta }_{\max },{\delta }_{\max }]$$

或等价地：

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

其中：

• $v$：后轴中心速度（沿车身方向）
• $\delta$：前轮转角
• $\omega =\dot{\delta }$：转向角速度

1.3 状态空间的拓扑结构

状态空间 $\mathcal{X}={\mathbb{R}}^2\times {\mathbb{S}}^1$ 是特殊欧几里得群$\text{SE}(2)$ 的子流形：

$$\text{SE}(2)=\left\{\left[\begin{array}{ccc}\cos \psi & -\sin \psi & x\\ \sin \psi & \cos \psi & y\\ 0& 0& 1\end{array}\right]:x,y\in \mathbb{R},\psi \in {\mathbb{S}}^1\right\}$$

二、运动学约束的引入

2.1 后轴速度约束

后轮只能沿轮面方向滚动（无侧滑）：

$$\dot{y}\cos \psi -\dot{x}\sin \psi =0$$

2.2 速度分解

后轴速度在大地坐标系中的分解：

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]=v\left[\begin{array}{c}\cos \psi \\ \sin \psi \end{array}\right]$$

其中 $v$ 为车身纵向速度。

2.3 前轴速度约束

前轮速度方向与前轮转角一致：

$$v_{fx}=v\cos \delta ,v_{fy}=v\sin \delta$$

三、状态方程的推导

3.1 几何关系

由车辆几何关系，横摆角速度与速度的关系：

$$\dot{\psi }=\frac{v}{L}\tan \delta$$

其中 $L$ 为轴距。

3.2 推导过程

前轴中心的速度在大地坐标系中为：

$$\left[\begin{array}{c}{\dot{x}}_f\\ {\dot{y}}_f\end{array}\right]=\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]+\dot{\psi }\left[\begin{array}{c}-L\sin \psi \\ L\cos \psi \end{array}\right]$$

前轴速度方向必须与前轮转角一致：

$$\frac{{\dot{y}}_f}{{\dot{x}}_f}=\tan (\psi +\delta )$$

代入并整理得：

$$\dot{\psi }=\frac{v}{L}\tan \delta$$

3.3 完整状态方程

综合以上推导，单轨运动学模型的状态方程为：

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}v\cos \psi \\ v\sin \psi \\ \frac{v}{L}\tan \delta \end{array}\right]$$

四、状态空间形式的标准表达

4.1 非线性状态方程

写成标准非线性状态空间形式：

$$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$$

其中：

$$f(\mathbf{x},\mathbf{u})=\left[\begin{array}{c}v\cos \psi \\ v\sin \psi \\ \frac{v}{L}\tan \delta \end{array}\right]$$

4.2 仿线形式（速度作为输入）

若将速度 $v$ 和转向角速度 ${\omega }_{\delta }=\dot{\delta }$ 作为输入：

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\psi }\\ \dot{\delta }\end{array}\right]=\left[\begin{array}{c}\cos \psi \\ \sin \psi \\ \frac{1}{L}\tan \delta \\ 0\end{array}\right]v+\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]{\omega }_{\delta }$$

这是输入仿射（Input-Affine）形式：

$$\dot{\mathbf{x}}=f_0(\mathbf{x})+f_1(\mathbf{x})u_1+f_2(\mathbf{x})u_2$$

4.3 曲率形式

引入曲率 $\kappa =\frac{1}{R}=\frac{\tan \delta }{L}$：

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}v\cos \psi \\ v\sin \psi \\ v\kappa \end{array}\right]$$

五、模型的奇异性分析

5.1 转向角奇点

当 $\delta =\pm \pi /2$ 时，$\tan \delta \to \infty$，模型奇异。

物理上，这对应于前轮转 90 度，车辆无法前进。

5.2 速度零点

当 $v=0$ 时，$\dot{\psi }=0$，车辆无法改变航向。

这是非完整系统的特征：静止时无法通过转向改变位姿。

5.3 有效工作区域

实际工作区域限制为：

$$\begin{array}{rl}v& \in [v_{\min },v_{\max }]\\ \delta & \in [-{\delta }_{\max },{\delta }_{\max }],{\delta }_{\max }\approx 35^{\circ }\end{array}$$

六、状态转移性质

6.1 解的存在唯一性

由于 $f(\mathbf{x},\mathbf{u})$ 关于 $\mathbf{x}$ 连续且局部 Lipschitz，根据 Picard-Lindelöf 定理，给定初始状态 $\mathbf{x}(0)$ 和输入 $\mathbf{u}(t)$，存在唯一解。

6.2 状态转移映射

状态转移映射 $\Phi :{\mathbb{R}}^3\times \mathcal{U}\to {\mathbb{R}}^3$：

$$\mathbf{x}(t)=\Phi (t;{\mathbf{x}}_0,\mathbf{u})$$

6.3 群结构

状态空间具有李群结构，状态转移可视为 $\text{SE}(2)$ 上的右平移。

七、相关内容

• 02-非完整约束方程的数学构建与物理意义
• 02-离散化运动学递推与轨迹积分理论
• 01-单轨几何假设与非完整约束理论