1 概述

机械臂的运动学研究机械臂关节空间和任务空间的映射关系，分为正运动学和逆运动学。其中，在关节空间已知的条件下求解任务空间的位置和姿态称为正动学问题；相反，通过已知的任务空间求解关节空间各个关节的位置，称为逆运动学问题。冗余机器人的逆向运动学主要有以下几种方法：

本节着重在于基于任务扩充的方法解决冗余机器人逆向运动学的问题。对于冗余机械臂的位置控制，本章中采用基于运动学的构型控制理论进行机械臂位置控制。基于该理论可以避免遇到机械臂奇异构型时关节速度突变。

6自由度空间机械臂具有完整的笛卡尔位姿控制能力。笛卡尔空间某一位姿，6自由度非冗余机械臂仅具有有限组关节角度以及构型与之对应。所以对于已经非冗余机械臂的控制，其运动控制是唯一确定的。与6自由度空间非冗余机械臂相比，空间冗余机械臂可以利用冗余自由度进行运动学与动力学优化，从而达到更好的关节运动限制，避奇异，避障和优化关节驱动力矩等[i]。因此，在空间环境中利用冗余机械臂可以更有效的完成相应的空间复杂情况下的任务。

当 $m<n$ 时，其为运动学冗余机械臂，冗余自由度 $r=m-n$ 。对于相同的机械臂末端轨迹，关节运动情况无法唯一确定。对于冗余机械臂，对应于相同的末端位置和姿态，机械臂存在无数种构型。所以当末端位姿确定时，其关节仍运动的现象叫做机械臂的“自运动”，这是冗余机械臂在实际运动过程中与非冗余机械臂最大的区别，也是冗余机械臂冗余度的具体体现。

2 构型控制原理

为了消除冗余度的影响，确定某时刻下机械臂的构型，因此需要扩展 $r$ 维任务向量 $X_c$ 。扩展任务可以选择与关节角度相关的运动学函数，所选择的运动学函数可以作为机械臂自运动的一个指标。其与机械臂末端位姿向量 $X_e$ 均为与关节角相关的函数

$X_e=f(q)$

$X_c=g(q)$

冗余机械臂的构型在关节空间可以由其各关节角度唯一确定，其在笛卡尔空间可以由机械臂末端笛卡尔位置和姿态以及扩展的运动学函数唯一确定，其构成机械臂的构型变量，如下所示：

$X=\begin{bmatrix}X_e \\ X_c\\ \end{bmatrix}=\begin{bmatrix}f(q) \\ g(q)\\ \end{bmatrix}$

对式两边求导可以得到增广运动学微分式

$\dot X=\begin{bmatrix}J_e(q) \\ J_c(q)\\ \end{bmatrix}\dot q=J\dot q$

式中 $J_e(q)$ :主任务雅克比矩阵, $J_c(q)$ ：扩展任务雅克比矩阵； $J(q)$ :增广雅克比矩阵.

在构型控制中，与扩展任务相关的雅克比矩阵应与主任务雅克比矩阵线性无关，否则增广雅克比矩阵不满秩。

冗余机械臂的期望构型取决于笛卡尔空间末端轨迹 $X^d_e$ ，以及运动学扩展任务 $X^d_c$ ,即 $X^d=[X^d_e;X^d_c]$ ；对于冗余机械臂的分解，末端主任务速度级误差 $\dot E = \dot X-J\dot q$ ;若由 $\dot q = J^{-1}\dot X^d$ ，求出关节角度代入误差中，则 $\dot E=0$ ；。但是这种求解方法没有考虑到机械臂的避关节限制，避免奇异等任务

为了优化冗余机械臂的控制，在求解冗余机械臂各关节角度值时，引用优化的思想，选择如下正定目标函数

$L=\dot E^T_eW_e\dot E_e+\dot E^T_cW_c\dot E_c+\dot q^TW_v\dot q$

其中权重 $W_v$ 可以避免关节速度在奇异构型时的突变，但 $W_v$ 的增加同时任务完成的误差也会增加。通常：

$W_e=diag[w_{ei}],W_c=diag[w_{ci}],W_v=diag[w_{vi}]$

为了找到目标函数 $L$ 取值最小的关节角速度 $\dot q$ 的值，首先对目标函数L的表达式两边 $\dot q$ 取导数，可以得到：

$\frac {\partial {L}}{\partial{\dot q}}=-2J^TWX^d+2J^TWJ\dot q+2W_v\dot q$

令 $\frac {\partial {L}}{\partial{\dot q}}=0$ ，则可以得到

$\dot q^d=[J^TWJ+W_v]^{-1}J^TW\dot X^d =[J^T_eW_eJ_e+J^T_cW_cJ_c+W_v]^{-1}[J^T_eW_e\dot X_e+J^T_cW_c\dot X_c]$

对该期望加速度积分可以得到关节空间运动轨迹。

3 冗余机械臂臂角

七自由度冗余机械臂，其运动学函数可以选择机械臂的臂角。臂角即机械臂臂平面与某参考平面之间的夹角。通过臂角参数化七自由度冗余机械臂的自运动，即若机械臂有自运动现象则其臂平面会相应转动，进而臂角也在改变。

以 $1、4$ 和 $7$ 关节处坐标原点组成的平面作为该七自由度机械臂的臂平面。为了确定机械臂构型，通过某一时刻机械臂臂平面与某参考平面之间的夹角(臂角)来确定。

在下图中，w $O_1,O_4,O_7$ 分别对应机械臂的肩部 $S$ 、肘部 $E$ 以及腕部 $W$ .设 $e=O_1O_4,w=O_1O_7$ , $V$ 为为基坐标系的垂直方向单位向量。定义参考平面的单位向 $\hat l=l/\left\|l\right\|$ ,其中 $l=(w\times V)\times w$ 。将 $e$ 投影到 $w$ 可以得到 $d=\hat w(\hat w^Te)$ ，规定向量 $\hat \delta=\delta/\left\|\delta\right\|$ 表示向量 $\delta$ 的单位向量，则 $\hat w=w/\left\|w\right\|$ .点 $E$ 到 $w$ 的垂直距离向量 $p=e-d=(I-\hat w\hat w^T)e$ .

臂角的几何定义

文献中给出了臂角的正切计算公式如下：

$tan\psi=\frac{\hat w^T(\hat V \times p)}{\hat V p}$

进而，机械臂的臂形角的计算公式如下

$\psi=atan2 [{\hat w^T(\hat V \times p)},{\hat V p}]$

臂角为冗余机械臂各个关节角的函数，对于臂角的变化速率与各关节角速度的关系可以通过臂角雅克比矩阵(扩展任务雅克比

$J_c$ )得到：

$\psi=J_c\dot q$

为了求解雅克比矩阵 $J_{\psi}$ ，先引入与 $e$ 和 $w$ 相关的雅克比矩阵，由于 $e$ 与 $w$ 为与机械臂各个关节角度相关的函数，即 $\dot e=J_1\dot q$ , $\dot w=J_2 \dot q$ ,且 $J_1$ 和 $J_2$ 可以由下式计算：

$J_1=(z_1\times O_{41}，z_2\times O_{42}，z_3\times O_{43}，0，……，0)$

$J_2=(z_1\times O_{71}，……，z_6\times O_{76}，0)$

由于 $\psi$ 为向量 $l$ 到向量 $p$ 的角度， $\dot \psi$ 可以表示如下：

结合J_1的表达式和J_2的表达式,可以得到：

进一步推导可以得到

[[i]]Seraji H. Configuration control of redundant manipulators: Theory and implementation[J]. Robotics and Automation, IEEE Transactions on, 1989, 5(4): 472-490.

V2CE – 七自由度冗余机械臂扩展任务逆运动学:扩展雅可比法

1 概述

2 构型控制原理

3 冗余机械臂臂角