王 强，鲍文亮

1煤炭科学研究总院 北京 100013

2中国煤炭科工集团太原研究院有限公司 山西太原 030006

凿岩机器人作为煤矿特种机器人，在优化钻孔布置、提高工作效率和减少对围岩破坏方面有着重要作用。高定位精度的钻臂机构，是保障作业质量和安全的重要组成部分[1]。因此，许多学者通过对钻臂的建模与运动学求解研究，来实现钻臂的精准控制。

王宪伦等人[2]使用几何分析法和 D-H 模型结合的方法，采用曲线拟合算法，完成了三角钻臂的运动学分析；何清华等人[3]通过建立钻臂运动学模型，根据某凿岩机器人钻臂的特殊结构形式提出一种逆解计算方法；黄开启等人[4]通过 D-H 参数法、坐标反变换法和经典消元法完成了对锚杆钻机的运动学分析。

以上方法均能完成对钻臂运动学正逆向求解，但是缺少对钎头定位误差补偿的研究。笔者使用一种基于遗传算法优化后的 BP 神经网络(GA-BP)方法，实现了对钎头的定位误差补偿，该方法能够快速、精准地得到钎头的位姿信息。

1 设计思路

首先通过分析凿岩机器人钻臂系统机构组成，结合改进型 D-H 参数法，建立钻臂的理想运动学模型；然后使用 GA-BP 神经网络得到的钎头位姿误差，经过补偿，可获取修正后的钎头位姿信息。通过运动学逆解得到修正后的关节变量，最终通过控制器将钎头送达钻孔位置。

该补偿方法流程如图1 所示。

图1 基于 GA-BP 算法的位姿补偿流程Fig.1 Posture compensation process based on GA-BP algorithm

2 钻臂结构组成

钻臂系统是凿岩机器人的重要组成部分，是完成定位和凿岩任务的主要执行机构。钻臂系统主要包括大臂机构和推进机构，如图2 所示。其中大臂机构通过液压缸的伸缩实现大跨度伸缩，以及大角度俯仰、偏转和翻转的动作；推进器机构负责完成推进钎杆和炮眼的钻凿任务[5-6]。

图2 钻臂结构组成Fig.2 Structure of drilling arm

在实际巷道的凿岩现场中，由于受到作业环境空间限制的原因，钻臂系统多采用多冗余自由度的关节型机械臂，以增加机构作业的灵活性，避免装备间的碰撞。钻臂系统一共具有7 个自由度，其中包括5 个转动副和2 个移动副[7-8]。

3 基于 D-H 参数模型的运动学求解

3.1 建立 D-H 参数模型

凿岩机器人作为行业特种机器人，在结构形式上与普通工业机器人具有相似性，是由一系列连杆串联起来的多关节机械臂。要实现井下自动化钻孔作业，需要对钻臂进行运动学分析，以解决凿岩机器人钻臂关节空间坐标到末端工作空间坐标的变换问题[9]。

D-H 参数法是由 Denavit 和 Hartenberg 提出以关节链的每个杆件建立坐标系的矩阵方法。笔者以凿岩机器人作为试验本体，采用改进型 D-H 参数法建立运动学模型，规定每个关节的4 个参数为θi，di，ai，αi，确定变换公式为T=R(θi)T(di)T(ai)R(αi)，用以描述相邻2 个连杆之间的运动学关系。变换公式可具体表达为

式中：θi为关节角，(°)；αi为杆件扭转角，(°)；ai为杆件长度，mm；di为关节横距，mm。

对凿岩机器人进行适当的机构简化，得到如图3所示的 D-H 参数模型，钻臂中各个连杆结构参数如表1 所列。

图3 凿岩机器人钻臂 D-H 模型Fig.3 D-H model of drilling arm of rock-drilling robot

表1 凿岩机器人钻臂连杆参数Tab.1 Parameters of linkage of drilling arm of rock-drilling robot

由于第5 关节轴与第6 关节轴存在轴线相交的现象，第5 坐标系原点需选择轴线相交点，因此第5 坐标系平移至图3 中所示位置[10]。同理确定第6 坐标系的位置。

3.2 正运动学求解

将表1 中的连杆参数代入变换公式，求出相邻连杆之间的变换方程，并由

得到钻臂的正向运动学求解方程，求出钎头相对于基坐标系的的位姿信息。简化方程为

式中：[nx ny nz]T、[ox oy oz]T、[ax ay az]T为钎头相对于基坐标系的姿态向量；[px py pz]T为钎头在基坐标系的位置向量；P为位移矩阵；R为旋转矩阵。

笔者将钎头相对于基坐标系的位姿定义为

式中：(x，y，z)为钎头的位置坐标，可从位移矩阵P中获取；(α，β，γ)为钎头坐标系与基座坐标系三坐标轴的夹角，可从旋转矩阵R中获取。

经过上述分析，可以得到钻臂各关节坐标系到钎头工作空间坐标系的变换关系，并由 D-H 模型确定钎头相对基坐标系的位姿信息。

3.3 逆运动学求解

笔者采用代数求解法完成逆运动学求解，逐个分离变量求解，得到各关节变量数值。具体方法为用各[i-1Ti]-1矩阵逐次左乘简化方程可得如下方程：

由于等号两侧矩阵中对应元素相等，所以可以得到12 个等式方程，即两侧矩阵中(n，o，a，p)对应等式。通过观察，优先对易分离的变量进行求解，求出变量对应值。如上式一次求逆操作，得到下一组等式，依次求解剩余关节角变量，具体方法可参考文献[11]。

4 GA-BP 神经网络模型建立

遗传算法优化 BP 神经网络包括3 个部分：第 1部分是确定 BP 网络结构，即确定输入层、隐含层和输出层的神经元个数，为确定遗传算法个体长度提供依据；第2 部分是遗传算法优化 BP 网络，即根据权值和阈值数量编码个体，通过个体选择、交叉和变异操作寻找到最优适应度值对应个体；第3 部分是将最优个体解码，得到权值和阈值的最优初始值，训练后预测输出。

4.1 确定 BP 网络结构

对钻臂运动学正解结果分析，发现钎杆末端的位置信息和姿态信息的数值单位和范围有明显的区别，这表明两类信息的非线性映射关系是不一致的，所以笔者建立了2 个 3-6-3 的网络结构，分别对位置误差和姿态误差进行预测。网络模型结构如图4 所示。

图4 3 层 BP 神经网络结构Fig.4 Structure of three-layer BP neural network

输入数据x为1 组三维向量，分别是目标点的位置向量[xthythzth]T或姿态向量[αthβthγth]T；输出数据z也是1 组三维向量，分别是位置误差[ΔxΔyΔz]T或姿态误差[ΔαΔβΔγ]T；wij、wjk分别是输入层与隐含层、隐含层与输出层之间的连接权值[12]。

4.2 遗传算法实现

BP 神经网络在利用梯度下降法进行非线性拟合过程中，存在局部逼近能力差和收敛速度缓慢等缺点，所以不适用于多自由度的机器人运动学求解预测[13]。笔者选择遗传算法的全局搜索的能力，实现BP 网络中权值和阈值的优化，具体步骤如下。

4.2.1 种群初始化

依照 BP 网络结构，采用实数编码法完成个体编码。由于设定的网络结构为 3-6-3，应有 3×6+6×3=36 个权值和 6+3=9 个阈值，所以个体长度应为 45。

4.2.2 确定个体适应度函数

个体i的适应度函数值

Fi=E，

式中：E为训练好的 BP 神经网络预测的误差函数。

4.2.3 选择操作

采用轮盘赌法，就是基于适应度比例的选择策略，个体i的选择概率

式中：N为种群数量，由于适应度值越小选择概率越大，所以需提前对其进行倒数求解；k为系数。

4.2.4 交叉操作

对实数编码需采用实数交叉法，即第n个染色体an和第m个染色体am在k位的交叉操作为

式中：定义交叉系数b范围为0～1。

4.2.5 变异操作

选择第n个染色体在j位上的基因发生突变，变异操作为

式中：amax、amin分别为染色体最大值和最小值；r为0～1 内随机数；g为当前迭代次数；Gmax为最大进化次数。

GA-BP 神经网络训练流程如图5 所示。

图5 GA-BP 神经网络训练流程Fig.5 Training process flow of GA-BP neural network

5 试验对比

5.1 数据处理

凿岩机器人钻孔分布如图6 所示。笔者使用蒙特卡罗算法，在钻臂工作空间内选择任意5 000 组位姿数据样本，样本数据包括目标位姿变量(Δx，Δy，Δz)和(α，β，γ)，及对应的位姿误差(x，y，z)和(Δα，Δβ，Δγ)。

图6 凿岩机器人钻孔分布Fig.6 Distribution of drilled holes of rock-drilling robot

为提高神经网络的预测效率和精度，需将样本集分为训练样本、检验样本和测试样本。依照样本数量的大小，划分方法有“留出法”“交叉验证法”和“自助法”。由于数据较多，笔者按照“留出法”进行划分，按照45∶4∶1 的比例，将样本互斥随机分类，得到训练样本集4 500 组，检验样本集400 组和测试样本集100 组，并对样本数据进行如下式的归一化处理，

式中：z′为归一化之后的数值；z为样本的原始值；zmin、zmax分别为样本集中的最小值和最大值。

5.2 误差计算

为方便进行钎杆末端定位误差分析，定义如下2 个误差计算公式，分别为位置误差Exyz和姿态误差Edeg，

式中：(x，y，z)、(α，β，γ)为目标位姿信息；(x′，y′，z′)、(α′，β′，γ′)为实际位姿信息。

为更直观展示 GA-BP 算法效果，分别计算位置误差和姿态误差的平均误差和标准方差，

式中：Ei为第i组测试样本误差；N为测试样本总数。

5.3 试验结果分析

使用蒙特卡罗算法，在钎杆末端可达空间内采集 100 点位姿数据，经 GA-BP 网络误差补偿后，利用上述公式对位置误差和姿态误差进行分析，并以补偿前的误差和 BP 网络补偿后的结果作为参考，对比结果如表2 所列。基于 GA-BP 算法相较于 BP 算法有更好的误差补偿效果，相较于补偿前的钎头定位，平均位置误差降低了 71.47%，平均姿态误差降低了56.29%，而且标定后的误差小于 100 mm，满足工程要求。

表2 机器人末端位姿误差结果对比Tab.2 Comparison of error results of robot end posture

6 结论

使用 GA-BP 神经网络算法实现了对某型凿岩机器人钎杆末端的定位误差辨识。经过误差补偿后，通过运动学逆解计算，得到目标位姿对应的关节变量，最终通过正向运动学控制钎杆末端移动，到达目标钻孔位置。经试验结果分析，该方法可有效降低钎杆末端的位姿误差，满足工程应用需求，且具有很好的通用性。BP 神经网络具有较好的非线性拟合能力，而且误差辨识结果准确度取决于网络结构和数据样本的质量，对钻臂的结构和自由度数的依赖不大。但是单纯的 BP 神经网络存在局部逼近能力差和收敛速度缓慢等缺点，因此经过遗传算法优化后的 BP 神经网络具有更好的辨识能力，更好的补偿效果。