双舵轮AGV运动学状态方程推导

一、模型假设与符号定义

1. 车体坐标系：

   • 车体质心为原点 $(O)$，$(x)$ 轴指向前进方向，$(y)$ 轴垂直向左，构成右手坐标系。
   • 舵轮安装位置：舵轮1在 $((a,b))$，舵轮2在 $((-a,-b))$（典型对角安装时 $(a=b)$）。
   • 舵轮间距（对角距离）：$(L=\sqrt{(2a{)}^2+(2b{)}^2})$。

2. 状态变量：

   • 车体位置：$(x,y)$（全局坐标系）。
   • 车体航向角：$(\theta )$（相对于全局坐标系 $(x)$ 轴）。

3. 控制输入：

   • 舵轮1线速度 $(v_1)$ 和转向角 $({\alpha }_1)$。
   • 舵轮2线速度 $(v_2)$ 和转向角 $({\alpha }_2)$。

二、运动学分析

每个舵轮的线速度在车体坐标系下的分量为：
$$\{\begin{array}{l}{v}_{1x}=v_1\cos {\alpha }_1,\\ v_{1y}=v_1\sin {\alpha }_1,\end{array}\{\begin{array}{l}{v}_{2x}=v_2\cos {\alpha }_2,\\ v_{2y}=v_2\sin {\alpha }_2.\end{array}$$

车体整体运动：

• 车体线速度由两舵轮速度的合力决定：
  $$v_x=\frac{v_{1x}+v_{2x}}{2},v_y=\frac{v_{1y}+v_{2y}}{2}.$$
• 车体角速度由两舵轮产生的力矩差决定：
  $$\omega =\frac{(v_{1y}\cdot a-v_{1x}\cdot b)+(v_{2y}\cdot (-a)-v_{2x}\cdot (-b))}{I},$$
  其中 $(I)$ 为车体转动惯量。
  简化处理：忽略动力学影响（如转动惯量），直接通过几何关系近似计算：
  $$\omega =\frac{v_1\sin {\alpha }_1-v_2\sin {\alpha }_2}{L}.$$

三、状态方程推导

1. 全局坐标系下的运动关系
车体速度在全局坐标系下的投影：
$$\{\begin{array}{l}\dot{x}=v_x\cos \theta -v_y\sin \theta ,\\ \dot{y}=v_x\sin \theta +v_y\cos \theta ,\\ \dot{\theta }=\omega .\end{array}$$

2. 代入舵轮控制输入
将 $(v_x,v_y,\omega )$ 替换为舵轮参数表达式：
$$\{\begin{array}{l}\dot{x}=\left(\frac{v_1\cos {\alpha }_1+v_2\cos {\alpha }_2}{2}\right)\cos \theta -\left(\frac{v_1\sin {\alpha }_1+v_2\sin {\alpha }_2}{2}\right)\sin \theta ,\\ \dot{y}=\left(\frac{v_1\cos {\alpha }_1+v_2\cos {\alpha }_2}{2}\right)\sin \theta +\left(\frac{v_1\sin {\alpha }_1+v_2\sin {\alpha }_2}{2}\right)\cos \theta ,\\ \dot{\theta }=\frac{v_1\sin {\alpha }_1-v_2\sin {\alpha }_2}{L}.\end{array}$$

四、状态方程简化（车体坐标系）

若在车体坐标系下分析（无需全局坐标旋转），状态方程为：
$$\{\begin{array}{l}{\dot{x}}_b=\frac{v_1\cos {\alpha }_1+v_2\cos {\alpha }_2}{2},\\ {\dot{y}}_b=\frac{v_1\sin {\alpha }_1+v_2\sin {\alpha }_2}{2},\\ \dot{\theta }=\frac{v_1\sin {\alpha }_1-v_2\sin {\alpha }_2}{L}.\end{array}$$
说明：

• $(x_b,y_b)$ 为车体坐标系下的位置，若需全局坐标，需通过旋转矩阵转换。
• 该模型适用于实时控制设计（如MPC），直接关联舵轮输入与车体运动。

五、方程验证

1. 纯平移运动
当 $({\alpha }_1={\alpha }_2=0)$, $(v_1=v_2=v)$：
$${\dot{x}}_b=v,{\dot{y}}_b=0,\dot{\theta }=0.$$
车体沿 $(x_b)$ 轴直线运动。

2. 纯旋转运动
当 $({\alpha }_1=90^{\circ })$, $({\alpha }_2=-90^{\circ })$, $(v_1=v_2=v)$：
$${\dot{x}}_b=0,{\dot{y}}_b=0,\dot{\theta }=\frac{v-(-v)}{L}=\frac{2v}{L}.$$
车体绕质心旋转，符合预期。

六、应用示例（C++代码片段）

#include <Eigen/Dense>

// 状态变量：x, y, theta
using State = Eigen::Vector3d;
// 控制输入：v1, alpha1, v2, alpha2
using Control = Eigen::Vector4d;

// 状态方程（车体坐标系）
State stateEquation(const State& state, const Control& u, double L) {
    double v1 = u[0], alpha1 = u[1];
    double v2 = u[2], alpha2 = u[3];
    
    double vx = (v1 * cos(alpha1) + v2 * cos(alpha2)) / 2.0;
    double vy = (v1 * sin(alpha1) + v2 * sin(alpha2)) / 2.0;
    double omega = (v1 * sin(alpha1) - v2 * sin(alpha2)) / L;
    
    State dstate;
    dstate << vx, vy, omega;
    return dstate;
}

七、总结

• 状态方程：
  $$\{\begin{array}{l}{\dot{x}}_b=\frac{v_1\cos {\alpha }_1+v_2\cos {\alpha }_2}{2},\\ {\dot{y}}_b=\frac{v_1\sin {\alpha }_1+v_2\sin {\alpha }_2}{2},\\ \dot{\theta }=\frac{v_1\sin {\alpha }_1-v_2\sin {\alpha }_2}{L}.\end{array}$$
• 适用场景：模型预测控制（MPC）、路径跟踪、动态轨迹规划。
• 扩展方向：引入动力学补偿（如摩擦、惯性）提升高速运动精度。