移动机器人运动学：轮速、非完整约束与里程计

1. 先选对参考点，再说机器人的速度

本节平面世界系记为 $W$，$z$ 朝上；机器人机体系 $B$ 的 $x$ 朝前、$y$ 朝左。偏航角 $\theta$ 是从世界 $x$ 轴逆时针转到机体前向的角，单位 rad。机器人状态 $q=[x,y,\theta]^T$ 中，$x,y$ 是选定参考点的世界坐标，单位 m。

差速车通常选两驱动轮轴线中点；自行车模型这里选后轴中点。把后轴模型的位置换成前保险杠或 IMU 位置，却不增加杆臂项，会产生系统性错误。

运动学输入是前向速度 $v$（m/s）和角速度 $\omega$（rad/s），不是电机电压、PWM 或轮上力。下游电机闭环能否实现指定速度属于动力学和执行器问题。

2. “不能横着滑”如何写成方程？

机体前向单位向量在世界系为 $e_f=[\cos\theta,\sin\theta]^T$，左向单位向量为 $e_l=[-\sin\theta,\cos\theta]^T$。若轮子理想纯滚动且无侧滑，参考点沿左向的速度投影为零：

这一条约束没有要求位置 $y$ 恒定。它要求任意时刻的速度沿车头方向；车转过头以后当然可以改变世界 $y$ 坐标。因此写成

这就是常见独轮车运动学。它也可以近似两轮差速车，但“独轮车”并不要求真实机器人只有一个轮子。

为什么叫非完整约束？

完整约束可以表示成纯位置/构型方程 $f(q)=0$，例如杆长固定使末端位于一个圆上。这里不能把无侧滑条件简化成“只能待在某条固定曲线上”：车可以前进、转向、再前进，最终到达平面许多位置。约束限制的是可用的瞬时速度方向，而不是把构型空间永久缩成一条线。

更形式化地，允许的两个速度场是

定义向量场括号 $[g_1,g_2]=(\partial g_2/\partial q)g_1-(\partial g_1/\partial q)g_2$，则

它是侧向方向，不在原来两个速度场的线性张成中。这解释了“转一下、走一下、反向转回、反向走回”的小动作序列能产生二阶侧向位移，却不意味着车辆突然获得直接横移电机。实际车辆受转角、曲率、空间与时间限制，不能把这种局部可达性当作随意横移能力。

3. 差速轮：为什么轮速之和控制前进，差控制转向？

令左右轮半径都为 $r$，轮间距为 $b$，轮转角速度为 $\dot\phi_L,\dot\phi_R$。正号统一定义为各自使车辆前进的滚动方向，而不是两个电机轴编码器接线的原始正号。无滑滚动时轮缘前向速度为

参考点到右轮的机体系向量为 $[0,-b/2,0]^T$。刚体点速度关系是 $v_{point}=v_{center}+\omega\times r_{point}$；正偏航 $\omega>0$ 时，右轮前向速度增加，左轮减少：

两式相加与相减，得到正速度运动学：

反过来，给定期望 $v,\omega$，得到轮速参考：

这些式子可对照作者开放教材的差速运动学章节。本节从刚体点速度重新推导，核心假设是刚性车体、轮径已知、轮间距已知且无显著滑移。

数例：$r=0.1$ m、$b=0.4$ m，左轮 $8$ rad/s、右轮 $12$ rad/s，得到 $v=1$ m/s、$\omega=1$ rad/s。瞬时转弯半径 $\rho=v/\omega=1$ m，正号表示向左转。两轮相同则直行；两轮等幅反向则中心原地转动。

四轮滑移转向底盘不能直接当作理想两轮差速车。 可以用有效轮距拟合低速模型，但侧滑、地面材料、载荷和转弯速度会影响该参数，不能靠一个“真实尺量轮距”覆盖所有工况。

4. 自行车模型：转向角不是偏航角速度

将汽车左右轮合成前后两个等效轮。轴距为 $L$，前轮相对车身转角为 $\delta$。在低速、平地、轮胎侧偏忽略时，后轴中点速度沿车身前向，前轮速度沿转向后的方向。

后轴转弯半径为 $\rho$。前后轮的瞬时法线交于同一瞬时旋转中心，直角三角形给出 $\tan\delta=L/\rho$。结合 $v=\omega\rho$：

$L$ 单位 m，$\delta$ 和 $\theta$ 单位 rad。车不前进时 $v=0$，这个模型不允许像差速车那样原地转向。若把 $\delta$ 直接当作 $\omega$，量纲和物理行为都错。

曲率 $\kappa=\omega/v=\tan\delta/L$，单位 $1/\mathrm m$。若机械最大转角为 $\delta_{max}$，最小几何转弯半径为 $L/|\tan\delta_{max}|$。高速时轮胎侧偏不可忽略，应转向横向动力学自行车模型；不是在这条运动学公式上随意增加一个增益。

上述后轴参考点和速度/转角输入约定可对照 MathWorks 官方自行车模型文档。这里使用的是低速几何模型，不是轮胎侧向力模型。

例：$L=2$ m、$v=1$ m/s、$\delta=\arctan(0.5)$，则 $\omega=0.25$ rad/s、$\rho=4$ m。倒车时 $v$ 变号，固定转角下偏航速度也变号。

5. 从速度变成位置：欧拉积分为什么会偏？

最简单的离散更新为

这相当于整段时间都沿起始车头方向走，转弯时会把圆弧近似成切线。采样足够短可以接受，但高转速/长间隔下误差明显。

若区间内 $v,\omega$ 常值，先积分角度 $\theta(t)=\theta_k+\omega t$，再积分正弦和余弦：

这里 $\omega\ne0$。当 $\omega\to0$，分子也趋于零，数学极限有限，但直接相除会数值消减。利用和差化积，令 $\alpha=\omega\Delta t/2$、$s=v\Delta t$：

其中 $\operatorname{sinc}(\alpha)=\sin\alpha/\alpha$，在零点定义为 $1$。小量时用 $1-\alpha^2/6+\alpha^4/120$，即可统一处理直线和圆弧。

若 $v=1$ m/s、$\omega=1$ rad/s、初始位姿为零、$\Delta t=1$ s，精确圆弧为 $(\sin1,1-\cos1,1)$，约 $(0.84147,0.45970,1)$；欧拉积分给 $(1,0,1)$。这不是滤波器出错，而是离散运动模型先产生了系统误差。

6. 编码器里程计：测量的是转角增量，不是绝对位置

若一次计数增量为 $\Delta N$，每轮一圈有效计数数为 $N_{rev}$，则

$N_{rev}$ 必须包含解码倍频与减速比的约定。单位是轮轴一圈，不能将电机轴一圈和车轮一圈混用。左右轮增量可计算

区间内曲率近似常值时，直接将上一节 $s$ 换成 $\Delta s$、$\alpha$ 换成 $\Delta\theta/2$，得到位姿增量。没有时间戳时仍可做位移积分，却无法可靠计算速度或与 IMU 对齐。

轮径比例误差使直行距离越走越偏；左右轮径不一致会让理论直行也慢慢转弯；轮距误差主要影响转角；打滑会破坏运动学假设。里程计连续平滑不等于绝对准确，闭环/外部位置校正仍有价值。

7. 怎样给里程计传播不确定性？

为便于手算，先用欧拉模型 $q_{k+1}=f(q_k,u_k)$，输入 $u=[v,\omega]^T$。在当前估计处线性化：

$F$ 把已有位姿误差传到下一时刻；$G$ 把区间输入误差传进去。若输入误差协方差为 $Q_u$，与当前状态误差独立，则

$Q_u$ 的前一对角元单位 $\mathrm{m^2/s^2}$，后一对角元单位 $\mathrm{rad^2/s^2}$；$P$ 的位置方差单位 $\mathrm{m^2}$，姿态方差单位 $\mathrm{rad^2}$，非对角项为相应单位乘积。这里 $Q_u$ 是离散区间速度误差协方差，不是不加换算就能使用的连续白噪声谱密度。

假设初始位置准确，偏航标准差为 $1^\circ\approx0.01745$ rad，沿 $x$ 方向以 $1$ m/s 前进 $1$ s，$F_{y\theta}=1$ m/rad。仅这个偏航不确定性就贡献约 $(0.01745)^2=0.000305\ \mathrm{m^2}$ 的横向位置方差，即 $1.745$ cm 标准差。这说明角度误差会转化为位置误差，不能仅给编码器距离一个噪声数。

实际使用精确圆弧模型时，应重新推导该模型的 $F,G$，或者做自动微分/有限差分验证；不能只把均值模型替换成圆弧，却宣称欧拉雅可比仍严格匹配。

8. 与 ROV 的联系和区别

差速模型把机体系横向速度设为零；水下机器人通常不能这么做。水流和横移推进器会产生真实横向速度。在六自由度刚体运动中，导航系位置速度满足 $\dot p^n=R_{nb}v^b$，角运动由姿态运动学描述。DVL 测得的三轴速度应保留其物理分量，不能套用轮式无侧滑约束强行抹掉横向速度。

但以下工程思想共通：明确参考点与坐标系、用正确时间间隔积分、给误差建模、区分运动学可行与动力学可实现。机械臂、车辆与 ROV 的输入维度不同，并不改变这些基本检查。

9. C++ 练习：一个公式同时覆盖直线、圆弧和原地转

任务是实现区间常速度的精确积分，并测试正负速度、极小转速和一整圈闭合。输入是 $v,\omega$，不是轮速；轮速先通过第三节转换。

#include <cassert>
#include <cmath>
#include <iostream>
struct Pose { double x,y,yaw; };
double sinc(double a) {
    if (std::abs(a)<1e-4) {
        double a2=a*a;
        return 1-a2/6+a2*a2/120;
    }
    return std::sin(a)/a;
}
Pose step(Pose q,double v,double w,double dt) {
    assert(dt>=0 && std::isfinite(dt));
    double a=0.5*w*dt, ds=v*dt*sinc(a);
    q.x+=ds*std::cos(q.yaw+a);
    q.y+=ds*std::sin(q.yaw+a);
    q.yaw+=w*dt; // 内部可保持连续角；显示时再wrap，避免累计转数丢失。
    return q;
}
bool near(double a,double b,double e=1e-10){return std::abs(a-b)<e;}
int main(){
    double pi=std::acos(-1.0);
    auto q=step({0,0,0},1,0,2);
    assert(near(q.x,2)&&near(q.y,0)&&near(q.yaw,0));
    q=step({0,0,0},1,1,1);
    assert(near(q.x,std::sin(1))&&near(q.y,1-std::cos(1)));
    q=step({2,3,0},0,1,pi);
    assert(near(q.x,2)&&near(q.y,3)&&near(q.yaw,pi));
    q=step({0,0,0},-1,0,1);assert(near(q.x,-1));
    q=step({0,0,0},1,1e-12,1);assert(near(q.x,1));
    q=step({0,0,0},1,1,2*pi);
    assert(near(q.x,0)&&near(q.y,0)&&near(q.yaw,2*pi));
    Pose repeated{0,0,0};for(int i=0;i<100;++i) repeated=step(repeated,1,1,0.01);
    auto whole=step({0,0,0},1,1,1);
    assert(near(repeated.x,whole.x)&&near(repeated.y,whole.y));
    std::cout<<"mobile PASS\n";
}

10. 读懂工程代码时应寻找什么？

• 编码器计数是否经过溢出处理，轮径/减速比/倍频是否只应用一次。
• 左右轮的正方向是否统一，前后安装电机是否需要反号。
• 里程计参考点是否与 IMU、激光雷达和控制参考点一致。
• 差速、Ackermann、自行车、滑移转向是不是按真实构型选择。
• 位置连续性与全局准确性是否分开管理，地图校正有没有直接变成电机突变。

接着学习 SLAM 残差与优化基础，就能看到这些里程计增量如何成为位姿之间的相对约束，而不是最终不可修改的真值。