社区首页 >专栏 >七自由度冗余机械臂梯度投影逆运动学

七自由度冗余机械臂梯度投影逆运动学

原创

ZC_Robot机器人技术

发布于 2020-10-28 15:55:17

6.9K0

冗余机械臂的微分逆运动学一般可以增加额外的优化任务。最常用的是梯度投影算法 GPM (Gradient Project Method)，文献 [1] 中第一次将梯度投影法应用于关节极限位置限位中。该算法中设计基于关节极限位置的优化指标，并在主任务的零空间中完成任务优化。此种思想也用于机械臂的奇异等指标优化中。 Colome 等对比分析了速度级微分逆向运动学中的关节极限位置指标优化问题，但是其研究中的算法存在一定的累计误差，因而系统的收敛性和算法的计算稳定性难以得到保证。其他学者综合多种机器人逆向运动学方法，衍生出二次计算方法、梯度最小二乘以及模糊逻辑加权最小范数方法等算法。Flacco 等针对七自由度机械臂提出一种新的零空间任务饱和迭代算法，当机械臂到达关节限位时，关节空间利用主任务的冗余度进行构型调整，从而使得机械臂回避极限位置。近年来，关于关节极限回避情况下的冗余机械臂运动规划成为了很多学者的研究方向，相应的改进策略也很多.

冗余自由度机械臂的运动学分析的难点在于其逆运动学具有多解性，即对应于同一末端（任务空间）位姿，其关节位置序列有无穷多个，也即对应的机械臂空间构型有无穷多个。同时，也正是由于冗余自由度机械臂的这种特性使其具有更好的灵活性，在避障、避奇异、避关节超限和优化性能指标等方面具有天然的优势。

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

对于冗余机械臂，对应于相同的末端位置和姿态，机械臂存在无数种构型。所以当末端位姿确定时，其关节仍运动的现象叫做机械臂的“自运动”，这是冗余机械臂在实际运动过程中与非冗余机械臂最大的区别，也是冗余机械臂冗余度的具体体现

1 系统建模

机械臂采用“3-1-3” 构型，共有 7 个自由度，且各自由度均为旋转自由度。忽略关节的柔性效应，将机械臂视为刚体，采用修正的 D-H 方法建模。建立的 D-H 坐标系如图，其中，l_0=l_8=700;l_1=l_2=l_6=l_7=430;l_4=387;(mm)

此时，坐标系o_{i-1}x_{i-1}y_{i-1}z_{i-1}和o_ix_iy_iz_i 的齐次变换矩阵为

在不同的构型下，其各关节连杆长度a_{i-1}连杆转角\alpha_{i-1}和连杆偏距d_i均是定值。

机械臂末端坐标系相对于基座坐标系的齐次变换矩阵为

冗余自由度机械臂的正运动学方程可用下式描述：

p=f(q)

机械臂的运动学求解，一般从其速度层面进行分析。将上式对时间求导可得

\dot p= J \dot q

用雅可比矩阵可以建立起机械臂末端运动速度与各关节速度之间的关系，以及末端与外界接触力和对应各关节力矩之间的关系，因此雅可比矩阵在机械臂的运动学和动力学分析中占有重要地位。为方便求解，本文采用矢量积方法计算七自由度冗余空间机械臂的雅可比矩阵。矢量积方法求取雅可比矩阵的过程是逐列分别计算，然后由所有列组成机械臂的m-n雅可比矩阵。

如果关节 i 为移动关节，则雅可比矩阵的第 i 列为

J_i=\begin{bmatrix}Z_i \\ 0\\ \end{bmatrix}

如果关节 i 为转动关节，则雅可比矩阵的第 i 列为：

J_i=\begin{bmatrix}Z_i \times ^ip^0_n \\ Z_i\\ \end{bmatrix}

由以上可求得机械臂的雅可比矩阵：

J=\begin{bmatrix}J_1 ,J_2.....J_n\\ \end{bmatrix}

2 奇异鲁棒逆解与梯度投影法

机械臂的逆运动学分析是指：已知任务空间速度向量求解关节空间速度向量, 对于非冗余自由度机械臂，一般可用其雅可比

矩阵的逆矩阵求解：

\dot q=J^{-1}\dot p

对于冗余自由度机械臂，由于其雅可比矩阵是一个长方阵，无法计算它的逆，此时用其伪逆表示

\dot q=J^{+}\dot p

针对某一具体构型的机械臂，上式可以视为一般的线性方程组求解问题，在雅可比矩阵行满秩的条件下，其通解为

\dot q=J^{+}\dot p+(I-J^{+1}J)\dot q_0

前项即为伪逆解，后项中q_0 为关节空间的任一速度矢量，正是通过调节q_0可实现冗余自由度机械臂的性能指标优化。采用这种方法进行机械臂的逆运动学求解称为伪逆法，亦称为最小范数法，因为其满足以下关系：

\begin{cases} min\left\|\dot q\right\| \\[5ex] subject -to: \left\|\dot p-J\dot q\right\| \end{cases}

通过对伪逆J^+进行奇异值分解可知：

J^+=U^TD^+V

D^+ (R^{n \times m}) 是由雅可比矩阵的伪逆J^+ 的奇异值组成的对角矩阵。且满足：

D^+(i,i)= \begin{cases} \cfrac {1}{\sigma_{m+1-i}}, & \sigma_{m+1-i} \ne 0 \\[5ex] 0, & \sigma_{m+1-i} = 0 \end{cases}

最小奇异值常用于表示当前时刻机械臂构型的奇异程度.

可得由雅可比伪逆法求得的关节速度范数：

\left\|\dot q\right\| =\sqrt{\sum^m_i \cfrac {\gamma^2_i}{\sigma^2_i}}

采用伪逆法求解机械臂的逆运动学，当机械臂接近奇异位型时，求解获得的机械臂关节速度急剧变大，即实际上获得的逆运动学解并不可行. 当采用伪逆法求解机械臂的逆运动学时，之所以产生关节速度急剧变大的情况，是因为以保证末端跟踪精度为前提条件，也就是保证了末端跟踪误差最小的优先级总是高于关节速度最小的优先级。但事实上，完全可以以牺牲部分末端跟踪精度为代价使得机械臂在奇异位型附近的关节速度不至于过大。采用这一思路进行机械臂的逆运动学求解即为阻尼最小二乘法.

阻尼最小二乘法求解的目标问题是：

\begin{cases} min\left\|\dot q\right\| \\[5ex] subject -to: \left\|\dot p-J\dot q\right\|+\lambda^2 \left\|\dot q\right\|^2 \end{cases}

由此可以得到奇异鲁棒通解：

\dot q= J^*\dot p+(I-J^*J)\dot q_0

其中，J^*=R^{nm},J^*=J^T(JJ^T+I)^{-1} 称为雅可比矩阵的奇异鲁棒性逆.

同样对奇异鲁棒性逆 J^*进行奇异值分解，可得由阻尼最小二乘法求得的关节速度范数：

\left\|\dot q\right\| =\sqrt{\sum^n_i \cfrac {\lambda^2\gamma^2_i}{(\lambda^2+\sigma^2_i)}}

由上式可知，由于阻尼系数的存在，即使机械臂的雅可比矩阵的最小奇异值\sigma_m 很小，只要\lambda 取合适的值，也可以使机械臂的关节速度不会过大。

根据式可求得采用阻尼最小二乘法时的末端跟踪误差为：

\left\|\dot p-J\dot q\right\| =\sqrt{\sum^r_{i=1} \cfrac {\lambda^2\gamma^2_i}{(\lambda^2+\sigma^2_i)}+\sum^m_{i=r+1}\gamma^2_i}

为了降低机械臂的末端跟踪误差，引入变阻尼系数，以雅可比矩阵的最小奇异\sigma_m 来度量机械臂接近奇异位型的程度：

\lambda^2= \begin{cases} \lambda^2_0(1-\cfrac {\sigma_m}{\sigma_0}), & {\sigma_m<\sigma_0} \\[5ex] 0, & {\sigma_m \geq \sigma_0} \end{cases}

采用阻尼最小二乘法实现了机械臂在奇异位型区间内逆运动学解的可行性，但并未发挥出冗余自由度在性能指标优化方面的优势。

采用梯度投影法，通过对关节空间速度矢量q_0取不同的值可实现性能指标的优化，设要优化的性能指标H(q) 则取关节空间速度矢量q_0为

\dot q_0=K\nabla H(q)

结合阻尼最小二乘法和梯度投影法各自优点

\dot q=J^*\dot p+K(I-J^*J)\nabla H(q)

前项即为奇异鲁棒性逆解，后项为性能优化指标的梯度H(q)在零空间的投影。同样，以雅可比矩阵的最小奇异值 \sigma_m表示当前时刻机械臂构型的奇异程度，可以采用变优化系数

K= \begin{cases} 0, & {0\leq\sigma_m<\sigma_0} \\[5ex] k_m[sin(\pi\sigma/\sigma_0)+\pi/2+1]/2, & {\sigma_0 \leq \sigma_m <2\sigma_0}\\[5ex] k_m, & {\sigma_m \geq 2\sigma_0} \end{cases}

H(q)取为避关节极限指标，当进行轨迹优化时，避关节极限指标越小越好：

H(q)=\sum^n_{i=1} \cfrac{1}{n}\cfrac{(q_{imax}-q_{imin})^2}{(q_{imax}-q_{i})(q_{i}-q_{imin})}

则其梯度向量可表示为

\nabla H(q)=[\frac{\partial H}{\partial q_1},\frac{\partial H}{\partial q_2},......,\frac{\partial H}{\partial q_n}]^T

其中：

\cfrac {\partial {H}}{\partial q_{i}}= \cfrac{1}{n}\cfrac{(q_{imax}-q_{imin})^2(2q_i-q_{imax}-q_{imin})}{(q_{imax}-q_{i})^2(q_{i}-q_{imin})^2}

3 总结

本文给出的基于奇异鲁棒逆和梯度投影法的结合是最符合大部分机器人运动控制算法的。也是实际工程中应用最多的速度级别的逆向运动学。奇异鲁棒逆保证了机器人在奇异构型下仍能很好的进行机器人的运动控制，梯度投影主要是针对冗余机器人的运动控制算法，其可以充分利用机器人的冗余度实现特定目标的优化。

Maciejewski A A, Klein C A. Obstacle avoidance for kinematically redundant manipulators in dynamically varying nvironments[J]. The international journal of robotics research, 1985, 4(3): 109-117.

原创声明：本文系作者授权腾讯云开发者社区发表，未经许可，不得转载。

如有侵权，请联系 cloudcommunity@tencent.com 删除。

机器人