液压旋翼飞行机械臂系统建模与控制方法

林天宇，齐俊桐，姚　俊

(1.中国科学院沈阳自动化研究所 机器人学国家重点实验室，沈阳 110016;2.沈阳理工大学 信息科学与工程学院，沈阳 110159)



液压旋翼飞行机械臂系统建模与控制方法

林天宇1,2，齐俊桐1，姚俊2

(1.中国科学院沈阳自动化研究所 机器人学国家重点实验室，沈阳 110016;2.沈阳理工大学 信息科学与工程学院，沈阳 110159)

摘要：在旋翼飞行机器人上加装机械臂而得到一种新的飞行机器人系统，扩展了旋翼飞行机器人的应用范围。本研究将小型液压机械臂系统应用到已有的旋翼飞行机器人上，通过分析无人机与机械臂的运动耦合，得到悬停模式下液压旋翼飞行机械臂系统的整体动力学模型；用LQR控制器控制机械臂完成平面内摆动动作；将机械臂运动对机器人本体的影响作为扰动进行考虑，并采用鲁棒控制器来稳定旋翼飞行机器人悬停时的位姿。通过仿真验证了本文所述模型结构与控制策略的有效性。

关键词：旋翼飞行机器人；液压机械臂；动力学建模；LQR控制；鲁棒控制

地面移动机器人结合机械臂组成的作业型地面移动机器人系统，在反恐防暴、救灾救援等多种场合已经开始应用并得到广泛的认可。随着机器人应用领域的扩展，人们期望飞行机器人对其所处环境能够施加主动的影响。实际上，大部分关于飞行机器人的研究通常都是尽量躲避障碍而不与环境“接触”。旋翼飞行机器人(Rotary-wing Unmanned Aerial Vehicle，RUAV)因其三维空间机动性强、具备悬停能力等特点，可以代替人完成高危环境下的信息获取与作业等任务。因此，人们希望旋翼机能够与环境进行作用来完成一些“接触”任务。悬停状态下的旋翼飞行机器人通过加载机械臂可将工作区域从二维扩展到三维，极大的拓展了操作型旋翼飞行机器人的应用。

文献[1]开发的耶鲁空中作业平台可以完成实物抓取，借助安装在平台上的柔顺抓取手臂系统实现落地或悬停时抓取的动作，还研究了载荷质量与偏离重心位置构成的系统稳定域。文献[2]采用参数估计法，对旋翼机抓取重物时的不确定特性参数进行估计。文献[3]研究的无人空中系统加载7自由度工业机械臂，可抓取直立在地面上的长杆，并研究了抓取时的重心变化与相应的控制问题。文献[4-5]是关于单机或多机协同运送被吊装的重物。

上述研究都是把抓手固定在旋翼飞行机器人机身上以抓取质量轻或形状确定的物体，且机械臂和抓手都由电机驱动。而实际中有很多需求不仅仅是简单的抓取动作就能满足，如在维修高压输电线路时需要用很大的力或力矩对损毁的高压线路进行剪断作业，这种作业要求末端执行器具有巨大的力或力矩，并在较短时间内完成剪切任务。而上述研究都不能完成此任务，因为其机械臂抓手由电机驱动，末端安装电机会不可避免的引入附加载荷，且电机振动问题也会带来附加扰动。

为解决上述问题，本文提出一个新的旋翼飞行机器人系统，即由飞行机器人和多自由度液压机械臂组成的移动操作型旋翼飞行机械臂系统。这里选择液压作为动力源，是因为液压系统有很多优于电动或气动系统的特点。液压传动装置依靠油液的连续流动进行传动，传动十分平稳、吸振能力强且振动小；输出同样功率，液压传动的体积和质量可减少很多，因而惯性小；更重要的是，液压传动易于获得很大的力和转矩，同时又可简化机械结构，减少机械零部件数目并降低故障几率。

因此，将小型液压机械臂安装在旋翼飞行机器人上而得到一种新的液压旋翼飞行机械臂系统，可获得更好的性能，完成更多的任务。本研究已经完成了机械结构的设计，构建了动力学模型并设计了控制器，以便用于模型仿真试验。本文将旋翼飞行机器人与液压机械臂之间的相对动力学看成内部扰动，并通过力学分析进行建模。首先根据牛顿-欧拉方程和旋翼系统的空气动力学特性，对液压旋翼飞行机械臂系统进行整体动力学建模。然后，在旋翼无人机悬停模式下使用LQR控制器操作液压机械臂，同时将机械臂的动作看成旋翼飞行机器人的扰动，并用鲁棒控制器调整飞行机器人的姿态。最后，对液压机械臂在空间进行画弧线仿真，并通过仿真结果来衡量旋翼飞行机器人LQR与鲁棒控制器的控制性能。

1液压旋翼飞行机械臂系统建模

为实现对液压旋翼飞行机械臂系统的控制，需要对其运动学和动力学模型进行分析和研究[6-7]。实际应用中，机械臂的动作一般都在旋翼飞行机器人空中悬停状态下进行。基于这种工作状态，在建立所设计机械臂的运动学模型时，可依据机器人机械臂的传统理论来进行。而对于系统整体动力学模型，即飞行机器人的空间6自由度刚体运动力学表示，可以考虑不同来源的力和力矩的组合。

1.1机械臂连杆坐标系的建立

将旋翼飞行机器人机体看成是液压机械臂的基座，称之为连杆0。四自由度机械臂旋转底盘为连杆1，并通过关节1与机体相连接。连杆2与连杆1通过关节2相连接，直到末端连杆。这里的坐标系规定与机器人机械臂传统描述方法：Denavit-Hartenberg (D-H)法类似。坐标系{i}的Z轴Zi与关节轴i共线，其指向可任选。坐标系{i}的X轴与Xi连杆公垂线重合，指向从关节i到关节i+1。利用右手定则确定坐标系的Y轴。机械臂模型的具体D-H坐标系规定如图1所示。

按照上述方法，确定四自由度机械臂的各个连杆参数并代入到相对应变换矩阵中，可得到相邻连杆坐标系之间的转换矩阵。四自由度机械臂的具体连杆参数见表1。

图1　机械臂抽象模型坐标系

连杆i杆长ai-1/mm扭角αi-1/(°)偏距di/mm关节角θi/(°)100°0θ12l1-90°0θ23l20°0θ34090°l3θ4

(1)

计算得到从末端到基座坐标系之间的转换关系：

(2)

式(2)中四自由度机械臂的底盘不需要转动，即θ1=0,cosθ1=1；sinθ1=0;始终保持末端液压剪刀的刀口姿态不变，即θ4=0，cosθ4=1,sinθ4=0,则式(2)简化为

(3)

1.2旋翼飞行机器人系统建模

对于带有机械臂的旋翼飞行机器人，传统的旋翼飞行机器人模型无法描述机械臂对旋翼机本体的影响。因此，需要对旋翼飞行机器人的模型[6-8]加以改进，增加机械臂对旋翼机的作用，满足本文情况下对旋翼飞行机器人的控制要求。下面给出旋翼飞行机器人模型的主要部分，在此基础上加入机械臂对旋翼飞行机器人的影响。

通常情况下，旋翼飞行机器人模型建立在本体坐标系下，即以机头方向为x轴，右侧为y轴，z轴指向机体下方，3个坐标轴满足右手定则。本研究中，需要对旋翼飞行机器人进行位置控制。无人机的位置是在世界坐标系中定义的，两个坐标系之间可以通过旋转矩阵进行转换。由本体坐标系到世界坐标系的变换矩阵为

(4)式中：φ、θ、ψ分别为旋翼飞行机器人的横滚角、俯仰角和航向角。本体速度与世界坐标系下的位置可由式(5)描述：

(5)

式中：P=[x,y,z]表示飞行机器人的位置；Vb=[ub,vb,wb]表示旋翼飞行机器人的前向、侧向及垂向的速度；下标w表示当前变量是在世界坐标系中定义的；下标b表示当前变量是在飞行机器人本体坐标系下定义的。

在本体坐标系下对旋翼飞行机器人的模型进行简要介绍。首先考虑旋转运动：

(6)

(7)

式中：Fb为空气动力矢量；Mb为空气动力矩矢量；Fg为重力矢量；Ib为转动惯量；m为旋翼飞行机器人的质量。通常情况下，空气动力矢量和空气动力矩可由下列非线性函数描述：

(8)

式中：ff、fm为非线性函数；a1s、b1s分别为主旋翼纵向和横向挥舞角；δcol为主旋翼总距角；δped为尾旋翼总距角。考虑主旋翼挥舞动力学，有

(9)

式中：τmr为主旋翼时间常数；Abs、Bas为耦合系数；δlon和δlat分别为前向和侧向控制量。

公式(4)～(9)简要描述了通用旋翼飞行机器人模型。下面将考虑机械臂对旋翼飞行机器人的影响，继而建立包含机械臂影响的液压旋翼飞行机械臂系统模型。假设机械臂与旋翼飞行机器人机身固定连接，则可将它们看作一个刚体，其6自由度刚体动力学模型可由牛顿-欧拉方程描述：

(10)

(11)

在机械臂运动过程中产生的力和力矩可以从末端连杆向基座进行迭代求解

(12)

根据牛顿第三定律(作用力与反作用力)，易得出下式描述的施加在旋翼飞行机器人本体上的力和力矩

(13)

2液压旋翼飞行机械臂系统控制

液压机械臂的操作只在旋翼飞行机器人悬停模式下进行，将机械臂对旋翼飞行机器人本体的影响作为扰动，使用鲁棒控制器加以抑制。

2.1液压机械臂系统控制

根据文献[9-10]可知，液压执行机构的开环传递函数为:

(14)

将上述开环传递函数写为状态空间形式可得

(15)

为实现对液压执行机构的控制，设计如下状态反馈控制器：

um(t)=Kmxm(t)+Kmrrefm

(16)

式中：refm为参考输入；Km为反馈矩阵，用于保证系统的稳定性，可用线性二次型调节器(LQR)方法计算得到；Kmr为前馈矩阵，用于实现对参考输入的跟踪。

(17)

式中[·]+表示伪逆。液压执行机构的状态xm无法直接测得，因此需要设计如下状态观测器对其进行观测：

(18)

式中观测器矩阵L可以通过极点配置的方法求得，则相应的状态反馈控制器为

(19)

2.2旋翼飞行机器人系统控制

为抑制机械臂运动对旋翼飞行机器人的影响，将Fm与Mm看作是作用在旋翼飞行机器人本体上的扰动，通过控制算法对其进行抑制。

选取旋翼飞行机器人悬停点附近的一个稳定状态作为平衡点，对2.1所述包含机械臂影响的旋翼飞行机器人模型进行线性化，可以得到如下线性模型。

yp(t)=C1xp(t)

(20)

zp(t)=C2xp(t)

为实现旋翼飞行机器人的跟踪控制，定义误差向量e(t)=∫ref-z(t)dt，则式(20)可以表示为

yp(t)=Ce1xe(t)

(21)

zp(t)=Ce2xe(t)

令Erω(t)=Eeωp(t)+Reref，则

(22)

为减小扰动对系统的影响并保证一定的跟踪精度，需要设计控制器对上述系统进行控制。由于系统状态中的a1s、b1s无法通过传感器直接测量，故设计如下动态输出反馈控制器

(23)

式中：xc为控制器状态向量，其维数与xp相同；Ac、Bc、Cc、Dc为待定的控制器矩阵，则闭环系统可以表示为

(24)

为尽可能减小扰动ω(t)对系统的影响，考虑H∞性能，‖e‖2=‖ref-zp(t)‖2≤γ‖ω‖2，与存在X>0使下列矩阵不等式存在等价[11]：

(25)

值得注意的是，式(25)并不是线性矩阵不等式(LMI)，因为A矩阵中存在待定矩阵，而X同样也是待定的。因此，无法直接对该矩阵不等式求解。可用消元法或变量替换法[12]，将上述矩阵不等式转化为线性矩阵不等式进行求解，从而得到动态输出反馈控制器的参数矩阵Ac、Bc、Cc、Dc。

这样，机械臂对于旋翼飞行机器人的影响可以被有效的抑制，为机械臂的准确操作提供了较为稳定的平台。

3系统仿真及验证

为验证机械臂控制方法的有效性，基于现有的液压执行机构开环模型，对机械臂执行机构闭环控制系统进行仿真，仿真结果如图2所示。虚线为期望角度，实线为机械臂实际运动角度。由仿真结果可知，机械臂可基本按照给定期望运动，满足控制要求。

为验证旋翼飞行机器人对机械臂扰动抑制的情况，对3种扰动情况进行仿真验证，仿真结果如图3所示。仿真过程中，设定旋翼飞行机器人与机械臂总重35kg，在第10s至20s，在旋翼飞行机器人机体坐标x轴方向施加频率为0.2Hz幅值为10N的力，用于模拟机械臂在x轴方向对旋翼飞行机器人产生的力扰动；第30s至40s，在y轴方向施加频率为0.2Hz幅值为10N的力；第50s至60s，在x轴上施加频率为0.2Hz幅值为5Nm的力矩扰动。图3显示不同的扰动对系统稳定性的影响。

图2　机械臂控制响应曲线

图3　旋翼飞行机器人对机械臂扰动的抑制情况

由图3可知，本文所用的H∞鲁棒控制方法对机械臂施加于旋翼飞行机器人的力扰动有很好的抑制效果，其中，姿态变化小于0.6rad，位置变化小于0.05m。对于力矩扰动的抑制效果相对较差，其中，姿态变化小于1rad，位置变化小于0.1m。

基于液压机械臂模型、旋翼飞行机器人模型及控制方法，对液压旋翼飞行机械臂系统进行综合仿真验证。为验证旋翼飞行机器人悬停状态下，安装于旋翼飞行机器人上的机械臂运动时其末端刀口的运动情况，令机械臂关节在一定角度内缓慢摆动，对刀口的运动轨迹进行绘制，仿真结果如图4所示。其中虚线为期望运动轨迹，实线为安装于旋翼飞行机器人上的液压机械臂末端刀口的运动轨迹。由仿真结果可知，由机械臂运动导致的旋翼飞行机器人的晃动对机械臂末端刀口运动轨迹的影响得到了较好的抑制，验证了本文建模方法与控制方法的可行性。

图4　刀口的运动轨迹仿真

4结束语

分析了液压旋翼飞行机械臂系统动力学模型，并基于模型设计不同的控制器。采用LQR控制器对机械臂进行控制，仿真验证了机械臂控制器的有效性。对旋翼飞行机器人与空中机械臂之间的相对扰动，使用鲁棒控制器进行抑制；对所施加的不同的扰动进行仿真，验证了控制器的鲁棒性，确保在旋翼机器人本体稳定的情况下进行机械臂操作作业的有效性。

下一步，将搭建物理实验仿真平台，实际测试本文提出的控制策略的性能，为未来的空中操作型机器人的建立做好准备工作。

参考文献：

[1]E I Paul,D R Bersak,A M Dollar.The Yale Aerial Manipulator:Grasping in Flight [C]//IEEE International Conference on Robotics and Automation.NY,USA:IEEE，2011:2974-2975.

[2]D Mellinger,Q Lindsey,M Shomin,et al.Design,modeling,estimation and control for aerial grasping and manipulation [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.NY,USA:IEEE,2011:2668-2673.

[3]F Huber,K Kondak,K Krieger,et al.First analysis and experiments in aerial manipulation using fully actuated redundant robot arm [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.NY,USA:IEEE,2013:3452-3457.

[4]M Bisgaard,J Bendtsen,A la Cour-Harbo.Modeling of Generic Slung Load System [J].Journal of Guidance,Control and Dynamics,2009,32(2):573-585.

[5]M Bernard,K Kondak.Generic Slung Load Transportation System Using small Size Helicopters [C]//IEEE International Conference on Robotics and Automation.NY,USA:IEEE,2009:1278-1284.

[6]D Song,J Qi,L Dai,et al.Modeling a small-size unmanned helicopter using optimal estimation in the frequency domain [J].International Journal of Intelligent Systems Technologies and Applications,2010,8(1-4):70-85.

[7]Cai G,Chen B,Lee T.Unmanned Rotorcraft System [M].London:Springer,2011.

[8]Shim H.Hierarchical fight control system synthesis for rotorcraft-based unmanned aerial vehicles [D].Berkeley:University of California,2000.

[9]Daqing Zhang,Qinghua He,Xuan Wu,et al.Adaptive control of hydraulic excavator manipulator [C]//IEEE International Conference on Mechatronics and Automation.Chengdu,China:IEEE,2012.

[10]程涛.液压挖掘机的轨迹规划与控制[D].沈阳：东北大学，2007.

[11]Gahinet P,Apkarian P.A linear matrix inequality approach toH∞control [J].International Journal of Robust and Nonlinear Control,1994,5(4):421-448.

[12]俞立.鲁棒控制——线性矩阵不等式处理方法 [M].北京：清华大学出版社，2002.

(责任编辑：赵丽琴)

The Modeling and Control Method for a Hydraulic Rotorcraft Aerial Manipulator

LIN Tianyu1,2，QI Juntong1，YAO Jun2

(1.State Key Laboratory of Robotics,Shenyang Institute of Automation,Chinese Academy of Sciences,Shenyang 110016,China；2.Shenyang Ligong University,Shenyang 110159，China)

Abstract：A Rotorcraft Aerial Manipulator (RAM) system is obtained through installing mechanical manipulator on RUAV,which expands the application scope of RUAVs.The research is done on installing miniature hydraulic manipulator system on the existing RUAV.The overall dynamic model of the hydraulic RAM systems is obtained through the analysis of the coupled motion of the RUAV and the manipulator.The planar swinging motion of the manipulator is controlled by LQR controller.The motion of the manipulator is considered as a disturbance to the RAM system and a robust controller is utilized to eliminate the disturbance and stabilize the R-UAV′s hovering pose.The model structure and the control strategy proposed are verified through simulation.

Key words：RUAV；hydraulic manipulator；dynamic modeling；LQR control；robust control

中图分类号：TP241.3

文献标志码：A

文章编号：1003-1251(2016)02-0054-06

作者简介：林天宇(1988—)，男，硕士研究生；通讯作者：齐俊桐(1981—)，男，研究员，研究方向：飞机机器人系统理论及技术、机器人自主控制等。

收稿日期：2014-12-10