一种带机械臂的无人机协同编队阻抗控制方法

技术特征：
1.一种带机械臂的无人机协同编队阻抗控制方法，其特征在于，包括以下步骤：步骤s1，根据齐次方程及雅克比矩阵构建带机械臂无人机的运动学模型；步骤s2，根据步骤s1中建立的运动学模型并基于拉格朗日-欧拉方程建立带机械臂无人机的动力学模型；步骤s3，参考带机械臂多无人机的抓取运动情况及受力和力矩情况，构建协同编队阻抗控制器；步骤s4，根据任务需求，设计参考物体轨迹，结合所述运动学模型的正运动控制及协同编队阻抗控制器的误差关系构建同步误差补偿控制器；步骤s5：采用同步误差补偿控制器对带机械臂无人机的运动学模型进行修正补偿。2.根据权利要求1所述的一种带机械臂的无人机协同编队阻抗控制方法，其特征在于，所述步骤s1中带机械臂无人机的运动学模型具体如下：所述步骤s1中带机械臂无人机的运动学模型具体如下：所述步骤s1中带机械臂无人机的运动学模型具体如下：所述步骤s1中带机械臂无人机的运动学模型具体如下：其中，p
ek
为第k架带机械臂无人机的机械臂末端位置矩阵；p
bk
为第k架带机械臂无人机位置矩阵；r
bk
为第k架带机械臂无人机的旋转矩阵，t为转置；为第k架带机械臂无人机的机械臂末端相对于该无人机的位置矩阵；ω
ek
为第k架带机械臂无人机的机械臂末端的角速度矩阵；ω
bk
为第k架带机械臂无人机的角速度矩阵；为第k架带机械臂无人机的机械臂末端相对于该无人机的角速度矩阵；v
ek
为第k架带机械臂无人机的机械臂末端速度矩阵；j
bk
为第k架带机械臂无人机的雅克比矩阵；j
ekbk
为第k架带机械臂无人机的机械臂的雅克比矩阵；为第k架带机械臂无人机的位姿速度矩阵；为第k架带机械臂无人机的机械臂角度矩阵。3.根据权利要求2所述的一种带机械臂的无人机协同编队阻抗控制方法，其特征在于，其中：其中：v
ek
＝[ν
ex ν
ez ν
eθ
]
t
；
ꢀꢀꢀꢀꢀ
(7)其中为第k架带机械臂无人机的x方向期望速度，为第k架带臂无人机的z方向期望速度，为第k架带机械臂无人机的期望俯仰角速度，为第k架带机械臂无人机的机械臂第1个关节的期望角速度，为第k架带机械臂无人机的机械臂第2个关节的期望角速度，
ν
ex
为第k架带机械臂无人机的机械臂末端x方向的速度，ν
ez
为第k架带机械臂无人机的机械臂末端z方向的速度，ν
eθ
为第k架带机械臂无人机的机械臂末端俯仰角速度。4.根据权利要求3所述的一种带机械臂的无人机协同编队阻抗控制方法，其特征在于，还包括以下步骤：设设由上式得：由上式得：式中，j
ζk
为第k架带机械臂无人机的雅克比矩阵j
bk
的第1到第3行和第1第2列数值，j
σk
为第k架带机械臂无人机的雅克比矩阵j
bk
的第1到第3行和第3列数值；设j
ηk
＝[j
ζk j
ekbk
]，由上式变换得：式中，χ
k
为第k架带机械臂无人机的对称增益矩阵；e
k
为第k架带机械臂无人机的机械臂末端位姿误差；而第k架带机械臂无人机的机械臂末端的位置y
ek
和偏航角ψ
ek
为无人机的位置y
bk
和偏航角ψ
bk
。5.根据权利要求1所述的一种带机械臂的无人机协同编队阻抗控制方法，其特征在于，所述步骤s2中带机械臂无人机的动力学模型具体如下：所述步骤s2中带机械臂无人机的动力学模型具体如下：x
b1
＝[x z θ]
t
；
ꢀꢀꢀꢀ
(15)q1＝[q
1 q2]
t
；
ꢀꢀꢀꢀ
(16)x
b2
＝[x z θ]
t
；
ꢀꢀꢀ
(17)q2＝[q
1 q2]
t
；
ꢀꢀꢀ
(18)其中x为带机械臂无人机x方向上的位置，z为带机械臂无人机z方向上的位置，θ为带机械臂无人机的俯仰角，q1、q2分别为第k架带机械臂无人机的机械臂第1个、第2个关节的角度；为第k架带机械臂无人机的惯性矩阵；为第k架带机械臂无人机的离心力和哥氏力构成的矩阵；为第k架带机械臂无人机的重力项构成的矩阵；u＝[u
1 u2]
t
，u1＝[f
x f
z τ
θ τ
q1 τ
q2
]
t
，u2＝[f
x f
z τ
θ τ
q1 τ
q2
]
t
，f
x
、f
z
分别为带机械臂无人机在x、z方向的
力，τ
θ
为带机械臂无人机的俯仰角力矩，τ
q1
、τ
q2
分别带机械臂无人机的第1个、第2个机械臂转角力矩；式中j
bk
为第k架带机械臂无人机的雅克比矩阵，j
ekbk
为第k架带机械臂无人机的机械臂的雅克比矩阵，f
ek
为第k架带机械臂无人机的机械臂末端与环境接触时的接触力矩阵。6.根据权利要求1所述的一种带机械臂的无人机协同编队阻抗控制方法，其特征在于，所述步骤s3中协同编队阻抗控制器的控制函数为：所述步骤s3中协同编队阻抗控制器的控制函数为：其中，m
sk
为第k架带机械臂无人机的期望惯量矩阵，b
sk
为第k架带机械臂无人机的期望阻尼矩阵，g
sk
为第k架带机械臂无人机的期望刚度矩阵，f
pk
为第k架带机械臂无人机的机械臂末端与环境接触力矩阵；x
pk
为第k架带机械臂无人机的机械臂末端期望位姿；x
sk
为第k架带机械臂无人机的机械臂末端实际位姿，为第k架带机械臂无人机的机械臂末端期望位姿速度，为第k架带机械臂无人机的机械臂末端实际位姿速度，为第k架带机械臂无人机的机械臂末端期望位姿加速度，为第k架带机械臂无人机的机械臂末端实际位姿加速度；根据映射关系，将带机械臂多无人机的机械臂末端位置和姿态互相映射和映射到物体上，其方程为下式：式中，x
p1-2
为第1架带机械臂无人机的机械臂末端位姿映射到第2架带机械臂无人机的机械臂末端位姿，x
p2-1
为第2架带机械臂无人机的机械臂末端位姿映射到第1架带机械臂无人机的机械臂末端位姿，x
12o
为第1架和第2架带机械臂无人机的机械臂末端映射到物体o的位姿，t
12
为第1架带机械臂无人机的机械臂末端到第2架带机械臂无人机的机械臂末端映射矩阵，t
21
为第2架带机械臂无人机的机械臂末端到第1架带机械臂无人机的机械臂末端映射矩阵，t
1o
为第1架带机械臂无人机的机械臂末端到物体o的映射矩阵，t
2o
为第2架带机械臂无人机的机械臂末端到物体o的映射矩阵；对于机械臂与被抓物体可得同步期望输入位姿为下式：式中，x
po
为物体的期望位姿，为第1架带机械臂无人机的阻抗控制映射关系输出值，为第2架带机械臂无人机的阻抗控制映射关系输出值，为物体映射关系输出值。
7.根据权利要求1所述的一种带机械臂的无人机协同编队阻抗控制方法，其特征在于，所述步骤s4中带机械臂无人机的同步阻抗控制的实际位姿输出如下：所述步骤s4中带机械臂无人机的同步阻抗控制的实际位姿输出如下：所述步骤s4中带机械臂无人机的同步阻抗控制的实际位姿输出如下：所述步骤s4中带机械臂无人机的同步阻抗控制的实际位姿输出如下：式中，k
e1
为第1架带机械臂无人机的机械臂的实际位姿与阻抗控制同步输入位姿的误差补偿参数；k
a1
为第1架与第2架带机械臂无人机的机械臂同步位姿误差补偿参数；k
q1
为第1架与第2架带机械臂无人机的机械臂末端互相映射误差补偿参数；k
e2
为第2架带机械臂无人机的机械臂的实际位姿与阻抗控制同步输入位姿的误差补偿参数；k
a2
为第1架与第2架带机械臂无人机的机械臂同步位姿误差补偿参数；k
q2
为第2架带机械臂无人机的机械臂末端的误差补偿参数；为带机械臂无人机的机械臂末端物体位姿，x
c1
为第1架带机械臂无人机的同步阻抗控制的实际位姿输出；x
c2
为第2架带机械臂无人机的同步阻抗控制的实际位姿输出；x1为第1架带机械臂无人机的机械臂末端实际输出位姿；x2为第2架带机械臂无人机的机械臂末端实际输出位姿。8.根据权利要求1所述的一种带机械臂的无人机协同编队阻抗控制方法，其特征在于，物体的运动轨迹与第1架和第2架带机械臂无人机的机械臂之间互相通过映射连接，其表达式为：式中，为物体位姿，为第1架带机械臂无人机的机械臂末端期望位姿，为第2架带机械臂无人机的机械臂末端期望位姿，t
o1
和t
o2
为物体与第1架和第2架带机械臂无人机的机械臂末端映射矩阵；根据上述公式可以得到物体的运动轨迹映射到第1架和第2架带机械臂无人机的机械臂上，进而实现第1架和第2架带机械臂无人机的机械臂的运动轨迹规划。

技术总结
本发明属于无人机飞行控制技术领域，具体涉及一种带机械臂的无人机协同编队阻抗控制方法，通过构建带机械臂无人机的运动学模型和动力学模型，构建协同编队阻抗控制器；根据任务需求，设计参考物体轨迹，结合所述运动学模型的正运动控制及协同编队阻抗控制器的误差关系构建同步误差补偿控制器；采用同步误差补偿控制器对带机械臂无人机的运动学模型进行修正补偿。本发明采用阻抗控制能够解决机械臂的柔顺性抓取，减少对物体的损坏，并减少对无人机的扰动。单个带机械臂无人机在自身升力不够的情况下，采用本发明可实现带机械臂多无人机协同抓取物体飞行。在带机械臂无人机协同抓取物体时采用本发明可保证飞行过程中的干扰及精度。及精度。及精度。


技术研发人员：周祖鹏 陈宝
受保护的技术使用者：桂林电子科技大学
技术研发日：2022.03.10
技术公布日：2022/5/30