速度到位姿转换 (Velocity to Pose)#
机器人运动学：速度到位姿的转换#
基本关系#
从线速度 \(v\) 和角速度 \(\omega\) 到位姿 \([x, y, \theta]^T\) 的转换关系为：
\[
\begin{bmatrix}
x(t+T) \\
y(t+T) \\
\theta(t+T)
\end{bmatrix}
=
\begin{bmatrix}
x(t) \\
y(t) \\
\theta(t)
\end{bmatrix}
+
\int_{t}^{t+T}
\begin{bmatrix}
v\cos\theta(\tau) \\
v\sin\theta(\tau) \\
\omega
\end{bmatrix}
d\tau
\]
两种常见情况#
1. 直线运动 (\(\omega = 0\))#
\[
\begin{cases}
x(t+T) = x(t) + vT\cos\theta(t) \\
y(t+T) = y(t) + vT\sin\theta(t) \\
\theta(t+T) = \theta(t)
\end{cases}
\]
2. 曲线运动 (\(\omega \neq 0\))#
精确解为：
\[
\begin{cases}
x(t+T) = x(t) + \dfrac{v}{\omega}[\sin(\theta(t) + \omega T) - \sin\theta(t)] \\
y(t+T) = y(t) - \dfrac{v}{\omega}[\cos(\theta(t) + \omega T) - \cos\theta(t)] \\
\theta(t+T) = \theta(t) + \omega T
\end{cases}
\]
小角度近似 (\(\omega T \ll 1\))#
当 \(\omega T\) 很小时，使用泰勒展开近似：
\[
\begin{cases}
x(t+T) \approx x(t) + vT\cos\theta(t) - \dfrac{v\omega T^2}{2}\sin\theta(t) \\
y(t+T) \approx y(t) + vT\sin\theta(t) + \dfrac{v\omega T^2}{2}\cos\theta(t) \\
\theta(t+T) = \theta(t) + \omega T
\end{cases}
\]
一阶近似（忽略 \(T^2\) 项）：
\[
\begin{cases}
x(t+T) \approx x(t) + vT\cos\theta(t) \\
y(t+T) \approx y(t) + vT\sin\theta(t) \\
\theta(t+T) = \theta(t) + \omega T
\end{cases}
\]
矩阵微分方程形式#
位姿变化率可表示为：
\[
\dot{\mathbf{q}} =\begin{bmatrix}
\dot{x} \\
\dot{y} \\
\dot{\theta}
\end{bmatrix}
=
\begin{bmatrix}
\cos\theta & 0 \\
\sin\theta & 0 \\
0 & 1
\end{bmatrix}
\begin{bmatrix}
v \\
\omega
\end{bmatrix}
\]
其中 \(\mathbf{q} = [x, y, \theta]^T\) 是位姿向量。
离散时间形式#
欧拉积分法#
\[
\mathbf{q}_{k+1} = \mathbf{q}_k + T
\begin{bmatrix}
\cos\theta_k & 0 \\
\sin\theta_k & 0 \\
0 & 1
\end{bmatrix}
\begin{bmatrix}
v_k \\
\omega_k
\end{bmatrix}
\]
精确积分法#
\[
\mathbf{q}_{k+1} = \mathbf{q}_k +
\begin{bmatrix}
\dfrac{v_k}{\omega_k}[\sin(\theta_k + \omega_k T) - \sin\theta_k] \\
-\dfrac{v_k}{\omega_k}[\cos(\theta_k + \omega_k T) - \cos\theta_k] \\
\omega_k T
\end{bmatrix}
\]
特殊情况#
纯旋转 (\(v = 0, \omega \neq 0\))#
\[
\begin{cases}
x(t+T) = x(t) \\
y(t+T) = y(t) \\
\theta(t+T) = \theta(t) + \omega T
\end{cases}
\]
纯平移 (\(\omega = 0, v \neq 0\))#
\[
\begin{cases}
x(t+T) = x(t) + vT\cos\theta(t) \\
y(t+T) = y(t) + vT\sin\theta(t) \\
\theta(t+T) = \theta(t)
\end{cases}
\]
逆变换#
从位姿差计算平均速度：
\[
\begin{aligned}
v &= \frac{\sqrt{(x_2 - x_1)^2 + (y_2 - y_1)^2}}{T} \\
\omega &= \frac{\Delta\theta}{T} = \frac{\theta_2 - \theta_1}{T}
\end{aligned}
\]
其中 \(\Delta\theta\) 需进行角度环绕处理。
几何解释#
- 运动轨迹是半径为 \(R = \frac{v}{\omega}\) 的圆弧
- 弧长为 \(s = vT\)
- 圆心角为 \(\phi = \omega T\)
- 弦长为 \(2R\sin\frac{\phi}{2} = \frac{2v}{\omega}\sin\frac{\omega T}{2}\)
误差分析#
- 欧拉法误差：\(O(T^2)\)
- 精确解：无近似误差（假设速度恒定）
- 实际应用中，\(T\) 越小，近似越精确