双臂液压钻车的运动学分析及优化

张付祥，孙书鹏

(河北科技大学机械工程学院，河北石家庄 050018)

双臂液压钻车的运动学分析及优化

张付祥，孙书鹏

(河北科技大学机械工程学院，河北石家庄 050018)

为了实现双臂钻车的自动化打孔，对其进行运动学分析和优化计算。优化计算的目的是要找到液压钻臂各个关节的合适的角度位置，能够在钻车工作时实现确定的位姿。采用粒子群算法在MATLAB环境下计算并得出最优解，结果证明粒子群算法能有效解决两臂钻车的运动学计算。为了方便操作和审核，建立了用户界面并生成了.exe文件，从而为钻车自动化打孔奠定了技术基础。

双臂钻车;运动学;粒子群算法;MATLAB

在发达国家，钻孔作业都早已完成了手工打孔向全面机械化打孔的过渡［1］。在我国，虽然钻车也在迅速发展，各式各样的钻车纷纷涌现，但是与国外的快速发展以及高质量、高精度的钻车钻孔相比，还是比较落后，主要是在质量、精度方面，因此对钻车进一步的研究显得尤为重要。在此主要对钻车的运动学方面进行简要分析，为实现钻车的自动化打孔奠定技术基础。图1所示为某煤矿机械厂CMJ2-27全液压钻井钻车结构简图［2］。

图1 CMJ2-27全液压钻车结构

1 运动学计算

此部分计算的目的是求解钻车手部期望位姿对应的关节变量，手部位姿为6个自由度，需要6轴联动［3］。这里建立CMJ2-27全液压钻车双臂运动学坐标系和用户坐标系，如图2所示。

图2 CMJ2-27全液压钻车的坐标系

考虑到研究的钻车为双臂操作，且两个机械结构基本一致，而且两个机械臂D-H参数中的常量部分参数完全一致［4］。根据建立的坐标系确定钻车的D-H参数，如表1所示。

表1 液压钻车单臂各连杆D-H参数数

钻车机械臂坐标变换的矩阵A表达式为

则用户坐标系到机械臂的变换矩阵为A(其中用户坐标系到左机械臂的变换式中xu＜0，用户坐标系到右机械臂的变换式中xu＞0)。

直接看基座坐标系的位置和各坐标轴在用户坐标系的投影［5］。考虑用户坐标系，建立机械臂统一的运动学方程如下:

此结构型式的运动学逆解很难求取，经努力发现并无解析解，经分析主要原因为:(1)结构型式不利于运动学求解，连杆均有扭角;(2)有扭转角的连杆长度不为零;(3)回转关节处连杆间距不为零。其中第 (1)条为关键因素，需要根据作业任务和运动学求解要求重新设计机械臂的结构形式，第 (2)、(3)条是次要因素，可简化运动学求解，但只改变这些解决不了根本问题，最终需要在改变结构型式的基础上尽量改善避免，第(2)、(3)条情况出现。因此如果想求取运动学逆解必须采取数值方法，这里采用粒子群算法来求解。

2 粒子群算法

2.1 粒子群简介

粒子群 (PSO)算法是近年来发展起来的一种新的进化算法。PSO算法属于进化算法的一种，和遗传算法相似，它也是从随机解出发，通过迭代寻找最优解，通过适应度来评价解的品质，通过追随当前搜索到的最优值来寻找全局最优解。PSO初始化为一群随机粒子 (随机解)，然后通过迭代找到最优解。在每一次迭代中，粒子通过跟踪两个“极值”来更新自己:第一个就是粒子本身所找到的最优解，这个解叫做个体极值pBest;另一个极值是整个种群目前找到的最优解，这个极值是全局极值gBest。另外也可以不用整个种群而只是用其中一部分作为粒子的邻居，那么在所有邻居中的极值就是局部极值。

在找到这两个最优值时，粒子根据如下的公式来更新自己的速度和新的位置［6］:

式中:v［］是粒子的速度，w是惯性权重，persent［］是当前粒子的位置;pbest［］和gbest［］如前定义;rand()是介于 (0，1)之间的随机数;c1、c2是学习因子，通常c1=c2=2。

2.2 粒子编码

为了解决x，应该正确地设计变量生成的代码用于SPO。根据算法的特点，可以直接使用真实的数字来表示参数。使用x={θ1，θ2，θ3，θ4，θ5，d6}表示粒子当前位置，v= {v1，v2，v3，v1，v2，v3}表示粒子当前速度，最终得出x的代码是

2.3 粒子群适应值的计算

解决逆运动学方程是要找到一个正确的关节位置得到预期的位置与方向。在粒子群算法的计算过程中，每个粒子可以得到一个位置和方向，使用直接运动学计算粒子的理想位置与方向。可以利用函数norm()计算两个向量的距离位置，MATLAB中计算公式如下

参考两个空间向量的余弦定理［7］

这里使用similar_n，similar_o和similar_a表示粒子实际位置、方向与预期的关系，得MATLAB表达式如下

2.4 粒子群的计算步骤

(1)设置算法参数如大小、尺寸、数量、权重因子w，空间位置xr和速度空间vr;

(2)设计相关常量参数;

(3)初始化粒子群。根据约束条件，在随机的粒子初始化位置和速度在参数区间内列出方程;

(4)计算粒子群的适应值;

(5)初始化当前粒子的位置，作为pbest和gbest的值;

(6)更新粒子的位置和速度;

(7)再次计算粒子的适应值;

(8)更新粒子的pbest和gbest;

(9)重复步骤(6)—(8)，直到迭代计算值达到设定精度;

(10)得到输出参数pbest和gbest的实验结果。

3 用户界面设计与EXE文件生成

(1)首先编辑生成M文件，当然不止一个M文件，M文件中存在嵌套关系。

(2)由于M文件都是一堆代码，不便于阅读与操作，出了问题也不便于修改、审核。为了使操作人一目了然，方便操作，在已存在M文件的基础上经过一系列的操作实现向GUI界面的转换［8］。生成的用户界面不仅简单明了，而且方便操作，使操作工人能很快上手，提高了工作效率，简单操作如下:在文本框中输入理想位姿坐标参数及孔参数，点击计算，就可以在结果栏输出结果。生成的用户界面如图3所示。

(3)进行GUI转化成.exe文件步骤:①安装编译器。可有多种选择，MATLAB自带了一个LCC，作者基于VC++6.0实现的;②设置编译器。在MATLAB命令行输入mbuild-setup，选择安装的C编译器;③调用编译器。在命令窗口输入mcc-m robot.m，生成robot.exe、mccExcludedFiles.log以及readme.txt文件;④到此生成了robot.exe文件。

图3 双臂液压钻车运动学计算用户界面

(4)EXE文件的运行。

要使robot.exe文件顺利运行，首先需要安装一个名为MCRInstaller.exe插件，安装完成后，即可实现操作，双击robot.exe文件，输入数据，点击计算，即可在结果栏输出结果。若输入数据符合要求，则直接输出结果;若输入数据不符合要求，则输出bigerror提醒输入错误。

4 计算结果与分析

图4 粒子群优化算法的优化过程

5 结论

钻车的普遍应用使作者有了升级钻车的想法，当然钻车的高精度自动化打孔是升级的主要目标，钻车打孔精度的提高首先必须依赖高精度的运动学分析以及粒子群算法分析，在此基础上还要加以简单易操作的程序控制，从而实现理论误差、操作误差减低到最小，这才是作者所要达到的终极目标，对钻车的运动学分析以及粒子群优化，使钻车的精度大大提高，增强了钻车的竞争力。

［1］李同河，路聚堂，王振江.液压掘进车岩掘进工程中的应用［J］.江西煤炭科技，2007(4):33－34.

［2］邹海波，刘锋，梁伟，等.煤矿用液压掘进钻车选型［J］.煤矿机械，2010，31(8):204－206.

［3］孙增圻.机器人系统仿真及应用［J］.系统仿真学报，1995，7(3):23－29.

［4］DOLLARHIDE Robert L，AGAH Arvin.Simulation and Control of Distributed Robot Search Teams［J］.Computers and Electrical Engineering，2003，29(5):625－642.

［5］ZHAO Qingjie，SUN Zengqi.Imagebased Robot Motion Simulation［J］.Optics Communications，2002，205(4): 257－263.

［6］周驰，高海兵，高亮，等.粒子群优化算法［J］.计算机应用研究，2003(12):7－11.

［7］方伟，孙俊，谢振平，等.量子粒子群优化算法的收敛性分析及控制参数研究［J］.物理学报，2010，59(6):3687－3693.

［8］CORKE Peter I.A Robotics Toolbox for MATLAB［J］.IEEE Robotics and Automation Magazine，1996，3(1):24－32.

Kinematics Analysis and Optimization of a Two-arm Hydraulic Drill Manipulator

ZHANG Fuxiang，SUN Shupeng
(College of Mechanical Engineering，Hebei University of Science and Technology，Shijiazhuang Hebei 050018，China)

In order to realize drilling holes automatically by a two-arm drilling manipulator，its direct kinematics analysis was done.The aim of optimization was to find the right position and orientation of the arm by which they could realize the position and orientation of the drill demanded on the drilling work.An optimization program of the PSO algorithm in the MATLAB environment was developed and the optimal calculation was done.The result proves the validity of the PSO based algorithm for the inverse kinematics of the 2-arm drilling manipulator.In order to facilitate the operation and audit，the user interface was set up and.exe files were made，so as to lay foundation for drilling holes automatically.

2-arm drilling manipulator;Kinematics;Particle swarm optimization(PSO);MATLAB

TH113

A

1001－3881(2014)9－101－4

10.3969/j.issn.1001－3881.2014.09.028

2013－03－06

石家庄市科技发展计划项目重大专项 (11108171A－1);河北科技大学杰出青年资助项目

张付祥 (1973—)，男，博士，副教授，研究方向为机电控制与机器人技术。E－mail:zhangfx@hebust.edu.cn。