CN110757450B - 一种肩关节康复机器人参数标定方法 - Google Patents

Abstract

本发明公开了一种肩关节康复机器人参数标定方法，获取康复机器人不同关节角度，输入至预先构建的指数积运动学模型，输出机器人末端位姿；将机器人末端位姿输入至预先构建的末端位置误差模型，通过对末端位置全局灵敏度分析获取机器人运动学参数的取舍情况；将保留的运动学参数输入至预先构建的降维指数积运动学误差模型，输出机器人末端位姿误差；将获得的机器人末端位姿误差输入预先构建的面约束下降维指数积运动学误差模型，输出康复机器人的某关节在面约束下的末端位姿误差；对康复机器人的某关节在面约束下的末端位姿误差基于利用线搜索的LM算法的运动学参数标定。优点：简化运算，实现对机器人的运动学参数标定，提高机机器人的运动精度。

Description

一种肩关节康复机器人参数标定方法

技术领域

本发明涉及一种肩关节康复机器人参数标定方法，属于标定技术领域。

背景技术

随着机器人技术的发展，越来越多的行业对机器人精度有了较高的要求，因此，开展机器人的定位精度研究尤为重要。运动学标定是提高机器人精度的关键技术，在机器人空前发展的今天有十分重要的意义。机器人运动学参数的标定方法可分为基于模型的运动学参数标定方法和非模型运动学参数标定方法。基于模型的运动学参数标定方法的主要步骤为：建模、测量、标定和误差补偿。

针对机器人运动学参数的标定工作，国内外的专业人士已经进行了相应的研究工作。能够对各种不同结构的机器人进行建模，但在处理相邻的平行关节建模问题时会使模型参数标定时出现奇异问题。

在串联机器人的实际应用中，机器人通常会受到点、直线、平面等各种类型的约束。机器人的各个运动关节都存在被各种约束条件限制从而影响其工作空间的情况。在机器人运动学参数标定完成后，对其进行误差补偿也是重要研究工作。

发明内容

本发明所要解决的技术问题是克服现有技术的缺陷，提供一种肩关节康复机器人参数标定方法，提高机器人的运动精度。

为解决上述技术问题，本发明提供一种肩关节康复机器人参数标定方法，

获取康复机器人不同关节角度，输入至预先构建的指数积运动学模型，所述预先构建的指数积运动学模型用于输出机器人末端位姿；将机器人末端位姿输入至预先构建的末端位置误差模型，通过对末端位置全局灵敏度分析获取机器人运动学参数的取舍情况；将保留的运动学参数输入至预先构建的降维指数积运动学误差模型，输出机器人末端位姿误差；将获得的机器人末端位姿误差输入至预先构建的面约束下降维指数积运动学误差模型，输出康复机器人的某关节在面约束下的末端位姿误差；对康复机器人的某关节在面约束下的末端位姿误差基于利用线搜索的LM算法的运动学参数标定。

进一步的，所述指数积运动学模型的构建过程为：

建立肩关节康复机器人的各连杆坐标系；根据所建立的各连杆坐标系，确定机器人末端初始位姿g_st(0)，各关节旋转轴方向的单位矢量w_i以及其反对称矩阵

并引入各旋转关节的运动旋量矩阵

以及其旋量坐标ξ_i；根据指数映射关系，利用Rodriguez公式计算各旋转关节的旋量矩阵指数

并确定各旋转关节的刚体位姿变换矩阵

将各旋转关节的刚体位姿变换矩阵

进行组合，得到康复机器人运动学的指数积公式。

指数积运动学模型不但克服了传统D-H运动学模型相对坐标系局部参数产生奇异性的缺点，还有效解决了传统D-H运动学模型不满足完整性，连续性和冗余性的问题。并且指数积运动学模型具有建模过程简单，结构性好，几何意义清晰等优点。

进一步的，所述末端位置误差模型通过所述康复机器人运动学的指数积公式，推导得到，末端位置误差模型的表达式为：

其中P^E(1),P^E(2),P^E(3)分别代表末端位置x,y,z轴的误差。

末端位姿误差是由末端位置误差和末端姿态误差共同组成，为分析末端位置误差对末端姿态误差的影响程度，建立了末端位置误差模型。在不同关节角度的数据集下，所对应的机器人运动学指数积公式也会不同，而所构建的位置误差模型只需要指数积公式中位置向量信息，建模过程简单，有效避免了传统使用测量工具的繁琐过程。

进一步的，所述降维指数积运动学误差模型的构建过程为：

根据康复机器人运动学的指数积公式，将机器人末端初始位姿g_st(0)表示为常旋量

的指数形式，得到康复机器人的理论运动学模型的表达形式；

根据康复机器人的理论运动学模型的表达形式，得到康复机器人理论运动学误差模型，分析机器人末端位姿误差的组成部分，得到康复机器人理论运动学误差模型的简化模型；

根据指数积运动学模型构建过程中所得的关节旋量坐标理论值与实际值之间的伴随变换关系，建立肩关节康复机器人的实际运动学模型；

根据所求得的实际运动学模型，对其取微分建立康复机器人的运动学误差模型；

将sobol算法引入所述末端位置误差模型中对运动学参数进行全局灵敏度分析，求得对机器人末端误差影响情况；

根据所得的误差影响情况，将影响程度较小的运动学参数剔除，求解剔除相关的运动学参数后的各旋转关节的旋量坐标，建立相对于工具坐标系的降维指数积运动学误差模型：

其中，y为在工具坐标系下机器人的末端位姿误差向量，J为在工具坐标系下机器人末端位姿误差与运动学参数误差的映射矩阵，

为机器人运动学参数误差向量。

建立运动学误差模型是参数辨识的基础，传统建模方法是通过分析全部运动学参数构建误差模型，实际情况中部分参数对于末端位姿误差影响不大，所以传统方法在辨识过程中会增加运算量，影响辨识效率。而降维指数积运动学误差模型可减少模型中需要辨识的参数个数，减少迭代次数，简化计算，提高机器人的运动精度。

进一步的，所述面约束下降维指数积运动学误差模型的构建过程为：

根据平面约束模型的建模原理，建立机器人基于平面约束的降维误差模型；

根据所建立的基于平面约束的降维误差模型，在机器人关节m处受到平面约束时，建立从基座到关节m的约束运动学误差模型；

根据所建立的基于平面约束的降维误差模型，在机器人关节m处受到平面约束时，建立从关节m到末端的约束运动学误差模型；

综合从基座到关节m的约束运动学误差模型和从关节m到末端的约束运动学误差模型，建立从基座到末端的约束运动学误差模型：

⁰y_n＝⁰J_n·⁰x_n

其中,⁰y_n代表在平面约束下机器人末端的位姿误差，⁰J_n代表从基座到末端的雅可比矩阵，⁰x_n代表机器人的运动学参数。

机器人的各个运动关节都存在被点约束，直线约束或平面约束条件限制而影响其工作空间的情况。为了提高机器人的标定精度，可以通过减少工作空间为代价换取定位精度的提高，所以在降维指数积运动学误差模型中增加平面约束从而构建面约束下降维指数积运动学模型。其中平面约束模型的建立可以不借助昂贵设备，并且可避免测量坐标系与机器人基坐标系之间的转换运算。

进一步的，所述对康复机器人的某关节在面约束下的末端位姿误差基于利用线搜索的LM算法的运动学参数标定的过程为：

使用激光跟踪仪来测量机器人末端位姿的实际值，根据所建立的指数积运动学模型来计算对应末端位姿的理论值，将两者之差作为末端位姿误差；

采用线搜索的Levenberg-Marquardt方法求解机器人运动学参数误差向量⁰x_n：

其中，

为在参考坐标系下机器人的末端位姿误差向量，

为在参考坐标系下机器人末端位姿误差与运动学参数误差的映射矩阵，

代表矩阵

的转置，对于n自由度机器人，I代表6(n+1)阶单位矩阵；

根据LM算法进行有限次迭代，得到满足精度要求的运动学参数，完成对机器人运动学参数的标定。

在机器人运动学参数标定完成后，对其进行误差补偿也是重要研究工作。传统的最小二乘法虽然迭代过程简单，但计算量相对较大。线搜索的LM算法是对最小二乘法改进的算法，其结合了梯度下降和Gauss-Newton法的优点，能有效处理冗余参数问题，收敛速度快，鲁棒性强。

本发明所达到的有益效果：

本发明可实现减少在面约束下机器人运动学误差模型中需要辨识的参数个数，减少迭代次数，简化运算，实现对机器人的运动学参数标定，提高机机器人的运动精度。

附图说明

图1是肩关节康复机器人模型图；

图2是肩关节康复机器人各关节的建系情况图；

图3是肩关节康复机器人的运动学参数图；

图4是末端位置误差模型中运动学参数的全局灵敏度分析图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

本发明通过建立基于指数积公式的三自由度肩关节康复机器人的运动学模型，采用sobol算法在末端位置误差模型中对运动学参数进行全局灵敏度分析，通过分析后的结果建立指数积运动学误差模型，并推导机器人的某关节受到平面约束的情况下降维指数积运动学误差模型的建立。输入多组关节角度，通过指数积运动学模型求解名义位姿数据，通过激光跟踪仪获取实际位姿数据，两者之差可计算出补偿前的末端位姿误差，并使用线搜索的LM算法求解运动学参数误差，之后通过补偿名义运动学参数求解补偿后的机械臂末端位姿，最后通过比较参数补偿前后的末端位姿误差来验证是否提高了运动学精度，以完成标定。通过这套流程能够对本肩关节康复机器人进行标定，包括以下步骤：

S1、获取康复机器人不同关节角度，输入至预先构建的指数积运动学模型，所述预先构建的指数积运动学模型用于输出机器人末端位姿；

S1-1、根据指数积运动学模型的建系规则，从参考坐标系开始建立肩关节康复机器人的各连杆坐标系；

考虑刚体在空间中运动的位姿表示，基于机器人基座建立参考坐标系，基于机器人末端执行器建立工具坐标系，刚体相对于参考坐标系的位姿可由下式描述：

SE(3)＝{(R,t):R∈SO(3),t∈R³}

其中，SE(3)为特殊欧式群；R为姿态旋转矩阵；t为位置向量；SO(3)为特殊正交群；

图1为肩关节康复机器人模型图，其采用了串联的三个正交转动关节J1,J2和J3，分别位于组件1、组件2和组件3的位置。图2为肩关节康复机器人各关节的建系情况图，其中L1,L2,L3和L4代表连杆长度，w1,w2,w3为各旋转关节的旋转轴，在此建系情况下可求得各关节的刚体运动变换矩阵；图3为肩关节康复机器人的运动学参数图，通过建立指数积运动学模型，代入不同关节输入角度求解机器人末端位姿的理论值。

S1-2、根据所建立的各连杆坐标系，确定机器人末端初始位姿g_st(0)，各关节旋转轴方向的单位矢量w_i以及其反对称矩阵

具体实施步骤如下：

(a)求解机器人末端初始位姿g_st(0)

机器人末端初始位姿g_st(0)是由3*3初始姿态矩阵R_st(0)和3*1初始位置向量P_st(0)所组成，其中初始姿态矩阵R_st(0)是当θ＝90°时绕x轴旋转的旋转矩阵。则可得：

其中，各连杆长度为l₁＝308mm，l₂＝232mm，l₃＝330mm，l₄＝308mm。

(b)求解各关节旋转轴方向的单位矢量w_i以及其反对称矩阵

取w＝(w₁,w₂,w₃)^T∈R³为旋转轴方向的单位矢量，根据图2的机器人各关节建系情况图可知，旋转关节1的w₁方向沿z轴正方向，旋转关节2的w₂方向沿x轴反方向，旋转关节3的w₃方向沿y轴正方向。则可得：

旋转轴方向单位矢量w的反对称矩阵

可通过两者之间的转换关系求得，其转换关系如下所示：

S1-3、根据连杆坐标系的坐标原点为旋转轴上的一点，引入各旋转关节的运动旋量矩阵

以及其旋量坐标ξ_i；

设r为旋转轴上的一点，则各旋转关节的运动旋量矩阵

是由3*3旋转轴方向的单位矢量的反对称矩阵

和3*1移动方向的单位矢量v组成，而各旋转关节的运动旋量矩阵的旋量坐标ξ_i是由3*1旋转轴方向的单位矢量w和3*1移动方向的单位矢量v组成，其形式为：

其中v＝r×w，r表示旋转轴上任意一点的坐标向量。

S1-4、根据指数映射关系，利用Rodriguez公式计算各旋转关节的旋量矩阵指数

规定I为3*3单位矩阵，利用Rodriguez公式来计算矩阵指数可得：

S1-5、根据Chasles定理，确定各旋转关节的刚体位姿变换矩阵

任何刚体运动都可以通过螺旋运动即通过绕某轴的转动与沿该轴移动的复合运动实现，也就是说刚体运动与螺旋运动是等价的。因此刚体运动变换可用旋量指数积形式表示为：

S1-6、将各旋转关节的位姿变换矩阵进行组合，推导机器人运动学的指数积公式，即机器人运动学模型；

肩关节康复机器人由3个旋转关节组成，由S1-5求得的关节1,2,3的刚体运动变换矩阵为

将这3个刚体运动变换矩阵组合并右乘机器人初始末端位姿g_st(0)，可得机器人运动学模型：

至此完成肩关节康复机器人运动学模型的推导。

S2、将机器人末端位姿输入至预先构建的末端位置误差模型，通过末端位置全局灵敏度分析获取机器人运动学参数的取舍情况，将保留的运动学参数输入至预先构建的降维指数积运动学误差模型，输出机器人末端位姿误差；

S2-1、根据所建立的肩关节康复机器人运动学模型，将机器人末端初始位姿g_st(0)表示为常旋量

的指数形式，从而得到机器人的理论运动学模型的表达形式；

根据S1所求得的肩关节康复机器人运动学模型的指数积公式为：

其中，g_st(0)可以写成常旋量

的指数形式，即

则肩关节康复机器人运动学模型也可以表示为：

S2-2、根据肩关节康复机器人的理论运动学模型的表达形式，分析机器人末端位姿误差的组成部分；

对S2-1中求得的肩关节康复机器人的理论运动学模型取微分，可以得到其运动学误差模型，可表示为：

其中，ξ＝[ξ₁ ξ₂ ξ₃]∈R^6×3；θ＝[θ₁ θ₂ θ₃]^T∈R³。

由肩关节康复机器人运动学误差模型可知，肩关节康复机器人的末端位姿误差是由关节旋量误差δξ，关节零位误差δθ和初始位姿变换旋量误差δξ_st造成的。由于关节零位误差δθ可以归结为关节旋量误差δξ，不需要单独辨识。因此，肩关节康复机器人在运动学参数标定过程中仅需识别和补偿关节旋量误差δξ和初始位姿变换旋量误差δξ_st。肩关节康复机器人运动学误差模型可简化为：

S2-3、根据关节旋量坐标理论值与实际值之间的伴随变换关系，建立肩关节康复机器人的实际运动学模型；

关节旋量坐标ξ的理论值和实际值都应满足相应的关节约束条件，且第i关节旋量坐标的理论值

和实际值

之间的关系可用固定的刚体变换关系

来表征，则它们之间的关系可以用旋量的伴随变换表示：

其中，

R_i为旋转矩阵；b_i为平移矢量。

将关节旋量坐标的理论值

和实际值

的关系表达式展开，可得：

旋转关节应满足的关节约束条件为：

||w||＝1,w^Tv＝0

易知，旋转关节的旋量坐标经伴随变换之后仍满足关节约束条件要求，即

根据关节旋量坐标理论值与实际值之间的伴随变换关系和恒等式

可以建立肩关节康复机器人的实际运动学模型为：

S2-4、根据所求得的实际运动学模型，对其取微分建立肩关节康复机器人的运动学误差模型；

将S2-3所求得的肩关节康复机器人的实际运动学模型代入S2-2中所求得的肩关节康复机器人运动学误差模型表达形式，可得：

其中，∨为将4*4矩阵映射为6*1的向量的算子。由恒等式

可得：

指数映射关于旋量微分的显性表达式：

则：

其中：

则肩关节康复机器人运动学误差模型可化简为：

S2-5、根据所建立的机器人运动学模型，推导末端位置误差模型，并引入sobol算法在此误差模型中对运动学参数进行全局灵敏度分析，求得对机器人末端误差影响情况；

根据S1所建立的肩关节机器人运动学模型，可推导末端位置误差模型，具体实施步骤如下：

(a)建立末端位置误差模型

机器人末端位置误差P^E是末端实际位置P^A和末端理论位置P^N之差，则可定义末端位置误差模型为

其中P^E(1),P^E(2),P^E(3)分别代表末端位置x,y,z轴的误差。

(b)末端位置全局灵敏度分析

将末端位置误差模型代入sobol算法中，计算在末端位置误差模型中机器人运动学参数的全局灵敏度。24个运动学参数w_i1,w_i2,w_i3,v_i1,v_i2.v_i3可以被视为是均匀随机分布的相互独立的变量。根据实际情况估计24个运动学参数的误差范围，分别为：

Δw_i1∈(-1.5,1.5)，Δw_i2∈(-1.5,1.5)，Δw_i3∈(-1.5,1.5)

Δv_i1∈(-200,200)，Δv_i2∈(-200,200)，Δv_i3∈(-200,200)

图4为末端位置误差模型中运动学参数的全局灵敏度分析图，可知其中w₁₂,w₃₂,v₃₁这3个参数的数值较大，说明这些参数影响末端位置的程度较大。数值较小参数分别为v₂₃,w₃₁,v₃₃,w_st3,v_st1,v_st2，这些参数在建立基于位置灵敏度的降维误差模型中需要被剔除。

S2-6、根据所得的误差影响情况，将影响程度较小的运动学参数剔除，求解剔除相关参数后的各旋转关节的旋量坐标，建立相对于工具坐标系的机器人运动学误差模型:

其中，y为在工具坐标系下机器人的末端位姿误差向量，J为在工具坐标系下机器人末端位姿误差与运动学参数误差的映射矩阵，

为机器人运动学参数误差向量。

根据S2-5中使用sobol算法对末端位置进行全局灵敏度分析可知：参数v₂₃,w₃₁,v₃₃,w_st3,v_st1,v_st2在建立基于位置灵敏度的降维误差模型中需要被剔除。所以需要取各关节旋量坐标为:

u₁＝[w₁₁ w₁₂ w₁₃ v₁₁ v₁₂ v₁₃]^T；u₂＝[w₂₁ w₂₂ w₂₃ v₂₁ v₂₂ 0]^T；

u₃＝[0 w₃₂ w₃₃ v₃₁ v₃₂ 0]^T；ξ_st＝[w_st1 w_st2 0 0 0 v_st3]^T。

可整理为：

其中：

J＝[J₁ J₂ J₃ J₄]∈R^6×24；

S3、将获得的机器人末端位姿误差输入至预先构建的面约束下降维指数积运动学误差模型，输出机器人的某关节在面约束下的末端位姿误差；

S3-1、根据平面约束模型的建模原理，建立机器人基于平面约束的降维误差模型；

平面约束模型的建模原理是利用机器人末端在平面上采取测量点时，测量点理论上处于同一平面，而实际测量过程中，测量点所拟合而成的平面与理论平面间会存在偏差。利用这个性质便可建立其误差模型。

平面的一般数学表达式为：

AX+BY+CZ+D＝0

其中，A,B,C,D为平面系数。

由于理论平面和拟合平面之间存在误差，机器人末端的实际位置可表达为：

其中，

代表末端实际位置，

代表末端理论位置，J_x,J_y,J_z代表雅可比矩阵在x,y,z三个方向上的分量。

由于拟合平面与理论平面的误差，导致平面系数也存在误差ΔA,ΔB,ΔC,ΔD,因此将机器人末端的实际位置代入平面数学表达式，将其进一步扩展为：

将上式中的各项展开，并剔除高阶项后，简化为下式：

将上式展开为矩阵形式，如式所示：

其中，矩阵AJ_x+BJ_y+CJ_z，平面系数A,B,C,D都是已知的参数，末端理论位置

能够通过理想运动学参数求解而得，运动学参数误差x＝[Δθ^T,Δα^T,Δd^T,Δa^T]^T以及平面系数误差ΔA,ΔB,ΔC,ΔD是需要求解的参数。

然而，单一平面的约束是无法完全求解出运动学参数误差的，至少需要三个平面约束才能够完全辨识，因此，设定一个立方体模型，取三个互相垂直的平面作为约束平面。通过3个平面的总约束，建立机器人基于平面约束的参数误差辨识模型，即：

y＝J·x

其中，y＝[e₁,e₂,e₃]^T，J＝[J₁,J₂,J₃]^T，x＝[x,ΔA_i,ΔB_i,ΔC_i,ΔD_i]^T(i＝1,2,3)，e₁,e₂,e₃代表在三个平面内的末端位置误差，J代表雅可比矩阵，x代表机器人运动学参数的误差向量，ΔA_i,ΔB_i,ΔC_i,ΔD_i代表三个平面的平面系数。

S3-2、根据所建立的约束降维误差模型，在机器人关节m处受到平面约束时，建立从基座到关节m的约束运动学误差模型；

基于约束关节m的误差模型建模方法则是先将关节m作为机器人的末端，选择平面约束建立约束误差模型。按照S2中误差模型建立方法，建立从基座到约束关节m的误差模型：

⁰y_m＝⁰J_m·⁰x_m

其中，⁰y_m代表在平面约束下关节m作为末端的末端位姿误差，⁰J_m代表从基座到关节m的雅可比矩阵，⁰x_m代表前m个关节的运动学参数。

S3-3、根据所建立的约束降维误差模型，在机器人关节m处受到平面约束时，建立从关节m到末端的约束运动学误差模型；

由于剩下的关节并未受到约束，所以可以直接按照S2中误差模型的建立方法建立从基座到末端的误差模型，其形式为：

^my_n＝^mJ_n·^mx_n

其中，^my_n代表在平面约束下从关节m到末端的末端位姿误差，^mJ_n代表从关节m到末端的雅可比矩阵，^mx_n代表n-m个关节的运动学参数。

S3-4、综合从基座到关节m的约束运动学误差模型和从关节m到末端的约束运动学误差模型，建立从基座到末端的约束运动学误差模型。

综合建立的两块模型，完成从机器人基座到末端的误差模型的建立：

⁰y_n＝⁰J_n·⁰x_n

其中,⁰y_n代表在平面约束下机器人末端的位姿误差，⁰J_n代表从基座到末端的雅可比矩阵，⁰x_n代表机器人的运动学参数。

S4、对机器人某关节在面约束下的末端位姿误差基于利用线搜索的LM算法的运动学参数标定。

S4-1、使用激光跟踪仪来测量机器人末端位姿的实际值，根据所建立的指数积运动学模型来计算对应末端位姿的理论值，将两者之差作为末端位姿误差,此末端位姿误差是由3×1维的末端位置误差向量和3×1维的末端姿态误差向量所共同组成，所以末端位置误差和末端姿态误差都会对末端位姿误差产生影响，而构建的末端位置误差模型是用于剔除末端位置误差对末端位姿误差影响较小的运动学参数；

实际应用中通常使用的是机器人工具坐标系相对于参考坐标系的指数积运动学误差模型，因此使用下式进行转换：

其中，Re是机器人工具坐标系相对于基座坐标系的姿态变换矩阵。

S4-2、为保证方程的可求解性，选取足够多的末端姿态数据，采用线搜索的Levenberg-Marquardt方法求解机器人运动学参数误差向量⁰x_n：

其中，

为在参考坐标系下机器人的末端位姿误差向量，

为在参考坐标系下机器人末端位姿误差与运动学参数误差的映射矩阵；

对于n自由度的串联机器人，使用运动学参数建立的运动学模型的参数数量为6(n+1)。为了保证方程的可求解性，所使用的末端位姿数据数量m应不少于6(n+1)/6。根据Y＝M·⁰x_n，在末端数据下令：

将上式变化为

其中

是一个6m×1的向量，

是一个6m×6(n+1)的矩阵。肩关节康复机器人的参数个数为24个，则末端位姿数据数量m至少为4组。

S4-3、根据LM算法进行有限次迭代，得到满足精度要求的运动学参数。

本发明建立指数积运动学模型和运动学误差模型的前提下，选取线搜索的LM算法来补偿运动学参数误差，提高末端运动精度。具体实施步骤如下：

(a)令k＝0,采集m组末端位姿数据以保证求解的可靠性，计算出当前位姿误差

以及运动学参数误差矩阵

(b)根据位姿误差

以及

计算当前的运动学参数误差向量：

其中

(c)取ε＝10^-4，若μ_k≤ε，则停止迭代，得出运动学参数误差向量，结束整体算法；否则进入(d)；

(d)根据(b)得到的⁰x_n ^(k)，得到迭代一次后修正的运动学参数，根据迭代一次后的运动学参数，计算此时理想状态下的位姿数据以及此时的位姿误差，即

取η＝0.5，若

则令a_k+1＝a_k+⁰x_n ^(k)(即进行一次迭代后的运动学参数)，否则寻找满足Armijo线搜索准则的α_k，使a_k+1＝a_k+α_k ⁰x_n ^(k)，令k＝k+1，计算此时的理想位姿数据

进入(b)；

这套算法通过有限次的迭代，保证

完成对机器人运动学参数的标定。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (5)

1.一种肩关节康复机器人参数标定方法，其特征在于，

获取康复机器人不同关节角度，输入至预先构建的指数积运动学模型，所述预先构建的指数积运动学模型用于输出机器人末端位姿；将机器人末端位姿输入至预先构建的末端位置误差模型，通过对末端位置全局灵敏度分析获取机器人运动学参数的取舍情况；将保留的运动学参数输入至预先构建的降维指数积运动学误差模型，输出机器人末端位姿误差；将获得的机器人末端位姿误差输入至预先构建的面约束下降维指数积运动学误差模型，输出康复机器人的某关节在面约束下的末端位姿误差；对康复机器人的某关节在面约束下的末端位姿误差基于利用线搜索的LM算法进行运动学参数标定；

所述降维指数积运动学误差模型的构建过程为：

根据康复机器人运动学的指数积公式，将机器人末端初始位姿g_st(0)表示为常旋量

的指数形式，得到康复机器人的理论运动学模型的表达形式；

根据康复机器人的理论运动学模型的表达形式，得到康复机器人理论运动学误差模型，分析机器人末端位姿误差的组成部分，得到康复机器人理论运动学误差模型的简化模型；

根据指数积运动学模型构建过程中所得的关节旋量坐标理论值与实际值之间的伴随变换关系，建立肩关节康复机器人的实际运动学模型；

根据所求得的实际运动学模型，对其取微分建立康复机器人的运动学误差模型；

将sobol算法引入所述末端位置误差模型中对运动学参数进行全局灵敏度分析，求得对机器人末端误差影响情况；

根据所得的误差影响情况，将影响程度较小的运动学参数剔除，求解剔除相关的运动学参数后的各旋转关节的旋量坐标，建立相对于工具坐标系的降维指数积运动学误差模型：

其中，y为在工具坐标系下机器人的末端位姿误差向量，J为在工具坐标系下机器人末端位姿误差与运动学参数误差的映射矩阵，

为机器人运动学参数误差向量。

2.根据权利要求1所述的肩关节康复机器人参数标定方法，其特征在于，所述指数积运动学模型的构建过程为：

建立肩关节康复机器人的各连杆坐标系；根据所建立的各连杆坐标系，确定机器人末端初始位姿g_st(0)，各关节旋转轴方向的单位矢量w_i以及其反对称矩阵

并引入各旋转关节的运动旋量矩阵

以及其旋量坐标ξ_i；根据指数映射关系，利用Rodriguez公式计算各旋转关节的旋量矩阵指数

并确定各旋转关节的刚体位姿变换矩阵

将各旋转关节的刚体位姿变换矩阵

进行组合，得到康复机器人运动学的指数积公式。

3.根据权利要求1所述的肩关节康复机器人参数标定方法，其特征在于，所述末端位置误差模型通过所述康复机器人运动学的指数积公式，推导得到，末端位置误差模型的表达式为：

其中P^E(1),P^E(2),P^E(3)分别代表末端位置x,y,z轴的误差。

4.根据权利要求1所述的肩关节康复机器人参数标定方法，其特征在于，所述面约束下降维指数积运动学误差模型的构建过程为：

根据平面约束模型的建模原理，建立机器人基于平面约束的降维误差模型；

根据所建立的基于平面约束的降维误差模型，在机器人关节m处受到平面约束时，建立从基座到关节m的约束运动学误差模型；

根据所建立的基于平面约束的降维误差模型，在机器人关节m处受到平面约束时，建立从关节m到末端的约束运动学误差模型；

综合从基座到关节m的约束运动学误差模型和从关节m到末端的约束运动学误差模型，建立从基座到末端的约束运动学误差模型：

⁰y_n＝⁰J_n·⁰x_n

其中,⁰y_n代表在平面约束下机器人末端的位姿误差，⁰J_n代表从基座到末端的雅可比矩阵，⁰x_n代表机器人的运动学参数。

5.根据权利要求1所述的肩关节康复机器人参数标定方法，其特征在于，所述对康复机器人的某关节在面约束下的末端位姿误差基于利用线搜索的LM算法进行运动学参数标定的过程为：

使用激光跟踪仪来测量机器人末端位姿的实际值，根据所建立的指数积运动学模型来计算对应末端位姿的理论值，将两者之差作为末端位姿误差；

采用线搜索的Levenberg-Marquardt方法求解机器人运动学参数误差向量⁰x_n：

其中，

为在参考坐标系下机器人的末端位姿误差向量，

为在参考坐标系下机器人末端位姿误差与运动学参数误差的映射矩阵，

代表矩阵

的转置，对于n自由度机器人，I代表6(n+1)阶单位矩阵；

根据LM算法进行有限次迭代，得到满足精度要求的运动学参数，完成对机器人运动学参数的标定。

Images