基于单位四元数的锚杆钻车铺网单元控制方法_杨泽源.pdf

第42卷第03期2023年03月煤炭技术Coal TechnologyVol.42 No.03Mar.2023doi:10.13301/ki.ct.2023.03.0480前言近年来，随着矿井开采范围的扩展，开采深度不断增加，巷道开挖后，如不及时进行有效的防护，围岩整体性将遭到破坏，甚至出现围岩垮塌现象,因此支护作业必不可少。目前国内条件较好的煤矿多采用锚杆钻车进行巷道内的快速锚杆支护。当前所用的锚杆钻车，在巷道铺网过程中，通常是由作业人员巷道顶部和边帮上固定网片，导致传统的巷道铺网操作存在人工成本高、作业效率低等问题；此外，呼吸性粉尘、操作人员处于空顶区等危险因素严重威胁着作业人员的健康和安全。为解决此问题，本文提出了一种用于锚杆钻车铺网单元的控制方法，在当前网片待安装位置已经确定后，将其作为铺网单元控制系统的目标姿态信息和系统输入，控制机械手爪对网片进行抓取及运输，完成网片安装。1控制对象概况本文提出的控制方法所针对的对象为锚杆钻车后附单机械手爪，对网片进行抓取及运输。锚杆钻车后附加网片底座，机械手爪对底座上的网片进行抓取，然后将其运输至已经确定的安装位置，过程中采用本文所述的控制方法保证其姿态旋转和运输过程的准确性及稳定性，提高铺网效率和安全性。2控制系统说明本文所采用的控制方法整体流程图如图1所示。首先得到当前网片的安装位置，并以此得到铺网单元机械手爪的目标姿态信息；通过观测器得到铺网单元当前的姿态信息；将外来扰动和当前姿态信息看作控制器的输入，通过与目标姿态的误差得到控制指令并发送给执行机构，输出控制力矩给到铺网单元，以修正其姿态，完成一次控制，之后将其反馈到控制器，做出下一步的控制指令，直到姿态信息基于单位四元数的锚杆钻车铺网单元控制方法*杨泽源1，贾运红1，2，3，4（1.煤炭科学研究总院，北京100013；2.中国矿业大学(北京)机电与信息工程学院，北京100083；3.中国煤炭科工集团太原研究院有限公司，太原030006；4.煤矿采掘机械装备国家工程实验室，太原030006)摘要：针对现有铺网过程仍需人工手动操作、人工成本高、危险系数高、作业效率低等问题，提出了一种基于单位四元数的锚杆钻车铺网单元控制方法，通过对铺网单元机械手爪进行基于单位四元数的姿态建模，设计了扩张状态观测器完成自抗扰控制，并对所设计的控制系统进行了仿真验证。结果表明，该方法使网片运输和安装到目标位置的过程实现了自动化，并在遭受扰动时能很快回归稳态，为实现井下锚护过程自动化打下坚实的基础。关键词：姿态模型；单位四元数；扩张状态观测器；自抗扰控制中图分类号：TP273文献标志码：A文章编号：1008 8725（2023）03 241 04Anchor Drill Rig Mesh-laying Unit Control Method Based on UnitQuaternionYANG Zeyuan1，JIA Yunhong1，2，3，4(1.China Coal Research Institute,Beijing 100013,China；2.School of Mechanical Electronic and InformationEngineering,China University of Mining and Technology-Beijing,Beijing 100083,China；3.China Coal Technology andEngineering Group Taiyuan Research Institute Co.,Ltd.,Taiyuan 030006,China；4.China National EngineeringLaboratory for Coal Mining Machinery,Taiyuan 030006,China)Abstract:Aiming at the problems of manual operation,high labor cost,high risk coefficient and lowoperation efficiency in the existing mesh laying process,a unit control method for anchor drill rigmesh-laying based on unit quaternion has been proposed.This method models the attitude based on thequaternion of the mesh-laying unit,designs the expansion state observer to complete the self-immunitycontrol,and simulates the designed control system.The results show that the proposed method automatesthe process of mesh transport and installation to the target location,and can quickly return to homeostasiswhen disturbed,laying a solid foundation for the automation of the downhole anchoring process.Key words:attitude model；unit quaternion；extended state observer；self-immunity control*国家重点研发计划资助项目(2020YFB1314003)；山西省重点研发计划（2020XXX001）241第42卷第03期基于单位四元数的锚杆钻车铺网单元控制方法杨泽源，等Vol.42 No.03达到期望值。图1控制方法流程图2.1铺网单元姿态模型建立（1）常用姿态模型描述方法本文所针对的铺网单元为机械手爪，确定其姿态信息至少需要2个坐标系：机械手自身的坐标系和选定的空间参考坐标系。常用的刚体姿态描述方法一般有3种：方向余弦法、欧拉角描述法和单位四元数描述法。方向余弦法是最具一般性的方法，直接用x，y，z3个矢量来表示坐标轴的方向，则2套坐标系之间的方向余弦共有9个。以x轴为例，2个x轴之间的方向余弦Axx=xbxr（1）式中xb刚体自身坐标系x轴的单位向量；xr参考坐标系x轴的单位向量。依此类推，将各轴之间所有的方向余弦写为矩阵AAxxAxyAxzAyxAyyAyzAzxAzyAzz（2）方向余弦矩阵A又被称为姿态矩阵。用方向余弦表示的姿态矩阵是姿态描述的最常用形式。此方法普适性强，但其参数仅有数学意义，几何意义不明确，计算过程复杂，因此不被采用。欧拉角描述法即根据旋转角表示刚体姿态。根据欧拉定理，刚体绕固定点的位移可以由绕该点若干次的有限转动合成。这意味着空间参考坐标系旋转3次后，就可以得到本体坐标系，每次的旋转轴为参考坐标系的坐标轴，旋转角被称为欧拉角。作为姿态参数，欧拉角在工程设计中较为常用，可以直接用姿态敏感器测量得到，且具有清晰、直观的几何意义。3次旋转的欧拉角分别记为，通过上述的3次旋转，可以得到欧拉角描述法的姿态矩阵。当=90时，不能唯一确定，这就是奇点问题，也是欧拉角方法的缺陷。为了避免出现奇点，同时也为了简化计算过程，减少复杂的三角函数计算，定义另外4个参数来表达欧拉角，即采用单位四元数法定义刚体姿态。定义四元数向量q由三维矢量q=q1，q2，q3和标量q组成，则在刚体系统中，定义四元数q1=exsin（/2）q2=eysin（/2）q3=ezsin（/2）q4=cos（/2）（3）式中ex沿x轴方向的单位向量；ey沿y轴方向的单位向量；ez沿z轴方向的单位向量；坐标轴转动角度。此4个参数满足约束方程：q12+q22+q32+q42=1（4）相比方向余弦方法，四元数方法仅含4个变量和1个约束方程；相比欧拉角方法，四元数方法中不含三角函数，其姿态矩阵依然是坐标变换矩阵。（2）基于单位四元数的姿态建模通过对上述3种方法的综合比较，本研究选择单位四元数法来表示铺网单元机械手爪的姿态。设xyz为机械手爪旋转时的角速度矢量，表示出其姿态动力学方程J+J=T（5）其中，控制力矩和外界干扰的总力矩T=u+d=TxTyTz角速度矢量=0-zyz0-x-yx0（6）式中J刚体本身的转动惯量矩阵；u控制器输出力矩；d外部扰动力矩。结合上述方程，根据计算推导得到机械手爪由单位四元数形式表示的姿态模型q=12（qI3+q）q=-12qT（7）J=-J+u+d其中，四元数矢量分量q=q1q2q3（8）四元数标量分量q=q4（9）式中I333的单位矩阵。2.2扩张状态观测器设计观测器的目的是为了实时观察被控对象的状态信息，以便系统及时对产生的扰动做出调整。当扰动能够反映在系统的输出里时，将所有扰动视为系统的一个状态变量，称为扩张状态变量，通过观察扰动，估计其可能造成的影响，然后用补偿的方法消除其-目标姿态+-自抗扰控制器姿态观测器控制指令执行机构外来扰动当前姿态铺网单元控制力矩242第42卷第03期Vol.42 No.03基于单位四元数的锚杆钻车铺网单元控制方法杨泽源，等影响。该观测器被称为扩张状态观测器。定义动态补偿器i=12ii+12ii+1Nj=0aij（j-i）i=12iTi+1Nj=0aij（j-i）（10）iSi2Nj=0aij（j-i）式中i被观测量的矢量分量；i被观测量的标量分量；i，j被观测量；1，2正实数；S33的实对称矩阵；aij矩阵中ij位置的元素系数。这个补偿器有着常规状态观测器的属性。在现有状态观测器的基础上添加一个扩张状态，即可构成扩张状态观测器。在本研究中，为了设计扩张状态观测器，首先需要建立机械手爪姿态控制系统所对应的误差系统。设本系统的姿态追踪误差四元数为qe=（qe，qe）T，误差四元数同样满足单位四元数约束条件。系统由误差信号表示的姿态模型qe=12（qeqeI3）eqe=12qeTeJe=（eC0）J（eC0）J（eC0C0）u+d式中C常数矩阵；0期望角速度，0R3；e当前角速度与目标角速度误差。由于运输网片过程中会产生抖动，以及巷道环境的不确定性，外部扰动也会对机械手爪本身的惯量矩阵产生影响，所以应用变换式J=J0+J，系统的扩张状态x2=F+G+d（12）其中，外部扰动F=J0-1J0+J0（eC0C）12J0K（qeI3+qe）eG=J0-1J+J（eC0C0）12JK（qeI3+qe）e+JJ+J（eC0C0）+12JK（qeI3+qe）+Ju（13）外部扰动的标量分量d=（J0-1+J）d（14）式中J0已知的非奇异的常数矩阵；J受扰动影响的惯量所对应的状态变量；J共轭矩阵；K正对角矩阵。结合上述方程，由误差信号表示的被控对象x1=x2+J0-1ux2=h（t）式中h（t）扩张状态变量的微分，假设其有界并满足|h（t）|Mi，其中Mi是未知的非负常数。所设计的扩张状态观测器x1（t）=x2（t）+g1（1x1（t）x1（t），x1（t0）=x10 x2（t）=1g2（1x1（t）x1（t），x2（t0）=x20其中，增益系数1=diag11，12，13（17）式中x10 x1（t）的初值；x20 x2（t）的初值；观测器的高增益参数；g1，g233的实数矩阵。2.3状态反馈控制律设计结合前文设计的扩张状态观测器，经过计算推导，得到状态反馈控制器u=-J01Kx1-J02x2（18）其中，参数K=diag（K1，K2，K3）式中K1，K2，K3正实数。3仿真结果系统的目标姿态信息q0=12（q0I3q0）0，q0=12q0T0，0S0（19）其中，目标姿态q0q0q0q1q2q3q4（20）通过信号跟踪效果的好坏可以判断所设计控制方法的效果。设定常数矩阵S=01 010 0