双悬臂截割机器人相对动力学建模与力位混合控制研究

关键词：巷道掘进；双悬臂截割机器人；力位混合控制；相对动力学；相对雅可比矩阵

中图分类号：TD632 文献标志码：A

0引言

随着我国煤矿开采深度不断增加及煤炭赋存条件日益复杂化，现有的单纵轴断面成形工艺因可达范围有限，对于大断面无法一次截割成形，在应对挑战时显露出种种局限性[1]。对此，马宏伟等[2]提出了双悬臂快速掘进机器人系统方案，可实现大断面巷道的掘支锚运同步作业。该系统中，双悬臂截割机器人能够在不左右横移的情况下完成大断面巷道截割成形。双悬臂截割机器人的研究与应用主要面临2 个关键难题：① 双输出共运动导致统一建模难。在进行大断面截割时，移动平台需要同时配合左右截割臂产生的截割轨迹，移动平台对双臂提供的公共运动势必对两臂末端截割头的动态产生重要影响，两截割头输出导致难以建立机器人统一描述模型。② 双输出强交互导致截割控制难。双悬臂截割机器人通过两截割头同时对煤岩施加作用力实现大断面快速成形，双臂需同时满足规划的运动及对煤岩施加的力2 个方面要求。因此，双悬臂截割机器人与煤岩的动态交互给双悬臂截割机器人控制带来了严峻挑战。

为解决上述两大关键难题，需建立双悬臂截割机器人的动力学模型，并基于动力学模型设计控制器。目前关于双悬臂截割机器人动力学建模与截割控制的研究鲜见报道。双悬臂截割机器人通过两截割臂相互配合实现大断面成形，是一种典型的双臂机器人。在双臂机器人建模方面，A. Caballero 等[3]在规划器中引入双臂空中机器人系统的动力学模型来保证避障的可靠性。Shen Haoyu 等[4]基于解耦自然正交补的递归实现了双臂机器人的逆动力学建模。Wang Jian 等[5]分析了双臂协同机器人的动力学模型，求解了双臂机器人在搬运、装配和加工任务时的动力学特性。程靖等[6]使用拉格朗日方程和牛顿−欧拉法建立了双臂空间机器人和目标卫星的系统动力学模型，推导了闭链混合体系统的动力学模型。刘佳等[7]基于Udwadia−Kalaba 方程建模，克服了传统拉格朗日方程求解动力学方程需借助拉格朗日乘子的缺点。R. Jamisola 等[8]将单臂动力学模型与相对雅可比矩阵结合，建立了模块化双臂动力学模型。董楸煌等[9]研究了双臂空间机器人抓取目标的动力学控制，使用第二拉格朗日方程推导了机器人动力学模型。张建华等[10]基于矢量解析法和相对动力学模型推导了双臂机器人的协作动力学模型，将双臂独立的动力学整合为1 个整体。王登峰等[11]建立了关节一体化双臂机器人的动力学模型，并采用最小二乘法实现动力学模型参数辨识。对于双悬臂截割机器人而言，移动平台为机器人双臂提供公共运动，使得传统的动力学建模方法无法准确地同时描述两臂运动状态。

在双臂机器人控制方面，Jing Xin 等[12]提出了内/外循环混合控制策略，用于控制双臂机器人在抓取物体时冗余自由度的内部力。艾海平等[13]提出了减轻双臂空间机器人捕捉目标产生的冲击效应及保持系统稳定性的控制算法。Zhang Fuhai 等[14]设计了协调控制策略，用于根据空间平行弹簧和阻尼模型控制双臂机器人的相对臂姿态。Jiang Yiming 等[15]引入径向基函数神经网络来处理双臂机器人动力学参数的不确定性问题。Jiang Wei 等[16]设计了力/位置控制器，以减少双臂机器人形成闭环链路时产生的内部力对机器人可靠性的影响。赵明辉[17]设计了双臂并联式矸石分拣机器人，通过关节空间轨迹规划提高机器人分拣速度。李贺立等[18]设计了双臂机器人阻抗控制方法，克服了双臂机器人搬运柔性物体时协调性不足的问题。刘江文等[19]基于双臂机器人的动力学建模与伺服系统控制算法，设计了控制机器人关节转矩的控制器。郑晓薇等[20]提出了基于力同步的自适应阻抗控制方法，实现了双臂手术机器人对期望接触力的跟踪。上述研究主要以双臂接触同一对象形成运动闭链为前提。而双悬臂截割机器人的控制需要同时满足双臂运动及末端截割头输出力的要求，控制算法更为复杂。因此，如何将双臂整合为1 个系统进行控制尚待研究。

本文基于相对雅可比矩阵建立了统一描述机器人两臂动态的相对动力学模型，基于动力学模型设计了机器人力位混合控制系统，以实现对双臂运动和截割头输出力的同步控制。

1双悬臂截割机器人建模

双悬臂截割机器人采用全断面双纵轴结构，其模型如图1 所示。其单臂结构与EBZ200A 悬臂式掘进机（伸缩型）相同。2 个偏航关节的轴线间距为2 m，俯仰关节与偏航关节轴线间的距离为1m，伸长关节的伸缩行程为4.18～4.73 m，移动平台质量为120000kg，连接偏航关节与俯仰关节的连杆质量为10000 kg，伸长关节质量为5000 kg。机器人处于巷道最前端，且可在沿巷道掘进方向的滑轨上前进。在切割大断面时，两截割臂的工作相互协调，处于滑轨上的移动平台给左右截割臂提供公共运动，为双悬臂截割机器人提供冗余的自由度，使两截割臂同时实现在滑轨上的前后推移。在双臂截割大断面时，左右截割臂沿着规划的路径互相配合开挖巷道，移动平台根据实际情况推移机身。两截割臂下方的铲板用于收集碎岩。

4双悬臂截割机器人控制仿真

在实现大断面截割时，基于蒙特卡洛方法生成的单悬臂与双悬臂截割机器人产生的工作空间如图6所示。

从图6可看出，对于单悬臂截割机器人而言，因工作空间有限，大断面的截割需开挖2 条巷道才能完成，在完成一侧巷道截割后移机进行另一条巷道的截割[21]，工序复杂且效率低下。而双悬臂截割机器人的移动平台为2 个截割臂提供了公共运动，机器人拥有更大的工作空间，可在不移动机身的前提下一次性完成大断面截割。

为了使控制系统产生的解与实际工况下掘进机运动相符，假设移动平台的实际运动与期望运动相符，设置从静止状态开始加速运动，到达工作速度后保持匀速，两截割臂沿S 形轨迹进行异步截割。为了减小机器人截割过程中产生的动力学耦合效应，对机器人移动平台运动及双臂截割运动均进行五次多项式插值规划， 端点之间的移动时间为10 s，每秒被划分为10 个步长。五次多项式规划的边界条件：① 起点处移动平台速度和加速度均为0，到达S 形轨迹的第1 个拐点处时移动平台的速度为0.02 m/s，加速度为0。② 双臂各关节在S 形轨迹拐点处的速度和加速度均为0。

五次多项式规划后的机器人截割头运动轨迹如图7 所示，其中序号1−8 表示截割头运动轨迹拐点。当截割头处于S 形轨迹的拐点时，移动平台和各关节参数见表2。

由于双悬臂截割机器人在工作时不形成运动学闭链，仿真不考虑机器人在工作过程中的关节内力分配问题，即式（13）的内力项中为零矩阵。当双悬臂截割机器人处在静止状态时，移动平台的质心相对于基坐标系的位置为[1 m 0 0]T，此时两截割臂的偏航关节处于0 弧度。由于截割头自上而下截割，截割臂俯仰关节的初始角度和伸长关节的伸长量均为最大值，根据双悬臂截割机器人的相对运动学模型，两截割头的期望相对位置可由五次多项式规划的关节运动代入式（1）得出。期望的相对力设置为定值[255 N 240 N −25 N]^T。两末端截割头的初始相对位置为[−2.4271m 4.2039 m 2.025 8 m]^T。两截割头期望的相对位置变化如图8所示。

在力位混合控制系统的作用下，两臂对规划的轨迹进行跟踪，系统输出的机器人关节力如图9 所示。由于移动平台的运动被锁定为匀速前推，力位混合控制系统只产生了1组符合条件的解。由于两截割头需同时满足图7 中截割轨迹并保持期望的相对力，关节输出力将同时受到相对力控制环和相对控制位置环产生的力的影响而发生改变。当两截割头经过图7 中轨迹拐点时，各关节的输出力发生较大振荡。这是因为截割头运动经五次多项式规划后，处于截割轨迹拐点时两截割头相对移动平台是静止的，此时截割头仅输出沿x₀ 方向的力，在其他方向的力为0，截割头实际相对力与期望相对力存在较大误差，即相对力控制环中e_f 较大，导致关节输出力发生振荡。

根据控制系统输出的机器人各关节力矩，相对位置的控制结果如图10所示。可看出双悬臂截割机器人两截割头的实际相对位置对期望值保持了良好的跟踪，但实际相对位置在S 形轨迹的拐点处发生了不同程度的振荡，原因是期望的相对力为常数向量，而在轨迹拐点处，两截割头期望的绝对速度和加速度均为0，导致下一步长的两截割头输出力产生较大变化，以实现两截割头对期望相对力的跟踪。在S 形轨迹的拐点处，两截割头输出力发生的振荡通过图4 的相对力控制环转换为关节输出力，使力位混合控制系统输出的总关节力产生较大的变化，导致实际相对位置发生波动，即轨迹拐点处发生相对位置振荡。

系统对截割头相对力的控制结果如图11所示。在S形轨迹拐点处，两截割头的相对位置和相对力均发生不同程度的振荡，在控制系统作用下，两截割头的相对力依然能够收敛到期望值。

考虑双悬臂截割机器人两截割头的初始位置并不总是位于规划的截割轨迹上，且不同硬度的煤岩巷道所需截割输出力不同，为更好地模拟不同截割负载下的实际工况，令双悬臂截割机器人初始相对位置为随机向量[−3.4375 m 3.149 0 m 0.8631 m]T，期望相对力为随机向量[65 N 60 N 36 N]T，此时力位混合控制结果如图12 所示。可看出存在初始误差且期望相对力发生改变时，在30个步长内实际相对位置仍能振荡收敛至期望值附近，且实现了对期望相对力的同步跟踪。

对于双悬臂截割机器人的力位混合控制系统，定义绝对误差e 为在双臂对期望轨迹的跟踪过程中，每个步长下两截割头实际相对位置与期望相对位置之间误差向量最大值的二范数，即

经计算，力位混合控制系统对相对位置跟踪的绝对误差为0.3132m，均方根误差为0.1447 m。

综合仿真和计算结果可看出，力位混合控制系统能够有效跟踪截割头的期望相对位置，且对每个关节变量的期望值具有良好的跟踪效果。

5结论

1） 基于机器人相对雅可比矩阵建立双悬臂截割机器人模型，能将机器人双臂动力学整合为1 个整体，通过单一变量同时描述机器人双臂运动状态。

2） 基于双悬臂截割机器人相对动力学模型，提出机器人双臂力位混合控制方案，通过李雅普诺夫函数对系统进行稳定性分析，验证了系统的可行性。

3） 对双悬臂截割机器人结构及力位混合控制系统进行仿真分析，结果表明：双悬臂截割工艺较单悬臂截割拥有更大的工作空间，具有一次性实现大断面截割的能力，简化了大断面成形工序，具有更高的效率；力位混合控制系统能够完成对期望相对位置和期望相对力的同步控制，从而实现双臂对煤岩的截割，相对位置跟踪绝对误差在0.3132 m 以内，均方根误差为0.1447 m。