CN110900608B - 基于最优测量构型选择的机器人运动学标定方法 - Google Patents

Abstract

基于最优测量构型选择的机器人运动学标定方法，本发明涉及机器人运动学标定方法。本发明的目的是为了解决现有机器人运动学标定精度差的问题。基于最优测量构型选择的机器人运动学标定方法具体过程为：步骤一、确定用于最优测量构型选择的度量指标；步骤二、建立用于运动学标定的改进型粒子群算法；步骤三、基于改进型粒子群算法对机器人进行最优测量构型的选择。本发明用于机器人运动学标定领域。

Description

基于最优测量构型选择的机器人运动学标定方法

技术领域

本发明涉及机器人运动学标定方法，尤其涉及基于最优测量构型选择的机器人运动学标定方法。

背景技术

微创外科手术机器人技术是一种融合先进机器人技术和微创手术医学的交叉学科研究课题，相对于传统微创外科手术，具有目标定位精确、手术易于实现微创化、可进行主从控制与远程手术等优点，能够大幅提高手术成功率、降低患者的手术痛楚、缩短恢复时间，具有广阔的研究与应用前景。

运动学标定是一种通过辨识运动学参数误差以修正机器人运动学模型来提高机械臂末端绝对位姿精度的方法。鉴于医疗机器人高精度以及精细操作的需求，医疗机器人运动学标定有着十分重要的理论与现实意义。

在现有的技术中，文献名“Amethod for industrial robot calibration”提出了将距离误差模型应用于机器人的标定，提高了机器人的轨迹精度。“一种基于平面精度的机器人标定方法及仿真”(《中国机械工程》，2011,22(17),2039-2042)中提出在机器人的平面运动过程中，由关节驱动器记录平面上各采样点处的关节值并将这些值作为标定数据，避免了使用其他测量工具的复杂测量过程。授权公告号CN102022989B专利，公开了一种基于指数积模型的机器人标定方法，包括步骤：提供工业机器人、独立于机器人的测量仪器、末端立体成像设备及一立体标定块；根据旋量理论与指数积相结合的方式建立机器人的运动学误差模型，利用线性方法直接解得关节旋量误差参数，并对机器人关节的名义值进行了补偿，避免了机器人坐标系与测量坐标系的转换，而且标定过程简单，关节旋转一次测量n(n>3)个标记点的坐标值即可实现标定。授权公告号CN101231749A专利，公开了一种工业机器人的标定方法，通过建立机器人工具坐标系相对于基础坐标系的位姿变换矩阵，再通过该位姿变换矩阵建立标定块上激光跟踪仪的中心相对于基础坐标系的位置坐标矢量，对该位置坐标矢量进行全微分后得到位置误差、结构参数误差与关节变量误差的线性函数，将实验数据带入上述线性关系，解得各个关节变量误差值以及结构参数误差值，进而获得较准确的姿态精度。

在上述的现有技术中，多数针对工业机器人，通过建立合适的误差模型来提高机器人的标定精度，但却均忽略了测量噪声以及测量构型对标定精度的影响。不同的测量姿态，运动学参数误差对其末端误差的影响有很大的不同。在运动学参数误差相同的情况下，某些姿态对应的末端误差较大，也即这些姿态较其他位姿更能反映机械臂的运动学参数误差。因此，通过机械臂的位姿，可以“观测”运动学参数误差。通过算法选择最能观测运动学参数误差的一组姿态用于机器人的运动学标定，可以提高标定的精度。

发明内容

本发明的目的是为了解决现有机器人运动学标定精度差的问题，而提出基于最优测量构型选择的机器人运动学标定方法。

基于最优测量构型选择的机器人运动学标定方法具体过程为：

步骤一、确定用于最优测量构型选择的度量指标；

步骤二、建立用于运动学标定的改进型粒子群算法；

步骤三、基于改进型粒子群算法对机器人进行最优测量构型的选择。

本发明的有益效果为：

本发明通过确定用于最优测量构型选择的度量指标；建立用于运动学标定的改进型粒子群算法；基于改进型粒子群算法对机器人进行最优测量构型的选择，克服现有机器人运动学标定精度差的问题，本发明提供基于最优测量构型选择的机器人运动学标定方法，考虑测量噪声的影响，实现在现有机器人运动学误差模型基础上选择出对标定精度影响最小的最优测量构型，达到提高运动学标定精度的目的。

附图说明

图1是本发明的一种基于最优测量构型选择的微创腹腔手术机器人运动学标定的流程框图；

图2是本发明的机器人的DH框图，X为机械臂基础坐标系X轴，Z为机械臂基础坐标系Z轴，Y₁为坐标系1的Y轴，Z₁为坐标系1的Z轴，Y₂为坐标系2的Y轴，Z₂为坐标系2的Z轴，X₃为坐标系3的X轴，Z₃为坐标系3的Z轴，X₄为坐标系4的X轴，Y₄为坐标系4的Y轴，X₅为坐标系5的X轴，Z₅为坐标系5的Z轴，X₆为坐标系6的X轴，Y₆为坐标系6的Y轴，X₇为坐标系7的X轴，Z₇为坐标系7的Z轴，X₈为坐标系8的X轴，Z₈为坐标系8的Z轴，θ₁为关节1转角，θ₃为关节3转角，θ₅为关节5转角，θ₆为关节6转角，θ₇为关节7转角，d₂为沿着Z₁轴，Z₁与X₂交点到坐标系X₁Y₁Z₁坐标系原点的距离，d₄为沿着Z₃轴，Z₃与X₄交点到坐标系X₃Y₃Z₃坐标系原点的距离，d₆为沿着Z₅轴，Z₅与X₆交点到坐标系X₅Y₅Z₅坐标系原点的距离，d₈为沿着Z₇轴，Z₇与X₈交点到坐标系X₇Y₇Z₇坐标系原点的距离；

图3是改进型粒子群算法的流程框图；

图4为基于激光跟踪仪测量的手术机器人的实验装置图；

图5为运行350次得到整个种群的最优、平均适应度值示意图；

图6为运行1600次得到整个种群的最优、平均适应度值示意图；

图7为基于最优测量构型以及随机构型标定得到的运动学参数误差值示意图；

图8为基于最优测量以及随机构型得到的运动学参数误差的均方根值示意图。

具体实施方式

具体实施方式一：结合图1说明本实施方式，本实施方式一种基于最优测量构型选择的机器人运动学标定方法具体过程为：

步骤一、确定用于最优测量构型选择的度量指标，用以衡量机械臂测量位姿对运动学参数误差的观测程度；

步骤二、建立用于运动学标定的改进型粒子群算法；

步骤三、基于改进型粒子群算法对机器人的整个工作空间里进行最优测量构型的选择。

具体实施方式二：本实施方式与具体实施方式一不同的是，所述步骤一中确定用于最优测量构型选择的度量指标；具体过程为：

考虑串联机器人的运动学误差模型为非线性的，在忽略高阶误差项的条件下，上述运动学误差可转成DH参数误差与工具坐标系位姿误差的线性方程，为解上述方程最常用的方法为最小二乘法。

根据最小二乘估计的F检验理论，可给出各个最小二乘参数估计量构成的误差椭圆，基于该误差椭球的具体方程可得，该误差椭圆方程可以由辨识雅克比矩阵来确定。

具体讲,当采用相异的测量姿态构型来进行运动学标定，构成的误差椭圆就会有差异，若误差椭球越小，各个最小二乘参数估计量的方差就越小，则各个最小二乘估计量就会越来越接近他的真值。换句话讲，当选择相异的测量构型来进行运动学标定，得到的最小二乘估计量的那个精度就会相异。基于上述原因，提出了一种用于衡量测量构型对最小二乘估计量的估计精度的准则。

图2所示，运用修改D-H参数法建立机器人运动学正解模型。

步骤一一、建立线性运动学误差模型；具体过程为：

如图2，根据DH法则建立机构的运动学简图。

根据上述运动简图推导机构的运动学误差模型。

图2所示，运用修改D-H参数法建立机器人运动学正解模型。

根据机器人运动学正解以及微分运动理论，建立线性运动学误差模型，表达式为：

e⁽ⁱ⁾＝J⁽ⁱ⁾·δ

其中，e⁽ⁱ⁾为第i次标定得到的机械臂工具末端位姿误差矩阵，J⁽ⁱ⁾为第i次标定的辨识雅克比矩阵，i＝1,2,…,n；δ为运动学误差参数列向量；

根据激光跟踪仪测量得到的机械臂工具坐标系相对于基础坐标系的位姿矩阵以及机器人控制器中的运动学理论正解程序(通过在机械臂工具上固定一个靶球固定装置，激光跟踪仪可测量得到工具坐标系)，获得机械臂共计n个运动学标定辨识雅克比矩(J＝[(J⁽¹⁾)^T…(J⁽ⁿ⁾)^T]^T)；

测量得到的机械臂工具坐标系的x轴即附图2里的X8，Z轴即附图2力的Z8，Y轴可由右手定则直接得到。

机械臂基础系就指机械臂基础坐标系，其X轴即图2里的X轴，Z轴即图2里的Z轴，其Y轴可由右手定则直接得到。

步骤一二、根据构建的误差模型，获得机械臂的运动学标定辨识雅克比矩阵；

测量得到的机械臂工具坐标系的x轴即附图2里的X8，Z轴即附图2力的Z8，Y轴可由右手定则直接得到。

机械臂基础系就指机械臂基础坐标系，其X轴即图2里的X轴，Z轴即图2里的Z轴，其Y轴可由右手定则直接得到。

根据n个辨识雅克比矩阵，构造出机械臂工具末端位姿误差矩阵：

E＝J·δ

其中，E为机械臂工具末端位姿误差矩阵，E＝[(e⁽¹⁾)^T…(e⁽ⁿ⁾)^T]^T，e⁽ⁿ⁾为第n次标定中得到的机械臂工具末端位姿误差矩阵，T为转置，J＝[(J⁽¹⁾)^T…(J⁽ⁿ⁾)^T]^T称为运动学标定辨识雅克比矩阵；

步骤一三、确定度量指标

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述步骤一三中确定度量指标

具体过程为：

将运动学标定辨识雅克比矩阵J的转置与自身J相乘，得到一个新的对称型标定误差度量矩阵：C＝J*J^T；

依据得到的新的对称型标定误差度量矩阵C，对C取迹运算Trace(J*J^T)；

求取新的对称型标定误差度量矩阵C的各个特征值k_i，并对C的各个特征值k_i进行排序k₁≥k₂…≥k_i，其中i值由线性独立的运动误差参数(根据机器人构型差异，其值会产生改动)决定；

对C的各个特征值k_i取算数平方根，得到C的各个奇异值r_i，并对C的各个奇异值进行排序r₁≥r₂…≥r_i；

根据C的各个奇异值倒数的代数和，确定用于最优测量构型选择的度量指标

若O的奇异值不存在小于0的数值，构造用于离散型粒子群搜索的优化目标函数，即O＝(r₁ ^-1+r₂ ^-1+…r_i ^-1)^-1；

若O的奇异值存在小于0的数值，则应将该奇异值直接删掉，根据剩余的奇异值倒数的代数和来构造用于离散型粒子群搜索的优化目标函数，即O＝(r₁ ^-1+r₂ ^-1+…r_i ^-1)^-1。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述步骤二中建立用于运动学标定的改进型粒子群算法；具体过程为：

搭建如图4所示的实验台，该实验台主要包括：一个自由度为8的医疗机器人，一台API激光跟踪仪，控制系统采用beckhoff的工控机以及采集端子；

得到该机构的各个关节运动范围以及DH参数的理论值；

转动各个关节，利用激光跟踪仪测量工具坐标系上的标记点在基础坐标系下的位置，根据这3个点的位置坐标，建立在该组姿态配置情况下，工具坐标系相对于基础坐标系的坐标变换矩阵。

重复上述的过程160次，可得160个备选姿态组。

根据图3的流程，基于上述的备选姿态组，可得种群的平均适应度、最优适应度值曲线(如图5与图6)以及对应的最优测量构型。

图7所示，为了评估基于最优构型选择运动学标定的精度，首先给定各个运动学参数的理论值以及误差参数值，角度参数为0.01弧度，长度误差参数值为0.006米。

图7所示，基于上述粒子群优化得到用于运动学标定的最优测量构型，并将运动学测量构型用于标定，从而解得各个运动学参数误差的值。

图7所示，随机选择一组测量构型，将其用于标定，亦可获得各个运动学参数误差的实际值。

重复上述过程16次，可得16组误差参数的均方根值。图8所示，基于最优测量构型的标定精度要由于基于随机测量姿态得到的标定精度。

步骤二一、将机器人的各个关节进行等间距的采样，生成一定的备选姿态组N；

步骤二二、对各个备选姿态组分配一个对应的正整数，确定最小的正整数对应的为最优姿态组数量；

步骤二三、初始化给定的粒子群；过程为：

根据最优姿态组数量的最小值n以及机械臂的自由度数量D(机械臂自由度为8)，计算得到每个粒子所代表的列向量的维度n*D；

步骤二四、根据实际运行情况以及各个粒子的列向量维度，对给定的改进型粒子群算法进行初始化，包括：

粒子群的种群数量P，惯性因子线性递减，最大惯性因子0.9、最小惯性因子0.4以及运行次数；

选择度量指标

当做适应度函数；

步骤二五、初始化给定的粒子群的位置矢量以及速度矢量(通常由matlab的round函数实现)；

步骤二六、确定各个粒子的初始最优适应度值

个体最优位置矢量以及初始化整个种群的最优位置矢量

具体过程为：

随机给出L个粒子，给出各个粒子的初始最优适应度值，并将各个粒子的初始最优适应度值设置为当前位置；

根据所述的适应度函数计算出各个粒子的适应度值(适应度值由观测指数来进行计算

按大小顺序进行排序，最大的即为最优适应度值最大的粒子)，选择适应度值最优的粒子对应的位置当做初始的全局最优位置

步骤二七、更新各个粒子的适应度值(根据搜索算法，迭代一次，机器人的关节转角产生修改，从而得到相异的最优适应度值，进而实现了更新)、计算各个粒子位置

以及速度矢量

步骤二八、判断粒子当代的适应度值是否优于粒子最优适应度值，若否，保留原最优适应度值；若是，更新粒子的最优位置向量(粒子的最优位置向量更新通常由粒子群搜索算法的固有机制来更新，通常将适应度值大的那个粒子对应的位置当作粒子当前最优位置)，执行步骤二九；

步骤二九、判断粒子的最优适应度值是否优于种群最优适应度值，若否，保留原粒子的最优适应度值；若是，更新种群的最优位置向量(将种群中适用度值最大的那个粒子对应的位置向量当最种群最优位置向量)，执行步骤二十；

步骤二十、判断是否达到最大的迭代次数，若是，输出粒子最优适应度值以及粒子最优位置向量；若否，另迭代次数i+1，执行步骤二七。

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是，所述步骤二七中各个粒子位置

以及速度矢量

的表达式为：

式中，

为t+1时刻第i个粒子的速度矢量，

为t时刻第i个粒子的速度矢量，ω^(t)为惯性权重因子，c₁为认知因子，

为介于0与1之间的随机数，

为t时刻第i个粒子的初始最优适应度值，P_i ^(t)为t时刻第i个粒子的位置，c₂为学习因子，

为为介于0与1之间的随机数，

为初始的全局最优位置，

为t时刻第i个粒子的位置，

为t+1时刻第i个粒子的位置。

其它步骤及参数与具体实施方式一至四之一相同。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (1)

1.基于最优测量构型选择的机器人运动学标定方法，其特征在于：所述方法具体过程为：

步骤一、确定用于最优测量构型选择的度量指标；

步骤二、建立用于运动学标定的改进型粒子群算法；

步骤三、基于改进型粒子群算法对机器人进行最优测量构型的选择；

所述步骤一中确定用于最优测量构型选择的度量指标；具体过程为：

步骤一一、建立线性运动学误差模型；表达式为：

e⁽ⁱ⁾＝J⁽ⁱ⁾·δ

其中，e⁽ⁱ⁾为第i次标定得到的机械臂工具末端位姿误差矩阵，J⁽ⁱ⁾为第i次标定的辨识雅克比矩阵，i＝1,2,…,n；δ为运动学误差参数列向量；

步骤一二、根据构建的误差模型，获得机械臂的运动学标定辨识雅克比矩阵；

根据n个辨识雅克比矩阵，构造出机械臂工具末端位姿误差矩阵：

E＝J·δ

其中，E为机械臂工具末端位姿误差矩阵，E＝[(e⁽¹⁾)^T…(e⁽ⁿ⁾)^T]^T，e⁽ⁿ⁾为第n次标定中得到的机械臂工具末端位姿误差矩阵，T为转置，J＝[(J⁽¹⁾)^T…(J⁽ⁿ⁾)^T]^T称为运动学标定辨识雅克比矩阵；

步骤一三、确定度量指标

所述步骤一三中确定度量指标

具体过程为：

将运动学标定辨识雅克比矩阵J的转置与自身J相乘，得到一个新的对称型标定误差度量矩阵：C＝J*J^T；

依据得到的新的对称型标定误差度量矩阵C，对C取迹运算Trace(J*J^T)；

求取新的对称型标定误差度量矩阵C的各个特征值k_i，并对C的各个特征值k_i进行排序k₁≥k₂…≥k_i；

对C的各个特征值k_i取算数平方根，得到C的各个奇异值r_i，并对C的各个奇异值进行排序r₁≥r₂…≥r_i；

根据C的各个奇异值倒数的代数和，确定用于最优测量构型选择的度量指标

所述步骤二中建立用于运动学标定的改进型粒子群算法；具体过程为：

步骤二一、将机器人的各个关节进行等间距的采样，生成备选姿态组N；

步骤二二、对各个备选姿态组分配一个对应的正整数，确定最小的正整数对应的为最优姿态组数量；

步骤二三、初始化给定的粒子群；过程为：

根据最优姿态组数量的最小值n以及机械臂的自由度数量D，计算得到每个粒子所代表的列向量的维度n*D；

步骤二四、根据各个粒子的列向量维度，进行初始化，包括：

粒子群的种群数量P，最大惯性因子0.9、最小惯性因子0.4以及运行次数；

选择度量指标

当做适应度函数；

步骤二五、初始化给定的粒子群的位置矢量以及速度矢量；

步骤二六、确定各个粒子的初始最优适应度值

个体最优位置矢量以及初始化整个种群的最优位置矢量

具体过程为：

随机给出L个粒子，给出各个粒子的初始最优适应度值，并将各个粒子的初始最优适应度值设置为当前位置；

根据所述的适应度函数计算出各个粒子的适应度值，选择适应度值最优的粒子对应的位置当做初始的全局最优位置

步骤二七、更新各个粒子的适应度值、计算各个粒子位置P_i ^(t+1)以及速度矢量V_i ^(t+1)；

步骤二八、判断粒子当代的适应度值是否优于粒子最优适应度值，若否，保留原最优适应度值；若是，更新粒子的最优位置向量，执行步骤二九；

步骤二九、判断粒子的最优适应度值是否优于种群最优适应度值，若否，保留原粒子的最优适应度值；若是，更新种群的最优位置向量，执行步骤二十；

步骤二十、判断是否达到最大的迭代次数，若是，输出粒子最优适应度值以及粒子最优位置向量；若否，另迭代次数i+1，执行步骤二七；

所述步骤二七中各个粒子位置P_i ^(t+1)以及速度矢量V_i ^(t+1)的表达式为：

P_i ^(t+1)＝P_i ^(t)+V_i ^(t+1)

式中，V_i ^(t+1)为t+1时刻第i个粒子的速度矢量，V_i ^(t)为t时刻第i个粒子的速度矢量，ω^(t)为惯性权重因子，c₁为认知因子，

为介于0与1之间的随机数，

为t时刻第i个粒子的初始最优适应度值，P_i ^(t)为t时刻第i个粒子的位置，c₂为学习因子，

为为介于0与1之间的随机数，

为初始的全局最优位置，P_i ^(t)为t时刻第i个粒子的位置，P_i ^(t+1)为t+1时刻第i个粒子的位置。

Images