煤矿巡检机器人动力学及独立关节控制研究_蔡玉强.pdf

第42卷第03期2023年03月煤炭技术Coal TechnologyVol.42 No.03Mar.2023doi:10.13301/ki.ct.2023.03.0430前言随着煤矿井下掘进技术发展，对现代煤矿巷道机械设备要求不断提升，煤矿井下巡检机器人替代人工井下检测是大势所趋。巡检机器人作业质量取决于机械臂控制，机械臂运动学根据实际工作需求规划运动轨迹；动力学计算关节力矩是实现运动控制的基础；采用独立关节控制方法建立机械臂系统控制模型，前馈输入计算信号，反馈补偿误差，对比关节角位移变化情况反映机械臂运动控制精度，为提高巡检机器人井下作业质量提供理论依据。1巡检机器人运动学分析1.1结构设计为适应煤矿井下作业要求，完成巡检机器人结构设计，本体结构包含行走系统、机身、控制器、六自由度机械臂。机械臂包含基座、大臂、小臂、腕部及末端执行器，其中机械臂基座与机身相连，在SolidWorks软件搭建巡检机器人机械臂三维模型如图1所示。1.2巡检机器人正逆运动学求解采用Denavit和Hartenberg提出的运动参数表示机械臂建模方法，4个参数为关节角度、连杆长度、连杆偏移、连杆弯曲度，获取巡检机器人机械臂标准D-H参数如表1所示。煤矿巡检机器人动力学及独立关节控制研究*蔡玉强1，范祺1，贾思楠2（1.华北理工大学 机械工程学院，河北 唐山063210；2.华北理工大学 矿业工程学院，河北 唐山063210）摘要：针对煤矿巡检机器人机械臂运动精度问题，提出结合前馈与反馈的独立关节控制方法。首先在SolidWords软件建立巡检机器人机械臂三维模型，获取机械臂D-H参数完成正逆运动学分析并在MATLAB软件机器人工具箱中软件建立机械臂空间模型；采用B样条曲线优化五次多项式插值方法完成巡检机器人轨迹规划，仿真得到运动过程中角位移、角速度、角加速度随时间变化关系；建立机械臂动力学方程，仿真得到关节运动所需力矩；结合前馈与反馈的独立关节控制方法实现机械臂运动控制，在Simulink平台下建立控制系统模型。关节位置运动控制效果对比发现机械臂按预期轨迹运动精度高，满足巡检机器人井下作业需求。关键词：巡检机器人；轨迹规划；动力学；独立关节控制中图分类号：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 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-dimensional 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 optimization 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 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 found 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巡检机器人机械臂三维模型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）nys1c23（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）；s23sin（1+1）。逆运动学是已知机械臂末端位置时，反求出各个关节角度。用代数方法表示先将运动方程表述为6T=nxoxaxpxnyoyaypynzozazpz0 0 011T2T3T4T5T6T（3）根据已知位姿向量和连杆参数可反求出多组关节角逆解，部分解逆因关节活动范围无法实现，应在可用解中依据不同原则选择最优解以适应巡检机器人工作需求。在MATLAB软件机器人工具箱中调用fkine函数和ikine函数可实现运动学正逆解的快速求解，并与理论计算结果对比是否一致来验证模型建立的正确性。1.3巡检机器人运动轨迹规划机器人轨迹规划问题是在已知运动始末位置点和中间路径点的基础上，施加约束条件求解得到运动轨迹特性。轨迹规划常采用空间轨迹规划方法中的五次多项式插值方法，其核心思想是建立五次多项式，限制速度和加速度条件完成对多项式的6个未知参数求解。在MATLAB机器人工具箱中调用jtraj函数完成巡检机器人空间轨迹规划，输出各关节角位移、角速度、角加速度与时间的函数关系来表示轨迹变化过程五次多项式插值方法适用于关节空间下不同位形间生成拟合运动轨迹，该方法进行轨迹规划仿真时能保证轨迹加速度曲线连续，但曲线存在突变现象是运动过程中机械臂产生冲击振动的主要原因。为解决这一问题提出用B样条曲线来优化五次多项式插值方法，B样条曲线的方程：C（u）=ni=0diNi，k（u）（4）式中u函数节点；di控制节点；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-16020018013536000012345123456-101YZ1.51.00.50-0.5-1.0-1.5巡检机器人X-101ZXY221中各关节角位移、角速度、角加速度随时间变化关系如图3所示。优化方法规划的轨迹保证了角位移曲线连续，加速度曲线无大幅突变，减少了运动过程中的冲击振动现象，为后续动力学计算及关节控制提供了理论依据。（a）角位移曲线（b）角速度曲线（c）角加速度曲线图3运动学轨迹规划仿真结果2巡检机器人动力学分析2.1巡检机器人动力学方程考虑到巡检机器人井下作业的特殊环境，在机械臂动力学分析中结合多种因素来分析运动过程与力矩间的关系。机器人动力学分为正动力学和逆动力学两类问题，正动力学问题是已知各关节的驱动力矩求解各关节位置、速度、加速度；逆动力学问题是已知各关节某一位形的位置、速度、加速度，反求处各关节的驱动力矩大小。考虑到重力、摩擦力、向心力等多种因素的动力学微分方程：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函数求解。摩擦力矩阵是由各关节的给定摩擦力参数值决定的，在运动过程中与各关节转速存在函数关系。在运动开始前主要受到静摩擦力作用，运动开始后主要受到黏性摩擦力的作用。调用friction函数求解已知关节转速下的摩擦力大小。2.3动力学仿真动力学方程采用牛顿-欧拉法求解，利用递推思想，在连杆参数坐标系中从基座坐标向末端杆件迭代计算出各杆速度，加速度，再从末端杆件向基座坐标递推计算出各杆受力和力矩情况，所求力矩即为运动实现所需理论力矩。在MATLAB软件中，基于牛顿-欧拉法求解动力学问题采用SerialLink类函数。在2.3节给定的机械臂轨迹规划基础上对巡检机器人进行逆动力学分析，调用rne函数，结合考虑机械臂动力学关键信息，计算得到巡检机器人机械臂各关节运动所需力矩如图4所示，仿真结果作为各关节运动所需理论力矩值为后续关节控制工作做了准备。第42卷第03期煤矿巡检机器人动力学及独立关节控制研究蔡玉强，等Vol.42 No.03 123456789100时间/s角位移/rad876543210-1-2-3-4-5关节1关节2关节3关节4关节5关节6123456789100时间/s43210-1-2-3-4关节1关节2关节3关节4关节5关节6123456789100时间/s角加速度/rads-286420-2-4-6-8关节1关节2关节3关节4关节5关节6角速度/rads-1222图4逆动力学仿真结果3巡检机器人独立关节控制3.1关节驱动电机模型巡检机器人机械臂按照预期轨迹运动精度问题的关键在于关节控制。机械臂关节运动采用电机驱动，频域内电机关键参数构建方程组：T=Kmieb=Keddtea=Ldidt+Ri+ebT=Jmd2dt2+Bddt+Tci=ea-ebLs+R（6）式中T电机转矩，Nm；i电机电流，A；Km电机扭矩常数；eb反电动势，V；ea电枢电压，V；Ke反电动势常数；L电感，H；电枢转角，（）；Jm相对转动惯量；B相对黏性摩擦因数；Tc库伦摩擦力矩，Nm。3.2关节控制模型独立关节控制方法是通过保证每一个关节运动精度来实现机械臂控制。采用PID控制器进行双闭环控制，外环为位置环，内环为速度环。外环保证位置精度并为内环反馈误差信息，从而内环获得控制最小误差的速度要求；内环利用PID控制器保证电机电枢电压，输出适合的驱动力矩来实现实际速度紧密追随需求速度的要求。控制系统采用前馈-反馈控制，前馈控制输入计算所得信号，反馈控制补偿其他误差源。添加极限力矩饱和器在控制系统内部模拟最大转矩。这样通过逆动力学求解得到的理论关节力矩作为前馈输入，结合反馈补偿和内部模拟，有利于减少信号扰动和实现时间最优控制。单关节位置控制模型如图5所示。第42卷第03期Vol.42 No.03煤矿巡检机器人动力学及独立关节控制研究蔡玉强，等3.3控制系统仿真机器人运动仿真按照前文轨迹过程，先建立独立关节控制模型下的电机驱动模型，封装子系统得到PIDcontrol模块，将运动学轨迹规划中的信息作为输入，逆动力学计算得到的理论关节力矩作为前馈信息添加到控制模型中。构建得到完整Simulink系统控制模型如图6所示。设置仿真时长10 s，时间步数间隔为0.03 s，反复调节PID控制参数并输出仿真结果为关节1预期运动位置与实际控制位置结果对比如图7所示。仿真结果发现，独立关节控制机械臂运动轨迹与目标轨迹拟合程度较好，该控制模型可实现机械臂运动的精准控制，满足巡检机器人工作需求。4结语（1）完成巡检机器人机械臂设计，根据D-H参数完成机械臂空间建模，进行运动学理论推导；完成B样条曲线优化五次多项式方法的轨迹规划。（2）建立机械臂动力学方程，设置求解信息，完成逆动力学仿真得到轨迹运动实现所需的关节力矩。（3）采用独立关节控制方法实现机械臂运动控制，在Simulink平台建立系统控制模型，仿真发现关节位置精度控制效果较好，满足巡检机器人实际作业需求。图5独立关节控制模型123456789100时间/s关节力矩/Nm6050403020100-10-20关节1关节2关节3关节4关节5关节6关节位置关节速度2tau_fKnum（s）1qPID（s）+PID（s）den（s）1/Ls+R+num（s）den（s）1KmKe1/Jm+Bs1q_ref极限力矩223图7控制结果与预期对比参考文献：1张文典,黄家才,胡凯.基于Matlab的机器人轨迹仿真及关节控制J.制造技术与机床,2020(5):54-58.2王成军,王智慧,金力,等.双臂型井下巡检机器人动力学分析J.煤炭技术,2021,40(11):181-185.3方建文,晁永生,袁逸萍.改进B样条插值法的焊接机器人关节轨迹优化J.机械设计与制造,2021(10):1-4,10.4崔国华,冀霖,张智涛,等.结合前馈与反馈控制机器人伺服系统研究J.机械设计与制造,2021(10):246-249,254.作者简介：蔡玉强（1967-），河北唐山人，教授，博士，硕士生导师，研究方向：机械动力学与机器人学，电子信箱：.责任编辑：李景奇收稿日期：20220415第42卷第03期煤矿巡检机器人动力学及独立关节控制研究蔡玉强，等Vol.42 No.03图6控制系统Simulink模型123456789100时间/s43210-1预期运动位置实际控制位置角位移/radtrajectorytoqqddtqd66Flnverse DynamicsqJ_AFJ_TcnConfigJ_VTcr_fq_refq1PID control1qTcr_fq_refPID control2qTcr_fq_refPID control3qTcr_fq_refPID control4qTcr_fq_refPID control5qTcr_fq_refPID control6q2q3q4q5q6224