差速移动机器人MPC中误差状态矩阵和控制矩阵推导

最新推荐文章于 2025-09-06 10:22:24 发布

原创最新推荐文章于 2025-09-06 10:22:24 发布 · 342 阅读

5 ·

CC 4.0 BY-SA版权

文章标签：

#矩阵 #机器人

【Part 1：基于全局坐标】

将状态变量和控制变量表示为：

$X=\begin{bmatrix} x\\ y \\\theta \end{bmatrix}$ $u=\begin{bmatrix} \omega_l\\ \omega_r \end{bmatrix}$

定义误差状态方程和控制状态方程：

$\tilde{X} = X-X_{ref}$ $\tilde{u} = u-u_{ref}$

将误差状态方程表示为：

$\dot{\tilde{X}}=A\tilde{X}+B\tilde{u}$

其中： $A=\begin{bmatrix} 0 & 0 & -v_{ref}sin\theta_{ref}\\ 0& 0 &v_{ref}cos\theta_{ref} \\ 0&0 &0 \end{bmatrix}$ $B=\begin{bmatrix} \frac{R}{2} cos\theta_{ref}&\frac{R}{2} cos\theta_{ref} \\ \frac{R}{2} sin\theta_{ref}& \frac{R}{2} cos\theta_{ref}\\ \frac{R}{L} & -\frac{R}{L} \end{bmatrix}$

$v_{ref}$ 是参考速度， $\theta_{ref}$ 是参考角度。

具体推导过程见下图。

【Part 1：基于局部坐标】

定义误差状态：

$e_x=x-x_{ref}$ $e_y=y-y_{ref}$ $e_{\theta}= \theta-\theta_{ref}$

$e=\begin{bmatrix} e_x\\e_y \\ e_{\theta} \end{bmatrix}$

由几何关系，状态误差随时间变化如下：

$\dot{e_x}=v_rcose_{\theta}-(v_r+\Delta v)cos(\theta_r+e_{\theta})$

$\dot{e_y}=v_rsine_{\theta}-(v_r+\Delta v)sin(\theta_r+e_{\theta})$

$\dot{e_{\theta}}=\omega-\omega_r$

其中，vr,ωr 是参考轨迹的速度和角速度，Δv=v−vr是线速度误差

然后对小角度 $e_\theta$ 进行一阶泰勒展开：

$cos(\theta_r+e_{\theta})\approx cos\theta_r-e_{\theta}sin{\theta_r}$ $sin(\theta_r+e_{\theta})\approx sin\theta_r+e_{\theta}cos\theta_r$

代入忽略高阶项：

$\dot{e_x}= -e_y\omega_r +\Delta v$

$\dot{e_y}= e_x\omega_r +v_re_{\theta}$

$\dot{e_{\theta}}=\Delta \omega$

转换为矩阵的形式：

$\begin{bmatrix} \dot{e_x}\\\dot{e_y} \\ \dot{e_{\theta}} \end{bmatrix}=\begin{bmatrix} 0 & -\omega_r &0 \\ \omega_r& 0 &v_r \\ 0&0 & 0 \end{bmatrix}\begin{bmatrix} e_x\\e_y \\ e_{\theta} \end{bmatrix}+\begin{bmatrix} 1&0\\ 0&0\\ 0&1 \end{bmatrix}\begin{bmatrix} \Delta v\\ \Delta \omega \end{bmatrix}$