[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN109623812B - 考虑航天器本体姿态运动的机械臂轨迹规划方法 - Google Patents

考虑航天器本体姿态运动的机械臂轨迹规划方法 Download PDF

Info

Publication number
CN109623812B
CN109623812B CN201811470615.5A CN201811470615A CN109623812B CN 109623812 B CN109623812 B CN 109623812B CN 201811470615 A CN201811470615 A CN 201811470615A CN 109623812 B CN109623812 B CN 109623812B
Authority
CN
China
Prior art keywords
manipulator
spacecraft
attitude
trajectory
motion
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
CN201811470615.5A
Other languages
English (en)
Other versions
CN109623812A (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN201811470615.5A priority Critical patent/CN109623812B/zh
Publication of CN109623812A publication Critical patent/CN109623812A/zh
Application granted granted Critical
Publication of CN109623812B publication Critical patent/CN109623812B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，属于多体系统轨迹规划领域。本发明具体实现方法如下：首先在航天器初始状态下，通过五次多项式路径规划方法确定机械臂末端的位置和姿态指向的变化轨迹；根据航天器中心体姿态运动的规划，求取航天器本体坐标系下原期望位置与指向由于中心体姿态运动而产生的变化量，以此对机械臂的运动规划进行补偿；将机械臂末端的原轨迹规划与补偿规划对应求和，记为最终机械臂末端点轨迹，通过运动学伪逆算法对机械臂各关节角的运动轨迹进行规划，进而实现考虑航天器本体姿态运动的机械臂轨迹规划；本发明具有减轻星载计算机实时计算压力，提高轨迹规划效率等优点。

Description

考虑航天器本体姿态运动的机械臂轨迹规划方法
技术领域
本发明涉及一种考虑航天器姿态运动的机械臂轨迹规划方法，属于多体系统轨迹规划领域。
背景技术
携带机械臂的航天器系统在执行在轨操作过程中，控制目标通常为机械臂末端作动器的位置和姿态指向，而航天器系统模型中可以直接控制的物理量为航天器中心体的位置与姿态和各节机械臂关节的转角。因此，为了使末端作动器在空间中以某种姿态指向某期望位置，需要通过轨迹规划算法对机械臂的运动进行规划。在机械臂轨迹规划的相关研究中，出于工程测量的实际条件，机械臂的位置和姿态指向通常都是在航天器中心体坐标系中描述。但是实际任务中航天器中心体可能存在一定的姿态运动，因而当期望到达的空间位置和姿态指向不变的情况下，相对于航天器中心体，原期望位置和姿态指向也会发生变化。因此，在航天器中心体存在姿态运动的情况下，机械臂的轨迹规划中需要考虑航天器中心体的运动影响并加入相应的运动补偿。但是在现有的相关文献中，大多数研究只提及基于星载敏感器对目标的测量进行机械臂的实时运动补偿，这种方法运算量较大，对星载计算机实时运算能力提出了较高的要求，较少有文献对这种机械臂运动补偿进行开环规划。
发明内容
本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，要解决的问题是：机械臂上末端作动器的期望位置和指向在惯性空间中固定不变时，依据航天器中心体姿态运动信息补偿机械臂末端作动器的轨迹规划，使机械臂末端作动器能够在航天器中心体有姿态运动的情况下达到惯性空间中的原定期望位置和指向，实现考虑航天器本体姿态运动的机械臂轨迹规划，具有规划效率高的优点。
本发明目的是通过下述技术方案实现的。
本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，首先在航天器初始状态下，通过五次多项式路径规划方法确定机械臂上末端作动器的位置和姿态指向的变化轨迹。根据航天器中心体姿态运动的规划，求取航天器本体坐标系下原期望位置与指向由于中心体姿态运动而产生的变化量，以此对机械臂的运动规划进行补偿。将机械臂末端的原轨迹规划与补偿规划对应求和，记为最终机械臂末端点轨迹，通过运动学伪逆算法获得机械臂各关节角的运动轨迹，进而实现考虑航天器本体姿态运动的机械臂轨迹规划。该方法具有减轻实时计算压力，提高轨迹规划效率等优点。
本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，包括如下步骤：
步骤一：在航天器初始状态下，通过五次多项式路径规划方法确定机械臂上末端作动器的位置和姿态指向的变化轨迹。
在航天器初始状态下，定义初始时刻的服务航天器的中心体体坐标系fb0与惯性系fe重合，中心体体坐标系fb随中心体运动而运动。根据服务航天器中心体和机械臂的初始状态，获得机械臂末端作动器在本体系下的初始位置
Figure GDA0002596077670000021
和初始姿态指向
Figure GDA0002596077670000022
预设服务航天器中心体与机械臂运动速度的最大值、航天器中心体坐标系下的原期望位置
Figure GDA0002596077670000023
与原期望姿态指向
Figure GDA0002596077670000024
由于初始姿态指向
Figure GDA0002596077670000025
矢量和原期望姿态指向
Figure GDA0002596077670000027
矢量中分别具有不完全独立的三个分量，初始姿态指向
Figure GDA0002596077670000028
矢量和原期望姿态指向
Figure GDA0002596077670000026
矢量均通过方向余弦表示，矢量的模均为1，因而各选取前两个分量作为控制量，但是对应的第三个分量存在正负两种可能。为避免所述存在正负两种可能导致的不确定，将由方向余弦表示的姿态指向转化为空间中的两个方位角αn和αe，求取末端作动器空间方位角的一阶和二阶时间导数，再将其转化为方向余弦的一阶和二阶时间导数。定义方位角αn代表姿态指向
Figure GDA0002596077670000029
与本体系XbObZb平面之间的夹角，αe代表姿态指向
Figure GDA00025960776700000210
在本体系XbObZb平面上的投影与本体系 ObZb轴之间的夹角。
根据对方位角αn和αe的定义，有
Figure GDA0002596077670000031
其中：
Figure GDA0002596077670000032
表示姿态指向
Figure GDA0002596077670000033
中的第二个分量。
在基于姿态指向
Figure GDA0002596077670000034
求解方位角αe的过程中，为了避免分母为零产生的奇异，将方位角αe的求解过程定义为：
Figure GDA0002596077670000035
其中：
Figure GDA0002596077670000036
表示姿态指向
Figure GDA0002596077670000037
中的第二个分量，
Figure GDA0002596077670000038
表示姿态指向
Figure GDA0002596077670000039
中第三个分量。
通过末端作动器初始姿态指向
Figure GDA00025960776700000310
求得初始方位角αn_0和αe_0，通过末端作动器原期望姿态指向
Figure GDA00025960776700000311
求得原期望方位角αn_r0和αe_r0。将末端作动器原期望位置
Figure GDA00025960776700000312
和原期望方位角αn_r0和αe_r0与初始位置
Figure GDA00025960776700000313
和初始方位角αn_0和αe_0做差，得到末端作动器的原位置变化量与原方位角变化量。
在五次多项式规划方法中，根据变量在任务始末状态的差值和变化率限制，求出五次多项式的待定系数，进而得到规划量关于时间二阶平滑的变化过程。将规划量θ的期望值记为θr，其初始状态的值记为θ0，规定参数变化中一阶导数的最大值为
Figure GDA00025960776700000314
二阶导数的最大值为
Figure GDA00025960776700000315
则根据五次多项式，有：
Figure GDA00025960776700000316
其中，
Figure GDA00025960776700000317
时间系数τ为当前时间t与任务时长tf的比值。
根据五次多项式的特性与规划量变化率受限等约束条件，任务时长有如下约束：
Figure GDA0002596077670000041
选择同时满足公式所述条件中最小的一个作为任务所需最短时间，由此得到满足各个机械臂运动约束的最短任务时长tf。在得到任务时长tf之后，基于五次多项式的方法，规划量θ及其一阶和二阶时间导数为：
Figure GDA0002596077670000042
在机械臂轨迹规划过程中，规划量为机械臂末端作动器的位置
Figure GDA0002596077670000043
和姿态指向
Figure GDA0002596077670000044
按式所述的五次多项式的方法求得机械臂末端作动器位置
Figure GDA0002596077670000045
和方位角
Figure GDA0002596077670000046
和
Figure GDA0002596077670000047
光滑连续的一阶和二阶时间导数。
由于机械臂末端作动器的方位角
Figure GDA0002596077670000048
和
Figure GDA0002596077670000049
的变化难以写成显含机械臂关节角角速度的形式，而姿态指向
Figure GDA00025960776700000410
的轨迹则可以写成显含机械臂关节角角速度的形式，有利于机械臂关节轨迹规划。因此在通过机械臂逆运动学关系求解关节角度运动规律之前，将机械臂上末端作动器的方位角
Figure GDA00025960776700000411
和
Figure GDA00025960776700000412
及其轨迹转化为姿态指向
Figure GDA00025960776700000413
的变化轨迹。根据方位角
Figure GDA00025960776700000414
和
Figure GDA00025960776700000415
与姿态指向
Figure GDA00025960776700000416
之间的几何关系，有：
Figure GDA00025960776700000417
对姿态指向
Figure GDA00025960776700000418
求一阶和二阶时间导数，有
Figure GDA00025960776700000419
其中α为方位角
Figure GDA00025960776700000420
和
Figure GDA00025960776700000421
组成的矩阵，Φ为方位角α矩阵与姿态指向
Figure GDA00025960776700000422
之间的转换矩阵，具体写作：
Figure GDA0002596077670000051
至此，通过五次多项式方法规划得到末端作动器在笛卡尔空间下位置
Figure GDA0002596077670000052
与姿态指向
Figure GDA0002596077670000053
平滑连续的理想一阶和二阶时间导数矩阵
Figure GDA0002596077670000054
和
Figure GDA0002596077670000055
Figure GDA0002596077670000056
由于姿态指向
Figure GDA0002596077670000057
矢量中的三个分量不完全独立，令
Figure GDA0002596077670000058
表示姿态指向
Figure GDA0002596077670000059
矢量中的前两个分量，记为
Figure GDA00025960776700000510
其中：
Figure GDA00025960776700000511
公式求得的机械臂末端作动器位置
Figure GDA00025960776700000512
与姿态指向
Figure GDA00025960776700000513
平滑连续的一阶和二阶时间导数矩阵
Figure GDA00025960776700000514
和
Figure GDA00025960776700000515
即为通过五次多项式规划方法求得的机械臂末端作动器运动到原期望位置
Figure GDA00025960776700000516
与原期望姿态指向
Figure GDA00025960776700000517
的轨迹。
步骤二、根据航天器中心体姿态运动需求进行姿态运动规划，同时求取本体系下原期望位置
Figure GDA00025960776700000518
与姿态指向
Figure GDA00025960776700000519
由于中心体姿态运动而产生的变化轨迹，对机械臂的轨迹规划进行补偿。
航天器中心体中某一期望位置
Figure GDA00025960776700000520
用其所在的惯性空间位置矢量
Figure GDA00025960776700000521
与航天器中心体质心所在的惯性空间位置矢量R之间的相对位置表示，并且从惯性系转换到航天器本体系下的转换过程为：
Figure GDA00025960776700000522
其中：Abe表示惯性系到航天器本体系的坐标转换矩阵，
Figure GDA00025960776700000523
表示航天器中心体中某一期望位置
Figure GDA00025960776700000524
在惯性系下表示。
根据坐标系旋转关系，坐标转换矩阵Abe的变化率与航天器中心体转动角速度ωb之间的关系为：
Figure GDA00025960776700000525
则原期望点位置的相对运动一阶导数和二阶导数表示为：
Figure GDA0002596077670000061
惯性空间下的某一姿态指向efe转换到航天器本体系的过程为：
efb＝Abeefe (1.11)
根据坐标转换矩阵变化率与坐标系转动速度之间的关系，航天器本体系下该姿态指向efb的一阶和二阶时间导数为：
Figure GDA0002596077670000062
其中：Abe表示惯性系到航天器本体系的坐标转换矩阵，
Figure GDA0002596077670000063
表示航天器中心体转动角速度ωb的叉乘矩阵，具体写作：
Figure GDA0002596077670000064
由此得到航天器中心体在姿态运动的情况下，惯性空间中某一期望位置
Figure GDA0002596077670000065
和某一姿态指向efb在航天器中心体坐标系下变化的一阶和二阶时间导数，将其记为：
Figure GDA0002596077670000066
其中：
Figure GDA0002596077670000067
表示姿态指向矢量efb中的前两个分量。
式中求得的
Figure GDA0002596077670000068
和
Figure GDA0002596077670000069
为惯性空间中某一位置与姿态指向由于航天器中心体姿态运动而产生的变化轨迹，依据所述变化轨迹对机械臂的轨迹规划进行补偿。
步骤三：将步骤一与步骤二中得到的末端点运动轨迹对应求和，采用机械臂运动学伪逆算法求解各关节角的运动轨迹，实现考虑航天器本体姿态运动情况的机械臂轨迹规划。
将步骤一中得到的在航天器初始状态下机械臂末端点的轨迹
Figure GDA00025960776700000610
和
Figure GDA00025960776700000611
与步骤二中得到的航天器中心体姿态运动而产生的补偿量
Figure GDA00025960776700000612
和
Figure GDA00025960776700000613
对应求和，记为末端点轨迹
Figure GDA00025960776700000614
和
Figure GDA00025960776700000615
作为机械臂关节轨迹规划的输入：
Figure GDA0002596077670000071
基于末端作动器的位置和姿态指向与机械臂系统之间的运动学关系，通过对雅可比矩阵伪逆求解的方式，将机械臂在笛卡尔空间中的运动转化为关节构型空间下的运动。
将笛卡尔空间中末端作动器的位置与姿态指向矩阵记为
Figure GDA0002596077670000072
将各节机械臂关节角记为η，则位姿矩阵与关节角一阶导数之间具有如下关系：
Figure GDA0002596077670000073
其中J(η)为机械臂关节角的雅可比矩阵。
根据式，依据所求得的末端点轨迹
Figure GDA0002596077670000074
和
Figure GDA0002596077670000075
和已知的机械臂运动学关系，反向求解机械臂关节角的运动轨迹。在求解过程中对雅可比矩阵进行伪逆运算，求解的运算公式如下：
Figure GDA0002596077670000076
其中
Figure GDA0002596077670000077
通过公式获得机械臂各关节角的理想运动轨迹
Figure GDA0002596077670000078
和
Figure GDA0002596077670000079
使机械臂能够在航天器中心体进行姿态运动的情况下达到惯性空间中的原定期望位置和姿态指向，进而实现考虑航天器本体姿态运动的机械臂轨迹规划。
还包括步骤四：将步骤三中规划的机械臂各关节角轨迹
Figure GDA00025960776700000710
和
Figure GDA00025960776700000711
传递给控制器，通过控制器实现对机械臂的运动控制。
作为优选，为改善雅可比矩阵伪逆运算的过程中容易出现奇异的情况，在运算中加入鲁棒调节项，以牺牲精度为代价一定程度上规避奇异。鲁棒调节项中的影响调节权重的因子记为λ，λ的值越大，对雅克比矩阵的调节作用就越大，同时也意味着带来的误差较大。调整后的伪逆运算表达式中
Figure GDA00025960776700000712
为：
Figure GDA0002596077670000081
其中In×n表示单位矩阵，其维数与规划的末端点轨迹矩阵
Figure GDA0002596077670000082
和
Figure GDA0002596077670000083
的行数相同。
有益效果：
1、本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，在规划航天器中心体姿态运动的同时求取本体系下原惯性空间期望位置与姿态指向的变化，对机械臂的运动规划进行补偿，使得机械臂在中心体进行姿态运动时仍能到达惯性空间中原定期望位置与姿态指向。
2、本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，将航天器中心体姿态运动的作用影响考虑到机械臂的开环运动规划中，不必依靠期望位置与姿态指向的测量信息实时规划机械臂的运动，进而在有航天器姿态运动需求情况下降低机械臂轨迹规划的计算需求，减轻机械臂在轨实时轨迹规划的计算压力，提高规划效率。
3、本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，分别在两个传统求解方法中做了一点补充。其一，在通过姿态指向
Figure GDA0002596077670000084
求解方位角αe时，为了避免分母为零产生的奇异，在传统方位角的求取方法中增加一个判断，当姿态指向
Figure GDA0002596077670000085
中第一和第三个分量同时为0时定义方位角αe为0。其二，为降低机械臂关节角雅克比矩阵伪逆求解发生奇异的可能性，在伪逆求解过程中加入鲁棒调节项，以牺牲精度为代价一定程度上规避奇异。
附图说明
图1为本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法的流程示意图。
图2为末端作动器姿态指向矢量与方位角示意图。
图3(a)为x轴位置追踪结果图。
图3(b)为y轴位置追踪结果图。
图3(c)为z轴位置追踪结果图。
图4(a)为机械臂末端点位置与期望位置之间的差值图。
图4(b)为机械臂末端点姿态指向与期望姿态指向之间的差值图。
图5为机械臂轨迹规划过程中，机械臂关节角雅可比矩阵的行列式值在是否补充鲁棒调节项的对比。
具体实施方式
为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。
实施例1：
本实施例公开对一个五节臂杆组成的机械臂进行轨迹规划，针对有航天器中心体姿态运动任务情况，对机械臂末端位置及姿态指向进行轨迹规划与仿真。如图1所示，本实施例公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，可通过下述步骤来完成：
步骤一：在航天器初始状态下，通过五次多项式路径规划方法确定机械臂上末端作动器的位置和姿态指向的变化轨迹。
设航天器及其携带的机械臂相关参数的初值如表1所示。
表1航天器中心体与机械臂初始参数
Figure GDA0002596077670000091
定义方位角αn代表姿态指向
Figure GDA0002596077670000092
与本体系XbObZb平面之间的夹角，αe代表姿态指向
Figure GDA0002596077670000093
在本体系XbObZb平面上的投影与本体系ObZb轴之间的夹角。根据对方位角αn和αe的定义，有
Figure GDA0002596077670000101
其中：
Figure GDA0002596077670000102
表示姿态指向
Figure GDA0002596077670000103
中的第二个分量。
在基于姿态指向
Figure GDA0002596077670000104
求解方位角αe的过程中，为了避免分母为零产生的奇异，将方位角αe的求解过程定义为：
Figure GDA0002596077670000105
其中：
Figure GDA0002596077670000106
表示姿态指向
Figure GDA0002596077670000107
中的第二个分量，
Figure GDA0002596077670000108
表示姿态指向
Figure GDA0002596077670000109
中第三个分量。
根据表1所示数据，通过航天器运动学关系得到机械臂末端在航天器本体系下的初始位置
Figure GDA00025960776700001010
与方位角
Figure GDA00025960776700001011
和
Figure GDA00025960776700001012
组成的矩阵方位角矩阵α0为：
Figure GDA00025960776700001013
α0＝[14.94 95.14]Tdeg
中心体姿态、机械臂末端位置与姿态指向在始末状态间的差值如表2所示。
表2始末状态参数差值
Figure GDA00025960776700001014
在五次多项式规划方法中，根据变量在任务始末状态的差值和变化率限制，求出五次多项式的待定系数，进而得到规划量关于时间二阶平滑的变化过程。将规划量θ的期望值记为θr，其初始状态的值记为θ0，规定参数变化中一阶导数的最大值为
Figure GDA00025960776700001015
二阶导数的最大值为
Figure GDA00025960776700001016
则根据五次多项式，有：
Figure GDA00025960776700001017
其中，
Figure GDA0002596077670000111
时间系数τ为当前时间t与任务时长tf的比值。
根据五次多项式的特性与变量在任务始末状态下变化率等约束条件，对任务时长有如下约束：
Figure GDA0002596077670000112
代入机械臂关节运动等约束条件，得到同时满足航天器中心体运动和各个机械臂运动约束的最短任务时长tf为57.5s。
采用五次多项式的方法，对机械臂末端位置
Figure GDA0002596077670000113
和方位角αn、αe和航天器中心体姿态运动进行规划，规划量及其一阶和二阶导数表示为：
Figure GDA0002596077670000114
根据方位角αn和αe与姿态指向
Figure GDA0002596077670000115
之间的几何关系，有：
Figure GDA0002596077670000116
对姿态指向
Figure GDA0002596077670000117
求一阶和二阶时间导数，有
Figure GDA0002596077670000118
Figure GDA0002596077670000119
其中，Φ为方位角α矩阵与姿态指向
Figure GDA00025960776700001110
之间的转换矩阵，具体写作：
Figure GDA00025960776700001111
至此，通过五次多项式方法规划得到末端作动器在笛卡尔空间下位置
Figure GDA00025960776700001112
与姿态指向
Figure GDA00025960776700001113
平滑连续的理想一阶和二阶时间导数矩阵
Figure GDA00025960776700001114
和
Figure GDA0002596077670000121
Figure GDA0002596077670000122
由于姿态指向
Figure GDA0002596077670000123
矢量中的三个分量不完全独立，令
Figure GDA0002596077670000124
表示姿态指向
Figure GDA0002596077670000125
矢量中的前两个分量，记为
Figure GDA0002596077670000126
其中：
Figure GDA0002596077670000127
求得的机械臂末端作动器位置
Figure GDA0002596077670000128
与姿态指向
Figure GDA0002596077670000129
平滑连续的一阶和二阶时间导数矩阵
Figure GDA00025960776700001210
和
Figure GDA00025960776700001211
即为通过五次多项式规划方法求得的机械臂末端作动器运动到原期望位置
Figure GDA00025960776700001212
与原期望姿态指向
Figure GDA00025960776700001213
的轨迹。如图3所示图例为“p1”的实线，为仅进行原空间固定位置而不补偿追踪情况的结果。
步骤二：根据航天器中心体姿态运动需求进行姿态运动规划，同时求取本体系下原期望位置
Figure GDA00025960776700001214
与姿态指向
Figure GDA00025960776700001215
由于中心体姿态运动而产生的变化轨迹，对机械臂的轨迹规划进行补偿。
航天器中心体中某一期望位置
Figure GDA00025960776700001216
用其所在的惯性空间位置矢量
Figure GDA00025960776700001217
与航天器中心体质心所在的惯性空间位置矢量R之间的相对位置表示，并且从惯性系转换到航天器本体系下的转换过程为：
Figure GDA00025960776700001218
其中：Abe表示惯性系到航天器本体系的坐标转换矩阵，
Figure GDA00025960776700001219
表示航天器中心体中某一期望位置
Figure GDA00025960776700001220
在惯性系下表示。
根据坐标系旋转关系，坐标转换矩阵Abe的变化率与航天器中心体转动角速度ωb之间的关系为：
Figure GDA00025960776700001221
则原期望点位置的相对运动一阶导数和二阶导数表示为：
Figure GDA00025960776700001222
Figure GDA00025960776700001223
惯性空间下的某一姿态指向efe转换到航天器本体系的过程为：
efb＝Abeefe
根据坐标转换矩阵变化率与坐标系转动速度之间的关系，航天器本体系下该姿态指向efb的一阶和二阶时间导数为：
Figure GDA0002596077670000131
Figure GDA0002596077670000132
其中：Abe表示惯性系到航天器本体系的坐标转换矩阵，ωb表示航天器中心体转动角速度，其上方的波浪号为叉乘矩阵标记，
Figure GDA00025960776700001317
具体写作：
Figure GDA0002596077670000133
由此得到航天器中心体在姿态运动的情况下，惯性空间中某一期望位置
Figure GDA0002596077670000134
和某一姿态指向efb在航天器中心体坐标系下变化的一阶和二阶时间导数，将其记为：
Figure GDA0002596077670000135
其中：
Figure GDA0002596077670000136
表示姿态指向矢量efb中的前两个分量。
Figure GDA0002596077670000137
和
Figure GDA0002596077670000138
为惯性空间中某一位置与姿态指向由于航天器中心体姿态运动而产生的变化轨迹，依据所述变化轨迹对机械臂的轨迹规划进行补偿。
步骤三：将步骤一与步骤二中得到的末端点运动轨迹对应求和，采用机械臂运动学伪逆算法求解各关节角的运动轨迹，实现考虑航天器本体姿态运动情况的机械臂轨迹规划。
将步骤一中得到的在航天器初始状态下机械臂末端点的轨迹
Figure GDA0002596077670000139
和
Figure GDA00025960776700001310
与步骤二中得到的航天器中心体姿态运动而产生的补偿量
Figure GDA00025960776700001311
和
Figure GDA00025960776700001312
对应求和，记为末端点轨迹
Figure GDA00025960776700001313
和
Figure GDA00025960776700001314
作为机械臂关节轨迹规划的输入：
Figure GDA00025960776700001315
Figure GDA00025960776700001316
基于末端作动器的位置和姿态指向与机械臂系统之间的运动学关系，通过对雅可比矩阵伪逆求解的方式，将机械臂在笛卡尔空间中的运动转化为关节构型空间下的运动。将笛卡尔空间中末端作动器的位置与姿态指向矩阵记为
Figure GDA0002596077670000141
将各节机械臂关节角记为
Figure GDA00025960776700001411
则位姿矩阵与关节角一阶导数之间具有如下关系：
Figure GDA0002596077670000142
其中J(η)为机械臂关节角的雅可比矩阵：
Figure GDA0002596077670000143
式中Abj为第j节机械臂坐标系到本体系的坐标转换矩阵，Γj为第j节机械臂转轴方向矩阵，ejt为第j节机械臂与末端作动器之间姿态指向矢量，rjt为第j节机械臂质心到末端作动器质心的矢量。
为了使机械臂自由度数目满足位置与姿态指向的规划需求，使机械臂末端关节具有三个自由度。为了确定末端作动器的姿态指向，定义末端作动器相对上一节机械臂的转动角速度与其关节角度之间的关系与航天器姿态运动学中的“3-1-2”旋转方式下角速度和姿态角之间的关系相同。末端作动器坐标系f5与上一节机械臂坐标系f4之间转换矩阵为A45＝(Ay(θ(2))Ax(θ(1))Az(θ(3)))T，θ(k)表示姿态角θ第k个分量。
依据所求得的末端点轨迹
Figure GDA0002596077670000144
和
Figure GDA0002596077670000145
和已知的机械臂运动学关系，反向求解机械臂关节角的运动轨迹。在求解过程中对雅可比矩阵进行伪逆运算，求解的运算公式如下：
Figure GDA0002596077670000146
Figure GDA0002596077670000147
其中
Figure GDA0002596077670000148
得到的机械臂各关节角的理想运动轨迹
Figure GDA0002596077670000149
和
Figure GDA00025960776700001410
能够实现考虑航天器本体姿态运动的机械臂轨迹规划，使机械臂能够在航天器中心体进行姿态运动的情况下达到惯性空间中的原定期望位置和姿态指向。如图3(a)、图3(b)与图3(c)所示图例为“p12”的虚线，为依据中心体姿态运动进行机械臂运动补偿的结果，大幅度减小了不进行补偿规划时与期望目标之间的差值，提高了规划精度。如图4(a)和图4(b)所示为考虑航天器本体姿态运动的机械臂轨迹规划与期望位置与姿态指向之间的差值，最终收敛至零，说明能够达期望值。
还包括步骤四：将步骤三中规划的机械臂各关节角轨迹
Figure GDA0002596077670000151
和
Figure GDA0002596077670000152
传递给控制器，通过控制器实现对机械臂的运动控制。
作为优选，为改善雅可比矩阵伪逆运算的过程中容易出现奇异的情况，在运算中加入鲁棒调节项，以牺牲精度为代价一定程度上规避奇异。鲁棒调节项中的影响调节权重的因子记为λ，λ的值越大，对雅克比矩阵的调节作用就越大，同时也意味着带来的误差较大。修改后的伪逆运算表达式中
Figure GDA0002596077670000153
为：
Figure GDA0002596077670000154
式中I5×5表示五维的单位矩阵。
本实施例中取调节因子λ＝10-3。如图5所示为是否添加鲁棒调节项对雅可比矩阵行列式值的影响，越接近零越容易产生奇异。
在上述机械臂的轨迹规划方法下，机械臂末端能够在航天器中心体进行一定姿态运动的情况下到达惯性空间中某一固定的期望位置与姿态指向，不必依靠期望位置与姿态指向的测量信息实时规划机械臂的运动，减轻机械臂在轨实时轨迹规划的计算压力，提高规划效率。
以上所述的具体描述，对发明的目的、技术方案和有益效果进行进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：包括如下步骤，
步骤一：在航天器初始状态下，通过五次多项式路径规划方法确定机械臂末端作动器的位置和姿态指向的变化轨迹；
步骤二、根据航天器中心体姿态运动需求进行姿态运动规划，同时求取本体系下原期望位置
Figure FDA0002596077660000011
与姿态指向
Figure FDA0002596077660000012
由于中心体姿态运动而产生的变化轨迹，对机械臂的轨迹规划进行补偿；
步骤三：将步骤一与步骤二中得到的末端点运动轨迹对应求和，采用机械臂运动学伪逆算法求解各关节角的运动轨迹，实现考虑航天器本体姿态运动情况的机械臂轨迹规划；
步骤一具体实现方法为，
在航天器初始状态下，定义初始时刻的服务航天器的中心体体坐标系fb0与惯性系fe重合，中心体体坐标系fb随中心体运动而运动；根据服务航天器中心体和机械臂的初始状态，获得机械臂末端在本体系下的初始位置
Figure FDA0002596077660000013
和初始姿态指向
Figure FDA0002596077660000014
预设服务航天器中心体与机械臂运动速度的最大值、航天器中心体坐标系下的原期望位置
Figure FDA0002596077660000015
与原期望姿态指向
Figure FDA0002596077660000016
由于初始姿态指向
Figure FDA0002596077660000017
矢量和原期望姿态指向
Figure FDA0002596077660000018
矢量中均通过方向余弦表示且矢量的模均为1，使得初始姿态指向
Figure FDA0002596077660000019
矢量和原期望姿态指向
Figure FDA00025960776600000110
分别具有不完全独立的三个分量，因而各选取前两个分量作为变量，但是对应的第三个分量存在正负两种可能；为避免所述存在正负两种可能导致的不确定，将由方向余弦表示的姿态指向转化为空间中的两个方位角αn和αe，求取末端作动器空间方位角的一阶和二阶时间导数，再将其转化为方向余弦的一阶和二阶时间导数；定义方位角αn代表姿态指向
Figure FDA00025960776600000111
与本体系XbObZb平面之间的夹角，αe代表姿态指向
Figure FDA00025960776600000112
在本体系XbObZb平面上的投影与本体系ObZb轴之间的夹角；
根据对方位角αn和αe的定义，有
Figure FDA00025960776600000113
其中：
Figure FDA00025960776600000114
表示姿态指向
Figure FDA00025960776600000115
中的第二个分量；
在基于姿态指向
Figure FDA00025960776600000116
求解方位角αe的过程中，为了避免分母为零产生的奇异，将方位角αe的求解过程定义为：
Figure FDA0002596077660000021
其中：
Figure FDA0002596077660000022
表示姿态指向
Figure FDA0002596077660000023
中的第二个分量，
Figure FDA0002596077660000024
表示姿态指向
Figure FDA0002596077660000025
中第三个分量；
通过末端作动器初始姿态指向
Figure FDA0002596077660000026
求得初始方位角αn_0和αe_0，通过末端作动器原期望姿态指向
Figure FDA0002596077660000027
求得原期望方位角αn_r0和αe_r0；将末端作动器原期望位置
Figure FDA0002596077660000028
和原期望方位角αn_r0和αe_r0与初始位置
Figure FDA0002596077660000029
和初始方位角αn_0和αe_0做差，得到末端作动器的原位置变化量与原方位角变化量；
在五次多项式规划方法中，根据变量在任务始末状态的差值和变化率限制，求出五次多项式的待定系数，进而得到规划量关于时间二阶平滑的变化过程；将规划量θ的期望值记为θr，其初始状态的值记为θ0，规定参数变化中一阶导数的最大值为
Figure FDA00025960776600000210
二阶导数的最大值为
Figure FDA00025960776600000211
则根据五次多项式，有：
Figure FDA00025960776600000212
其中，
Figure FDA00025960776600000213
时间系数τ为当前时间t与任务时长tf的比值；
根据五次多项式的特性与规划量变化率受限等约束条件，任务时长有如下约束：
Figure FDA00025960776600000214
选择同时满足公式所述条件中最小的一个作为任务所需最短时间，由此得到满足各个机械臂运动约束的最短任务时长tf；在得到任务时长tf之后，基于五次多项式的方法，规划量θ及其一阶和二阶时间导数为：
Figure FDA0002596077660000031
在机械臂轨迹规划过程中，规划量为机械臂末端作动器的位置
Figure FDA0002596077660000032
和姿态指向
Figure FDA0002596077660000033
按式所述的五次多项式的方法求得机械臂末端作动器位置
Figure FDA0002596077660000034
和方位角
Figure FDA0002596077660000035
和
Figure FDA0002596077660000036
光滑连续的一阶和二阶时间导数；
由于机械臂末端作动器的方位角
Figure FDA0002596077660000037
和
Figure FDA0002596077660000038
的变化难以写成显含机械臂关节角角速度的形式，而姿态指向
Figure FDA0002596077660000039
的轨迹则可以写成显含机械臂关节角角速度的形式，有利于机械臂关节轨迹规划；因此在通过机械臂逆运动学关系求解关节角度运动规律之前，将机械臂上末端作动器的方位角
Figure FDA00025960776600000310
和
Figure FDA00025960776600000311
及其轨迹转化为姿态指向
Figure FDA00025960776600000312
的变化轨迹；根据方位角
Figure FDA00025960776600000313
和
Figure FDA00025960776600000314
与姿态指向
Figure FDA00025960776600000315
之间的几何关系，有：
Figure FDA00025960776600000316
对姿态指向
Figure FDA00025960776600000317
求一阶和二阶时间导数，有
Figure FDA00025960776600000318
其中α为方位角
Figure FDA00025960776600000319
和
Figure FDA00025960776600000320
组成的矩阵，Φ为方位角α矩阵与姿态指向
Figure FDA00025960776600000321
之间的转换矩阵，具体写作：
Figure FDA00025960776600000322
至此，通过五次多项式方法规划得到末端作动器在笛卡尔空间下位置
Figure FDA00025960776600000323
与姿态指向
Figure FDA00025960776600000324
平滑连续的理想一阶和二阶时间导数矩阵
Figure FDA00025960776600000325
和
Figure FDA00025960776600000326
Figure FDA00025960776600000327
由于姿态指向
Figure FDA00025960776600000328
矢量中的三个分量不完全独立，令
Figure FDA00025960776600000329
表示姿态指向
Figure FDA00025960776600000330
矢量中的前两个分量，记为
Figure FDA00025960776600000331
其中：
Figure FDA0002596077660000041
公式求得的机械臂末端作动器位置
Figure FDA0002596077660000042
与姿态指向
Figure FDA0002596077660000043
平滑连续的一阶和二阶时间导数矩阵
Figure FDA0002596077660000044
和
Figure FDA0002596077660000045
即为通过五次多项式规划方法求得的机械臂末端作动器运动到原期望位置
Figure FDA0002596077660000046
与原期望姿态指向
Figure FDA0002596077660000047
的轨迹。
2.如权利要求1所述的考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：还包括步骤四，将步骤三中规划的机械臂各关节角轨迹
Figure FDA0002596077660000048
和
Figure FDA0002596077660000049
传递给控制器，通过控制器实现对机械臂的运动控制。
3.如权利要求1所述的考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：步骤二具体实现方法为，
航天器中心体中某一期望位置
Figure FDA00025960776600000410
用其所在的惯性空间位置矢量
Figure FDA00025960776600000411
与航天器中心体质心所在的惯性空间位置矢量R之间的相对位置表示，并且从惯性系转换到航天器本体系下的转换过程为：
Figure FDA00025960776600000412
其中：Abe表示惯性系到航天器本体系的坐标转换矩阵，
Figure FDA00025960776600000413
表示航天器中心体中某一期望位置
Figure FDA00025960776600000414
在惯性系下表示；
根据坐标系旋转关系，坐标转换矩阵Abe的变化率与航天器中心体转动角速度ωb之间的关系为：
Figure FDA00025960776600000415
则原期望点位置的相对运动一阶导数和二阶导数表示为：
Figure FDA00025960776600000416
惯性空间下的某一姿态指向efe转换到航天器本体系的过程为：
efb＝Abeefe (0.11)
根据坐标转换矩阵变化率与坐标系转动速度之间的关系，航天器本体系下该姿态指向efb的一阶和二阶时间导数为：
Figure FDA00025960776600000417
其中：Abe表示惯性系到航天器本体系的坐标转换矩阵，
Figure FDA00025960776600000418
表示航天器中心体转动角速度ωb的叉乘矩阵你，具体写作：
Figure FDA0002596077660000051
由此得到航天器中心体在姿态运动的情况下，惯性空间中某一期望位置
Figure FDA0002596077660000052
和某一姿态指向efb在航天器中心体坐标系下变化的一阶和二阶时间导数，将其记为：
Figure FDA0002596077660000053
其中：
Figure FDA0002596077660000054
表示姿态指向矢量efb中的前两个分量；
式中求得的
Figure FDA0002596077660000055
和
Figure FDA0002596077660000056
为惯性空间中某一位置与姿态指向由于航天器中心体姿态运动而产生的变化轨迹，依据所述变化轨迹对机械臂的轨迹规划进行补偿。
4.如权利要求3所述的考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：步骤三具体实现方法为，
将步骤一中得到的在航天器初始状态下机械臂末端点的轨迹
Figure FDA0002596077660000057
和
Figure FDA0002596077660000058
与步骤二中得到的航天器中心体姿态运动而产生的补偿量
Figure FDA0002596077660000059
和
Figure FDA00025960776600000510
对应求和，记为末端点轨迹
Figure FDA00025960776600000511
和
Figure FDA00025960776600000512
作为机械臂关节轨迹规划的输入：
Figure FDA00025960776600000513
基于末端作动器的位置和姿态指向与机械臂系统之间的运动学关系，通过对雅可比矩阵伪逆求解的方式，将机械臂在笛卡尔空间中的运动转化为关节构型空间下的运动；
将笛卡尔空间中末端作动器的位置与姿态指向矩阵记为
Figure FDA00025960776600000514
将各节机械臂关节角记为η，则位姿矩阵与关节角一阶导数之间具有如下关系：
Figure FDA00025960776600000515
其中J(η)为机械臂关节角的雅可比矩阵；
根据式，依据所求得的末端点轨迹
Figure FDA00025960776600000516
和
Figure FDA00025960776600000517
和已知的机械臂运动学关系，反向求解机械臂关节角的运动轨迹；在求解过程中对雅可比矩阵进行伪逆运算，求解的运算公式如下：
Figure FDA0002596077660000061
其中
Figure FDA0002596077660000062
通过公式获得机械臂各关节角的理想运动轨迹
Figure FDA0002596077660000063
和
Figure FDA0002596077660000064
使机械臂能够在航天器中心体进行姿态运动的情况下达到惯性空间中的原定期望位置和姿态指向，进而实现考虑航天器本体姿态运动的机械臂轨迹规划。
5.如权利要求4所述的考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：为改善雅可比矩阵伪逆运算的过程中容易出现奇异的情况，在运算中加入鲁棒调节项，以牺牲精度为代价一定程度上规避奇异；鲁棒调节项中的影响调节权重的因子记为λ，λ的值越大，对雅克比矩阵的调节作用就越大，同时也意味着带来的误差较大；调整后的伪逆运算表达式中
Figure FDA0002596077660000065
为：
Figure FDA0002596077660000066
其中In×n表示单位矩阵，其维数与规划的末端点轨迹矩阵
Figure FDA0002596077660000067
和
Figure FDA0002596077660000068
的行数相同。
CN201811470615.5A 2018-12-04 2018-12-04 考虑航天器本体姿态运动的机械臂轨迹规划方法 Active CN109623812B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811470615.5A CN109623812B (zh) 2018-12-04 2018-12-04 考虑航天器本体姿态运动的机械臂轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811470615.5A CN109623812B (zh) 2018-12-04 2018-12-04 考虑航天器本体姿态运动的机械臂轨迹规划方法

Publications (2)

Publication Number Publication Date
CN109623812A CN109623812A (zh) 2019-04-16
CN109623812B true CN109623812B (zh) 2020-09-15

Family

ID=66070836

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811470615.5A Active CN109623812B (zh) 2018-12-04 2018-12-04 考虑航天器本体姿态运动的机械臂轨迹规划方法

Country Status (1)

Country Link
CN (1) CN109623812B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108068108B (zh) * 2016-11-16 2021-02-02 沈阳高精数控智能技术股份有限公司 平面内摆动伸缩型机器人运动控制方法
CN112318512B (zh) * 2020-11-03 2021-07-02 北京理工大学 一种机器鼠脊椎部的自由度确定方法及系统
CN114505865B (zh) * 2022-03-15 2024-06-25 上海大学 一种基于位姿跟踪的机械臂路径生成方法及系统
CN116494250B (zh) * 2023-06-26 2023-11-03 极限人工智能(北京)有限公司 基于速度补偿的机械臂控制方法、控制器、介质及系统
CN118493411B (zh) * 2024-07-18 2024-11-05 吉林省恒创智能装备有限公司 一种协作机器人最优控制方法、装置、介质及产品

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103235597A (zh) * 2013-04-09 2013-08-07 北京理工大学 一种航天器的快速姿态机动快速稳定联合控制方法
CN106891335A (zh) * 2017-03-23 2017-06-27 北京空间飞行器总体设计部 一种空间机器人在轨抓捕过程的柔顺与协调控制方法
CN108132601A (zh) * 2017-12-06 2018-06-08 西北工业大学 一种利用机械臂抑制航天器基座姿态干扰的方法
CN108214519A (zh) * 2017-12-18 2018-06-29 北京航空航天大学 一种空中任意姿态至落地姿态自调整的四足机器人
EP3351355A1 (en) * 2015-09-18 2018-07-25 Kawasaki Jukogyo Kabushiki Kaisha Device and method for positioning processing tool
CN108621162A (zh) * 2018-05-09 2018-10-09 广西科技大学 一种机械臂运动规划方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103235597A (zh) * 2013-04-09 2013-08-07 北京理工大学 一种航天器的快速姿态机动快速稳定联合控制方法
EP3351355A1 (en) * 2015-09-18 2018-07-25 Kawasaki Jukogyo Kabushiki Kaisha Device and method for positioning processing tool
CN106891335A (zh) * 2017-03-23 2017-06-27 北京空间飞行器总体设计部 一种空间机器人在轨抓捕过程的柔顺与协调控制方法
CN108132601A (zh) * 2017-12-06 2018-06-08 西北工业大学 一种利用机械臂抑制航天器基座姿态干扰的方法
CN108214519A (zh) * 2017-12-18 2018-06-29 北京航空航天大学 一种空中任意姿态至落地姿态自调整的四足机器人
CN108621162A (zh) * 2018-05-09 2018-10-09 广西科技大学 一种机械臂运动规划方法

Also Published As

Publication number Publication date
CN109623812A (zh) 2019-04-16

Similar Documents

Publication Publication Date Title
CN109623812B (zh) 考虑航天器本体姿态运动的机械臂轨迹规划方法
AU2021103765A4 (en) Robotic arm motion programming method based on fixed-parameter neural network
CN108381553B (zh) 一种用于空间非合作目标捕获的相对导航近距离跟踪方法及系统
CN107505947B (zh) 一种空间机器人捕获目标后消旋及协调控制方法
CN108508749A (zh) 一种用于抓捕非合作目标的空间机械臂系统抗干扰迭代学习控制方法
CN103869704B (zh) 基于扩展雅克比矩阵的空间机器人星臂协调控制方法
CN107263466B (zh) 空间机器人基于二次规划问题的基座无扰控制方法
CN108068113B (zh) 7-dof仿人臂飞行物体作业最小加速度轨迹优化
CN108227485A (zh) 一种带有执行器饱和的空间机器人自抗扰控制方法
CN109782601A (zh) 一种协调机械臂自适应神经网络同步鲁棒控制器设计方法
CN107450317A (zh) 一种空间机械臂自适应动力学协调控制方法
CN110340898A (zh) 一种具有指定跟踪性能的自由漂浮空间机械臂自适应容错控制方法
CN109426147A (zh) 捕获卫星后组合航天器的自适应增益调整控制方法
Jiang et al. Model-free control of flexible manipulator based on intrinsic design
Jun-Pei et al. Neural network control of space manipulator based on dynamic model and disturbance observer
CN104076685A (zh) 一种减少基座姿态扰动的空间机械臂路径规划方法
CN114671050B (zh) 基于一体化线性算子和抗饱和技术的航天器跟踪控制方法
CN109270955B (zh) 一种基于机械臂驱动的飞行器位姿耦合快速稳定控制方法
CN110641738A (zh) 一种空间五自由度自由飞行机械臂的轨迹跟踪控制方法
CN113954077A (zh) 带有能量优化的水下游动机械臂轨迹跟踪控制方法及装置
CN113305831A (zh) 空间机械臂自适应协调控制的位置观测器设计方法及系统
CN115256371B (zh) 一种基于神经网络的道路救援移动机械臂运动时间预设方法
Yan et al. Neural dynamics-based model predictive control for mobile redundant manipulators with improved obstacle avoidance
CN116880170A (zh) 一种基于自抗扰的无模型自适应胶囊机械臂控制方法
CN116540721A (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