CN116330300A - 一种面向动态目标捕获的飞行机械臂运动轨迹计算方法 - Google Patents

一种面向动态目标捕获的飞行机械臂运动轨迹计算方法 Download PDF

Info

Publication number
CN116330300A
CN116330300A CN202310602936.0A CN202310602936A CN116330300A CN 116330300 A CN116330300 A CN 116330300A CN 202310602936 A CN202310602936 A CN 202310602936A CN 116330300 A CN116330300 A CN 116330300A
Authority
CN
China
Prior art keywords
mechanical arm
vector
flying
target
joint
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.)
Granted
Application number
CN202310602936.0A
Other languages
English (en)
Other versions
CN116330300B (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.)
Xianheng International Hangzhou Aviation Automation Co ltd
Hangzhou Innovation Research Institute of Beihang University
Original Assignee
Xianheng International Hangzhou Aviation Automation Co ltd
Hangzhou Innovation Research Institute of Beihang University
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 Xianheng International Hangzhou Aviation Automation Co ltd, Hangzhou Innovation Research Institute of Beihang University filed Critical Xianheng International Hangzhou Aviation Automation Co ltd
Priority to CN202310602936.0A priority Critical patent/CN116330300B/zh
Publication of CN116330300A publication Critical patent/CN116330300A/zh
Application granted granted Critical
Publication of CN116330300B publication Critical patent/CN116330300B/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
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明提供一种面向动态目标捕获的飞行机械臂运动轨迹计算方法，目标是协调规划无人机与机械臂的运动轨迹，实现对动态目标的飞行捕获，以及利用系统的冗余自由度特性，完成额外的任务指标并满足系统状态约束。为此，首先根据目标的当前位置测量值，利用卡尔曼滤波预测其未来一段时域内的轨迹。其次，将轨迹预测值作为参考信号，利用线性模型预测控制，计算飞行机械臂末端的期望加速度。随后，建立各个任务目标矢量与飞行机械臂关节矢量间的映射关系，并构造任务代价函数；最后，利用二次规划求解任务代价函数，根据末端的期望加速度计算飞行机械臂的各个关节的运动轨迹，实现飞行机械臂的协调运动。本发明能够协调规划飞行机械臂的运动轨迹。

Description

一种面向动态目标捕获的飞行机械臂运动轨迹计算方法
技术领域
本发明属于无人机与机器人的运动规划领域，具体涉及一种面向动态目标捕获的飞行机械臂运动轨迹计算方法，适用于搭载多自由度串联机械臂进行主动式飞行作业任务的飞行机械臂系统。
背景技术
近十年来，一类新型空中机器人——飞行机械臂到了学术界与工业界的广泛关注。其结构新颖，通常由无人机和机械臂组成，在空中抓取、设备装配、设施检测、维修等主动物理交互场景中有着广泛的应用前景。飞行机械臂在运动学层面具有冗余特性，在执行任务时往往存在多种不同的系统构型；机械臂与无人机在动力学层面具有耦合效应，机械臂的运动会导致系统质心偏移，影响系统的飞行性能。飞行机械臂执行作业任务时，系统状态往往受到约束，如：系统的加速度、速度、位置约束等等，添加各类约束是为了避免系统状态超出系统的物理极限或安全边界。
现有技术发明中，鲜有针对动态目标抓取的场景提出技术解决方案。而针对飞行机械臂协调规划，中国发明专利申请CN201810477920.0设计了一种搭载了冗余自由度机械臂的飞行机械臂系统，采用二次规划的方法对机械臂进行运动规划。为了减小机械臂与无人机在动力学层面的耦合效应，中国发明专利申请CN201810094313.6提出了一种基于动态质心补偿的控制算法，通过建立质心偏移模型，计算出质心偏移造成的干扰力矩，并在控制回路中进行补偿。
然而现有的发明未能在全面地利用系统的运动学冗余的同时，抑制系统的质心偏移造成的动力学耦合干扰；也未将飞行机械臂的任务拓展到动态目标抓取等较为复杂的场景中去。
发明内容
针对现有技术发明中存在的问题，本发明提出一种面向动态目标捕获的飞行机械臂运动轨迹计算方法。首先根据目标的当前位置测量值，利用卡尔曼滤波对其未来一段时域内的轨迹进行预测。其次，将目标的轨迹预测值作为参考信号，利用线性模型预测控制的思想，计算出飞行机械臂末端的期望加速度信号。随后，建立各个任务目标矢量与飞行机械臂关节矢量间的映射关系，并构造任务代价函数；最后，利用二次规划的思想对任务代价函数进行求解，根据末端的期望加速度信号解出飞行机械臂的各个关节的运动轨迹，实现飞行机械臂的协调运动。本发明既考虑了无人机平台与机械臂的轨迹协调，同时也从规划层面满足各类任务的约束条件、最大程度地减小系统的质心偏移。
为达到上述目的，本发明采用如下技术方案：
一种面向动态目标捕获的飞行机械臂运动轨迹计算方法，包括如下步骤：
第一步、根据卡尔曼滤波模型，对目标在未来一段时域内轨迹进行预测；
第二步、将目标的轨迹预测值作为参考信号，利用线性模型预测控制的方法，计算出飞行机械臂末端的期望加速度信号；
第三步、建立各个任务目标矢量与飞行机械臂关节矢量间的映射关系，并构造任务代价函数；
第四步、利用分层二次规划得到方法对任务代价函数进行求解，根据末端的期望加速度信号解出飞行机械臂的各个关节的运动轨迹。
进一步地，所述第一步包括：
对于三维惯性坐标系的
Figure SMS_1
轴，用如下二阶线性模型作为动态目标的运动方程：
Figure SMS_2
，
其中，
Figure SMS_4
，/>
Figure SMS_7
，/>
Figure SMS_8
分别代表目标沿/>
Figure SMS_5
轴的位置、速度、加速度矢量；/>
Figure SMS_9
为采样间隔；下标/>
Figure SMS_10
，/>
Figure SMS_11
为采样时刻；/>
Figure SMS_3
为/>
Figure SMS_6
时刻的过程噪声，服从高斯分布；记：
Figure SMS_12
，/>
Figure SMS_13
；
其中，
Figure SMS_14
为状态转移矩阵，/>
Figure SMS_15
为输入矩阵。
运动目标的位置观测值由机载传感器（如相机、雷达等）测得，观测方程表示为：
Figure SMS_16
；
其中，
Figure SMS_17
为目标沿/>
Figure SMS_18
轴的位置的观测值，/>
Figure SMS_19
为观测噪声，服从高斯分布；记：
Figure SMS_20
；；
其中，
Figure SMS_21
为观测矩阵。
令
Figure SMS_22
，根据卡尔曼滤波原理，目标在/>
Figure SMS_23
时刻的位置
Figure SMS_24
由以下公式计算得出：
Figure SMS_25
；
其中，
Figure SMS_33
，/>
Figure SMS_28
分别代表过程噪声与观测噪声的协方差矩阵；/>
Figure SMS_34
为利用二次线性模型递推得到的目标在/>
Figure SMS_32
时刻的位置估计，/>
Figure SMS_37
为/>
Figure SMS_38
时刻的目标位置观测值，/>
Figure SMS_39
为/>
Figure SMS_30
时刻的目标位置预测，/>
Figure SMS_35
与/>
Figure SMS_26
分别为/>
Figure SMS_41
时刻与/>
Figure SMS_29
时刻预测误差的协方差矩阵，/>
Figure SMS_40
为/>
Figure SMS_31
时刻预测误差的协方差矩阵的估计，/>
Figure SMS_36
为卡尔曼增益矩阵，/>
Figure SMS_27
为单位矩阵；
通过卡尔曼滤波得到目标在
Figure SMS_42
时刻的位置预测/>
Figure SMS_43
后，进一步利用二阶线性模型对目标位置进行预测：
Figure SMS_44
；
其中，
Figure SMS_45
与/>
Figure SMS_46
（/>
Figure SMS_47
）表示未来某一时刻；
目标在三维惯性坐标系的
Figure SMS_48
轴，/>
Figure SMS_49
轴的预测轨迹可类似地表示为：
Figure SMS_50
，
Figure SMS_51
；
其中，
Figure SMS_54
，/>
Figure SMS_57
，/>
Figure SMS_58
分别代表目标沿/>
Figure SMS_53
轴的位置、速度、加速度矢量；/>
Figure SMS_55
，/>
Figure SMS_56
，/>
Figure SMS_59
分别代表目标沿/>
Figure SMS_52
轴的位置、速度、加速度矢量；
进一步地，所述第二步包括：
定义飞行机械臂末端的位置矢量
Figure SMS_60
与速度矢量/>
Figure SMS_61
；构建如下离散线性预测模型：
Figure SMS_62
；
其中，
Figure SMS_63
为模型的状态向量；/>
Figure SMS_64
，/>
Figure SMS_65
，/>
Figure SMS_66
分别表示时刻飞行机械臂末端的位置、速度、加速度矢量；/>
Figure SMS_67
表示矩阵或向量的转置操作；记/>
Figure SMS_68
为模型输入，即末端的加速度轨迹；
利用离散线性预测模型，构建如下最优控制问题求解
Figure SMS_69
：
Figure SMS_70
，
Figure SMS_71
；
其中，
Figure SMS_74
为预测时域长度；/>
Figure SMS_78
为求和函数；/>
Figure SMS_82
表示2-范数；上标/>
Figure SMS_73
代表期望值；/>
Figure SMS_77
为/>
Figure SMS_80
时刻末端的参考信号（由卡尔曼滤波预测得到）；/>
Figure SMS_84
，/>
Figure SMS_72
与/>
Figure SMS_76
代表权重矩阵；/>
Figure SMS_81
与/>
Figure SMS_85
代表模型状态的上下界；/>
Figure SMS_75
与/>
Figure SMS_79
代表控制输入的上下界；/>
Figure SMS_83
表示求取/>
Figure SMS_86
使目标函数达到最小值；
求解上述最优控制问题，得到飞行机械臂末端的期望加速度信号。
进一步地，所述第三步包括：
假定飞行机械臂在执行任务过程中不进行大机动飞行，因此无人机的俯仰角、滚转角均视为常值
Figure SMS_87
；在此基础上，定义飞行机械臂的关节矢量/>
Figure SMS_88
。其中，/>
Figure SMS_89
为无人机位置矢量；/>
Figure SMS_90
为无人机偏航角；/>
Figure SMS_91
为机械臂关节角矢量；
（1）对于末端跟踪任务，任务矢量等价为飞行机械臂末端位置矢量
Figure SMS_92
，其与飞行机械臂关节矢量间的映射关系用以下非线性方程表示：
Figure SMS_93
；
其中，
Figure SMS_94
为飞行机械臂的关节矢量/>
Figure SMS_95
到飞行机械臂末端位置矢量/>
Figure SMS_96
的非线性映射函数。
将方程两边同时对时间求导，得到：
Figure SMS_97
；
进一步对时间求导，得到：
Figure SMS_98
；
其中，
Figure SMS_99
为对应的任务雅各比矩阵；/>
Figure SMS_100
为/>
Figure SMS_101
对时间的导数；/>
Figure SMS_102
与/>
Figure SMS_103
分别为飞行机械臂的关节速度与加速度；
将上式离散化，得到：
Figure SMS_104
；
其中，
Figure SMS_107
，/>
Figure SMS_109
，/>
Figure SMS_111
，/>
Figure SMS_106
，/>
Figure SMS_108
，/>
Figure SMS_110
，/>
Figure SMS_112
为对应矩阵或矢量在/>
Figure SMS_105
时刻的值。
为确保实现末端对目标的精确跟踪与抓取，构造如下代价函数：
Figure SMS_113
；
其中，
Figure SMS_114
与/>
Figure SMS_115
代表/>
Figure SMS_116
时刻飞行机械臂末端的期望位置与期望速度，设置为目标的位置与速度；/>
Figure SMS_117
表示求取/>
Figure SMS_118
使目标函数达到最小值；
（2）对于最小化质心偏移任务，令任务矢量为飞行机械臂在水平方向的质心偏移位置矢量
Figure SMS_119
，其与飞行机械臂关节矢量间的映射关系用以下非线性方程表示：
Figure SMS_120
；
其中，
Figure SMS_121
为飞行机械臂的关节矢量/>
Figure SMS_122
到质心偏移位置矢量/>
Figure SMS_123
的非线性映射函数。
将方程两边同时对时间求导，得到：
Figure SMS_124
；
进一步对时间求导，得到：
Figure SMS_125
；
其中，
Figure SMS_126
为对应的任务雅各比矩阵；/>
Figure SMS_127
为/>
Figure SMS_128
对时间的导数；
将上式离散化，可得：
Figure SMS_129
；
其中，
Figure SMS_130
，/>
Figure SMS_131
，/>
Figure SMS_132
，/>
Figure SMS_133
，/>
Figure SMS_134
为对应矩阵或矢量在/>
Figure SMS_135
时刻的值。
为确保实现末端对目标的精确跟踪与抓取，构造如下代价函数：
Figure SMS_136
；
其中，
Figure SMS_137
与/>
Figure SMS_138
代表/>
Figure SMS_139
时刻质心偏移的期望位置与期望速度；令
Figure SMS_140
，/>
Figure SMS_141
由下式计算：
Figure SMS_142
；
其中，
Figure SMS_143
为增益矩阵。
进一步地，所述第四步包括：
（1）将离散模型方程
Figure SMS_144
，
带入代价函数
Figure SMS_145
，展开后可得二次规划问题①：
Figure SMS_146
；
其中，
Figure SMS_147
，
Figure SMS_148
Figure SMS_149
；
优化函数的约束条件为：
Figure SMS_150
；
其中，
Figure SMS_153
是模型预测控制求解得到的期望末端加速度；/>
Figure SMS_155
是飞行机械臂的关节加速度的上下界；/>
Figure SMS_157
与/>
Figure SMS_151
是飞行机械臂的关节速度的上下界；/>
Figure SMS_154
与/>
Figure SMS_156
是飞行机械臂的关节w位置的上下界；/>
Figure SMS_158
与/>
Figure SMS_152
是增益矩阵；
（2）将离散模型方程
Figure SMS_159
，
带入代价函数
Figure SMS_160
，展开后可得二次规划问题模型②：
Figure SMS_161
；
其中，
Figure SMS_162
，
Figure SMS_163
二次规划问题②采用与二次规划问题①相同的约束条件, 在此基础上，将二次规划问题①的解添加到二次规划问题模型②中作为一项等式约束：
Figure SMS_164
；
其中，
Figure SMS_165
是二次规划问题①的解；
求解二次规划问题②，得到飞行机械臂系统各个自由度的期望加速度
Figure SMS_166
轨迹。
本发明与现有技术相比的有益效果在于：
本发明充分利用目标当前位置信息与未来轨迹的预测信息，生成飞行机械臂末端的期望加速度信号。进一步地，本发明采用数值优化的理论方法，通过分层二次规划的思想，根据飞行机械臂末端的期望加速度信号求出系统各个关节的期望加速度轨迹。在优化求解过程中，充分考虑了系统的状态约束，如位置、速度、加速度的上下界。此外，本方法还充分利用了系统的运动学冗余特性，在实现末端对动态目标跟踪的同时，最小化了系统的质心偏移，提高飞行机械臂的飞行稳定性与作业性能。
附图说明
图1为本发明提出的一种面向动态目标捕获的飞行机械臂运动轨迹计算方法的流程图；
图2 为本发明针对的飞行机械臂系统结构图。
具体实施方式
为了使本发明的目的、技术方案及优点更为清晰，以下结合附图及实施例，对本发明进行详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
如图1所示，本发明的一种面向动态目标捕获的飞行机械臂运动轨迹计算方法具体包括如下步骤：
第一步，根据卡尔曼滤波模型，对目标在未来一段时域内轨迹进行预测：
对于三维惯性坐标系的
Figure SMS_167
轴，用如下二阶线性模型作为动态目标的运动方程：
Figure SMS_168
；
其中，
Figure SMS_171
，/>
Figure SMS_173
，/>
Figure SMS_176
分别代表目标沿/>
Figure SMS_170
轴的位置、速度、加速度矢量；/>
Figure SMS_172
为采样间隔；下标/>
Figure SMS_174
，/>
Figure SMS_177
为采样时刻；/>
Figure SMS_169
为/>
Figure SMS_175
时刻的过程噪声，服从高斯分布；记：
Figure SMS_178
，/>
Figure SMS_179
；
其中，
Figure SMS_180
为状态转移矩阵，/>
Figure SMS_181
为输入矩阵。
运动目标的位置观测值由机载传感器（如相机、雷达等）测得，观测方程表示为：
Figure SMS_182
；
其中，
Figure SMS_183
为目标沿/>
Figure SMS_184
轴的位置的观测值，/>
Figure SMS_185
为观测噪声，服从高斯分布。记：
Figure SMS_186
；
其中，
Figure SMS_187
为观测矩阵。
令
Figure SMS_188
，根据卡尔曼滤波原理，目标在/>
Figure SMS_189
时刻的位置/>
Figure SMS_190
由以下公式计算得出：
Figure SMS_191
；
其中，
Figure SMS_198
，/>
Figure SMS_193
分别代表过程噪声与观测噪声的协方差矩阵；/>
Figure SMS_207
为利用二次线性模型递推得到的目标在/>
Figure SMS_197
时刻的位置估计，/>
Figure SMS_200
为/>
Figure SMS_204
时刻的目标位置观测值，
Figure SMS_206
为/>
Figure SMS_199
时刻的目标位置预测，/>
Figure SMS_201
与/>
Figure SMS_192
分别为/>
Figure SMS_205
时刻与/>
Figure SMS_195
时刻预测误差的协方差矩阵，/>
Figure SMS_202
为/>
Figure SMS_196
时刻预测误差的协方差矩阵的估计，/>
Figure SMS_203
为卡尔曼增益矩阵，/>
Figure SMS_194
为单位矩阵；
通过卡尔曼滤波得到目标在
Figure SMS_208
时刻的位置预测/>
Figure SMS_209
后，进一步利用二阶线性模型对目标位置进行预测：
Figure SMS_210
；
其中，
Figure SMS_211
与/>
Figure SMS_212
（/>
Figure SMS_213
）表示未来某一时刻；
目标在三维惯性坐标系的
Figure SMS_214
轴，/>
Figure SMS_215
轴的预测轨迹可类似地表示为：
Figure SMS_216
，
Figure SMS_217
；
其中，
Figure SMS_219
，/>
Figure SMS_221
，/>
Figure SMS_223
分别代表目标沿/>
Figure SMS_220
轴的位置、速度、加速度矢量；/>
Figure SMS_222
，/>
Figure SMS_224
，/>
Figure SMS_225
分别代表目标沿/>
Figure SMS_218
轴的位置、速度、加速度矢量；
第二步，将目标的轨迹预测值作为参考信号，利用线性模型预测控制的思想，计算出飞行机械臂末端的期望加速度信号，具体步骤包括：
定义飞行机械臂末端的位置矢量
Figure SMS_226
与速度矢量/>
Figure SMS_227
；构建如下离散线性预测模型：
Figure SMS_228
；
其中，
Figure SMS_229
为模型的状态向量；/>
Figure SMS_230
，/>
Figure SMS_231
，/>
Figure SMS_232
分别表示时刻飞行机械臂末端的位置、速度、加速度矢量；/>
Figure SMS_233
表示矩阵或向量的转置操作；记/>
Figure SMS_234
为模型输入，即末端的加速度轨迹；
利用离散线性预测模型，构建如下最优控制问题求解
Figure SMS_235
：
Figure SMS_236
，
Figure SMS_237
；
其中，
Figure SMS_239
为预测时域长度；/>
Figure SMS_242
为求和函数；/>
Figure SMS_247
表示2-范数；上标/>
Figure SMS_240
代表期望值；/>
Figure SMS_243
为/>
Figure SMS_248
时刻末端的参考信号（由卡尔曼滤波预测得到）；/>
Figure SMS_251
，/>
Figure SMS_238
与/>
Figure SMS_244
代表权重矩阵；/>
Figure SMS_249
与/>
Figure SMS_250
代表模型状态的上下界；/>
Figure SMS_241
与/>
Figure SMS_245
代表控制输入的上下界；/>
Figure SMS_246
表示求取/>
Figure SMS_252
使目标函数达到最小值；
求解上述最优控制问题，得到飞行机械臂末端的期望加速度信号。
第三步，建立各个任务目标矢量与飞行机械臂关节矢量间的映射关系，并构造任务代价函数：
假定飞行机械臂在执行任务过程中不进行大机动飞行，因此无人机的俯仰角、滚转角均视为常值
Figure SMS_253
；在此基础上，定义飞行机械臂的关节矢量/>
Figure SMS_254
。其中，/>
Figure SMS_255
为无人机位置矢量；/>
Figure SMS_256
为无人机偏航角；/>
Figure SMS_257
为机械臂关节角矢量；
（1）对于末端跟踪任务，任务矢量等价为飞行机械臂末端位置矢量
Figure SMS_258
，其与飞行机械臂关节矢量间的映射关系用以下非线性方程表示：
Figure SMS_259
；
其中，
Figure SMS_260
为飞行机械臂的关节矢量/>
Figure SMS_261
到飞行机械臂末端位置矢量/>
Figure SMS_262
的非线性映射函数。
将方程两边同时对时间求导，得到：
Figure SMS_263
；
进一步对时间求导，得到：
Figure SMS_264
；
其中，
Figure SMS_265
为对应的任务雅各比矩阵；/>
Figure SMS_266
为/>
Figure SMS_267
对时间的导数；/>
Figure SMS_268
与/>
Figure SMS_269
分别为飞行机械臂的关节速度与加速度；
将上式离散化，得到：
Figure SMS_270
；
其中，
Figure SMS_272
，/>
Figure SMS_275
，/>
Figure SMS_277
，/>
Figure SMS_273
，/>
Figure SMS_274
，/>
Figure SMS_276
，/>
Figure SMS_278
为对应矩阵或矢量在/>
Figure SMS_271
时刻的值。
为确保实现末端对目标的精确跟踪与抓取，构造如下代价函数：
Figure SMS_279
；
其中，
Figure SMS_281
与/>
Figure SMS_284
代表/>
Figure SMS_286
时刻飞行机械臂末端的期望位置与期望速度，可以设置为目标的位置与速度。其中，/>
Figure SMS_282
与/>
Figure SMS_283
代表/>
Figure SMS_285
时刻飞行机械臂末端的期望位置与期望速度，设置为目标的位置与速度；/>
Figure SMS_287
表示求取/>
Figure SMS_280
使目标函数达到最小值；
（2）对于最小化质心偏移任务，令任务矢量为飞行机械臂在水平方向的质心偏移位置矢量
Figure SMS_288
，其与飞行机械臂关节矢量间的映射关系用以下非线性方程表示：
Figure SMS_289
；
其中，
Figure SMS_290
为飞行机械臂的关节矢量/>
Figure SMS_291
到质心偏移位置矢量/>
Figure SMS_292
的非线性映射函数。
将方程两边同时对时间求导，得到：
Figure SMS_293
；
进一步对时间求导，得到：
Figure SMS_294
；
其中，
Figure SMS_295
为对应的任务雅各比矩阵；/>
Figure SMS_296
为/>
Figure SMS_297
对时间的导数；
将上式离散化，可得：
Figure SMS_298
；
其中，
Figure SMS_299
，/>
Figure SMS_300
，/>
Figure SMS_301
，/>
Figure SMS_302
，/>
Figure SMS_303
为对应矩阵或矢量在/>
Figure SMS_304
时刻的值。
为确保实现末端对目标的精确跟踪与抓取，构造如下代价函数：
Figure SMS_305
；
其中，
Figure SMS_306
与/>
Figure SMS_307
代表/>
Figure SMS_308
时刻质心偏移的期望位置与期望速度，令
Figure SMS_309
，/>
Figure SMS_310
由下式计算：
Figure SMS_311
；
其中，
Figure SMS_312
为增益矩阵。/>
第四步，利用分层二次规划的思想对任务代价函数进行求解，根据末端的期望加速度信号解出飞行机械臂的各个关节的运动轨迹：
（1）将离散模型方程
Figure SMS_313
，
带入代价函数
Figure SMS_314
，展开后可得二次规划问题①：
Figure SMS_315
；
其中，
Figure SMS_316
，
Figure SMS_317
Figure SMS_318
；
优化函数的约束条件为：
Figure SMS_319
；
其中，
Figure SMS_322
是模型预测控制求解得到的期望末端加速度；/>
Figure SMS_324
与/>
Figure SMS_326
是飞行机械臂的关节加速度的上下界；/>
Figure SMS_321
与/>
Figure SMS_325
是飞行机械臂的关节速度的上下界；/>
Figure SMS_327
与/>
Figure SMS_328
是飞行机械臂的关节w位置的上下界。/>
Figure SMS_320
与/>
Figure SMS_323
是增益矩阵。
（2）将离散模型方程
Figure SMS_329
，
带入代价函数
Figure SMS_330
，展开后可得二次规划问题模型②：
Figure SMS_331
；
其中，
Figure SMS_332
，
Figure SMS_333
，
二次规划问题②采用与二次规划问题①相同的约束条件，在此基础上，将二次规划问题①的解添加到二次规划问题模型②中作为一项等式约束：
Figure SMS_334
；
其中，
Figure SMS_335
是二次规划问题①的解。
求解二次规划问题②，即可得到飞行机械臂系统各个自由度的期望加速度轨迹
Figure SMS_336
。
如图2所示为本发明的飞行机械臂系统结构图，其由一四旋翼无人机和多连杆串联机械臂所构成。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种面向动态目标捕获的飞行机械臂运动轨迹计算方法，其特征在于，包括如下步骤：
第一步、根据卡尔曼滤波模型，对目标在未来一段时域内轨迹进行预测；
第二步、将目标的轨迹预测值作为参考信号，利用线性模型预测控制的方法，计算出飞行机械臂末端的期望加速度信号；
第三步、建立各个任务目标矢量与飞行机械臂关节矢量间的映射关系，并构造任务代价函数；
第四步、利用分层二次规划的方法对任务代价函数进行求解，根据末端的期望加速度信号解出飞行机械臂的各个关节的运动轨迹。
2.根据权利要求1所述的一种面向动态目标捕获的飞行机械臂运动轨迹计算方法，其特征在于，所述第一步包括：
对于三维惯性坐标系的
Figure QLYQS_1
轴，用如下二阶线性模型作为动态目标的运动方程：
Figure QLYQS_2
；
其中，
Figure QLYQS_3
，/>
Figure QLYQS_6
，/>
Figure QLYQS_9
分别代表目标沿/>
Figure QLYQS_4
轴的位置、速度、加速度矢量；/>
Figure QLYQS_7
为采样间隔；下标/>
Figure QLYQS_8
为采样时刻；/>
Figure QLYQS_10
为/>
Figure QLYQS_5
时刻的过程噪声，服从高斯分布；记：
Figure QLYQS_11
，/>
Figure QLYQS_12
；
其中，
Figure QLYQS_13
为状态转移矩阵，/>
Figure QLYQS_14
为输入矩阵；
运动目标的位置观测值由机载传感器测得，观测方程表示为：
Figure QLYQS_15
；
其中，所述机载传感器包括相机、雷达，
Figure QLYQS_16
为目标沿/>
Figure QLYQS_17
轴的位置的观测值，/>
Figure QLYQS_18
为观测噪声，服从高斯分布；记：
Figure QLYQS_19
；
其中，
Figure QLYQS_20
为观测矩阵；
令
Figure QLYQS_21
，根据卡尔曼滤波原理，目标在/>
Figure QLYQS_22
时刻的位置/>
Figure QLYQS_23
由以下公式计算得出：
Figure QLYQS_24
；
其中，
Figure QLYQS_29
，/>
Figure QLYQS_26
分别代表过程噪声与观测噪声的协方差矩阵；/>
Figure QLYQS_39
为利用二次线性模型递推得到的目标在/>
Figure QLYQS_28
时刻的位置估计，/>
Figure QLYQS_34
为/>
Figure QLYQS_30
时刻的目标位置观测值，/>
Figure QLYQS_38
为
Figure QLYQS_32
时刻的目标位置预测，/>
Figure QLYQS_35
与/>
Figure QLYQS_25
分别为/>
Figure QLYQS_37
时刻与/>
Figure QLYQS_31
时刻预测误差的协方差矩阵，/>
Figure QLYQS_40
为/>
Figure QLYQS_33
时刻预测误差的协方差矩阵的估计，/>
Figure QLYQS_36
为卡尔曼增益矩阵，/>
Figure QLYQS_27
为单位矩阵；
通过卡尔曼滤波得到目标在
Figure QLYQS_41
时刻的位置预测/>
Figure QLYQS_42
后，进一步利用二阶线性模型对目标位置进行预测：
Figure QLYQS_43
；
其中，
Figure QLYQS_44
与/>
Figure QLYQS_45
（/>
Figure QLYQS_46
）表示未来某一时刻；
目标在三维惯性坐标系的
Figure QLYQS_47
轴，/>
Figure QLYQS_48
轴的预测轨迹表示为：
Figure QLYQS_49
；
Figure QLYQS_50
；
其中，
Figure QLYQS_53
，/>
Figure QLYQS_55
，/>
Figure QLYQS_57
分别代表目标沿/>
Figure QLYQS_52
轴的位置、速度、加速度矢量；/>
Figure QLYQS_54
，/>
Figure QLYQS_56
，/>
Figure QLYQS_58
分别代表目标沿/>
Figure QLYQS_51
轴的位置、速度、加速度矢量。
3.根据权利要求2所述的一种面向动态目标捕获的飞行机械臂运动轨迹计算方法，其特征在于，所述第二步包括：
定义飞行机械臂末端的位置矢量
Figure QLYQS_59
与速度矢量/>
Figure QLYQS_60
；构建如下离散线性预测模型：
Figure QLYQS_61
；
其中，
Figure QLYQS_62
为模型的状态向量；/>
Figure QLYQS_63
，/>
Figure QLYQS_64
，/>
Figure QLYQS_65
分别表示时刻飞行机械臂末端的位置、速度、加速度矢量；/>
Figure QLYQS_66
表示矩阵或向量的转置操作；记/>
Figure QLYQS_67
为模型输入，即末端的加速度轨迹；
利用离散线性预测模型，构建如下最优控制问题求解
Figure QLYQS_68
：
Figure QLYQS_69
；
Figure QLYQS_70
；
其中，
Figure QLYQS_73
为预测时域长度；/>
Figure QLYQS_76
为求和函数；/>
Figure QLYQS_82
表示2-范数；上标/>
Figure QLYQS_74
代表期望值；
Figure QLYQS_75
为/>
Figure QLYQS_79
时刻末端的参考信号（由卡尔曼滤波预测得到）；/>
Figure QLYQS_83
，/>
Figure QLYQS_71
与/>
Figure QLYQS_78
代表权重矩阵；/>
Figure QLYQS_81
与/>
Figure QLYQS_85
代表模型状态的上下界；/>
Figure QLYQS_72
与/>
Figure QLYQS_77
代表控制输入的上下界；/>
Figure QLYQS_80
表示求取/>
Figure QLYQS_84
使目标函数达到最小值；
求解上述最优控制问题，得到飞行机械臂末端的期望加速度信号。
4.根据权利要求3所述的一种面向动态目标捕获的飞行机械臂运动轨迹计算方法，其特征在于，所述第三步包括：
假定飞行机械臂在执行任务过程中不进行大机动飞行，因此无人机的俯仰角、滚转角均视为常值
Figure QLYQS_86
；在此基础上，定义飞行机械臂的关节矢量/>
Figure QLYQS_87
，其中，/>
Figure QLYQS_88
为无人机位置矢量；/>
Figure QLYQS_89
为无人机偏航角；/>
Figure QLYQS_90
为机械臂关节角矢量；
（1）对于末端跟踪任务，任务矢量等价为飞行机械臂末端位置矢量
Figure QLYQS_91
，其与飞行机械臂关节矢量间的映射关系用以下非线性方程表示：
Figure QLYQS_92
；
其中，
Figure QLYQS_93
为飞行机械臂的关节矢量/>
Figure QLYQS_94
到飞行机械臂末端位置矢量/>
Figure QLYQS_95
的非线性映射函数；
将方程两边同时对时间求导，得到：
Figure QLYQS_96
；
进一步对时间求导，得到：
Figure QLYQS_97
；
其中，
Figure QLYQS_98
为对应的任务雅各比矩阵；/>
Figure QLYQS_99
为/>
Figure QLYQS_100
对时间的导数；/>
Figure QLYQS_101
与/>
Figure QLYQS_102
分别为飞行机械臂的关节速度与加速度；
将上式离散化，得到：
Figure QLYQS_103
；
其中，
Figure QLYQS_105
，/>
Figure QLYQS_107
，/>
Figure QLYQS_109
，/>
Figure QLYQS_106
，/>
Figure QLYQS_108
，/>
Figure QLYQS_110
，/>
Figure QLYQS_111
为对应矩阵或矢量在/>
Figure QLYQS_104
时刻的值；
为确保实现末端对目标的精确跟踪与抓取，构造如下代价函数：
Figure QLYQS_112
；
其中，
Figure QLYQS_113
与/>
Figure QLYQS_114
代表 />
Figure QLYQS_115
时刻飞行机械臂末端的期望位置与期望速度，设置为目标的位置与速度；/>
Figure QLYQS_116
表示求取/>
Figure QLYQS_117
使目标函数达到最小值；
（2）对于最小化质心偏移任务，令任务矢量为飞行机械臂在水平方向的质心偏移位置矢量
Figure QLYQS_118
，其与飞行机械臂关节矢量间的映射关系用以下非线性方程表示：
Figure QLYQS_119
；
其中，
Figure QLYQS_120
为飞行机械臂的关节矢量/>
Figure QLYQS_121
到质心偏移位置矢量/>
Figure QLYQS_122
的非线性映射函数；
将方程两边同时对时间求导，得到：
Figure QLYQS_123
；
进一步对时间求导，得到：
Figure QLYQS_124
；
其中，
Figure QLYQS_125
为对应的任务雅各比矩阵；/>
Figure QLYQS_126
为/>
Figure QLYQS_127
对时间的导数；
将上式离散化，得到：
Figure QLYQS_128
；
其中，
Figure QLYQS_129
，/>
Figure QLYQS_130
，/>
Figure QLYQS_131
，/>
Figure QLYQS_132
，/>
Figure QLYQS_133
为对应矩阵或矢量在/>
Figure QLYQS_134
时刻的值；
为确保实现末端对目标的精确跟踪与抓取，构造如下代价函数：
Figure QLYQS_135
；
其中，
Figure QLYQS_136
与/>
Figure QLYQS_137
代表 />
Figure QLYQS_138
时刻质心偏移的期望位置与期望速度；令/>
Figure QLYQS_139
，
Figure QLYQS_140
由下式计算：
Figure QLYQS_141
；
其中，
Figure QLYQS_142
为增益矩阵。
5.根据权利要求4所述的一种面向动态目标捕获的飞行机械臂运动轨迹计算方法，其特征在于，所述第四步包括：
（1）将离散模型方程
Figure QLYQS_143
；
带入代价函数
Figure QLYQS_144
，展开后可得二次规划问题①：
Figure QLYQS_145
；
其中，
Figure QLYQS_146
，
Figure QLYQS_147
Figure QLYQS_148
；
优化函数的约束条件为：
Figure QLYQS_149
；
其中，
Figure QLYQS_151
是模型预测控制求解得到的期望末端加速度；/>
Figure QLYQS_154
是飞行机械臂的关节加速度的上下界；/>
Figure QLYQS_156
与/>
Figure QLYQS_152
是飞行机械臂的关节速度的上下界；/>
Figure QLYQS_153
与/>
Figure QLYQS_155
是飞行机械臂的关节位置的上下界；/>
Figure QLYQS_157
与/>
Figure QLYQS_150
是增益矩阵；
（2）将离散模型方程
Figure QLYQS_158
，
带入代价函数
Figure QLYQS_159
，展开后可得二次规划问题模型②：
Figure QLYQS_160
；
其中，
Figure QLYQS_161
；
Figure QLYQS_162
；
二次规划问题②采用与二次规划问题①相同的约束条件, 在此基础上，将二次规划问题①的解添加到二次规划问题模型②中作为一项等式约束：
Figure QLYQS_163
；
其中，
Figure QLYQS_164
是二次规划问题①的解；
求解二次规划问题②，得到飞行机械臂系统各个自由度的期望加速度
Figure QLYQS_165
轨迹。
CN202310602936.0A 2023-05-26 2023-05-26 一种面向动态目标捕获的飞行机械臂运动轨迹计算方法 Active CN116330300B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310602936.0A CN116330300B (zh) 2023-05-26 2023-05-26 一种面向动态目标捕获的飞行机械臂运动轨迹计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310602936.0A CN116330300B (zh) 2023-05-26 2023-05-26 一种面向动态目标捕获的飞行机械臂运动轨迹计算方法

Publications (2)

Publication Number Publication Date
CN116330300A true CN116330300A (zh) 2023-06-27
CN116330300B CN116330300B (zh) 2023-08-22

Family

ID=86888013

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310602936.0A Active CN116330300B (zh) 2023-05-26 2023-05-26 一种面向动态目标捕获的飞行机械臂运动轨迹计算方法

Country Status (1)

Country Link
CN (1) CN116330300B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108381555A (zh) * 2018-05-30 2018-08-10 华南理工大学 一种飞行作业机器人冗余度机械臂下位控制器的设计方法
CN108638068A (zh) * 2018-05-18 2018-10-12 华南理工大学 一种携带冗余度机械臂的飞行机器人控制系统设计方法
CN109270955A (zh) * 2018-10-30 2019-01-25 中国运载火箭技术研究院 一种基于机械臂驱动的飞行器位姿耦合快速稳定控制方法
CN109591017A (zh) * 2019-01-07 2019-04-09 西北工业大学 空间机器人捕获翻滚目标后的轨迹规划方法
CN115431271A (zh) * 2022-09-23 2022-12-06 北京航空航天大学杭州创新研究院 一种飞行机械臂末端抗干扰指向控制方法
WO2023037437A1 (ja) * 2021-09-08 2023-03-16 東京ロボティクス株式会社 無人飛行機、飛行体及び飛行ロボット
CN115877869A (zh) * 2023-01-10 2023-03-31 北京航空航天大学 一种无人机路径规划方法及系统

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108638068A (zh) * 2018-05-18 2018-10-12 华南理工大学 一种携带冗余度机械臂的飞行机器人控制系统设计方法
CN108381555A (zh) * 2018-05-30 2018-08-10 华南理工大学 一种飞行作业机器人冗余度机械臂下位控制器的设计方法
CN109270955A (zh) * 2018-10-30 2019-01-25 中国运载火箭技术研究院 一种基于机械臂驱动的飞行器位姿耦合快速稳定控制方法
CN109591017A (zh) * 2019-01-07 2019-04-09 西北工业大学 空间机器人捕获翻滚目标后的轨迹规划方法
WO2023037437A1 (ja) * 2021-09-08 2023-03-16 東京ロボティクス株式会社 無人飛行機、飛行体及び飛行ロボット
CN115431271A (zh) * 2022-09-23 2022-12-06 北京航空航天大学杭州创新研究院 一种飞行机械臂末端抗干扰指向控制方法
CN115877869A (zh) * 2023-01-10 2023-03-31 北京航空航天大学 一种无人机路径规划方法及系统

Also Published As

Publication number Publication date
CN116330300B (zh) 2023-08-22

Similar Documents

Publication Publication Date Title
CN108381553B (zh) 一种用于空间非合作目标捕获的相对导航近距离跟踪方法及系统
CN107563044B (zh) 基于在线安全学习的四旋翼无人机路径跟踪控制方法
CN105786024A (zh) 一种基于模型误差补偿的机载光电平台高精度跟踪控制器及其跟踪控制方法
CN113220021B (zh) 基于虚拟领导者的飞行编队协同自适应跟踪控制方法
CN112947572B (zh) 一种基于地形跟随的四旋翼飞行器自适应运动规划方法
Mills et al. Vision based control for fixed wing UAVs inspecting locally linear infrastructure using skid-to-turn maneuvers
CN112558621A (zh) 一种基于解耦控制的飞行机械臂系统
Wang et al. Research on logistics autonomous mobile robot system
CN115431271A (zh) 一种飞行机械臂末端抗干扰指向控制方法
Yue et al. Cooperative control for multiple quadrotors under position deviations and aerodynamic drag
CN115556111A (zh) 基于变惯性参数建模的飞行机械臂耦合扰动控制方法
CN113110593B (zh) 基于虚拟结构和估计信息传输的飞行编队协同自适应控制方法
CN116330300B (zh) 一种面向动态目标捕获的飞行机械臂运动轨迹计算方法
CN112650217B (zh) 一种基于评估函数的机器人轨迹跟踪策略动态优化方法
CN111872938B (zh) 空间三维大尺度运动学仿真系统及方法
Chen et al. A moving target tracking control and obstacle avoidance of quadrotor UAV based on sliding mode control using artificial potential field and RBF neural networks
CN116714780A (zh) 一种旋翼飞行机械臂及空中快速抓取的规划和控制方法
CN110908398A (zh) 能量耦合四旋翼无人机吊挂空运系统自适应控制方法
CN114415655A (zh) 一种基于改进slam的巡检机器人导航控制方法
Xu et al. An Active Landing Recovery Method for Quadrotor UAV: Localization, Tracking and Buffering Landing
Lee et al. Robust control scheme for mobile manipulator on uneven terrain based on IMU-based motion compensation and time-delay control
Alzorgan et al. Actuator trajectory planning for uavs with overhead manipulator using reinforcement learning
Zhang et al. Hybrid force/position control for switchable unmanned aerial manipulator between free Flight and contact operation
Shi et al. Design of trajectory tracking controller with backstepping method for quadrotor unmanned aerial vehicles
Bai et al. A fuzzy adaptive drogue control method in autonomous aerial refueling

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