CN110667845A - 一种面向阀门旋拧的双臂作业型飞行机器人系统及方法 - Google Patents

Abstract

本发明提出了一种面向阀门旋拧的双臂作业型飞行机器人系统及方法，作业型飞行机器人系统由非共线的倾斜六旋翼飞行器、作业装置和主端人机接口装置组成，其中作业装置包括一对二自由度机械臂及机械手组成；主端人机接口装置包括PC机和力反馈手控器；包括如下步骤：构建系统实施平台，所述系统实施平台由非共线的倾斜六旋翼飞行器、作业装置和主端人机接口装置组成；建立作业型飞行机器人系统的运动学和动力学模型；操作员通过主端人机接口装置远程控制进行旋拧阀门作业，建立主端力反馈人机接口设备的动力学模型。本发明解决了传统多旋翼无人机的欠驱动问题，系统的容错性和稳定性大大提高，且融合了视觉反馈与力觉反馈，提高了阀门旋拧作业的效率。

Description

一种面向阀门旋拧的双臂作业型飞行机器人系统及方法

技术领域

本发明涉及一种多旋翼无人机空中作业系统，尤其涉及一种搭载机械臂的非共线倾斜式六旋翼飞行器面向阀门旋拧的作业系统及方法。

背景技术

随着无人机技术的不断发展，飞行控制理论研究的不断深入，使得多旋翼飞行器的应用领域越来越广泛，诸如航拍摄影、安全巡检和农林植保等等。但是多旋翼的欠驱动性一直是无人机领域一个亟待解决的问题，尤其是涉及多旋翼无人机与物理环境交互方面，所以发明一种全驱动、可靠性高的无人机也是目前国内外研究的热点。

核能发电已经成为目前电力生产的一大趋势，但是它的工作环境具有高放射性、高温、高压等特点。因此一当发生安全事故，核电站环境将会变得更加高危，操作人员更加不宜进入现场进行作业。旋拧阀门是核电救灾的一个重要环节，利用无人机携带机械臂进入救灾现场旋拧阀门，将作业空间扩展到三维空间，从而可以快速的飞越地面屏障进入作业现场。

发明内容

本发明的目的是为了解决上述存在的问题，针对多旋翼无人机与物理环境交互时要求稳定性强，可靠性高等特点以及核电救灾中旋拧阀门的任务要求，本发明通过使用一种非共线的倾斜式六旋翼飞行器来执行阀门旋拧任务，解决了传统多旋翼无人机的欠驱动问题；建立安全有效的主端控制通道，提高阀门旋拧作业的效率。

为达到上述目的，本发明采用的方法是：一种面向阀门旋拧的双臂作业型飞行机器人系统，包括非共线的倾斜式六旋翼飞行器、作业装置和主端人机接口装置；所述的非共线倾斜式六旋翼飞行器包括六旋翼中心架、飞行控制器、倾斜机构和摄像头；所述倾斜机构包括六旋翼机架、无刷电机倾斜底座、无刷电机和螺旋桨；所述作业装置包括一个或多个机械关节、一个或多个用于连接和驱动连杆的舵机以及末端用于夹住物体的机械手；所述主端人机接口装置包括PC机和力反馈手控器；所述的力反馈手控器具有三个位置自由度、三个关节自由度和一对按键；其中三个位置自由度能够控制飞行机器人的运动，包括上下运动、前后运动、左右运动；关节自由度用于控制机械臂关节转动，按键用于控制机械手的张合；力反馈手控器输出信息给PC机，PC机再与飞行机器人通讯传送控制信号；所述的摄像头将实时图像传送给PC机，操作员根据接收到的视觉反馈在线决策，操作员通过主端人机接口设备远程操控从端的飞行器作业系统执行阀门旋拧任务，力反馈再通过手控器返回给操作员。

作为本发明的一种改进，所述的机械手内壁附着弧形形状的海绵体，一方面可以增加机械手与阀门接触的摩擦力，另一方面可以降低摩擦对机械手内壁的磨损。

作为本发明的一种改进，六旋翼飞行器中心架采用碳纤维材料制成。

作为本发明的一种改进，所述的作业装置采用合成树脂3D打印加工而成。

本发明还公开了上述一种面向阀门旋拧的双臂作业型飞行机器人系统的设计方法，包括如下步骤：

步骤一：构建系统实施平台，所述系统实施平台由非共线的倾斜式六旋翼飞行器、作业装置和主端人机接口装置组成；

步骤二：根据六旋翼飞行器的螺旋桨倾斜角度α、β，建立携带双臂的倾斜式六旋翼飞行器作业系统的运动学和动力学模型；

步骤三：操作员通过力反馈手控器远程控制作业装置进行旋拧阀门作业，建立主端力反馈人机接口设备的动力学模型。

步骤一中，构建系统实施平台，包括如下步骤：

步骤1.1：所述非共线倾斜式六旋翼飞行器包括六旋翼中心架、飞行控制器、倾斜机构和摄像头；

步骤1.2：所述作业装置包括一个或多个机械关节、一个或多个用于连接和驱动连杆的舵机以及末端用于夹住物体的机械手；

步骤1.3：所述主端人机接口装置包括PC机和力反馈手控器；

步骤1.4：假设螺旋桨的旋转中心与倾斜机构的质心相重合，以第i个螺旋桨的旋转中心为原点，建立螺旋桨坐标系{O_i}，其中i表示螺旋桨的序号，i＝{1,2,3,4,5,6}。L_i表示螺旋桨旋转中心到六旋翼飞行器质心之间的距离，为了保证六个螺旋桨的中心位于一个平面上，我们设置

R_z()表示围绕Z轴的旋转矩阵，e₁＝[1,0,0]；

步骤1.5：α_i表示第i个螺旋桨相对于轴X_i的倾斜角，β_i表示第i个螺旋桨相对于轴y_i的倾斜角；

步骤二中，建立携带双臂的倾斜式六旋翼飞行器作业系统的运动学和动力学模型，包括如下步骤：

步骤2.1：假设六旋翼飞行器、机械臂都是刚体，忽略其弹性形变，且质量分布均匀。对相关坐标系做如下定义：固连在地面上的基坐标系{O_W}为世界惯性坐标系，包括X_W、y_W、Z_W三个基准轴；{O_b}为机体坐标系，以六旋翼飞行机器人的重心为原点，包括X_b、y_b、Z_b三个基准轴；{O_e}为机械臂末端坐标系，包括X_e、y_e、Z_e三个基准轴；

步骤2.2：令P＝[x,y,z]^T表示机体坐标系{O_b}的原点在世界惯性坐标系{O_W}中的位置矢量，Ω＝[p,q,r]^T表示机体坐标系{O_b}相对于世界惯性坐标系{O_W}的角速度，Φ＝[φ,θ,ψ]^T表示机体坐标系{O_b}在世界惯性坐标系{O_W}中用欧拉角表示的姿态向量，其中，Φ是绕X_b轴的横滚角，θ是绕y_b轴的俯仰角，Ψ是绕Z_b轴的偏航角。坐标系{O_b}到坐标系{O_W}的旋转矩阵R_b ^w可以表示为：

R_b ^w＝R(x,φ)R(y,θ)R(z,ψ)

步骤2.3：对于二自由度的机械臂，表示机械臂末端坐标系{O_e}的原点在世界惯性坐标系{O_w}中的位置矢量和姿态矢量分别为P_e ^w和Φ_e ^w。其与六旋翼飞行机器人的位置和姿态有如下关系：

P_e ^w＝P+P_e ^bR_b ^w

Φ_e ^w＝Φ_e ^bR_b ^w

其中，P_e ^b和Φ_e ^b分别为机械臂末端在机体坐标系{O_b}的位置和姿态。

步骤2.4：对于由非共线倾斜式六旋翼飞行机器人和机械臂组成的复合系统，由于机械臂在实际旋拧阀门过程中与六旋翼飞行器可以看作刚性连接，并且各关节的运动是平滑缓慢的，这种缓慢的变化仅影响复合系统的重心和转动惯量，对飞行机器人系统而言可以看成变化的静态状态。综合以上分析，本项目将飞行机器人和机械臂二者间的耦合当作外部扰动，对复合系统静态情况下的动力学进行建模分析，采用牛顿-欧拉动力学方程构建复合系统动力学方程：

其中，m表示复合系统的质量，G＝[0,0,-mg]^T表示复合系统的重力矢量，U＝[f₁,f₂,f₃,f₄,f₅,f₆]^T表示螺旋桨产生的升力大小矩阵，I_m表示复合系统重心处的惯性张量，向量r_G＝[x_G,y_G,z_G]^T表示复合系统的重心在机体坐标系{O_b}中的偏移。F_b(α,β)表示桨叶作用于复合系统上的推动力矢量，τ_AM(α,β)为作用于复合系统的输入扭矩，包括电机推力和与电机旋转方向相反的反作用力产生的扭矩两部分，F_env为复合系统与外部环境的交互力，τ_Fenv为复合系统与外部环境的交互力矩。

步骤三中，操作员通过力反馈手控器远程控制作业装置进行旋拧阀门作业，建立主端力反馈人机接口设备的动力学模型，包括如下步骤：

步骤3.1：利用力反馈手控器作为遥操作控制系统的主端设备，所述的力反馈手控器具有三个位置自由度、三个关节自由度和一对按键；其中三个位置自由度能够控制飞行机器人的运动，包括上下运动、前后运动、左右运动，三个关节自由度能够控制机械臂关节的转动，按键能够控制机械手的张合；手控器输出信息给PC机，PC机再与飞行机器人通讯传送控制信号；

步骤3.2：六旋翼下的摄像头将实时图像传送给PC机，操作员根据接收到的视觉反馈在线决策，操作员通过主端人机接口设备远程操控从端的飞行器作业系统执行阀门旋拧任务，力反馈再通过手控器返回给操作员；主端力反馈人机接口设备可视为一个完全驱动的常规机械系统，经过重力补偿的动力学模型可以简化为：

其中，q∈R³是力反馈人机接口设备末端执行器的位置，M_m∈R^3×3是惯性矩阵，f_h∈R³是操作员施加于力反馈人机接口设备上的力，f_c∈R³是主端本地控制力。

有益效果：与现有技术相比，本发明具有如下显著优势：

本发明通过使用一种非共线的倾斜式六旋翼飞行器来执行阀门旋拧任务，系统的容错性和稳定性大大提高。1、全驱动的倾斜式六旋翼飞行器能够提供六自由度的驱动力，解决了传统多旋翼无人机的欠驱动问题，同时增加了旋拧阀门时的稳定性和灵活性；2、本发明的双臂作业型机器人系统质量轻，六旋翼飞行器构架采用碳纤维材料，机械臂采用合成树脂3D打印加工而成；3、本发明的主端人机接口装置利用力反馈手控器远程控制作业系统进行阀门旋拧作业，融合了操作员接收到的视觉反馈与手控器接收到的力觉反馈，提高了阀门旋拧作业的效率。

附图说明

图1是本发明的系统结构示意图；

图2是本发明中螺旋桨绕X_i轴旋转示意图；

图3是本发明中螺旋桨绕y_i轴旋转示意图；

图4是本发明中二自由度机械臂及机械手结构图；

图5是发明中作业型飞行机器人复合系统坐标系示意图；

图6是本发明中力反馈手控器结构图。

具体实施方式

下面结合附图和实例对本发明的技术方案作进一步说明。

本发明针对多旋翼无人机与物理环境交互时要求稳定性强，可靠性高等特点以及核电救灾中旋拧阀门的任务要求，本发明通过使用一种非共线的倾斜式六旋翼飞行器来执行阀门旋拧任务，系统的容错性和稳定性大大提高，解决了传统多旋翼无人机的欠驱动问题，增加了旋拧阀门时的稳定性和灵活性。

如图1所示，本实施例公开的一种面向阀门旋拧的双臂作业型飞行机器人系统，包括非共线的倾斜式六旋翼飞行器、作业装置和主端人机接口装置。

所述的非共线倾斜式六旋翼飞行器包括六旋翼中心架1、飞行控制器2、倾斜机构3和摄像头4；所述倾斜机构包括六旋翼机架3-1、无刷电机倾斜底座3-2、无刷电机3-4和螺旋桨3-3。所述作业装置包括一个或多个机械关节5-1、5-2、一个或多个用于连接和驱动连杆的舵机5-3、5-4、5-5以及末端用于夹住物体的机械手5-6。

所述主端人机接口装置包括PC机6和力反馈手控器7。所述的力反馈手控器具有三个位置自由度、三个关节自由度和一对按键；其中三个位置自由度7-1、7-2、7-3能够控制飞行机器人的运动，包括上下运动、前后运动、左右运动；关节自由度7-4能够控制机械臂关节5-1转动，关节自由度7-5能够控制机械臂关节5-2转动；按键7-6能够控制机械手5-6的张合。

力反馈手控器7输出信息给PC机6，PC机6再与飞行机器人通讯传送控制信号；所述的摄像头4将实时图像传送给PC机6，操作员根据接收到的视觉反馈在线决策，操作员通过主端人机接口设备远程操控从端的飞行器作业系统执行阀门8旋拧任务，力反馈再通过手控器返回给操作员。

所述的机械手5-6内壁附着弧形形状的海绵体。六旋翼飞行器中心架采用碳纤维材料制成。所述的作业装置采用合成树脂3D打印加工而成。

本实施例还公开了一种面向阀门旋拧的双臂作业型飞行机器人系统的设计方法，包括如下步骤：

步骤一：构建系统实施平台，所述系统实施平台由非共线的倾斜式六旋翼飞行器、作业装置和主端人机接口装置组成；

步骤二：根据六旋翼飞行器的螺旋桨倾斜角度α、β，建立携带双臂的倾斜式六旋翼飞行器作业系统的运动学和动力学模型；

步骤三：操作员通过力反馈手控器远程控制作业装置进行旋拧阀门作业，建立主端力反馈人机接口设备的动力学模型。

步骤一中，构建系统实施平台，包括如下步骤：

步骤1.1：如图1所示，所述非共线倾斜式六旋翼飞行器包括六旋翼中心架1、飞行控制器2、倾斜机构3和摄像头4；

步骤1.1.1：如图2所示，所述倾斜机构包括六旋翼机架3-1、无刷电机倾斜底座3-2、无刷电机3-4和螺旋桨3-3；

步骤1.2：如图4所示，所述作业装置包括一个或多个机械关节5-1、5-2、一个或多个用于连接和驱动连杆的舵机5-3、5-4、5-5以及末端用于夹住物体的机械手5-6；

步骤1.2.1：所述机械手内壁附着弧形形状的海绵体，一方面可以增加机械手与阀门接触的摩擦力，另一方面可以降低摩擦对机械手内壁的磨损；

步骤1.3：如图1所示，所述主端人机接口装置包括PC机6和力反馈手控器7；

步骤1.4：假设螺旋桨的旋转中心与倾斜机构的质心相重合，如图2所示，以第i个螺旋桨的旋转中心为原点，建立螺旋桨坐标系{O_i}，其中i表示螺旋桨的序号，i＝{1,2,3,4,5,6}，包括X_i、y_i、Z_i三个基准轴，X_i表示穿过螺旋桨旋转中心并沿着机架向外延伸的方向，Z_i为第i个是螺旋桨旋转的轴，与产生拉力的方向一致，y_i表示垂直于X_i和y_i的方向，L_i表示螺旋桨旋转中心到六旋翼飞行器质心之间的距离，为了保证六个螺旋桨的中心位于一个平面上，我们设置

R_z()表示围绕Z轴的旋转矩阵，e₁＝[1,0,0]；

步骤1.5：如图2所示，α_i表示第i个螺旋桨相对于轴X_i的倾斜角，如图3所示，β_i表示第i个螺旋桨相对于轴y_i的倾斜角；六旋翼的倾转角度α、β在飞行前进行调整，根据指定轨迹的需要改变角度，α＝(α₁,α₂,α₃,α₄,α₅,α₆)，β＝(β₁,β₂,β₃,β₄,β₅,β₆)，我们对倾斜角做如下约束：α₁＝±α₂＝±α₃＝±α₄＝±α₅＝±α₆＝α，β₁＝±β₂＝±β₃＝±β₄＝±β₅＝±β₆＝β；

步骤二中，建立携带双臂的倾斜式六旋翼飞行器作业系统的运动学和动力学模型，包括如下步骤：

步骤2.1：假设六旋翼飞行器、机械臂都是刚体，忽略其弹性形变，且质量分布均匀。如图5所示，对相关坐标系做如下定义：固连在地面上的基坐标系{O_W}为世界惯性坐标系，包括X_W、y_W、Z_W三个基准轴；{O_b}为机体坐标系，以六旋翼飞行机器人的重心为原点，包括X_b、y_b、Z_b三个基准轴；{O_e}为机械臂末端坐标系，包括X_e、y_e、Z_e三个基准轴；

步骤2.2：如图所示，令P＝[x,y,z]^T表示机体坐标系{O_b}的原点在世界惯性坐标系{O_W}中的位置矢量，Ω＝[p,q,r]^T表示机体坐标系{O_b}相对于世界惯性坐标系{O_W}的角速度，Φ＝[φ,θ,ψ]^T表示机体坐标系{O_b}在世界惯性坐标系{O_W}中用欧拉角表示的姿态向量，其中，Φ是绕X_b轴的横滚角，θ是绕y_b轴的俯仰角，Ψ是绕Z_b轴的偏航角。坐标系{O_b}到坐标系{O_W}的旋转矩阵R_b ^w可以表示为：

R_b ^w＝R(x,φ)R(y,θ)R(z,ψ)

步骤2.3：对于二自由度的机械臂，表示机械臂末端坐标系{O_e}的原点在世界惯性坐标系{O_w}中的位置矢量和姿态矢量分别为P_e ^w和Φ_e ^w。其与六旋翼飞行机器人的位置和姿态有如下关系：

P_e ^w＝P+P_e ^bR_b ^w

Φ_e ^w＝Φ_e ^bR_b ^w

其中，P_e ^b和Φ_e ^b分别为机械臂末端在机体坐标系{O_b}的位置和姿态。

步骤2.4：对于由非共线倾斜式六旋翼飞行机器人和机械臂组成的复合系统，由于机械臂在实际旋拧阀门过程中与六旋翼飞行器可以看作刚性连接，并且各关节的运动是平滑缓慢的，这种缓慢的变化仅影响复合系统的重心和转动惯量，对飞行机器人系统而言可以看成变化的静态状态。综合以上分析，本项目将飞行机器人和机械臂二者间的耦合当作外部扰动，对复合系统静态情况下的动力学进行建模分析，采用牛顿-欧拉动力学方程构建复合系统动力学方程：

其中，m表示复合系统的质量，G＝[0,0,-mg]^T表示复合系统的重力矢量，U＝[f₁,f₂,f₃,f₄,f₅,f₆]^T表示螺旋桨产生的升力大小矩阵，I_m表示复合系统重心处的惯性张量，向量r_G＝[x_G,y_G,z_G]^T表示复合系统的重心在机体坐标系{O_b}中的偏移。F_b(α,β)表示桨叶作用于复合系统上的推动力矢量，τ_AM(α,β)为作用于复合系统的输入扭矩，包括电机推力和与电机旋转方向相反的反作用力产生的扭矩两部分，F_env为复合系统与外部环境的交互力，τ_Fenv为复合系统与外部环境的交互力矩。

步骤2.4.1：所述

f_i表示第i个螺旋桨产生的升力大小，在螺旋桨坐标系下，f_i＝[0,0,K_fw_i ²]^T，K_f表示螺旋桨升力系数；

步骤2.4.2：所述τ_AM(α,β)U＝τ_f+τ_d，τ_f为螺旋桨升力产生的力矩，

τ_d为与螺旋桨旋转方向相反的反作用力产生的阻力扭矩，

K_M表示螺旋桨阻力系数；

步骤三中，操作员通过力反馈手控器远程控制作业装置进行旋拧阀门作业，包括如下步骤：

步骤3.1：利用力反馈手控器作为遥操作控制系统的主端设备，如图6所示，所述的力反馈手控器具有三个位置自由度、三个关节自由度和一对按键；其中三个位置自由度7-1、7-2、7-3能够控制飞行机器人的运动，包括上下运动、前后运动、左右运动；关节自由度7-4能够控制机械臂关节5-1转动，关节自由度7-5能够控制机械臂关节5-2转动；按键7-6能够控制机械手5-6的张合；手控器输出信息给PC机，PC机再与飞行机器人通讯传送控制信号；

步骤3.2：六旋翼下的摄像头4将实时图像传送给PC机，操作员根据接收到的视觉反馈在线决策，操作员通过主端人机接口设备远程操控从端的飞行器作业系统执行阀门8旋拧任务，力反馈再通过手控器返回给操作员；主端力反馈人机接口设备可视为一个完全驱动的常规机械系统，经过重力补偿的动力学模型可以简化为：

其中，q∈R³是力反馈人机接口设备末端执行器的位置，M_m∈R^3×3是惯性矩阵，f_h∈R³是操作员施加于力反馈人机接口设备上的力，f_c∈R³是主端本地控制力。

Claims (8)

1.一种面向阀门旋拧的双臂作业型飞行机器人系统，其特征在于：包括非共线的倾斜式六旋翼飞行器、作业装置和主端人机接口装置；所述的非共线倾斜式六旋翼飞行器包括六旋翼中心架(1)、飞行控制器(2)、倾斜机构(3)和摄像头(4)；所述倾斜机构包括六旋翼机架(3-1)、无刷电机倾斜底座(3-2)、无刷电机(3-4)和螺旋桨(3-3)；所述作业装置包括一个或多个机械关节、一个或多个用于连接和驱动连杆的舵机以及末端用于夹住物体的机械手(5-6)；所述主端人机接口装置包括PC机(6)和力反馈手控器(7)；所述的力反馈手控器(7)具有三个位置自由度、三个关节自由度和一对按键；其中三个位置自由度能够控制飞行机器人的运动，包括上下运动、前后运动、左右运动；关节自由度用于控制机械臂关节转动，按键用于控制机械手(5-6)的张合；力反馈手控器(7)输出信息给PC机(6)，PC机(6)再与飞行机器人通讯传送控制信号；所述的摄像头(4)将实时图像传送给PC机(6)，操作员根据接收到的视觉反馈在线决策，操作员通过主端人机接口设备远程操控从端的飞行器作业系统执行阀门(8)旋拧任务，力反馈再通过手控器返回给操作员。

2.根据权利要求1所述的面向阀门旋拧的双臂作业型飞行机器人系统，其特征在于：所述的机械手(5-6)内壁附着弧形形状的海绵体。

3.根据权利要求1所述的面向阀门旋拧的双臂作业型飞行机器人系统，其特征在于：六旋翼飞行器中心架采用碳纤维材料制成。

4.根据权利要求1所述的面向阀门旋拧的双臂作业型飞行机器人系统，其特征在于：所述的作业装置采用合成树脂3D打印加工而成。

5.一种面向阀门旋拧的双臂作业型飞行机器人系统的设计方法，其特征在于，包括如下步骤：

步骤一：构建如权利要求1中所述的系统实施平台；

步骤二：根据六旋翼飞行器的螺旋桨倾斜角度α、β，建立携带双臂的倾斜式六旋翼飞行器作业系统的运动学和动力学模型；

步骤三：操作员通过力反馈手控器远程控制作业装置进行旋拧阀门作业，建立主端力反馈人机接口设备的动力学模型。

6.根据权利要求5所述的一种面向阀门旋拧的双臂作业型飞行机器人系统的设计方法，其特征在于，步骤一中，构建系统实施平台，包括如下步骤：

步骤1.1：假设螺旋桨的旋转中心与倾斜机构的质心相重合，以第i个螺旋桨的旋转中心为原点，建立螺旋桨坐标系{O_i}，其中i表示螺旋桨的序号，i＝{1,2,3,4,5,6}。L_i表示螺旋桨旋转中心到六旋翼飞行器质心之间的距离，为了保证六个螺旋桨的中心位于一个平面上，我们设置

R_z()表示围绕Z轴的旋转矩阵，e₁＝[1,0,0]；

步骤1.2：α_i表示第i个螺旋桨相对于轴X_i的倾斜角，β_i表示第i个螺旋桨相对于轴y_i的倾斜角。

7.根据权利要求5所述的一种面向阀门旋拧的双臂作业型飞行机器人系统的设计方法，其特征在于，步骤二中，建立携带双臂的倾斜式六旋翼飞行器作业系统的运动学和动力学模型，包括如下步骤：

步骤2.1：假设六旋翼飞行器、机械臂都是刚体，忽略其弹性形变，且质量分布均匀，对相关坐标系做如下定义：固连在地面上的基坐标系{O_W}为世界惯性坐标系，包括X_W、y_W、Z_W三个基准轴；{O_b}为机体坐标系，以六旋翼飞行机器人的重心为原点，包括X_b、y_b、Z_b三个基准轴；{O_e}为机械臂末端坐标系，包括X_e、y_e、Z_e三个基准轴；

步骤2.2：令P＝[x,y,z]^T表示机体坐标系{O_b}的原点在世界惯性坐标系{O_W}中的位置矢量，Ω＝[p,q,r]^T表示机体坐标系{O_b}相对于世界惯性坐标系{O_W}的角速度，Φ＝[φ,θ,ψ]^T表示机体坐标系{O_b}在世界惯性坐标系{O_W}中用欧拉角表示的姿态向量，其中，Φ是绕X_b轴的横滚角，θ是绕y_b轴的俯仰角，Ψ是绕Z_b轴的偏航角，坐标系{O_b}到坐标系{O_W}的旋转矩阵R_b ^w可以表示为：

R_b ^w＝R(x,φ)R(y,θ)R(z,ψ)

步骤2.3：对于二自由度的机械臂，表示机械臂末端坐标系{O_e}的原点在世界惯性坐标系{O_w}中的位置矢量和姿态矢量分别为P_e ^w和Φ_e ^w，其与六旋翼飞行机器人的位置和姿态有如下关系：

P_e ^w＝P+P_e ^bR_b ^w

Φ_e ^w＝Φ_e ^bR_b ^w

其中，P_e ^b和Φ_e ^b分别为机械臂末端在机体坐标系{O_b}的位置和姿态；

步骤2.4：将飞行机器人和机械臂二者间的耦合当作外部扰动，对复合系统静态情况下的动力学进行建模分析，采用牛顿-欧拉动力学方程构建复合系统动力学方程：

其中，m表示复合系统的质量，G＝[0,0,-mg]^T表示复合系统的重力矢量，U＝[f₁,f₂,f₃,f₄,f₅,f₆]^T表示螺旋桨产生的升力大小矩阵，I_m表示复合系统重心处的惯性张量，向量r_G＝[x_G,y_G,z_G]^T表示复合系统的重心在机体坐标系{O_b}中的偏移，F_b(α,β)表示桨叶作用于复合系统上的推动力矢量，τ_AM(α,β)为作用于复合系统的输入扭矩，包括电机推力和与电机旋转方向相反的反作用力产生的扭矩两部分，F_env为复合系统与外部环境的交互力，τ_Fenv为复合系统与外部环境的交互力矩。

8.根据权利要求5所述的一种面向阀门旋拧的双臂作业型飞行机器人系统的设计方法，其特征在于，步骤三中，操作员通过力反馈手控器远程控制作业装置进行旋拧阀门作业，建立主端力反馈人机接口设备的动力学模型，包括如下步骤：

步骤3.1：利用力反馈手控器作为遥操作控制系统的主端设备，所述的力反馈手控器具有三个位置自由度、三个关节自由度和一对按键；其中三个位置自由度能够控制飞行机器人的运动，包括上下运动、前后运动、左右运动，三个关节自由度能够控制机械臂关节的转动，按键能够控制机械手的张合；手控器输出信息给PC机，PC机再与飞行机器人通讯传送控制信号；

步骤3.2：摄像头将实时图像传送给PC机，操作员根据接收到的视觉反馈在线决策，操作员通过主端人机接口设备远程操控从端的飞行器作业系统执行阀门旋拧任务，力反馈再通过手控器返回给操作员；主端力反馈人机接口设备可视为一个完全驱动的常规机械系统，经过重力补偿的动力学模型可以简化为：

其中，q∈R³是力反馈人机接口设备末端执行器的位置，M_m∈R^3×3是惯性矩阵，f_h∈R³是操作员施加于力反馈人机接口设备上的力，f_c∈R³是主端本地控制力。

Images