冗余机器人的控制核心思想是零空间的充分利用，即在冗余机器人的零空间内完成奇异优化、操作度优化、关节极限位置优化，力矩优化，甚至阻抗控制或者导纳控制。最核心的是以下五个方面

冗余机器人的梯度投影逆向运动学
速度级别的多优先级逆向运动学
加速度级别的多优先级逆向运动学
零空间内力矩优化
零空间内阻抗控制

图片链接：here

1 冗余机器人梯度投影逆向运动学

假设机器人关节空间变量 $q \in R^n$ , $n$ 是机器人的自由度，机器人笛卡尔空间 $x \in R^m$ ,二者之间的关系可以采用雅可比矩阵联系， $\dot x=J\dot q$ , $J \in R^{m \times n}$ 。

基于梯度投影的逆向运动学具体表达式如下

$\dot q=J^+\dot x+(I-J^+J)\nabla H(q)$

$\nabla H(q)$ 为优化指标 $H(q)$ 的梯度方向。

2 速度级别多优先级逆向运动学

假设机器人笛卡尔任务 $x$ 中存在任务优先级， $x=[x_1,x_2],$ $x_1 \in R^{m1}, x_2 \in R^{m2}$ , 机器人的正向运动学具体如下所示

$\dot x_k=J_k(q)\dot q，k=1,2$

依据机器人的梯度投影法，两个层次的逆向运动学具体如下所示

$\dot q=J^+_1\dot x_1+(I-J^+_1J_1)\dot q_0=J^+_1\dot x_1+N_1\dot q_0$

将其带入到 $\dot x_2=J_2(q)\dot q$ ，则

$\dot x_2=J_2J^+_1\dot x_1+J_2(I-J^+_1J_1)\dot q_0=J_2J^+_1\dot x_1+J_2N_1\dot q_0\\ \dot q_0=(J_2(I-J^+_1J_1))^+(\dot x_2-J_2J^+_1\dot x_1)=(J_2N_1)^+(\dot x_2-J_2J^+_1\dot x_1)$

进一步带入到任务1 的雅可比零空间内

$\dot q=J^+_1\dot x_1+(I-J^+_1J_1)(J_2N_1)^+(\dot x_2-J_2J^+_1\dot x_1)=J^+_1\dot x_1+N_1(J_2N_1)^+(\dot x_2-J_2J^+_1\dot x_1)\\=J^+_1\dot x_1+(J_2N_1)^+(\dot x_2-J_2J^+_1\dot x_1)$

上述表达式将任务2的实现放在任务1 的零空间内实现，从而使得机器人会首先完成任务1，于此同时，尽可能完成任务2.即任务1 是高优先级任务，任务2 为次优先级任务。

如果机器人在完成任务1和任务2 的同时，仍然有充足的自由度，则仍然存在零空间优化

$\dot q=J^+_2\dot x_2+(I-J^+_2J_2)\dot q_0=J^+_2\dot x_2+N_2\dot v_0$

将基于任务1 的运动学代入其中，可以得到

$\dot q=J^+_1\dot x_1+(J_2N_1)^+(\dot x_2-J_2J^+_1\dot x_1)+N_2\dot v_0$

3 加速度级多优先级逆向运动学

对 $\dot x_k=J_k(q)\dot q，k=1,2$ 两边求导数，可以得到

$\ddot x_k=J_k(q)\ddot q+\dot J_k(q)\dot q，k=1,2$

采用类似的推导，可以得到

$\ddot q=J^+_1(\ddot x-\dot J_1\dot q)+\hat J^+_2(\ddot x_2-\dot J_2\dot q-J_2J^+_1(\ddot x_1-\dot J_1\dot q))+N_2v_0$

4 零空间内力矩优化

机器人的动力学模型可以有

$M(q)\ddot q+C(q,\dot q)\dot q+G(q)=\tau$

在机器人雅可比零空间内进行动力学量的优化是机器人运动控制的重要一环节。可以根据如下模型

$min G_T=\tau^T\tau\\ subject.to:\ddot x-\dot J\dot q=J\ddot q，\tau=M\ddot q+N$

为了进行动力学量的优化，优化目标可以转化为

$G_T=\left\| M\ddot q+N\right\|=\left\| MJ^+(\ddot x-\dot J\dot q)+N\right\|$

由此可以得到优化关节驱动力矩的零空间向量如下所示：

$q_0=-[MN]^+[MJ^+(\ddot x-\dot J\dot q)+N]$

因而机器人关节角加速度为

$\ddot q=J^+(\ddot x-\dot J\dot q)-[MN]^+[MJ^+(\ddot x-\dot J\dot q)+N]$

5 零空间阻抗控制

机器人关节空间阻抗控制可以表示如下所示

$M_d(\ddot q^d-\ddot q)+B_d(\dot q^d-\dot q)+K_d(q^d-q)=\tau_j\\ \ddot q=\ddot q^d+M^{-1}_d(B_d(\dot q^d-\dot q)+K_d(q^d-q)-\tau_j)$

由此可以得到包含机器人的关节空间阻抗的零空间控制

$\ddot q=J^+(\ddot x-\dot J\dot q)+N[\ddot q^d+M^{-1}_d(B_d(\dot q^d-\dot q)+K_d(q^d-q)-\tau_j)]$

多优先级运动控制；速度级别逆向运动学；加速度级别逆向运动学；冗余机器人；

绝密笔记 | 冗余机器人多优先级控制：速度级/加速度级

1 冗余机器人梯度投影逆向运动学

2 速度级别多优先级逆向运动学

3 加速度级多优先级逆向运动学

4 零空间内力矩优化

5 零空间阻抗控制