机器人阻抗控制（二）

#阻抗控制 #机器人控制 #动力学方程
《Cartesian Impedance Control of Redundant and Flexible-Joint Robots》

One of the core statements of the impedance control methodology is that the controller should modulate the mechanical impedance4 of the manipulator.

具有外部输入力矩的任务空间动力学方程

根据[[力、运动-力混合控制#关节空间中的动力学方程|关节空间机器人动力学方程]]（不考虑关节柔性与关节阻尼）：
$$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}+\mathbf{g}(\mathbf{q})=\mathbf{\tau }+{\mathbf{\tau }}_{ext}\text{\hspace{1em}(1)}$$
式中，${\mathbf{\tau }}_{ext}$表示外部力矩向量。
根据[[开链机器人的静力学#1. 雅可比矩阵|雅可比矩阵]]定义可知，末端位姿与关节角之间的[[阻抗控制（一）#阻抗控制算法|微分运动关系]]为：
$$\begin{array}{rl}\dot{\mathbf{x}}& =\frac{\partial \mathbf{f}\left(\mathbf{q}\right)}{\partial \mathbf{q}}\dot{\mathbf{q}}=\mathbf{J}\left(\mathbf{q}\right)\dot{\mathbf{q}}\\ \ddot{\mathbf{x}}& =\mathbf{J}\left(\mathbf{q}\right)\ddot{\mathbf{q}}+\dot{\mathbf{J}}\left(\mathbf{q}\right)\dot{\mathbf{q}}\end{array}\text{\hspace{1em}(2)}$$

根据式(2)可知，$\dot{\mathbf{q}}={\mathbf{J}}^{-1}\left(\mathbf{q}\right)\dot{\mathbf{x}}$，$\ddot{\mathbf{q}}={\mathbf{J}}^{-1}\left(\mathbf{q}\right)\left[\ddot{\mathbf{x}}-\dot{\mathbf{J}}\left(\mathbf{q}\right)\dot{\mathbf{q}}\right]$，将其带入式(1)，并考虑到${\mathbf{\tau }}_{ext}={\mathbf{J}}^{-T}\left(\mathbf{q}\right){\mathbf{F}}_{ext}$，可以得到[[开链机器人的静力学#3. 开链机器人的静力学|任务空间的动力学方程]]，
M ( q ) J − 1 ( q ) ( x ¨ − J ˙ ( q ) q ˙ ) + C ( q , q ˙ ) J − 1 ( q ) x ˙ + g ( q ) = τ + J T ( q ) F e x t ⇒ J − T ( q ) M ( q ) J − 1 ( q ) ( x ¨ − J ˙ ( q ) q ˙ ) + J − T ( q ) C ( q , q ˙ ) J − 1 ( q ) x ˙ + J − T ( q ) g ( q ) = J − T ( q ) τ + F e x t ⇒ J − T ( q ) M ( q ) J − 1 ( q ) x ¨ + J − T ( q ) C ( q , q ˙ ) J − 1 ( q ) x ˙ − J − T ( q ) M ( q ) J − 1 ( q ) J ˙ ( q ) J − 1 ( q ) x ˙ + J − T ( q ) g ( q ) = J − T ( q ) τ