CN108248845A - 一种基于动态重心补偿的旋翼飞行机械臂系统及算法 - Google Patents

Abstract

本发明公开了一种基于动态重心补偿的旋翼飞行机械臂系统及算法，所述系统包括旋翼飞行平台、图像传感器、连接架、机械臂系统、系统控制器和地面站控制装置；所述旋翼飞行平台包括旋翼飞行器和飞行控制器；所述图像传感器安装在所述旋翼飞行平台的前下方；所述连接架是一块机械板，用于固定连接所述旋翼飞行平台、图像传感器和机械臂系统；所述机械臂系统安装在旋翼飞行平台的正下方；所述系统控制器安装在旋翼飞行平台的正上方；所述地面站控制装置与所述系统控制器之间采用无线方式进行通讯。该系统结构设计合理，安全性高，稳定性强；该算法提升了抓取的准确率，同时也提高了抓取时间效率。

Description

一种基于动态重心补偿的旋翼飞行机械臂系统及算法

技术领域

本发明属于机器人设计和控制技术领域，涉及一种飞行机器人，具体涉及一种基于动态重心补偿的旋翼飞行机械臂系统及算法。

背景技术

在地面移动机器人技术高速发展的同时，无人飞行器的研究也取得了突破性进展，因其具有体积小、操作灵活、悬停等特点，在监测、地图遥感、商业航拍、农业应用等领域得到了广泛的应用，但大多属于被动观测。随着研究技术的不断发展，人们期望飞行机器人能与环境相互作用。随着地面机器人装载多自由度机械臂而构成的地面操作机器人在救灾救援以及反恐防爆等领域得到认可并成功应用，受此激发，人们把在飞行器平台上搭载多自由度机械臂组成具有高性能的飞行平台称为无人空中操纵器。此举措将移动机器人的工作区域从二维空间扩展到三维空间，同时扩展了无人飞行器的应用范围，如空中操纵、空中运输等。

2015年，马乐、杨保林等人在公开号为“CN105014687A”、名称为“一种带有多旋翼无人机的机械臂”的发明专利中公开了一种带有多旋翼无人机的机械臂，机械臂由大臂、小臂和夹持器组成，固定连接在多旋翼无人机下方。该发明虽然解决了无法自由在高空运作问题，但是由于所述机械臂自由度少，缺乏灵活性，应用范围有限。2017年，陈浩耀、全凤宇在公开号为“CN106985159A”、名称为“一种带有柔性抓取器的飞行机械臂”的发明专利中公开了一种带有柔性抓取器的旋翼飞行机械臂，其中多自由度机械臂和旋翼飞行器相连，柔性抓取器和多自由度机械臂相连，以更好的适应抓取物外形。虽然该发明所述机械臂自由度增加，具备足够的灵活性，但只在结构上加以改进，而没有充分考虑机械臂与旋翼飞行器之间的耦合以及干扰，在抓取作业中缺乏稳定性，很可能导致抓取失败，同时仅依靠人工定位方式实现抓取，一般需要多次试探，效率低下，而且在目标与操作人员相距较远的情况下无法完成抓取。

发明内容

针对现有技术中存在的上述问题，本发明提供了一种基于动态重心补偿的旋翼飞行机械臂系统及算法，该系统结构设计合理，安全性高，稳定性强；该算法在保证旋翼飞行机械臂系统稳定飞行的基础上，基于视觉伺服控制方法能更准确的自动定位在抓取目标上，提升了抓取的准确率，同时也提高了抓取时间效率。

为此，本发明采用了以下技术方案：

一种基于动态重心补偿的旋翼飞行机械臂系统，包括旋翼飞行平台、图像传感器、连接架、机械臂系统、系统控制器和地面站控制装置；所述旋翼飞行平台包括旋翼飞行器和飞行控制器；所述图像传感器安装在所述旋翼飞行平台的前下方；所述连接架是一块机械板，用于固定连接所述旋翼飞行平台、图像传感器和机械臂系统；所述机械臂系统安装在旋翼飞行平台的正下方；所述系统控制器安装在旋翼飞行平台的正上方；所述地面站控制装置与所述系统控制器之间采用无线方式进行通讯。

进一步地，所述旋翼飞行器包括机架、机臂、动力设备及驱动装置、电源模块、定位模块、遥控与接收器；所述机架是飞行器平台的机身，也是其他组成系统模块的基本载体；所述机臂连接在机身上；所述动力设备及驱动装置包括无刷电机、无刷电调和螺旋桨，所述无刷电调与无刷电机相连，所述螺旋桨安装在无刷电机上；所述电源模块包括航模电池、稳压电路以及电池低压报警装置；所述定位模块用于确定旋翼飞行器相对地面的位置。

进一步地，所述机臂为4个，相邻机臂相互垂直；所述动力设备及驱动装置为4组，每组对应一个机臂；所述机架为550轴距的碳纤维铝合金机架；所述飞行控制器采用内核32位处理器，包括能实时测量飞行器姿态信息的9轴惯性测量单元，对一切飞行控制信号集中处理，同时接收并处理系统控制器的信号。

优选地，所述图像传感器用于实时获取目标信息并传送至系统控制器。

优选地，所述机械臂系统是一个五自由度机械臂系统，包括五个机器人舵机和抓取手；所述机械臂系统的执行机构包括第一转向机构、第一收放机构、第二收放机构、第二转向机构和夹持机构；第一舵机分别与连接架和第一连杆相连组成第一转向结构，第二舵机和第三舵机分别与第一连杆、第二连杆和第三连杆串接组成第一收放机构和第二收放机构，第四舵机和第三连杆组成第二转向机构，第五舵机与抓取手组成夹持机构；夹持机构是直接与抓取目标接触的部件，收放机构带动夹持机构接触目标，转向机构通过自身旋转调整夹持机构的动作方向，使夹持机构以正确的方向打开抓取手。

优选地，所述地面站控制装置包括机械臂系统控制模块、旋翼飞行器控制模块、图像信息显示模块；所述机械臂系统控制模块用于实现机械臂的运动模式选择，包括仿真模式和实际模式、串口设置、机械臂角度变化速度调节、机械臂运动控制，所述机械臂运动控制包括在笛卡尔空间的线运动控制和关节角运动控制；旋翼飞行器控制模块包括飞行器在笛卡尔空间的上下左右运动步长调节设置和位置坐标和姿态角信息的显示；图像信息显示模块包括相机串口设置、目标的图像显示和像素坐标显示。

优选地，所述地面站控制装置、系统控制器与飞行控制器之间保持实时通讯；所述飞行控制器通过融合惯性测量单元和GPS等传感器测量数据获取实时位姿信息，并通过动态重心补偿算法实现飞行器的位置控制、姿态控制，实现旋翼飞行机械臂系统的稳定飞行；所述系统控制器通过视觉伺服控制方法来控制机械臂各关节的协调运动，实现目标的自主定位和抓取。

一种动态重心补偿算法，包括以下步骤：

步骤一，数据读取；

步骤二，系统重心估算；

步骤三，基于重心补偿设计旋翼飞行器的位置和姿态控制器。

进一步地，步骤二的具体过程如下：

(1)计算关节臂i的质心相对于机体坐标系{B}的位置坐标p_i：

式中：分别表示关节臂i坐标系J_i相对于飞行平台坐标系{B}的旋转变换矩阵和位置坐标，是关节臂i的质心相对于关节臂i坐标系J_i的局部坐标；

(2)估算目标抓取物体质量m_d，当旋翼飞行器悬停于某一点时，其俯仰角和翻滚角都为0的情况下，其升力u₁大小与系统所受总重力相等，根据受力分析有：

利用最小二乘法进行求解，可得m_d的估计值为：

(3)令机体坐标系为系统重心参考坐标系，根据多体系统质心求解理论，旋翼飞行机械臂系统的重心为：

进一步地，步骤三的具体过程如下：

(1)定义机体坐标系{B}在世界坐标系{_I}中位置矢量和方向向量分别为ξ＝[x,y,z]^T，具有重心补偿的目标姿态角度和总升力分别为：

其中：m为旋翼飞行机械臂系统总质量，a₁，a₂，a₃为位置重心补偿项；

(2)根据位置控制器的输出得到期望的姿态信息，结合系统动力学模型，加入姿态控制重心补偿项，设计姿态稳定控制器：

其中：(I_xx,I_yy,I_zz)^T为系统惯性张量，c₁,c₂,c₃为重心补偿项，β_i(i＝1,2,...,6)为常数，输出分别表示旋翼飞行器做横滚运动、俯仰运动以及偏航运动所需控制量；

(3)经过解耦输出到各电机，驱动螺旋桨旋转，控制旋翼飞行机械臂系统稳定飞行。

与现有技术相比，本发明的有益效果是：

(1)稳定性强。旋翼飞行机械臂系统的稳定性受外部干扰和机械臂运动影响，尤其是机械臂在执行抓取动作时，机械臂运动引起旋翼飞行机械臂系统重心严重偏移。加入动态重心补偿项具有良好的补偿效果，提高了系统的姿态控制性能，增强了稳定性。

(2)准确率高，效率高。本发明提出的动态重心补偿算法在保证旋翼飞行机械臂系统的稳定飞行的基础上，基于视觉伺服控制方法能更准确的自动定位在抓取目标上，提升了抓取的准确率，同时也提高了抓取时间效率。

附图说明

图1是本发明所提供的一种基于动态重心补偿的旋翼飞行机械臂系统的整体结构示意图。

图2是机械臂系统的局部结构图。

图3是本发明所提供的一种动态重心补偿算法的流程图。

附图标记说明：1、旋翼飞行器；2、图像传感器；3、连接架；4、机械臂系统；5、第一转向机构；6、第一收放机构；7、第二收放机构；8、第二转向机构；9、夹持机构。

具体实施方式

下面结合附图以及具体实施例来详细说明本发明，其中的具体实施例以及说明仅用来解释本发明，但并不作为对本发明的限定。

如图1所示，本发明公开了一种基于动态重心补偿的旋翼飞行机械臂系统，包括四旋翼飞行器1、图像传感器2、连接架3和机械臂系统4，所述图像传感器2的摄像头方向朝下，并固定于连接架3前下方；连接架3是用于固定连接旋翼飞行器1和机械臂系统4、图像传感器2的一块机械板；多自由度机械臂系统4一般能实现在空间任意位姿的操作，机械臂结构的自由度至少为三，本发明机械臂结构的自由度为五，具有任意空间位置的灵活性，为尽量减小重心偏移和作业方便其固定于连接架3正下方。

如图1所示的四旋翼飞行器1的硬件结构包括机架、4组动力设备及驱动装置、飞行控制器、电源模块、定位模块、遥控与接收器等部分。机架是飞行器平台的机身和嵌联在平台上的四个机臂，相邻机臂相互垂直，采用550轴距的碳纤维铝合金机架，在减轻机体自重的同时提高机体稳固性。机架也是其他组成系统模块的基本载体，如飞行控制器、电源、定位模块都安装在平台上方。定位模块指GPS，由其确定旋翼飞行器目前在相对地面的位置。飞行控制器采用内核32位处理器，并包括能实时测量飞行器姿态信息的9轴惯性测量单元，对一切飞行控制信号集中处理，同时接收并处理系统控制器的信号。动力设备及驱动装置，包括螺旋桨、无刷电调和无刷电机，无刷电调接收飞行控制器的控制信号，无刷电机再接收电调输出的PWM信号，驱动螺旋桨旋转，提供整个飞行机械臂系统的总升力。电源模块通过降压之后为旋翼飞行器其他模块和机械臂系统供电。

如图1所示的机械臂系统4与旋翼飞行器1通过连接架3固定相连，如图2所示，机械臂系统由五个机器人舵机和抓取手组成，是一个五自由度机械臂系统；所述机械臂系统的执行机构包括第一转向机构5、第一收放机构6、第二收放机构7、第二转向机构8和夹持机构9；第一舵机分别与连接架和第一连杆相连组成第一转向结构5，第二舵机和第三舵机分别与第一连杆、第二连杆和第三连杆串接组成第一收放机构6和第二收放机构7，第四舵机和第三连杆组成第二转向机构8，第五舵机与抓取手组成夹持机构9；夹持机构9是直接与抓取目标接触的部件，收放机构6、7带动夹持机构9接触目标，转向机构5、8通过自身旋转调整夹持机构9的动作方向，使夹持机构9以正确的方向打开抓取手。机械臂系统接收系统控制器的控制信号，驱动各机器人舵机转动，实现机械臂末端定位在空间任意位置。

旋翼飞行机械臂的地面控制装置由三部分组成，包括机械臂系统控制模块、四旋翼飞行器控制模块、图像信息显示模块，包括相机串口设置、目标的图像显示和像素坐标显示。首先确定系统运动模式，设置机械臂角度变化速度，旋翼飞行器在笛卡尔空间的上下左右运动步长和相机串口设置。实际飞行时，首先控制旋翼飞行器运动到抓取目标附近，再进一步利用图像信息，基于视觉伺服同时控制旋翼飞行器和机械臂运动自动定位在目标上方，完成目标抓取。

地面控制装置与系统控制器通过wifi进行实时通讯，系统控制器与飞行控制器通过USB串口进行实时通讯。飞行控制器通过融合自身惯性测量单元实时测量的飞行器姿态信息以及GPS测得的位置坐标信息可得到当前飞行系统准确的姿态角和位置坐标，并通过动态重心补偿算法实现飞行器的位置控制、姿态控制，从而克服由于机械臂运动对四旋翼飞行器造成的干扰，实现旋翼飞行机械臂系统的稳定飞行，其中飞行器的期望信号由系统控制器结合来自地面控制站、图像传感器的任务指令和目标图像信息得到，或者通过遥控器手动控制。同时，系统控制器通过处理图像传感器的目标位置信息利用视觉伺服控制方法输出机械臂和旋翼飞行器的期望信号控制机械臂各关节和旋翼飞行器的协调运动，从而实现目标的自主定位和抓取。

实施例

一种具有动态重心补偿算法的旋翼飞行机械臂系统的姿态控制框图如图3所示，包括以下步骤：

(1)数据读取，包括旋翼飞行器位置姿态信息和机械臂各关节角度；

(2)系统重心估算；

如图3，估算系统重心在机体坐标系{B}中的向量r_G＝[x_G，y_G,z_G]^T，由机械臂系统传送到系统控制器的各个关节角度θ_i，求得关节臂i的质心相对于关节臂i的坐标系J_i的局部坐标为分别表示关节臂i坐标系J_i相对于飞行平台坐标系{B}的旋转变换矩阵和位置坐标。可计算关节臂i的质心相对于机体坐标系{B}的位置坐标p_i，i＝0,…,4。

根据多体系统质心求解理论，旋翼飞行机械臂系统的重心r_G可表示为

其中是抓取目标在机体坐标系{B}的位置矢量，m_i为各个关节臂的质量，根据上述表达式可知求取r_G之前必须先求目标抓取物体的质量m_d。根据系统动力学模型可知，当系统悬停于某一点，并且飞行平台的俯仰和横滚角都为零的情况下，其4个螺旋桨的升力大小与系统所受重力相等且方向相反，根据受力分析可得利用最小二乘法进行求解得到m_d的估计值：

综上，在目标物体质量估计完成之后，重心只与机械臂的关节角θ_i有关,可表示为：

r_G＝g(θ₀,θ₁,θ₂,θ₃,θ₄)

(3)基于重心补偿设计旋翼飞行器的位置和姿态控制器；

根据GPS和惯性测量单元返回的数据计算机体坐标系{B}在世界坐标系{I}中位置矢量和方向向量分别为ξ＝[x,y,z]^T，取假设飞行平台目标位置点为ξ_d＝[x_d,y_d,z_d]^T，则飞行器的位置误差为ξ_e＝ξ_d-ξ，构造位置误差方程

因此结合系统动力学模型，可得具有重心补偿的目标姿态角度和总升力为

m为旋翼飞行机械臂系统总质量，a₁，a₂，a₃为计算的位置重心补偿项，其中cα,sα分别表示cosα,sinα的缩写。

如图3所示根据位置控制器的输出得到期望的姿态信息，结合系统动力学模型，并基于反步法定义角度误差z_i,其中i＝1,…,6，加入姿态控制重心补偿项，设计具有重心补偿项的姿态稳定控制器。

其中(I_xx,I_yy,I_zz)^T为系统惯性张量，c₁,c₂,c₃为重心补偿项，β_i(i＝1,2,...,6)为常数，输出分别表示旋翼飞行器做横滚运动，俯仰运动以及偏航运动所需控制量，然后经过解耦输出到各电机，驱动螺旋桨旋转，控制旋翼飞行机械臂系统稳定飞行。

以上所述仅为本发明的较佳实施例而已，并不用于限制本发明，凡在本发明的精神和原则范围之内所作的任何修改、等同替换以及改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于动态重心补偿的旋翼飞行机械臂系统，包括旋翼飞行平台、图像传感器、连接架、机械臂系统、系统控制器和地面站控制装置，其特征在于：所述旋翼飞行平台包括旋翼飞行器和飞行控制器；所述图像传感器安装在所述旋翼飞行平台的前下方；所述连接架是一块机械板，用于固定连接所述旋翼飞行平台、图像传感器和机械臂系统；所述机械臂系统安装在旋翼飞行平台的正下方；所述系统控制器安装在旋翼飞行平台的正上方；所述地面站控制装置与所述系统控制器之间采用无线方式进行通讯。

2.根据权利要求1所述的一种基于动态重心补偿的旋翼飞行机械臂系统，其特征在于：所述旋翼飞行器包括机架、机臂、动力设备及驱动装置、电源模块、定位模块、遥控与接收器；所述机架是飞行器平台的机身，也是其他组成系统模块的基本载体；所述机臂连接在机身上；所述动力设备及驱动装置包括无刷电机、无刷电调和螺旋桨，所述无刷电调与无刷电机相连，所述螺旋桨安装在无刷电机上；所述电源模块包括航模电池、稳压电路以及电池低压报警装置；所述定位模块用于确定旋翼飞行器相对地面的位置。

3.根据权利要求2所述的一种基于动态重心补偿的旋翼飞行机械臂系统，其特征在于：所述机臂为4个，相邻机臂相互垂直；所述动力设备及驱动装置为4组，每组对应一个机臂；所述机架为550轴距的碳纤维铝合金机架；所述飞行控制器采用内核32位处理器，包括能实时测量飞行器姿态信息的9轴惯性测量单元，对一切飞行控制信号集中处理，同时接收并处理系统控制器的信号。

4.根据权利要求1所述的一种基于动态重心补偿的旋翼飞行机械臂系统，其特征在于：所述图像传感器用于实时获取目标信息并传送至系统控制器。

5.根据权利要求1所述的一种基于动态重心补偿的旋翼飞行机械臂系统，其特征在于：所述机械臂系统是一个五自由度机械臂系统，包括五个机器人舵机和抓取手；所述机械臂系统的执行机构包括第一转向机构、第一收放机构、第二收放机构、第二转向机构和夹持机构；第一舵机分别与连接架和第一连杆相连组成第一转向结构，第二舵机和第三舵机分别与第一连杆、第二连杆和第三连杆串接组成第一收放机构和第二收放机构，第四舵机和第三连杆组成第二转向机构，第五舵机与抓取手组成夹持机构；夹持机构是直接与抓取目标接触的部件，收放机构带动夹持机构接触目标，转向机构通过自身旋转调整夹持机构的动作方向，使夹持机构以正确的方向打开抓取手。

6.根据权利要求1所述的一种基于动态重心补偿的旋翼飞行机械臂系统，其特征在于：所述地面站控制装置包括机械臂系统控制模块、旋翼飞行器控制模块、图像信息显示模块；所述机械臂系统控制模块用于实现机械臂的运动模式选择，包括仿真模式和实际模式、串口设置、机械臂角度变化速度调节、机械臂运动控制，所述机械臂运动控制包括在笛卡尔空间的线运动控制和关节角运动控制；旋翼飞行器控制模块包括飞行器在笛卡尔空间的上下左右运动步长调节设置和位置坐标和姿态角信息的显示；图像信息显示模块包括相机串口设置、目标的图像显示和像素坐标显示。

7.根据权利要求1所述的一种基于动态重心补偿的旋翼飞行机械臂系统，其特征在于：所述地面站控制装置、系统控制器与飞行控制器之间保持实时通讯；所述飞行控制器通过融合惯性测量单元和GPS等传感器测量数据获取实时位姿信息，并通过动态重心补偿算法实现飞行器的位置控制、姿态控制，实现旋翼飞行机械臂系统的稳定飞行；所述系统控制器通过视觉伺服控制方法来控制机械臂各关节的协调运动，实现目标的自主定位和抓取。

8.根据权利要求1至7任一项所述的一种动态重心补偿算法，其特征在于：包括以下步骤：

步骤一，数据读取；

步骤二，系统重心估算；

步骤三，基于重心补偿设计旋翼飞行器的位置和姿态控制器。

9.根据权利要求8所述的一种动态重心补偿算法，其特征在于：步骤二的具体过程如下：

(1)计算关节臂i的质心相对于机体坐标系{B}的位置坐标p_i：

式中：分别表示关节臂i坐标系J_i相对于飞行平台坐标系{B}的旋转变换矩阵和位置坐标，是关节臂i的质心相对于关节臂i坐标系J_i的局部坐标；

(2)估算目标抓取物体质量m_d，当旋翼飞行器悬停于某一点时，其俯仰角和翻滚角都为0的情况下，其升力u₁大小与系统所受总重力相等，根据受力分析有：

利用最小二乘法进行求解，可得m_d的估计值为：

(3)令机体坐标系为系统重心参考坐标系，根据多体系统质心求解理论，旋翼飞行机械臂系统的重心为：

10.根据权利要求9所述的一种动态重心补偿算法，其特征在于：步骤三的具体过程如下：

(1)定义机体坐标系{B}在世界坐标系{I}中位置矢量和方向向量分别为ξ＝[x,y,z]^T，具有重心补偿的目标姿态角度和总升力分别为：

其中：m为旋翼飞行机械臂系统总质量，a₁，a₂，a₃为位置重心补偿项；

(2)根据位置控制器的输出得到期望的姿态信息，结合系统动力学模型，加入姿态控制重心补偿项，设计姿态稳定控制器：

其中：(I_xx,I_yy,I_zz)^T为系统惯性张量，c₁,c₂,c₃为重心补偿项，β_i(i＝1,2,...,6)为常数，输出分别表示旋翼飞行器做横滚运动、俯仰运动以及偏航运动所需控制量；

(3)经过解耦输出到各电机，驱动螺旋桨旋转，控制旋翼飞行机械臂系统稳定飞行。