传感器数据处理笔记

里程计模型：

• 两轮差分地盘的运动学模型
• 三轮全向底盘的运动学模型
• 航迹推算（Dead Reckoning）

里程计标定

• 线性最小二乘的基本原理
• 最小二乘的直线拟合
• 最小二乘在里程计标定中的应用

差分底盘的优势就是：

• 结构简单
• 便宜（2个电机）
• 模型简单

两轮差分地盘的运动学模型：

欠驱动系统：xy$\theta$三个自由度，vl和vr（两个xx）（意味着运动是耦合的）

(相关过程直接省略)使用的是$\frac{V_L}{r-d}=\frac{V_R}{r+d}$然后得到$r=\frac{v_R+v_L}{v_R-v_L}d$

公式：
$$\begin{array}{rl}\mathbf{v}& =\frac{v_R+v_L}{2}\\ \omega & =\frac{v_R-v_L}{2d}\end{array}$$
从上面两式解出 $v_R,v_L$ ：
$$\{\begin{array}{l}{v}_R=v+\omega d,\\ v_L=v-\omega d.\end{array}$$

三轮全向底盘

具有以下的特点：

• 任何方向平移
• 全驱动系统： 3个自由度xy$\theta$ 和 v1、v2、v3输入–》可以向任意方向运动
• 可进行运动学分解

分解的方法就是假设只有一个方向

第一个：直接相加

第二个：是第一个的矩阵形式

第三个：第二个的逆矩阵

运动学预测（Prediction）——从增量到全局位姿

1. 概述

在移动机器人导航与定位中（如 EKF、粒子滤波等），预测步骤就是把机器人在自身坐标系下测得的运动增量 $(dx,dy,d\theta )$，转换到全局坐标系并更新全局位姿 $(x,y,\theta )$。

2. 坐标系与示意图

   markdown复制编辑     机器人自身坐标系             全局坐标系
           ▲ y_body                    ▲ Y
           │                          /
           │                        /
           │                      /
           └───► x_body        ●───► X
   

   • 自身坐标系：机器人车体前进方向为 $x_{body}$，左侧为 $y_{body}$。
   • 全局坐标系：环境中的固定参考系，机器人初始位姿为 $(x,y,\theta )$。

3. 运动增量与同质变换

   机器人在自身坐标系下运动一小段距离，测得增量向量
   $$\Delta {\mathbf{p}}_{body}=\left[\begin{array}{c}dx\\ dy\\ d\theta \end{array}\right].$$
   要将它叠加到全局位姿，使用 3×3 同质坐标变换：
   $$
   T(\theta) ;=;
   \begin{bmatrix}
   \cos\theta & -\sin\theta & 0\[4pt]
   \sin\theta & ;\cos\theta & 0\[4pt]
   0 & 0 & 1
   \end{bmatrix},
   \quad
   \begin{bmatrix} x’ \ y’ \ \theta’ \end{bmatrix}

   \begin{bmatrix} x \ y \ \theta \end{bmatrix}
   ;+;
   T(\theta),\Delta \mathbf{p}_{body}.
   $$展开即得：$$
   \begin{bmatrix} x’ \ y’ \ \theta’ \end{bmatrix}

   \begin{bmatrix} x \ y \ \theta \end{bmatrix}
   +
   \begin{bmatrix}
   \cos\theta & -\sin\theta & 0\
   \sin\theta & ;\cos\theta & 0\
   0 & 0 & 1
   \end{bmatrix}
   \begin{bmatrix} dx \ dy \ d\theta \end{bmatrix}.
   $$

   • 第一行 $x^{\prime }=x+\cos \theta \cdot dx-\sin \theta \cdot dy$
   • 第二行 $y^{\prime }=y+\sin \theta \cdot dx+\cos \theta \cdot dy$
   • 第三行 ${\theta }^{\prime }=\theta +d\theta$

4. 旋转矩阵详解

   同质矩阵 $T(\theta )$ 中的前 2×2 子矩阵
   $$R(\theta )=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$

   • 用途：将自身坐标系下的平移增量 $(dx,dy{)}^T$ 旋转到全局坐标系下。
   • 逆变换：若要把全局系增量映回自身系，使用 $R^{-1}=R^T$：

   $$
   R(\theta)^{-1}
   =R(\theta)^T

   \begin{bmatrix}
   \cos\theta & \sin\theta\
   -\sin\theta & \cos\theta
   \end{bmatrix}.
   $$

5. 含噪声的运动模型

   实际测量与执行存在误差，引入高斯噪声向量 $\epsilon =[{\epsilon }_x,{\epsilon }_y,{\epsilon }_{\theta }{]}^T$：
   $$
   \Delta \mathbf{p}_{body}^{noisy}

   \begin{bmatrix} dx \ dy \ d\theta \end{bmatrix}
   +
   \begin{bmatrix}\varepsilon_x\\varepsilon_y\\varepsilon_\theta\end{bmatrix}.
   $$完整预测更新为：$$
   \begin{bmatrix} x’ \ y’ \ \theta’ \end{bmatrix}

   \begin{bmatrix} x \ y \ \theta \end{bmatrix}
   +
   T(\theta),\Delta \mathbf{p}_{body}^{noisy}.
   $$

里程计标定

使用线性最小二乘法进行标定

对于不同规模关系的线性方程组 $Ax=b$，常用的求解或近似方法如下：

1. 适定方程组（$m=n$）

• 矩阵可逆（$\det A\ne 0$）时
  $$x=A^{-1}b$$
  或者更稳定地用 LU 分解、QR 分解等数值方法直接求解。

• 矩阵不可逆（$\det A=0$）时
  退化为欠定／超定的情形，见下面。

2. 欠定方程组（$m<n$，方程少于未知数）

• 无穷多解：通解可以写成
  $$x=x_0+N(A),$$
  其中 $x_0$ 是任意一个特解，$N(A)$ 是齐次解空间（维度 $n-m$）。

• 最小范数解（最常用）
  取唯一的 “最小二乘范数” 解（Moore–Penrose 伪逆），即
  $$x^{\ast }=A^T(A{A}^T{)}^{-1}b,$$
  它也是所有解中 $\Vert x\Vert$ 最小者。

3. 超定方程组（$m>n$，方程多于未知数）

• 无精确解：通常不存在满足所有方程的 $x$。

• 最小二乘解
  求解
  $$\underset{x}{\min }\Vert Ax-b{\Vert }^2,$$
  通解为正规方程
  $$A^{T}Ax=A^{T}b⟹x^{\ast }=(A^{T}A{)}^{-1}{A}^{T}b.$$
  在数值上，推荐使用 QR 分解 或 SVD 求解最小二乘，以保证稳定性。

即

• $m=n$：直接求逆或分解；
• $m：参数化通解，常取伪逆给出最小范数解；
• $m>n$：最小二乘，正规方程或分解方法。

一般来说通解是$x^{\ast }=(A^{T}A{)}^{-1}{A}^{T}b.$但是会出现一些问题

4. 超定方程组（$m>n$，方程多于未知数）

• 最小二乘解
  $$x^{\ast }=(A^{T}A{)}^{-1}{A}^{T}b$$

• 数值问题

  1. 条件数放大：$\kappa (A^{T}A)\approx \kappa (A{)}^2$，小的浮点误差被平方放大，解可能极不准确。
  2. 对 $b$ 的微小扰动敏感：当 $b\to b+\delta b$ 时，$x^{\ast }$ 的变化往往远大于 $\delta b$。
  3. $A^{T}A$ 不可逆或病态：若 $A$ 列不满秩，$A^{T}A$ 奇异；即便可逆，也可能因病态（高条件数）而不可靠。

• 改进方法

  • QR 分解：令 $A=QR$，解 $Rx=Q^{T}b$，避免直接构造并求逆 $A^{T}A$。

  • SVD／伪逆：$A=U\Sigma V^T$，取
    $$x^{\ast }=V{\Sigma }^{+}{U}^{T}b,$$
    可处理秩亏且更稳定。

  • 正则化（Ridge）：
    $$x^{\ast }=(A^{T}A+\lambda I{)}^{-1}{A}^{T}b,\lambda >0$$
    对抗病态与噪声。