一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法与流程

本发明公开了一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法，可以解决安装在航天器上的空间五自由度自由飞行机械臂捕获和操纵空间物体的准确性问题。

背景技术：

这几年，各个国家发射的卫星数量逐渐增加，为了确保各个卫星的正常工作，降低空间设备的成本以及延长卫星的使用寿命，就需要空间机器人在轨道上进行不断的检查，装配和维修等。因此，人们对于提高在轨服务的空间机器人系统的可靠性有迫切的需求。为了提高在轨机器人系统的机动性，科学家们提出了操纵器安装在航天器上的空间自由飞行机械臂(sffr)。

对于空间自由飞行机械臂而言，实现捕获的准确性和操纵空间物体的准确性是现如今研究的重点问题，这种准确性的实现是以空间自由飞行机械臂的轨迹控制的准确性为基础的。空间五自由度自由飞行机械臂是目前应用最为广泛的空间自由飞行机械臂，它是由两个坐标位置和三个转动关节组成的机械装置。其中，机械臂的每一个转动关节都由相应的旋转电机来驱动。但由于在空间中，机械臂受到的外力可能来自不同的物体，亦不受重力影响，这就决定了空间自由飞行机械臂的特殊性。同时，它不能在地球表面环境内进行有效的实物仿真实验，所以在微重力环境中研究机械臂的动态特性，研究系统的运动学建模和动力学建模，并发明一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法是非常有必要的。

目前，对于机械臂的控制策略大多基于其运动学模型。在忽略机械臂动力学特性的情况下，只针对机械臂基座、关节和连杆的运动情况进行控制的方法称为机械臂的运动学控制。这种控制方法首先要求计算机械臂实际轨迹与理想轨迹之间的偏差，然后对机械臂进行负反馈控制，并且我们一般采用pd或pid控制作为我们的控制器。机械臂运动学控制方法的特点是控制规律简单，很容易通过软件来进行仿真实现，但是对于高速，高精度的机械臂控制来说，这种方法难以保证我们所控制的机械臂能够具有期望的静态与动态特性，仅仅依靠运动学方法对其进行控制是完全不够的。

相较于运动学控制方法，本发明除了对机械臂的位置信息进行控制之外，还对机械臂所受力与力矩进行分析建模，同时建立了机械臂的运动学模型与动力学模型。机械臂的动力学则是分析作用在机械臂上的力或者力矩对于机械臂运动轨迹的影响，从而建立作用在刚体上的力或者力矩与关节位移、速度和加速度之间的联系。这样得到的控制性能会有很大的提升。

技术实现要素：

本发明应用于基座位于惯性坐标系x-y平面的空间五自由度自由飞行机械臂。

本发明的技术方案：

一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法，

1)应用d-h方法建立机械臂的正运动学模型，构建机械臂末端位置与自由度之间的联系；2)建立拉格朗日方程，导出系统质量矩阵和非线性速度项矢量；3)结合雅可比转置矩阵与pd控制器，计算系统广义力矢量；4)构造动力学微分方程，实现轨迹跟踪控制；具体步骤如下：

(1)应用d-h方法建立机械臂的正运动学模型：

分别将基座坐标系、关节1处坐标系、关节2处坐标系编号为坐标系{0}、{1}、{2}，基座质心在惯性坐标系下的位置向量rc0＝[x0y00]^t，由于基座位于惯性坐标系的x-y平面，故rc0的z轴分量为0；令广义坐标向量其中θ0、θ1、θ2分别表示基座旋转的欧拉角、关节1处的旋转角和关节2处的旋转角；

由图1可知，机械臂运动时坐标系均绕z轴转动；则：

坐标系{0}到惯性坐标系的旋转矩阵

坐标系{1}到坐标系{0}的旋转矩阵

坐标系{2}到坐标系{1}的旋转矩阵

坐标系{1}到惯性坐标系的旋转矩阵

坐标系{2}到惯性坐标系的旋转矩阵

如图1所示，关节1与惯性坐标系原点之间的距离为r0，关节1与连杆1质心之间的距离为l1，关节2与连杆1之间的距离为r1，关节2与连杆2之间的距离为l2，末端与连杆2之间的距离为r2；

由此，分别计算出关节1与惯性坐标系原点之间的距离为r0，关节2与关节1之间的距离为r1+l1，末端与关节2之间的距离为r2+l2；

于是得到机械臂末端位置坐标在惯性坐标系中表示为

将每一项分别对x0、y0、θ0、θ1和θ2进行求偏导运算，得到jx矩阵；

(2)建立拉格朗日方程，导出系统质量矩阵和非线性速度项矢量：

由于sffr的典型机动长度和持续时间相对较短，因此与控制力相比，由轨道力学引起的微重力和动态效应忽略不计；因此，在惯性坐标系中考虑系统运动，并且将系统势能取为零；这种系统的拉格朗日方程为

其中，n是机械臂的自由度数，t是系统动能，qi、和qi分别是第i个元素的广义坐标向量、广义速度和广义力；系统动能t的计算公式如下：

其中，m是系统总质量，是机械臂上任意点p的速度；

系统动能由以下三部分组成：

其中，ω表示旋转角速度矢量，r表示系统中两点之间的位置向量，i表示机械臂各部分的转动惯量矢量；对于如图1所示的空间五自由度自由飞行机械臂系统，得到：

基座绕z轴旋转的角速度在基座坐标系{0}中的矢量表示为

连杆1的角速度在坐标系{1}中的矢量表示为

连杆2的角速度在坐标系{2}中的矢量表示为

关节1相对于惯性坐标系原点的位置向量为

r0＝[r0cosθ0r0sinθ00]^t(9)

第一个关节相对于连杆1质心的位置向量为

l1＝[-l1cos(θ0+θ1)-l1sin(θ0+θ1)0]^t(10)

第二个关节相对于连杆1质心的位置向量为

r1＝[r1cos(θ0+θ1)r1sin(θ0+θ1)0]^t(11)

第二个关节相对于连杆2质心的位置向量为

l2＝[-l2cos(θ0+θ1+θ2)-l2sin(θ0+θ1+θ2)0]^t(12)

末端相对于连杆2质心的位置向量为

r2＝[r2cos(θ0+θ1+θ2)r2sin(θ0+θ1+θ2)0]^t(13)

将式(3)-(5)分别代入拉格朗日方程，得到

其中，n为机械臂的自由度数，mk为第k个连杆的质量，与q分别表示基座质心在惯性坐标系下的位置向量和广义坐标矢量；

提取式(14)-(16)中的加速度项系数，得到系统质量矩阵

提取式(14)-(16)中的速度项系数，得到系统非线性速度矢量

其中

和

其中，m＝1...n，n表示机械臂个数，本发明只考虑一台机械臂，故n为1；k＝1...nm，nm表示连杆个数，如图1所示，空间五自由度自由飞行机械臂有两条连杆，故nm为2；i0为基座的转动惯量，表示第m台机械臂中第k条连杆的转动惯量；表示在惯性坐标系下的第k个连杆的质心位置矢量，其计算公式为

其中，m为机械臂个数，nm为第m个机械臂的自由度数；

将式(9)-(13)代入式(21)，得到在惯性系中连杆1和连杆2的质心位置矢量分别为

rc1＝[r0cosθ0+l1cos(θ0+θ1)r0sinθ0+l1sin(θ0+θ1)0]^t(22)

在计算系统质量矩阵和系统非线性速度项矢量之前，对式(17)、(19)和(20)进行简化；

对式(17)进行简化结果为

在式(19)中，有

则式(19)的简化结果为

式(20)中的偏微分项结果均为0，则c2＝0；

将式(6)-(8)、式(22)、式(23)分别代入式(24)与式(25)，计算出系统质量矩阵h和系统非线性速度项矢量c中的每一项，结果如下所示：

其中，

h11＝m0+m1+m2；

h12＝h21＝0；

h13＝h31＝-m1(r0sin(θ0)+l1sin(θ0+θ1))-m2(r0sin(θ0)+(r1+l1)sin(θ0+θ1)+l2sin(θ0+θ1+θ2))；

h14＝h41＝-m1l1sin(θ0+θ1)-m2((r1+l1)sin(θ0+θ1)+l2sin(θ0+θ1+θ2))；

h15＝h51＝-m2l2sin(θ0+θ1+θ2)；

h22＝m0+m1+m2；

h23＝h32＝m1(r0cos(θ0)+l1cos(θ0+θ1))+m2(r0cos(θ0)+(r1+l1)cos(θ0+θ1)+l2cos(θ0+θ1+θ2))；

h24＝h42＝m1l1cos(θ0+θ1)+m2((r1+l1)cos(θ0+θ1)+l2cos(θ0+θ1+θ2))；

h25＝h52＝m2l2cos(θ0+θ1+θ2)；

其中，

c11＝c12＝0；

c21＝c22＝0；

c31＝c32＝0；

c41＝c42＝0；

c51＝c52＝c55＝0；

(3)结合雅可比转置矩阵与pd控制器，计算广义力矢量；

雅可比转置矩阵为

pd控制器的设计如下所示：

广义力矢量表示为雅可比转置矩阵与pd控制器输出的乘积q＝j^tu；

(4)构造动力学微分方程

其中c2＝0，则

由式(1)和式(26)，即搭建出图2所示的仿真模型；

最后，根据空间五自由度自由飞行机械臂的动力学微分方程，应用simulink软件对模型进行仿真。

本发明的有益效果：本发明应用拉格朗日方程建立空间五自由度自由飞行机械臂的动力学模型，分析系统中力与力矩对机械臂轨迹的影响，应用雅可比转置矩阵与pd控制器结合的方法构造广义力矢量，对机械臂的轨迹进行控制。提高了常规基于运动学的机械臂轨迹控制方法的精度与速度，更适用于太空中对性能要求极高的空间自由飞行机械臂的轨迹跟踪控制。

附图说明

图1是空间五自由度自由飞行机械臂的结构示意图。图中坐标系x-y表示惯性坐标系，坐标系x0-y0、x1-y1、x2-y2分别是机械臂基座、第一个关节、第二个关节处的坐标系。rc0表示基座质心在惯性坐标系下的坐标向量，l1、r1分别表示第一个关节和第二个关节相对于第一根连杆质心的位置向量，l2、r2分别表示第二个关节和末端相对于第二根连杆质心的位置向量。分别表示在惯性系中，连杆1和连杆2的质心位置矢量。

图2是仿真示例所得到的基座x轴方向的运动轨迹与理想轨迹之间的误差曲线。

图3是仿真示例所得到的基座y轴方向的运动轨迹与理想轨迹之间的误差曲线。

图4是仿真示例所得到的基座欧拉角的运动轨迹与理想轨迹之间的误差曲线。

图5是仿真示例所得到的末端x轴方向的运动轨迹与理想轨迹之间的误差曲线。

图6是仿真示例所得到的末端y轴方向的运动轨迹与理想轨迹之间的误差曲线。

具体实施方式

以下结合附图和技术方案，进一步说明本发明的具体实施方式。

设定机械臂基座的质量m0、连杆1的质量m1和连杆2的质量m2分别为40kg、4kg、3kg；关节1与惯性坐标系原点之间的距离r0、关节1与连杆1质心之间的距离l1、关节2与连杆1之间的距离r1、关节2与连杆2之间的距离l2、末端与连杆2之间的距离r2长度均为0.5m；基座转动惯量i0、连杆1转动惯量i1、连杆2转动惯量i2分别为6.667kg·m²、0.333kg·m²和0.250kg·m²。

分别设定机械臂基座与末端运动的理想轨迹如下：

基座x轴方向运动的理想轨迹：基座y轴方向运动的理想轨迹：基座旋转欧拉角的理想轨迹：末端x轴方向运动的理想轨迹：末端y轴方向运动的理想轨迹：

令机械臂在初始时刻，基座位于惯性坐标系原点处，旋转角度为0，关节1处的旋转角度为关节2处的旋转角度为末端x轴方向初始位移1.5，末端y轴方向上无初始位移。

同时，设定pd控制器的参数为

仿真结果如图2-图6所示，机械臂基座与末端的运动轨迹与理想轨迹之间的误差，在零点几秒的时间内均可以收敛到0。结果表明，本发明可以实现对空间五自由度自由飞行机械臂运动轨迹快速、稳定的跟踪控制，实现对空间物体高速、准确的抓取。