作业飞行机器人动态滑翔抓取与力位混合控制方法与流程

1.本发明涉及无人机技术领域，涉及一种作业飞行机器人动态滑翔抓取与力位混合控制方法。


背景技术：

2.无人机实现无人驾驶的方式从遥控驾驶，到机载计算机自主控制。无人机已经是成熟的飞行平台，可以在飞行平台上搭载不同组件扩展飞行平台在不同领域的应用。比如，航拍勘测、农药喷洒、目标跟踪等领域都有无人机应用的潜能。其中，这些应用不乏需要在无人机平台上搭载机械臂，将二者结合起来就是作业型飞行机器人，如此高端的设备可使工业获得很大的便利。随着研究员对这块领域的深入，已经有学者实现了无人机搭载机械臂在实际中的应用。具有7自由度机械臂的作业型飞行机器人,能够灵活完成抓取和装配作业；在作业型飞行机器人系统上增加了视觉伺服控制,可以完成自主的抓取任务；使用作业型飞行机器人的机械臂末端和物体接触,以代替力传感器完成接触力测量工作；采用并联作业型飞行机器人系统,可以更好的仿生工作。
3.上述的这些应用都有一个飞行抓取的动作。而想要凭借指令飞行抓取，就还有一些技术难点需要攻克。关于抓取方式，是控制工程首要解决的问题。仿生物体滑翔抓取物体毫无疑问是当今研究的热点之一，由于滑翔抓取物体会产生较大的冲击力，对飞行平台会产生较大的影响，若飞行速度过快或者所抓取物体过重，会使飞行平台偏离规划好的位置甚至失控。
4.对于作业型飞行机器人抓取物体的问题，已经有很多学者做出了控制方法。例如使用作业型飞行机器人的机械臂末端和物体接触,以代替力传感器完成接触力测量工作；分别建立飞行平台和机械臂动力学模型，使用h
∞
控制方法对所抓取物体进行控制。上述控制方法大多只能抓取质量较小物体，对抓取较大的物体会产生较大的误差甚至导致失控。


技术实现要素：

5.有鉴于此，本发明的目的在于提供一种作业飞行机器人动态滑翔抓取与力位混合控制方法，能够有效提高无人机的抓取控制精度。
6.为实现上述目的，本发明采用如下技术方案：
7.一种作业飞行机器人动态滑翔抓取与力位混合控制方法，包括以下步骤：
8.步骤s1：考虑重心偏移以及抓取过程中受力和力矩，构建搭载机械臂的四旋翼无人机系统模型和二自由度机械手模型；
9.步骤s2：通过对机械手与物体瞬时接触力和抓取力的分析，计算机械手末端受到的瞬时接触力f
m
和抓取力f1,f2；
10.步骤s3：构建参数估计器，并对飞行平台的质量m以及惯性张量i
x
,i
y
,i
z
进行估计；
11.步骤s4：根据步骤s3得到的估计参数，进行飞行平台位置控制，在建模误差存在的情况下进行神经网络滑模自适应控制，并解算出通过升力u1、翻滚力矩u2、俯仰力矩u3、偏航
力矩u4；
12.步骤s5：针对所抓取的力分析进行二自由度机械臂力控制，并解算出控制力矩τ1,τ2；
13.步骤s6：通过升力u1、翻滚力矩u2、俯仰力矩u3、偏航力矩u4解算出四个旋翼的转速ω
i
，i＝1,2,3,4；
14.步骤s7：通过解算得到的控制力矩τ1,τ2和四个旋翼的转速ω
i
，控制无人机。
15.进一步的，所述构建搭载机械臂的四旋翼无人机系统模型具体为：
16.利用牛顿
‑
欧拉方程法对搭载机械臂的四旋翼无人机系统进行建模，并根据力平衡和力矩平衡得到飞行平台的动力学：
[0017][0018]
其中x为飞行平台x轴方向位移，y为飞行平台y方向位移，z为飞行平台z轴方向位移，φ为飞行平台翻滚角，θ为飞行平台俯仰角，ψ为分型平台偏航角，为四旋翼作业型飞行机器人位置和姿态[x,y,z,φ,θ,ψ]
t
的二阶导，即加速度，u1为飞行平台升力，[u2,u3,u4]为飞行平台力矩，m为作业型飞行机器人总质量，i
x
,i
y
,i
z
分别为对飞行平台x轴，y轴，z轴产生的惯性张量，f
b
＝[
b
f
x
,
b
f
y
,
b
f
z
,
b
m
x
,
b
m
y
,
b
m
z
]为抓取过程中产生的绕动力和扰动力矩，s和c分别表示sin(
·
)和cos(
·
)，a1,a2,a3,b1,b2,b3,d1,d2,d3为重心偏移引起的扰动项；
[0019]
建立的二自由度机械臂动力学如下所示：
[0020][0021]
其中，m
f
(q)、和g
f
(q)分别为和机械手转动惯量、质量、转速有关的系统变量，q＝[q1,q2]为二自由度机械臂的转速，τ
d
＝[τ1,τ2]为二自由度机械臂的输入力矩，τ
f
为抓取过程中受到的力和力矩。
[0022]
进一步的，所述步骤s2具体为：
[0023]
步骤s21:构建作业型飞行机器人的动力学方程：
[0024]
[0025]
其中：m
u
＝diag(m,m,m,i
x
,i
y
,i
z
)，ξ＝[x,y,z,φ,θ,ψ,q1,q2]
t
,τ＝[v1,v2,v3,u2,u3,u4,τ1,τ2]
t
，f
m
＝[f
x
,f
y
,f
z
]
t
。
i
r
e
＝[
i
r
eu
,
i
r
ef
]
t
，v1,v2,v3为虚拟控制量；
[0026]
步骤s22：在作业型飞行机器人抓取物体瞬间，由动量定理可得：
[0027][0028]
其中：t0为抓取开始时间，δt为抓取时间；
[0029]
同时可以计算出所抓取物体的动量：
[0030][0031]
ξ
m
＝[x
m
,y
m
,z
m
]
t
为所抓取物体的位移；
[0032]
步骤s23：利用冲量定理，计算出运动过程中产生的冲量；
[0033][0034]
其中p
m
为碰撞过程中产生的冲量；
[0035]
步骤s24：碰撞后物体速度和作业型飞行机器人末端执行器速度相同：
[0036][0037]
其中：
i
r
b
＝[i3×3,rp
x
,rp
q
],],
[0038]
步骤s26：联立式(6)
‑
(9)得出瞬时接触力f
m
：
[0039][0040]
步骤s27：由牛顿运动力学得出抓取过程中所受到的抓取力为：
[0041][0042]
其中θ
r
为末端执行器手指转速，k
r
,k
r2
(ζ)为抓取可变参数。
[0043]
进一步的，所述步骤s3具体为：
[0044]
步骤s31:根据步骤s1所设计的动力学模型，转化为：
[0045][0046]
其中：f＝f
m
f
r1
f
r2
[0047]
步骤s32：设计参数估计器和如下：
[0048][0049]
其中c和k是用户定义的正定增益矩阵，而是估计状态，状态估计误差定义为
[0050]
步骤s33：使用式(12)和等式(13)，将误差动态写为：
[0051][0052]
步骤s34：m
u
表达为：
[0053]
m
u
＝mβ1 i
x
β2 i
y
β3 i
z
β4,
ꢀꢀ
(15)
[0054]
其中：
[0055]
β1＝diag(1,1,1,0,0,0),β2＝diag(0,0,0,1,0,0),β3＝diag(0,0,0,0,1,0),β4＝diag(0,0,0,0,0,1)
[0056]
针对误差动态方程(14)，自适应控制律设计如下：
[0057][0058]
状态误差模型的目标是渐近稳定的的状态；
[0059]
步骤s35：对公式(16)进行证明，为了说明误差动力学方程(16)的收敛性，定义以下lyapunov候选函数：
[0060][0061]
步骤s36：对v1进行求导：
[0062][0063]
将等式(16)代入等式(18)，v1的时间导数可得出：
[0064]
[0065]
进一步的，所述步骤s4具体包括以下步骤：
[0066]
步骤s41：定义期望向量χ
d
＝[x
d
,y
d
,z
d
,φ
d
,θ
d
,ψ
d
]
t
，定义误差如下：
[0067]
e＝χ
‑
χ
d
ꢀꢀ
(20)
[0068]
步骤s42：将滑动表面变量s定义为：
[0069][0070]
其中λ是对角增益矩阵；
[0071]
步骤s43：使用rbf神经网络进行质量评估，网络算法定义如下：
[0072][0073]
其中w
t
和分别是网络算法的权重和误差；
[0074]
步骤s44：估算的质量定义为：
[0075][0076]
步骤s45：从等式(22)至(23)，得到如下的质量估计误差：
[0077][0078]
其中是一个有界变量，并定义
[0079]
步骤s46：符号f
*
定义如下：
[0080][0081]
步骤s47：抓取力动力学中存在建模误差，定义为δ
u
,让δ
m
＝f
‑
m
s
f
*
，并且δ＝δ
u
δ
m
，则错误估计被设计为
[0082]
针对无人机的动力学，滑模神经网络控制器的设计如下：
[0083][0084]
其中：
[0085]
进一步的，所述步骤s5具体为：
[0086]
步骤s51：定义将瞬时接触力写成：
[0087][0088]
其中j＝
‑
[δt(
i
r
b
‑1m
s
‑1 m
‑
1i
r
e
)]，
[0089]
步骤s52：将操纵器动力学重写为：
[0090]
[0091]
步骤s53：机械手的预期位移定义为并定义系统误差如下：
[0092][0093]
步骤s54：令等式(28)可以重写为：
[0094][0095]
步骤s55：rbf神经网络用于估计e，并且网络算法可以表示为：
[0096][0097]
其中和分别是网络算法的权重和误差，而
[0098]
步骤s56：针对无人机(28)的动态特性，鲁棒的自适应神经网络控制器的设计如下：
[0099][0100]
其中是e的估计值，而v
f
是一个鲁棒补偿器项，定义为：
[0101][0102]
其中其中
[0103]
进一步的，所述步骤s6具体为：
[0104]
步骤s61：由公式(26)得出系统控制力和控制力矩u，其中u1、u2、u3和u4的关系为：
[0105]
u1＝c1(ω
12
ω
22
ω
32
ω
42
)
[0106]
u2＝c1(
‑
ω
22
ω
42
),u3＝c1(
‑
ω
12
ω
32
)
[0107]
u4＝c2(ω
12
‑
ω
22
ω
32
‑
ω
42
)
[0108]
(34)
[0109]
步骤s62：解算出四个旋翼的转速ω
i
，i＝1,2,3,4。
[0110]
本发明与现有技术相比具有以下有益效果：
[0111]
本发明构建了基于瞬时接触力和抓取的动力学模型.并采力位混合控制策略,能够完成滑翔抓取作业任务；通过脉冲和动量定理分析滑行抓握过程中的瞬时接触力，其中考虑了影响抓握性能的一些因素，包括物体的不规则形状，物体滚动和几何上不对称的抓握。同时，把持物体的质量和惯性张量视为未知的有界项。有益的是，保证了uam滑行抓握的更精确的动力学模型。然后，基于鲁棒自适应神经网络估计器的混合力/位置控制器被用于uam，以克服内部干扰和外部干扰。
附图说明
[0112]
图1是本发明实施例的作业型飞行平台滑翔抓取示意图。
[0113]
图2是本发明实施例的流程结构示意图。
[0114]
图3是本发明实施例的位置控制器中x轴分量的控制效果示意图。
[0115]
图4是本发明实施例的位置控制器中y轴分量的控制效果示意图。
[0116]
图5是本发明实施例的位置控制器中z轴分量的控制效果示意图。
[0117]
图6是本发明实施例的姿态控制器中对翻滚角的控制效果示意图。
[0118]
图7是本发明实施例的姿态控制器中对俯仰角θ的控制效果示意图。
[0119]
图8是本发明实施例的姿态控制器中对翻滚角ψ的控制效果示意图。
[0120]
图9是本发明实施例的机械手控制器中对机械臂1的控制效果示意图。
[0121]
图10是本发明实施例的机械手控制器中对机械臂2的控制效果示意图。
具体实施方式
[0122]
下面结合附图及实施例对本发明做进一步说明。
[0123]
请参照图2，本发明提供一种基于作业飞行机器人动态滑翔抓取的建模与力位混合控制方法，包括以下步骤：
[0124]
步骤s1：考虑重心偏移，构建搭载机械臂的四旋翼无人机系统模型和二自由度机械手模型；
[0125]
所述构建搭载机械臂的四旋翼无人机系统模型具体为：利用牛顿
‑
欧拉方程法对搭载机械臂的四旋翼无人机系统进行建模，并根据力平衡和力矩平衡得到飞行平台的动力学：
[0126][0127]
其中x为飞行平台x轴方向位移，y为飞行平台y方向位移，z为飞行平台z轴方向位移，φ为飞行平台翻滚角，θ为飞行平台俯仰角，ψ为分型平台偏航角，为四旋翼作业型飞行机器人位置和姿态[x,y,z,φ,θ,ψ]
t
的二阶导，即加速度，u1为飞行平台升力，[u2,u3,u4]为飞行平台力矩，m为作业型飞行机器人总质量，i
x
,i
y
,i
z
分别为对飞行平台x轴，y轴，z轴产生的惯性张量，f
b
＝[
b
f
x
,
b
f
y
,
b
f
z
,
b
m
x
,
b
m
y
,
b
m
z
]为抓取过程中产生的绕动力和扰动力矩，s和c分别表示sin(
·
)和cos(
·
)，a1,a2,a3,b1,b2,b3,d1,d2,d3为重心偏移引起的扰动项，有如下定义：
[0128][0129]
其中r
g
＝[x
g
,y
g
,z
g
]为重心偏移量。
[0130]
所建立的二自由度机械臂动力学如下所示：
[0131][0132]
其中，m
f
(q)、和g
f
(q)分别为和机械手转动惯量、质量、转速有关的系统变量，q＝[q1,q2]为二自由度机械臂的转速，τ
d
＝[τ1,τ2]为二自由度机械臂的输入力矩，τ
f
为抓取过程中受到的力和力矩。
[0133]
步骤s2：通过对机械手与物体瞬时接触力和抓取力的分析，计算出机械手末端受到的瞬时接触力f
m
和抓取力f1,f2；
[0134]
步骤s21:联立公式(1)和(3)得出作业型飞行机器人的动力学方程：
[0135][0136]
其中：m
u
＝diag(m,m,m,i
x
,i
y
,i
z
)，ξ＝[x,y,z,φ,θ,ψ,q1,q2]
t
,τ＝[v1,v2,v3,u2,u3,u4,τ1,τ2]
t
，f
m
＝[f
x
,f
y
,f
z
]
t
。
i
r
e
＝[
i
r
eu
,
i
r
ef
]
t
，v1,v2,v3为虚拟控制量：
[0137][0138]
步骤s22：在作业型飞行机器人抓取物体瞬间，由动量定理可得：
[0139][0140]
其中：t0为抓取开始时间，δt为抓取时间。
[0141]
同时可以计算出所抓取物体的动量：
[0142][0143]
ξ
m
＝[x
m
,y
m
,z
m
]
t
为所抓取物体的位移。
[0144]
步骤s23：利用冲量定理，计算出运动过程中产生的冲量；
[0145][0146]
其中p
m
为碰撞过程中产生的冲量；
[0147]
步骤s24：碰撞后物体速度和作业型飞行机器人末端执行器速度相同：
[0148][0149]
其中：
i
r
b
＝[i3×3,rp
x
,rp
q
],],
[0150]
步骤s26：联立式(6)
‑
(9)得出瞬时接触力f
m
：
[0151][0152]
步骤s27：由牛顿运动力学得出抓取过程中所受到的抓取力为：
[0153][0154]
其中θ
r
为末端执行器手指转速，k
r
,k
r2
(ζ)为抓取可变参数。
[0155]
步骤s3：设计参数估计器，对飞行平台的质量m以及惯性张量i
x
,i
y
,i
z
进行估计；
[0156]
步骤s31:根据步骤s1所设计的动力学模型，转化为如下形式：
[0157][0158]
其中：f＝f
m
f
r1
f
r2
.
[0159]
步骤s32：设计参数估计器和如下：
[0160][0161]
其中c和k是用户定义的正定增益矩阵，而是估计状态，状态
[0162]
估计误差定义为
[0163]
步骤s33：使用式(12)和等式(13)，可以将误差动态写为：
[0164][0165]
步骤s34：m
u
可以被表达为：
[0166]
m
u
＝mβ1 i
x
β2 i
y
β3 i
z
β4,
ꢀꢀ
(15)
[0167]
其中：
[0168]
β1＝diag(1,1,1,0,0,0),β2＝diag(0,0,0,1,0,0),β3＝diag(0,0,0,0,1,0),β4＝diag(0,0,0,0,0,1)
[0169]
针对误差动态方程(14)，自适应控制律设计如下：
[0170][0171]
状态误差模型的目标是渐近稳定的的状态。
[0172]
步骤s35：对公式(16)进行证明，为了说明误差动力学方程(16)的收敛性，我们定义以下lyapunov候选函数：
[0173][0174]
步骤s36：对v1进行求导：
[0175][0176]
将等式(16)代入等式(18)，v1的时间导数可得出：
[0177][0178]
步骤s4：基于所涉及的参数估计器进行飞行平台位置控制，在建模误差存在的情况下进行神经网络滑模自适应控制，并解算出通过升力u1、翻滚力矩u2、俯仰力矩u3、偏航力矩u4；
[0179]
步骤s41：定义期望向量χ
d
＝[x
d
,y
d
,z
d
,φ
d
,θ
d
,ψ
d
]
t
，定义误差如下：
[0180]
e＝χ
‑
χ
d
.
ꢀꢀ
(20)
[0181]
步骤s42：让我们将滑动表面变量s定义为：
[0182][0183]
其中λ是对角增益矩阵。
[0184]
步骤s43：使用rbf神经网络进行质量评估，网络算法定义如下：
[0185][0186]
其中w
t
和分别是网络算法的权重和误差。
[0187]
步骤s44：估算的质量定义为：
[0188][0189]
步骤s45：从等式(22)至(23)，可以得到如下的质量估计误差：：
[0190][0191]
其中是一个有界变量，并定义
[0192]
步骤s46：符号f
*
可以定义如下：
[0193][0194]
步骤s47：抓取力动力学中存在建模误差，定义为δ
u
,让δ
m
＝f
‑
m
s
f
*
，并且δ＝δ
u
δ
m
，则错误估计被设计为
[0195]
针对无人机的动力学(4)，滑模神经网络控制器的设计如下：
[0196][0197]
其中：然后，方程(26)的动力学在平衡状态e＝0处逐渐稳定。
[0198]
步骤s5：针对所抓取的力分析进行二自由度机械臂力控制，并解算出控制力矩τ1,τ2；
[0199]
步骤s51：定义式(10)可写成：
[0200][0201]
其中j＝
‑
[δt(
i
r
b
‑1m
s
‑1 m
‑
1i
r
e
)]，
[0202]
步骤s52：使用等式(3)和(27)，可以将操纵器动力学重写为：
[0203][0204]
步骤s53：机械手的预期位移定义为并定义系统误差如下：
[0205][0206]
步骤s54：令等式(28)可以重写为：
[0207]
[0208]
步骤s55：rbf神经网络用于估计e，并且网络算法可以表示为：
[0209][0210]
其中和分别是网络算法的权重和误差，而
[0211]
步骤s56：针对无人机(28)的动态特性，鲁棒的自适应神经网络控制器的设计如下：
[0212][0213]
其中是e的估计值，而v
f
是一个鲁棒补偿器项，定义为：
[0214][0215]
其中其中等式(28)的动力学在平衡状态e
f
＝0处逐渐稳定。
[0216]
步骤s6：通过升力u1、翻滚力矩u2、俯仰力矩u3、偏航力矩u4解算出四个旋翼的转速ω
i
，i＝1,2,3,4；
[0217]
步骤s61：由公式(26)可以得出系统控制力和控制力矩u，其中u1、u2、u3和u4的关系为：
[0218]
u1＝c1(ω
12
ω
22
ω
32
ω
42
)
[0219]
u2＝c1(
‑
ω
22
ω
42
),u3＝c1(
‑
ω
12
ω
32
)
[0220]
u4＝c2(ω
12
‑
ω
22
ω
32
‑
ω
42
)
[0221]
(34)
[0222]
步骤s7：通过解算得到的数据，控制无人机。
[0223]
在本实施例中，参考图3
‑
图10，用一个具体的应用实例对本发明的操作进行详细说明，根据本发明控制方法进一步设计的控制器，主要研究在摩擦力和接触力的影响下滑翔抓取物体时的控制跟踪效果。具体设置如下：
[0224]
1)设定在摩擦力和接触力存在的条件下抓取0.5kg物体,并且设置接触碰撞时间较短,为0.02s：
[0225]
2)在模拟过程中考虑转动惯量随时间不断变化,同时考虑了外界的扰动对飞行平台产生的影响：
[0226]
3)硬件参数如表1所示：
[0227]
表1硬件参数
[0228]
[0229][0230]
如图3
‑
图10所示，根据本实施例控制方法进一步设计的控制器可以使作业型飞行机器人位置和姿态以及机械手转速各分量在较小的波动下追踪到目标轨迹。之后使作业型飞行机器人在较小的稳态误差下运动。图7可以看出，俯仰角在0.5s内有很明显的抖振。但超调量小、响应时间的短。仍认为控制器效果好。图3
‑
图10证明了本发明的有效性和优越性。
[0231]
以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。