CN107627299B - 一种绳索驱动并联机器人的运动学参数误差标定方法 - Google Patents

Abstract

本发明公开了一种绳索驱动并联机器人运动学参数误差标定方法，包括：1、建立绳索驱动并联机器人运动学模型和绳索驱动并联机器人的运动学参数误差模型，2、利用优化算法对参数误差进行辨识，3、优化标定位姿测量装置的参考坐标系相对绳索驱动并联机器人的参考坐标系的位置关系，4、结合迭代优化算法联合标定运动学参数误差。本发明能够准确标定绳索驱动并联机器人的运动学参数误差，从而提高绳索驱动并联机器人的运动学精度。

Description

一种绳索驱动并联机器人的运动学参数误差标定方法

技术领域

本发明涉及机器人运动学参数标定领域，具体涉及一种基于位姿测量的绳索驱动并联机器人运动学参数误差标定方法。

背景技术

机器人机械结构在加工和装配过程中不可避免会产生一些结构参数误差，这些结构参数误差会导致机器人的运动学模型计算的结果产生误差。所以在机器人生产过程中，必须要对机器人的运动学参数误差进行标定。

对于常用的标定工具有相机、激光干涉仪等位置和姿态测量装置，这些位姿测量装置可以测量机器人执行器末端动平台的位置和姿态。测量装置是以自身坐标系作为参考坐标系测量机器人末端执行器的位姿，所以在标定机器人运动学参数误差之前必须测量机器人参考坐标系和测量装置参考坐标系之间的相对位置关系。但是由于测量装置的参考测量位置和机器人结构之间存在误差，导致测量的相对位置关系与实际位置关系之间存在误差，使得最终标定的机器人运动学参数误差精度降低。工业生产过程中需要对很多机器人进行标定，传统方法需要对测量装置的位置关系进行手动标定，导致标定速度慢，标定过程繁琐，生产效率低下。所以亟需一种可以自动进行标定，并且具有较高精度的标定方法，以期能够提高生产效率，扩大经济效益。

发明内容

本发明为克服现有技术的不足之处，提供一种绳索驱动并联机器人的运动学参数误差标定方法，以期能够准确标定绳索驱动并联机器人的运动学参数误差，从而提高绳索驱动并联机器人的运动学精度。

本发明为达到上述发明目的，采用如下技术方案：

1、一种绳索驱动并联机器人的运动学参数误差标定方法，是应用于绳索驱动并联机器人的标定过程中，并在所述绳索驱动并联机器人一侧设置有位姿测量装置；其特征是，所述运动学参数误差标定方法是按照下步骤进行：

步骤1、在所述位姿测量装置的测量工作范围内，建立所述绳索驱动并联机器人的参考坐标系O_s，机器人末端动平台上的参考坐标系O_p，位姿测量装置的参考坐标系O_c，并将所述位姿测量装置的参考坐标系O_c作为世界坐标系；

步骤2、建立所述绳索驱动并联机器人的运动学模型；

步骤2.1、对于为m绳索驱动装置输出的n自由度的绳索驱动并联机器人，所述机器人末端动平台在所述绳索驱动并联机器人的参考坐标系O_s中的理论位姿表示为X_s＝[P_sΦ_s]^T，其中P_s表示所述机器人末端动平台的位置，Φ_s表示所述机器人末端动平台的姿态；

令b_i表示第i绳索驱动装置的绳索连接点在所述机器人末端动平台上的参考坐标系O_p上的位置，a_i表示第i绳索驱动装置的绳索输出点在所述绳索驱动并联机器人的参考坐标系O_s上的位置；i＝1…m；

利用式(1)表示单个绳索驱动装置的闭链方程：

l_i＝a_i-P_s-R_sp(Φ_s)b_i (1)

式(1)中，l_i表示第i绳索驱动装置的绳索输出点到所述机器人末端动平台的绳索连接点的绳索向量，R_sp表示所述机器人末端动平台上的参考坐标系O_p到所述绳索驱动并联机器人的参考坐标系O_s的旋转矩阵；

步骤2.2、根据式(2)得到绳索驱动并联机器人的逆运动学方程：

||l_i||₂＝(a_i-P_s-R_sp(Φ_s)b_i)^T(a_i-P_s-R_sp(Φ_s)b_i),i＝1,2,…,m (2)

由式(2)得到所述绳索驱动并联机器人的绳索长度向量为l＝[||l₁||₂,||l₂||₂,…||l_i||₂,…,||l_m||₂]^T；

步骤3、建立所述绳索驱动并联机器人的运动学参数误差模型；

步骤3.1、令由机械加工和装配因素导致第i绳索驱动装置的机器人运动学误差参数表示为其中，Δb_i表示所述第i绳驱动装置的绳索连接点在所述机器人末端动平台上的参考坐标系O_p上的位置误差，Δa_i表示所述第i绳索驱动装置的绳索输出点在所述绳索驱动并联机器人的参考坐标系O_s上的位置误差，表示电机编码器单位转动角度对应绳索的输出长度误差，则需要辨识的运动学参数误差共有m个7×1维的第i绳索驱动装置的机器人运动学误差参数Cal_i所组成的运动学参数误差Cal＝[Cal₁ Cal₂ …Cal_i … Cal_m]^T；

步骤3.2、利用所述位姿测量装置测量N组机器人末端动平台的位姿，并利用第j组测量的位姿求得如式(3)所示的电机编码器输出角度θ_i的误差

式(3)中，{θ_m,j}表示用第j组测量的位姿求得的m个电机编码器反馈的实际转动角度，l^j表示用第j组测量的位姿求得的绳索长度向量；表示第j组测量的位姿中电机编码器单位转动角度对应绳索的输出长度误差；

步骤4、由式(3)得到如式(4)所示的优化目标方程：

式(4)中，e_θ表示所述N组机器人末端动平台的位姿求得的m个电机编码器输出角度的误差方程，并有：

采用最小二乘法对式(4)进行优化求解，得到如式(5)所示的运动学参数误差辨识模型：

式(5)中，为误差方程e_θ对运动学参数误差Cal的雅可比矩阵，W_t表示运动学参数误差Cal的归一化矩阵；

步骤5、标定位姿测量装置的参考坐标系O_c相对所述绳索驱动并联机器人的参考坐标系O_s的位置关系；

步骤5.1、令所述机器人末端动平台的实际测量位姿表示为X_cs＝[P_cs Φ_cs]^T，其中，P_cs表示所述绳索驱动并联机器人的参考坐标系O_s在所述位姿测量装置的参考坐标系O_c中的位置，Φ_cs表示绳索驱动并联机器人的参考坐标系O_s在所述位姿测量装置的参考坐标系O_c中的姿态；则利用式(6)表示所述机器人末端动平台在所述位姿测量装置的参考坐标系O_c中的位置P_c：

P_c＝R_cs(Φ_cs)P_s+P_cs (6)

式(6)中，R_cs表示绳索驱动并联机器人的参考坐标系O_s相对于所述位姿测量装置的参考坐标系O_c的旋转矩阵；

步骤5.2、简化式(6)，从而利用式(7)建立位姿测量装置的参考坐标系O_c与所述绳索驱动并联机器人的参考坐标系O_s的位置关系方程：

(P_c-P_cs)^T(P_c-P_cs)＝P_s ^TP_s (7)

利用式(8)建立位姿测量装置的参考坐标系O_c与所述绳索驱动并联机器人的参考坐标系O_s的位置关系优化方程，再利用L-M算法对式(8)进行优化求解，得到所述绳索驱动并联机器人的参考坐标系O_s在所述位姿测量装置的参考坐标系O_c中的位置P_cs：

minf(P_cs)＝||(P_c-P_cs)^T(P_c-P_cs)-(P_s ^TP_s)||₂ (8)

步骤5.3、令P_c-P_cs＝A，P_s＝B，根据式(6)得到R_cs(Φ_cs)B＝A，则利用式(9)得到位姿测量装置的参考坐标系O_c与所述绳索驱动并联机器人的参考坐标系O_s的姿态关系方程，再利用L-M算法对式(9)进行优化求解，得到绳索驱动并联机器人的参考坐标系O_s在所述位姿测量装置的参考坐标系O_c中的姿态Φ_cs：

R_cs(Φ_cs)＝AB^T(B·BT)^-1 (9)

步骤6、联合标定运动学参数误差；

步骤6.1、利用式(10)计算位姿测量装置测量N组机器人末端动平台的位姿的均方误差SD：

式(10)中，X_c表示在位姿测量装置的参考坐标系O_c中，位姿测量装置测量的机器人末端动平台的实际位姿，表示在位姿测量装置的参考坐标系O_c中，机器人末端动平台的理论位姿；

步骤6.2、初始化迭代次数iter＝1，运动学参数误差Cal，最大迭代次数为itermax；

步骤6.3、利用步骤5第iter次计算位姿测量装置的参考坐标系O_c与所述绳索驱动并联机器人的参考坐标系O_s的相对关系

步骤6.4、利用最小二乘法第iter次标定运动学参数误差Cal^iter；

步骤6.5、将Cal^iter+Cal赋值给Cal^iter，将iter+1赋值给iter；

步骤6.6、如果SD＜＜δ且iter＜iter max，其中δ为极小值，则标定结束，输出Cal^iter；如果iter＞iter max，则终止循环；如果SD＞δ且iter＜iter max，则返回步骤6.3。

与已有技术相比，本发明的有益效果体现在：

1、本发明通过建立运动学参数误差模型，接着利用标定算法标定机器人参考坐标系和测量装置参考坐标系之间的相对位置关系，然后利用优化算法辨识运动学参数误差，从而提高了绳索驱动并联机器人的运动学参数误差标定精度；在一次标定过程中由于存在测量误差，因此采用迭代的优化算法，对运动学参数误差标定过程进行迭代优化，使得最终的运动学误差降到最低，大大提高了参数误差标定精度。在整个标定过程中算法可以自动进行，不需要人工干预，大大提高了标定效率。

2、本发明建立了绳索驱动并联机器人的运动学参数误差模型，并使用优化算法对误差进行辨识，使得参数误差标定结果更准确；

3、本发明利用优化算法标定位姿测量装置的自身参考坐标系和并联机器人自身参考坐标系的相对位置关系，大大提高了位姿测量装置的测量精度。

4、本发明采用迭代的参数误差辨识方法，对标定的位姿测量装置的自身参考坐标系和并联机器人自身参考坐标系相对位置关系和辨识的运动学参数误差进行迭代辨识，整个标定过程可以自动执行，不需要人工干预，大大提高了标定效率和运动学参数误差辨识精度。

附图说明

图1是本发明标定过程中参考坐标系示意图。

具体实施方式

本实施例中，一种基于位置姿态测量的绳索驱动并联机器人运动学参数误差标定方法，是应用于绳索驱动并联机器人的标定过程中，在绳索驱动并联机器人一侧设置有位置姿态测量装置；具体的说，该误差标定方法包括：并联机器人运动学参数误差辨识模型、位置姿态测量装置坐标系标定模块、机器人运动学参数误差辨识模块。

绳索驱动并联机器人存在运动学参数误差，依据并联机器人的运动学模型建立并联机器人运动学参数误差模型；根据运动学误差参数模型和位置姿态测量装置测量的误差得到参数误差辨识模型，并利用这个参数误差辨识模型进行参数辨识。

位置姿态测量装置坐标系标定模型以位置姿态测量装置建立世界坐标系，以绳索驱动并联机器人建立参考坐标系，根据位置测量装置测量得到的并联机器人末端位置姿态和参考坐标系下的位置姿态求解标定参考坐标系和世界坐标系的相对位置关系。

机器人运动学参数误差辨识模块以并联机器人参数误差辨识模型和位置姿态测量装置坐标系标定模块为基础，利用参数辨识优化算法对运动学误差参数进行辨识。

本实施例中，一种绳索驱动并联机器人的运动学参数误差标定方法，是应用于绳索驱动并联机器人的标定过程中，并在绳索驱动并联机器人一侧设置有位姿测量装置，位置姿态测量装置为通用的位置姿态的测量装置，可以利用位置姿态测量装置测量得到末端动平台的运动误差；在标定过程中可以辨识运动学参数误差，提高绳索驱动并联机器人运动学精度；具体的说，该运动学参数误差标定方法是按照下步骤进行：

步骤1、在位姿测量装置的测量工作范围内，测量装置泛指可以测量绳索驱动机器人末端动平台位姿的装置，可以采用激光干涉仪，6D摄像机以及运动捕捉***，一般要求测量装置的精度要远高于机器人本身的精度；建立绳索驱动并联机器人的参考坐标系O_s，机器人末端动平台上的参考坐标系O_p，参考坐标系O_s可以建立在绳索驱动并联机器人的绳索驱动装置的绳索输出点上，使得参考系原点和绳索输出点向重合，便于建立运动学方程；机器人末端动平台上的参考坐标系O_p建立在动平台的几何中心点，方便建立运动学模型；位姿测量装置的参考坐标系O_c，并将位姿测量装置的参考坐标系O_c作为世界坐标系；参考坐标系O_c是测量装置设定好的，一般在测量装置上会标出；

步骤2、建立绳索驱动并联机器人的运动学模型；

步骤2.1、对于为m绳索驱动装置输出的n自由度的绳索驱动并联机器人，末端动平台在绳索驱动并联机器人的参考坐标系O_s中的理论位姿表示为X_s＝[P_s Φ_s]^T，其中P_s表示末端动平台的位置，Φ_s表示末端动平台的姿态；

如图1所示，令b_i表示第i绳索驱动装置的绳索连接点在机器人末端动平台上的参考坐标系O_p上的位置，a_i表示第i绳索驱动装置的绳索输出点在绳索驱动并联机器人的参考坐标系O_s上的位置；i＝1…m；

利用式(1)表示单个绳索驱动装置的闭链方程：

l_i＝a_i-P_s-R_sp(Φ_s)b_i (1)

式(1)中，l_i表示第i绳索驱动装置的绳索输出点到机器人末端动平台的绳索连接点的绳索向量，R_sp表示机器人末端动平台上的参考坐标系O_p到绳索驱动并联机器人的参考坐标系O_s的旋转矩阵；

步骤2.2、根据式(2)得到绳索驱动并联机器人的逆运动学方程：

||l_i||₂＝(a_i-P_s-R_sp(Φ_s)b_i)^T(a_i-P_s-R_sp(Φ_s)b_i),i＝1,2,…,m (2)

由式(2)得到绳索驱动并联机器人的绳索长度向量为l＝[||l₁||₂,||l₂||₂,…||l_i||₂,…,||l_m||₂]^T；

步骤3、建立绳索驱动并联机器人的运动学参数误差模型；

步骤3.1、令由机械加工和装配因素导致的机器人运动学误差参数表示为其中Δb_i表示第i绳驱动装置的绳索连接点在机器人末端动平台上的参考坐标系O_p上的位置误差，Δa_i表示第i绳索驱动装置的绳索输出点在绳索驱动并联机器人的参考坐标系O_s上的位置误差，表示电机编码器单位转动角度对应绳索的输出长度误差，一般b_i和a_i中各有3个参数，有1个参数，则需要辨识的运动学参数误差共有m个7×1维的第i绳索驱动装置的机器人运动学误差参数Cal_i所组成的运动学参数误差Cal＝[Cal₁Cal₂ … Cal_i … Cal_m]^T；

步骤3.2、利用位姿测量装置测量N组末端动平台的位姿，为了保证参数辨识的鲁棒性，要求测量数据N的数目要远大于辨识参数个数；每个测量装置可以提供1个方程，所以N×m＞＞7m，为了保证标定速度，可以使得N＝20；并利用第j组测量的位姿求得如式(3)所示的电机编码器输出角度θ_i的误差

式(3)中，，{θ_m,j}表示用第j组测量的位姿求得的m个电机编码器反馈的实际转动角度，l^j表示用第j组测量的位姿求得的绳索长度向量；表示第j组测量的位姿中电机编码器单位转动角度对应绳索的输出长度误差；

步骤4、由式(3)得到如式(4)所示的优化目标方程：

式(4)中，e_θ表示N组末端动平台的位姿求得的m个电机编码器输出角度的误差方程，并有：

采用最小二乘法对式(4)进行优化求解，得到如式(5)所示的运动学参数误差辨识模型：

式(5)中，为误差方程e_θ对运动学参数误差Cal的雅可比矩阵，W_t表示运动学参数误差Cal的归一化矩阵；

步骤5、标定位姿测量装置的参考坐标系O_c相对绳索驱动并联机器人的参考坐标系O_s的位置关系；

步骤5.1、令末端动平台的实际测量位姿表示为X_cs＝[P_cs Φ_cs]^T，其中，P_cs表示绳索驱动并联机器人的参考坐标系O_s在位姿测量装置的参考坐标系O_c中的位置，Φ_cs表示绳索驱动并联机器人的参考坐标系O_s在位姿测量装置的参考坐标系O_c中的姿态；则利用式(6)表示机器人末端动平台在位姿测量装置的参考坐标系O_c中的位置P_c：

P_c＝R_cs(Φ_cs)P_s+P_cs (6)

式(6)中，R_cs表示绳索驱动并联机器人的参考坐标系O_s相对于位姿测量装置的参考坐标系O_c的旋转矩阵；

步骤5.2、由于R_cs为齐次矩阵，有R_cs(Φ_cs)^TR_cs(Φ_cs)＝1，将式(6)变形，从而利用式(7)建立位姿测量装置的参考坐标系O_c与绳索驱动并联机器人的参考坐标系O_s的位置关系方程：

(P_c-P_cs)^T(P_c-P_cs)＝P_s ^TP_s (7)

利用式(8)建立位姿测量装置的参考坐标系O_c与绳索驱动并联机器人的参考坐标系O_s的位置关系优化方程，再利用L-M算法对式(8)进行优化求解，得到绳索驱动并联机器人的参考坐标系O_s在位姿测量装置的参考坐标系O_c中的位置P_cs：

minf(P_cs)＝||(P_c-P_cs)^T(P_c-P_cs)-(P_s ^TP_s)||₂ (8)

步骤5.3、令P_c-P_cs＝A，P_s＝B，根据式(6)得到R_cs(Φ_cs)B＝A，则利用式(9)得到位姿测量装置的参考坐标系O_c与绳索驱动并联机器人的参考坐标系O_s的姿态关系方程，再利用L-M算法对式(9)进行优化求解，得到绳索驱动并联机器人的参考坐标系O_s在位姿测量装置的参考坐标系O_c中的姿态Φ_cs：

R_cs(Φ_cs)＝AB^T(B·B^T)^-1 (9)

步骤6、联合标定运动学参数误差；

步骤6.1、利用式(10)计算位姿测量装置测量N组末端动平台的位姿的均方误差SD：

式(10)中，X_c表示在位姿测量装置的参考坐标系O_c中，位姿测量装置测量的机器人末端动平台的实际位姿，表示在位姿测量装置的参考坐标系O_c中，机器人末端动平台的理论位姿；

步骤6.2、初始化迭代次数iter＝1，运动学参数误差Cal，最大迭代次数为itermax，可以取值iter max＝10；

步骤6.3、利用步骤5第iter次计算位姿测量装置的参考坐标系O_c与绳索驱动并联机器人的参考坐标系O_s的相对关系

步骤6.4、利用最小二乘法第iter次标定运动学参数误差Cal^iter；

步骤6.5、将Cal^iter+Cal赋值给Cal^iter，将iter+1赋值给iter；

步骤6.6、如果SD＜＜δ且iter＜iter max，其中δ为极小值，取δ＝10^-6；则标定结束，输出Cal^iter；如果iter＞iter max，则终止循环；如果SD＞δ且iter＜iter max，则返回步骤6.3。

Claims (1)

1.一种绳索驱动并联机器人的运动学参数误差标定方法，是应用于绳索驱动并联机器人的标定过程中，并在所述绳索驱动并联机器人一侧设置有位姿测量装置；其特征是，所述运动学参数误差标定方法是按照下步骤进行：

步骤1、在所述位姿测量装置的测量工作范围内，建立所述绳索驱动并联机器人的参考坐标系O_s，机器人末端动平台上的参考坐标系O_p，位姿测量装置的参考坐标系O_c，并将所述位姿测量装置的参考坐标系O_c作为世界坐标系；

步骤2、建立所述绳索驱动并联机器人的运动学模型；

步骤2.1、对于为m绳索驱动装置输出的n自由度的绳索驱动并联机器人，所述机器人末端动平台在所述绳索驱动并联机器人的参考坐标系O_s中的理论位姿表示为X_s＝[P_s Φ_s]^T，其中P_s表示所述机器人末端动平台的位置，Φ_s表示所述机器人末端动平台的姿态；

令b_i表示第i绳索驱动装置的绳索连接点在所述机器人末端动平台上的参考坐标系O_p上的位置，a_i表示第i绳索驱动装置的绳索输出点在所述绳索驱动并联机器人的参考坐标系O_s上的位置；i＝1…m；

利用式(1)表示单个绳索驱动装置的闭链方程：

l_i＝a_i-P_s-R_sp(Φ_s)b_i (1)

式(1)中，l_i表示第i绳索驱动装置的绳索输出点到所述机器人末端动平台的绳索连接点的绳索向量，R_sp表示所述机器人末端动平台上的参考坐标系O_p到所述绳索驱动并联机器人的参考坐标系O_s的旋转矩阵；

步骤2.2、根据式(2)得到绳索驱动并联机器人的逆运动学方程：

||l_i||₂＝(a_i-P_s-R_sp(Φ_s)b_i)^T(a_i-P_s-R_sp(Φ_s)b_i),i＝1,2,…,m (2)

由式(2)得到所述绳索驱动并联机器人的绳索长度向量为l＝[||l₁||₂,||l₂||₂,…||l_i||₂,…,||l_m||₂]^T；

步骤3、建立所述绳索驱动并联机器人的运动学参数误差模型；

步骤3.1、令由机械加工和装配因素导致第i绳索驱动装置的机器人运动学误差参数表示为其中，Δb_i表示所述第i绳驱动装置的绳索连接点在所述机器人末端动平台上的参考坐标系O_p上的位置误差，Δa_i表示所述第i绳索驱动装置的绳索输出点在所述绳索驱动并联机器人的参考坐标系O_s上的位置误差，表示电机编码器单位转动角度对应绳索的输出长度误差，则需要辨识的运动学参数误差共有m个7×1维的第i绳索驱动装置的机器人运动学误差参数Cal_i所组成的运动学参数误差Cal＝[Cal₁ Cal₂ … Cal_i …Cal_m]^T；

步骤3.2、利用所述位姿测量装置测量N组机器人末端动平台的位姿，并利用第j组测量的位姿求得如式(3)所示的电机编码器输出角度θ_i的误差

式(3)中，{θ_m,j}表示用第j组测量的位姿求得的m个电机编码器反馈的实际转动角度，l^j表示用第j组测量的位姿求得的绳索长度向量；表示第j组测量的位姿中电机编码器单位转动角度对应绳索的输出长度误差；

步骤4、由式(3)得到如式(4)所示的优化目标方程：

式(4)中，e_θ表示所述N组机器人末端动平台的位姿求得的m个电机编码器输出角度的误差方程，并有：

采用最小二乘法对式(4)进行优化求解，得到如式(5)所示的运动学参数误差辨识模型：

式(5)中，为误差方程e_θ对运动学参数误差Cal的雅可比矩阵，W_t表示运动学参数误差Cal的归一化矩阵；

步骤5、标定位姿测量装置的参考坐标系O_c相对所述绳索驱动并联机器人的参考坐标系O_s的位置关系；

步骤5.1、令所述机器人末端动平台的实际测量位姿表示为X_cs＝[P_cs Φ_cs]^T，其中，P_cs表示所述绳索驱动并联机器人的参考坐标系O_s在所述位姿测量装置的参考坐标系O_c中的位置，Φ_cs表示绳索驱动并联机器人的参考坐标系O_s在所述位姿测量装置的参考坐标系O_c中的姿态；则利用式(6)表示所述机器人末端动平台在所述位姿测量装置的参考坐标系O_c中的位置P_c：

P_c＝R_cs(Φ_cs)P_s+P_cs (6)

式(6)中，R_cs表示绳索驱动并联机器人的参考坐标系O_s相对于所述位姿测量装置的参考坐标系O_c的旋转矩阵；

步骤5.2、简化式(6)，从而利用式(7)建立位姿测量装置的参考坐标系O_c与所述绳索驱动并联机器人的参考坐标系O_s的位置关系方程：

(P_c-P_cs)^T(P_c-P_cs)＝P_s ^TP_s (7)

利用式(8)建立位姿测量装置的参考坐标系O_c与所述绳索驱动并联机器人的参考坐标系O_s的位置关系优化方程，再利用L-M算法对式(8)进行优化求解，得到所述绳索驱动并联机器人的参考坐标系O_s在所述位姿测量装置的参考坐标系O_c中的位置P_cs：

minf(P_cs)＝||(P_c-P_cs)^T(P_c-P_cs)-(P_s ^TP_s)||₂ (8)

步骤5.3、令P_c-P_cs＝A，P_s＝B，根据式(6)得到R_cs(Φ_cs)B＝A，则利用式(9)得到位姿测量装置的参考坐标系O_c与所述绳索驱动并联机器人的参考坐标系O_s的姿态关系方程，再利用L-M算法对式(9)进行优化求解，得到绳索驱动并联机器人的参考坐标系O_s在所述位姿测量装置的参考坐标系O_c中的姿态Φ_cs：

R_cs(Φ_cs)＝AB^T(B·B^T)^-1 (9)

步骤6、联合标定运动学参数误差；

步骤6.1、利用式(10)计算位姿测量装置测量N组机器人末端动平台的位姿的均方误差SD：

式(10)中，X_c表示在位姿测量装置的参考坐标系O_c中，位姿测量装置测量的机器人末端动平台的实际位姿，表示在位姿测量装置的参考坐标系O_c中，机器人末端动平台的理论位姿；

步骤6.2、初始化迭代次数iter＝1，运动学参数误差Cal，最大迭代次数为itermax；

步骤6.3、利用步骤5第iter次计算位姿测量装置的参考坐标系O_c与所述绳索驱动并联机器人的参考坐标系O_s的相对关系

步骤6.4、利用最小二乘法第iter次标定运动学参数误差Cal^iter；

步骤6.5、将Cal^iter+Cal赋值给Cal^iter，将iter+1赋值给iter；

步骤6.6、如果SD＜＜δ且iter＜itermax，其中δ为极小值，则标定结束，输出Cal^iter；如果iter＞itermax，则终止循环；如果SD＞δ且iter＜itermax，则返回步骤6.3。