CN115533920A - 一种求解绳驱机械臂逆运动学的协同规划方法及***、计算机存储介质 - Google Patents

Abstract

本发明公开了一种求解绳驱机械臂逆运动学的协同规划方法及***、计算机存储介质，其中方法如下：选取绳驱机械臂的末端点和部分中间关节的交点作为被控制点；在被控制点附近则自由设定对应的控制点；计算控制点和被控制点之间的欧几里得距离（欧式距离）；结合梯度投影法，将计算得到的欧式距离作为优化目标函数，通过调整控制点及比例因子等参数则可以在绳驱机械臂自运动空间内完成整臂构型的调整。规划方式为：当任意欧式距离大于对应安全阈值时，则通过基于梯度投影法的全局优化使得欧式距离逼近零，从而优化绳驱机械臂的整臂自由度，使得绳驱机械臂的整臂构型与控制点所设定构型的一致性达到最优化状态，实现“整臂构型‑末端位姿”协同规划目标。

Description

一种求解绳驱机械臂逆运动学的协同规划方法及***、计算 机存储介质

技术领域

本发明涉及机器人控制领域，尤其是一种求解绳驱机械臂逆运动学的协同规划方法及***、计算机存储介质，以适用于绳驱机械臂狭小空间作业的需求。

背景技术

区别于传统的6或7自由度绳驱机械臂，绳驱机械臂具有更多或无穷多自由度，在形态和动作上类似于大象鼻子或蛇等多节爬行动物，具有极好的环境适应性、相容性和极高的避障能力。因此，绳驱机械臂具有足够大的灵巧操作空间、能够穿越狭窄范围、对基座产生的干扰小，并具备避障、避奇异、关节力矩优化等能力，可以满足对大型非合作航天器进行全面的检测和维修的要求。为了适应在非合作航天器狭小环境中作业，需要充分发挥绳驱机械臂的灵巧运动的优势。

由于大量自由度及障碍物的存在，考虑绳驱机械臂运动学逆解将变的十分复杂，因此轨迹规划及控制一直以来都是绳驱机械臂研究中的重点和难点。通常冗余绳驱机械臂的逆运动学求解方法可以分为三类：(1)代数法(2)迭代法，例如神经网络法、遗传算法、模糊算法(3)几何法。如今通常用运动学解耦等方法简化分析过程。当特种绳驱机械臂构型的封闭解不存在时，通常采用数值法或多种算法混合的方法求解其运动学逆解。绳驱机械臂往往具有比实现末端位置姿态所需要自由度更多的自由度，在这种情况下采用数值解将变得非常有效。在逆解过程中这种超冗余的结构可以实现末端位置姿态跟踪的同时完成各种附加任务，比如障碍物回避、关节极限回避、奇异回避、优化关节速度、优化驱动力矩等，可以满足这些附加任务约束也正是冗余绳驱机械臂相对于传统绳驱机械臂的优势。Freund等提出了一种在线规划多障碍物环境下多机器人避障规划的方法。将避障规划问题转换为二次凸优化问题，为了实现障碍物的回避，该方法采用了可以直接控制关节运动的加速度求解方式，并能同时考虑潜在的碰撞危险；由于是基于模型的规划方法，因此该方法具有一定的通用性。Homayoun等提出了一种实时规划的方法。该方法将轨迹规划问题公式化为力位混合控制问题。Yoshida等基于迭代的双阶段规划方法提出了绳驱机械臂在复杂三维环境下的规划算法。Mu等针对绳驱机械臂的轨迹规划问题提出了分段几何法及改进模式函数法，研究了绳驱机械臂的运动规划问题，但还需要结合作业环境拓展适应性算法研究。

综上所述，在以往的研究中，绳驱机械臂轨迹规划问题已有了较为完善的成果。然而，针对狭小空间作业还需要结合绳驱机械臂提出具有避障、避奇异、避关节超限以及“整臂构型-末端位姿”的协同规划方法。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本发明的一个目的是提供一种求解绳驱机械臂逆运动学的协同规划方法及***、计算机存储介质，用于实现绳驱机械臂“整臂构型-末端位姿”的协同规划处理。

本发明所采用的一种技术方案是：一种求解绳驱机械臂逆运动学的协同规划方法，包括以下步骤：

步骤一，在绳驱机械臂的末端执行器和中间关节设定i个被控制点，对应的在空间设置i个控制点；

步骤二，根据绳驱机械臂的特点将被控制点设定为绳驱机械臂上每N(N＝2,4,…m，其中m为自由度总数)个自由度之间的节点，被控制点与相应控制点之间的“接近度”则为待优化目标；

步骤三，判断任意欧式距离是否大于对应安全阈值，若判断结果为是，则通过基于梯度投影法的全局优化使得欧式距离逼近零，从而得到绳驱机械臂在空间的期望构型；

步骤四，判断任意欧式距离是否为零，若判断结果为是，表示绳驱机械臂的构型与期望构型完全一致。

步骤五，判断任意欧式距离是否为零，若判断结果为否，则通过优化绳驱机械臂的整臂自由度，使得接近度的加权和最小，即绳驱机械臂的整臂构型与控制点所设定构型的一致性达到最优化状态，实现“整臂构型-末端位姿”协同规划目标。

进一步地，对于被控制点的选择，充分利用位置的冗余性，将每四个自由度之间的关节节点设定为绳驱机械臂的被控点。

对于控制点的选择，为了能够实现按照操作人员灵活调整绳驱机械臂中间构型的目标，提出基于变控制点的绳驱机械臂轨迹规划，以“按需”抓取点的方式获取控制点在空间的位置坐标，在算法中通过坐标转换的方式将抓取点投影到D-H坐标系的YZ面，垂足和圆心的连线与控制圆的交点即为控制点。

进一步地，建立绳驱机械臂构型寻优准则函数，当优化目标为0时，被控点和控制点完全重合，绳驱机械臂处于期望构型，此时问题转化为：在末端位姿不变的情况下，通过调节绳驱机械臂在零空间自运动实现基于变控制点优化臂型的问题。

进一步地，应用梯度投影法解决绳驱机械臂的空间构型在当前关节空间的某一组合下沿着哪一个方向有最大的自运动变化率的问题，优化绳驱机械臂的关节构型，同时优化绳驱机械臂关节角的角速度。

进一步地，建立关节空间与任务空间的映射雅克比矩阵，输入已知条件：

1)绳驱机械臂末端执行器在任务空间的期望位姿；

2)当前关节角度；

3)D-H坐标系参数；

4)控制点的期望位置。

进一步计算当前位姿差，利用雅克比矩阵，通过梯度投影法得到位姿差与关节变量之间的关系，计算绳驱机械臂关节新的输入，进行绳驱机械臂的运动控制，实现绳驱机械臂的运动。

进一步地，通过调整改进对应参数实现所述绳驱机械臂的协同规划处理。

进一步地，所述协同规划参数包括整臂构型参数与末端位姿参数。

本发明所采用的另一技术方案是：一种求解绳驱机械臂逆运动学的协同规划***，包括

位置选择单元：用于根据作业需求获取控制点在空间的位置坐标；

位置检测单元：用于检测绳驱机械臂当前位置，以计算计算控制点和被控制点之间的欧几里得距离；

位置判定单元：用于判定欧式距离是否大于对应安全阈值，判定控制点和被控制点之间的欧式距离是否为零；

规划单元：用于计算当前位姿差，利用雅克比矩阵，通过梯度投影法得到位姿差与关节变量之间的关系，计算绳驱机械臂关节新的输入，进行绳驱机械臂的运动控制；

输入单元：用于输入规划完成的关节构型与绳驱机械臂关节角的角速度到绳驱机械臂，实现绳驱机械臂的运动。

进一步地，通过调整改进对应参数实现所述绳驱机械臂的协同规划处理。。

进一步地，所述协同规划参数包括整臂构型参数与末端位姿参数。

本发明所采用的另一技术方案是：一种计算机存储介质，其上存储有计算机程序，所述程序被处理器执行时实现以下步骤：

步骤一，在绳驱机械臂的末端执行器和中间关节设定i个被控制点，对应的在空间设置i个控制点；

步骤二，根据绳驱机械臂的特点将被控制点设定为绳驱机械臂上每N个自由度之间的节点，被控制点与相应控制点之间的“接近度”则为待优化目标；

步骤三，判断任意欧式距离是否大于对应安全阈值，若判断结果为是，则通过基于梯度投影法的全局优化使得欧式距离逼近零，从而得到绳驱机械臂在空间的期望构型；

步骤四，判断任意欧式距离是否为零，若判断结果为是，表示绳驱机械臂的构型与期望构型完全一致。

步骤五，判断任意欧式距离是否为零，若判断结果为否，则通过优化绳驱机械臂的整臂自由度，使得接近度的加权和最小，即绳驱机械臂的整臂构型与控制点所设定构型的一致性达到最优化状态，实现“整臂构型-末端位姿”协同规划目标。

本发明的有益效果是：

该方法针对绳驱机械臂在轨执行狭小空间检测任务的需求，分别选取绳驱机械臂的末端点和部分中间关节的交点作为被控制点；在被控制点附近则可以自由设定对应的控制点；控制点和被控制点之间的笛卡尔距离定义为接近度。结合梯度投影法将接近度作为优化的目标函数，通过调整控制点及比例因子等参数则可以在绳驱机械臂自运动空间内完成整臂构型及末端位姿的调整。在满足末端位姿到达的前提下，通过优化绳驱机械臂所有的自由度的方式，在自运动空间内使绳驱机械臂的被控制点逼近或远离控制点要求的位置。因此，该方法满足了“整臂构型-末端位姿”协同规划的需求，克服了传统方法仅考虑末端位置而无法调整整臂构型的不足。

附图说明

图1是本发明中一种求解绳驱机械臂逆运动学的协同规划方法一具体实施例D-H坐标系建立方式示意图。

图2是本发明中一种求解绳驱机械臂逆运动学的协同规划方法一具体实施例原理图。

图3是本发明中一种求解绳驱机械臂逆运动学的协同规划方法控制点与其所在坐标系示意图。

图4是本发明中一种求解绳驱机械臂逆运动学的协同规划方法整臂构型优化的逆运动学求解方法流程图。

图5是本发明中一种求解绳驱机械臂逆运动学的协同规划方法一具体实施例在阻尼系数c＝0与c＝-0.8下形成的臂型角示意图。

图6是本发明中一种求解绳驱机械臂逆运动学的协同规划方法一具体实施例调整“整臂构型”的控制点分布示意图。

具体实施方式

为了使本领域的技术人员能够更好的理解本发明的技术方案，下面将结合实施例来对本发明的具体技术方案进行清楚、完整地描述，以助于本领域的技术人员进一步理解本发明。显然，本案所描述的实施例仅是本发明一部分的实施例，而不是全部的实施例。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思及相互不冲突的前提下，本申请中的实施例及实施例中的特征可以相互组合。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的揭露及保护范围。

此外本发明的说明书和权利要求书及附图中的术语“第一”、“第二”、“步骤1”、“步骤2”、“进一步地”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里描述的那些以外的顺序实施。同时本发明中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。此外对于本领域的普通技术人员而言，可以根据具体情况，结合现有技术来理解上述术语在本案中的具体含义。

针对传统绳驱机械臂逆运动学求解的轨迹规划方法仅考虑末端位置而无法调整整臂构型的不足，本发明提供了一种求解绳驱机械臂逆运动学的协同规划方法，具体技术方案如下：

参考图1，图1是本发明中一种求解绳驱机械臂逆运动学的协同规划方法一具体实施例D-H坐标系建立方式示意图。本方法中将基于经典D-H法对绳驱机械臂建系，求解绳驱机械臂工作空间与关节空间之间的内在关系，为实现绳驱机械臂“整臂构型-末端位姿”协同规划打下基础。

按D-H方法建立了坐标系后，相邻连杆坐标系间的齐次变换矩阵^i-1T_i可按下面公式计算：

sθ_i＝sin(θ_i),sα_i＝sin(α_i)；

cθ_i＝cos(θ_i),cα_i＝cos(α_i)。

根据绳驱机械臂齐次变换矩阵的关系，可得末端坐标系位姿与各个关节坐标系位姿关系如式(2)所示：

⁰T_n＝⁰T₁…^n-1T_n＝f(Θ) (2)

参考图2，图2是本发明中一种求解绳驱机械臂逆运动学的协同规划方法一具体实施例原理图，图2所展示的原理为：首先保证末端轨迹与期望轨迹一致，然后在绳驱机械臂设定i个被控制点，对应的在空间设置i个控制点。根据绳驱机械臂的特点将被控制点设定为绳驱机械臂上每4个自由度之间的节点，被控制点与相应控制点C_i,(i＝1,2,3,4)之间的“接近度”则为待优化目标。当判断任意欧式距离大于对应安全阈值(s_ti,(i＝1,2,3,4))时，则通过基于梯度投影法的全局优化使得欧式距离d_i逼近零，从而得到绳驱机械臂在空间的期望构型。当任意欧式距离d_i为零时，表示绳驱机械臂的构型与期望构型完全一致。当任意欧式距离d_i不能为零时，则通过优化绳驱机械臂的整臂自由度，使得接近度d_i的加权和

最小(λ_i为权重系数)，即绳驱机械臂的整臂构型与控制点所设定构型的一致性达到最优化状态，实现“整臂构型-末端位姿”协同规划目标。

为了使本领域的技术人员能够更好的理解本发明的技术方案，下面将结合实施例来对本发明的一些具体技术方案进行清楚、完整地解释：

(1)绳驱机械臂控制点与被控制点的选择。

参考图2，充分利用绳驱机械臂构型的冗余性，将每四个自由度之间的关节节点N_i,(i＝1,2,3,4)设定为绳驱机械臂的被控点。根据机器人正运动学可知，被控制点N_i在{0}坐标系下的齐次矩阵可表示为：

式(4)中

表示被控制点N_i在{0}坐标系下的位置；

表示被控制点N_i在{0}坐标系下的姿态。

参考图3，图3是本发明中一种求解绳驱机械臂逆运动学的协同规划方法控制点与其所在坐标系示意图；为了能够实现按照操作人员灵活调整绳驱机械臂中间构型的目标，本发明提出基于变控制点的绳驱机械臂轨迹规划思路，通过VR(Virtual Reality)设备在空间选择控制点控制绳驱机械臂的整臂构型对于VR操作人员来讲具有直观、灵活的特点，但抓取的各个控制点坐标之间会存在不可避免的较大突变，难以保证控制点在空间位置值的连续性和均匀性。因此，为了保留直观、灵活的特点操作特点，依然采用VR设备“按需”抓取点的方式获取控制点在空间的位置坐标。而在算法中会通过坐标转换的方式将抓取点投影到坐标系{i}的YZ面，垂足Pt和圆心O_i的连线与控制圆的交点即为控制点(控制圆半径R_c及控制圆角度α)。

参考图3，设定抓取点G的空间坐标为⁰P^G＝[⁰P_x ^G,⁰P_y ^G,⁰P_z ^G]，将其转换到坐标系{i}中的坐标表示为ⁱP^G＝[ⁱP_x ^G,ⁱP_y ^G,ⁱP_z ^G]，因此垂足Pt坐标表示为ⁱP^Pt＝[0,ⁱP_y ^G,ⁱP_z ^G]。因此，控制圆半径R_c及控制圆角度α满足式(5)：

其中，控制圆半径R_c可以人为设定，不失一般性设定其满足式(6)：

(2)绳驱机械臂构型寻优准则函数的建立

参考图2和图3，基于以上设定的控制点和被控点信息，对应点的接近度可表达为：

设定接近度的权重为λ_i其满足：

加权后的优化目标可表示为：

式中λ_i——权重系数；

s_ti——接近度的安全阈值；

d_i(θ)——接近度，即对应控制点和被控点的距离。

通过上述优化目标可见，当H(θ)＝0时，被控点和控制点完全重合，绳驱机械臂处于期望构型。这样就把问题转化为：在末端位姿不变的情况下，通过调节绳驱机械臂在零空间自运动实现基于变控制点优化臂型的问题。

(3)绳驱机械臂基于梯度投影法的轨迹寻优方法介绍

绳驱机械臂逆运动学问题有无穷多组解，因而有必要引入一些优化准则以获得最有效的那组解。绳驱机械臂多目标综合优化也克服了单一性能指标的缺点，更加全面地发挥了绳驱机械臂的优良性能。梯度投影法是绳驱机械臂求逆解常用的方法之一。绳驱机械臂梯度投影法的应用是为了回答一个问题：绳驱机械臂的空间构型在当前关节空间的某一组合下沿着哪一个方向有最大的自运动变化率。利用梯度投影法可以优化绳驱机械臂的关节构型，同时可以优化绳驱机械臂关节角的角速度，式(9)的性能指标H(θ)是各关节角的函数：

式中

——绳驱机械臂关节角速度；

——绳驱机械臂末端速度；

J——绳驱机械臂雅克比矩阵；

J⁺——为J的伪逆；

k——比例因子；

I——单位矩阵；

H(θ)——性能指标函数；

其中，

是性能指标函数H(θ)在θ处的梯度向量，可表示为：

J⁺为J的广义逆(或称为伪逆)，按下式计算：

J⁺＝J^T(JJ^T)^-1 (12)

式(10)右边第一项，即

为方程的最小范数解；第二项为方程的齐次解，该齐次解与最小范数解正交，属于矩阵J的零空间N(J)，即

齐次解对应于绳驱机械臂的自运动，即不影响绳驱机械臂末端位姿的关节运动。该自运动可用于在不改变期望末端位姿的情况下，优化性能指标函数H(θ)。

在公式(10)中，比例因子k的值可正可负，1)当k为负值，这与要求被控点要尽量靠近对应控制点的期望是一致的。当k取负值时才能使得规划构型趋向于控制点约束的构型，也就是接近度越来越逼近零值，从而性能指标函数也变小，使得绳驱机械臂的构型向期望构型逼近。2)当k为正值，会使绳驱机械臂远离控制点，但也可以用来调整绳驱机械臂的空间构型以达到避障等期望目的。因此，比例因子k的值在应用中根据实际需要选择。

比例因子k的选取是非常关键的，因为它可以看作是绳驱机械臂的主运动与自运动的一个比值。当这个比值过大时，绳驱机械臂的主运动占优势过于明显，自运动几乎不起作用，反之当这个比值过小时，绳驱机械臂的自运动占优势，绳驱机械臂关节角会在自运动空间发生较大角度的跳跃，有时甚至无法保证主运动的合理性。本发明用度量指标G衡量绳驱机械臂的主运动和自运动的比例是否合理，即G的作用是判断系数k是否在合理区间，能够使得G值约为0.5的k值为理想值，此时能够保证绳驱机械臂的齐次解与特解在同一数量级，度量指标G表示如下：

其中||·||表示矩阵的二范数。k值过大或过小将造成特解和齐次解的巨大差值，引起算法失效，解决这一问题的途径是实时校正k值，我们可以定义k值如下：

式中c为一个可以任意选择的阻尼系数，用来成比例的改变k值大小，从而改变k在程序中的作用。因此k的值就是绳驱机械臂末端的主运动范数与绳驱机械臂在零空间自运动范数的一个比值。当加入c值后就可以调节由于二范数通常很小而引起的关节超限等故障。

(4)整臂雅克比矩阵求解

参考图4，图4是本发明中一种求解绳驱机械臂逆运动学的协同规划方法整臂构型优化的逆运动学求解方法流程图；本发明使用了雅克比矩阵求解方法，速度级运动学建立了绳驱机械臂关节速度与末端速度之间的映射关系。若关节i为旋转关节，其角速度

产生的末端线速度和角速度分别为：

其中，ξ_i为关节i旋转轴的单位矢量，ρ_i→n为关节i指向绳驱机械臂末端点的位置矢量。

因此，对应于旋转关节i，可得雅克比矩阵第i列为：

所有关节的运动产生的末端合成运动为：

J_i实际为关节i的运动速度对绳驱机械臂末端运动速度的传动比。

根据D-H坐标系定义，坐标系{i-1}的z_i-1轴指向关节i的轴向(i＝1,…,n)，因此在{0}下的表示为：

⁰ξ_i＝⁰z_i-1＝⁰T_i-1(1:3,3)(i＝1,…,n) (18)

其中：⁰z₀＝[0 0 1]^T；⁰T_i-1(1:3,3)、⁰T_i-1(1:3,4)分别表示齐次变换矩阵⁰T_i-1第3列、第4列的第1～3个元素，代表矢量⁰z_i-1和⁰p_i-1；⁰T_n(1:3,4)为齐次变换矩阵⁰T_n第4列的第1～3个元素，为末端位置矢量⁰p_n。

将式(18)、式(19)代入式(16)后，可得以基座坐标系{0}为参考系的雅克比矩阵⁰J(q)。

参考图4，根据任务需求，输入已知条件为：

1)绳驱机械臂末端执行器在任务空间的期望位姿为x_e(或

)；

2)当前关节角度θ_c；

3)D-H坐标系参数{a,α,d,θ}(、连杆扭角、连杆距离、连杆夹角)；

4)4个控制点的期望位置

其中：a——连杆长度；

α——连杆扭角；

d——连杆距离；

θ——连杆夹角；

进一步计算当前位姿差dx，然后，利用关节空间与任务空间的映射雅克比矩阵，通过梯度投影法得到dx与关节变量dθ之间的关系，将(θ+dθ)作为绳驱机械臂关节新的输入，进行绳驱机械臂的运动控制，实现绳驱机械臂的运动。

(5)“整臂构型-末端位姿”协同规划参数功能介绍

通过以上分析可以得出，在绳驱机械臂轨迹规划过程中可以将“整臂构型”及“末端位姿”分别定义为可主动控制的参数，实现对绳驱机械臂构型的有效控制。

①当比例因子k＝0时，整臂构型基于最小范数求得，因此只与末端位姿参数有关；

②当比例因子k≠0时，由于自运动存在，整臂构型同时受控制点和末端位姿参数影响；

③当控制点C_i与节点N_i坐标值相等时，控制点对整臂构型无影响；

④当控制点C_i与节点N_i坐标值不相等时，整臂调整使节点N_i趋向于控制点C_i运动；

⑤当末端位置参数全部约束时，绳驱机械臂关节角度依赖于整臂构型参数；

⑥当末端位置参数部分约束时，可以改变没有约束的末端位置参数调整整臂构型。

为了便于理解，本发明将针对比例因子、控制点、末端位姿参数分别介绍。

参考图5，为了讨论“整臂构型”及“末端位姿”参数对绳驱机械臂的影响，在实施中例依次连接⁰p₀、⁰p₉及⁰p₁₈三点作为臂型的参考臂型面，通过臂型面的变化可以宏观反映出整臂构型的状态。

当比例因子k＝0(阻尼系数c＝0)时，采用雅各比的广义逆求解绳驱机械臂的逆运动学解,此时绳驱机械臂的逆运动学解是对于期望末端位姿的最小范数解。

参考图5，在实施中参数改变前

与参数改变后

之间的夹角作为表征臂型面之间的臂型角，图5是本发明中一种求解绳驱机械臂逆运动学的协同规划方法具体实施例在阻尼系数c＝0与c＝-0.8下形成的臂型角示意图。

因此，当比例因子k≠0(阻尼系数c≠0)时，根据式(14)，将影响比例因子k的阻尼系数c作为自变量可以计算出因变量臂型角余弦值cos(ψ)与阻尼系数c的映射，即通过调整阻尼系数c可以相应的调整绳驱机械臂的空间构型，其中，当阻尼系数c＝0时为参考臂型面所对应的绳驱机械臂构型。

参考图6，图6是本发明中一种求解绳驱机械臂逆运动学的协同规划方法一具体实施例调整“整臂构型”的控制点分布示意图。当绳驱机械臂末端位姿一定时，整臂的中间构型除了受到自运动比例因子k(阻尼系数c)影响外，还可以通过调整式(9)中的性能指标H(θ)来调整绳驱机械臂的构型。针对绳驱机械臂的特点，本实例设定4个控制点C_i,(i＝1,2,3,4)，通过改变控制点的空间位置改变性能指标H(θ)，从而实现对绳驱机械臂的整臂构型控制。

对于实施例中的绳驱机械臂，分别将控制点与所在的坐标系关系设定为：控制点C₁位于坐标系{4}、控制点C₂位于坐标系{8}、控制点C₃位于坐标系{12}、控制点C₄位于坐标系{16}。

以控制点C₃为例开展分析。当控制点选定为C₃时，对应的坐标系为{12}，期望主要通过关节角度θ₁,θ₂…θ₁₂的自运动变化来调整整臂的构型，因此需要将式(10)的性能指标函数加以改造，使其成为θ₁,θ₂…θ₁₂关节角的函数如式(20)所示：

通过改变控制圆的角度α的大小可以调整绳驱机械臂的构型。并且，在同样控制角度的条件下，由于控制圆半径的不同绳驱机械臂的构型也会发生变化。

与此同时，由于末端位姿的状态是由工作目标决定的，故基于工作目标规划的末端位姿变化也引起绳驱机械臂构型相应的变化。

基于上述方法，本发明还提供一种求解绳驱机械臂逆运动学的协同规划***，包括

位置选择单元：用于根据被控制点的位置与作业需求获取控制点在空间的位置坐标；

位置检测单元：用于检测绳驱机械臂当前位置，以计算计算控制点和被控制点之间的欧几里得距离；

位置判定单元：用于判定欧式距离是否大于对应安全阈值，判定控制点和被控制点之间的欧式距离是否为零；

规划单元：用于计算当前位姿差dx，利用雅克比矩阵，通过梯度投影法得到dx与关节变量dθ之间的关系，将(θ+dθ)作为绳驱机械臂关节新的输入，进行绳驱机械臂的运动控制；

输入单元：用于输入规划完成的关节构型与绳驱机械臂关节角的角速度到绳驱机械臂，实现绳驱机械臂的运动；

其中，调整改进对应参数实现所述绳驱机械臂的协同规划处理。所述协同规划参数包括整臂构型参数与末端位姿参数。

一种求解绳驱机械臂逆运动学的协同规划***的具体工作过程描述可参照上述求解绳驱机械臂逆运动学的协同规划方法的描述，在此不再赘述。

另外，本发明还提供一种计算机存储介质，其上存储有计算机程序，程序被处理器执行时实现以下步骤：

步骤一，在绳驱机械臂的末端执行器和中间关节设定i个被控制点，对应的在空间设置i个控制点；

步骤二，根据绳驱机械臂的特点将被控制点设定为绳驱机械臂上每N个自由度之间的节点，被控制点与相应控制点之间的“接近度”则为待优化目标；

步骤三，判断任意欧式距离是否大于对应安全阈值，若判断结果为是，则通过基于梯度投影法的全局优化使得欧式距离逼近零，从而得到绳驱机械臂在空间的期望构型；

步骤四，判断任意欧式距离是否为零，若判断结果为是，表示绳驱机械臂的构型与期望构型完全一致；

步骤五，判断任意欧式距离是否为零，若判断结果为否，则通过优化绳驱机械臂的整臂自由度，使得接近度的加权和最小，即绳驱机械臂的整臂构型与控制点所设定构型的一致性达到最优化状态，实现“整臂构型-末端位姿”协同规划目标。

计算机存储介质上存储的计算机程序的工作过程可参照上述求解绳驱机械臂逆运动学的协同规划方法的描述，在此不再赘述。

同时，本发明的方法除了对绳驱机械臂适用外，也适用于其余多种超冗余绳驱机械臂。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (7)

1.一种求解绳驱机械臂逆运动学的协同规划方法,包括以下步骤：

步骤一，在绳驱机械臂的末端执行器和中间关节设定i个被控制点，对应的在空间设置i个控制点；

步骤二，根据绳驱机械臂的特点将被控制点设定为绳驱机械臂上每N个自由度之间的节点，被控制点与相应控制点之间的“接近度”则为待优化目标；

步骤三，判断任意欧式距离是否大于对应安全阈值，若判断结果为是，则通过基于梯度投影法的全局优化使得欧式距离逼近零，从而得到绳驱机械臂在空间的期望构型；

步骤四，判断任意欧式距离是否为零，若判断结果为是，表示绳驱机械臂的构型与期望构型完全一致；

步骤五，判断任意欧式距离是否为零，若判断结果为否，则通过优化绳驱机械臂的整臂自由度，使得接近度的加权和最小，即绳驱机械臂的整臂构型与控制点所设定构型的一致性达到最优化状态，实现“整臂构型-末端位姿”协同规划目标。

2.根据权利要求1所述的求解绳驱机械臂逆运动学的协同规划方法，其特征在于：

对于被控制点的选择，充分利用绳驱机械臂自由度的冗余性，将每N个自由度之间的关节节点设定为绳驱机械臂的被控点；

对于控制点的选择，为了能够实现按照操作人员灵活调整绳驱机械臂中间构型的目标，提出基于变控制点的绳驱机械臂轨迹规划，以“按需”抓取点的方式获取控制点在空间的位置坐标，在算法中通过坐标转换的方式将抓取点投影到经典D-H(Denavit-Hartenberg)坐标系的YZ面，垂足和圆心的连线与控制圆的交点即为控制点。

3.根据权利要求1至2任一项所述的求解绳驱机械臂逆运动学的协同规划方法，其特征在于：

应用梯度投影法解决绳驱机械臂的空间构型在当前关节空间的某一组合下沿着哪一个方向有最大的自运动变化率的问题，优化绳驱机械臂的关节构型，同时优化绳驱机械臂关节角的角速度。

建立关节空间与任务空间的映射雅克比(Jacobian)矩阵，输入已知条件：

1)绳驱机械臂末端执行器在任务空间的期望位姿；

2)当前关节角度；

3)D-H坐标系参数；

4)控制点的期望位置；

进一步计算当前位姿差，利用雅克比矩阵，通过梯度投影法得到位姿差与关节变量之间的关系，计算绳驱机械臂关节新的输入，进行绳驱机械臂的运动控制，实现绳驱机械臂的运动。

4.根据权利要求1至3所述的求解绳驱机械臂逆运动学的协同规划方法，其特征在于：

通过调整改进对应参数实现所述绳驱机械臂的“整臂构型-末端位姿”协同规划处理；

所述协同规划参数包括整臂构型参数与末端位姿参数。

5.一种求解绳驱机械臂逆运动学的协同规划***，其特征在于，包括：

位置选择单元：用于根据作业需求获取控制点在空间的位置坐标；

位置检测单元：用于检测绳驱机械臂当前构型，以计算计算控制点和被控制点之间的欧几里得距离；

位置判定单元：用于判定欧式距离是否大于对应安全阈值，判定控制点和被控制点之间的欧式距离是否为零；

规划单元：用于计算当前位姿差，利用雅克比矩阵，通过梯度投影法得到位姿差与关节变量之间的关系，计算绳驱机械臂关节新的输入，进行绳驱机械臂的运动控制；

输入单元：用于输入规划完成的关节构型与绳驱机械臂关节角的角速度到绳驱机械臂，实现绳驱机械臂的运动。

6.根据权利要求5所述的求解绳驱机械臂逆运动学的协同规划***，其特征在于：

通过调整改进对应参数实现所述绳驱机械臂的协同规划处理；

所述协同规划参数包括整臂构型参数与末端位姿参数。

7.一种计算机存储介质，其特征在于，其上存储有计算机程序，所述程序被处理器执行时实现以下步骤：

步骤一，在绳驱机械臂的末端执行器和中间关节设定i个被控制点，对应的在空间设置i个控制点；

步骤二，根据绳驱机械臂的特点将被控制点设定为绳驱机械臂上每N个自由度之间的节点，被控制点与相应控制点之间的“接近度”则为待优化目标；

步骤三，判断任意欧式距离是否大于对应安全阈值，若判断结果为是，则通过基于梯度投影法的全局优化使得欧式距离逼近零，从而得到绳驱机械臂在空间的期望构型；

步骤四，判断任意欧式距离是否为零，若判断结果为是，表示绳驱机械臂的构型与期望构型完全一致；

步骤五，判断任意欧式距离是否为零，若判断结果为否，则通过优化绳驱机械臂的整臂自由度，使得接近度的加权和最小，即绳驱机械臂的整臂构型与控制点所设定构型的一致性达到最优化状态，实现“整臂构型-末端位姿”协同规划目标。

Images