飞行器的安全控制方法、装置及存储介质

技术特征：
1.一种飞行器的安全控制方法，其特征在于，所述飞行器上搭载有l个机械臂，l为大于1的正整数，每个所述机械臂均分别包括若干依次连接的关节，每个所述机械臂末端分别设有一个操作机构，所有机械臂及其末端的操作机构共同构成多臂操作机构；所述安全控制方法包括：构建所述多臂操作机构的质心运动学模型；利用所述多臂操作机构的质心运动学模型估计所述多臂操作机构对所述飞行器的偏转力和偏转力矩；利用所述多臂操作机构对所述飞行器的偏转力和力矩构建基于自适应滑模控制方法的安全控制器，得到所述飞行器的安全路径跟踪策略，所述飞行器按照所述安全路径跟踪策略执行操作任务。2.根据权利要求1所述的安全控制方法，其特征在于，构建所述多臂操作机构的质心运动学模型为：动学模型为：其中，c(t)为t时刻多臂操作机构的质心矢量，为t时刻多臂操作机构的质心速度，m
s
为第s个机械臂的质量，c
s
(t)为t时刻机体坐标系下第s个机械臂的质心位置，为t时刻机体坐标系下第s个机械臂的质心速度。3.根据权利要求2所述的安全控制方法，其特征在于，利用所述多臂操作机构的质心运动学模型估计所述多臂操作机构对所述飞行器的偏转力和偏转力矩，计算公式如下：其中，为t时刻多臂操作机构对飞行器的偏转力及力矩的估计向量；c
x
(t)和c
y
(t)分别为t时刻多臂操作机构的质心矢量c(t)在机体坐标系下沿x，y轴的分量；α(t)为t时刻多臂操作机构的质心矢量在机体坐标系中xy平面的投影与x轴正方向的夹角，θ(t)为t时刻多臂操作机构的质心矢量与z轴正方向的夹角，和分别为α(t)和θ(t)对时间的一阶导数；a
uav
(t)为t时刻飞行器的飞行加速度，g为重力加速度矢量。4.根据权利要求3所述的安全控制方法，其特征在于，当多臂操作机构为相对飞行器于基座对称分布的双臂操作机构时，将t时刻多臂操作机构对飞行器的偏转力及力矩的估计
向量简化为：5.根据权利要求3所述的安全控制方法，其特征在于，按照以下步骤得到所述飞行器的安全路径跟踪策略：构建飞行器的机体动力学模型如下：其中，m(
·
)、c(
·
)和g(
·
)分别为飞行器的惯性矩阵、科氏力矩阵和重力矩阵；p(t)为t时刻飞行器在惯性坐标系下的位姿向量，和分别为t时刻飞行器在惯性坐标系下的位姿变化速度向量和位姿变化加速度向量；为t时刻飞行器的驱动力和力矩向量，为t时刻多臂操作机构对飞行器的偏转作用力和/或偏转作用力矩，f
d
(t)为有界干扰量，满足||f
d
(t)||≤k，k≥0为干扰输入的界值；设所述安全控制器的控制目标为：min(p(t)-p
d
(t))
t
(p(t)-p
d
(t))s.t.p(t)∈s1∩s2其中，p
d
(t)为t时刻飞行器在惯性坐标系下的期望位姿向量；s1为飞行器的无碰撞位姿集合，h1(p(t))为与避障相关的障碍控制函数，(p(t))为与避障相关的障碍控制函数，(p(t))为与避障相关的障碍控制函数，为t时刻飞行器在惯性坐标系下的位姿向量p(t)与t时刻障碍物在惯性坐标下的位置矢量p
c
(t)的欧式距离的平方，d
c
为飞行器与障碍物的安全距离界值；s2为飞行器在可飞行球形区域内的位姿集合，h2(p(t))为与可飞行球形区域相关的障碍控制函数，h2(p(t))＝p(t)
t
p(t)-d
s
(t)2，d
s
(t)为t时刻惯性坐标系下飞行器的可飞行球形区域半径；构建基于自适应滑模控制方法的安全控制器如下：构建基于自适应滑模控制方法的安全控制器如下：其中，为基于自适应滑模控制方法的安全控制器输出的t时刻飞行器的安全路径跟踪策略；f
bnorm
(t)为t时刻的参考控制输入，(t)为t时刻的参考控制输入，(t)为t时刻的参考控制输入，为t时刻的自适应项，п为自适应率，s(t)为滑模面，模面，为t时刻飞行器在惯性坐标系下的期望位姿变化速度向量，λ为指数趋近律，和分别为飞行器的惯性矩阵和科氏力矩阵的估计矩阵，为t时刻飞行器的期望位姿变化速度向量，为t时刻飞行器的期望位姿变化速度向量，为t时刻飞行器的期望位姿变化加速度向量，k为滑模面趋近律，k
h
障碍控制比例系数，和分别为与避障相关的障碍控制函数h1(p(t))对时间的一阶导数和二阶
导数，和分别为与可飞行区域限制相关的障碍控制函数h2(p(t))对时间的一阶导数和二阶导数。6.根据权利要求5所述的安全控制方法，其特征在于，当所述多臂操作机构存在空闲臂时，所述安全控制方法还包括：在设定的质心偏移量最小化控制目标下求解执行操作任务的若干机械臂动作对飞行器的质心偏移量和质心偏移速度，根据所述质心偏移量和质心偏移速度得到空闲臂的质心调节控制策略，以此得到多臂操作机构对飞行器的偏转力和偏转力矩的估计向量，其中，在所述质心调节控制策略下多臂操作机构对飞行器的偏转力和偏转力矩的估计向量在旋翼所在平面的分量为0。7.根据权利要求6所述的安全控制方法，其特征在于，按照以下步骤得到所述空闲臂的质心调节控制策略：设l个机械臂中有m个任务臂和n个空闲臂，1≤m＜l，n＝l-m；设初始状态时，在惯性坐标系下多臂操作机构的质心与机体质心在同一竖直线上，设t时刻多臂操作机构的期望质心矢量为c
d
(t)，且满足c
dx
(t)＝0，c
dy
(t)＝0，c
dx
(t)，c
dy
(t)分别为t时刻多臂操作机构的期望质心矢量c
d
(t)在机体坐标系下沿x，y轴的分量，并设质心调节的控制目标为c
x
(t)＝0，c
y
(t)＝0，c
x
(t)，c
y
(t)分别为t时刻多臂操作机构的质心矢量c(t)在机体坐标系下沿x，y轴的分量，根据所述多臂操作机构的质心运动模型求解得到各空闲臂的期望质心矢量和期望质心速度向量，计算公式如下：心速度向量，计算公式如下：其中，和分别为第j个空闲臂的期望质心矢量在机体坐标系下沿x，y轴的分量，和分别为第j个空闲臂的期望质心速度向量在机体坐标系下沿x，y轴的分量；m
i
为机体坐标系下第i个任务臂的质量，m
j
为机体坐标系下第j个空闲臂的质量；α
j
(t)为t时刻第j个空闲臂的期望质心分配权重，满足根据所述各空闲臂的期望质心矢量和期望质心速度向量构建空闲臂控制器为：其中，u
j
(t)为t时刻第j个空闲臂的控制力矩，即所述空闲臂的质心调节控制策略；q
jd
(t)和分别为由机械臂逆运动学解析方法计算得到的t时刻第j个空闲臂各关节的期望转动角度和期望角速度，k
p
，k
d
为空闲臂控制器增益。8.一种飞行器的安全控制装置，其特征在于，所述飞行器上搭载有l个机械臂，l为大于1的正整数，每个所述机械臂均分别包括若干依次连接的关节，每个所述机械臂末端分别设有一个操作机构，所有机械臂及其末端的操作机构共同构成多臂操作机构，所述飞行器的安全控制装置包括：质心运动学模型构建模块，用于构建所述多臂操作机构的质心运动学模型；
偏转力和偏转力矩估计模块，用于利用所述多臂操作机构的质心运动学模型估计所述多臂操作机构对所述飞行器的偏转力和偏转力矩；安全路径跟踪控制模块，用于利用所述多臂操作机构对所述飞行器的偏转力和力矩构建基于自适应滑模控制方法的安全控制器，得到所述飞行器的安全路径跟踪策略，所述飞行器按照所述安全路径跟踪策略执行操作任务。9.根据权利要求8所述的安全控制装置，其特征在于，当所述多臂操作机构存在空闲臂时，所述安全控制装置还包括：质心调节控制模块，用于在设定的质心偏移最小化控制目标下求解执行操作任务的若干机械臂动作对飞行器的质心偏移量和质心偏移速度，根据所述质心偏移量和质心偏移速度得到空闲臂的质心调节控制策略，以此得到多臂操作机构对飞行器的偏转力和偏转力矩的估计向量，其中，在所述质心调节控制策略下多臂操作机构对飞行器的偏转力和偏转力矩的估计向量在机体坐标系xy平面的分量为0。10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储计算机指令，所述计算机指令用于使所述计算机执行权利要求1～7任一项所述的安全控制方法。

技术总结
本公开实施例提供的飞行器的安全控制方法、装置及存储介质，所述飞行器上搭载有至少两个机械臂，所有机械臂及其末端的操作机构共同构成多臂操作机构；本公开实施例包括：构建所述多臂操作机构的质心运动学模型；利用多臂操作机构的质心运动学模型估计多臂操作机构对飞行器的偏转力和偏转力矩；利用多臂操作机构对飞行器的偏转力和力矩构建基于自适应滑模控制方法的安全控制器，得到飞行器的安全路径跟踪策略，飞行器按照安全路径跟踪策略执行操作任务。本公开实施例可保证飞行器安全、稳定地执行复杂的操作任务。定地执行复杂的操作任务。定地执行复杂的操作任务。


技术研发人员：孙富春 吴双双 宋亦旭 时振宇
受保护的技术使用者：清华大学
技术研发日：2022.04.30
技术公布日：2022/8/30