推力矢量控制机械臂运动规划及仿真

许路路，郭卫东，杜志贵，邹庆晓

（1.北京航空航天大学机器人研究所，北京 100191；2.中国科学院光电研究院，北京 100094）

1 引言

推力矢量控制技术作为一种非传统的飞行控制技术，主要应用在军用飞机的设计和改型方面，是指飞机的推进系统除提供向前的推力外，还能通过改变飞机发动机的喷射方向来提供飞机俯仰、偏航和翻滚的推力，因此在短距起降和超机动性能方面相对于常规类型的飞机有着不可比拟的优势。目前常采用三轴承推力矢量喷管[1]的结构形式，并成功应用于苏联雅克141战斗机和美国F-35B战斗机[2-3]。空间飞行器常采用离子发动机作为推进系统，结构复杂，成本高，并且只能提供向前的推力，机动性很差。而将推力矢量技术应用在空间飞行器的应用上很少有人研究，结合近年来机械臂技术发展迅猛，提出一种将推力矢量喷管与机械臂相结合的机构，作为空间飞行器的推进系统，并且机械臂的控制方法采用推力矢量控制。这种结构简单、成本低，并大大提高了空间飞行器的机动性能。

推力矢量控制技术应用在机械臂上主要是指在机械臂末端安装矢量喷管，喷管喷气使该机械臂末端获得反推力，通过调整矢量喷管的方向，可对飞行器的位置和姿态进行控制。通过力的传递性原理可知，力的作用点只需在该力的作用线上即可，具体位置并不影响力臂的大小。因此可知，推力矢量控制不同于常规机械臂的确定位置和确定姿态的控制，该机械臂的末端位置不是固定的，末端原点坐标只需在该力的作用线上即可。该区别主要体现在对机械臂进行运动学反解的过程中。气浮平台是指通过压缩空气在气浮轴承和轴承座之间形成气膜，从而使气浮台悬浮，抵消重力影响的同时实现了近似无摩擦的相对运动条件，能够模拟飞行器在轨失重条件下的姿态运动，是在地面条件下研究空间飞行器姿态动力学和控制系统的理想平台[4]。搭载矢量控制机械臂的气浮平台系统，如图1所示。

图1 气浮平台Fig.1 Air-floating Platform

推力矢量控制机械臂具有四个自由度，如图1所示。采用相邻转轴两两垂直的构型。采用单臂的控制方式，只模拟飞行器的位置控制，主要控制因素为末端机械臂输出力的方向，而力臂的大小在此不作为控制因素。因此已知条件为：机械臂末端喷嘴轴线的方向（i，j，k）和该方向通过一定点（x0，y0，z0），改点设定为系统的质心，力的作用线经过系统的质心，因此可使系统沿该力的方向移动。

首先采用参数法对该机械臂进行正向和逆向运动学的求解[5-9]，通过求解结果论证了该结构应用于机械臂末端矢量控制的正确性，其中逆解的数目取决于关节数目、连杆参数和关节变量的活动范围；然后利用MATLAB中的robotics工具箱仿真分析了该机械臂的工作范围；最后进行算例验证所建立的运动学模型的准确性。

2 运动学分析

2.1 正运动学分析

利用D-H参数法建立坐标系，如图2所示。

图2 D-H坐标系Fig.2 D-H Coordinates

机械臂各连杆参数以及关节变量，如表1所示。

表1 D-H参数表Tab.1 D-H Parameters

为建立运动学方程，用齐次变换矩阵来表示连杆坐标系i在连杆坐标系i-1中的位置和姿态，可得：

即可求得机械臂末端的位姿，即喷管反推力的位置和方向。

2.2 逆运动学分析

一般对常规机械臂进行控制时，已知条件为机械臂的确定位置和确定姿态。前言中提到，采用推力矢量控制时，该机械臂的末端位置不是固定的，因此在进行运动学反解的过程中，需要根据条件首先求解机械臂末端的位置，然后再求解机械臂各关节的转角。

已知条件为：

（1）机械臂末端坐标系O4x4y4z4中x4在基坐标系下的方向余弦，即：

（2）机械臂末端所在直线过一定点，设该点为M（xm，ym，zm），即空间飞行器的质心坐标。

通过上述两个条件，反推力方向确定，并经过空间飞行器质心。将运动学反解过程分为两部，首先求解机械臂末端位置坐标，其次求解各关节转角。

2.2.1 求解机械臂末端位置坐标

设机械臂末端所在直线l的方程为：

式中：S（nx，ny，nz）—直线 l的方向向量，即 x4—轴在基坐标下的方向余弦，P（px，py，pz）—坐标系 O4x4y4z4的原点位置。

已知直线 l过点 M（xm，ym，zm），则联立式（4）、式（5）和式（9），消元 t和 sθ2可得：

根据费拉里法可求得式（10）中pz的四个解，具体过程在此不做介绍。

接下来联立式（4）、式（5）和式（9），并判断 nz是否为 0，即可求得px，py，pz的数值。至此，机械臂末端位置坐标求解完毕。

2.2.2 求解各关节转角

根据式（6），可求解出：

已知 l1＞l2+l3，因此 l1+（l2+l3）sθ2＞0，因此根据式（4）和式（5）可知：

根据式（7），可求解出：

已知-90°＜θ4＜90°，因此可知 cθ4≠0，根据式（8），可求解出：

至此，关节转角 θ1、θ2、θ3、θ4可全部求解。

3 MATLAB仿真分析

3.1 利用link和robot函数建立机械臂模型

link 函数的调用形式为：L=link（［alpha A theta D sigma］，CONVENTION）；robot函数的调用形式为：robot（link，…）。即用link创建机械臂对象。矢量控制机械臂模型，如图3所示。

图3 矢量控制机械臂模型Fig.3 The Model of Vector Control Manipulator

已知连杆参数：l1=200mm，l2=150mm，l3=30mm，-90°＜θ1＜90°，0°＜θ2＜180°，-180°＜θ3＜180°，-90°＜θ4＜90°。

建立机械臂模型需要的命令如下：

L1=link（［00000］，‘mod’）；

L2=link（［-pi/2200000］，‘mod’）；

L3=link（［-pi/20pi/21500］，‘mod’）；

L4=link（［pi/200300］，‘mod’）；

RRRR.name=‘推力矢量控制机械臂’；

drivebot（RRRR）；

在MATLAB上运行可生成矢量控制机械臂模型，如图4所示。

3.2 模型的验证

基于主要对机械臂末端推力矢量的方向进行控制的要求，完成了该机械臂的运动模型的建立，下面主要是验证该模型是否满足规定要求。

假设质心M（xm，ym，zm）在基坐标系的平面y0o0z0，即xm=-200，需要验证的是：

（1）验证点M在y0o0z0平面中的范围，即质心得取值范围。

（2）给定确定数值的质心坐标，机械臂末端绕y4轴和z4轴的摆动范围，即反推力的方向。

fkine函数能够实现机械臂正向运动学的求解。调用形式为：T=fkine（ROBOT，q）。

由式（2）可以得到机械臂末端的位置及姿态相关参数nx，ny，nz，px，py，pz。

[2] James E. Dougherty, Robert L. Pfaltzgraff, Contending Theories of International Relations: a Comprehensive Survey(5th Edition), 北京：北京大学出版社, 2004年, pp. 165-166.

而点 M（xm，ym，zm）满足直线 l方程，并已知 xm=-200，因此可确定点M（xm，ym，zm）的值。

3.2.1 验证点M在y0o0z0平面中的范围

设定各关节转动步长为9°，将计算得出的点M（xm，ym，zm）在y0o0z0平面表示，如图5所示。

图4 平面内的质心坐标Fig.4 Centroid Coordinates in XY Plane

由图4可知，质心坐标集中在以坐标原点为圆心，半径为500mm的圆形区域中，通过调节连杆l1，l2，l3的长度及各关节转角范围，即可改变扇形区域的大小，从而使得机构的质心坐标在圆形区域内，满足使用要求。

3.2.2 验证机械臂末端的摆动范围

选定质心的坐标为 M（-200，100，0），由式（13）可知 px，py，pz，nx，ny，nz满足方程：

设定各关节转动步长为 3°，利用 fkine 函数求解出 px，py，pz，nx，ny，nz，根据转动步长为 2°，修改式（13）为：

通过式（16）将数据进行筛选，提取符合要求的数据中的ny和nz，求解ny和nz的反余弦，如图6所示。

图5 机械臂末端的摆动范围Fig.5 The Swing Range of ManipulatorEnd-effector

摆角是在基坐标系O0x0y0z0下衡量的角度，如图5所示。由此可知，给定质心坐标后，摆角范围即可确定，工作空间即确定。通过调节连杆的长度以及各关节转角范围，可改变该范围的大小，从而使该机械臂满足使用要求。

4 算例验证

选定机构质心M的坐标为（-200，100，0），机械臂末端x4轴的摆角为 y=60°，z=80°，由式（3）可得：

将 xm，ym，zm，nx，ny，nz代入已解出各反解方程，可得关节转角，如表2所示。

表2 反解结果Tab.2 The Results of Inverse Kinematics

由表2可知存在8组解，需要找到最优解，可根据以下准则寻优：

首先，排除虚数解的情况；其次，因为θ3有两组解，但并非所有的解都适合方程，可通过验证正向运动学模型是否成立来判断哪组解满足要求；最后，如果有多组解都满足要求，根据运动的连续性，各个关节转动角度最小的原则，选取与机械臂上一姿态最为接近的解作为最优解。

通过分析可知第1组解即为需要的最优解，由正解可解此时质心坐标为（-200，100，0），并且 nx=0.8484，ny=0.5，nz=0.1736，满足要求。

5 结论

提出了一种采用推力矢量控制方式的四自由度机械臂。首先分析了该机械臂异于常规机械臂的控制原理；其次，对该机械臂进行了运动学建模，并验证了结构的正确性；最后利用MATLAB软件对正向运动学进行了仿真分析，得到了一定杆长下质心坐标的范围，并对一定质心坐标下机械臂末端的摆动范围进行了分析。

结合推力矢量控制技术在军用飞机上大范围的应用以及目前机械臂的迅猛发展，提出的推力矢量控制机械臂为机械臂在飞行器上作为推进器的应用提供了一定的参考价值。