绳驱多关节柔性机器人自标定方法及系统、存储介质与流程

本发明涉及机器人标定领域，尤其是一种绳驱多关节柔性机器人自标定方法及系统、存储介质。

背景技术：

柔性机器人相对于传统机械臂有纤细的躯干，冗余的自由度，在复杂多障碍的环境中体现出了极强的灵活性，因此被广泛应用于核电领域、航天领域大型设备的检修、维护、装配等作业任务。这些狭小空间下的精细作业任务，往往要求柔性机器人具有较高的末端绝对定位精度。然而在以下几个方面因素，将影响着柔性机器人的精度，进而影响其作业的能力：

(1)在柔性机器人的零部件加工和安装装配过程中，其存在着许多误差。这些误差经过多个关节的累积和放大，最终导致末端较大的误差。

(2)其关节处的有限空间以及特殊的绳索驱动方式，导致了其传感器主要集中在机器人的根部，并不能直接反馈关节的角度大小，存在着关节角度的误差。

(3)绳索具有一定的弹性，在多次使用之后柔性机器人的驱动绳索将会变长，进而也影响关节的角度，导致末端的误差。

为了提高柔性机器人的末端定位精度，进一步增强其狭小空间的精细作业的能力，我们有必要定期对柔性机器人相关运动学参数的进行标定，提高其性能。然而，传统的标定方法流程繁杂，不方便实施，效率低且成本高。

技术实现要素：

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本发明的一个目的是提供一种绳驱多关节柔性机器人自标定方法及系统、存储介质，自标定方法的流程简单、方便实施、效率高且成本低，提高机器人的末端定位精度。

本发明所采用的技术方案是：一种绳驱多关节柔性机器人自标定方法，包括以下步骤：

s1、设置当前关节的一根驱动绳为力控绳，设置所述关节剩余的驱动绳为位控绳，每个关节通过至少3根驱动绳进行驱动，利用恒力控制模式控制的驱动绳为所述力控绳，利用位置控制模式控制的驱动绳为所述位控绳；

s2、控制所述关节到达第一平衡状态，

s3、控制其中一根位控绳发生运动，剩余位控绳的位置保持不变，所述力控绳发生随动并到达第二平衡状态；

s4、获取发生运动的位控绳的绳长变化量以及力控绳的绳长变化量；

s5、重复步骤s2至s4，改变发生运动的位控绳，直到获取所有位控绳的绳长变化量以及力控绳对应的绳长变化量；

s6、获取所述力控绳的绳长变化量与位控绳的绳长变化量的映射矩阵，并根据所述映射矩阵与关节角的相关关系获取关节的关节角，实现当前关节的关节角标定。

进一步地，根据所述映射矩阵与关节角的相关关系并利用最小二乘迭代法获取关节的关节角。

进一步地，按照关节的关节号的顺序标定所述绳驱多关节柔性机器人的关节，其中，标定关节i时，控制关节号小于i的关节的驱动绳处于张紧状态，控制关节号大于i的关节的驱动绳处于放松状态。

进一步地，一根驱动绳由一个电机控制，根据所述电机的位置变化量获得驱动绳的绳长变化量。

本发明所采用的另一技术方案是：一种绳驱多关节柔性机器人自标定系统，包括

第一模块，用于执行步骤s1，设置当前关节的一根驱动绳为力控绳，设置所述关节剩余的驱动绳为位控绳，每个关节通过至少3根驱动绳进行驱动，利用恒力控制模式控制的驱动绳为所述力控绳，利用位置控制模式控制的驱动绳为所述位控绳；

第二模块，用于执行步骤s2，控制所述关节到达第一平衡状态，

第三模块，用于执行步骤s3，控制其中一根位控绳发生运动，剩余位控绳的位置保持不变，所述力控绳发生随动并到达第二平衡状态；

第四模块，用于执行步骤s4，获取发生运动的位控绳的绳长变化量以及力控绳的绳长变化量；

第五模块，用于执行步骤s5，重复步骤s2至s4，改变发生运动的位控绳，直到获取所有位控绳的绳长变化量以及力控绳对应的绳长变化量；

第六模块，用于执行步骤s6，获取所述力控绳的绳长变化量与位控绳的绳长变化量的映射矩阵，并根据所述映射矩阵与关节角的相关关系获取关节的关节角，实现当前关节的关节角标定。

进一步地，所述第五模块根据所述映射矩阵与关节角的相关关系并利用最小二乘迭代法获取关节的关节角。

进一步地，所述绳驱多关节柔性机器人自标定系统按照关节的关节号的顺序标定所述绳驱多关节柔性机器人的关节，其中，标定关节i时，控制关节号小于i的关节的驱动绳处于张紧状态，控制关节号大于i的关节的驱动绳处于放松状态。

进一步地，一根驱动绳与一个电机连接并受其控制，所述第三模块根据所述电机的位置变化量获得驱动绳的绳长变化量。

进一步地，所述第三模块包括电机编码器，用于获取所述电机的位置变化量。

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

s1、设置当前关节的一根驱动绳为力控绳，设置所述关节剩余的驱动绳为位控绳，每个关节通过至少3根驱动绳进行驱动，利用恒力控制模式控制的驱动绳为所述力控绳，利用位置控制模式控制的驱动绳为所述位控绳；

s2、控制所述关节到达第一平衡状态，

s3、控制其中一根位控绳发生运动，剩余位控绳的位置保持不变，所述力控绳发生随动并到达第二平衡状态；

s4、获取发生运动的位控绳的绳长变化量以及力控绳的绳长变化量；

s5、重复步骤s2至s4，改变发生运动的位控绳，直到获取所有位控绳的绳长变化量以及力控绳对应的绳长变化量；

s6、获取所述力控绳的绳长变化量与位控绳的绳长变化量的映射矩阵，并根据所述映射矩阵与关节角的相关关系获取关节的关节角，实现当前关节的关节角标定。

本发明的有益效果是：

本发明一种绳驱多关节柔性机器人自标定方法及系统、存储介质，设置关节的一根驱动绳为力控绳，设置关节剩余的驱动绳为位控绳，每个关节通过至少3根驱动绳进行驱动；控制关节到达第一平衡状态后，控制其中一根位控绳发生运动，剩余位控绳的位置保持不变，力控绳发生随动并到达第二平衡状态；获取发生运动的位控绳的绳长变化量以及力控绳的绳长变化量；改变发生运动的位控绳，关节回到第一平衡状态，重新获取发生运动的位控绳的绳长变化量以及力控绳的绳长变化量，直到获取所有位控绳的绳长变化量以及力控绳对应的绳长变化量；获取力控绳的绳长变化量与位控绳的绳长变化量的映射矩阵，并根据映射矩阵与关节角的相关关系获取关节的关节角，实现当前关节的关节角标定；克服了柔性机器人的运动学误差引起的其末端定位精度低，精细作业能力受限且传统的标定方法流程繁杂，不方便实施，效率低且成本高的技术问题，自标定方法的流程简单、方便实施、效率高且成本低，提高机器人的末端定位精度。

附图说明

下面结合附图对本发明的具体实施方式作进一步说明：

图1是绳索驱动超冗余机械臂的运动学空间关系描述示意图；

图2是绳索驱动超冗余机械臂dh坐标系示意图；

图3是绳驱多关节柔性机器人的示意图；

图4是绳索驱动超冗余机械臂的一具体实施例关节示意图；

图5是图4的关节简化等效图；

图6是图4的关节模型坐标系分析图；

图7是本发明的一种绳驱多关节柔性机器人自标定方法的一具体实施例流程图；

图8是本发明的一种绳驱多关节柔性机器人自标定方法的最小二乘迭代法计算关节角的一具体实施例流程图；

图9是绳长变化时关节角的计算流程图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

一种绳驱多关节柔性机器人自标定方法，绳驱多关节柔性机器人自标定方法包括以下步骤：

s1、设置当前关节的一根驱动绳为力控绳，设置关节剩余的驱动绳为位控绳，每个关节通过至少3根驱动绳进行驱动，一根驱动绳由一个电机控制，利用恒力控制模式控制的驱动绳为力控绳，利用位置控制模式控制的驱动绳为位控绳；本实施例中，每个关节的电机都采用力位混合控制的模式，即电机可以采用位置控制模式或者恒定拉力控制模式进行控制，电机的控制模式可以进行切换；

s2、控制关节到达第一平衡状态，

s3、控制其中一根位控绳发生运动，剩余位控绳的位置保持不变，力控绳发生随动并到达第二平衡状态；

s4、获取发生运动的位控绳的绳长变化量以及力控绳的绳长变化量；

s5、重复步骤s2至s4，改变发生运动的位控绳，直到获取所有位控绳的绳长变化量以及力控绳对应的绳长变化量；

s6、获取力控绳的绳长变化量与位控绳的绳长变化量的映射矩阵，并根据映射矩阵与关节角的相关关系获取关节的关节角，实现当前关节的关节角标定。

本发明的自标定方法中，通过力位混合控制模式，利用柔性机器人冗余驱动的特点，分别获得关节处一个驱动绳运动对另一个驱动绳的长度影响，进而得到力控绳的绳长变化量与位控绳的绳长变化量的映射矩阵，结合映射矩阵与关节角度的关系，完成该关节处关节角度的标定。克服了柔性机器人的运动学误差引起的其末端定位精度低，精细作业能力受限且传统的标定方法流程繁杂，不方便实施，效率低且成本高的技术问题，自标定方法的流程简单、方便实施、效率高且成本低，提高机器人的末端定位精度。

具体地，根据电机的位置变化量可以获得驱动绳的绳长变化量，利用电机编码器可以知道电机的位置信息，则可以获知电机的位置变化量，而由于驱动绳是电机驱动的，因此，根据电机的位置变化量可以获取驱动绳的绳长变化量。根据力控绳的绳长变化量与位控绳的绳长变化量的映射矩阵与关节角的相关关系并利用最小二乘迭代法获取关节的关节角。

进一步地，按照关节的关节号的顺序标定绳驱多关节柔性机器人的关节，其中，标定关节i时，控制关节号小于i的关节的驱动绳处于张紧状态，达到最小张紧力以上，控制关节号大于i的关节的驱动绳处于放松状态。当关节i(1≤i≤n)标定完成时,该关节的电机保持不变，进行关节i+1的标定，直到i＝n时，完成柔性机器人的标定。

另外，绳长与关节角度之间存在一定的关系，下面进行具体说明：绳驱超冗余机械臂的运动学分析不仅包括了关节空间以及操作空间之间的映射关系，还包括了绳索驱动空间到关节空间的映射关系。因此其运动学分析可以分为两步：参考图1，图1是绳索驱动超冗余机械臂的运动学空间关系描述示意图；首先推导关节空间与操作空间映射关系，即机器人末端坐标系位置和关节变量即关节角ψ、α之间的关系；再推导绳索驱动空间与关节空间映射关系，即关节角ψ、α和驱动绳索长度变化量δl之间的关系。

首先，进行关节与操作空间映射分析，采用正运动学分析，为了对绳索驱动超冗余机械臂进行运动学分析与求解，需要建立d-h坐标系。d-h坐标法虽然是机器人学中通用方法，但当前d-h坐标系的建立规则不一，且没有形成统一的标准，大多根据个人习惯进行建立。本实施例中，按照改善后的d-h坐标系规则建立d-h坐标系，参考图2，图2是绳索驱动超冗余机械臂dh坐标系示意图；d-h坐标系建立规则如下：zi轴沿第i关节的轴线方向，xi轴垂直于zi轴，并指向离开zi轴的方向；yi轴按右手坐标系的规则建立。并得到d-h参数表如表1所示。

表1绳索驱动超冗余机械臂dh参数

根据d-h坐标系建立规则以及d-h参数表，可以依次得到齐次变换矩阵：

将上述齐次变换矩阵依次连乘即可得到21号坐标系在1号坐标系中的表示，亦即机械臂的正运动学求解：

¹t21＝¹t2·²t3…²⁰t21＝f(θ1,θ2,…,θ20)(1)

再进行绳索空间与关节空间映射分析，参考图3和图4，图3是绳驱多关节柔性机器人的示意图，图4是绳索驱动超冗余机械臂的一具体实施例关节示意图；绳索驱动超冗余机械臂的单个关节子系统中具有3个输入，2个输出，换句话说，其是一个并联机器人，具有三根驱动绳索和2个自由度。单个关节依靠3个独立的绳子进行驱动，实现其三维空间的旋转运动。因此，根据前面臂的设计，在关节的臂杆中的绳子长度不会发生变化，而引起该关节角度发生变化的原因则是在关节处两个圆盘之间的绳索长度发生了变化。为了准确描述绳驱动机械臂关节的转动角度与关节处绳子的长度之间的关系，将关节模型进行简化，建立了单关节的运动学模型，如图5所示，图5是图4的关节简化等效图，在图5中，面b1b2b3、面a1a2a3分别代表布线圆盘2和布线圆盘1，线段a1b1、a2b2、a3b3分别代表三个独立的绳索l1、l2、l3，点p则代表关节的中心。分别以面b1b2b3、面a1a2a3中心o1、o2为原点，关节臂杆轴心的方向为z轴，关节两个自由度旋转的方向为x、y轴，建立坐标系坐标系{1}、{2}。由图5可以知道关节的中心点p则固定不变，为方便分析，在关节中心点处建立中间坐标系{0}，如图6所示，图6是图4的关节模型坐标系分析图：

假设在关节初始位置时，两圆盘之间的距离为d，那么对于坐标系{0}、{1}，如图6所示，当{1}绕其x轴旋转α角，再继续往上平移d/2后与{0}重合。于是可以得到齐次变换矩阵：

对于坐标系{0}、{2}，坐标系{0}绕其y轴旋转ψ角，再继续沿着旋转后的坐标系的z轴往上平移d/2后与{2}重合。于是可以得到齐次变换矩阵：

于是可以知道坐标系{1}、{2}之间的变化矩阵为：

在布线圆盘2上任意取一点b1，从图6中可以知道∠b1o2x2＝β，那么在圆盘1上与b1对应点a1，也有∠a1o1x1＝β，于是在坐标系{1}中有：

在坐标系{2}中：

利用前面计算的齐次变换矩阵，可以得到在坐标系{1}中的¹b1为：

其中，s为sin，c为cos，r为驱动绳索的绳孔的分布圆的半径；于是进一步计算可得绳子长度l1为：

同理，对于绳索a2b2、a3b3，那么有：

|a2b2|＝((dsψ-rcβ+2/3π+rcψcβ+2/3π+rsαsψsβ+2/3π)²+(rsβ+2/3π(cα-1))²+(dcψ-rcβ+2/3πsψ+rcψsαsβ+2/3π)²)^1/2(0.9)

|a3b3|＝((dsψ-rcβ+4/3π+rcψcβ+4/3π+rsαsψsβ+4/3π)²+(rsβ+4/3π(cα-1))²+(dcψ-rcβ+4/3πsψ+rcψsαsβ+4/3π)²)^1/2(0.10)

因此，可以绳索的长度可以统一地表示为：

最后，进行绳长-关节角运动学分析，在一些实际的控制过程中，往往需要根据当前的绳长来计算关节的角度大小，因此有必要进行关节的绳长-关节角的运动学分析。该分析作为关节角-绳长运动学的逆过程，因此满足以下几个条件：

其中fi(ψ,α)＝f(ψ,α,βi)，并且关节的初始条件为：

li＝fi(ψ,α)|(0,0)＝23(13)

对式子(12)进行求导，可以得到:

其中jc是关于绳索空间和关节空间的雅克比矩阵，具体地可以表示为：

那么，式子(14)可以利用各变量差分进行等效，得到：

其中是雅克比矩阵jc的逆。

基于上述分析可知，通过运动学的关系，柔性机器人具有以下运动学方程：

f(θ)＝x

li,k＝fi,k(θi)

其中，fi,k(θi)为绳索k(1≤k≤3)长度的计算公式，即：

其中，为第i关节处的关节角度。

下面具体说明本发明的自检定方法，本实施例中，以绳驱柔性机器人有n个关节，每个关节具有2个自由度，每个关节采用3个独立绳索冗余驱动的方式为例进行说明；每根绳子连接一个电机，并且针对每个关节都设置有电机位置传感器(即电机编码器)和拉力传感器，电机编码器用于获取电机的位置，拉力传感器用于获取驱动绳索的拉力，主要用于电机的恒力控制模式。参考图7，图7是本发明的一种绳驱多关节柔性机器人自标定方法的一具体实施例流程图；首先，在力位混合控制模式下，让关节i(i＝1,2…n)处的3个电机先达到任意的第一平衡状态，使得力控绳索的拉力为f。本实施例中，以关节i＝1为例，包括电机1、电机2和电机3，电机1、电机2为位置控制模式的电机，下面简称位控电机1和位控电机2，电机3为恒力控制模式的电机，下面简称力控电机3。保持位控电机2的位置不变，先微小地改变位控电机1的位置，使其发生绳长变化，即δl1，此时，力控电机3将进行随动，整个关节达到第二平衡状态。通过电机编码器可获得此时力控电机3在新位置对应的绳长变化δl31。在力位混合控制模式下，再次让系统达到第一平衡状态下，保持位控电机1不变，微小地改变位控电机2的位置，使得其绳长变化δl2，可获得此时力控模式电机3新位置下对应的绳长变化δl32。通过改变电机的不同组合控制模式，可得到关节处3个绳索的之间的长度变化映射关系，即

其中，

则通过绳索长度变化之间的映射矩阵j，利用最小二乘迭代的方法，可以得到关节i的当前角度值完成柔性机器人关节i的关节角标定。

下面具体说明利用最小二乘迭代法获取关节角的具体计算方法，参考图8，图8是本发明的一种绳驱多关节柔性机器人自标定方法的最小二乘迭代法计算关节角的一具体实施例流程图；

1、设定初始的θi,n＝[00]^t；

2、计算j(θi,0)，进一步得到δj(θi,0)＝j(θi,0)-j；

3、由δj(θi,0)映射到关节角误差，即

其中

4、关节角度叠加：θi,n+1＝θi,n+δθi,n+1

5、重复上述步骤2-4，直到norm(δj(θi,n))小于设定值ε，比如ε为0.01，即可计算获得关节角。

下面具体说明j(θi,0)的计算方法：

令

则j(θi)为与第i关节角度相关的矩阵，fi,k(θi)为关节i处绳长k(1≤k≤3)，并且在相同的关节角度和δl1、δl2足够小时，可以认为：j(θi)＝j。则和可以通过绳长计算函数fi,k(θi)求偏导可得。可以在假定的关节角度情况下，由fi,k(θi)函数分别计算出对应的绳索长度l1和l2。根据绳长l1和l2,分别利用[l1+δl1l2]和[l1l2+δl2]通过图9所示的方法可以计算出对应的关节角度θi，图9是绳长变化时关节角的计算流程图，从而得出两种情况下的变化量δθ2i-1和δθ2i，而δl1和δl2分别为给定值，如0.001；而即可以得到同理，可以得到和至此，即可计算得到。

本发明还提供一种绳驱多关节柔性机器人自标定系统，包括

第一模块，用于执行步骤s1，设置当前关节的一根驱动绳为力控绳，设置关节剩余的驱动绳为位控绳，每个关节通过至少3根驱动绳进行驱动，利用恒力控制模式控制的驱动绳为力控绳，利用位置控制模式控制的驱动绳为位控绳；

第二模块，用于执行步骤s2，控制关节到达第一平衡状态，

第三模块，用于执行步骤s3，控制其中一根位控绳发生运动，剩余位控绳的位置保持不变，力控绳发生随动并到达第二平衡状态；

第四模块，用于执行步骤s4，获取发生运动的位控绳的绳长变化量以及力控绳的绳长变化量；

第五模块，用于执行步骤s5，重复步骤s2至s4，改变发生运动的位控绳，直到获取所有位控绳的绳长变化量以及力控绳对应的绳长变化量；

第六模块，用于执行步骤s6，获取力控绳的绳长变化量与位控绳的绳长变化量的映射矩阵，并根据映射矩阵与关节角的相关关系获取关节的关节角，实现当前关节的关节角标定。

进一步地，一根驱动绳与一个电机连接并受其控制，第三模块包括电机编码器，用于获取电机的位置变化量。第三模块根据电机的位置变化量获得驱动绳的绳长变化量。绳驱多关节柔性机器人自标定系统按照关节的关节号的顺序标定绳驱多关节柔性机器人的关节，其中，标定关节i时，控制关节号小于i的关节的驱动绳处于张紧状态，控制关节号大于i的关节的驱动绳处于放松状态。

一种绳驱多关节柔性机器人自标定系统的工作过程参照上述绳驱多关节柔性机器人自标定方法的描述，不再赘述。

一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：

s1、设置当前关节的一根驱动绳为力控绳，设置关节剩余的驱动绳为位控绳，每个关节通过至少3根驱动绳进行驱动，利用恒力控制模式控制的驱动绳为力控绳，利用位置控制模式控制的驱动绳为位控绳；

s2、控制关节到达第一平衡状态，

s3、控制其中一根位控绳发生运动，剩余位控绳的位置保持不变，力控绳发生随动并到达第二平衡状态；

s4、获取发生运动的位控绳的绳长变化量以及力控绳的绳长变化量；

s5、重复步骤s2至s4，改变发生运动的位控绳，直到获取所有位控绳的绳长变化量以及力控绳对应的绳长变化量；

s6、获取力控绳的绳长变化量与位控绳的绳长变化量的映射矩阵，并根据映射矩阵与关节角的相关关系获取关节的关节角，实现当前关节的关节角标定。

一种计算机可读存储介质的工作过程参照上述绳驱多关节柔性机器人自标定方法的描述，不再赘述。

本发明根据绳驱柔性机器人的冗余驱动的特点，提供一种绳驱多关节柔性机器人自标定方法。将整个柔性多关节机器人的运动学参数，分解成每个关节的参数。基于每个关节力位混合控制的模式下，根据一个绳索长度变化导致另一个绳索长度的变化影响，得到绳长变化之间的映射矩阵。结合映射矩阵与关节角度的关系，通过最小二乘迭代法，完成该关节处关节角度等的标定。通过一个接一个关节的标定，可以完成整个柔性机器人的关节角度标定。与传统的标定方法相比，该方法具有求解唯一、成本低，效率高，并且方便实施等特点，，而且可以提高机器人末端定位精度，十分适用于绳驱柔性机器人，可广泛应用于柔性机器人的自我校正等作用。

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