DOI: 10.11817/j.issn.1672-7207.2019.01.007

基于位姿误差补偿的全电脑凿岩台车钎杆定位算法

夏毅敏^{1, 2}，李正光^{1, 2}，罗建利³，曾桂英^{1, 2}，马劼嵩^{1, 2}

(1. 中南大学 机电工程学院，湖南 长沙，410083；

2. 中南大学 高性能复杂制造国家重点实验室，湖南 长沙，410083；

3. 中国铁建重工集团有限公司，湖南 长沙，410100)

摘要：针对全电脑凿岩台车钻炮孔时钎杆的定位精度要求，从钎杆末端位姿误差补偿角度出发，以DH运动学模型为基础，计算臂架的动态误差和静态误差，建立钎杆定位误差补偿运动学模型；采用多种群遗传算法(MPGA)，根据无误差正向运动学逆解所得关节变量(角度和距离)优化各关节值的搜索范围，结合移民算子和人工选择算子进行并行计算，建立基于钎杆运动学误差补偿模型和MPGA求逆解确定关节变量的钎杆定位算法。研究结果表明：基于误差补偿模型进行钎杆定位时，实测钎杆定位误差小于0.1 m，满足工程实际定位要求。

关键词：全电脑凿岩台车；钎杆定位；DH运动学模型；位姿误差补偿；多种群遗传算法

中图分类号：TP241.2        文献标志码：A         文章编号：1672-7207(2019)01-0044-08

Positioning algorithm of drill rod for full computer-controlled rock-drilling jumbo based on pose error compensation

XIA Yimin^{1, 2}, LI Zhengguang^{1, 2}, LUO Jianli³, ZENG Guiying^{1, 2}, MA Jiesong^{1, 2}

(1. School of Mechanical and Electrical Engineering, Central South University, Changsha 410083, China;

2. State Key Laboratory of High Performance Complex Manufacturing, Central South University, Changsha 410083, China;

3. China Railway Construction Heavy Industry, Changsha 410100, China)

Abstract: With consideration of the drill rod accuracy requirement of full computer-controlled rock-drilling jumbo, and the pose error compensation of the drill rod, an error compensation kinematics model of the drill rod was proposed based on the DH kinematic model and the dynamic and static error of the arm. A positioning algorithm of drill rod based on kinematic error compensation model of drill rod was established by using multiple population genetic algorithm(MPGA) which optimized search range of each joint according to the analytic solutions of the forward kinematics without error compensation and combined migration operator and artificial selection operator. The results show that when the drill rod is positioned based on the error compensation model, the positioning error of the drill rod is less than 0.1 m, which meets the requirement of the actual positioning of the project.

Key words: full computer-controlled rock-drilling jumbo; drill rod positioning; DH kinematics model; pose error compensation; multiple population genetic algorithm

全电脑凿岩台车是一种集机械、电子、液压等多学科技术于一体的隧道施工设备。与传统凿岩设备相比，全电脑凿岩台车能有效控制超挖量^[1]，降低因地质原因引起的施工事故，保障工作人员的施工安全。在施工过程中，关键技术之一是钎杆的快速准确定位。钎杆的自动定位精度和定位速度经常出现不满足工程实际需求的情况，严重影响了隧道施工效率和施工质量。目前，凿岩台车在进行钎杆定位时的研究主要包括运动学误差分析和误差补偿运动学求逆解2部分。近年来，国内外学者对凿岩台车的运动学研究较多，如：JIANG等^[2-3]在求解6r-2p凿岩台车的逆解时，为避免求解非线性系统，改变关节输入条件，计算过程不涉及数值迭代和操纵矩阵；WANG等^[4]利用对偶四元数法替代DH法进行建模，对六自由度凿岩台车进行逆运动学求解；何清华等^[5]建立了多关节钻臂的运动学方程，根据钻臂的实际工作特点和工作范围提出了一种求逆解的方法；周友行等^[6]提出了一种基于运动轨迹的求解多关节冗余机器人运动学逆解方法。上述研究均未充分考虑凿岩台车臂架运动学误差的影响，并且未分析存在误差时的逆解计算方法。为此，本文作者借鉴工业机器人运动学误差补偿研究^[7-10]和遗传算法在工程中的应用^[11-14]，以DH法建立臂架正向运动学模型，通过计算臂架动态误差和静态误差，建立运动学误差补偿模型；在进行钎杆定位时，采用多种群遗传算法，结合无误差补偿运动学逆解结果、移民算子和人工选择算子，计算误差补偿模型逆解，保证凿岩台车钎杆的定位精度和速度。

1  钎杆定位方法

1.1  臂架结构

全电脑凿岩台车臂架包括6个回转关节和2个移动关节，钎杆安装在推进梁上，其主要结构如图1所示。

1.2  钎杆定位方法

影响凿岩台车钎杆位姿误差的主要因素分为动态误差和静态误差，其中，动态误差包括外力、惯性力等导致的弹性变形和振动误差，静态误差包括结构参数误差、环境因素(温度、磨损)导致的尺寸误差和传感器误差等。

在给定钎杆末端位姿情况下，可以根据无误差正向运动学模型求逆解得到对应的关节变量(角度和距离)，并将其作为计算臂架动态误差、静态误差的输入值和钎杆误差补偿模型求逆解的初始搜索值。同时，为保证计算速度和计算精度，预先设定迭代次数，通过MPGA算法对各关节值不断搜索和误差补偿，得到臂架优化关节值，使钎杆达到期望位姿。根据所提的研究思路，对钎杆定位问题进行研究，具体研究路线如图2所示。

图1  凿岩台车臂架结构图

Fig. 1  Boom structure of rock-drilling jumbo

图2  凿岩台车钎杆定位研究

Fig. 2  Positioning research on drill rod for rock-drilling jumbo

2  钎杆运动学误差补偿模型

2.1  钎杆无误差运动学求解

本文只对锚杆油缸固定的一般工况进行讨论，即锚杆关节为过渡关节，关节角恒为0°。由于DH法属于参数建模方法，坐标系需要建立在关节轴的延长线上，因此，需在不改变运动学结果的前提下对部分坐标系进行位姿变换。根据凿岩台车结构参数，建立各关节坐标系，将推进梁回转坐标系原点o₆沿关节轴z₆移动距离d₆至x₇o₇z₇平面，将锚杆坐标系o₇沿关节轴x₇移动距离a₆至y₈o₈z₈平面。臂架D-H参数如表1所示，臂架运动学模型如图3所示。

图3  钎杆正向运动学模型

Fig. 3  Forward kinematics model of drill rod

将参数代入关节坐标变换矩阵，得

 (1)

所得臂架正向运动学模型为

        (2)

式中：i表示第i个关节；；；，为钎杆位置矩阵；，为钎杆的姿态矩阵。

式中：；； ；。在实际施工过程中，为了随时观察凿岩机的工作情况，需要在臂架孔序规划后先确定推进梁回转关节θ₆，共0°，90°，180°和270°这4种工况。

根据臂架正向运动学模型进行推导，得

             (3)

表1  臂架DH参数

Table 1  DH parameters of boom

为防止出现分母为0导致计算溢出，将运动学求逆解分为的一般工况和的特殊工况。

2.1.1  一般工况求解

当时，

    (4)

     (5)

 (6)

               (7)

      (8)

          (9)

         (10)

           (11)

      (12)

在一般工况下，运动学逆解的求解顺序为。

2.1.2  特殊工况求解

当时，。由于数值解不封闭，有多组解，故先给定大臂伸缩关节d₃。

        (13)

      (14)

                (15)

            (16)

   (17)

在特殊工况下，运动学逆解的求解顺序为。

2.2  臂架动态误差补偿

2.2.1  大臂及推进梁动态误差计算

将大臂和推进梁分别简化为一端固定的Bernoulli-Euler梁。臂架等效模型如图4所示。由于大臂末端到推进梁的部件总质量超过2 t，大臂末端最大变形达到0.1 m以上，属于大挠度变形，经典挠度公式不再适用，故重新对挠度公式进行改进。

图4  臂架等效模型

Fig. 4  Equivalent model of boom

1) 当臂架末端受集中载荷时，臂架任意一点的扭矩为

   (18)

式中：M_e为臂架末端所受集中扭矩；F为末端所受集中力。x_A和z_A分别为末端位置x方向和z方向的位移；θ_A为转角变形；M_e和F受推进梁重心位置影响，其影响参数为大臂俯仰值θ₂、推进梁俯仰值θ₄和推进梁伸缩值d₈。

臂架上任意位置的变形为

             (19)

式中：z为挠度变形；θ为端截面转角变形；x为轴向变形；E为臂架弹性模量；I为等效转动惯量。

经计算，得到臂架末端角度变形为

   (20)

式中：L为臂架的全长。综合式(18)~(20)得

   (21)

2) 臂架受均布载荷时，根据式(19)，令

       (22)

代入边界条件， (其中，为末端角度变形)，。赋值，，得

   (23)

式中：；q为均布载荷；β为臂架俯仰

角度，当臂架对应为大臂时，，当臂架对应为推进梁时，。

由式(19)得

   (24)

式中：

    (25)

影响臂架挠度和端截面转角的参数为大臂伸缩值d₃、大臂俯仰值θ₂、推进梁伸缩值d₈和推进梁俯仰值θ₄。其中，主要影响参数为大臂俯仰值θ₂和大臂伸缩值d₃。在旋转关节θ₂和移动关节d₃的可变范围内对大臂变形求解，设定θ₄和d₈处于零位，对2种载荷施加方式进行线性叠加，挠度变形Δz和端截面转角Δθ变形结果分别如图5和图6所示。挠度变形Δz分布于0.02~0.12 m之间，端截面转角Δθ分布于0.46°~1.86°之间，变形与大臂长度成正相关。大臂轴向变形Δx较小，数量级为，在实际误差补偿中可以忽略。推进梁动态误差规律与大臂的动态误差规律相同，但是数量级过小，在动态误差补偿过程中可忽略。

图5  大臂挠度变形

Fig. 5  Deflection of large arm

图6  大臂端截面转角

Fig. 6  Cross rotation angle of large arm

2.2.2  臂架动态误差补偿

对大臂移动关节d₃进行动态误差补偿，在进行臂架定位时，基于无误差补偿模型求逆解所得各关节值，进而得到挠度变形Δz和端截面转角Δθ，并将数值代入传递矩阵，变换方式如图7所示。

图7  动态误差补偿坐标变换

Fig. 7  Coordinate transformation of dynamic error compensation

图7中，坐标系o₃先沿x轴移动挠度值Δz，再沿轴旋转端截面转角Δθ，变换至坐标系，进行坐标系传递变换至。变换关系为

    (26)

式中：T_Δ为动态误差补偿矩阵。

2.3  臂架静态误差补偿

基于工业机器人离线标定方法，利用全站仪和角度仪对相邻关节轴进行距离标定和偏转角度标定，修正各关节结构参数(α，a，θ，d)。根据摄动微分理论，对式(1)传递矩阵求偏导，得

      (27)

式中：为第i关节的传递矩阵。

2.4  钎杆误差补偿运动学模型建立

结合臂架动态误差和静态误差补偿，得到钎杆运动学误差补偿模型：

 (28)

式中：。

3  钎杆误差补偿模型逆解算法

根据钎杆的期望位姿和建立的凿岩台车钎杆运动学误差补偿模型，采用多种群遗传算法(MPGA)对误差补偿模型求逆解，对各关节变量不断搜索和误差补偿，计算各关节优化值，使钎杆达到期望位姿。

3.1  钎杆运动学MPGA模型

3.1.1  构造初始种群

初始种群为MPGA算法的搜索出发点。本文构造的初始种群数为10个，每个种群个体数为100个。设定每个关节值在个体的染色体中占据20个基因位置，保证算法的求解精度。

3.1.2  构造目标函数

已知钎杆期望位姿为

              (29)

式中：R_g为钎杆期望姿态矩阵；P_g为期望位置矩阵。

将种群中各个个体的关节值代入臂架运动学误差补偿模型，得到个体的当前钎杆位姿矩阵：

             (30)

式中：R_c为钎杆当前姿态矩阵；P_c为当前位置矩阵。

构造目标函数：

         (31)

式中：，为各种群中个体的钎杆计算姿态和期望姿态的姿态误差；，为钎杆计算位置和期望位置的位置误差。

3.1.3  设计约束条件

根据正向运动学求逆结果，将各旋转关节角范围缩减为，将各移动关节距离范围缩减为，优化了各关节变量的搜索范围，提高了算法的求解速度。

交叉率P_c决定了遗传算法的全局搜索能力，传统交叉率P_c取值范围为0.7~0.9。MPGA采用多点交叉方式，增强了算法搜索能力，避免早熟收敛。

变异率P_m决定了局部搜索能力，传统P_m取值范围为0.001~0.050。由于搜索范围已经相对减小，需要增强局部搜索能力，因此，变异率P_m相对增大，取P_m为0.05。

3.1.4  构造适应度函数

采用轮盘赌法选择适应度高的个体进入下一代，构造适应度函数F为

         (32)

式中：a和b分别为姿态误差和位置误差的权重系数。姿态参数单位为rad，位置参数单位为m，两者数量级相同。经过大量测试确定权重系数为a=1，b=1.5~2.5。

3.1.5  优化方案

采用移民算子，打破各种群的独立性，将各种群的最优个体引入其他种群，替代其他种群中的最劣个体，实现各种群协同进化。

采用人工选择算子，将每代中最优个体放入精华种群，精华种群不进行选择、交叉和变异等操作，只记录和更新每一代最优种群。

3.2  求解精度分析

给定一组关节值(θ₁=20°，θ₂=-45°，d₃=0.4 m，θ₄=-15°，θ₅=0°，d₈=2.0 m)，推进梁回转关节角θ₆和锚杆关节角θ₇为0°，采用MATLAB进行算法编程，基于运动学误差补偿模型所得到的钎杆位姿计算20次逆解。对比计算关节值和给定关节值相对误差平均值，结果如表2所示。旋转关节的相对误差远小于移动关节的相对误差，两者相差1个数量级。这是因为凿岩台车结构尺寸较大，旋转关节对位姿误差的影响要远大于移动关节的影响。大臂伸缩关节d₃和推进梁伸缩关节d₈具有明显互补关系，可以适当提高适应度函数中位置误差的权重系数b，增大移动关节对末端位姿的影响。

表2  逆解计算关节值及相对误差平均值

Table 2  Inverse calculation of joint value and average relative error value

分别选取算法中最快和最慢种群的进化过程进行对比，结果如图8所示。MPGA算法在400代内使适应度达到0.99以上，求解能力强，收敛速度快，计算结果稳定可靠，不会发生早熟收敛现象。因此，将算法结束条件设置为计算到第400代种群。

4  试验验证

根据凿岩台车施工时的实际停车位置和炮眼设计，给定18组凿岩台车钎杆期望位姿值，期望位姿如图9所示。其中，圆点位置代表钎杆定位位置，其上引出的线段为钎杆需要达到的姿态在掌子面上的投影。设定直径为11 m级隧道掌子面距离凿岩台车大臂横摆中心距离为12 m，图示坐标系水平向左为y轴，竖直向上为z轴，垂直掌子面向里为x轴。进行基于误差补偿模型的逆运动学求解，得到各关节对应数值。在车间使用全站仪于大臂横摆中心设站，驱动臂架各关节至计算关节变量，利用全站仪测量钎杆位置，测量结果如表3所示(其中，[x, y, z]为期望坐标，为臂架移动后的钎杆实际坐标)。

图8  MPGA遗传算法进化曲线

Fig. 8  Evolution curves of MPGA genetic algorithm

图9  隧道炮眼图

Fig. 9  Borehole of tunnel

表3  钎杆测量结果

Table 3  Measuring results of drill rod

对比测试位置与期望位置的位置误差，结果如图10所示。从图10可见：x方向平均误差为0.031 m，y方向平均误差为0.028 m，z方向平均误差为0.029 m，总距离平均误差为0.056 m，并且总距离误差均小于0.100 m，满足工程实际定位要求。

图10  钎杆末端位置误差

Fig. 10  End position errors of drill rod

5  结论

1) 臂架负载和自重产生的动态误差不容忽略，大臂挠度变形范围为0.04~0.12 m，端截面转角范围为0.46°~1.86°；轴向变形数量级为10^-5~10^-4，变形较小。推进梁的动态误差规律与大臂的动态误差规律相同。

2) 针对工程中全电脑凿岩台车钎杆快速定位问题，提出了一种基于运动学误差补偿模型和MPGA算法求逆解的钎杆定位方法，根据无误差正向运动学逆解结果优化各关节变量的搜索范围，结合移民算子和人工选择算子，提高算法的求解速度和收敛精度，得到给定位姿下的优化关节变量。

3) 在车间对全电脑凿岩台车进行钎杆定位试验，根据钎杆期望位姿矩阵求逆解得到优化关节变量，驱动臂架各关节至对应关节变量，实测钎杆位置与期望位姿值误差小于0.100 m，满足工程实际定位要求。

参考文献：

[1] KIM Y K, MOON H K. Application of the guideline for overbreak control in granitic rock masses in Korean tunnels[J]. Tunnelling & Underground Space Technology, 2013, 35(3): 67-77.

[2] JIANG Qimi, AHMED S N. The kinematics of 6R-2P mining drill jumbo[J]. Journal of Mechanisms and Robotics, 2014, 6(3): 034501.

[3] WANG Yujun, FANG Can, JIANG Qimi, et al. The automatic drilling system of 6R-2P mining drill jumbos[J]. Advances in Mechanical Engineering, 2015, 7(2): 504861.

[4] WANG Hengsheng, ZHAN Deyou, HUANG Pinglun, et al. Inverse kinematics of a heavy duty manipulator with 6-DOF based on dual quaternion[J]. Journal of Central South University, 2015, 22(7): 2568-2577.

[5] 何清华, 周宏兵. 凿岩机器人钻臂的运动学研究[J]. 中南工业大学学报(自然科学版), 1998, 29(5): 483-486.

HE Qinghua, ZHOU Hongbing. Kinematic study of rock drilling robot arm[J]. Journal of Central South University of Technology(Natural Science Edition), 1998, 29(5): 483-486.

[6] 周友行, 唐稳庄, 张建勋. 基于运动轨迹的机器人运动学逆解研究[J]. 机械科学与技术, 2009, 28(7): 862-866.

ZHOU Youhang, TANG Wenzhuang, ZHANG Jianxun. A study of the inverse kinematics of a multi-joint redundant robot based on its moving path[J]. Mechanical Science and Technology for Aerospace Engineering, 2009, 28(7): 862-866.

[7] WANG Wei, WANG Gang, YUN Chao. A calibration method of kinematic parameters for serial industrial robots[J]. Industrial Robot, 2014, 41(2): 157-165.

[8] WANG Wei, WANG Lei, YUN Chao. Design of a two-step calibration method of kinematic parameters for serial robots[J]. Chinese Journal of Mechanical Engineering, 2017, 30(2): 438-448.

[9] ZENG Yuanfan, TIAN Wei, LIAO Wenhe. Positional error similarity analysis for error compensation of industrial robots[J]. Robotics and Computer-Integrated Manufacturing, 2016, 42: 113-120.

[10] LI Qinchuan, HERVE J, HUANG Pengcheng. Type synthesis of a special family of remote center-of-motion parallel manipulators with fixed linear actuators for minimally invasive surgery[J]. Journal of Mechanisms and Robotics, 2017, 9(3): 031012.

[11] AYYILDIZ M, MUSTAFA Z, ETINKAYA K. Comparison of four different heuristic optimization algorithms for the inverse kinematics solution of a real 4-DOF serial robot manipulator[J]. Neural Computing and Applications, 2016, 27(4): 825-836.

[12] 陶金, 孙青林, 朱二琳, 等. 基于遗传算法带约束的翼伞系统归航轨迹设计[J]. 中南大学学报(自然科学版), 2017, 48(2): 404-410.

TAO Jin, SUN Qinglin, ZHU Erlin, et al. Genetic algorithm based homing trajectory planning of parafoil system with constraints[J]. Journal of Central South University(Science and Technology), 2017, 48(2): 404-410.

[13] ZHENG J N, CHEN C F, GEN M. Multi-objective multi- population biased random-key genetic algorithm for the 3-D container loading problem[J]. Computers and Industrial Engineering, 2015, 89: 80-87.

[14] 韩锟, 潘迪夫, 韩洪飞. 机车车辆二系弹簧载荷分配优化的改进遗传算法[J]. 中南大学学报(自然科学版), 2016, 47(7): 2521-2527.

HAN Kun, PAN Difu, HAN Hongfei. An improved genetic algorithm for secondary spring load equalization of railway vehicles[J]. Journal of Central South University(Science and Technology), 2016, 47(7): 2521-2527.

(编辑  陈灿华)

收稿日期：2018-02-12；修回日期：2018-04-16

基金项目(Foundation item)：国家高技术研究发展计划(863计划)项目(2012AA041803)；中南大学中央高校基本科研业务费专项资金资助项目(2017zzts400) (Project(2012AA041803) supported by the National High Technology Research and Development Program(863 Program) of China; Project(2017zzts400) supported by the Fundamental Research Funds for the Central Universities of Central South University)

通信作者：夏毅敏，博士，教授，从事大型掘进装备研究；E-mail: xiaymj@ csu.edu.cn