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

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

Info

Publication number
CN110641738B
CN110641738B CN201910975424.2A CN201910975424A CN110641738B CN 110641738 B CN110641738 B CN 110641738B CN 201910975424 A CN201910975424 A CN 201910975424A CN 110641738 B CN110641738 B CN 110641738B
Authority
CN
China
Prior art keywords
mechanical arm
coordinate system
vector
joint
connecting rod
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910975424.2A
Other languages
English (en)
Other versions
CN110641738A (zh
Inventor
汪锐
吴桢
孙希明
吕宗阳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Dalian University of Technology
Original Assignee
Dalian University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Dalian University of Technology filed Critical Dalian University of Technology
Priority to CN201910975424.2A priority Critical patent/CN110641738B/zh
Publication of CN110641738A publication Critical patent/CN110641738A/zh
Application granted granted Critical
Publication of CN110641738B publication Critical patent/CN110641738B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
    • B64G1/00—Cosmonautic vehicles
    • B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
    • B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
    • B64G1/242—Orbits and trajectories

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明公开了一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法，可以解决安装在航天器上的五自由度空间自由飞行机械臂捕获和操纵空间物体的准确性问题。主要包括以下四个要点：1)应用D‑H方法建立机械臂的正运动学模型；2)应用拉格朗日方程建立机械臂的动力学模型；3)结合雅可比转置矩阵与PD控制器，引入广义力矢量；4)根据动力学微分方程，使用Simulink软件搭建仿真模型。本发明同时搭建了机械臂的运动学与动力学模型，应用雅可比转置矩阵结合PD控制器的方法，有效地解决了目前应用较为广泛的运动学控制方法无法很好地跟踪控制空间高速、高精度机械臂的问题。

Description

一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法
技术领域
本发明公开了一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法，可以解决安装在航天器上的空间五自由度自由飞行机械臂捕获和操纵空间物体的准确性问题。
背景技术
这几年，各个国家发射的卫星数量逐渐增加，为了确保各个卫星的正常工作，降低空间设备的成本以及延长卫星的使用寿命，就需要空间机器人在轨道上进行不断的检查，装配和维修等。因此，人们对于提高在轨服务的空间机器人系统的可靠性有迫切的需求。为了提高在轨机器人系统的机动性，科学家们提出了操纵器安装在航天器上的空间自由飞行机械臂(SFFR)。
对于空间自由飞行机械臂而言，实现捕获的准确性和操纵空间物体的准确性是现如今研究的重点问题，这种准确性的实现是以空间自由飞行机械臂的轨迹控制的准确性为基础的。空间五自由度自由飞行机械臂是目前应用最为广泛的空间自由飞行机械臂，它是由两个坐标位置和三个转动关节组成的机械装置。其中，机械臂的每一个转动关节都由相应的旋转电机来驱动。但由于在空间中，机械臂受到的外力可能来自不同的物体，亦不受重力影响，这就决定了空间自由飞行机械臂的特殊性。同时，它不能在地球表面环境内进行有效的实物仿真实验，所以在微重力环境中研究机械臂的动态特性，研究系统的运动学建模和动力学建模，并发明一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法是非常有必要的。
目前，对于机械臂的控制策略大多基于其运动学模型。在忽略机械臂动力学特性的情况下，只针对机械臂基座、关节和连杆的运动情况进行控制的方法称为机械臂的运动学控制。这种控制方法首先要求计算机械臂实际轨迹与理想轨迹之间的偏差，然后对机械臂进行负反馈控制，并且我们一般采用PD或PID控制作为我们的控制器。机械臂运动学控制方法的特点是控制规律简单，很容易通过软件来进行仿真实现，但是对于高速，高精度的机械臂控制来说，这种方法难以保证我们所控制的机械臂能够具有期望的静态与动态特性，仅仅依靠运动学方法对其进行控制是完全不够的。
相较于运动学控制方法，本发明除了对机械臂的位置信息进行控制之外，还对机械臂所受力与力矩进行分析建模，同时建立了机械臂的运动学模型与动力学模型。机械臂的动力学则是分析作用在机械臂上的力或者力矩对于机械臂运动轨迹的影响，从而建立作用在刚体上的力或者力矩与关节位移、速度和加速度之间的联系。这样得到的控制性能会有很大的提升。
发明内容
本发明应用于基座位于惯性坐标系X-Y平面的空间五自由度自由飞行机械臂。
本发明的技术方案：
一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法，
1)应用D-H方法建立机械臂的正运动学模型，构建机械臂末端位置与自由度之间的联系；2)建立拉格朗日方程，导出系统质量矩阵和非线性速度项矢量；3)结合雅可比转置矩阵与PD控制器，计算系统广义力矢量；4)构造动力学微分方程，实现轨迹跟踪控制；具体步骤如下：
(1)应用D-H方法建立机械臂的正运动学模型：
分别将基座坐标系、关节1处坐标系、关节2处坐标系编号为坐标系{0}、{1}、{2}，基座质心在惯性坐标系下的位置向量RC0＝[x0 y0 0]T，由于基座位于惯性坐标系的X-Y平面，故RC0的Z轴分量为0；令广义坐标向量
Figure BDA0002233459830000031
其中θ0、θ1、θ2分别表示基座旋转的欧拉角、关节1处的旋转角和关节2处的旋转角；
由图1可知，机械臂运动时坐标系均绕z轴转动；则：
坐标系{0}到惯性坐标系的旋转矩阵
Figure BDA0002233459830000032
坐标系{1}到坐标系{0}的旋转矩阵
Figure BDA0002233459830000033
坐标系{2}到坐标系{1}的旋转矩阵
Figure BDA0002233459830000034
坐标系{1}到惯性坐标系的旋转矩阵
Figure BDA0002233459830000035
；
坐标系{2}到惯性坐标系的旋转矩阵
Figure BDA0002233459830000036
如图1所示，关节1与惯性坐标系原点之间的距离为r0，关节1与连杆1质心之间的距离为l1，关节2与连杆1之间的距离为r1，关节2与连杆2之间的距离为l2，末端与连杆2之间的距离为r2；
由此，分别计算出关节1与惯性坐标系原点之间的距离为r0，关节2与关节1之间的距离为r1+l1，末端与关节2之间的距离为r2+l2；
于是得到机械臂末端位置坐标在惯性坐标系中表示为
Figure BDA0002233459830000041
将每一项分别对x0、y0、θ0、θ1和θ2进行求偏导运算，得到Jx矩阵；
Figure BDA0002233459830000042
Figure BDA0002233459830000043
(2)建立拉格朗日方程，导出系统质量矩阵和非线性速度项矢量：
由于SFFR的典型机动长度和持续时间相对较短，因此与控制力相比，由轨道力学引起的微重力和动态效应忽略不计；因此，在惯性坐标系中考虑系统运动，并且将系统势能取为零；这种系统的拉格朗日方程为
Figure BDA0002233459830000044
其中，N是机械臂的自由度数，T是系统动能，qi、
Figure BDA0002233459830000049
和Qi分别是第i个元素的广义坐标向量、广义速度和广义力；系统动能T的计算公式如下：
Figure BDA0002233459830000045
其中，M是系统总质量，
Figure BDA0002233459830000046
是机械臂上任意点P的速度；
系统动能由以下三部分组成：
Figure BDA0002233459830000047
Figure BDA0002233459830000048
Figure BDA0002233459830000051
其中，ω表示旋转角速度矢量，r表示系统中两点之间的位置向量，I表示机械臂各部分的转动惯量矢量；对于如图1所示的空间五自由度自由飞行机械臂系统，得到：
基座绕z轴旋转的角速度在基座坐标系{0}中的矢量表示为
Figure BDA0002233459830000052
连杆1的角速度在坐标系{1}中的矢量表示为
Figure BDA0002233459830000053
连杆2的角速度在坐标系{2}中的矢量表示为
Figure BDA0002233459830000054
关节1相对于惯性坐标系原点的位置向量为
r0＝[r0 cosθ0 r0 sinθ0 0]T (9)
第一个关节相对于连杆1质心的位置向量为
l1＝[-l1 cos(θ0+θ1) -l1 sin(θ0+θ1) 0]T (10)
第二个关节相对于连杆1质心的位置向量为
r1＝[r 1cos(θ0+θ1) r1 sin(θ0+θ1) 0]T (11)
第二个关节相对于连杆2质心的位置向量为
l2＝[-l2 cos(θ0+θ1+θ2) -l2 sin(θ0+θ1+θ2) 0]T (12)
末端相对于连杆2质心的位置向量为
r2＝[r2 cos(θ0+θ1+θ2) r2 sin(θ0+θ1+θ2) 0]T (13)
将式(3)-(5)分别代入拉格朗日方程，得到
Figure BDA0002233459830000061
Figure BDA0002233459830000062
Figure BDA0002233459830000063
其中，N为机械臂的自由度数，mk为第k个连杆的质量，
Figure BDA0002233459830000064
与q分别表示基座质心在惯性坐标系下的位置向量和广义坐标矢量；
提取式(14)-(16)中的加速度项
Figure BDA0002233459830000065
系数，得到系统质量矩阵
Figure BDA0002233459830000066
提取式(14)-(16)中的速度项
Figure BDA0002233459830000067
系数，得到系统非线性速度矢量
Figure BDA0002233459830000068
其中
Figure BDA0002233459830000071
和
Figure BDA0002233459830000072
其中，m＝1...n，n表示机械臂个数，本发明只考虑一台机械臂，故n为1；k＝1...Nm，Nm表示连杆个数，如图1所示，空间五自由度自由飞行机械臂有两条连杆，故Nm为2；I0为基座的转动惯量，
Figure BDA0002233459830000073
表示第m台机械臂中第k条连杆的转动惯量；
Figure BDA0002233459830000074
表示在惯性坐标系下的第k个连杆的质心位置矢量，其计算公式为
Figure BDA0002233459830000075
其中，m为机械臂个数，Nm为第m个机械臂的自由度数；
将式(9)-(13)代入式(21)，得到在惯性系中连杆1和连杆2的质心位置矢量分别为
rC1＝[r0 cosθ0+l1 cos(θ0+θ1) r0 sinθ0+l1 sin(θ0+θ1) 0]T (22)
Figure BDA0002233459830000076
在计算系统质量矩阵和系统非线性速度项矢量之前，对式(17)、(19)和(20)进行简化；
对式(17)进行简化结果为
Figure BDA0002233459830000081
在式(19)中，有
Figure BDA0002233459830000082
则式(19)的简化结果为
Figure BDA0002233459830000083
式(20)中的偏微分项结果均为0，则C2＝0；
将式(6)-(8)、式(22)、式(23)分别代入式(24)与式(25)，计算出系统质量矩阵H和系统非线性速度项矢量C中的每一项，结果如下所示：
Figure BDA0002233459830000084
其中，
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)；
Figure BDA0002233459830000091
Figure BDA0002233459830000092
Figure BDA0002233459830000093
Figure BDA0002233459830000094
Figure BDA0002233459830000095
Figure BDA0002233459830000096
Figure BDA0002233459830000097
其中，
C11＝C12＝0；
Figure BDA0002233459830000098
Figure BDA0002233459830000099
Figure BDA0002233459830000101
C21＝C22＝0；
Figure BDA0002233459830000102
Figure BDA0002233459830000103
Figure BDA0002233459830000104
C31＝C32＝0；
Figure BDA0002233459830000105
Figure BDA0002233459830000106
Figure BDA0002233459830000107
C41＝C42＝0；
Figure BDA0002233459830000108
Figure BDA0002233459830000109
Figure BDA00022334598300001010
C51＝C52＝C55＝0；
Figure BDA00022334598300001011
Figure BDA00022334598300001012
(3)结合雅可比转置矩阵与PD控制器，计算广义力矢量；
雅可比转置矩阵为
Figure BDA0002233459830000111
PD控制器的设计如下所示：
Figure BDA0002233459830000112
广义力矢量表示为雅可比转置矩阵与PD控制器输出的乘积Q＝JTu；
(4)构造动力学微分方程
Figure BDA0002233459830000113
其中C2＝0，则
Figure BDA0002233459830000114
由式(1)和式(26)，即搭建出图2所示的仿真模型；
最后，根据空间五自由度自由飞行机械臂的动力学微分方程，应用Simulink软件对模型进行仿真。
本发明的有益效果：本发明应用拉格朗日方程建立空间五自由度自由飞行机械臂的动力学模型，分析系统中力与力矩对机械臂轨迹的影响，应用雅可比转置矩阵与PD控制器结合的方法构造广义力矢量，对机械臂的轨迹进行控制。提高了常规基于运动学的机械臂轨迹控制方法的精度与速度，更适用于太空中对性能要求极高的空间自由飞行机械臂的轨迹跟踪控制。
附图说明
图1是空间五自由度自由飞行机械臂的结构示意图。图中坐标系X-Y表示惯性坐标系，坐标系X0-Y0、X1-Y1、X2-Y2分别是机械臂基座、第一个关节、第二个关节处的坐标系。RC0表示基座质心在惯性坐标系下的坐标向量，l1、r1分别表示第一个关节和第二个关节相对于第一根连杆质心的位置向量，l2、r2分别表示第二个关节和末端相对于第二根连杆质心的位置向量。
Figure BDA0002233459830000121
分别表示在惯性系中，连杆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·m2、0.333kg·m2和0.250kg·m2。
分别设定机械臂基座与末端运动的理想轨迹如下：
基座X轴方向运动的理想轨迹：
Figure BDA0002233459830000122
基座Y轴方向运动的理想轨迹：
Figure BDA0002233459830000131
基座旋转欧拉角的理想轨迹：
Figure BDA0002233459830000132
末端X轴方向运动的理想轨迹：
Figure BDA0002233459830000133
末端Y轴方向运动的理想轨迹：
Figure BDA0002233459830000134
令机械臂在初始时刻，基座位于惯性坐标系原点处，旋转角度为0，关节1处的旋转角度为
Figure BDA0002233459830000135
关节2处的旋转角度为
Figure BDA0002233459830000136
末端X轴方向初始位移1.5，末端Y轴方向上无初始位移。
同时，设定PD控制器的参数为
Figure BDA0002233459830000137
仿真结果如图2-图6所示，机械臂基座与末端的运动轨迹与理想轨迹之间的误差，在零点几秒的时间内均可以收敛到0。结果表明，本发明可以实现对空间五自由度自由飞行机械臂运动轨迹快速、稳定的跟踪控制，实现对空间物体高速、准确的抓取。

Claims (1)

1.一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法，
1)应用D-H方法建立机械臂的正运动学模型，构建机械臂末端位置与自由度之间的联系；2)建立拉格朗日方程，导出系统质量矩阵和非线性速度项矢量；3)结合雅可比转置矩阵与PD控制器，计算系统广义力矢量；4)构造动力学微分方程，实现轨迹跟踪控制；其特征在于，步骤如下：
(1)应用D-H方法建立机械臂的正运动学模型：
分别将基座坐标系、关节1处坐标系、关节2处坐标系编号为坐标系{0}、{1}、{2}，基座质心在惯性坐标系下的位置向量RC0＝[x0 y0 0]T，由于基座位于惯性坐标系的X-Y平面，故RC0的Z轴分量为0；令广义坐标向量
Figure FDA0002233459820000011
其中θ0、θ1、θ2分别表示基座旋转的欧拉角、关节1处的旋转角和关节2处的旋转角；
机械臂运动时坐标系均绕z轴转动；则：
坐标系{0}到惯性坐标系的旋转矩阵
Figure FDA0002233459820000012
坐标系{1}到坐标系{0}的旋转矩阵
Figure FDA0002233459820000013
坐标系{2}到坐标系{1}的旋转矩阵
Figure FDA0002233459820000014
坐标系{1}到惯性坐标系的旋转矩阵
Figure FDA0002233459820000015
；
坐标系{2}到惯性坐标系的旋转矩阵
Figure FDA0002233459820000021
关节1与惯性坐标系原点之间的距离为r0，关节1与连杆1质心之间的距离为l1，关节2与连杆1之间的距离为r1，关节2与连杆2之间的距离为l2，末端与连杆2之间的距离为r2；
由此，分别计算出关节1与惯性坐标系原点之间的距离为r0，关节2与关节1之间的距离为r1+l1，末端与关节2之间的距离为r2+l2；
于是得到机械臂末端位置坐标在惯性坐标系中表示为
Figure FDA0002233459820000022
将每一项分别对x0、y0、θ0、θ1和θ2进行求偏导运算，得到Jx矩阵；
Figure FDA0002233459820000023
Figure FDA0002233459820000024
(2)建立拉格朗日方程，导出系统质量矩阵和非线性速度项矢量：
由于SFFR的典型机动长度和持续时间相对较短，因此与控制力相比，由轨道力学引起的微重力和动态效应忽略不计；因此，在惯性坐标系中考虑系统运动，并且将系统势能取为零；这种系统的拉格朗日方程为
Figure FDA0002233459820000031
其中，N是机械臂的自由度数，T是系统动能，qi、
Figure FDA0002233459820000032
和Qi分别是第i个元素的广义坐标向量、广义速度和广义力；系统动能T的计算公式如下：
Figure FDA0002233459820000033
其中，M是系统总质量，
Figure FDA0002233459820000034
是机械臂上任意点P的速度；
系统动能由以下三部分组成：
Figure FDA0002233459820000035
Figure FDA0002233459820000036
Figure FDA0002233459820000037
其中，ω表示旋转角速度矢量，r表示系统中两点之间的位置向量，I表示机械臂各部分的转动惯量矢量；对于空间五自由度自由飞行机械臂系统，得到：基座绕z轴旋转的角速度在基座坐标系{0}中的矢量表示为
Figure FDA0002233459820000038
连杆1的角速度在坐标系{1}中的矢量表示为
Figure FDA0002233459820000039
连杆2的角速度在坐标系{2}中的矢量表示为
Figure FDA00022334598200000310
关节1相对于惯性坐标系原点的位置向量为
r0＝[r0cosθ0 r0sinθ0 0]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)分别代入拉格朗日方程，得到
Figure FDA0002233459820000041
Figure FDA0002233459820000042
Figure FDA0002233459820000043
其中，N为机械臂的自由度数，mk为第k个连杆的质量，
Figure FDA0002233459820000044
与q分别表示基座质心在惯性坐标系下的位置向量和广义坐标矢量；
提取式(14)-(16)中的加速度项
Figure FDA0002233459820000045
系数，得到系统质量矩阵
Figure FDA0002233459820000051
提取式(14)-(16)中的速度项
Figure FDA0002233459820000058
系数，得到系统非线性速度矢量
Figure FDA0002233459820000052
其中
Figure FDA0002233459820000053
和
Figure FDA0002233459820000054
其中，m＝1...n，n表示机械臂个数，本发明只考虑一台机械臂，故n为1；k＝1...Nm，Nm表示连杆个数，空间五自由度自由飞行机械臂有两条连杆，故Nm为2；I0为基座的转动惯量，
Figure FDA0002233459820000055
表示第m台机械臂中第k条连杆的转动惯量；
Figure FDA0002233459820000056
表示在惯性坐标系下的第k个连杆的质心位置矢量，其计算公式为
Figure FDA0002233459820000057
其中，m为机械臂个数，Nm为第m个机械臂的自由度数；
将式(9)-(13)代入式(21)，得到在惯性系中连杆1和连杆2的质心位置矢量分别为
Figure FDA0002233459820000061
Figure FDA0002233459820000062
在计算系统质量矩阵和系统非线性速度项矢量之前，对式(17)、(19)和(20)进行简化；
对式(17)进行简化结果为
Figure FDA0002233459820000063
在式(19)中，有
Figure FDA0002233459820000064
则式(19)的简化结果为
Figure FDA0002233459820000065
式(20)中的偏微分项结果均为0，则C2＝0；
将式(6)-(8)、式(22)、式(23)分别代入式(24)与式(25)，计算出系统质量矩阵H和系统非线性速度项矢量C中的每一项，结果如下所示：
Figure FDA0002233459820000071
其中，
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)；
Figure FDA0002233459820000072
Figure FDA0002233459820000073
Figure FDA0002233459820000074
Figure FDA0002233459820000075
Figure FDA0002233459820000076
Figure FDA0002233459820000077
Figure FDA0002233459820000078
其中，
C11＝C12＝0；
Figure FDA0002233459820000081
Figure FDA0002233459820000082
Figure FDA0002233459820000083
C21＝C22＝0；
Figure FDA0002233459820000084
Figure FDA0002233459820000085
Figure FDA0002233459820000086
C31＝C32＝0；
Figure FDA0002233459820000087
Figure FDA0002233459820000088
Figure FDA0002233459820000089
C41＝C42＝0；
Figure FDA00022334598200000810
Figure FDA00022334598200000811
Figure FDA00022334598200000812
C51＝C52＝C55＝0；
Figure FDA0002233459820000091
Figure FDA0002233459820000092
(3)结合雅可比转置矩阵与PD控制器，计算广义力矢量；
雅可比转置矩阵为
Figure FDA0002233459820000093
PD控制器的设计如下所示：
Figure FDA0002233459820000094
广义力矢量表示为雅可比转置矩阵与PD控制器输出的乘积Q＝JTu；
(4)构造动力学微分方程
Figure FDA0002233459820000095
其中C2＝0，则
Figure FDA0002233459820000096
由式(1)和式(26)，即搭建出仿真模型；
最后，根据空间五自由度自由飞行机械臂的动力学微分方程，应用Simulink软件对模型进行仿真。
CN201910975424.2A 2019-10-15 2019-10-15 一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法 Active CN110641738B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910975424.2A CN110641738B (zh) 2019-10-15 2019-10-15 一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910975424.2A CN110641738B (zh) 2019-10-15 2019-10-15 一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法

Publications (2)

Publication Number Publication Date
CN110641738A CN110641738A (zh) 2020-01-03
CN110641738B true CN110641738B (zh) 2022-08-09

Family

ID=69012875

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910975424.2A Active CN110641738B (zh) 2019-10-15 2019-10-15 一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法

Country Status (1)

Country Link
CN (1) CN110641738B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111872938B (zh) * 2020-07-30 2022-01-25 清华大学 空间三维大尺度运动学仿真系统及方法
CN113821935B (zh) * 2021-09-30 2024-02-02 合肥工业大学 基于对称约束的动力学模型的建立方法及其系统
CN114589691B (zh) * 2022-01-28 2023-04-07 苏州大学 三自由度纳米机器人操作系统动力学建模方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05238493A (ja) * 1992-03-03 1993-09-17 Hitachi Ltd 人工衛星の制御装置
JP2008036742A (ja) * 2006-08-03 2008-02-21 Tokyo Institute Of Technology 逐次検索法による６自由度ロボットアームの逆運動学、およびそれを使用するロボットのシステム、制御方法、プログラム
CN108052008A (zh) * 2018-01-10 2018-05-18 大连理工大学 一种倾转旋翼飞行器过渡态切换过程的几何最优控制器设计方法
CN108381553A (zh) * 2018-04-28 2018-08-10 北京空间飞行器总体设计部 一种用于空间非合作目标捕获的相对导航近距离跟踪方法及系统
CN208468429U (zh) * 2018-07-06 2019-02-05 中国人民解放军32032部队 一种新型的空间机械臂

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5506617B2 (ja) * 2009-12-28 2014-05-28 本田技研工業株式会社 ロボットの制御装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05238493A (ja) * 1992-03-03 1993-09-17 Hitachi Ltd 人工衛星の制御装置
JP2008036742A (ja) * 2006-08-03 2008-02-21 Tokyo Institute Of Technology 逐次検索法による６自由度ロボットアームの逆運動学、およびそれを使用するロボットのシステム、制御方法、プログラム
CN108052008A (zh) * 2018-01-10 2018-05-18 大连理工大学 一种倾转旋翼飞行器过渡态切换过程的几何最优控制器设计方法
CN108381553A (zh) * 2018-04-28 2018-08-10 北京空间飞行器总体设计部 一种用于空间非合作目标捕获的相对导航近距离跟踪方法及系统
CN208468429U (zh) * 2018-07-06 2019-02-05 中国人民解放军32032部队 一种新型的空间机械臂

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
五自由度机械臂建模与MATLAB仿真研究;陈思宇;《机电信息》;20160225(第06期);全文 *
五自由度机械臂的动力学建模与模糊滑模控制;李耀文等;《现代制造工程》;20160618(第06期);全文 *
五自由度机械臂的运动学分析及时滞控制;张晓艳等;《西安交通大学学报》;20131010(第10期);全文 *
五自由度机械臂运动学系统建模与仿真;吴硕等;《自动化与仪器仪表》;20170225(第02期);全文 *
具有初始动量的空间机械臂零反作用轨迹跟踪;廖志祥等;《中国空间科学技术》;20151025(第05期);全文 *
自由浮动冗余度空间机器人的姿态稳定控制;朱雷等;《机械科学与技术》;20080515(第05期);全文 *

Also Published As

Publication number Publication date
CN110641738A (zh) 2020-01-03

Similar Documents

Publication Publication Date Title
Zhang et al. Robust control of an aerial manipulator based on a variable inertia parameters model
CN110641738B (zh) 一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法
Arleo et al. Control of quadrotor aerial vehicles equipped with a robotic arm
Khalifa et al. Modeling and control of a new quadrotor manipulation system
CN109606753A (zh) 一种空间双臂机器人协同抓捕目标的控制方法
CN107263466B (zh) 空间机器人基于二次规划问题的基座无扰控制方法
Meng et al. Vibration suppression control of free-floating space robots with flexible appendages for autonomous target capturing
Nakanishi et al. Impedance control for free-flying space robots-basic equations and applications
Caccavale et al. Quaternion-based kinematic control of redundant spacecraft/manipulator systems
CN107169196B (zh) 空间机器人由末端执行器向基座的动力学建模方法
CN108469737B (zh) 一种空间非合作目标导航捕获的动力学控制方法及系统
Li et al. Dynamic modelling and control of a tendon-actuated lightweight space manipulator
Dongming et al. Impedance control of multi-arm space robot for the capture of non-cooperative targets
Ma et al. Hand-eye servo and impedance control for manipulator arm to capture target satellite safely
Xu et al. Modeling and planning of a space robot for capturing tumbling target by approaching the dynamic closest point
Ivanovic et al. Exploiting null space in aerial manipulation through model-in-the-loop motion planning
Zhang et al. Design of an aerial manipulator system applied to capture missions
Shi et al. Hybrid control of space robot in on-orbit screw-driving operation
CN111650836B (zh) 基于作业飞行机器人动态滑翔抓取物体的控制方法
Korpela et al. Hardware-in-the-loop verification for mobile manipulating unmanned aerial vehicles
Hu et al. Dynamic control of free‐floating coordinated space robots
Peng et al. Dynamic analysis of the compounded system formed by dual-arm space robot and the captured target
Six et al. Dynamic modeling and trajectory tracking controller of a novel flying parallel robot
CN111975770B (zh) 一种采用空间双臂机器人的自旋目标抓捕方法
CN105965508B (zh) 一种机器人宇航员稳定攀爬控制方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant