仿人机器人运动控制（1）：简化动力学模型

Robot, Python

介绍了足式机器人的动力学模型，包括运动模型、角速度动力学近似简化、动力学简化等内容，并给出了相应的公式和参考文献。

足式机器人动力学

由于机器人腿部质量占比较小，运动模型通常忽略腿部质量简化成全身质量集中于躯干一点CoM（center of mass）。足底接触点反作用力 \(\mathbf{f}_i \in \mathbb{R}^3\)，到CoM的向量表示为 \(\mathbf{r}_i \in \mathbb{R}^3\)。世界坐标系下刚体动力学公式

\[ \ddot{\mathbf{p}}=\frac{\sum_{i=1}^n \mathbf{f}_i}{m}-\mathbf{g} \tag{1} \]

\[ \frac{\mathrm{d}}{\mathrm{d} t}(\mathbf{I} \boldsymbol{\omega})=\sum_{i=1}^n \mathbf{r}_i \times \mathbf{f}_i \tag{2} \]

\[ \dot{\mathbf{R}}=[\boldsymbol{\omega}]_{\times} \mathbf{R} \tag{3} \]

其中 \(\mathbf{p} \in \mathbb{R}^3\)表示机器人位置，\(m\)表示机器人质量，\(\mathbf{g} \in \mathbb{R}^3\) 表示重力加速度，

\(\mathbf{I} \in \mathbb{R}^3\) 表示机器人惯性张量， \(\boldsymbol{\omega} \in \mathbb{R}^3\) 表示为机器人角速度，\(\mathbf{R}\) 表示为从局部坐标系到世界坐标系下的旋转变换矩阵。 \([\mathbf{x}]_{\times} \in \mathbb{R}^{3 \times 3}\) 是一个反对称矩阵（满足\(A^T = -A\)， \(A[i][i] = 0\) ）即\([\mathbf{x}]_{\times} \mathbf{y}=\mathbf{x} \times \mathbf{y},\mathbf{x}, \mathbf{y} \in \mathbb{R}^3\)。

公式 (2) (3) 包含了非线性动力学，需要线性近似化，避免模型预测器求解非凸优化问题。

角速度动力学近似简化

机器人的姿态用欧拉角 Z-Y-X 表示 \(\boldsymbol{\Theta}=\left[\begin{array}{lll}\phi & \theta & \psi\end{array}\right]^{\top}\)，其中 \(\psi\) 表示 yaw 角， \(\theta\) 为 pitch 角， \(\phi\) 为 roll 角。从机器人局部坐标系到世界坐标系下的欧拉角需要经过系列变换，表示为

\[ \mathbf{R}=\mathbf{R}_z(\psi) \mathbf{R}_y(\theta) \mathbf{R}_x(\phi) \tag{4} \]

其中\(\mathbf{R}_n(\alpha)\)表示绕 \(n\) 轴旋转角度 \(\alpha\) 。世界坐标系下角速度表示为

\[ \boldsymbol{\omega}=\left[\begin{array}{ccc} \cos (\theta) \cos (\psi) & -\sin (\psi) & 0 \\ \cos (\theta) \sin (\psi) & \cos (\psi) & 0 \\ -\sin (\theta) & 0 & 1 \end{array}\right]\left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right] \tag{5} \]

当\(\cos (\theta) \neq 0\)时，公式 (5) 可表示为

\[ \left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right]=\left[\begin{array}{ccc} \cos (\psi) / \cos (\theta) & \sin (\psi) / \cos (\theta) & 0 \\ -\sin (\psi) & \cos (\psi) & 0 \\ \cos (\psi) \tan (\theta) & \sin (\psi) \tan (\theta) & 1 \end{array}\right] \boldsymbol{\omega} \tag{6} \]

当 roll 角和 pitch 角 \((\phi, \theta)\) 值较小时，即机器人躯干绕 x 轴和 y 轴没有发生较大的俯仰和摇摆，保持水平上的稳定，上式（6）近似为

\[ \left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right] \approx\left[\begin{array}{ccc} \cos (\psi) & \sin (\psi) & 0 \\ -\sin (\psi) & \cos (\psi) & 0 \\ 0 & 0 & 1 \end{array}\right] \boldsymbol{\omega} \tag{7} \]

上式简化为

\[ \left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right] \approx \mathbf{R}_z^{\top}(\psi) \boldsymbol{\omega} \tag{8} \]

注意，欧拉角旋转的顺序直接影响最终的计算结果

那么公式 （2）线性化近似表示为

\[ \frac{\mathrm{d}}{\mathrm{d} t}(\mathbf{I} \boldsymbol{\omega})=\mathbf{I} \dot{\boldsymbol{\omega}}+\boldsymbol{\omega} \times(\mathbf{I} \boldsymbol{\omega}) \approx \mathbf{I} \dot{\boldsymbol{\omega}} \tag{9} \]

上式忽略了机器人旋转体的进动和章动的影响，故 \(\boldsymbol{\omega} \times(\mathbf{I} \boldsymbol{\omega})\) 项数值较小，可省略。世界坐标系下机器人惯性张量表示为

\[ \mathbf{I}=\mathbf{R}_{\mathcal{B}} \mathbf{I} \mathbf{R}^{\top} \tag{10} \]

当 roll 角和 pitch 角较小时，近似为

\[ \hat{\mathbf{I}}=\mathbf{R}_z(\psi){ }_{\mathcal{B}} \mathbf{I} \mathbf{R}_z(\psi)^{\boldsymbol{\top}} \tag{11} \]

其中 \({ }_{\mathcal{B}} \mathbf{I}\) 为躯干坐标系下惯性张量。

动力学简化

基于上一节假设，带入机器人动力学公式

\[ \begin{aligned} & \frac{\mathrm{d}}{\mathrm{d} t}\left[\begin{array}{c} \hat{\mathbf{\Theta}} \\ \hat{\mathbf{p}} \\ \hat{\boldsymbol{\omega}} \\ \hat{\dot{\mathbf{p}}} \end{array}\right]=\left[\begin{array}{llll} \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{R}_z^{\top}(\psi) & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{1}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \end{array}\right]\left[\begin{array}{c} \hat{\mathbf{\Theta}} \\ \hat{\mathbf{p}} \\ \hat{\boldsymbol{\omega}} \\ \hat{\dot{\mathbf{p}}} \end{array}\right]+ \\ & {\left[\begin{array}{ccc} \mathbf{0}_3 & \cdots & \mathbf{0}_3 \\ \mathbf{0}_3 & \cdots & \mathbf{0}_3 \\ \hat{\mathbf{I}}^{-1}\left[\mathbf{r}_1\right]_{\times} & \cdots & \hat{\mathbf{I}}^{-1}\left[\mathbf{r}_n\right]_{\times} \\ \mathbf{1}_3 / m & \cdots & \mathbf{1}_3 / m \end{array}\right]\left[\begin{array}{c} \mathbf{f}_1 \\ \vdots \\ \mathbf{f}_n \end{array}\right]+\left[\begin{array}{c} 0 \\ 0 \\ 0 \\ \mathbf{g} \end{array}\right]} \\ & \end{aligned} \tag{12} \]

上式（12）改写为状态空间形式

\[ \dot{\mathbf{x}}(t)=\mathbf{A}_c(\psi) \mathbf{x}(t)+\mathbf{B}_c\left(\mathbf{r}_1, \ldots, \mathbf{r}_n, \psi\right) \mathbf{u}(t) \tag{13} \]

其中\(\mathbf{A}_c \in \mathbb{R}^{13 \times 13}\)和\(\mathbf{B}_c \in \mathbb{R}^{13 \times 3 n}\)。基于躯干保持水平状态，状态空间方程简化为线性表示，主要取决于 yaw 角和落足点的位置变化，动力学方程近似变为线性时变。

公式总结

假设：

  • （1）机器人质量集中于躯干；
  • （2）运动过程中躯干的 roll 角和 pitch 角变化忽略不计；

存在如下变换 \[ \begin{aligned} \dot{\boldsymbol{\Theta}} & \approx \boldsymbol{R}_z(\psi) \boldsymbol{\omega} \\ \mathcal{G} \boldsymbol{I} & \approx \boldsymbol{R}_z(\psi)_{\mathcal{B}} \boldsymbol{I} \boldsymbol{R}_z(\psi)^{\top}, \end{aligned} \tag{14} \]

其中

  • \(\dot{\Theta}=\left[\begin{array}{lll}\dot{\phi} & \dot{\theta} & \dot{\psi}\end{array}\right]^{\top}\)为躯干欧拉角 roll （\(\phi\)），pitch（\(\theta\)），yaw（\(\psi\)）对应的角速度；
  • \(\boldsymbol{R}_z(\psi)\)为全局坐标系下的旋转\(\omega\)到局部坐标系下的欧拉角旋转变换矩阵；
  • \({ }_{\mathcal{G}} \boldsymbol{I}\) 和 \({ }_{\mathcal{B}} \boldsymbol{I}\) 分别是从全局和局部坐标系下的惯性张量；

\[ \frac{d}{d t}(\boldsymbol{I} \boldsymbol{\omega})=\boldsymbol{I} \dot{\boldsymbol{\omega}}+\boldsymbol{\omega} \times(\boldsymbol{I} \boldsymbol{\omega}) \approx \boldsymbol{I} \dot{\boldsymbol{\omega}} \tag{15} \]

通过以上简化，系统的离散动力学可以表示为

\[ \mathbf{x}(k+1)=\boldsymbol{A}_k \mathbf{x}(k)+\boldsymbol{B}_k \hat{\mathbf{f}}(k)+\hat{\mathbf{g}} \tag{16} \]

其中

\[ \begin{aligned} \mathbf{x} & =\left[\begin{array}{llll} \boldsymbol{\Theta}^{\top} & \mathbf{p}^{\top} & \boldsymbol{\omega}^{\top} & \dot{\mathbf{p}}^{\top} \end{array}\right]^{\top}, \\ \hat{\mathbf{f}} & =\left[\begin{array}{lll} \mathbf{f}_1 & \cdots & \mathbf{f}_n \end{array}\right]^{\top}, \\ \hat{\mathbf{g}} & =\left[\begin{array}{llll} \mathbf{0}_{1 \times 3} & \mathbf{0}_{1 \times 3} & \mathbf{0}_{1 \times 3} & \mathbf{g}^{\top} \end{array}\right]^{\top}, \end{aligned} \tag{17} \]

\[ \begin{aligned} \boldsymbol{A} & =\left[\begin{array}{cccc} \mathbf{1}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \boldsymbol{R}_z\left(\psi_k\right) \Delta t & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{1}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{1}_{3 \times 3} \Delta t \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{1}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{1}_{3 \times 3} \end{array}\right], \\ \boldsymbol{B} & =\left[\begin{array}{ccc} \mathbf{0}_{3 \times 3} & \cdots & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \cdots & \mathbf{0}_{3 \times 3} \\ \mathcal{G} \boldsymbol{I}^{-1}\left[\mathbf{r}_1\right]_{\times} \Delta t & \cdots & { }_{\mathcal{G}} \boldsymbol{I}^{-1}\left[\mathbf{r}_n\right]_{\times} \Delta t \\ \mathbf{1}_{3 \times 3} \Delta t / m & \cdots & \mathbf{1}_{3 \times 3} \Delta t / m \end{array}\right] . \end{aligned} \tag{18} \]

参考文献

  • Di Carlo, Jared, et al. “Dynamic locomotion in the mit cheetah 3 through convex model-predictive control.” 2018 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2018.