CN111590566B - 一种适用于全构型的空间机械臂运动学参数在轨标定方法 - Google Patents

Abstract

本发明一种适用于全构型的空间机械臂运动学参数在轨标定方法，采用修正的DH建模方法，更适合于描述机械臂运动学参数偏差对末端位姿的影响，可应用于具有各种几何构型的机械臂，减小了机械臂相邻关节平行对机械臂运动学参数标定矩阵的影响，大大降低了标定结果出现病态的概率，保证了求解的精度和可靠性。以空间机械臂上通常配备的手眼双目相机作为测量工具，无需额外增加测量设备，标定成本低。通过在机械臂安装面上放置测量靶标，即可实现机械臂末端位姿测量，方法简单可靠。本发明设定机械臂构型的判断准则，提升了标定结果的一致性和可靠性。

Description

一种适用于全构型的空间机械臂运动学参数在轨标定方法

技术领域

本发明涉及一种适用于全构型的空间机械臂运动学参数在轨标定方法，属于空间机械臂运动学参数标定领域。

背景技术

在发射前，空间机械臂的运动学参数会采用精密设备进行标定。但是，在发射过程中，机械臂会受到很大的冲击和过载；入轨后，由于所受重力很小，又会造成应力释放。这些因素都会导致空间机械臂的运动学参数发生变化。为确保空间操作任务的成功完成，必须对空间机械臂的运动学参数进行在轨标定。然而，对空间机械臂运动学参数进行在轨标定面临很多困难挑战，体现在以下两方面：

(1)对空间机械臂末端位姿缺少有效的在轨测量手段；

(2)基于常规DH方法对机械臂进行运动学参数标定时，若机械臂相邻两个关节的轴线近乎平行，由于它们之间的公垂线难以定义，将会使标定矩阵出现病态现象，导致无法获得理想标定结果。

发明内容

本发明的目的在于克服现有技术的不足，提供一种适用于全构型的空间机械臂运动学参数在轨标定方法，解决了机械臂相邻关节平行情况下的运动学参数标定病态问题，保证了求解的精度和可靠性。

本发明目的通过如下技术方案予以实现：

提供一种适用于全构型的空间机械臂运动学参数在轨标定方法，包括如下步骤：

(1)机械臂安装基板上安装靶标，对于给定的一组机械臂构型，由机械臂末端安装的手眼双目相机测量得到靶标中心点坐标系相对于手眼坐标系的位姿；

(2)根据靶标和手眼相机的安装矩阵，得出机械臂末端在基座坐标系中的位姿测量值；

(3)基于空间机械臂标称运动学参数和关节角度遥测值，由正运动学计算得到末端执行器在机械臂基座坐标系的位姿标称值；

(4)由空间机械臂末端位姿的测量值和标称值，得到一组关于机械臂运动学参数的偏差方程；

(5)如果得到l组关于机械臂运动学参数的偏差方程，则进入步骤(6)，否则更换一组机械臂构型，返回步骤(1)；l满足6l≥5n，n为机械臂的自由度；

(6)求解l组偏差方程得到机械臂运动学参数误差，再根据运动学参数误差计算出更为准确的运动学参数估计值，完成空间机械臂运动学参数标定。

优选的，步骤(1)中由机械臂末端安装的手眼双目相机测量得到靶标中心点坐标系相对于手眼坐标系的位姿的具体方法为：设机械臂当前构型为θ＝[θ₁ … θ_n]^T，手眼双目相机测量得到靶标中心点坐标系相对于手眼坐标系的位姿由齐次变换矩阵^camT_blank∈R^4×4给出。

优选的，步骤(2)中根据靶标和手眼相机的安装矩阵，得出机械臂末端在基座坐标系中的位姿测量值的具体方法为：

机械臂末端在基座坐标系中的标称位姿⁰T_end∈R^4×4为

⁰T_end＝⁰T_blank·(^endT_cam·^camT_blank)^-1

⁰T_end可写为如下形式

其中，⁰R_end∈R^3×3为机械臂末端在基座坐标系中的标称姿态矩阵，⁰p_end∈R^3×1为机械臂末端坐标系原点在基座系中的位置坐标；^endT_cam∈R^4×4为手眼相机坐标系相对于机械臂末端系的安装矩阵；

将机械臂末端在基座坐标系中的标称姿态的最小表示记为φ∈R^3×1，由三个角度表示，具体形式为：

根据姿态矩阵⁰R_end，得到对应某种转序的欧拉角表示

ψ；

机械臂末端在基座坐标系中的位姿测量值x_m∈R^6×1为

优选的，步骤(3)中采用空间机械臂标称运动学参数和关节角度遥测值，由正运动学计算得到末端执行器在机械臂基座坐标系的位姿标称值的具体方法为：

令a＝[a₁ … a_n]^T，α＝[α₁ … α_n]^T，d＝[d₁ … d_n]^T，θ＝[θ₁ … θ_n]^T，β＝[β₁ …β_n]^T表示n自由度机械臂的修正DH参数，其中a、α、d、θ分别表示机械臂关节杆件长度、杆件扭角、相邻关节距离、相邻关节转角，β为两个相邻关节的平行度；

根据机械臂正运动学，机械臂末端在基座系中的位姿标称值x_nom∈R^6×1为：

x_nom＝k(a_nom,α_nom,d_nom,θ_nom,β_nom)

其中，机械臂正运动学函数k(·)给出了从机械臂关节空间到任务空间的映射，a_nom,α_nom,d_nom,θ_nom,β_nom分别表示机械臂修正DH参数a，α，d，θ，β的标称值。

优选的，步骤(4)中由空间机械臂末端位姿的测量值和标称值，得到一组关于机械臂运动学参数的偏差方程的具体方法为：

机械臂末端位姿偏差Δx＝x_m-x_nom，采用一阶近似得到

其中，Δa，Δα，Δd，Δθ，Δβ表示机械臂DH参数真实值与标称值的偏差；

表示机械臂正运动学函数k(·)对该DH参数的偏微分，为参数空间与工作空间之间的雅克比变换；

将机械臂修正DH参数写为矢量形式

ζ＝[a^T α^T d^T θ^T β^T]^T

令ζ_true∈R^5n×1、ζ_nom∈R^5n×1分别表示机械臂修正DH参数的真实值与标称值，则机械臂修正DH参数偏差Δζ∈R^5n×1为

Δζ＝ζ_true-ζ_nom

定义机械臂运动学标定矩阵Φ∈R^6×5n如下

该标定矩阵可由机械臂修正DH参数标称值ζ_nom计算得出；

机械臂末端位姿偏差的矩阵形式为：Δx＝Φ·Δζ。

优选的，求解l组偏差方程获得机械臂运动学参数误差的方法为采用最小二乘法迭代求解。

优选的，最小二乘法迭代求解的终止条件为，两次迭代得到的Δζ之差的绝对值小于设定阈值。

优选的，获得机械臂运动学参数误差后，计算得到机械臂运动学参数的估计值，具体方法为：

求解

获得Δζ，机械臂运动学参数的估计值为ζ′＝ζ_nom+Δζ。

优选的，机械臂构型满足：将矩阵

进行奇异值分解，在手眼相机能够清晰看到靶标的前提下，选用矩阵

的最小奇异值作为度量指标，选择最小奇异值高于设定阈值的机械臂构型。

优选的，机械臂安装基板上装两个圆形测量靶标，分别位于安装基板的两侧，且关于机械臂在安装基板上的锁紧构型对称设置，两个圆形测量靶标的中心点距离大于手眼双目相机光心之间的距离。

本发明与现有技术相比具有如下优点：

(1)本发明采用了修正的DH建模方法，更适合于描述机械臂运动学参数偏差对末端位姿的影响，可应用于各种几何构型的机械臂，降低了机械臂相邻关节平行对标定矩阵的影响，大大降低了标定结果出现病态的概率，保证了求解的精度和可靠性。

(2)本发明以空间机械臂上通常配备的手眼双目相机作为测量工具，无需额外增加测量设备，标定成本低。通过在机械臂安装面合理放置靶标，即可实现机械臂末端位姿测量，方法简单可靠。

(3)本发明设定机械臂构型的判断准则，提升了标定结果的一致性和可靠性。

附图说明

图1为本发明的方法流程示意图；

图2为机械臂修正DH参数示意图。

具体实施方式

基于搭建的空间机械臂运动学参数标定地面试验系统，使用本发明所公开的机械臂运动学参数规划方法。机械臂采用串联结构，末端安装手眼双目相机。机械臂安装基板上装有圆形的测量靶标。对各设备状态确认无误后，开始进行标定试验，试验流程如下：①给定一组机械臂构型，基于手眼双目相机对靶标的测量，得到靶标中心点坐标系相对于手眼坐标系的位姿；②根据靶标和手眼相机的安装矩阵，得出机械臂末端位姿测量值；③根据修正的D-H方法，采用标称运动学参数和机械臂关节角度，计算得到机械臂末端位姿标称值；④由机械臂末端在基座坐标系中位姿的测量值和标称值，得到一组关于机械臂运动学参数的偏差方程；⑤重复上述步骤l次，得到l组关于机械臂运动学参数的偏差方程；⑥采用最小二乘等方法，计算得到机械臂运动学参数的估计值。

本发明目的通过如下技术方案予以实现：一种适用于全构型的空间机械臂运动学参数在轨标定方法，结合图1，包括步骤如下：

(1)给定一组机械臂构型，由手眼双目相机测量得到靶标中心点坐标系相对于手眼坐标系的位姿；

(2)根据靶标和手眼相机的安装矩阵，得出机械臂末端在基座坐标系中的位姿，即为测量值；

(3)根据修正的D-H方法，采用空间机械臂标称运动学参数和关节角度遥测值，由正运动学计算得到末端执行器在机械臂基座坐标系的位姿，即为标称值；

(4)由机械臂末端在基座坐标系中位姿的测量值和标称值，得到一组关于机械臂运动学参数的偏差方程；

(5)重复步骤(1)～(4)，得到l组关于机械臂运动学参数的偏差方程；

(6)采用最小二乘等方法，求解机械臂运动学参数误差，计算得到机械臂运动学参数的估计值。

所述步骤(1)的具体方法为：

给定一组机械臂构型，由手眼双目相机测量得到靶标中心点坐标系相对于手眼坐标系的位姿，具体为：

设机械臂当前构型为θ＝[θ₁ … θ_n]^T，n为机械臂自由度，手眼双目相机测量得到靶标中心点坐标系相对于手眼相机坐标系的相对位姿，由齐次变换矩阵^camT_blank∈R^4×4表示。

所述步骤(2)的具体方法为：

根据靶标和手眼相机的安装矩阵，得出机械臂末端在基座坐标系中的位姿，即为测量值，具体为：

令靶标中心相对于机械臂基座系的安装矩阵为⁰T_blank∈R^4×4，通过事先测量获得。手眼相机通常安装于机械臂最后一个，即第n个连杆上，令手眼相机坐标系相对于机械臂末端系的安装矩阵为^endT_cam∈R^4×4，通过事先测量获得。于是，机械臂末端在基座坐标系中的标称位姿⁰T_end∈R^4×4为

⁰T_end＝⁰T_blank·(^endT_cam·^camT_blank)^-1

⁰T_end可写为

其中，⁰R_end∈R^3×3为机械臂末端在基座坐标系中的标称姿态矩阵，⁰p_end∈R^3×1为机械臂末端坐标系原点在基座系中的位置坐标，机械臂末端坐标系原点通过DH建模得到，基座系通过DH建模得到。

将机械臂末端在基座坐标系中的标称姿态的最小表示记为φ∈R^3×1，可由三个角度表示，具体形式为

根据姿态矩阵⁰R_end，可以得到对应某种转序的欧拉角表示

ψ，即为φ。

于是，机械臂末端在基座坐标系中的位姿测量值x_m∈R^6×1为

所述步骤(3)的具体方法为：

根据修正的D-H方法，采用空间机械臂标称运动学参数和关节角度遥测值，由正运动学计算得到末端执行器在机械臂基座坐标系的位姿，即为标称值，具体为：

令a＝[a₁ … a_n]^T，α＝[α₁ … α_n]^T，d＝[d₁ … d_n]^T，θ＝[θ₁ … θ_n]^T，β＝[β₁ …β_n]^T表示n自由度机械臂的修正DH参数的标称值，其中a、α、d、θ与经典DH参数的含义相同分别为关节杆件长度、杆件扭角、相邻关节距离、相邻关节转角，β为机械臂两个相邻关节平行时引入的新的DH参数，表征两个相邻关节的平行度。

根据机械臂正运动学，可得机械臂末端在基座系中的位姿标称值x_nom∈R^6×1为

x_nom＝k(a_nom,α_nom,d_nom,θ_nom,β_nom)

其中，机械臂正运动学函数k(·)给出了从机械臂关节空间到任务空间的映射，a_nom,α_nom,d_nom,θ_nom,β_nom分别表示机械臂修正DH参数a、α、d、θ、β的标称值。

所述步骤(4)的具体方法为：

由机械臂末端在基座坐标系中位姿的测量值和标称值，得到一组关于机械臂运动学参数的偏差方程，具体为：

机械臂末端位姿偏差Δx＝x_m-x_nom反映了机械臂在该构型下的末端精度度量。基于小偏差假设，采用一阶近似，可以得到

其中，Δa，Δα，Δd，Δθ，Δβ表示机械臂DH参数真实值与标称值的偏差。

表示机械臂正运动学函数k(·)对该DH参数的偏微分，为参数空间与工作空间之间的雅克比变换。

将机械臂DH参数写为矢量形式

ζ＝[a^T α^T d^T θ^T β^T]^T

令ζ_true∈R^5n×1、ζ_nom∈R^5n×1分别表示机械臂DH参数的真实值与标称值，则机械臂DH参数偏差Δζ∈R^5n×1为

Δζ＝ζ_true-ζ_nom

定义机械臂运动学标定矩阵Φ∈R^6×5n如下

该标定矩阵可由机械臂DH参数标称值ζ_nom计算得出。

于是，机械臂末端位姿偏差可写为矩阵形式

Δx＝Φ·Δζ

所述步骤(5)的具体方法为：

重复步骤(1)～(4)，得到l组关于机械臂运动学参数的偏差方程，具体为：

步骤(4)中得到的机械臂末端位姿偏差方程组中，共有6个方程、5n个未知数，且5n>6。为求解机械臂DH参数偏差Δζ，必须进行足够次数的末端位姿测量，得到一个不少于5n个方程的方程组。因此，重复上述步骤(1)～(4)，进行l次末端位姿测量，可得

上式中，基于机械臂DH参数标称值计算Φ_i(i＝1,…,l)时，需要注意到机械臂标称几何参数(即a_n，α_n，d_n，β_n)为常值，而机械臂标称关节位置θ_n依赖于机械臂构型。

所述步骤(6)的具体方法为：

采用最小二乘等方法，计算得到机械臂运动学参数的估计值，具体为：

为了避免步骤(5)中矩阵

出现病态，需要使机械臂末端位姿测量次数l满足条件：6l>>5n，然后采用最小二乘法求解Δζ。于是，有

其中，

为矩阵

的左伪逆。根据机械臂DH参数标称值ζ_n计算得到

可以得到机械臂DH参数的首次估计值

ζ′＝ζ_nom+Δζ

这是一个非线性参数估计问题，因此同样可以采用迭代求解直至Δζ收敛到某一事先给定的阈值范围内。在每一步迭代时，标定矩阵Φ都要根据上一步迭代得到的DH参数估计值ζ′进行更新。同样地，根据l次测量得到的机械臂末端位姿和对应的基于上一步迭代给出的机械臂DH参数估计值进行正运动学计算得到的机械臂末端位姿，可以得到机械臂末端位姿偏差

采用如上所述的机械臂运动学标定流程，可以得到更加准确的机械臂DH参数的估计值。

本发明方法实施例所涉及的试验系统包括一部固定基座七自由度串联机械臂。机械臂安装基板上装有两个圆形测量靶标，机械臂末端安装双目手眼相机。在对DH参数进行标定时，手眼双目相机通过对靶标的测量，得到机械臂末端位姿信息。

图2为机械臂修正DH参数示意图。由图2可见，相对于常规DH参数，机械臂修正DH参数多了一个新的参数β，用于解决传统D-H参数法在相邻关节平行时求解的不足。

在标定试验中，不同的机械臂末端测量位姿对标定结果存在明显影响。在选定机械臂的每个测量构型时，首先将矩阵

进行奇异值分解，然后根据一定准则对该标定点的机械臂构型进行评价。若满足该评价准则，则选用该次测量结果，反之弃用该次测量。在试验中，对机械臂构型的评价准则为：在手眼相机能够清晰看到靶标的前提下，选用矩阵

的最小奇异值作为度量指标，该指标越大，表明采用该机械臂构型的标定结果，就越接近机械臂DH参数真实值。

以上所述，仅为本发明最佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

Claims (3)

1.一种适用于全构型的空间机械臂运动学参数在轨标定方法，其特征在于，包括如下步骤：

(1)机械臂安装基板上安装靶标，对于给定的一组机械臂构型，由机械臂末端安装的手眼双目相机测量得到靶标中心点坐标系相对于手眼坐标系的位姿；

(2)根据靶标和手眼相机的安装矩阵，得出机械臂末端在基座坐标系中的位姿测量值；

(3)基于空间机械臂标称运动学参数和关节角度遥测值，由正运动学计算得到机械臂末端在机械臂基座坐标系的位姿标称值；

(4)由空间机械臂末端位姿的测量值和标称值，得到一组关于机械臂运动学参数的偏差方程；

(5)如果得到l组关于机械臂运动学参数的偏差方程，则进入步骤(6)，否则更换一组机械臂构型，返回步骤(1)；l满足6l≥5n，n为机械臂的自由度；

(6)求解l组偏差方程得到机械臂运动学参数误差，再根据运动学参数误差计算出最后的运动学参数估计值，完成空间机械臂运动学参数标定；

步骤(1)中由机械臂末端安装的手眼双目相机测量得到靶标中心点坐标系相对于手眼坐标系的位姿的具体方法为：设机械臂当前构型为θ＝[θ₁…θ_n]^T，手眼双目相机测量得到靶标中心点坐标系相对于手眼坐标系的位姿由齐次变换矩阵^camT_blank∈R^4×4给出；

步骤(2)中根据靶标和手眼相机的安装矩阵，得出机械臂末端在基座坐标系中的位姿测量值的具体方法为：

机械臂末端在基座坐标系中的标称位姿⁰T_end∈R^4×4为

⁰T_end＝⁰T_blank·(^endT_cam·^camT_blank)^-1

⁰T_end写为如下形式

其中，⁰R_end∈R^3×3为机械臂末端在基座坐标系中的标称姿态矩阵，⁰p_end∈R^3×1为机械臂末端坐标系原点在基座系中的位置坐标；^endT_cam∈R^4×4为手眼相机坐标系相对于机械臂末端系的安装矩阵；

将机械臂末端在基座坐标系中的标称姿态的最小表示记为φ∈R^3×1，由三个角度表示，具体形式为：

根据姿态矩阵⁰R_end，得到对应某种转序的欧拉角表示

ψ；

机械臂末端在基座坐标系中的位姿测量值x_m∈R^6×1为

步骤(3)中采用空间机械臂标称运动学参数和关节角度遥测值，由正运动学计算得到机械臂末端在机械臂基座坐标系的位姿标称值的具体方法为：

令a＝[a₁…a_n]^T，α＝[α₁…α_n]^T，d＝[d₁…d_n]^T，θ＝[θ₁…θ_n]^T，β＝[β₁…β_n]^T表示n自由度机械臂的修正DH参数，其中a、α、d、θ分别表示机械臂关节杆件长度、杆件扭角、相邻关节距离、相邻关节转角，β为两个相邻关节的平行度；

根据机械臂正运动学，机械臂末端在基座系中的位姿标称值x_nom∈R^6×1为：

x_nom＝k(a_nom,α_nom,d_nom,θ_nom,β_nom)

其中，机械臂正运动学函数k(·)给出了从机械臂关节空间到任务空间的映射，a_nom,α_nom,d_nom,θ_nom,β_nom分别表示机械臂修正DH参数a，α，d，θ，β的标称值；

步骤(4)中由空间机械臂末端位姿的测量值和标称值，得到一组关于机械臂运动学参数的偏差方程的具体方法为：

机械臂末端位姿偏差Δx＝x_m-x_nom，采用一阶近似得到

其中，Δa，Δα，Δd，Δθ，Δβ表示机械臂DH参数真实值与标称值的偏差；

表示机械臂正运动学函数k(·)对该DH参数的偏微分，为参数空间与工作空间之间的雅克比变换；

将机械臂修正DH参数写为矢量形式

ζ＝[a^T α^T d^T θ^T β^T]^T

令ζ_true∈R^5n×1、ζ_nom∈R^5n×1分别表示机械臂修正DH参数的真实值与标称值，则机械臂修正DH参数偏差Δζ∈R^5n×1为

Δζ＝ζ_true-ζ_nom

定义机械臂运动学标定矩阵

该标定矩阵由机械臂修正DH参数标称值ζ_nom计算得出；

机械臂末端位姿偏差的矩阵形式为：Δx＝Φ·Δζ。

2.根据权利要求1所述适用于全构型的空间机械臂运动学参数在轨标定方法，其特征在于，求解l组偏差方程获得机械臂运动学参数误差的方法为采用最小二乘法迭代求解。

3.根据权利要求2所述适用于全构型的空间机械臂运动学参数在轨标定方法，其特征在于，最小二乘法迭代求解的终止条件为，两次迭代得到的Δζ之差的绝对值小于设定阈值。

Images