传感器数据处理笔记
里程计模型：
- 两轮差分地盘的运动学模型
- 三轮全向底盘的运动学模型
- 航迹推算（Dead Reckoning）
里程计标定
- 线性最小二乘的基本原理
- 最小二乘的直线拟合
- 最小二乘在里程计标定中的应用
差分底盘的优势就是：
- 结构简单
- 便宜（2个电机）
- 模型简单
两轮差分地盘的运动学模型：
欠驱动系统：xy θ \theta θ三个自由度，vl和vr（两个xx）（意味着运动是耦合的）
(相关过程直接省略)使用的是 V L r − d = V R r + d \frac{V_L}{r-d}=\frac{V_R}{r+d} r−dVL=r+dVR然后得到 r = v R + v L v R − v L d r=\frac{v_R+v_L}{v_R-v_L}d r=vR−vLvR+vLd
公式：
v = v R + v L 2 ω = v R − v L 2 d \begin{align} \mathbf{v} &= \frac{v_R + v_L}{2} \\ \omega &= \frac{v_R - v_L}{2d} \end{align} vω=2vR+vL=2dvR−vL
从上面两式解出 v R , v L v_R,\,v_L vR,vL ：
{ v R = v + ω d , v L = v − ω d . \begin{cases} v_R = v + \omega\,d,\\ v_L = v - \omega\,d. \end{cases} {vR=v+ωd,vL=v−ωd.
三轮全向底盘
具有以下的特点：
- 任何方向平移
- 全驱动系统： 3个自由度xy θ \theta θ 和 v1、v2、v3输入–》可以向任意方向运动
- 可进行运动学分解
分解的方法就是假设只有一个方向
第一个：直接相加
第二个：是第一个的矩阵形式
第三个：第二个的逆矩阵
运动学预测（Prediction）——从增量到全局位姿
在移动机器人导航与定位中（如 EKF、粒子滤波等），预测步骤就是把机器人在自身坐标系下测得的运动增量 ( d x , d y , d θ ) (dx,dy,d\theta) (dx,dy,dθ)，转换到全局坐标系并更新全局位姿 ( x , y , θ ) (x,y,\theta) (x,y,θ)。
-
坐标系与示意图
markdown复制编辑 机器人自身坐标系 全局坐标系▲ y_body ▲ Y│ /│ /│ /└───► x_body ●───► X- 自身坐标系：机器人车体前进方向为 x b o d y x_{body} xbody，左侧为 y b o d y y_{body} ybody。
- 全局坐标系：环境中的固定参考系，机器人初始位姿为 ( x , y , θ ) (x,y,\theta) (x,y,θ)。
-
运动增量与同质变换
机器人在自身坐标系下运动一小段距离，测得增量向量
Δ p b o d y = [ d x d y d θ ] . \Delta \mathbf{p}_{body} = \begin{bmatrix} dx \\ dy \\ d\theta \end{bmatrix}. Δpbody= dxdydθ .
要将它叠加到全局位姿，使用 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 ′ = x + cos θ ⋅ d x − sin θ ⋅ d y x' = x + \cos\theta\cdot dx - \sin\theta\cdot dy x′=x+cosθ⋅dx−sinθ⋅dy
- 第二行 y ′ = y + sin θ ⋅ d x + cos θ ⋅ d y y' = y + \sin\theta\cdot dx + \cos\theta\cdot dy y′=y+sinθ⋅dx+cosθ⋅dy
- 第三行 θ ′ = θ + d θ \theta' = \theta + d\theta θ′=θ+dθ
-
旋转矩阵详解
同质矩阵 T ( θ ) T(\theta) T(θ) 中的前 2×2 子矩阵
R ( θ ) = [ cos θ − sin θ sin θ cos θ ] R(\theta)= \begin{bmatrix} \cos\theta & -\sin\theta\\ \sin\theta & \;\cos\theta \end{bmatrix} R(θ)=[cosθsinθ−sinθcosθ]- 用途：将自身坐标系下的平移增量 ( d x , d y ) T (dx,dy)^T (dx,dy)T 旋转到全局坐标系下。
- 逆变换：若要把全局系增量映回自身系，使用 R − 1 = R T R^{-1}=R^T R−1=RT：
$$
R(\theta)^{-1}
=R(\theta)^T\begin{bmatrix}
\cos\theta & \sin\theta\
-\sin\theta & \cos\theta
\end{bmatrix}.
$$ -
含噪声的运动模型
实际测量与执行存在误差，引入高斯噪声向量 ε = [ ε x , ε y , ε θ ] T \varepsilon=[\varepsilon_x,\varepsilon_y,\varepsilon_\theta]^T ε=[εx,εy,εθ]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}.
$$
里程计标定
使用线性最小二乘法进行标定
对于不同规模关系的线性方程组 A x = b A x = b Ax=b，常用的求解或近似方法如下：
- 适定方程组（ m = n m = n m=n）
-
矩阵可逆（ det A ≠ 0 \det A \neq 0 detA=0）时
x = A − 1 b x = A^{-1} b x=A−1b
或者更稳定地用 LU 分解、QR 分解等数值方法直接求解。 -
矩阵不可逆（ det A = 0 \det A = 0 detA=0）时
退化为欠定／超定的情形，见下面。
- 欠定方程组（ m < n m < n m<n，方程少于未知数）
-
无穷多解：通解可以写成
x = x 0 + N ( A ) , x = x_0 + N(A), x=x0+N(A),
其中 x 0 x_0 x0 是任意一个特解， N ( A ) N(A) N(A) 是齐次解空间（维度 n − m n-m n−m）。 -
最小范数解（最常用）
取唯一的 “最小二乘范数” 解（Moore–Penrose 伪逆），即
x ∗ = A T ( A A T ) − 1 b , x^* = A^T (A A^T)^{-1} b, x∗=AT(AAT)−1b,
它也是所有解中 ∥ x ∥ \|x\| ∥x∥ 最小者。
- 超定方程组（ m > n m > n m>n，方程多于未知数）
-
无精确解：通常不存在满足所有方程的 x x x。
-
最小二乘解
求解
min x ∥ A x − b ∥ 2 , \min_x \|A x - b\|^2, xmin∥Ax−b∥2,
通解为正规方程
A T A x = A T b ⟹ x ∗ = ( A T A ) − 1 A T b . A^T A\,x = A^T b \quad\Longrightarrow\quad x^* = (A^T A)^{-1} A^T b. ATAx=ATb⟹x∗=(ATA)−1ATb.
在数值上，推荐使用 QR 分解 或 SVD 求解最小二乘，以保证稳定性。
即
- m = n m=n m=n：直接求逆或分解；
- $m：参数化通解，常取伪逆给出最小范数解；
- m > n m>n m>n：最小二乘，正规方程或分解方法。
一般来说通解是 x ∗ = ( A T A ) − 1 A T b . x^* = (A^T A)^{-1} A^T b. x∗=(ATA)−1ATb.但是会出现一些问题
- 超定方程组（ m > n m>n m>n，方程多于未知数）
-
最小二乘解
x ∗ = ( A T A ) − 1 A T b x^* = (A^T A)^{-1} A^T b x∗=(ATA)−1ATb -
数值问题
- 条件数放大： κ ( A T A ) ≈ κ ( A ) 2 \kappa(A^TA)\approx\kappa(A)^2 κ(ATA)≈κ(A)2，小的浮点误差被平方放大，解可能极不准确。
- 对 b b b 的微小扰动敏感：当 b → b + δ b b\to b+\delta b b→b+δb 时， x ∗ x^* x∗ 的变化往往远大于 δ b \delta b δb。
- A T A A^TA ATA 不可逆或病态：若 A A A 列不满秩， A T A A^TA ATA 奇异；即便可逆，也可能因病态（高条件数）而不可靠。
-
改进方法
-
QR 分解：令 A = Q R A=Q R A=QR，解 R x = Q T b R\,x=Q^T b Rx=QTb，避免直接构造并求逆 A T A A^TA ATA。
-
SVD／伪逆： A = U Σ V T A=U \Sigma V^T A=UΣVT，取
x ∗ = V Σ + U T b , x^* = V\,\Sigma^+\,U^T b, x∗=VΣ+UTb,
可处理秩亏且更稳定。 -
正则化（Ridge）：
x ∗ = ( A T A + λ I ) − 1 A T b , λ > 0 x^* = (A^T A + \lambda I)^{-1}A^T b,\quad \lambda>0 x∗=(ATA+λI)−1ATb,λ>0
对抗病态与噪声。
-