CN111195915A - 一种基于遗传算法的机械臂绝对定位精度标定方法及系统 - Google Patents

Abstract

一种基于遗传算法的机械臂绝对定位精度标定方法，包括：步骤1)利用理论DH参数建立机械臂运动学参数模型，并得到理论终端坐标；步骤2)获取机械臂终端实际坐标，以及对应关节的角度；步骤3)在理论DH参数上加上一个微小误差，并建立含有误差的机械臂终端坐标；步骤4)对所有采集到的点位计算

并求和，其中，P_err为含有误差的机械臂终端坐标，P_real为机械臂终端实际坐标；步骤5)通过多阶段遗传算法求得误差参数方程

极小时的微小误差以作为机构参数修正量，然后加上理论DH参数得到最优机构参数该方法采用遗传算法对机械臂绝对定位精度进行分段标定，通过在不同阶段中采用不同的交叉、变异策略使得遗传算法标定结果的精确度更高。

Description

一种基于遗传算法的机械臂绝对定位精度标定方法及系统

技术领域

本发明涉及到数值优化以及误差分析领域，具体涉及到一种基于遗传算法的机械臂绝对定位精度标定方法及系统。

背景技术

绝对定位精度表示机械臂到达某一给定位置的能力。对机械臂绝对精度标定一直是外科手术机器人领域长期以来的研究热点，随着达芬奇手术机器人的出现，外科手术机器人在我国的普及率越来越高。然而，如何提高手术机器人的精度成为了制约手术机器人领域发展的一个大问题。由于传统的工业机械手或协作式机械手对绝对定位精度要求不是很高，因此在机械臂生产制造之初，多数生产厂家并没有对其绝对定位精度进行限制，再加上不可避免的装配误差以及后期使用过程中的机械磨损，必然会对其绝对定位精度造成影响，进而达不到外科手术的要求。为了解决这一问题，就要对机械臂进行动力学建模，采用数值优化的方式对其进行标定，从而使其绝对定位误差降至外科手术要求的范围内。

发明内容

本发明提出了一种基于遗传算法和NDI Opototrak的六自由度机械臂绝对定位精度标定方法及系统，该方法采用遗传算法对机械臂绝对定位精度进行分段标定，通过在不同阶段中采用不同的交叉、变异策略使得遗传算法标定结果的精确度更高。

本发明提供的技术方案如下：

本发明提供了一种基于遗传算法的机械臂绝对定位精度标定方法，包括：步骤1)利用理论DH参数建立机械臂运动学参数模型，并得到理论终端坐标；步骤2)获取机械臂终端实际坐标，以及对应关节的角度；步骤3)在理论DH参数上加上一个微小误差，并建立含有误差的机械臂终端坐标；步骤4)对所有采集到的点位计算

并求和，其中，P_err为含有误差的机械臂终端坐标，P_real为机械臂终端实际坐标；步骤5)通过多阶段遗传算法求得误差参数方程

极小时的微小误差以作为机构参数修正量，然后加上理论DH参数得到最优机构参数。

优选的，在上述机械臂绝对定位精度标定方法中，在步骤1)中，包括：在对机械臂建模之前必须预先规定好各个关节的坐标系，模型建立完成后，进行可视化仿真。

优选的，在上述机械臂绝对定位精度标定方法中，在步骤1)中，利用前向运动学，得到理论终端坐标，其中前向运动学公式主要通过机械臂各关节间的坐标变换求得，由坐标系n到坐标系n+1的变换矩阵由下式所示：

用S表示sin函数，C表示cos函数，得到下式：

进而得到机械臂的前向运动学公式：

¹T₆＝A_lA₂A₃A₄A₅A₆

同时，¹T₆具有如下形式：

其中3×3的矩阵

代表从基坐标系到终端执行器坐标系的旋转矩阵，向量[P_x P_y P_z]^T表示终端执行器在基坐标系下的坐标。

优选的，在上述机械臂绝对定位精度标定方法中，在步骤2)中，使用控制器控制机械臂运行到几个指定位置，使用三坐标测量仪测量此时的机械臂终端实际坐标，同时记录控制器中机械臂各个关节的角度。

优选的，在上述机械臂绝对定位精度标定方法中，在步骤3)中，在获取到实际坐标和对应到关节角之后，创建24个微小误差参数，需要通过在24个机械臂运动学参数上加上一个微小误差，并使用前向运动学公式建立含有误差的机械臂终端坐标的方程；假设某六自由度机械臂的参数误差如下所示：

Δθ＝(Δθ₁，Δθ₂，Δθ₃，Δθ₄，Δθ₅，Δθ₆)

Δα＝(Δα₁，Δα₂，Δα₃，Δα₄，Δα₅，Δα₆)

Δd＝(Δd₁，Δd₂，Δd₃，Δd₄，Δd₅，Δd₆)

Δa＝(Δa₁，Δa₂，Δa₃，Δa₄，Δa₅，Δa₆)，

结合机械臂前向运动学公式，可以求出机械臂终端坐标系相对于基坐标系的旋转变换矩阵，进而可以求得含有误差的机械臂终端坐标P_err，

P_err＝[P_errx P_erry P_errz]^T＝f(Δθ，Δα，Δd，Δa)。

本发明还提供了一种基于遗传算法的机械臂绝对定位精度标定系统，用于实现权利要求上述基于遗传算法的机械臂绝对定位精度标定方法，包括机械臂绝对精度标定数学模型和多阶段遗传算法单元，其中，所述机械臂绝对精度标定数学模型，用于构建机械臂参数误差模型；多阶段遗传算法单元，用于对所述机械臂的参数误差模型进行优化，根据不同阶段的优化结果，动态调整遗传算法中的选择、交叉算子，以及精英种群比例、变异比率。

本发明的有益效果是：

1.)相较于传统的莱文贝格-马夸特算法，本发明采用的遗传算法不依赖于人为设置的学习率等参数，其寻优能力更强，搜索范围更大，更不容易陷入局部极值；

2.)相较于一般的遗传算法，本发明方法有效地避免了遗传算法容易出现的收敛慢、出现超级个体等问题。采用多阶段标定的策略，在每个阶段采用不同的交叉、变异、选择算子，有效地避免了传统遗传算法容易出现的弊端。同时，在不同的阶段，设置不同的搜索上下限，逐步的缩小搜索空间，这样既可以避免算法陷入局部极值，又可以逐步提高精度。

下面结合附图，通过实施例子对本发明进一步说明如下：

附图说明

图1为本发明的基于遗传算法的机械臂绝对定位精度标定方法的流程图；

图2为机械臂各关节坐标系图；

图3为六自由度机械臂仿真建模图；

图4是传统遗传算法的迭代图；

图5是改进搜索策略后的迭代图；

图6是二次搜索迭代图；以及

图7是三次搜索迭代图。

具体实施方式

由于绝对定位精度误差主要由装配误差以及机械磨损造成，即机械臂的实际运动学参数偏离控制器中的理论运动学参数，从而导致机械臂末端工具坐标的实际值和控制器中的示值有较大差异。因此，校正机械臂绝对定位精度需要建立机械臂关节角度和终端坐标的关系式，同时，需要求取机械臂终端实际坐标和含有误差的理论坐标之间的差值，即误差参数模型，通过使用优化算法进行参数辨识即可完成标定。六自由度机械臂具有第一到第六关节。

随着控制技术的发展，越来越多的机械臂被应用在临床医学、精密加工等领域，这也就对机械臂的绝对定位精度要求越来越高，本发明提出的基于遗传算法的多阶段机械臂绝对精度标定方法，是一种“由粗到精，由快到慢”的标定方法，其利用机械臂的运动学参数-Denavit-Hartenberg(简称为DH)参数和机械臂第六关节的终端坐标构建误差模型，将机械臂绝对精度标定问题转换为一个数值优化问题，在此基础上进行最值求取，从而得到机械臂的真实DH参数，进而结合前向运动学获得机械臂的真实终端坐标。该方法由于引入了多阶段标定策略，因此可以很好的避免陷入局部极值这种情况的发生。同时，相比于传统的利用激光三维测量仪获取机械臂终端坐标的方式，NDI Optotrak可以不间断地获取机械臂终端坐标，测量精度以及稳定性远高于传统测量方法。

本发明方法的原理是：1.)本发明采用Denavit和Hartenberg提出的DH参数表示法对机械臂进行建模。DH参数由四组不同的参数组成，分别为关节角度theta、关节扭角alpha、连杆长度a、关节偏置长度d。根据机械臂的自由度的不同，参数数量也不相同。常见的六自由度机械臂每组有六个参数，共24个DH参数。建模时一般将坐标系规定在机械臂第一根连杆底面的中心处，在坐标系确定之后，就可以利用前向运动学对机械臂进行建模，从而获得机械臂关节角度和机械臂终端坐标之间的关系。之后利用NDI Optotrak等坐标测量工具获取机械臂终端实际坐标P_real，同时记录达到某一坐标时机械臂各个关节角度。分别将24个DH参数的误差值假设出来，利用前向运动学求出含有误差的机械臂终端坐标P_err，最终求P_real和P_err的差值的欧几里得范数结果极小时的DH参数的误差值。在求得误差值之后，对机械臂的DH参数进行校正，从而完成标定。

本发明提出的绝对精度标定方法主要包含三个部分：首先，利用理论DH参数加上假设出的未知误差建立前向运动学方程；其次，利用坐标测量设备获取机械臂实际终端坐标，并记录达到某一坐标时的关节角度；最后，列出误差方程，利用遗传算法进行参数优化。

基于遗传算法的机械臂绝对定位精度标定系统，包括机械臂绝对精度标定数学模型(对应下面的步骤1)至4))和多阶段遗传算法单元(对应下面的步骤5))，其中：

机械臂绝对精度标定数学模型，主要用于构建机械臂参数误差模型，具体地，使用理论DH参数配合前向运动学获取机械臂的理论终端坐标P₀，使用三坐标测量仪如NDIOptotrak获取机械臂实际位姿即机械臂终端坐标，记作P_real，在机械臂的24个理论DH参数上分别添加一个微小误差，得到含有误差的机械臂DH参数模型，重新利用前向运动学获取含有误差的机械臂终端坐标，记作P_err，机械臂的参数误差模型即为

上式中θ表示六自由度机械臂每个关节转角的误差，即θ₁-θ₆，α、d、a，均表示六自由度机械臂的六个参数误差。

多阶段遗传算法单元，主要用于对上述机械臂的参数误差模型进行优化，根据不同阶段的优化结果，动态调整遗传算法中的选择、交叉算子，以及精英种群比例、变异比率。遗传算法中的目标函数为机械臂参数误差模型，初始种群为理论DH参数，优化目标为求出误差参数模型中的24个微小误差，使得目标函数在容许的精度条件下，整体的值最小。

其中，遗传算法搜索范围上下限设置，关节一处设置较大的关节角上下限范围，关节二至六设计较小的上下限范围，同时，由于较大的搜索范围会导致遗传算法陷入局部最优，而较小的范围会导致遗传算法过早的收敛，因此，上下限应根据不同阶段的搜索结果动态调整。将第上次搜索的结果作为下一次搜索的初值，在初始的搜索过程中使用较大的精英种群比例，使得搜索过程快速收敛，在之后的搜索过程中减小精英种群所占比例，提高惩罚，并启用启发式交叉算子，减小种群中“超级个体“的生成，避免过早收敛于局部极值处。

基于遗传算法的机械臂绝对定位精度标定方法的主要步骤如下：

1.)利用理论DH参数q₀建立机械臂运动学参数模型，并得到理论终端坐标P₀；

2.)通过坐标测量设备获取机械臂终端实际测量位姿P_real，以及对应关节的角度；

3.)在理论DH参数q₀上加上一个微小误差Δq，并建立含有误差的机械臂终端坐标P_err；

4.)对所有采集到的点位计算

并求和；

5.)通过数值优化算法求误差参数方程

极小时对应的微小误差Δq，即机构参数修正量。

图1为本发明的基于遗传算法的机械臂绝对定位精度标定方法的流程图，一种基于遗传算法的机械臂标定方法，整体操作流程现分述如下：

1)利用运动学参数(理论DH参数)q₀，建立机械臂运动学参数模型，然后利用前向运动学，得到理论终端坐标P₀(即理论位姿P₀)。具体地，本发明以六自由度机械臂为例，利用机械臂理论参数q₀，采用DH参数表示法建模。在对机械臂建模之前必须预先规定好各个关节的坐标系，机械臂各关节坐标系图如图2所示。模型建立完成后，可用Robotic Toolbox进行可视化仿真，六自由度机械臂仿真建模图如图3所示。之后需要使用前向运动学，计算理论终端坐标P₀。前向运动学主要用来表达机械臂关节角和终端执行器坐标之间的关系。前向运动学公式主要通过机械臂各关节间的坐标变换求得。由坐标系n到坐标系n+1的变换矩阵由下式所示：

用S表示sin函数，C表示cos函数，得到下式：

进而得到机械臂的前向运动学公式：

¹T₆＝A_lA₂A₃A₄A₅A₆

同时，¹T₆具有如下形式：

其中3×3的矩阵

代表从基坐标系(关节一所在坐标系)到终端执行器坐标系(关节六所在坐标系)的旋转矩阵，向量[P_x P_y P_z]^T表示终端执行器在基坐标系下的坐标。

2)获取机械臂终端实际坐标P_real和对应关节的角度。使用控制器控制机械臂运行到几个指定位置(点位)，使用三坐标测量仪，例如，NDI Optotrak，测量此时的机械臂终端实际坐标(实际测量位姿)P_real，同时记录控制器中机械臂各个关节的角度。为了使标定结果尽可能的准确，需要控制机械臂在工作空间内，尽可能多的运行到一些点位，同时由于万向锁的存在，要避免运行到机械臂的奇异点。由于六自由度机械臂的误差参数方程含有24个未知数，要求取这24个未知数，必须列出不少于24个等式，即采集的点位不小于24个。在实际标定中通常选用40个点位。

3)在理论DH参数q₀上加上一个微小误差参数Δq(即机构参数修正量)，并建立含有误差的机械臂终端坐标P_err。在获取到实际坐标和对应到关节角之后，创建24个微小误差参数Δq，需要通过在24个机械臂运动学参数(DH参数q₀)上加上一个微小误差Δq，并使用前向运动学公式建立含有误差的机械臂终端坐标P_err的方程，假设某六自由度机械臂的参数误差如下所示：

Δθ＝(Δθ₁，Δθ₂，Δθ₃，Δθ₄，Δθ₅，Δθ₆)

Δα＝(Δα₁，Δα₂，Δa₃，Δα₄，Δα₅，Δα₆)

Δd＝(Δd₁，Δd₂，Δd₃，Δd₄，Δd₅，Δd₆)

Δa＝(Δa₁，Δa₂，Δa₃，Δa₄，Δa₅，Δa₆)

则含有误差的机械臂DH参数如表1所示：

表1含有误差的机械臂DH参数

根据表1所示DH参数，结合机械臂前向运动学公式，可以求出机械臂终端坐标系相对于基坐标系的旋转变换矩阵，进而可以求得含有误差的机械臂终端坐标P_err。

P_err＝[P_errx P_erry P_errz]^T＝f(Δθ，Δα，Δd，Δa) (2.16)

4)建立误差参数方程

对采集到的所有点位计算

并进行求和。

5)通过多阶段遗传算法求误差参数方程

极小时的机构参数修正量Δq，在得到修正量Δq之后，最优机构参数q等于理论DH参数加上机构参数修正量Δq。

首先将机械臂的理想DH参数当作遗传算法的初值进行迭代。初次迭代的需要设置较大的搜索范围，从而可以使种群的多样性有所提升。初次迭代搜索选用轮盘赌选择方式以及中值交叉法，历经492代，得到如图4所示结果，可见中值交叉的方式不利于种群中产生极具优势的个体，也不利于父代将优势遗传给子代。因此使用启发式交叉方式进行改进，改进后的算法历经374代，得到如图5所示结果，总体误差较改进前的方法降低了62.35％。接着使用本次迭代的结果作为下一次迭代的初值，进一步改进搜索策略，同时降低种群中精英个体所占比例进行第二次迭代。最终，历经2400次迭代得到如图6所示结果。此次总体误差较上一次迭代进一步降低，降幅达到了19.32％。由于此次迭代结束是因为达到了所设迭代次数上限，因此继续重复此次过程，将本次迭代搜索的结果作为下一次迭代的初值，同时降低搜索范围，改变搜索策略，继续进行搜索。第三次搜索结果如图7所示，经过461次迭代之后，最终40个点位的总误差为35.54mm，达到临床要求。

在获取到参数误差之后将其补偿到机械臂控制器当中，再控制机械臂重新获取十个点位坐标，测试实际效果，结果如表2所示：

表2修改DH参数后实际测试表格

在机械臂工作区间内重新获取了10个点位的坐标，标定后机械臂的绝对定位精度达到了0.76mm。使用原始DH参数计算标定前机械臂的绝对定位精度，结果为29.93mm，即本次标定将绝对定位精度提高了38.38倍，由此可知标定后的机械臂绝对定位精度达到外科手术临床要求。

以上实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (6)

1.一种基于遗传算法的机械臂绝对定位精度标定方法，其特征在于，包括：

步骤1)利用理论DH参数建立机械臂运动学参数模型，并得到理论终端坐标；

步骤2)获取机械臂终端实际坐标，以及对应关节的角度；

步骤3)在理论DH参数上加上一个微小误差，并建立含有误差的机械臂终端坐标；

步骤4)对所有采集到的点位计算

并求和，其中，P_err为所述含有误差的机械臂终端坐标，P_real为所述机械臂终端实际坐标；

步骤5)通过多阶段遗传算法求得误差参数方程

极小时的微小误差以作为机构参数修正量，然后加上理论DH参数得到最优机构参数。

2.根据权利要求1所述的机械臂绝对定位精度标定方法，其特征在于，在步骤1)中，包括：在对机械臂建模之前必须预先规定好各个关节的坐标系，模型建立完成后，进行可视化仿真。

3.根据权利要求1所述的机械臂绝对定位精度标定方法，其特征在于，在步骤1)中，利用前向运动学，得到理论终端坐标，其中前向运动学公式主要通过机械臂各关节间的坐标变换求得，由坐标系n到坐标系n+1的变换矩阵由下式所示：

用S表示sin函数，C表示cos函数，得到下式：

进而得到机械臂的前向运动学公式：

¹T₆＝A₁A₂A₃A₄A₅A₆

同时，¹T₆具有如下形式：

其中3×3的矩阵

代表从基坐标系到终端执行器坐标系的旋转矩阵，向量[P_x P_y P_z]^T表示终端执行器在基坐标系下的坐标。

4.根据权利要求1所述的机械臂绝对定位精度标定方法，其特征在于，在步骤2)中，使用控制器控制机械臂运行到几个指定位置，使用三坐标测量仪测量此时的机械臂终端实际坐标，同时记录控制器中机械臂各个关节的角度。

5.根据权利要求1所述的机械臂绝对定位精度标定方法，其特征在于，在步骤3)中，创建24个微小误差参数，需要通过在24个机械臂运动学参数上加上一个微小误差，并使用前向运动学公式建立含有误差的机械臂终端坐标的方程；假设某六自由度机械臂的参数误差如下所示：

Δθ＝(Δθ₁，Δθ₂，Δθ₃，Δθ₄，Δθ₅，Δθ₆)

Δα＝(Δα₁，Δα₂，Δα₃，Δα₄，Δα₅，Δα₆)

Δd＝(Δd₁，Δd₂，Δd₃，Δd₄，Δd₅，Δd₆)

Δa＝(Δa₁，Δa₂，Δa3，Δa4，Δa₅，Δa6)，

结合机械臂前向运动学公式，可以求出机械臂终端坐标系相对于基坐标系的旋转变换矩阵，进而可以求得含有误差的机械臂终端坐标P_err，

P_err＝[P_errx P_erry P_errz]^T＝f(Δθ，Δα，Δd，Δa)。

6.一种基于遗传算法的机械臂绝对定位精度标定系统，用于实现权利要求1-5任一项所述的基于遗传算法的机械臂绝对定位精度标定方法，包括机械臂绝对精度标定数学模型和多阶段遗传算法单元，其中，

所述机械臂绝对精度标定数学模型，用于构建机械臂参数误差模型；

多阶段遗传算法单元，用于对所述机械臂的参数误差模型进行优化，根据不同阶段的优化结果，动态调整遗传算法中的选择、交叉算子，以及精英种群比例、变异比率。

Images