1、第42卷第03期2023年03月煤炭技术Coal TechnologyVol.42 No.03Mar.2023doi:10.13301/ki.ct.2023.03.0430前言随着煤矿井下掘进技术发展，对现代煤矿巷道机械设备要求不断提升，煤矿井下巡检机器人替代人工井下检测是大势所趋。巡检机器人作业质量取决于机械臂控制，机械臂运动学根据实际工作需求规划运动轨迹；动力学计算关节力矩是实现运动控制的基础；采用独立关节控制方法建立机械臂系统控制模型，前馈输入计算信号，反馈补偿误差，对比关节角位移变化情况反映机械臂运动控制精度，为提高巡检机器人井下作业质量提供理论依据。1巡检机器人运动学分析1.1结构设
2、计为适应煤矿井下作业要求，完成巡检机器人结构设计，本体结构包含行走系统、机身、控制器、六自由度机械臂。机械臂包含基座、大臂、小臂、腕部及末端执行器，其中机械臂基座与机身相连，在SolidWorks软件搭建巡检机器人机械臂三维模型如图1所示。1.2巡检机器人正逆运动学求解采用Denavit和Hartenberg提出的运动参数表示机械臂建模方法，4个参数为关节角度、连杆长度、连杆偏移、连杆弯曲度，获取巡检机器人机械臂标准D-H参数如表1所示。煤矿巡检机器人动力学及独立关节控制研究*蔡玉强1，范祺1，贾思楠2（1.华北理工大学 机械工程学院，河北 唐山063210；2.华北理工大学 矿业工程学院，河
3、北 唐山063210）摘要：针对煤矿巡检机器人机械臂运动精度问题，提出结合前馈与反馈的独立关节控制方法。首先在SolidWords软件建立巡检机器人机械臂三维模型，获取机械臂D-H参数完成正逆运动学分析并在MATLAB软件机器人工具箱中软件建立机械臂空间模型；采用B样条曲线优化五次多项式插值方法完成巡检机器人轨迹规划，仿真得到运动过程中角位移、角速度、角加速度随时间变化关系；建立机械臂动力学方程，仿真得到关节运动所需力矩；结合前馈与反馈的独立关节控制方法实现机械臂运动控制，在Simulink平台下建立控制系统模型。关节位置运动控制效果对比发现机械臂按预期轨迹运动精度高，满足巡检机器人井下作业需
4、求。关键词：巡检机器人；轨迹规划；动力学；独立关节控制中图分类号：TP242文献标志码：A文章编号：1008 8725（2023）03 220 05Research on Dynamics and Independent Joint Control of Coal MineInspection RobotCAI Yuqiang1，FAN Qi1，JIA Sinan2(1.School of Mechanical Engineering,North China University of Science and Technology,Tangshan 063210,China;2.School
5、of Mining Engineering,North China University of Science and Technology,Tangshan 063210,China)Abstract:Aiming at the problem of the motion accuracy of the robot robot for coal mine inspection,anindependent joint control method combining feedforward and feedback is proposed.Firstly,the three-dimension
6、al model of the robot robot robot is established in SolidWords software,the D-H parametersof the robot arm are obtained to complete the positive and negative kinematic analysis,and the spacemodel of the robot arm is established in the software of MATLAB software robot toolbox;the B-splinecurve optim
7、ization five-time polynomial interpolation method is used to complete the trajectoryplanning of the inspection robot,and the relationship between angular displacement,angular velocityand angular acceleration during the movement is simulated;the dynamic equation of the robot arm isestablished,and the
8、 torque required for joint motion is simulated.Combining the independent jointcontrol method of feed-forward and feedback,the robot arm motion control is realized,and the controlsystem model is established under the Simulink platform.Comparing the motion control effects of thejoint position,it is fo
9、und that the robot arm has high movement accuracy according to the expectedtrajectory,which meets the needs of the underground operation of the inspection robot.Key words:inspection robot；trajectory planning；dynamics；independent joint control*河北省教育厅基金项目（2019GJJG216）；华北理工大学博士启动基金项目（BS2017094）220图1巡检机
10、器人机械臂三维模型1.关节驱动电机2.末端执行器3.腕部4.小臂5.大臂6.基座表1巡检机器人机械臂D-H参数表根据机械臂标准D-H参数，在MATLAB软件机器人工具箱中调用link类函数建立机械臂运动学空间模型如图2所示。图2巡检机器人机械臂MATLAB工具箱模型机械臂正运动学是在已知机器人各关节变量的前提下求解机械臂空间姿态和末端执行器相对于参考坐标系的位置。按照相邻关节坐标系递推得到首末关节坐标系变换矩阵6T=nxoxaxpxnyoyaypynzozazpz0 0 01（1）用关节角解释变换矩阵nxc1c23（c4c5c6s4s6）-c1s23s5s6+s1（s4c5c6+c4s6）ny
11、s1c23（c4c5c6s4s6）-s1s23s5s6-c1（s4c5c6+c4s6）nz-s23（c4c5c6s4s6）-c23s5s6ox-c1c23（c4c5c6+s4c6）+c1s23s5s6-s1（s4c5c6-c4c6）oy-s1c23（c4c5s6+s4c6）+s1s23s5s6-c1（s4c5s6-c4c6）ozs23（c4c5s6+s4c6）+c23s5s6pxa1c1+a2c1c2+a3c1c23+d4c1s23pya1s1+a2s1c2+a3s1c23-d4a1s23pz-a2s2-a3s23-d4c23（2）式中s1sin 1；c1cos 1；c23cos（1+1）；s
12、23sin（1+1）。逆运动学是已知机械臂末端位置时，反求出各个关节角度。用代数方法表示先将运动方程表述为6T=nxoxaxpxnyoyaypynzozazpz0 0 011T2T3T4T5T6T（3）根据已知位姿向量和连杆参数可反求出多组关节角逆解，部分解逆因关节活动范围无法实现，应在可用解中依据不同原则选择最优解以适应巡检机器人工作需求。在MATLAB软件机器人工具箱中调用fkine函数和ikine函数可实现运动学正逆解的快速求解，并与理论计算结果对比是否一致来验证模型建立的正确性。1.3巡检机器人运动轨迹规划机器人轨迹规划问题是在已知运动始末位置点和中间路径点的基础上，施加约束条件求解得
13、到运动轨迹特性。轨迹规划常采用空间轨迹规划方法中的五次多项式插值方法，其核心思想是建立五次多项式，限制速度和加速度条件完成对多项式的6个未知参数求解。在MATLAB机器人工具箱中调用jtraj函数完成巡检机器人空间轨迹规划，输出各关节角位移、角速度、角加速度与时间的函数关系来表示轨迹变化过程五次多项式插值方法适用于关节空间下不同位形间生成拟合运动轨迹，该方法进行轨迹规划仿真时能保证轨迹加速度曲线连续，但曲线存在突变现象是运动过程中机械臂产生冲击振动的主要原因。为解决这一问题提出用B样条曲线来优化五次多项式插值方法，B样条曲线的方程：C（u）=ni=0diNi，k（u）（4）式中u函数节点；di
14、控制节点；k基函数次数；Ni，kk次规范B样条基函数。用优化方法进行轨迹规划仿真，确定机械臂起始点和目标位置点，添加适当中间位置点，先进行五次多项式轨迹规划，合理选取控制节点，对加速度曲线突变区域进行B样条曲线优化。设置初始和终止速度为零，仿真时间10 s，时间间隔0.1 s。运动过程第42卷第03期Vol.42 No.03煤矿巡检机器人动力学及独立关节控制研究蔡玉强，等连杆123456i/（）0-900-9090-90i-1/（）020056013000f/（）90-900000di/mm00062000变量范围/（）180-90150-160200180135360000123451234
15、56-101YZ1.51.00.50-0.5-1.0-1.5巡检机器人X-101ZXY221中各关节角位移、角速度、角加速度随时间变化关系如图3所示。优化方法规划的轨迹保证了角位移曲线连续，加速度曲线无大幅突变，减少了运动过程中的冲击振动现象，为后续动力学计算及关节控制提供了理论依据。（a）角位移曲线（b）角速度曲线（c）角加速度曲线图3运动学轨迹规划仿真结果2巡检机器人动力学分析2.1巡检机器人动力学方程考虑到巡检机器人井下作业的特殊环境，在机械臂动力学分析中结合多种因素来分析运动过程与力矩间的关系。机器人动力学分为正动力学和逆动力学两类问题，正动力学问题是已知各关节的驱动力矩求解各关节位置
16、、速度、加速度；逆动力学问题是已知各关节某一位形的位置、速度、加速度，反求处各关节的驱动力矩大小。考虑到重力、摩擦力、向心力等多种因素的动力学微分方程：M（q）q+C（q，q）q+F（q）+N（q）=+J（q）Tf（5）式中q广义的关节位置；q广义的关节速度；q广义的关节加速度；M空间惯量矩阵；C科氏力和向心力耦合矩阵；F摩擦力；f摩擦因数；N重力；广义驱动力向量；J雅可比矩阵。2.2关键矩阵设置求解机械臂动力学问题要对关键矩阵进行求解设置，包括关节空间惯量矩阵、科氏矩阵、重力矩阵、摩擦力矩阵。机器人关节空间惯量矩阵是机械臂各关节的位姿函数，在不同位形时具有不同值。使用inertia函数来计算不同位形的关节空间惯量矩阵。科氏矩阵是关节坐标和关节速度的函数，是科氏力和向心力的耦合正定矩阵。使用coriolis函数来计算给定关节坐标和关节速度下的科氏矩阵。重力矩阵表现为关节所受重力，不受机器人运动影响，仅与机器人所处位形有关，在已知机器人位形的情况下使用gravload函数求解。摩擦力矩阵是由各关节的给定摩擦力参数值决定的，在运动过程中与各关节转速存在函数关系。在运动开始前主要受到静摩擦力
