新型救援工程机械末端执行器轨迹控制研究

吴海宇， 陈欠根，， 赵喻明，， 陈冬良， 王广松

（1.中南大学 高性能复杂制造国家重点实验室，湖南 长沙 410012；2.山河智能装备股份有限公司 国家级企业技术中心，湖南 长沙410100）

由于全球的地震灾害进入了高发期（仅2010年前4个月，国内外便连续发生了41次6级以上的地震），复杂的地震坍塌现场急需专业、高效的救援机械装备［1］。因此，各种新型救援工程机械相继诞生，如俄罗斯乌拉尔拖拉机厂生产的171.1k3型防核辐射推土机、美国雪龙公司生产的融雪机、俄罗斯生产的Rm140型全路面运输车以及美国卡特彼勒的挖掘装载车［2］等。

地震救援是一种特殊的行为动作，对机械动作有极高的要求：救援机械在切割钢筋混凝土建筑构件、预应力柱时要保证稳定和可靠，防止二次垮塌。能否完成这一要求取决于人员操作的准确性，故救援工程机械需引入机电一体化［3］。在工程机械领域，机电一体化控制包含机械电子化、自动化和机器人化［4］。已有学者在这些方面进行了研究，如文献［5］在对挖掘机机器人化改造的基础上，建立了工作装置运动学和动力学模型，针对系统动力学高非线性、参数不确定性等特点，提出了在自适应鲁棒基础上非连续映射的方法处理运动系统，实现了挖掘机智能化目的；文献［6］基于行为控制的思想进行轨迹任务控制，对挖掘机器人工作状态进行评估，实现自主挖掘等。

救援工程机械工作时环境恶劣，负载复杂多变，自身惯性大，且存在多种非线性因素，故其需要一种基于现场实时监控反馈的半自动化控制模式。本课题研究设计了一种多功能7自由度双臂手抢险救援工程机械，其多功能体现在末端执行器为可快速更换的属具，本文结合其结构特点与作业环境提出了3种不同的操作模式，其中属具末端模式通过Matlab中的SimMechanics对动态系统进行建模、仿真和分析，得到所需的各关节角度值，从而实现半自主控制［7－9］。

1 新型救援工程机械

本课题救援工程机械的机械臂手具有冗余度，有较大的操作空间，能满足较多的约束，可以有效躲避自身物理极限和环境障碍物［8］。该机械每个臂手有7个自由度，均由单个手柄控制，如图1所示。采用电控控制，可实现动作轻便及远程遥控。

图1 新型救援工程机械

2 3种操作模式

如前所述，救援工程机械所受负载为变载荷、动载荷，故控制需要考虑现场环境，在实现控制能动基础上，结合自动控制原理，实现半自动化的控制。实现的最可靠的方式是从操作员的操作方式入手，因此本文提出了3种不同的操作模式。

操作员处在司机室内，操作必须避免误动作，故需要手柄控制简单、轻便。本机采用了Gessmann公司的单轴多控手柄，如图2所示，手柄主轴沿X、Y、Z方向运动，手柄面板上2个小按键可分别沿X、Y方向运动，可同时实现7个自由度的控制。单手柄可实现控制单臂。

图2 操作手柄

2.1 常规关节模式

常规关节模式中执行器单独控制，单手柄对应控制单臂手。此控制模式的优点是控制灵活，能够通过操作实现机械臂手的各种位姿，但对操作员技能要求较高。

手柄的控制方式如图3所示。

图3 常规关节模式

2.2 属具末端模式

当救援机械处于需要实现特定的救援动作，如需要将抓起的重物往水平直线路径放置，此控制点为腕部中心，对中心点的移动通过联合主臂摆动、变幅和副臂变幅油缸实现直线轨迹，操作手柄也只需控制手柄X、Y、Z方向控制受控点（即腕部中心）。控制过程中的3个执行油缸通过算法实时反馈，实现自动的控制输出。

手柄的控制如图4所示。

图4 属具末端模式

对于属具末端模式，其难点在于机器人技术与大型机械控制技术的融合问题。机器人技术常用于稳定载荷工况下的简化分析，而大型工程机械中必须考虑变负载、大尺寸等复杂工况，故需要取舍。

2.3 双臂协调模式

为了适应多种救援环境，本新型机械采用了双臂手设计，故在控制中必须考虑双臂的协调问题。在半自主控制中，特殊救援过程需要实现双臂的协调动作、协同工作，如双臂同时抬起一块石板且不倾覆。把双臂看成一个整体，操作人员通过只操作一个手柄同时控制双臂的运动，而此运动是基于属具末端实现的，同样实现末端轨迹直线控制，故手柄操作仍为图4所示的控制方式。

此操作模式的难点在于如何将同一个手柄的输出信号通过系统识别来分别控制2个臂手的输出信号，同时2个臂手末端的相对位置不改变。

3 控制策略简述

整机控制，需要保证目标的可靠性及稳定性。基于前述救援机械的特殊要求以及工程机械特点，救援工程机械的电控系统设计如图5所示。通过电液比例阀实现控制的精细化及高可靠性，同时设置各种传感器，即对臂手的位置、位姿进行实时跟踪分析及反馈，从而实现闭环控制，达到自动化控制的目的。双臂手分别控制，能很好地满足后续的避碰以及协调作业的要求。控制策略简图如图5所示。

图5 控制策略简图

4 属具末端操作实现

实现臂手末端模式控制的第1步是对救援机械进行数学建模，其系统为与运动学、动力学密切相关的、强耦合、非线性的多变量系统，故在建模时须充分考虑这些因素。

建模时首先建立臂手的运动学模型，利用DH法分析，确定各个关节的坐标及D－H参数，得到运动学正解和逆解；再根据逆解进行末端执行器运动规划，实现末端控制［7－11］。臂手的结构简图如图6所示。

在模型搭建过程中，不能忽视单臂自身的尺寸对其工作范围的影响，故需要限制其可运动角度范围。

图6 臂手的结构简图

4.1 运动学分析

表1所列为臂手的各关节参数，求解运动学方程式，用齐次变换矩阵Ai来描述第i个相对于第（i－1）个坐标系的位置方向和方位［6］。

表1 D－H参数表

臂手的末端相对于基坐标系的位置和姿态，用A60表示为：

运动学逆解的求解方法是将运动方程（7）式的两端依次左乘各A矩阵的逆矩阵，并使两端矩阵的对应元素相等，即可求各个关节变量［8－9］。

4.2 轨迹规划和仿真数据

轨迹规划是指根据作业任务要求（作业规划），对末端执行器在工作流程中位姿变化的位置、姿态以及它们的速度、加速度的人为设定，并通过运动学逆解得到各关节的运动指令［12－13］。根据本机械所要精确跟踪的轨迹，确定给定路径的解析函数式，并利用Matlab中SimMechanics进行运动学仿真，通过编写输入轨迹控制Function文件，对机构进行实时再现仿真，以得到关节空间的各个关节所需的角度值［13－15］。

根据臂手的机构组成，基于SimMechanics的仿真模型如图7所示。

（1）物理模型。例如图7中左下角的Revolute、Body模块以及theta1输入模块共同组成了关节1的物理模型。6个关节，6个连杆和6个关节驱动器尺寸、初始位姿等参数均由各模块对话框设置，进行动力学测量还需要设置连杆的质心位置、质量和惯性矩等参数。

（2）传感器。在各关节放置关节传感器，以测量角度（如图中Joint Sensor模块），在手抓末端放置刚性传感器，以测量末端运动状态，数据输出至workspace供绘制三维轨迹。

（3）驱动模块。此为本模型最关键部分，需根据上述运动学逆解，建立各关节在实际轨迹发生时各关节角度驱动所需的角度值，如图7中的theta1、theta2等输入值可通过4.1中运动学逆解计算得到。由于关节以角度、角加速度驱动，以弧度为单位，故需要转换为角度值。

仿真系统设定后运行，可观察救援机械臂手的模拟运动，结束后观察各曲线，并输出数据给后续计算以实现末端控制。连杆的设计长度参数为l1＝150mm，l2＝4 663mm，l3＝2 586mm，l4＝400mm，l5＝1 907mm，经过转化得到目标物在基坐标中的坐标。臂手的三维简图如图8所示。

属具末端模式下的控制是通过手柄X、Y、Z方向来分别控制末端在基座坐标系上X、Y、Z方向的运动，故本文采用了3段行程来模拟末端的控制（分别为X方向上7 000～9 000mm、Y方向上0～2 000mm、Z方向上0～1 500mm，起始坐标都是（7 000，0，0））。

经过仿真实验可以得到3段运动各关节的角度曲线，如图9所示。

图7 基于SimMechanics的运动学仿真模型

图8 臂手的三维简图

图9 X、Y、Z方向运动各关节的角度值

对于臂手的动作，执行器是液压油缸，故通过已求得的各关节角度值，经过换算得到油缸所需流量，便可通过电控系统实现所需角度值控制，即实现末端控制。

5 试验验证

把上述控制程序写入控制器中，标定好倾角传感器，如图10、图11所示。通过手柄控制实时再现上述3段轨迹，记录各传感器数据，通过计算得到末端执行器中心点相对转台坐标系的轨迹，见表2所列，如图12所示。

图10 司机室控制器

图11 倾角传感器

表2 末端执行器的轨迹数据 mm

图12 通过试验实测的末端轨迹

由试验结果可以得到，当分别进行3段行程时，轨迹变动都在5%误差范围内，精度满足实际需求。分析可知，在实际操作中此算法完全可以实现精确末端轨迹控制。

6 结束语

本文针对大型救援工程机械在实际救援过程中所需的特定动作进行了控制研究。结合实际工况，提出了3种不同的操作控制模式，能很好地应对救援实际所需。属具末端控制模式结合机器人末端执行器轨迹控制原理，利用基于SimMechanics的仿真模型，建立运动学仿真模型，通过参数设置及仿真类型选择进行仿真。仿真分析可知，通过控制可以实现救援工程机械的臂手末端平稳连续运动，且无柔性冲击。根据所得数据，换算得到液压油缸所需控制流量，结合液压系统，对臂手进行控制试验。试验分析表明，通过手柄能够实现对末端执行器中心点的准确控制。

［1］刘红亮.双臂手救援工程机械转台设计及优化［D］.大连：大连理工大学，2012.

［2］刘国辉，赵晓宇.国内外新型应急救援工程机械设备分析［J］.中国机械，2014（2）：40－41.

［3］胡道鹏.挖掘机作业运动轨迹控制研究［D］.广州：华南理工大学，2013.

［4］何 冰，温银萍.论工程机械机电一体化及机器人化［J］.科技风，2011（15）：152.

［5］何清华，张大庆，郝 鹏，等.液压挖掘机工作装置模型及控制的试验研究［J］.中南大学学报：自然科学版，2006，37（3）：542－546.

［6］戴兴建.挖掘机机器人化技术研究［D］.沈阳：东北大学，2010.

［7］熊有伦.机器人技术基础［M］.武汉：华中科技大学出版社，2002：30－40.

［8］刘亚军，黄 田.6R操作臂逆运动学分析与轨迹规划［J］.机械工程学报，2012，48（3）：9－15.

［9］Qiao S，Liao Q，Wei S，et al.Inverse kinematic analysis of the general 6Rseriel manipulators based on double quaternions［J］.Mechanism and Machine Theroy，2010，45（2）：193－199.

［10］Guo Chuangqiang，Ni Fenglei，Sun Jingting，et al.Joint driver and control design for large torge，long arm space remote manipulator［C］／／International Conference on Information and Automation.IEEE，2010：1426－1431.

［11］王鲁敏，李艳文.基于Matlab的教学型机器人空间运动轨迹仿真［J］.机械与电子，2005（9）：55－57.

［12］王健强，程 汀.SCARA机器人结构设计及轨迹规划算法［J］.合肥工业大学学报：自然科学版，2008，31（7）：1026－1028，1041.

［13］李万莉，陈熙巍，茹 兰，等.基于SimMechanics的4自由度机器人的轨迹规划和仿真系统设计［J］.中国工程机械学报，2008，6（2）：144－148.

［14］MathWorks Corp.SimMechanics user’s guide 1.1［R］.Natick：MathWorks Corporation，2002.

［15］罗家佳，胡国清.基于MATLAB的机器人运动仿真研究

［J］.厦门大学学报：自然科学版，2005，44（5）：640－644.