有关节约束超冗余机械臂的增益优化轨迹规划

王文瑞;刘克俭;顾金麟;李昂;储海荣;朱明超;徐振邦

【摘 要】为了实现超冗余机械臂的实时在线规划,本文提出一种基于雅克比转置矩阵的人工势场轨迹规划方法.轨迹规划不仅要满足末端跟踪精度要求,而且要满足关节速度和角度约束,关节速度主要由轨迹规划算法的增益决定,而增益的大小决定系统稳态性能的好坏,通过优化势场函数和使用加权关节速度,在避免关节的前提下减小关节速度范数,从而能选择更大的增益.使用蒙特卡洛法建立最大关节速度与增益的关系,从而确定增益范围.分别在点对点运动和轨迹跟踪运动中选取不同的增益证明算法的正确性和有效性,并在轨迹跟踪运动中引入速度前馈,通过李雅普诺夫稳定性定理证明算法稳定性.通过以超冗余机械臂为模型仿真验证,得出在点对点运动下末端位置偏差小于10-4 m m,姿态偏差小于1×10-5 rad;轨迹跟踪运动的位置偏差小于10-3 mm,姿态偏差小于1×10-4 rad.最后进行实验验证,虽然实验过程中轨迹偏差相比于仿真增加一个数量级,但仍符合实验任务需求. 【期刊名称】《光学精密工程》 【年(卷),期】2019(027)005 【总页数】12页(P1075-1086)

【关键词】人工势场;关节约束;加权关节速度;蒙特卡洛法;速度前馈;李雅普诺夫稳定性定理

【作 者】王文瑞;刘克俭;顾金麟;李昂;储海荣;朱明超;徐振邦

【作者单位】中国科学院长春光学精密器械与物理研究所 ,吉林长春130033;中国科学院大学材料与光电研究中心 ,北京100049;中国人民大学遥感中心,北京

100038;中国科学院长春光学精密器械与物理研究所 ,吉林长春130033;中国科学院大学材料与光电研究中心 ,北京100049;中国科学院长春光学精密器械与物理研究所 ,吉林长春130033;中国科学院大学材料与光电研究中心 ,北京100049;中国科学院长春光学精密器械与物理研究所 ,吉林长春130033;中国科学院长春光学精密器械与物理研究所 ,吉林长春130033;中国科学院长春光学精密器械与物理研究所 ,吉林长春130033 【正文语种】中 文 【中图分类】TP242.6 1 引 言

随着人工智能的普及和发展，工业生产的精密化和智能化程度不断提高。冗余机械臂具有很高的自由度和灵活性，满足现代化精密装配或者精细化操作的需求，因此它在将来的智能化进程中有着不可替代的作用。冗余机械臂指自由度大于6的机械臂，对于机械臂而言，自由度越高，灵活性越高，避障能力越强，同时机械臂的运动学也会更加复杂，尤其是自由度大于9的超冗余机械臂。冗余机械臂应用的关键是通过运动学规划一组轨迹来执行特定的任务，并且在这一过程中机械臂的跟踪精度、关节角度和速度等要满足指标。

目前，冗余机械臂轨迹规划方法主要有几何参数法[1-2]，迭代算法[3-6]和代数数值解法[7-11]3种。其中，几何参数法是通过解析各关节角与机械臂末端位姿的关系，从而确定特定的末端姿态所对应的关节角，该方法精度高，计算速度快，但是要求机械臂具有特定的结构，局限性较大。迭代算法是将机械臂的逆运动学问题简化为迭代计算的优化求解问题或约束变分问题，可以使用神经网络[4-5]或遗传算法求解[6]，但是由于迭代法运算量较大，所以很难用于实时在线控制。代数数值

解法通过使用雅克比矩阵或其变形，根据末端执行器期望位姿的变化关系实时计算关节角度，包括雅克比矩阵伪逆法[7-8]、最小二乘法[9-10]和梯度投影法[11]等。这种方法通用性好，可用于实时控制，然而，累积误差的存在会影响控制精度。通过改善代数数值解法的控制过程可以减小甚至消除累积误差，其通用性强、实时性好的特点会使它成为更优越的冗余机械臂轨迹规划方法。

基于人工势场的轨迹规划的代数数值解法是一种使用雅克比转置矩阵将机械臂末端在势场中受到的力转化为关节速度的方法。然而，为了满足关节速度约束和数值计算的稳定性要求，使用雅克比转置矩阵轨迹规划会导致增益过低的问题。因为关节速度与机械臂末端在势场中受到的力成正相关，本文把圆锥形势阱和抛物线势阱结合起来定义引力场函数，使机械臂末端受到的力在一个阈值内。在机械臂末端受到的力最大的情况下，通过蒙特卡洛法建立关节最大加权速度与增益的关系，确定增益范围，保证无论点对点运动还是轨迹跟踪运动，都可以选择较为合适的增益提高轨迹规划算法的性能。对于轨迹跟踪运动，需要引入雅克比矩阵的伪逆与末端期望速度的乘积作为前馈[12]才能克服累积误差问题，保证轨迹跟踪误差收敛，并使用李雅普诺夫稳定性定理对点对点运动和轨迹跟踪运动规划算法的稳定性加以证明。本文以自主研制的九自由度超冗余机械臂为模型，通过MATLAB仿真和实物实验验证了所提出方法的可行性和优越性。 2 超冗余机械臂结构参数

图1 九自由度超冗余机械臂Fig.1 Nine-DOF hyper-redundant manipulator 九自由度超冗余机械臂结构如图1所示。该机械臂采用仿蛇形机器人关节的十字相交设计，此种结构令该机械臂不仅具有传统机械臂的装配和打磨等功能，而且具有蛇形机器人的移动能力，能移动到特定位置实现安装重构。

本文使用D-H参数法对机械臂的关节和连杆进行运动学建模。根据图2的D-H坐标系建立方式，可以得到表1所示的机械臂关节参数。

图2 九自由度超冗余机械臂坐标系Fig.2 Coordinate system of 9-DOF hyper-redundant manipulator表1 9自由度超冗余机械臂D-H参数 Tab.1 Denavit-Hartenberg parameters of 9-DOF hyper-redundant manipulator

关节θnαn-1an-1/mmdn关节角/(°)1θ100 0±902θ2-φ/2165.50±903θ3φ/2165.50±904θ4-φ/2165.50±905θ5φ/2165.50±906θ6-φ/2165.50±907θ7φ/2165.50±908θ8-φ/2165.50±909θ9φ/2165.50±90 3 增益优化算法

实际应用过程中，受机械臂结构以及电机参数等的影响，轨迹规划算法不仅要满足末端执行器的跟踪精度要求，还要满足关节角度和关节速度等约束条件。尤其在使用雅克比转置矩阵进行轨迹规划时，速度约束会导致算法增益局限于一个非常小的值，无法保证算法性能。因此，对于使用雅克比转置矩阵规划冗余机械臂的一个重点就是选择一个较为合适的增益，提高算法性能。 3.1 优化人工势场函数

n自由度机械臂末端执行器初始位姿所对应的初始构型和对应目标位姿的最终构型在图3中分别用实线和虚线表示，其中末端执行器的目标位姿所对应的关节的构型不是唯一的。从示意图中可以获得两个位姿的关系： ptarget=pinitial+Δp， (1)

其中ptarget，pintial和Δp分别表示目标位姿、初始位姿及这两个位姿之间的偏差。它们都是6×1的列向量，包括3个平移分量和3个旋转分量，所以末端执行器的任何姿态都可以写成: p=[pt pr]T=[x y z α β γ]T. (2)

假设存在一个虚拟势场，该势场提供的力能使机械臂末端执行器从初始位姿运动到目标位姿。这个力不仅能使末端执行器平动也能使它发生转动，也就是说人工势场提供的力作用在机械臂末端执行器上能使其6个状态分量发生改变，因此， 这个力也是个六维向量，而对应的势力场也应该被定义为六维势场。 图3 势场原理示意图Fig.3 Schematic of potential field

如上所述，力F是以目标位姿为场源所构建的人工势场所提供的，F直接作用在机械臂末端使其运动到目标位姿，所以这个势场需要满足一些标准。显而易见，势场应随着初始位姿和目标位姿之间的偏差增加而增大。然而，如果只定义势场和偏差的线性关系，即所谓圆锥形势阱，由于位于场源处的场强为零而存在不连续性[13]，所以会引起稳定性问题。为了获得连续可导的势场，定义与目标位姿误差成平方增长的势场，即抛物线势阱如下： Uatt=[Ut Ur]T= (3)

其中ξ是用来调节势场作用效果的一个参数，因为势力场对末端执行器各位姿分量的作用效果不同，不妨假设势场对位姿各个平移分量的作用相同，且对旋转分量的作用也相同，所以ξ可表示为： ξ=diag(ξt,ξt,ξt,ξr,ξr,ξr). (4)

F就可表示为： F=[Ft Fr]T=

[Fx Fy Fz Fα Fβ Fγ]T= ξΔp=ξ[Δx Δy Δz Δα Δβ Δγ]T. (5)

其中Fi(i=1,2,…,6)会随着姿态误差Δpi的减小线性趋近于零，也会随着误差Δpi的增加无地变大。这种情况下，如果初始位姿pintiali与目标位姿ptargeti的偏差太大，F将会产生一个非常大的分量，关节速度也会相应地增大。因此，将抛物线势阱与圆锥形势阱结合起来定义用于轨迹规划的势力场。当末端执行器当前位姿与距期望位姿偏差较大时，使用圆锥形势阱定义；反之，则使用抛物线势阱定义。当然，圆锥形势场与抛物线势场的界限η需要明确定义，显而易见，平动分量和转动分量的界限不能用同一个标准定义，所以η可表示为： η=[ηt ηr]T, (6)

其中ηt和ηr分别表示平动和转动分量的界限，势力场定义为: (7)

由此Fi表示为： (8)

在如此定义的情况下，整个势场的强度都是连续的，由势场所形成的力也是连续的，力Fi的阈值为 3.2 加权关节速度

人工势场所形成的虚拟力F作用在机械臂的末端执行器上，将F映射成关节力矩： τ=JTF. (9)

其中：τ表示作用在机器人关节上的虚拟扭矩，是一个9×1的向量；JT为雅克比矩阵的转置，是一个9×6的矩阵，可表示为：

(10)

其中：末端位姿关于θi的传动雅克比：为了满足关节角度约束，并且在一定程度上减小关节角速度，这里使用加权最小范数优化JT矩阵： (11)

其中W=diag(w1,w2,…,wi,…,w9)，wi的定义如下： (12)

其中H(θ)是为了避免关节构造的指标函数。如图4所示，H(θ)可以表示为： (13)

图4 指标函数Fig.4 Indicator function

假设机械臂不具有关节电机且只有黏性摩擦存在，关节速度可表示为： (14)

其中：B表示关节黏性摩擦系数(假设所有关节的黏性摩擦系数相同[14])，关节角度θ可以通过对关节速度积分获得。 机械臂一个关节的加权速度可以表示为： (15)

其中K=ξ/B是一个6×6的对角阵，作为增益，通过调节其大小来控制位姿偏差对关节速度的影响。根据图4，wi的取值是1～+，所以通过规划关节速度时，能在一定程度上起到减小关节速度范数的作用。尤其当关节角度趋近角度时，对应

关节速度趋近0能保证规划过程满足关节角度约束。 3.3 蒙特卡洛法求解增益范围

考虑机械臂的任意关节构型，关节速度主要由增益及机械臂末端在人工势场中受到的虚拟力决定。根据优化人工势场函数定义的函数可知，作用在机械臂的虚拟力，其任意方向的分量的绝对值都不会超过考虑到关节的速度，不妨假设机械臂末端任意分量受到的力均为该势场下的最大值，由此在选定增益K的情况下，关节速度可表示为： (16)

采用蒙特卡洛法，结合9自由度超冗余机械臂运动学模型对机械臂的雅克比矩阵进行分析，原理为： J={f(θ):θ∈Q}⊂R6×9, (17)

其中J为雅克比矩阵， f(θ)为求取雅克比矩阵的关节变量函数，θ为广义关节变量，Q为关节空间，R6×9表示雅克比矩阵的维度。根据上文所提到的机械臂关节参数，每一个关节的运动范围均为-90°≤θi≤90°，根据式(16)的映射关系，在关节运动范围内，通过均匀分布对关节变量赋予一定数量的随机量，从而求得机械臂随机构型对应的雅克比矩阵。

由设定的增益求取关节速度，由式(4)增益K可写成K=diag(kt kt kt kr kr kr)，表示机械臂末端平移的单位为毫米(mm)，而表示姿态角的单位为弧度(rad)。考虑平移分量及旋转分量的变化范围并适应所使用的计量单位，满足控制精度的要求，令kr=10 000kt。选取两组η的值，通过对关节最大速度的讨论选取最合适的增益矩

阵。蒙特卡洛法求取最大关节速度主要分为以下几个步骤： (1)通过均匀分布对每一个关节变量定义数目相同的随机值。

(2)根据关节角度求取对应的雅克比矩阵，从而得到雅克比矩阵的转置进而求取关节速度。

(3)选取绝对值最大的关节速度，改变关节变量数目，绘制最大速度与关节变量数目的关系图。

选取 η1=[10 0.1]，η2=[0.01 0.000 1]，得到关节速度的最大绝对值与关节变量数目的关系如图5所示。

图5 最大关节速度与关节变量数目的关系Fig.5 Relation of maximum joint speed and number of joint variables

对于一个选定的机器人，因为其结构参数是确定的，且存在关节角度约束，根据雅克比矩阵的定义，雅克比矩阵的每一个分量都有最大值。所以一旦当选定增益K和η，关节的最大角速度必然是有一个临界值的，在应用蒙特卡洛法求解的过程中所取关节角数目变量越多，关节最大角速度越接近真实值，因为关节角速度不能大于2 rad/s，所以对于图5中两种情况，当η=η1时，kt应该不大于1.11×10-4；当η=η2时，kt应该不大于0.111。因为通过蒙特卡洛法算出的最大关节速度是一个十分接近但仍略小于实际最大关节速度的值，所以应该在保证数值计算稳定性的前提下选择一个较小的kt。 4 轨迹规划算法设计

根据机械臂末端的运动形式，其运动轨迹可以分为点到点的运动，和轨迹跟踪运动即沿着期望的轨迹连续运动[15]。针对两种运动形式设计算法，因为点对点运动的初始位姿偏差可能较大，所以要在满足机械臂关节约束的前提下，分别选取不同的增益来提高轨迹规划算法的性能。

4.1 点对点运动轨迹规划

点对点轨迹规划原理如图6所示。根据初始位姿与期望位姿的偏差大小，在满足快速性和关节约束的前提下选择合适的增益矩阵，结合当前角度信息通过正运动学求取机械臂末端的当前位姿进行实时规划。

图6 点对点轨迹规划原理图Fig.6 Block diagram of point-to-point motion trajectory

点对点轨迹规划算法的稳定性可以通过李雅普诺夫稳定性定理证明，选取李雅普诺夫函数为： (18)

其中Δx=xtar-x。因为xtar是个定值，所以可以表示为： (19) 使用替代 (20) 根据式 (21)

其中K和Kω均为正对角阵，由此可得总是负定的，所以点对点轨迹规划的误差是收敛的。

4.2 轨迹跟踪运动轨迹规划

点到点的运动是轨迹跟踪运动的基础，轨迹跟踪运动可以分解为无数组点到点的运动，如图7所示。假设每两个相邻位姿之间的偏差足够小以至于每次运动都十分

接近预定的期望位姿，当前位置信息与期望位置信息随着机械臂的运动不断更新，在期望轨迹上选取足够多的位姿作为期望位姿时，就可以认为机械臂末端执行器沿着期望轨迹运动。由此在轨迹跟踪运动规划过程中也可以选择较大的增益。 图7 轨迹跟踪运动示意图Fig.7 Schematic of trajectory tracking motion 图8 轨迹跟踪运动规划原理图Fig.8 Block diagram of trajectory tracking motion

然而由于整个过程中期望位姿xd是不断更新的，所以整个规划过程中存在累积误差，且无法通过点对点轨迹规划稳定性证明方法直接证明其稳定性，为了保证整个轨迹规划过程中误差收敛，把期望轨迹的速度与雅克比伪逆矩阵J+的乘积作为前馈输入，如图8所示，其中J+=JT(JJT)-1。选择相同的李雅普诺夫函数： (22)

其中：可以表示为： (23)

在式(23)中，使用替代并根据图5可得到:

-ΔxTKJJTKΔx. (24)

所以这里的也总是负定的，那么整个算法是稳定的，并且机械臂末端执行器沿期望轨迹运动过程中误差是收敛。不过引入雅克比矩阵的伪逆，会导致运算过程中出现伪逆矩阵奇异的问题，为了避免这点这里使用雅克比矩阵的奇异鲁棒性逆[16]。 J*=JT(JJT+λI)-1.

(25)

当矩阵趋近于奇异值时，λ取一个足够小的值避免矩阵奇异，并保证不会产生较大的跟踪误差，当矩阵远离奇异值时λ=0。这样既保证了整个系统的稳定性，又避免了出现奇异值的情况。 5 仿真结果及分析

本文提出的轨迹规划方法可以保证在点对点运动的轨迹规划过程中，无论初始位姿与目标位姿的偏差多大，都可以规划一组满足关节速度约束和角度约束的运动。选取机械臂的初始位姿和目标位姿分别为pintial=(1 262.1,337.5,0,0,0,0.698 1)，ptarget=(584.50,707.98,-621.10,-1.201,0.581,2.106)，其中旋转分量由α，β，γ角表示。为了保证系统的稳态性能和速度，需要选取较大的增益，不妨令η=[10,0.1]，同时取kt=1×10-4，仿真结果如图9所示。

图9 点对点运动规划结果Fig.9 Results of point-to-point motion

从图9(a) 中可以看出，机械臂任意关节的速度小于1.5 rad/s，由此可以证明，即使位姿偏差较大，选取的增益依然能保证点对点运动过程中满足关节速度约束。 因为使用雅克比矩阵对机械臂进行轨迹规划时位姿偏差是指数收敛的[17],所以轨迹跟踪误差也是渐进趋向于0。由图9(c)和9(d)可知，当仿真时间为20 s时，机械臂末端执行器位置偏差小于10-4 mm，姿态偏差小于10-5 rad。当减小η，取更大增益时，某一位姿分量的跟踪误差会明显增大，例如在上文的点对点运动中取η=η2=[0.01 0.000 1]，得到γ方向分量偏差如图10所示。因此在点对点运动时，尤其是位姿的各个分量偏差都很大时，应在满足约束的前提下选择较大的η。 图10 γ分量误差Fig.10 Rotation component error

定义轨迹跟踪运动机械臂的末端期望轨迹为: (26)

由于机械臂末端的初始位姿位于期望轨迹上，所以可以取较大的增益来使系统获得更好的稳态性能，令K=diag(0.1，0.1，0.1，1 000,1 000,1 000)。在增益相同的条件下，将引入和不引入速度前馈的轨迹跟踪误差进行对比，以此来验证算法的优越性，仿真结果如图11所示。

图11 轨迹跟踪运动规划结果Fig.11 Results of trajectory tracking motion 从图11(a)～11(c)可以验证引入速度前馈能有效减少跟踪误差，抑制规划过程中的累积误差，保证轨迹跟踪过程中位置偏差小于10-3 mm，姿态偏差小于1×10-4 rad。图11(d)～11(e)显示了当关节接近角度时关节角度趋近于0，有效地保证了关节角度不超过，满足关节角度约束。

综上所述，在对存在关节角度约束和速度约束的机械臂进行轨迹规划的过程中，无论是点对点的运动还是轨迹跟踪的运动规划，都可以通过蒙特卡洛法确定增益范围，选择一个较为合适的增益，并且通过引入最小范数和速度前馈来跟踪误差更小更符合要求的轨迹。 6 实 验

为了验证本文所提出的基于关节约束的增益优化超冗余机械臂轨迹规划方法，使用上文提到的九自由度超冗余机械臂进行圆弧轨迹跟踪实验，上位机根据机械臂末端在笛卡尔空间的期望轨迹实时解算机械臂的关节角，并将关节位置指令实时发送给机械臂，机械臂采用位置控制模式。为了突出实验效果，实验中机械臂末端在笛卡

尔空间画圆，并且尽量增加圆的半径，实验轨迹如下： (27)

实验结果如图12所示。实验过程中关节角度变化如图13(a)所示，关节速度变化如图13(b)所示。在实验过程中关节角度满足-π/2～π/2的，且关节速度远小于的2 rad/s。在实验过程中测量x，y和γ三个分量的偏差，如图13(c)～13(e)所示，可以看出x分量的最大偏差小于8×10-4 mm，y分量的最大偏差小于4×10-3 mm，γ方向的最大偏差小于4×10-4 mm，表明实验过程中偏差在仿真基础上的增加在一个数量级左右。

图12 机械臂末端做圆弧运动的实验结果Fig.12 Experimental results of circular motion by manipulator end-effector

图13 机械臂运动跟踪实验曲线Fig.13 Experimental curves of manipulator motion tracking

综上所述，本文设计的基于关节约束的增益优化超冗余机械臂轨迹规划方法在实验过程中，虽然摩擦力、振动和扰动力矩等造成的误差会使实验误差相比于仿真结果有所增加，但实验过程中关节速度及角度满足要求，并且各分量误差也满足机械臂的任务需求。 7 结 论

本文提出了一种以势力场为基础的使用雅克比矩阵转置的轨迹规划方法，通过使用最小范数法和蒙特卡洛法，在满足关节速度和角度约束的同时，一定程度上改善了使用雅克比矩阵转置进行轨迹规划过程中增益太低的缺点，有效提高了轨迹规划系统的稳态性能。仿真结果表明：点对点的轨迹规划不仅能满足关节速度约束，也能保证在一定时间内实际位姿与期望位姿平移分量的偏差小于10-4 mm，旋转分量

偏差小于10-5 rad；对于轨迹跟踪运动，平移分量偏差小于10-3 mm，旋转分量偏差小于10-4 rad。实验中由于扰动影响姿态偏差比仿真偏差增大一个数量级左右，不过仍满足实验需求，由此证明了本文提出算法的可行性。 参考文献：

【相关文献】

[1] 陈鹏,刘璐,余飞,等. 一种仿人机械臂的运动学逆解的几何求解方法[J].机器人, 2012, 34(2): 211-216.

CHEN P，LIU L，YU F，et al.. A geometrical method for inverse kinematics of a kind of humanoid manipulator [J]. Robot，2012, 34(2): 211-216. (in Chinese)

[2] ISIAH Z, LUIS B. A novel closed-form solution for the inverse kinematics of redundant manipulators through workspace analysis [J]. Mechanism and Machine Theory, 2018, 121: 829-843.

[3] CARLOS F, FLORA F, WOLFRAM E，et al.. Position-based kinematics for 7-DoF serial manipulators with global configuration control, joint limit and singularity avoidance [J]. Mechanism and Machine Theory, 2018, 121: 317-334.

[4] ZHANG Y N, WANG J, XIA Y S. A dual neural network for redundancy resolution of kinematically redundant manipulators subject to joint limits and joint velocity limits [J]. IEEE Transactons on Neural Networks, 2003, 14(3): 658-667.

[5] LUO X P, WEI W. A new immune genetic algorithm and its application in redundant manipulator path planning [J]. Journal of Field Robotics, 2010, 21(3): 141-151. [6] 欧群文，贠超，杨学兵，等. 基于神经网络的冗余机械臂运动学逆解研究[J]. 机电工程, 2016,33(6): 663-667.

OU Q W，YUN CH，YANG X B，et al.. Algorithm based on neural network for inverse kinematics of redundant manipulator [J]. Journal of Mechanical & Electrical Engineering, 2016, 33(6): 663-667. (in Chinese)

[7] GUO D S, XU F, YAN L C. New pseudoinverse-based path-planning scheme with PID characteristic for redundant robot manipulators in the presence of noise [J]. IEEE Transactions on Control Systems Technology, 2017: 1-12.

[8] YAZDANI M, NOVIN R S, MASOULEH M T, et al.. An experimental study on the failure tolerant control of a redundant planar serial manipulator via pseudo-inverse approach [C].

Rsi International Conference on Robotics and Mechatronics. IEEE, 2015: 365-370. [9] WAN J, WU H T, MA R, et al.. A study on avoiding joint limits for inverse kinematics of redundant manipulators using improved clamping weighted least-norm method [J]. Journal of Mechanical Science & Technology, 2018, 32(3):1367-1378.

[10] KUMAR S，SUKAVANAM N，BALASUBRAMANIAN R. Anoptimization approach to solve the inverse kinematics of redundant manipulator [J]. International Journal of Information and Systems Sciences，2010，6(4): 414-423.

[11] 黄水华. 多约束下的机械臂运动控制算法研究[D]. 杭州： 浙江大学， 2016.

HUANG SH H. Research on Kinematic Control of Manipulator with Multiple Constraints [D]. Hangzhou: Zhejiang University, 2016.(in Chinese)

[12] 朱明超, 刘慧, 张鑫,等. 惯性稳定平台自适应前馈控制[J]. 光学 精密工程, 2015, 23(1):141-148. ZHU M CH, LIU H, ZHANG X, et al.. Adaptive feed-forward control for inertially stabilized platform [J]. Opt. Precision Eng., 2015, 23(1):141-148. (in Chinese)

[13] MARK W S, SETH H, VIDYASAGAR M. Robot Modeling and Control [M]. John Wiley & Sons Inc, 19.

[14] PETER C. Robotics, Vision and Control [M]. Springer International Publishing, 2017. [15] 谭民, 徐德, 侯增广, 等. 先进机器人控制[M]. 北京:高等教出版社, 2006.

TAN M, XU D, HOU Z G, et al.. Advanced Robot Control [M]. Beijing: Higher Education Press, 2006. (in Chinese)

[16] BUSS S R. Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods [J]. IEEE Journal of Robotics and Automation, 2004, 17(16): 1-19.

[17] PIETRO F, CIRO N. On the stability of closed-loop inverse kinematics algorithms for redundant robots [J]. IEEE Transactions on Robotics, 2011, 27(4)：780-784.