一种柔性机器人运动学标定方法及系统与流程

本发明涉及机器人领域，尤其是一种柔性机器人运动学标定方法及系统。

背景技术：

目前，现有的2m自由度的柔性机器人，由m个关节段串联而成，每个关节段由3个以上的绳索驱动，带有电机位置传感器和绳索拉力传感器。参考图1，图1是现有柔性机器人的关节段的示意图；图1中，示意了一个联动关节段的驱动连接情况，关节段由3根驱动绳索进行驱动，关节段的子关节之间通过驱动绳索和联动绳索构成串并联结构。相对于传统机械臂，柔性机器人有纤细的躯干，冗余的自由度，在复杂多障碍的环境中体现出了极强的灵活性，因此被广泛应用于核电领域、航天领域大型设备的检修、维护、装配等作业任务。这些狭小空间下的精细作业任务，往往要求柔性机器人具有较高的末端绝对定位精度。然而在以下几个方面因素，将影响着柔性机器人的精度，进而影响其作业的能力：

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

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

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

为了提高柔性机器人的末端定位精度，进一步增强其狭小空间的精细作业的能力，需要定期对柔性机器人相关运动学参数的进行标定，以提高其性能。然而，由于绳驱柔性机器人属于串并联机器人，其运动学的参数繁多且参数之间呈现出互相强耦合的影响，导致每个运动学的运动学参数误差与末端的位置姿态误差之间具有非线性的关系，传统的基于模型的标定方法难以直接进行应用。

技术实现要素：

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本发明的一个目的是提供一种柔性机器人运动学标定方法及系统，提高机器人的标定精度。

本发明所采用的技术方案是：一种柔性机器人运动学标定方法，包括联动关节段标定步骤和机器人标定步骤，其中，

所述联动关节段标定步骤包括：

根据联动关节段的名义关节角度获取零联动角度误差下所述联动关节段的名义位姿；

利用参数解耦驱动结构驱动所述联动关节段以使驱动绳索到达所述联动关节段的名义绳长，并获取所述联动关节段的实际位姿，其中，根据所述联动关节段的名义关节角度计算得到所述联动关节段的名义绳长，所述参数解耦驱动结构为驱动绳索固定在所述联动关节段的第一个子关节上，所述联动关节段的所有子关节通过联动绳索连接的结构；

根据所述联动关节段的名义位姿和所述联动关节段的实际位姿获取所述联动关节段的联动角度误差；

所述机器人标定步骤包括：

利用所述联动关节段标定步骤获取多个联动关节段的联动角度误差；

根据所述多个联动关节段的联动角度误差进行组合得到机器人的联动角度误差，所述机器人包括多个联动关节段。

进一步地，所述机器人标定步骤还包括：

根据机器人的名义关节角度获取在所述机器人的联动角度误差下机器人的名义位姿；

利用正常驱动结构驱动所述机器人以使驱动绳索到达机器人的名义绳长，并获取所述机器人的实际位姿，其中，根据所述机器人的名义关节角度计算得到所述机器人的名义绳长，不同的所述联动关节段用不同的正常驱动结构驱动，所述正常驱动结构为驱动绳索固定在所述联动关节段的所有子关节上，所述联动关节段的所有子关节通过联动绳索连接的结构；

根据所述机器人的名义位姿和所述机器人的实际位姿获取所述机器人的初始绳长误差。

进一步地，根据多组所述机器人的名义位姿和所述机器人的实际位姿利用最小二乘迭代法获取所述机器人的初始绳长误差。

进一步地，利用最小二乘迭代法处理多组所述机器人的名义位姿和所述机器人的实际位姿以获取所述机器人的初始绳长误差。

进一步地，利用激光追踪法获取所述联动关节段的实际位姿。

本发明所采用的另一技术方案是：一种柔性机器人运动学标定系统，所述柔性机器人运动学标定系统包括联动关节段标定单元和机器人标定单元，其中，

所述联动关节段标定单元包括：

联动关节段的名义位姿获取模块，用于根据联动关节段的名义关节角度获取零联动角度误差下所述联动关节段的名义位姿；

联动关节段的实际位姿获取模块，用于利用参数解耦驱动结构驱动所述联动关节段以使驱动绳索到达所述联动关节段的名义绳长，并获取所述联动关节段的实际位姿，其中，根据所述联动关节段的名义关节角度计算得到所述联动关节段的名义绳长，所述参数解耦驱动结构为驱动绳索固定在所述联动关节段的第一个子关节上，所述联动关节段的所有子关节通过联动绳索连接的结构；

联动关节段的联动角度误差获取模块，用于根据所述联动关节段的名义位姿和所述联动关节段的实际位姿获取所述联动关节段的联动角度误差；

所述机器人标定单元包括：

联动角度误差标定模块，用于利用所述联动关节段标定单元获取多个联动关节段的联动角度误差；根据所述多个联动关节段的联动角度误差进行组合得到机器人的联动角度误差，所述机器人包括多个联动关节段。

进一步地，所述机器人标定单元还包括：

机器人的名义位姿获取模块，用于根据机器人的名义关节角度获取在所述机器人的联动角度误差下机器人的名义位姿；

机器人的实际位姿获取模块，用于利用正常驱动结构驱动所述机器人以使驱动绳索到达机器人的名义绳长，并获取所述机器人的实际位姿，其中，根据所述机器人的名义关节角度计算得到所述机器人的名义绳长，不同的所述联动关节段用不同的正常驱动结构驱动，所述正常驱动结构为驱动绳索固定在所述联动关节段的所有子关节上，所述联动关节段的所有子关节通过联动绳索连接的结构；

机器人的初始绳长误差获取模块，用于根据所述机器人的名义位姿和所述机器人的实际位姿获取所述机器人的初始绳长误差。

进一步地，所述机器人的末端设置有靶球，利用激光跟踪仪获取所述机器人的实际位姿。

进一步地，所述联动关节段的末端设置有靶球，利用激光跟踪仪获取所述联动关节段的实际位姿。

本发明的有益效果是：

本发明提供一种柔性机器人运动学标定方法及系统，根据联动关节段的名义关节角度获取零联动角度误差下联动关节段的名义位姿；利用参数解耦驱动结构驱动联动关节段到达联动关节段的名义绳长，并获取联动关节段的实际位姿，最后根据联动关节段的名义位姿和联动关节段的实际位姿获取联动关节段的联动角度误差；并根据多个联动关节段的联动角度误差获取机器人的联动角度误差；克服现有绳驱柔性机器人的运动学误差参数繁多，串并联结构导致参数之间互相耦合作用强，参数标定精度低的技术问题，提高机器人的标定精度。

附图说明

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

图1是现有柔性机器人的关节段的示意图；

图2是本发明中一种柔性机器人运动学标定方法的参数解耦驱动结构的一具体实施例示意图；

图3是本发明中一种柔性机器人运动学标定方法的一具体实施例中实际位姿的获取示意图；

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

图5是联动型关节段自由度配置图；

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

图7是图6的关节简化等效图；

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

图9是单个子关节的绳长到关节角的计算流程图；

图10是联动关节段的绳长到关节角度的计算流程图；

图11是柔性臂绳长到关节角度的计算流程图；

图12是本发明中一种柔性机器人运动学标定方法的一具体实施例联动关节段的联动角度误差标定流程图；

图13是本发明中一种柔性机器人运动学标定方法的一具体实施例机器人整臂的初始绳长误差标定流程图。

具体实施方式

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

实施例1

一种柔性机器人运动学标定方法，包括联动关节段标定步骤和机器人标定步骤，其中，联动关节段标定步骤包括：

根据联动关节段的名义关节角度(即理论关节角度)获取零联动角度误差下联动关节段的名义位姿(即理论位姿)，本发明中，末端位置和姿态简称位姿。

利用参数解耦驱动结构驱动联动关节段以使驱动绳索到达联动关节段的名义绳长(即理论绳长)，并获取联动关节段的实际位姿，其中，根据联动关节段的名义关节角度计算得到联动关节段的名义绳长，参考图2，图2是本发明中一种柔性机器人运动学标定方法的参数解耦驱动结构的一具体实施例示意图；本实施例中，联动关节段的驱动绳索有三根，参数解耦驱动结构为驱动绳索固定在联动关节段的第一个子关节的圆盘上，驱动绳索与驱动控制箱连接，联动关节段的所有子关节通过联动绳索连接的结构；一个驱动绳索配置一个电机来进行驱动，并且设置有电机位置传感器和绳索拉力传感器。本实施例中，三个电机均采用力位混合控制的模式，电机的控制模式可以进行切换。当驱动联动关节段时，有2个电机采用位置控制的模式，另一个电机采用恒定拉力控制的模式；电机驱动第一个子关节进行运动，剩余子关节由于联动绳索的作用也跟着第一个子关节一起运动。参考图3，图3是本发明中一种柔性机器人运动学标定方法的一具体实施例中实际位姿的获取示意图；本实施例中，在联动关节段的末端设置有靶球，利用api激光跟踪仪获取联动关节段的实际位姿。

根据联动关节段的名义位姿和联动关节段的实际位姿获取联动关节段的联动角度误差。

基于参数解耦驱动结构，完成机器人的每个联动关节段的联动角度误差的标定。然后将各个联动关节段首尾连接组装在一起，形成机器人的柔性操作臂，每个联动关节段用一个正常驱动结构驱动，参考图1，示意了一个联动关节段的正常驱动结构，正常驱动结构为驱动绳索固定在联动关节段的所有子关节上，联动关节段的所有子关节通过联动绳索连接的结构；本实施例中，正常驱动结构采用3根驱动绳索驱动联动关节段。机器人标定步骤包括：

利用联动关节段标定步骤获取多个联动关节段的联动角度误差；

机器人包括多个联动关节段，则根据多个联动关节段的联动角度误差进行组合得到机器人的联动角度误差。由于联动关节段的联动角度误差不随着联动关节段的重新组装而变化，因此，机器人的联动角度误差可直接参考联动关节段的联动角度误差的标定结果。比如一个联动关节段的联动角度误差是一个6×1的矩阵，那么5个这样的联动关节段组成的机器人的整臂的联动角度误差是30×1的矩阵。

与传统的标定方法相比，该方法利用参数解耦驱动结构驱动联动关节段具有参数之间互相解耦、标定精度高的优点，并且利用激光跟踪仪获取位姿不仅方便实施，而且标定效率高，解决了柔性机器人的运动学误差引起的其末端定位精度低，精细作业能力受限，并且由于机器人串并联的复杂结构，使得其运动学的参数之间呈现出强耦合的问题，提高了标定精度，可广泛应用于柔性机器人的标定。

进一步地，获取多组联动关节段的名义位姿和联动关节段的实际位姿数据，并利用最小二乘迭代法处理多组机器人的名义位姿和所述机器人的实际位姿以获取机器人的初始绳长误差。

实施例2

基于实施例1得到实施例2，机器人标定步骤还包括：

根据机器人的名义关节角度获取在机器人的联动角度误差下机器人的名义位姿；

参考图1，利用正常驱动结构驱动机器人以使驱动绳到达机器人的名义绳长，并获取机器人的实际位姿，根据机器人的名义关节角度计算得到机器人的名义绳长，不同的联动关节段用不同的正常驱动结构驱动，每一个联动关节段用一个正常驱动结构进行驱动。本实施例中，正常驱动结构采用3根驱动绳索驱动联动关节段，并且设置有电机位置传感器和绳索拉力传感器。在驱动机器人的整臂时，每个联动关节段中的3个驱动绳索的3个电机中，一个电机采用力控模式，其余的2个电机采用位控模式，以保证驱动绳索的张紧效果。进一步地，参考图3，同理，在机器人的末端设置有靶球，则利用api激光跟踪仪可以获取机器人的实际位姿；

根据机器人的名义位姿和机器人的实际位姿获取机器人的初始绳长误差。进一步地，获取多组机器人的名义位姿和机器人的实际位姿数据，并根据多组机器人的名义位姿和机器人的实际位姿利用最小二乘迭代法获取机器人的初始绳长误差。其中，机器人的初始绳长误差为机器人的驱动绳索的理想值和真实值之间的误差。

至此，本发明的柔性机器人运动学标定方法完成了联动关节段和机器人整臂的运动学参数标定，包括联动关节段的联动角度误差、机器人的整臂的联动角度误差以及初始绳长误差。

实施例3

实施例3是对柔性机器人运动学标定方法的具体实现过程进行具体说明：

首先，进行绳索驱动机械臂运动学的分析，绳索驱动超冗余机械臂的运动学分析不仅包括了关节空间以及操作空间之间的映射关系，还包括了绳索驱动空间到关节空间的映射关系。因此其运动学分析可以分为两步：首先推导关节空间与操作空间的映射关系，即机器人末端坐标系位姿和关节变量ψ、α之间的关系；再推导绳索驱动空间与关节空间的映射关系，即关节变量ψ、α和驱动绳索长度变化量δli之间的关系，如图4所示。

首先进行关节空间与操作空间的映射分析，对关节段进行正运动学分析，如图5所示，采用经典的d-h法，建立了为单个关节段的d-h坐标系。根据建立的坐标系，可以得到其对应的d-h参数表如表1所示。

表1联动关节段dh参数表

则第i+1参考系相对于第i参考系的齐次变换参数化矩阵，可记为

式中cθi＝cosθi,sθi＝sinθi。

由于柔性机械臂关节段的小关节之间采用了联动的运动方式，则关节角之间存在着以下的关系:

其中为联动关节段的第i关节的联动角度误差，为联动关节段m的基本关节角度，θm,i为其子关节的具体角度。则联动关节段的联动角度误差为共n个联动关节段时，柔性机器人的关节角度可以表示为：其中为臂的基本关节角，为整臂的联动角度误差。

由此可以得到单个联动关节段的正运动学表达式为：

其中θm＝[θm,1θm,2…θm,2i]为联动关节段m的具体关节角度，t为末端位姿x对应的齐次变换矩阵。

下面进行整臂的正运动学分析，由此，对于有n个联动关节段的柔性机器人，整臂的正运动学方程为：

最后进行绳索空间与关节空间的映射分析，先进行关节角到绳长的映射分析，第一，单个子关节角到绳长的分析：

如图6所示，绳索驱动超冗余机械臂的单个关节子系统中具有3个输入，2个输出，换句话说，其是一个并联机器人，具有3根驱动绳索和2个自由度。单个关节依靠3个独立的驱动绳索进行驱动，实现其三维空间的旋转运动。因此，根据前面臂的设计，在关节的臂杆中的绳子长度不会发生变化，而引起该关节角度发生变化的原因则是在关节处两个圆盘之间的绳索长度发生了变化。为了准确描述绳驱动机械臂关节的转动角度与关节处绳子的长度之间的关系，将关节模型进行简化，建立了单关节的运动学模型，如图7所示，面b1b2b3、面a1a2a3分别代表布线圆盘2和布线圆盘1，线段a1b1、a2b2、a3b3分别代表三个独立的绳索l1、l2、l3，点p则代表关节的中心。分别以面b1b2b3、面a1a2a3中心o1、o2为原点，关节臂杆轴心的方向为z轴，关节两个自由度旋转的方向为x、y轴，建立坐标系坐标系{1}、{2}。由图7可以知道关节的中心点p则固定不变，为方便分析，在关节中心点处建立中间坐标系{0}，如图8所示，假设关节初始位置时两圆盘之间的距离为d，那么对于坐标系{0}、{1}，当{1}绕其x轴旋转α角，再继续往上平移d/2后与{0}重合。于是可以得到齐次变换矩阵：

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

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

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

在坐标系{2}中：

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

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

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

因此，单个子关节的绳索长度可以统一地表示为：

其中，为第m联动关节段的第i关节的坐标变换旋转角。

而对于联动关节段的绳长计算，由上面可以知道，具有n个子关节的联动关节段m的绳索k的长度为：

因此整个联动关节段m的绳索长度计算公式可以写为：

其中为联动关节段m的基本关节角。

另外，整臂的绳长计算，考虑到控制联动关节段m的每个驱动绳索，需要穿过联动关节段1、2…m-1。因此，整臂上的驱动绳长可以表示为：

其中m＝int((k+2)/3)，代表绳索k控制的联动关节段号。

同样类似地，整臂的绳长可以表示为：

接着进行绳长到关节角的映射分析，首先，进行单个子关节的绳长到关节角映射分析，该分析作为关节角-绳长运动学的逆过程，因此满足以下几个条件：

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

li＝fjl,i(ψ,α)|(0,0)＝2d(20)

这里的2d指的是关节处两个圆盘之间的距离。

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

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

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

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

基于此，如图9所示，通过数值迭代法，可以在给定两个绳索的长度前提下，计算得到单个关节的两个旋转轴的角度值。

另外，单个联动关节段的绳长到关节角映射分析。对于任意的联动关节段m，则其驱动绳索的可以表示为：

其中代表联动关节段的m的绳索k长度计算公式，并且关节的初始条件为：

其中n是联动关节段中子关节的个数。

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

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

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

其中是雅克比矩阵jsc的逆.

基于此，参考图10，其中ls＝[l1l2]为联动关节段的其中2个驱动绳索的长度。通过数值迭代法，可以在给定两个绳索的长度和联动角度误差的前提下，计算得到联动关节段的两个基本关节角度值

最后，进行整臂绳长到关节角映射分析。如图11所示，整臂绳长到关节角度的映射分析主要分为以下几步。

给定绳长ld＝[ld,1ld,2…ld,n]＝[ld,1,ld,2…,ld,3n]，设定当前求解的联动关节段数m＝1；ld,i代表第i联动关节段的期望绳长。所有绳索在其余联动关节段(即对于绳索k而言，绳索k在联动关节段1到m-1处的绳索长度之和，m＝int((k+2)/3)，代表绳索k控制的联动关节段号)上的长度

根据绳索的总长以及绳索在其余段上的长度，求解联动关节段m上的3个驱动绳索的长度，如：

ls＝ld,m-lo,m＝[ld,3m-2,ld,3m-1,ld,3m]-[lo,3m-2,lo,3m-1,lo,3m]

利用绳长ls＝[lm,3m-2lm,3m-1lm,3m]以及图10，可以计算出联动关节段m的关节角度

更新联动关节段m处其余绳索k(3m﹤k﹤3n)的长度，即更新变量lo，即：lo,i＝lo,i+[fsl,m,3i-2(θm)fsl,m,3i-1(θm)fsl,m,3i(θm)]；i>m。

联动关节段数加一，即m＝m+1

重复以上的步骤，直到m≥n+1，完成整臂的绳长到关节角度的求解。

对于联动关节段的运动学参数标定，通过上述描述可知，其标定模型具有多层运动学和变量间强耦合的关系。其运动学关系包括了绳索驱动空间、关节空间和操作空间三层的关系。其运动学参数包括绳索长度和关节角度，它们之间互相耦合影响。它们的误差，对末端的位置和姿态之间的映射呈现非线性关系。其运动学误差模型推导如下：

由公式(3)可以得到：

联动关节段的位姿和关节角度的关系为θm＝[θ1…θ2n]；

由公式(16)可以得到：

联动关节段的绳长和关节角度的关系为ls＝fsl(θm)。

利用参数解耦驱动结构完成联动关节段的运动学参数标定，可实现参数之间的解耦，则其运动学误差模型推导如下：

由正运动学方程(公式(3))，可以得到：

两边求导可以得到：

其中jsq为柔性机器人关节段的笛卡尔空间到关节空间的映射雅克比矩阵，它可以表示如下:

m为联动关节段中子关节的个数。

则jsq可以拆分为和

θm可以拆分为θq1＝[θ1θ2]和θq2＝[θ3…θ2m]。

则

而

其中jc∈r^2×2为绳索空间和关节空间的雅克比矩阵(公式(22))。

则

由微分到差分，进一步地等价代换，可得：

联动关节段的位姿差为

而对于柔性机器人整臂的标定，可以通过多层运动学关系的推导，完成其运动学误差的模型的推导。

首先，整臂的位姿为

根据公式(18)可以得到整臂的绳长为l＝[l1…l3m]；

其中，为整臂的基本关节角度，n为联动关节段的个数。

而根据两边微分可以得到

其中，雅克比矩阵一个关节段由多个yaw和pitch自由度(即万向节)组成，v1代表关节段1中所有yaw自由度关节运动对末端产生的线速度，w1代表关节段1中所有yaw自由度关节运动对末端产生的角速度。

且

em,k代表联动关节段m的旋转轴k的单位矢量，rm,k为联动关节段m的旋转轴k中心到臂末端的矢量。v2m-1、v2m分别代表关节段m中所有yaw、pitch自由度关节运动对臂末端产生的线速度，w2m-1、w2m分别代表关节段m中所有yaw、pitch自由度关节运动对臂末端产生的角速度。

jl为关节空间到绳索空间的映射矩阵，

其中jsci,j代表联动关节段j处的绳索i的关节空间到绳索空间的映射，

fsl,j,i(θj)为联动关节段j的绳索i绳长计算公式。

可得到机器人整臂的名义位姿差为

接着，对联动关节段和机器人整臂的运动学参数标定的具体过程进行描述：

首先，本实施例中，机器人为具有2m自由度的柔性机器人，由m个联动关节段串联而成，每个联动关节段由3个以上的绳索驱动，带有电机位置传感器和绳索拉力传感器。柔性机器人联动关节段的标定，通过激光跟踪仪以及联动关节段末端的靶球标志器，能够获得每个联动关节段空间构型下的位置和姿态。通过测量的多组联动关节段的末端位置和姿态(实际位姿)，以及名义末端位置和姿态(名义位姿)，根据运动学误差的模型，用基于最小二乘的迭代法，可完成联动关节段的联动角度误差的标定。

具体地，首先，预先规划n组柔性机器人联动关节段m的关节角度这个时候规划的是利用参数解耦驱动结构驱动的联动关节段的关节角度，即联动关节段的第一个子关节的关节角度。根据绳长计算公式(16)，算出n组关节角度对应的绳索长度ls1…lsn，其中lsi＝[l1,il2,il3,i]代表第i组所有驱动绳长；再利用正运动学的方程可以计算得到联动关节段的名义位姿计算出不考虑联动角度误差的情况下，即的情况下，n组的柔性机器人联动关节段m的名义的末端位置和姿态xs,i。

再设定当前的数据组号i＝1，则当前的关节角绳长为lsi。分别控制电机，使得所有绳索到达对应的长度lsi。利用激光跟踪仪，记录此时的柔性机器人的联动关节段的末端相对于机器人根部的真实的三轴位置和姿态xrs,i；

接着，数据组号加一，即i＝i+1。

重复以上3个步骤，直到i>n，完成柔性机器人联动关节段m的实际位姿和名义位姿的获取。

根据测试的n组柔性机器人联动关节段m的真实和名义的末端位置和姿态数据xs,i，利用最小二乘迭代的方法，如图12所示，完成柔性机器人的联动关节段m的联动角度误差参数的标定。

参考图12，图12是本发明中一种柔性机器人运动学标定方法的一具体实施例联动关节段的联动角度误差标定流程图；标定的具体步骤如下：

(1)设定联动关节段的实际位姿为xs,real＝[xrs,1,xrs,2,xrs,3…xrs,n],名义位姿为xs,nom＝[xs,1,xs,2,xs,3…xs,n]。初始绳长误差和联动角度误差为：[εsl,εsθ]0＝0，其中εsl＝[εl1,εl2,εl3]∈r^3×1为联动关节段的3个绳索的初始绳长误差，为联动角度误差。测量n组标定构型下联动关节段的实际位姿，并记录对应的绳长。

初始(迭代次数为i＝0)情况下，n组构型下联动关节段的状态变量为：qs(i)＝[lcs(i),θcq2(i)]，其中lcs(i)＝[lc1(i),lc2(i),lc3(i)]代表n组所有绳长在第i次的组合,lck＝[lk,1,lk,2,…,lk,n]^t代表k(1≤k≤3)号绳索n组构型下的长度组合，θcq2,i＝[θq2,1,…,θq2,n]^t代表n组的关节角度θq2组合。

(2)计算初始情况下名义位姿和实际位姿的位姿差:δxs(i)＝xs,real(i)-xs,nom(i)，标定参数初始化[εsl,εsθ]0＝0，并且置迭代次数为i＝0；

(3)计算在当前的状态qs,i下联动关节段的组合雅克比矩阵，即

js(i)＝[js1(i),js2(i)],其中js2(i)＝[jq2,1,jq2,2,…,jq2,n]t代表n组构型下的雅克比矩阵jq2的组合，类似地

(4)根据雅克比矩阵js(i)以及末端位置姿态差δxs(i)，计算

js(i)⁺δxs(i)＝δqs(i)；

(5)更新初始绳长误差和联动角度误差，即[εsl,εsθ]i+1＝[εsl,εsθ]i+δqs(i)。

(6)更新每一组的驱动绳索的长度：lcs(i+1)＝[lc1(i+1),lc2(i+1),lc3(i+1)]＝εcl+[lc1(i),lc2(i),lc3(i)]。其中代表n个εsl矩阵的组合。

(7)根据绳长lcs(i+1)，利用绳长到关节角度的计算流程(见图10)，计算n组绳长对应的关节1角度θcq1(i+1)＝[θ1(i+1),θ2(i+1)]。

(8)更新子关节2-m的子关节角度，得到整个联动关节段的n组关节角对应的新角度，即：

θcq(i+1)＝[θcq1(i+1),θcq2(i+1)]，其中θcq2(i+1)＝θcq2(i)+εcθ(i)，其中代表n个εsθ矩阵的组合。

(9)根据n组的关节角度和正运动学方程，计算此时n组的名义位姿xs,nom(i+1)，根据实际的位姿和新的名义位姿计算位姿差δxs(i+1)。

(10)判断δxs(i+1)是否小于一定值，当判断结果为是时，迭代结束，否则迭代数i＝i+1。

(11)重复(2)-(10)，直到位姿差δxs(i+1)小于一定的值ε，便可完成单个联动关节段的联动角度误差的标定，即此时的联动角度误差εsθ为标定的结果。

而对于柔性机器人整臂的标定，可在多个联动关节段的标定之后，将联动关节段组装成整臂，进一步对新的驱动绳索初始长度进行标定。此时的机器人的联动角度误差，由于联动角度误差不随着联动关节段的重新组装而变化，因此，机器人的联动角度误差可直接参考联动关节段的标定结果。机器人的联动角度误差为多个联动关节段的联动角度误差的组合。而柔性机器人整臂的初始绳长误差标定，可通过激光跟踪仪测量若干组构型下的末端的位姿，以及推导的柔性机器人的运动学误差模型，完成整个柔性机器人的初始绳长误差标定。

具体地，预先规划n组柔性机器人的关节角度根据绳长计算公式(18)，算出n组关节角度对应的绳索长度l1…ln，其中li＝[l1,il2,i…l3n,i]代表第i组所有驱动绳长；再利用正运动学的方程计算出在已知的联动角度误差εθ情况下，n组的柔性机器人末端的名义位姿xi，其中为所有联动关节段的联动角度误差的组合。

设定当前的数据组号i＝1，则当前的关节角绳长为li。分别控制电机，使得所有绳索到达对应的长度li。

利用激光跟踪仪，记录此时的柔性机器人的末端相对于机器人根部的实际位姿xr,i；

数据组号i＝i+1。

重复以上3个步骤，直到i>n，完成柔性机器人的实际位姿和名义位姿的获取。

根据测试的n组柔性机器人的实际位姿和名义位姿，利用最小二乘迭代的方法，如图13所示，完成柔性机器人初始绳长误差的参数标定。

参考图13，图13是本发明中一种柔性机器人运动学标定方法的一具体实施例机器人整臂的初始绳长误差标定流程图；标定的具体步骤如下：

(1)设定机器人的真实位姿为xreal＝[xr,1,xr,2,xr,3…xr,n]，机器人的名义位姿为xnom＝[xnom,1,xnom,2,xnom,3…xnom,n]。初始的绳长误差和联动角度误差为：εl(0)＝0，其中εl＝[εl1,εl2，…,εl3n]∈r^3n×1为所有的驱动绳索初始绳长误差。测量n组标定构型下末端位姿，并记录对应的绳长。

初始(迭代次数为i＝0)情况下，n组构型下的柔性机器人状态变量为：lc(i)，其中lc(i)＝[lc1(i),lc2(i)，…,lc3n(i)]代表n组所有绳长在第i次的组合,lck＝[lk,1,lk,2,…,lk,n]^t代表k(1≤k≤3n)号绳索n组构型下的长度组合。

(2)计算初始情况下实际位姿与名义位姿的位姿差:δx(i)＝xreal(i)-xnom(i)，标定参数初始化εl(0)＝0，并且置迭代次数为i＝0；

(3)计算在当前的状态lc(i)情况下柔性机器人组合雅克比矩阵j(i)，其中j(i)＝[jq,1jl,1^-1,jq,2jl,2^-1,…,jq,njl,n^-1]^t代表n组构型下的雅克比矩阵j的组合，j＝jqjl^-1为柔性机器人笛卡尔空间到绳索空间的映射雅克比矩阵。

(4)根据雅克比矩阵j(i)以及末端位置姿态差δxs,i，计算

j(i)⁺δx(i)＝δl(i)，其中δl＝[δl1,δl2，…,δl3n]代表所有绳长误差的组合。

(5)更新绳长误差，即εl(i+1)＝εl(i)+δl(i)。

(6)更新每一组的驱动绳索的长度：lc(i+1)＝εcl+lc(i)。其中代表n个εl矩阵的组合。

(7)根据绳长lc(i+1)，利用绳长到关节角度的计算流程(见图11)，计算n组绳长对应的关节角度组合

(8)根据n组的关节角度，计算此时n组的名义位姿xnom(i+1)，根据实际位姿和名义位姿计算位姿差δx(i+1)。

(9)判断δx(i+1)是否小于一定值ε，当判断结果为是时，迭代结束，否则，迭代数i＝i+1。

(10)重复(2)-(9),直到位姿差δx(i+1)小于一定的值ε，便可完成柔性机器人标定，即此时的绳长初始误差εl为标定的结果。

综上，一种参数解耦的柔性机器人运动学标定方法，通过将整个柔性多关节机器人的运动学参数标定，分解成每个联动关节段的运动学参数和整臂情况下的运动学参数标定。基于运动学的误差模型以及参数解耦驱动结构，结合最小二乘迭代法，完成联动关节段以及整臂的标定。该标定的方法，具有参数之间互相解耦，效率高并且标定精度高等优点。因此比较适用于对柔性机器人的标定。

实施例4

本发明还公开一种柔性机器人运动学标定系统，包括联动关节段标定单元，联动关节段标定单元包括：

联动关节段的名义位姿获取模块，用于根据联动关节段的名义关节角度获取零联动角度误差下联动关节段的名义位姿；

联动关节段的实际位姿获取模块，用于利用参数解耦驱动结构驱动联动关节段到达联动关节段的名义绳长，并获取联动关节段的实际位姿，联动关节段的名义绳长根据联动关节段的名义关节角度计算得到，参数解耦驱动结构为驱动绳索固定在联动关节段的第一个子关节上，联动关节段的所有子关节通过联动绳索连接；进一步地，联动关节段的末端设置有靶球，利用激光跟踪仪获取联动关节段的实际位姿；

联动关节段的联动角度误差获取模块，用于根据联动关节段的名义位姿和联动关节段的实际位姿获取联动关节段的联动角度误差。

作为技术方案的进一步改进，柔性机器人运动学标定系统还包括机器人标定单元，机器人标定单元包括：

联动角度误差标定模块，用于利用联动关节段标定单元获取多个联动关节段的联动角度误差；根据多个联动关节段的联动角度误差组合得到机器人的联动角度误差，机器人包括多个联动关节段。

作为技术方案的进一步改进，机器人标定单元还包括：

机器人的名义位姿获取模块，用于根据机器人的名义关节角度获取在机器人的联动角度误差下机器人的名义位姿；

机器人的实际位姿获取模块，用于利用正常驱动结构驱动机器人到达机器人的名义绳长，并获取机器人的实际位姿，机器人的名义绳长根据机器人的名义关节角度计算得到，不同的联动关节段用不同的正常驱动结构驱动，正常驱动结构为驱动绳索固定在联动关节段的所有子关节上，联动关节段的所有子关节通过联动绳索连接；进一步地，机器人的末端设置有靶球，利用激光跟踪仪获取机器人的实际位姿；

机器人的初始绳长误差获取模块，用于根据机器人的名义位姿和机器人的实际位姿获取机器人的初始绳长误差。

柔性机器人运动学标定系统的工作过程参照上述实施例1、实施例2和实施例3中对柔性机器人运动学标定方法的具体描述，不再赘述。

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