Intelligent transversely moving belt conveyor for working face of Chaoyang Open-pit Coal Mine
-
摘要: 针对朝阳露天煤矿水文地质条件复杂，造成目前采用的挖掘机+自卸卡车+破碎站+带式输送机半连续采煤工艺平均每年停产约100多天，并存在安全风险大、生产成本高、连续性差、自动化程度低、产量调控困难等问题，指出工作面采用带式输送机运输才能满足智能化矿山建设发展需求。针对现有带式输送机由于存在拖移困难、拆装困难，均不能应用于朝阳露天煤矿采煤工作面的问题，研发了一种全新的横移带式输送机。该输送机通过前端配套转载机+斗轮机/连采机等，后端配套可伸缩带式输送机+端帮带式输送机+破碎站等可形成连续采煤工艺；通过挖掘机+转载机+横移带式输送机+可伸缩带式输送机+端帮带式输送机+破碎站等形成半连续采煤工艺，该采煤工艺无需自卸卡车等中间环节，直接将原煤装载到带式输送机上。
-
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，俯仰关节与偏航关节轴线间的距离为1 m，伸长关节的伸缩行程为4.18～4.73 m，移动平台质量为120 000 kg，连接偏航关节与俯仰关节的连杆质量为10 000 kg，伸长关节质量为5 000 kg。机器人处于巷道最前端，且可在沿巷道掘进方向的滑轨上前进。在切割大断面时，两截割臂的工作相互协调，处于滑轨上的移动平台给左右截割臂提供公共运动，为双悬臂截割机器人提供冗余的自由度，使两截割臂同时实现在滑轨上的前后推移。在双臂截割大断面时，左右截割臂沿着规划的路径互相配合开挖巷道，移动平台根据实际情况推移机身。两截割臂下方的铲板用于收集碎岩。
2. 双悬臂截割机器人相对动力学建模
2.1 相对雅可比矩阵
双悬臂截割机器人模型的D−H结构如图2所示。由于两臂关于滑轨轴线对称，移动平台可视作同时连接2个偏航关节和基坐标系的虚拟连杆。o0x0y0z0为基坐标系；oixiyizi为关节坐标系，i=1，2$, \cdots , $ 6，分别代表左偏航关节、左俯仰关节、左伸长关节、右偏航关节、右俯仰关节、右伸长关节；v为移动平台前进速度；l1为俯仰关节与偏航关节轴线间的距离；a1，a4为虚拟连杆长度，表示移动平台运动。
双悬臂截割机器人的D−H参数见表1。其中θi为xi轴与xi−1轴之间的夹角；di为xi轴与xi−1轴之间的距离；ai为zi轴与zi−1轴之间的距离；$ {\alpha _i} $为zi轴与zi−1轴之间的夹角。
表 1 双悬臂截割机器人的D−H参数Table 1. D-H parameters of dual-arm cutting robot关节编号 θi/rad di/m ai/m αi/rad i=1 [0, π/2] 0 a1 0 i=2 [−π/2,0] 0 1 π/2 i=3 [0, π/2] 0 [4.18，4.73] 0 i=4 [−π/2,0] 0 a4 0 i=5 [0, π/2] 0 1 π/2 i=6 [0, π/2] 0 [4.18，4.73] 0 双悬臂截割机器人的截割头相对于基坐标系o0x0y0z0的运动传递有2种表达方式：① o6x6y6z6相对于o0x0y0z0变换，即沿着o6x6y6z6所在的机械臂向o0x0y0z0方向传递，即6→5→4→0。② o6x6y6z6沿着o3x3y3z3传递至o0x0y0z0，即6→3→2→1→0。根据图1及表1，可求得旋转变换及旋转平移复合变换等式：
$$ \left\{\begin{aligned} & ^0 \boldsymbol{R}_3^3\boldsymbol{R}_6=^0 \boldsymbol{R}_4^4\boldsymbol{R}_6 \\ & ^0 \boldsymbol{X}_3+^0 \boldsymbol{R}_3^3\boldsymbol{X}_6=^0 \boldsymbol{X}_6\end{aligned}\right. $$ (1) 式中：$ {}^j{{\boldsymbol{R}}_i} $为坐标系oixiyizi相对于坐标系ojxjyjzj的旋转矩阵，j = 0,1$,\;\cdots, \;$6；$ ^j\boldsymbol{X}_i $为oi在坐标系ojxjyjzj下的位置。
对式（1）求导，得到两截割头的相对角速度及相对速度：
$$ \left\{ \begin{aligned} & {}^3{{\dot {\boldsymbol{R}}}_6} = {}^3{{\dot {\boldsymbol{R}}}_0}{}^0{{\boldsymbol{R}}_6} + {}^3{{\boldsymbol{R}}_0}{}^0{{\dot {\boldsymbol{R}}}_6} \\ &{{{\dot {\boldsymbol{X}}}_R} = {}^3{{\dot {\boldsymbol{R}}}_0}{}^0{{\boldsymbol{X}}_6} + {}^3{{\boldsymbol{R}}_0}{}^0{{\dot {\boldsymbol{X}}}_6} - {}^3{{\dot {\boldsymbol{R}}}_0}{}^0{{\boldsymbol{X}}_3} - } {}^3{{\boldsymbol{R}}_0}{}^0{{\dot {\boldsymbol{X}}}_3} \end{aligned}\right. $$ (2) 式中：$ {\dot {\boldsymbol{X}}_R} $为两截割头的相对速度；$ {}^j{\dot {\boldsymbol{X}}_i} $为oi在坐标系ojxjyjzj下的速度。
根据旋转矩阵理论，式（2）可写为
$$ \left\{ \begin{aligned} &{\boldsymbol{S}}\left( {{{\boldsymbol{\omega}} _{\mathrm{R}}}} \right){}^3{{\boldsymbol{R}}_6} = {\boldsymbol{S}}\left( {{}^3{{\boldsymbol{\omega}} _0}} \right){}^3{{\boldsymbol{R}}_0}{}^0{{\boldsymbol{R}}_6} + {}^3{{\boldsymbol{R}}_0}{\boldsymbol{S}}\left( {{}^0{{\boldsymbol{\omega}} _6}} \right){}^0{{\boldsymbol{R}}_6} \\ & {{{\dot {\boldsymbol{X}}}_R} { = - {\boldsymbol{S}}\left( {{}^3{{\boldsymbol{R}}_0}{}^0{{\boldsymbol{X}}_6}} \right){}^3{{\boldsymbol{\omega}} _0} + {}^3{{\boldsymbol{X}}_0}{}^0{{\dot {\boldsymbol{X}}}_6} + {\boldsymbol{S}}\left( {{}^3{{\boldsymbol{R}}_0}{}^0{{\boldsymbol{X}}_3}} \right){}^3{{\boldsymbol{\omega}} _0} - } {}^3{{\boldsymbol{R}}_0}{}^0{{\dot {\boldsymbol{X}}}_3}} \end{aligned}\right. $$ (3) 式中：$ {{\boldsymbol{\omega}} _{\mathrm{R}}} $为两截割头的相对角速度，$ {{\boldsymbol{\omega}} _{\mathrm{R}}}{\text{ = }} $ $ \left[ \begin{array}{*{20}{c}} {{{\boldsymbol{\omega}} _{{\mathrm{Rx}}}}}& {{{\boldsymbol{\omega}} _{{\mathrm{Ry}}}}}& {{{\boldsymbol{\omega}} _{{\mathrm{Rz}}}}} \end{array} \right]^{\mathrm{T}} $；S（·）为替换向量叉乘运算的斜对称矩阵，$ {\boldsymbol{S}}\left( {{{\boldsymbol{\omega}} _{\mathrm{R}}}} \right) = \left[ {\begin{array}{*{20}{c}} 0&{ - {{\boldsymbol{\omega}} _{{\mathrm{R}}{\textit{z}}}}}&{{{\boldsymbol{\omega}} _{{\mathrm{R}}y}}} \\ {{{\boldsymbol{\omega}} _{{\mathrm{R}}{\textit{z}}}}}&0&{ - {{\boldsymbol{\omega}} _{{\mathrm{R}}x}}} \\ { - {{\boldsymbol{\omega}} _{{\mathrm{R}}y}}}&{{{\boldsymbol{\omega }}_{{\mathrm{R}}x}}}&0 \end{array}} \right] $；$ {}^n{{\boldsymbol{\omega}} _m} $为坐标系omxmymzm相对于坐标系onxnynzn的角速度，$m,n = 0, 1 ,\cdots ,6 $。
在旋转矩阵理论中存在以下关系：
$$ {}^j{{\boldsymbol{R}}_i}{\boldsymbol{S}}\left( {{{\boldsymbol{\omega}} _{\mathrm{R}}}} \right){}^j{{\boldsymbol{R}}_i^{\mathrm{T}}} = {\boldsymbol{S}}\left( {{}^j{{\boldsymbol{R}}_i}{{\boldsymbol{\omega}} _{\mathrm{R}}}} \right) $$ (4) 将式（4）代入式（3）并化简，得
$$ \begin{split} \left[ {\begin{array}{*{20}{c}} {{{\dot {\boldsymbol{x}}}_R}} \\ {{{\boldsymbol{\omega}} _{\mathrm{R}}}} \end{array}} \right] =& \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{S}}\left( {{}^3{{\boldsymbol{X}}_6}} \right){}^3{{\boldsymbol{R}}_0}{{\boldsymbol{J}}_{{\text{ωL}}}}{{\dot {\boldsymbol{q}}}_{\mathrm{L}}} + {}^3{{\boldsymbol{R}}_0}{}^0{{\dot {\boldsymbol{X}}}_6} - {}^3{{\boldsymbol{R}}_0}{}^0{{\dot {\boldsymbol{X}}}_3}} \\ { - {}^3{{\boldsymbol{R}}_0}{{\boldsymbol{J}}_{{\text{ωL}}}}{{\dot {\boldsymbol{q}}}_{\mathrm{L}}} + {}^3{{\boldsymbol{R}}_0}{{\boldsymbol{J}}_{{\text{ωR}}}}{{\dot {\boldsymbol{q}}}_{\mathrm{R}}}} \end{array}} \right] =\\ & \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{\varPsi}} _{\mathrm{R}}}{}^3{{\boldsymbol{\varOmega}} _0}{{\boldsymbol{J}}_{\mathrm{L}}}}&{{}^3{{\boldsymbol{\varOmega}} _0}{{\boldsymbol{J}}_{{\mathrm{R}}1}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{{\dot {\boldsymbol{q}}}_{\mathrm{L}}}} \\ {{{\dot {\boldsymbol{q}}}_{\mathrm{R}}}} \end{array}} \right] \end{split} $$ (5) 式中：JωL，JωR分别为左右截割臂的角速度雅克比矩阵；JL，JR1分别为左右截割臂的雅可比矩阵；$ {\dot {\boldsymbol{q}}_{\mathrm{L}}} $，$ {\dot {\boldsymbol{q}}_{\mathrm{R}}} $分别为左右截割臂的关节速度向量；${{\boldsymbol{\varPsi}} _{\mathrm{R}}}{\text{ = }}\left[ {\begin{array}{*{20}{c}} {\boldsymbol{I}}&{ - {\boldsymbol{S}}\left( {{{\boldsymbol{X}}_{\mathrm{R}}}} \right)} \\ {\boldsymbol{0}}&{\boldsymbol{I}} \end{array}} \right] $，I为单位矩阵，${\boldsymbol{X}}_{\mathrm{R}} $为两截割头实际相对位置；${}^3{{\boldsymbol{\varOmega}} _0}{\text{ = }}\left[ {\begin{array}{*{20}{c}} {{}^3{{\boldsymbol{R}}_0}}&{\boldsymbol{0}} \\ {\boldsymbol{0}}&{{}^3{{\boldsymbol{R}}_0}} \end{array}} \right] $。
双悬臂截割机器人的相对雅可比矩阵为
$$ {{\boldsymbol{J}}_{\mathrm{R}}} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{\varPsi}} _{\mathrm{R}}}{}^3{{\boldsymbol{\varOmega}} _0}{{\boldsymbol{J}}_{\mathrm{L}}}}&{{}^3{{\boldsymbol{\varOmega}} _0}{{\boldsymbol{J}}_{{\mathrm{R}}1}}} \end{array}} \right] $$ (6) 式（6）建立了双悬臂截割机器人两臂之间的联系，描述了两臂末端截割头的相对运动关系，即
$$ {\dot {\boldsymbol{X}}_{\mathrm{R}}} = {{\boldsymbol{J}}_{\mathrm{R}}}\dot {\boldsymbol{q}} $$ (7) 式中：$ {\dot {\boldsymbol{X}}_{\mathrm{R}}} $为两臂末端截割头的相对速度；$ \dot {\boldsymbol{q}} $为机器人关节空间速度。
式（7）即双悬臂截割机器人的相对运动学模型。该模型通过截割头的相对运动，将两臂的运动学整合为1个整体，为相对动力学建模提供了理论支持。
2.2 相对动力学模型
由于机器人的2个偏航关节相对于移动底座质心的位置关于基坐标系的x0轴对称，所以2个偏航关节坐标系o1x1y1z1和o4x4y4z4相对于基坐标系的旋转变换，即绕z0轴的旋转角度符号相反、大小相等。另外，本研究未涉及双臂对基座的动态耦合效应，因此可在数值上用q1代替q4。根据图2，通过拉格朗日动力学方程建立双悬臂截割机器人的动力学模型。
$$ {{{\tau}} _i}{\text{ = }}\frac{{\text{d}}}{{{\text{d}}t}}\left( {\frac{{\partial L}}{{\partial {{\dot {{q}}}_i}}}} \right) - \frac{{\partial L}}{{\partial {{\dot {{q}}}_i}}} $$ (8) 式中：$ {\tau _i}$为机器人关节i产生的力矩；t为时间；L=K−P，K为系统的总动能，P为系统的总势能；qi为机器人关节i参数。
将双悬臂截割机器人的连杆视作匀质杆，计算连杆微元的动能并进行积分，代入式（8）并化简，求得双悬臂截割机器人的动力学模型：
$$ {\boldsymbol{M}}\left( {\boldsymbol{q}} \right)\ddot {\boldsymbol{q}} + {\boldsymbol{H}}\left( {{\boldsymbol{q}},\dot {\boldsymbol{q}}} \right) + {\boldsymbol{G}}\left( {\boldsymbol{q}} \right) = {\boldsymbol{\tau}} $$ (9) 式中：$ {\boldsymbol{M}}\left( {\boldsymbol{q}} \right) $为双悬臂截割机器人对称、正定的惯性矩阵，$ {\boldsymbol{q}}=[q_1 \;\;\; q_2 \;\;\;\cdots \;\;\;q_i]^{\mathrm{T}}$；$ \ddot {\boldsymbol{q}} $为机器人关节加速度；$ {\boldsymbol{H}}\left( {{\boldsymbol{q}},\dot {\boldsymbol{q}}} \right) $为科氏力、离心力组成的广义力矢量；$ {\boldsymbol{G}}\left( {\boldsymbol{q}} \right) $为机器人所受的重力矩。
对式（7）两侧取微分，得
$$ {\text{δ}} {{\boldsymbol{X}}_{\mathrm{R}}} = {{\boldsymbol{J}}_{\mathrm{R}}}{\text{δ}} {\boldsymbol{q}} $$ (10) 式中$ {\text{δ}} {{\boldsymbol{X}}_{\mathrm{R}}} $，$ {\text{δ}} {\boldsymbol{q}} $分别为两截割头相对位置和关节变量发生的微小位移。
根据虚位移与虚功原理，双悬臂截割机器人各关节所作的虚功之和与两截割头相对力FR所做的虚功相等，即
$$ {{\boldsymbol{\tau}} ^{\mathrm{T}}}{\text{δ}} {\boldsymbol{q}} = {{\boldsymbol{F}}_{\mathrm{R}}^{\text{T}}}{\text{δ}} {{\boldsymbol{X}}_{\mathrm{R}}} $$ (11) 式中${\boldsymbol{\tau}} $为机器人关节力矩，${\boldsymbol{\tau}}=[\tau_1 \;\;\;\tau _2 \;\;\;\cdots \;\;\;\tau _i] $。
将式（11）代入式（9），得
$$ {\boldsymbol{M}}\left( {\boldsymbol{q}} \right)\ddot {\boldsymbol{q}} + {\boldsymbol{H}}\left( {{\boldsymbol{q}},\dot {\boldsymbol{q}}} \right) + {\boldsymbol{G}}\left( {\boldsymbol{q}} \right){\text{ = }}{\boldsymbol{\tau}} {\text{ = }}{{\boldsymbol{J}}_{\mathrm{R}}^{\mathrm{T}}}{{\boldsymbol{F}}_{\mathrm{R}}} $$ (12) 基于广义逆定理，式（12）可写为
$$ {{\boldsymbol{F}}_{\mathrm{R}}}{\text{ = }}{\left( {{{\boldsymbol{J}}_{\mathrm{R}}^{\mathrm{T}}}} \right)^ + }\left[ {{\boldsymbol{M}}\left( {\boldsymbol{q}} \right)\ddot {\boldsymbol{q}} + {\boldsymbol{H}}\left( {{\boldsymbol{q}},\dot {\boldsymbol{q}}} \right) + {\boldsymbol{G}}\left( {\boldsymbol{q}} \right)} \right] + \left[ {{\boldsymbol{I}} - {{\left( {{\boldsymbol{J}}_{\mathrm{R}}^{\mathrm{T}}} \right)}^ + }{\boldsymbol{J}}_{\mathrm{R}}^{\mathrm{T}}} \right]{\boldsymbol{\delta}} $$ (13) 式（13）中$ \left[ {{\boldsymbol{I}} - {{\left( {{\boldsymbol{J}}_{\mathrm{R}}^{\mathrm{T}}} \right)}^ + }{\boldsymbol{J}}_{\mathrm{R}}^{\mathrm{T}}} \right]{\boldsymbol{\delta}} $为动力学方程的内力项，该矩阵产生的机器人内力不做功，在研究系统内力时需对该式展开讨论，具体分析过程本文不做论述。
式（13）为双悬臂截割机器人两截割头相对力的表达式，即双悬臂截割机器人相对动力学模型。该模型可直接通过机器人的关节运动得到此时的末端截割头相对力。此外，通过截割头相对力这一唯一变量，相对动力学模型能够同时描述两截割臂的运动状态，将两臂的运动整合为1个整体。
3. 双悬臂截割机器人力位混合控制
3.1 相对位置控制
根据式（11）建立的关节空间微分运动与截割头相对位置微分运动的映射关系，在截割头实际相对位置和期望相对位置产生误差时，通过相对雅可比矩阵将笛卡尔空间的相对位置误差转换为关节空间的关节变量误差，再通过PID算法将关节变量误差转换为关节输出的力矩。相对位置控制器的参考值为两截割头期望相对位置XRd，在产生关节力矩$ {{\boldsymbol{\tau}} _{\mathrm{p}}} $后，通过双悬臂截割机器人的动力学模型求出此时的关节变量，并通过机器人的正相对运动学模型求得反馈后的两截割头相对位置。相对位置控制器如图3所示。XRd为两截割头在笛卡尔空间中的期望相对位置；E为相对位置与相对力通道选择矩阵；$ {\boldsymbol{J}}_{\mathrm{R}}^ + $为双悬臂截割机器人相对雅可比矩阵的伪逆；kpd为相对位置控制的比例−微分系数矩阵；T为双悬臂截割机器人的正向相对运动学模型，即式（1）中$ ^3\boldsymbol{X}_6 $求得的截割头实际相对位置。
![]()
图 3 基于相对动力学模型的双悬臂截割机器人相对位置控制器Figure 3. Relative position controller for dual-arm cutting robot based on relative dynamics model根据图3，可求得$ {{\boldsymbol{\tau}} } _{\mathrm{p}}$的表达式：
$$ {{\boldsymbol{\tau}} }_{\mathrm{p}} = {{\boldsymbol{k}}_{{\mathrm{pd}}}}{\boldsymbol{J}}_{\mathrm{R}}^ + {\boldsymbol{E}}\left( {{{\boldsymbol{X}}_{{\mathrm{Rd}}}} - {\boldsymbol{T}}} \right) $$ (14) 相对位置控制是基于双悬臂截割机器人的相对雅可比矩阵，将机器人末端截割头相对位置的误差通过雅可比矩阵的伪逆转换到关节空间，即计算关节角度/长度的误差。该误差由PD控制器整定后作为关节力矩，其中比例环节用于增加系统反馈速度，减小误差，而微分环节能够减少控制时间，改善机器人在轨迹跟踪时的动态性能。由于截割时要求机器人对岩壁施加足够的力以完成对岩壁的破坏，所以仅在自由空间中对双悬臂截割机器人的相对位置控制是不充分的，对于末端施加在岩壁上的力需引入其他控制器。
3.2 相对力控制
相对力控制系统将两截割头期望的相对力与实际相对力的误差通过相对雅可比矩阵的转置转换为关节空间力的误差，经PI控制器将关节空间力的误差转换为关节力矩${{\boldsymbol{\tau}} _{\mathrm{f}}} $。相对力控制器如图4所示。FRd为两截割头的期望相对力；FR为两截割头的实际相对力；E1为相对位置与相对力通道选择矩阵；$ {{\boldsymbol{k}}_{{\mathrm{fp}}}} $为相对力控制的比例系数矩阵；$ \dfrac{{{{\boldsymbol{k}}_{{\mathrm{fi}}}}}}{s} $为相对力控制的积分系数矩阵，s为拉普拉斯算子。
![]()
图 4 基于相对动力学模型的双悬臂截割机器人相对力控制器Figure 4. Relative force controller for dual-arm cutting robot based on relative dynamics model根据图4，可求得$ {{\boldsymbol{\tau}} _{\mathrm{f}}} $的表达式：
$$ {{\boldsymbol{\tau}} _{\mathrm{f}}} = \left( {{{\boldsymbol{k}}_{{\mathrm{fp}}}} + \frac{{{{\boldsymbol{k}}_{{\mathrm{fi}}}}}}{s}} \right){\boldsymbol{J}}_{\mathrm{R}}^{\mathrm{T}}\left( {{\boldsymbol{I}} - {\boldsymbol{E}}_1} \right)\left( {{{\boldsymbol{F}}_{{\mathrm{Rd}}}} - {\boldsymbol{F}}_{\mathrm{R}}} \right) $$ (15) 3.3 力位混合控制
由于双悬臂截割机器人的关节和连杆具有较大质量，考虑到两臂产生的动态影响，需对两臂受到的重力矩进行补偿。这部分的关节空间力矩为
$$ {{\boldsymbol{\tau}} _{\mathrm{h}}} = \hat {\boldsymbol{G}}\left( {\boldsymbol{q}} \right) $$ (16) 式中$\hat {\boldsymbol{G}}\left( {\boldsymbol{q}} \right) $为机器人所受重力矩的补偿。
根据图3和图4，双悬臂截割机器人的关节输出力矩为2个控制器输出力矩及重力补偿力矩之和，即
$$ {\boldsymbol{\tau}} = {{\boldsymbol{\tau}} _{\mathrm{p}}} + {{\boldsymbol{\tau}} _{\mathrm{f}}} + {{\boldsymbol{\tau}} _{\mathrm{h}}} $$ (17) 双悬臂截割机器人力位混合控制系统如图5所示。
![]()
图 5 基于相对动力学模型的双悬臂截割机器人力位混合控制系统Figure 5. Force-position hybrid control system for dual-arm cutting robot based on relative dynamics model基于相对雅可比矩阵建立的双悬臂截割机器人的力位混合控制系统虽然能够控制双臂的关节输出力矩，但在系统受到外界干扰时，恢复平衡状态的能力未知，即该系统的稳定性不确定。因此，需要对系统稳定性进行数学验证。
3.4 控制系统稳定性分析
在力位混合控制系统中，实对称矩阵为机器人的PID系数矩阵，因此，选取该矩阵构造控制系统的能量函数。考虑以下正定李雅普诺夫函数：
$$ {\boldsymbol{V}} = \frac{1}{2}\left[ {{\boldsymbol{e}}_{\mathrm{x}}^{\mathrm{T}}{{\boldsymbol{k}}_{{\mathrm{pd}}}}{{\boldsymbol{e}}_{\mathrm{x}}} + {{\dot {\boldsymbol{q}}}^{\mathrm{T}}}{\boldsymbol{M}}({\boldsymbol{q}})\dot {\boldsymbol{q}}} \right] $$ (18) 式中：V为机器人系统的总能量；ex为两截割头实际相对位置与期望相对位置的误差，ex=XRd−XR。
对式（18）求导并化简，得
$$ \dot {\boldsymbol{V}} = {\dot {\boldsymbol{e}}_{\mathrm{x}}^{\mathrm{T}}}{{\boldsymbol{k}}_{{\mathrm{pd}}}}{{\boldsymbol{e}}_{\mathrm{x}}} + {\dot {\boldsymbol{q}}^{\mathrm{T}}}{\boldsymbol{M}}\left( {\boldsymbol{q}} \right)\ddot {\boldsymbol{q}} $$ (19) 如果力位混合控制系统对双悬臂截割机器人所受的重力矩估计准确，且能够完全抵消式（9）中动力学模型的重力项，则将式（9）代入式（19），得
$$ \begin{split} {\boldsymbol{M}}\left( {\boldsymbol{q}} \right)\ddot {\boldsymbol{q}} + &{\boldsymbol{H}}\left( {{\boldsymbol{q}},\dot {\boldsymbol{q}}} \right) = {{\boldsymbol{k}}_{{\mathrm{pd}}}}{{\boldsymbol{J}}_{\mathrm{R}}^ +} {\boldsymbol{E}}\left[ {{{\boldsymbol{X}}_{{\mathrm{Rd}}}} - {\boldsymbol{T}}} \right] +\\ & \left( {{{\boldsymbol{k}}_{{\mathrm{fp}}}} + \frac{{{{\boldsymbol{k}}_{{\mathrm{fi}}}}}}{s}} \right){\boldsymbol{J}}_{\mathrm{R}}^{\mathrm{T}}\left( {{\boldsymbol{I}} - {\boldsymbol{E}}_1} \right)\left( {{{\boldsymbol{F}}_{{\mathrm{Rd}}}} - {{\boldsymbol{F}}_{\mathrm{R}}}} \right) \end{split} $$ (20) 将式（20）代入式（19），能量函数的导数可化简为
$$ \dot {\boldsymbol{V}} = {{\dot {\boldsymbol{e}}}_{\mathrm{x}}^{\mathrm{T}}}{{\boldsymbol{k}}_{{\mathrm{pd}}}}{{\boldsymbol{e}}_{\mathrm{x}}} + {{\dot {\boldsymbol{q}}}^{\mathrm{T}}}\left[ \begin{gathered} {{\boldsymbol{k}}_{{\mathrm{pd}}}}{{\boldsymbol{J}}_{\mathrm{R}}^ +} {\boldsymbol{E}}{{\boldsymbol{e}}_{\mathrm{x}}} + {{\boldsymbol{k}}_{{\mathrm{pd}}}}{{\boldsymbol{J}}_{\mathrm{R}}^ +} {\boldsymbol{E}}{{\dot {\boldsymbol{e}}}_{\mathrm{x}}} \\ + {{\boldsymbol{k}}_2}{\boldsymbol{J}}_{\mathrm{R}}^{\mathrm{T}}\left( {{\boldsymbol{I}} - {\boldsymbol{E}}_1} \right){{\boldsymbol{e}}_{\mathrm{f}}} - {\boldsymbol{H}}\left( {{\boldsymbol{q}},\dot {\boldsymbol{q}}} \right) \\ \end{gathered} \right] \\ $$ (21) 式中：$ {{\boldsymbol{k}}_2} = {{\boldsymbol{k}}_{{\mathrm{fp}}}} + \dfrac{{{{\boldsymbol{k}}_{{\mathrm{fi}}}}}}{s} $为相对力控制环的PI系数矩阵；$ {{\boldsymbol{e}}_{\mathrm{f}}} $为两截割头的相对力误差，$ {{\boldsymbol{e}}_{\mathrm{f}}} = {{\boldsymbol{F}}_{{\mathrm{Rd}}}} - {{\boldsymbol{F}}_{\mathrm{R}}} $。
当两截割头期望的相对位置是一个由常数构成的向量时，$ {\dot {\boldsymbol{e}}_{\mathrm{x}}}^{\mathrm{T}} $可表示为
$$ {\dot {\boldsymbol{e}}_{\mathrm{x}}^{\mathrm{T}}}{\text{ = }}{\left( {{{\dot {\boldsymbol{X}}}_{{\mathrm{Rd}}}} - {{\dot {\boldsymbol{X}}}_{\mathrm{R}}}} \right)^{\mathrm{T}}}{\text{ = }} - {\dot {\boldsymbol{q}}^{\mathrm{T}}}{{\boldsymbol{J}}_{\mathrm{R}}^{\mathrm{T}}} $$ (22) 将式（22）代入式（21），系统能量函数的导数$ \dot {\boldsymbol{V}} $可化简为
$$ \dot {\boldsymbol{V}} = - {\dot {\boldsymbol{q}}^{\mathrm{T}}}{{\boldsymbol{k}}_{{\mathrm{pd}}}}\dot {\boldsymbol{q}} - {\dot {\boldsymbol{q}}^{\mathrm{T}}}{\boldsymbol{H}}\left( {{\boldsymbol{q}},\dot {\boldsymbol{q}}} \right) \leqslant {{\textit{0}}} $$ (23) 根据李雅普诺夫系统稳定性判据，对于双悬臂截割机器人的力位混合控制系统的能量函数，$ \boldsymbol{V}＞0 $且$ \dot {\boldsymbol{V}} \leqslant 0 $，即系统能量函数是正定的，而能量函数的导函数负定，说明系统的能量函数随时间减小，该系统是稳定的，可利用该系统对双悬臂截割机器人进行控制实验。
4. 双悬臂截割机器人控制仿真
在实现大断面截割时，基于蒙特卡洛方法生成的单悬臂与双悬臂截割机器人产生的工作空间如图6所示。
![]()
图 6 单悬臂与双悬臂截割工艺的工作空间Figure 6. Workspace for single-arm cutting and dual-arm cutting processes从图6可看出，对于单悬臂截割机器人而言，因工作空间有限，大断面的截割需开挖2条巷道才能完成，在完成一侧巷道截割后移机进行另一条巷道的截割[21]，工序复杂且效率低下。而双悬臂截割机器人的移动平台为2个截割臂提供了公共运动，机器人拥有更大的工作空间，可在不移动机身的前提下一次性完成大断面截割。
为了使控制系统产生的解与实际工况下掘进机运动相符，假设移动平台的实际运动与期望运动相符，设置从静止状态开始加速运动，到达工作速度后保持匀速，两截割臂沿S形轨迹进行异步截割。为了减小机器人截割过程中产生的动力学耦合效应，对机器人移动平台运动及双臂截割运动均进行五次多项式插值规划，端点之间的移动时间为10 s，每秒被划分为10个步长。五次多项式规划的边界条件：① 起点处移动平台速度和加速度均为0，到达S形轨迹的第1个拐点处时移动平台的速度为0.02 m/s，加速度为0。② 双臂各关节在S形轨迹拐点处的速度和加速度均为0。
五次多项式规划后的机器人截割头运动轨迹如图7所示，其中序号1−8表示截割头运动轨迹拐点。当截割头处于S形轨迹的拐点时，移动平台和各关节参数见表2。
![]()
图 7 双悬臂截割机器人截割头S形运动轨迹Figure 7. S-shaped motion trajectories of cutting heads of dual-arm cutting robot表 2 处于S形轨迹拐点处的双悬臂截割机器人关节变量Table 2. Joint variables of dual-arm cutting robot at inflection points of S-shaped trajectories端点
序号移动平台
推移量/mθ2/rad θ3/rad a3/m θ5/rad θ6/rad a6/m 1 1.0 0 π/3 4.73 0 π/3 4.73 2 1.1 −π/2 π/3 4.18 π/2 π/3 4.18 3 1.3 −π/2 2π/9 4.18 π/2 2π/9 4.18 4 1.5 0 2π/9 4.73 0 2π/9 4.73 5 1.7 0 π/9 4.18 0 π/9 4.18 6 1.9 −π/2 π/9 4.73 π/2 π/9 4.73 7 2.1 −π/2 0 4.18 π/2 0 4.18 8 2.3 0 0 4.73 0 0 4.73 由于双悬臂截割机器人在工作时不形成运动学闭链，仿真不考虑机器人在工作过程中的关节内力分配问题，即式（13）的内力项中$ {\boldsymbol{\delta}} $为零矩阵。当双悬臂截割机器人处在静止状态时，移动平台的质心相对于基坐标系的位置为[1 m 0 0]T，此时两截割臂的偏航关节处于0弧度。由于截割头自上而下截割，截割臂俯仰关节的初始角度和伸长关节的伸长量均为最大值，根据双悬臂截割机器人的相对运动学模型，两截割头的期望相对位置可由五次多项式规划的关节运动代入式（1）得出。期望的相对力设置为定值[255 N 240 N −25 N]T。两末端截割头的初始相对位置为[−2.427 1 m 4.203 9 m 2.025 8 m]T。两截割头期望的相对位置变化如图8所示。
![]()
图 8 双悬臂截割机器人两截割头的期望相对位置Figure 8. Expected relative position for two cutting heads of dual-arm cutting robot在力位混合控制系统的作用下，两臂对规划的轨迹进行跟踪，系统输出的机器人关节力如图9所示。由于移动平台的运动被锁定为匀速前推，力位混合控制系统只产生了1组符合条件的解。由于两截割头需同时满足图7中截割轨迹并保持期望的相对力，关节输出力将同时受到相对力控制环和相对控制位置环产生的力的影响而发生改变。当两截割头经过图7中轨迹拐点时，各关节的输出力发生较大振荡。这是因为截割头运动经五次多项式规划后，处于截割轨迹拐点时两截割头相对移动平台是静止的，此时截割头仅输出沿x0方向的力，在其他方向的力为0，截割头实际相对力与期望相对力存在较大误差，即相对力控制环中ef较大，导致关节输出力发生振荡。
根据控制系统输出的机器人各关节力矩，相对位置的控制结果如图10所示。可看出双悬臂截割机器人两截割头的实际相对位置对期望值保持了良好的跟踪，但实际相对位置在S形轨迹的拐点处发生了不同程度的振荡，原因是期望的相对力为常数向量，而在轨迹拐点处，两截割头期望的绝对速度和加速度均为0，导致下一步长的两截割头输出力产生较大变化，以实现两截割头对期望相对力的跟踪。在S形轨迹的拐点处，两截割头输出力发生的振荡通过图4的相对力控制环转换为关节输出力，使力位混合控制系统输出的总关节力产生较大的变化，导致实际相对位置发生波动，即轨迹拐点处发生相对位置振荡。
![]()
图 10 力位混合控制系统对截割头相对位置的控制结果Figure 10. Relative position tracking results of cutting head by force-position hybrid control system系统对截割头相对力的控制结果如图11所示。在S形轨迹拐点处，两截割头的相对位置和相对力均发生不同程度的振荡，在控制系统作用下，两截割头的相对力依然能够收敛到期望值。
![]()
图 11 力位混合控制系统对截割头相对力的控制结果Figure 11. Relative force tracking results of cutting heads by force-position hybrid control system考虑双悬臂截割机器人两截割头的初始位置并不总是位于规划的截割轨迹上，且不同硬度的煤岩巷道所需截割输出力不同，为更好地模拟不同截割负载下的实际工况，令双悬臂截割机器人初始相对位置为随机向量[−3.437 5 m 3.149 0 m 0.863 1 m]T，期望相对力为随机向量[65 N 60 N 36 N]T，此时力位混合控制结果如图12所示。可看出存在初始误差且期望相对力发生改变时，在30个步长内实际相对位置仍能振荡收敛至期望值附近，且实现了对期望相对力的同步跟踪。
![]()
图 12 初始相对位置存在误差时力位控制结果Figure 12. Tracking results of force-position with errors in initial relative position对于双悬臂截割机器人的力位混合控制系统，定义绝对误差e为在双臂对期望轨迹的跟踪过程中，每个步长下两截割头实际相对位置与期望相对位置之间误差向量最大值的二范数，即
$$ {{e}} = {\left\| {{{\boldsymbol{X}}_{\mathrm{R}}} - {{\boldsymbol{X}}_{{\mathrm{Rd}}}}} \right\|_{\max }} $$ (24) 双悬臂截割机器人力位混合控制系统的均方根误差eRMS计算方法：在对期望轨迹的跟踪过程中，计算机器人在每一步生成的末端相对位置误差列向量，然后计算全部N组列向量模长的平方和，之后取均值的平方根，即
$$ e_{{\mathrm{RMS}}} = \sqrt {\frac{1}{N}\sum{{{\left\| {{{\boldsymbol{X}}_{{\mathrm{Rd}}}} - {{\boldsymbol{X}}_{\mathrm{R}}}} \right\|}^2}} } $$ (25) 经计算，力位混合控制系统对相对位置跟踪的绝对误差为0.313 2 m，均方根误差为0.144 7 m。
综合仿真和计算结果可看出，力位混合控制系统能够有效跟踪截割头的期望相对位置，且对每个关节变量的期望值具有良好的跟踪效果。
5. 结论
1） 基于机器人相对雅可比矩阵建立双悬臂截割机器人模型，能将机器人双臂动力学整合为1个整体，通过单一变量同时描述机器人双臂运动状态。
2） 基于双悬臂截割机器人相对动力学模型，提出机器人双臂力位混合控制方案，通过李雅普诺夫函数对系统进行稳定性分析，验证了系统的可行性。
3） 对双悬臂截割机器人结构及力位混合控制系统进行仿真分析，结果表明：双悬臂截割工艺较单悬臂截割拥有更大的工作空间，具有一次性实现大断面截割的能力，简化了大断面成形工序，具有更高的效率；力位混合控制系统能够完成对期望相对位置和期望相对力的同步控制，从而实现双臂对煤岩的截割，相对位置跟踪绝对误差在
0.3132 m以内，均方根误差为0.144 7 m。 -
[1] 田会, 白润才, 赵浩.中国露天采矿的成就及发展趋势[J].露天采矿技术, 2019, 34(1):1-9. [2] 王建.露天矿开采工艺发展趋势与信息化现状[J].山西冶金, 2018, 41(2):126-128. [3] 李佳林.我国露天矿山开采工艺的发展现状及趋势[J].现代工业经济和信息化, 2018, 8(17):12-13. [4] 中国矿业大学.露天采矿手册[M].北京:煤炭工业出版社, 1986. [5] 于汝绶.露天采矿优化理论与实践[M].北京:煤炭工业出版社, 2004. [6] 赵浩.新时代露天煤矿发展趋势、面临的问题及相关建议[J].煤炭经济研究, 2019, 39(4):20-24. [7] 辛晓乐.我国露天矿山开采的薄弱环节及对策探讨[J].世界有色金属, 2019(1):48-49. [8] 杨志勇, 刘福明, 佟得君.露天煤矿智慧矿山创新设计理念[J].露天采矿技术, 2019, 34(2):6-9. [9] 吕鹏飞, 何敏, 陈晓晶, 等.智慧矿山发展与展望[J].工矿自动化, 2018, 44(9):84-88.
下载:
计量
- 文章访问数: 153
- HTML全文浏览量: 31
- PDF下载量: 12
