机器人动力学建模

机器人动力学在机器人设计、控制以及碰撞检测中是不可或缺的。常见的动力学建模方法有拉格朗日方程、牛顿欧拉方程以及虚功原理。

1、 拉格朗日方程

拉格朗日方程以系统的能量为基础建立动力学模型，可以避免内部刚体之间出现的作用力，简化了建模过程，但其物理意义不明确，而且对于复杂系统，拉格朗日函数的微分运算将变得十分繁琐。

1.1 动能

连杆的动能 = 移动动能 + 转动动能
$$K=\frac{1}{2}m{\mathbf{v}}_c^T{\mathbf{v}}_c+\frac{1}{2}{\mathbf{\omega }}^T{\mathbf{I}}_c{\mathbf{\omega }}_c$$

1.2 势能

连杆的势能 = 重力势能 + 零势能点
$$P=-m{\mathbf{g}}^T{\mathbf{p}}_c+m{\mathbf{g}}^T{\mathbf{h}}_c$$

1.3 拉格朗日算子

$$L(\mathbf{\theta },\dot{\mathbf{\theta }})=K(\mathbf{\theta },\dot{\mathbf{\theta }})-P(\mathbf{\theta })$$

1.4 拉格朗日方程

$$\frac{d}{dt}\frac{\partial L}{\partial \dot{\mathbf{\theta }}}-\frac{\partial L}{\partial \mathbf{\theta }}=\mathbf{\tau }$$

2、 牛顿-欧拉方程

牛顿方程描述了刚体平移所受的外力与刚体质心处质量和质心处加速度之间的关系；欧拉方程描述了刚体旋转所受的外力矩与角速度、角加速度和惯性张量之间的关系；因此，可以使用牛顿-欧拉方程建立刚体动力学方程。

2.1 线速度

$${\mathbf{r}}_A={\mathbf{r}}_{AB}{+}^A{\mathbf{R}}_B{\cdot }^B{\mathbf{r}}_B$$
等式两边同时对时间$t$求导可得：
$${\mathbf{v}}_A={\mathbf{v}}_{AB}{+}^A{\dot{\mathbf{R}}}_B{\cdot }^B{\mathbf{r}}_B{+}^A{\mathbf{R}}_B{\cdot }^B{\mathbf{v}}_B$$
根据导数和微分的定义可知：
$${}^A{\dot{\mathbf{R}}}_B={\mathbf{S}(\mathbf{\omega })}^A{\mathbf{R}}_B=\left[\begin{array}{ccc}0& -{\omega }_z& -{\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$
由于坐标系$B$和点$P$位于同一个连杆，因此${}^B{\mathbf{r}}_B$为定值，所以${}^B{\mathbf{v}}_B=0$
综上可知：
$${\mathbf{v}}_A={\mathbf{v}}_{AB}+\mathbf{S}(\mathbf{\omega }){\cdot }^A{\mathbf{R}}_B{\cdot }^B{\mathbf{r}}_B$$
即：
$${\mathbf{v}}_A={\mathbf{v}}_{AB}+\mathbf{\omega }\times {(}^A{\mathbf{R}}_B{\cdot }^B{\mathbf{r}}_B)$$

2.2 线加速度

对上式求导可得：
$${\mathbf{a}}_A={\mathbf{a}}_{AB}+\mathbf{\beta }\times {(}^A{\mathbf{R}}_B{\cdot }^B{\mathbf{r}}_B)+\mathbf{\omega }\times \mathbf{\omega }\times {(}^A{\mathbf{R}}_B{\cdot }^B{\mathbf{r}}_B)+{\mathbf{a}}_B$$

2.3 角速度

$${}^A{\mathbf{\omega }}_c{=}^A{\mathbf{\omega }}_B{+}^A{\mathbf{R}}_B{\cdot }^B{\mathbf{\omega }}_c$$

2.3 角加速度

等式两边同时对时间$t$求导得：
$${}^A{\mathbf{\beta }}_c{=}^A{\mathbf{\beta }}_B{+}^A{\dot{\mathbf{R}}}_B{\cdot }^B{\mathbf{\omega }}_c{+}^A{\mathbf{R}}_B{\cdot }^B{\mathbf{\beta }}_c$$
即：
$${}^A{\mathbf{\beta }}_c{=}^A{\mathbf{\beta }}_B{+}^A{\mathbf{\omega }}_B\times {(}^A{\mathbf{R}}_B{\cdot }^B{\mathbf{\omega }}_c){+}^A{\mathbf{R}}_B{\cdot }^B{\mathbf{\beta }}_c$$