CN107804474B - 携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法 - Google Patents

Abstract

本发明专利公开了一种携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，包括步骤：1）将飞行机器人整机系统分解为飞行机器人实体下位机和PC电脑地面站；2）分别设计飞行机器人实体下位机以及PC电脑地面站；3）设计多旋翼飞行器；4）设计飞行器控制集成电路模块；5）设计系统微处理器电路程序；6）设计冗余度机械臂；7）设计冗余度机械臂控制集成电路模块；8）设计机械臂微处理器电路程序；9）整合完成携带冗余度机械臂的多旋翼飞行机器人整机系统设计。本发明飞行机器人具有六自由度空间移动飞行能力，多旋翼灵活结构实现方向控制的简便及操作简单，同时冗余度机械臂的多自由度的优点使其具有协调精准地执行复杂任务的功能。

Description

携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法

技术领域

本发明专利属于机载机械臂的飞行装置，特别是涉及一种携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法。

背景技术

随着时代的发展，具备垂直起降、稳定悬停、无线传输、远程航拍和自主巡航能力的多旋翼飞行器相比传统的地面移动工具具有活动范围广，灵活度高的优点，被广泛地应用在军事、民用、以及科学研究等多个领域，其中，在物体抓取和短程货物配送方面已成为研究的热点。智能机器人是一种自动化的机器，这种机器具备一些与人或生物相似的智能能力，如感知能力、规划能力、动作能力和协同能力。随着机器人科学和机械臂技术的进步，具备冗余度机械臂的机器人具有可协调执行机械臂复杂任务、躲避关节极限、躲避奇异点，并且具有一定的容错特性，被广泛应在在各行各业，如家用服务型机器人，可以帮助人们完成一些琐碎的工作；或者工业用机械手等，有效地提高了生产精度和生产速率。但是，对于一些要求精度高、危险系数大的高空作业，不仅需要机械臂的高精度工作，同时也需要拥有灵活多变的工作平台，传统的机器人往往受局限于活动范围，虽然已经有一部分可以在复杂多变的地形上工作的机器人诞生，但是可以在一定高度上稳定工作的机器人应用依然是比较欠缺的。

发明内容

本发明针对上述背景技术存在的缺陷提出了一种携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法。

本发明所采用的技术方案如下：

携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，包括如下步骤：

1)将携带冗余度机械臂的多旋翼飞行机器人整机系统分解为飞行机器人实体下位机以及PC电脑地面站；

2)分别设计步骤1)中所述飞行机器人实体下位机以及PC电脑地面站；其中飞行机器人实体下位机包括多旋翼飞行器系统、轻量型冗余度机械臂系统，通信组件以及云台摄像头部件；所述PC电脑地面站包括通信组件和上位机软件；

3)根据步骤2)所述的多旋翼飞行器系统设计需求，设计多旋翼飞行器，包括多旋翼飞行器机架、电机及其螺旋桨、防震装置以及飞行器控制系统组件；

4)根据步骤3)中所述的飞行器控制系统组件需求，设计飞行器控制集成电路模块，包括系统微处理器电路、传感器信号采集电路、通信收发电路、电机控制输出电路以及电源稳压电路；

5)根据步骤4)中所述系统微处理器电路设计需求，设计系统微处理器电路程序，包括：主程序框架、传感器信号获取线程、信息处理线程、通信收发线程、控制线程以及系统单片机嵌入式底层线程；

6)根据步骤2)所述的轻量型冗余度机械臂系统设计需求，设计冗余度机械臂，包括舵机、末端执行器、机械零固件以及冗余度机械臂控制系统组件；

7)根据步骤6)中的冗余度机械臂控制系统组件设计需求，设计冗余度机械臂控制集成电路模块，包括机械臂微处理器电路、机械臂传感器信号采集电路、机械臂通信收发电路、机械臂电源稳压电路以及舵机控制信号输出电路；

8)根据步骤7)中所述机械臂微处理器电路设计需求，设计机械臂微处理器电路程序，包括：机械臂控制主程序框架，机械臂传感器信号获取线程，机械臂信息处理线程、机械臂通信收发线程、机械臂控制线程以及机械臂单片机嵌入式底层线程；

9)根据上述步骤设计内容，整合完成携带冗余度机械臂的多旋翼飞行机器人整机系统设计。

进一步地，步骤2)中所述的飞行机器人实体下位机的通信组件挂载在多旋翼飞行器上，包括无线传输模块以及无线图传发送模块，其中无线传输模块与 PC电脑地面站通信组件中的无线传输模块配对连接，实现飞行机器人实体下位机中多旋翼飞行器和PC电脑地面站上位机软件之间的通信互联；下位机无线图传发送模块连接地面站无线图传接收模块，实现飞行机器人实体下位机云台摄像头部件和地面站上位机软件之间的视觉图像传输；

所述的PC电脑地面站的通信组件挂载在PC电脑上，包括无线传输模块以及无线图传接收模块，其中无线传输模块与飞行机器人实体下位机通信组件中的无线传输模块配对连接；地面站无线图传接收模块连接下位机无线图传发送模块；

所述的PC电脑地面站的上位机软件通过labview软件和MATLAB程序设计完成，具有指令接收、指令发送、数据显示监控、图像显示以及冗余度机械臂运动规划功能，完成与实体下位机信息交流和指令传输。

进一步地，所述的飞行机器人实体下位机的云台摄像头部件包括云台以及摄像头，云台用于完成飞行机器人飞行期间的摄像稳定；摄像头采用常见的RGB 摄像头实现图像采集，通过多目摄像头完成对任务物体的识别与定位；

进一步地，系统微处理器电路采用单片机作为核心，用于完成各模块之间的协调控制以及信号传输处理；传感器信号采集电路完成飞行机器人姿态、高度、位置以及外界环境等信息的获取；通信收发电路完成与飞行机器人下位机通信组件以及冗余度机械臂之间的通信互联；电机控制输出电路完成对电机执行器转速控制输出；电源稳压电路完成对电源的电压分配以及模块的稳压供电；电源稳压电路连接飞行器机载电源，通过转接板和稳压电路为各模块提供稳定电压；传感器信号采集电路中的姿态、位置、高度等传感器所采集得到实时传感数据，通过采集电路接口反馈回系统微处理器电路，系统微处理器电路根据内置飞行器系统程序完成信号的处理，并通过控制程序，解算出所需的电机转速，把控制信号输入到电机控制输出电路，实现对每个电机的控制，从而实现飞行控制；系统微处理器电路通过单片机嵌入式底层程序完成与各个模块间接口协议；通信收发电路挂载飞行机器人实体下位机通信组件以及连接冗余度机械臂通信收发电路。

进一步地，所述主程序框架根据时序要求以及设计目标调用不同线程实现对多旋翼飞行器部分的模块调用与系统操控；其中由于不同线程程序运行时间具有差异，主程序框架通过定时器合理制定调用规则，安排程序调用进程，筛选不必要运行程序；同时通过合理的时间间隔以及时序要求完成传感器信号读取以及程序计算，提高系统准确度以及效率；

传感器信号获取线程包括接口程序以及传感器信号转化程序；其中接口程序包括IIC，SPI，串口等模块接口协议程序以及ADC等模数转化接口程序，上述接口程序用于完成同各传感器间信息交流；传感器信号转化程序将不同数据结构的传感器信号数据转化为微处理器可以识别的整型或浮点型数据，实现数据规范化；

信息处理线程包括滤波算法程序以及信号融合程序，用于：由于传感器获取的信号具有噪声以及测量不准确等因素，需要对采集到的原始信号进行相应的滤波处理或信号融合；采集原始姿态传感器信号后需运行四元数算法以及卡尔曼滤波算法得出飞行器姿态信息、采集高度传感器信号后进行滤波，并融合不同高度传感器信号后得到高度信息、采集位置传感器信息后需根据融合算法完成位置信号获取；

通信收发线程包括接口配置程序，收发指令与数据程序及编码与解码程序；其中接口配置程序用于与飞行机器人实体下位机通信组件连接，根据具体搭载无线通信模块配适；同时该程序也负责同冗余度机械臂通信连接；收发指令与数据程序采用DMA结合上下位机之间的通信协议完成接收与发送；通信协议编码与解码程序将数据与指令转化为相应的通信协议编码或将通信协议编码解码为相应的数据与指令格式；

所述控制线程包括：动力分配程序、姿态控制程序、高度控制程序、位置控制程序；所述姿态控制程序采用姿态控制算法，主要将处理后的姿态信息输入到设计的姿态控制器，得到姿态控制输出信号；所述高度控制程序采用高度控制算法，主要将高度信息输入到设计的高度控制器，得到高度控制输出信号；所述位置控制程序采用位置控制算法，主要将位置传感器信号输入到设计的位置控制器得到位置控制输出信号；所述动力分配算法将所有控制程序的输出控制量进行融合，通过动力分配方案将控制输出量转换成每个电机所需的脉冲调制宽度信号，并通过相应的控制量信号输出端口传输到电机控制输出电路；

所述系统单片机嵌入式底层线程用于配置微处理器嵌入式底层资源，包括定时器、中断、硬件协议接口、IO口状态、微处理器时钟频率；

进一步地，所述机械臂微处理器电路为冗余度机械臂核心处理器部件，完成冗余度机械臂各模块之间的协调控制以及信号传输处理；机械臂传感器信号采集电路完成机械臂关节角度，关节电流以及外界环境信息的获取；机械臂通信收发电路完成与多旋翼飞行器间通信；舵机控制信号输出电路完成对关节舵机执行器的关节角控制；机械臂电源稳压电路完成对电源的电压分配以及模块的稳压供电；其中，机械臂电源稳压电路连接冗余度机械臂电源，通过转接板和稳压电路为各模块提供稳定电压；机械臂传感器信号采集电路中的关节角度，电流传感器采集得到实时传感数据，通过采集电路接口反馈回机械臂微处理器电路，机械臂微处理器电路根据内置冗余度机械臂控制系统程序完成信号的处理，并通过控制程序，解算出所需的舵机关节角，把控制信号输入到舵机控制信号输出电路，实现对每个舵机的控制，从而实现冗余度机械臂控制；机械臂微处理器电路通过单片机嵌入式底层程序完成与各个模块接口协议；机械臂通信收发电路用于连接冗余度机械臂以及多旋翼飞行器，用于完成两个子系统间的通信交流。

进一步地，所述机械臂微处理器电路采用单片机作为核心，机械臂控制主程序框架根据时序要求以及设计目标调用不同线程实现对冗余度机械臂部分的模块调用与系统操控；其中由于不同线程程序运行时间具有差异，机械臂控制主程序框架通过定时器合理制定调用规则，安排程序调用进程，筛选不必要运行程序；同时通过合理的时间间隔以及时序要求完成传感器信号读取以及程序计算，提高系统准确度以及效率；

机械臂传感器信号获取线程包括接口程序部分以及传感器信号转化程序；其中接口程序部分包括IIC，SPI，串口等模块接口协议程序以及ADC等模数转化接口程序，上述接口程序用于完成同各传感器接口的信息读取与交流；传感器信号转化程序为将不同数据结构的传感器信号数据转化为单片机可以识别的整型或浮点型数据，实现数据规范化；

所述机械臂信息处理线程包括滤波算法程序以及信号融合程序，用于：由于传感器获取的信号带有噪声以及测量不准确等相关因素，需要对采集到的原始信号进行相应的滤波处理或信号融合，采集电流传感器信号后需进行滤波读取准确的舵机电流值以完成电流反馈、采集关节角角度数据需进行融合处理计算末端执行器位置；

所述机械臂通信收发线程包括通信接口配置程序，接收与发送程序及通信协议编码与解码程序；其中通信接口配置程序完成机械臂同飞行器的通信交流；接收与发送程序采用DMA结合通信协议完成接收；通信协议编码与解码程序将数据与指令转化为相应的通信协议编码或将通信协议编码解码为相应的数据与指令格式；

所述机械臂控制线程包括：关节角转换程序、电流控制程序以及舵机控制程序；所述关节角转换程序将相应的关节角角度映射到舵机控制范围；所述电流控制程序完成对关节舵机的电流监控，当在大负载或碰撞等易造成舵机损毁的情况下，关闭机械臂舵机电源以防止系统出错；舵机控制程序负责连接舵机控制信号输出电路，实现舵机控制信号输出；

所述机械臂单片机嵌入式底层线程用于配置微处理器嵌入式底层资源，包括定时器，中断，硬件协议接口，IO口状态，微处理器时钟频率。

进一步地，步骤1)中的携带冗余度机械臂的多旋翼飞行机器人可根据实际任务以及所需实现功能，设计飞行控制算法和机械臂运动规划算法。

进一步地，根据实际任务以及所需实现功能，设计飞行控制算法和机械臂运动规划算法的步骤具体包括：

通过数学建模分析飞行器物理模型，根据建模后物理模型设计飞行机器人控制器；根据传感器信号以及目标任务所需要的目标值，通过控制器求解相应电机控制量，完成电机控制；将所需执行冗余度机械臂任务，将冗余度机械臂的运动任务转化为二次型，转化为运动规划求解问题，对相应的运动规划求解问题通过二次规划求解出相应的二次型最优解，得出各个机械臂关节舵机关节角度，控制机械臂完成目标任务。

进一步地，所述机械臂运动规划算法通过机械臂运动规划方案和二次规划算法实现；冗余度机械臂的运动规划方案通过冗余度机械臂逆运动学实现，其中逆运动学方程可以描述为：

f(θ)＝r

其中r是机械臂末端的期望轨迹，f(·)为冗余度机械臂关节角度到末端轨迹的非线性映射方程；对方程两边同时求导可以得到冗余度机械臂在速度层上的逆运动学方程

其中，J(θ)∈R^m×n为实数域上的m×n维矩阵，J(θ)为冗余度机械臂的雅克比矩阵，n表示机械臂的自由度数，m表示机械臂末端轨迹的空间维数，

和

分别为冗余度机械臂关节角度和末端轨迹关于时间的导数；根据不同的设计目的和指标要求，将上述的逆运动学问题转换为受约束的时变凸二次规划问题，具体公式为：

s.t.Ax＝b

Cx≤d

其中Ax＝b为相应完成任务所需的等式约束，Cx≤d为不等式约束，

与

为对应关节角的双端不等式约束；根据二次规划的算法，可以设计神经网络求解相应的二次型最优解；根据求解出的二次型最优解作为机械臂的关节角状态，并通过相应的传输协议传输到飞行机器人实体下位机，控制飞行机器人完成相应的控制任务。

与现有技术相比，本发明的有益之处是：携带冗余度机械臂的多旋翼飞行机器人的设计方法结合了飞行器具有一定冗余度容错性能以及灵活多变的飞行特点和机械臂躲避奇异点以及精度高的抓取特点，能够完成更为复杂多变的工作，应用范围更大，开发领域更广。

附图说明

图1为一种携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法设计流程图；

图2为本发明实施例携带冗余度机械臂的多旋翼飞行机器人整机系统结构图；

图3为本发明多旋翼飞行器控制系统组件设计框图；

图4为本发明多旋翼飞行器系统微处理器电路程序设计流程图；

图5为本发明冗余度机械臂控制系统组件设计框图；

图6为本发明机械臂微处理器电路程序设计流程图；

图7为本发明实施例飞行机器人硬件系统框架图；

图8为本发明实施例飞行机器人飞行控制系统设计框图；

图9为本发明实施例的轻量型冗余度机械臂示意图；

图10为本发明实施例的云台摄像头部件示意图。

图中所示为：1-电机及其螺旋桨；2-多旋翼飞行器机架；3-飞行器控制系统组件；4-防震装置；5-舵机；6-末端执行器；7-机械零固件；8-云台摄像头部件；9-云台；10-调节舵机；11-摄像头。

具体实施方式

下面结合附图和具体实施例对本发明专利作进一步详细地描述，实施例不能在此一一赘述，但发明的实施方式并不因此限定于以下实施例。

如图1所示为一种携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法设计流程图；设计过程包括如下步骤：

1)将携带冗余度机械臂的多旋翼飞行机器人整机系统分解为飞行机器人实体下位机以及PC电脑地面站；

2)分别设计步骤1)中所述飞行机器人实体下位机以及PC电脑地面站；其中飞行机器人实体下位机包括多旋翼飞行器系统、轻量型冗余度机械臂系统，通信组件以及云台摄像头部件；所述PC电脑地面站包括通信组件和上位机软件；

3)根据步骤2)所述的多旋翼飞行器系统设计需求，设计多旋翼飞行器，包括多旋翼飞行器机架、电机及其螺旋桨、防震装置以及飞行器控制系统组件；

4)根据步骤3)中所述的飞行器控制系统组件需求，设计飞行器控制集成电路模块，包括系统微处理器电路、传感器信号采集电路、通信收发电路、电机控制输出电路以及电源稳压电路；

5)根据步骤4)中所述系统微处理器电路设计需求，设计系统微处理器电路程序，包括：主程序框架、传感器信号获取线程、信息处理线程、通信收发线程、控制线程以及系统单片机嵌入式底层线程；

6)根据步骤2)所述的轻量型冗余度机械臂系统设计需求，设计冗余度机械臂，包括舵机、末端执行器、机械零固件以及冗余度机械臂控制系统组件；

7)根据步骤6)中的冗余度机械臂控制系统组件设计需求，设计冗余度机械臂控制集成电路模块，包括机械臂微处理器电路、机械臂传感器信号采集电路、机械臂通信收发电路、机械臂电源稳压电路以及舵机控制信号输出电路；

8)根据步骤7)中所述机械臂微处理器电路设计需求，设计机械臂微处理器电路程序，包括：机械臂控制主程序框架，机械臂传感器信号获取线程，机械臂信息处理线程、机械臂通信收发线程、机械臂控制线程以及机械臂单片机嵌入式底层线程；

9)根据上述步骤设计内容，整合完成携带冗余度机械臂的多旋翼飞行机器人整机系统设计。

该设计方法设计的飞行机器人系统总体分为两大大部分：PC电脑地面站与飞行机器人实体下位机。其中PC电脑地面站通过通信组件实现地面站与飞行机器人实体下位机的通信，利用计算机实现飞行机器人实体下位机的信息数据的实时显示和分析计算；利用遥控器实现在紧急必要时刻能手持控制飞行机器人多旋翼飞行器以及冗余度机械臂；利用无线图传接收器接收摄像头所拍摄的图像并进行分析处理，对飞行机器人进行更进一步的控制。

如图2所示为装载冗余度机械臂的多旋翼飞行机器人模型。该模型主要由多旋翼飞行器系统、轻量型冗余度机械臂系统，通信组件以及云台摄像头部件组成。其中多旋翼飞行器由电机及其螺旋桨1，多旋翼飞行器机架2，飞行器控制系统组件3以及防震装置4组成，飞行器控制系统组件3包括系统微处理器电路、传感器信号采集电路、通信收发电路、电机控制输出电路以及电源稳压电路等模块组成；冗余度机械臂部分由舵机5，末端执行器6以及相应的机械零固件7以及相应的组成冗余度机械臂控制系统组件组成；通信组件挂载在飞行器控制系统组件3相应的通讯接口上；云台摄像头部件8包括云台装置和摄像头组成。

所述的飞行机器人实体下位机的通信组件挂载在多旋翼飞行器上，包括无线传输模块以及无线图传发送模块，其中无线传输模块与PC电脑地面站通信组件中的无线传输模块配对连接，实现飞行机器人实体下位机中多旋翼飞行器和 PC电脑地面站上位机软件之间的通信互联；下位机无线图传发送模块连接地面站无线图传接收模块，实现飞行机器人实体下位机云台摄像头部件和地面站上位机软件之间的视觉图像传输；

所述的PC电脑地面站的通信组件挂载在PC电脑上，包括无线传输模块以及无线图传接收模块，其中无线传输模块与飞行机器人实体下位机通信组件中的无线传输模块配对连接；地面站无线图传接收模块连接下位机无线图传发送模块；

所述的PC电脑地面站的上位机软件通过labview软件和MATLAB程序设计完成，具有指令接收、指令发送、数据显示监控、图像显示以及冗余度机械臂运动规划功能，完成与实体下位机信息交流和指令传输。

所述的飞行机器人实体下位机的云台摄像头部件包括云台以及摄像头，云台用于完成飞行机器人飞行期间的摄像稳定；摄像头采用常见的RGB摄像头实现图像采集，通过多目摄像头完成对任务物体的识别与定位。

如图3所示为本发明多旋翼飞行器控制系统组件设计框图；飞行器控制系统组件3包括系统微处理器电路、传感器信号采集电路、通信收发电路、电机控制输出电路以及电源稳压电路等模块组成；在本发明实施例中，系统微处理器电路采用STM32F4系列微处理器作为核心电路；传感器信号采集电路包括 MPU6050姿态传感器，MS5803高度传感器，GPS定位传感器以及Px4Flow光流传感器等传感器和相应的传感器采集接口构成；信号收发电路采用无线串口传输模块实现无线通信；电机控制输出电路为PWM信号输出电路以及电子调速器组成的电子调速控制电路构成；电源稳压电路由电源分电板以及各路降压稳压模块完成设计，包括5V稳压电路部分和3.3V稳压电路部分。

系统微处理器电路采用单片机作为核心，用于完成各模块之间的协调控制以及信号传输处理；传感器信号采集电路完成飞行机器人姿态、高度、位置以及外界环境等信息的获取；通信收发电路完成与飞行机器人下位机通信组件以及冗余度机械臂之间的通信互联；电机控制输出电路完成对电机执行器转速控制输出；电源稳压电路完成对电源的电压分配以及模块的稳压供电；电源稳压电路连接飞行器机载电源，通过转接板和稳压电路为各模块提供稳定电压；传感器信号采集电路中的姿态、位置、高度等传感器所采集得到实时传感数据，通过采集电路接口反馈回系统微处理器电路，系统微处理器电路根据内置飞行器系统程序完成信号的处理，并通过控制程序，解算出所需的电机转速，把控制信号输入到电机控制输出电路，实现对每个电机的控制，从而实现飞行控制；系统微处理器电路通过单片机嵌入式底层程序完成与各个模块间接口协议；通信收发电路挂载飞行机器人实体下位机通信组件以及连接冗余度机械臂通信收发电路。

如图4所示为本发明多旋翼飞行器系统微处理器电路程序设计流程图；机械臂微处理器电路程序包括：机械臂控制主程序框架，机械臂传感器信号获取线程，机械臂信息处理线程、机械臂通信收发线程、机械臂控制线程以及单片机嵌入式底层线程等；其中主程序框架在本轮进程中判断是否执行相应线程，若不执行则跳过相应程序，直至完成所有线程的判断与执行；若在本轮进程执行后未符合开启下一轮进程的要求，则等待直至满足要求。

如图5所示为本发明冗余度机械臂控制系统组件设计框图；冗余度机械臂控制集成电路模块包括机械臂微处理器电路、机械臂传感器信号采集电路、机械臂通信收发电路、机械臂电源稳压电路以及舵机控制信号输出电路；在本发明实施例中，机械臂微处理器电路采用STM32F4系列微处理器作为核心电路；机械臂传感器信号采集电路包括角度传感器，编码器以及电流传感器和相应的传感器采集接口构成；机械臂通信收发电路采用串行通信接口；舵机控制信号输出电路为PWM信号输出电路以及舵机关节角控制电路构成；电源稳压电路由电源分电板以及各路降压稳压模块完成设计，包括5V稳压电路部分和3.3V稳压电路部分。

如图6所示为本发明机械臂微处理器电路程序设计流程图；机械臂微处理器电路程序包括：机械臂控制主程序框架，机械臂传感器信号获取线程，机械臂信息处理线程、机械臂通信收发线程、机械臂控制线程以及单片机嵌入式底层线程等；其中主程序框架在本轮进程中判断是否执行相应线程，若不执行则跳过相应程序，直至完成所有线程的判断与执行；若在本轮进程执行后未符合开启下一轮进程的要求，则等待直至满足要求。

如图7所示为本发明实施例飞行机器人硬件系统框架图；该硬件系统包括STM32F4系列微处理器，MPU6050姿态传感器，MS5803高度传感器，GPS定位传感器，Px4Flow光流传感器无线传输模块等相关部件。本发明实施例中机载的MPU6050姿态传感器能够以I2C方式输出经过卡尔曼滤波后的三轴加速度和三轴角速度的数据，经过四元数算法进行姿态融合获取θ,φ,

姿态角数据； Px4Flow光流传感器和GPS模块获取飞行机器人位置信息，前者通常适用于室内，利用光流测速再积分定位，后者通常用于室外，利用卫星时延定位，获取位置数据x，y；MS5803气压计和超声波模块采集高度传感器信号后进行 Alpha-beta滤波后获得机载部分的h高度信息；通过上述步骤而得到的实时姿态位置高度数据反馈回飞行机器人飞行控制系统。

如图8所示为本发明实施例飞行机器人飞行控制系统设计框图；将实时姿态位置高度数据反馈回飞行机器人飞行控制系统；通过PID负反馈闭环回路设计相应的姿态位置高度控制器；通过姿态位置高度控制器向每个电子调速器输出相应的脉宽调制信号，再通过电子调速器实现对每个电机的速度控制，实现飞行机器人姿态、位置和高度控制。

GPS和光流传感器的数据融合得到的实时p_x，p_y与给定的

通过位置速度控制算法得出飞行器V_x,V_y，再通过速度姿态控制算法得到所需的姿态角度θ^*,φ^*，然后通过计算得出的θ^*,φ^*和给定的

与通过MPU6050姿态传感器的数据融合得到实时的θ,φ,

计算达到给定的姿态角度和进行飞行机器人大地坐标系和机体坐标系之间的坐标变换，由大地坐标系转换为机体坐标系得到所需的u₂,u₃,u₄，另一方面通过给定的

与超声波和气压计的数据融合得到实时的高度信息p_z计算达到给定高度所需的

然后通过给定的

与超声波和气压计的数据融合得到实时的V_z计算达到给定上升速度所需要的u₁，最后通过给定的u₁,u₂,u₃,u₄通过转速分配转化成每个电机的所需要的转速Ω₁,Ω₂,Ω₃,Ω₄,Ω₅,Ω₆，根据转速与电压的关系，给每个电机输出相应的脉宽调制信号使每个电机达到目标转速，使得电机产生旋转扭力和平移拉力，从而实现飞行机器人的的姿态位置高度控制。

上述坐标变换所使用的坐标变换矩阵可由此推出:假定飞行机器人是一个刚体，6个旋翼的转动平面均与机体的水平平面相平行，而且飞行器的6个臂互相成60度安装，且机械臂与其他模块相当于一个重物理想安装在机体的中心轴处,地面坐标系转换到机体坐标系的变换矩阵为：

则地面坐标系与机体坐标系对应关系为：

X_body＝SX_earth或X_earth＝S^TX_body

同样，通过数学建模分析可知各电机上升力F₁，F₂，F₃，F₄，F₅，F₆与姿态、高度位置控制器输出值u₁，u₂，u₃，u₄的变换矩阵是：

由于各电机上升力F₁，F₂，F₃，F₄，F₅，F₆由每个电机的所需要的转速Ω₁,Ω₂,Ω₃,Ω₄,Ω₅,Ω₆决定，因此可以根据上述变换矩阵完成对每个电机的动力分配。

如图9为为本发明实施例的轻量型冗余度机械臂示意图；该实施例模型为7 自由度冗余机械臂模型，由舵机5，末端执行器6以及相应的机械结构部件7组成。该冗余度机械臂总共有7个舵机，其中六个舵机实现机械臂的旋转和伸缩功能从而实现对机械臂6个自由度上的控制，而最后一个舵机是末端执行器的一部分，负责完成末端执行器的夹取与松开任务。通过设计相应的机械臂逆运动学设计算法以及二次规划算法，由相关的传感器反馈机械臂每个关节的旋转角度以及姿态位置信息，设计出相应的机械臂控制器；根据不同任务由PC电脑上位机解算出机械臂每个关节所需旋转的角度，并将相应的关节角度发送到飞行机器人实体下位机，控制每个舵机所需要的输出控制量，从而输出相应的脉冲调制信号，实现舵机打角至目标角度，实现机械臂的运动控制和不同任务对机械臂的运动状态的要求。

如图10为本发明实施例的云台摄像头部件示意图；其中云台摄像头部件部分包括：云台9，调节舵机10以及摄像头11；所述的云台摄像头部件8包括设置在机架2底部前端的云台9、固定在所述云台9上的摄像头11，所述的云台9包括三个正交设置的调节电机10、依次连接于各调节电机10之间的支架，所述调节电机10与所述飞行器控制系统组件3中的电路进行连接，所述摄像头11固定在位于云台9末端的支架上，使摄像头11具有空间内三个旋转自由度。利用云台可以实现摄像头的自稳定，再通过通讯组件中的无线图传发送模块实现飞行机器人实体下位机上的图像稳定实时获取，并通过PC电脑地面站对目标任务所需识别的物体图像进行图像特征获取，从而确定目标物体的位置，并由无线传输模块把目标物体的位置发回给飞行机器人，再由飞行机器人上的控制器解算电机的控制输出量和机械臂的控制输出量，实现对目标物体的识别与定位，有利于飞行机器人完成更多复杂任务。

根据上述图文描述以及设计流程，针对本发明专利的整体设计思路进行描述。首先搭建飞行机器人实体下位机以及建立用于实时通讯的PC电脑地面站，飞行机器人的冗余度机械臂在静止无工作指令状态下自然垂直放置。PC电脑地面站发送指令给飞行机器人后，飞行机器人通过姿态高度位置传感器获得当前的飞行状态，并在飞行系统的算法控制下调整各电机转速从而平稳且快速抵达目的地，在这个过程中，飞行机器人实时返回飞行器机器人飞行状态、任务执行信息、目标物体相应位置等信息，而且，利用云台摄像头部件实现摄像头自稳定，再由通讯组件把摄像头拍摄的图片、视频信息实时发送给PC电脑地面站进行处理，地面站通过对目标物体特征识别，从而确定目标物体的位置,并通过通讯组件回传飞行机器人。根据定位信息以及机械臂的控制需求，微控制器控制电机以及舵机，实现对冗余度机械臂的运动控制并进行准确抓取，完成指定动作。

携带冗余度机械臂的多旋翼飞行机器人可根据实际任务以及所需实现功能，设计飞行控制算法和机械臂运动规划算法。

进一步地，根据实际任务以及所需实现功能，设计飞行控制算法和机械臂运动规划算法的步骤具体包括：

通过数学建模分析飞行器物理模型，根据建模后物理模型设计飞行机器人控制器；根据传感器信号以及目标任务所需要的目标值，通过控制器求解相应电机控制量，完成电机控制；将所需执行冗余度机械臂任务，将冗余度机械臂的运动任务转化为二次型，转化为运动规划求解问题，对相应的运动规划求解问题通过二次规划求解出相应的二次型最优解，得出各个机械臂关节舵机关节角度，控制机械臂完成目标任务。

进一步地，所述机械臂运动规划算法通过机械臂运动规划方案和二次规划算法实现；冗余度机械臂的运动规划方案通过冗余度机械臂逆运动学实现，其中逆运动学方程可以描述为：

f(θ)＝r

其中r是机械臂末端的期望轨迹，f(·)为冗余度机械臂关节角度到末端轨迹的非线性映射方程；对方程两边同时求导可以得到冗余度机械臂在速度层上的逆运动学方程

其中，

为实数域上的m×n维矩阵，J(θ)为冗余度机械臂的雅克比矩阵，n表示机械臂的自由度数，m表示机械臂末端轨迹的空间维数，

和

分别为冗余度机械臂关节角度和末端轨迹关于时间的导数；根据不同的设计目的和指标要求，将上述的逆运动学问题转换为受约束的时变凸二次规划问题，具体公式为：

s.t.Ax＝b

Cx≤d

其中Ax＝b为相应完成任务所需的等式约束，Cx≤d为不等式约束，

与

为对应关节角的双端不等式约束；根据二次规划的算法，可以设计神经网络求解相应的二次型最优解；根据求解出的二次型最优解作为机械臂的关节角状态，并通过相应的传输协议传输到飞行机器人实体下位机，控制飞行机器人完成相应的控制任务。

本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (9)

1.携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，其特征在于，包括如下步骤：

1)将携带冗余度机械臂的多旋翼飞行机器人整机系统分解为飞行机器人实体下位机以及PC电脑地面站；

2)分别设计步骤1)中所述飞行机器人实体下位机以及PC电脑地面站；其中飞行机器人实体下位机包括多旋翼飞行器系统、轻量型冗余度机械臂系统，通信组件以及云台摄像头部件；所述PC电脑地面站包括通信组件和上位机软件；

3)根据步骤2)所述的多旋翼飞行器系统设计需求，设计多旋翼飞行器，包括多旋翼飞行器机架、电机及其螺旋桨、防震装置以及飞行器控制系统组件；

4)根据步骤3)中所述的飞行器控制系统组件需求，设计飞行器控制集成电路模块，包括系统微处理器电路、传感器信号采集电路、通信收发电路、电机控制输出电路以及电源稳压电路；

5)根据步骤4)中所述系统微处理器电路设计需求，设计系统微处理器电路程序，包括：主程序框架、传感器信号获取线程、信息处理线程、通信收发线程、控制线程以及系统单片机嵌入式底层线程；

6)根据步骤2)所述的轻量型冗余度机械臂系统设计需求，设计冗余度机械臂，包括舵机、末端执行器、机械零固件以及冗余度机械臂控制系统组件；

7)根据步骤6)中的冗余度机械臂控制系统组件设计需求，设计冗余度机械臂控制集成电路模块，包括机械臂微处理器电路、机械臂传感器信号采集电路、机械臂通信收发电路、机械臂电源稳压电路以及舵机控制信号输出电路；

8)根据步骤7)中所述机械臂微处理器电路设计需求，设计机械臂微处理器电路程序，包括：机械臂控制主程序框架，机械臂传感器信号获取线程，机械臂信息处理线程、机械臂通信收发线程、机械臂控制线程以及机械臂单片机嵌入式底层线程；

9)根据上述步骤设计内容，整合完成携带冗余度机械臂的多旋翼飞行机器人整机系统设计。

2.根据权利要求1所述的携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，其特征在于：

步骤2)中所述的飞行机器人实体下位机的通信组件挂载在多旋翼飞行器上，包括无线传输模块以及无线图传发送模块，其中无线传输模块与PC电脑地面站通信组件中的无线传输模块配对连接，实现飞行机器人实体下位机中多旋翼飞行器和PC电脑地面站上位机软件之间的通信互联；下位机无线图传发送模块连接地面站无线图传接收模块，实现飞行机器人实体下位机云台摄像头部件和地面站上位机软件之间的视觉图像传输；

所述的PC电脑地面站的通信组件挂载在PC电脑上，包括无线传输模块以及无线图传接收模块，其中无线传输模块与飞行机器人实体下位机通信组件中的无线传输模块配对连接；地面站无线图传接收模块连接下位机无线图传发送模块；

所述的PC电脑地面站的上位机软件通过labview软件和MATLAB程序设计完成，具有指令接收、指令发送、数据显示监控、图像显示以及冗余度机械臂运动规划功能，完成与实体下位机信息交流和指令传输。

3.根据权利要求1所述的携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，其特征在于：所述的飞行机器人实体下位机的云台摄像头部件包括云台以及摄像头，云台用于完成飞行机器人飞行期间的摄像稳定；摄像头采用常见的RGB摄像头实现图像采集，通过多目摄像头完成对任务物体的识别与定位。

4.根据权利要求1所述的携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，其特征在于：

系统微处理器电路采用单片机作为核心，用于完成各模块之间的协调控制以及信号传输处理；传感器信号采集电路完成飞行机器人姿态、高度、位置以及外界环境信息的获取；通信收发电路完成与飞行机器人下位机通信组件以及冗余度机械臂之间的通信互联；电机控制输出电路完成对电机执行器转速控制输出；电源稳压电路完成对电源的电压分配以及模块的稳压供电；电源稳压电路连接飞行器机载电源，通过转接板和稳压电路为各模块提供稳定电压；传感器信号采集电路中的姿态、位置、高度传感器所采集得到实时传感数据，通过采集电路接口反馈回系统微处理器电路，系统微处理器电路根据内置飞行器系统程序完成信号的处理，并通过控制程序，解算出所需的电机转速，把控制信号输入到电机控制输出电路，实现对每个电机的控制，从而实现飞行控制；系统微处理器电路通过单片机嵌入式底层程序完成与各个模块间接口协议；通信收发电路挂载飞行机器人实体下位机通信组件以及连接冗余度机械臂通信收发电路。

5.根据权利要求1所述的携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，其特征在于：所述主程序框架根据时序要求以及设计目标调用不同线程实现对多旋翼飞行器部分的模块调用与系统操控；其中由于不同线程程序运行时间具有差异，主程序框架通过定时器合理制定调用规则，安排程序调用进程，筛选不必要运行程序；同时通过合理的时间间隔以及时序要求完成传感器信号读取以及程序计算，提高系统准确度以及效率；

传感器信号获取线程包括接口程序以及传感器信号转化程序；其中接口程序包括IIC，SPI，串口模块接口协议程序以及ADC模数转化接口程序，上述接口程序用于完成同各传感器间信息交流；传感器信号转化程序将不同数据结构的传感器信号数据转化为微处理器能识别的整型或浮点型数据，实现数据规范化；

信息处理线程包括滤波算法程序以及信号融合程序，用于：由于传感器获取的信号具有噪声以及测量不准确的因素，需要对采集到的原始信号进行相应的滤波处理或信号融合；采集原始姿态传感器信号后需运行四元数算法以及卡尔曼滤波算法得出飞行器姿态信息、采集高度传感器信号后进行滤波，并融合不同高度传感器信号后得到高度信息、采集位置传感器信息后需根据融合算法完成位置信号获取；

通信收发线程包括接口配置程序，收发指令与数据程序及编码与解码程序；其中接口配置程序用于与飞行机器人实体下位机通信组件连接，根据具体搭载无线通信模块配适；同时该程序也负责同冗余度机械臂通信连接；收发指令与数据程序采用DMA结合上下位机之间的通信协议完成接收与发送；通信协议编码与解码程序将数据与指令转化为相应的通信协议编码或将通信协议编码解码为相应的数据与指令格式；

所述控制线程包括：动力分配程序、姿态控制程序、高度控制程序、位置控制程序；所述姿态控制程序采用姿态控制算法，主要将处理后的姿态信息输入到设计的姿态控制器，得到姿态控制输出信号；所述高度控制程序采用高度控制算法，主要将高度信息输入到设计的高度控制器，得到高度控制输出信号；所述位置控制程序采用位置控制算法，主要将位置传感器信号输入到设计的位置控制器得到位置控制输出信号；所述动力分配程序将所有控制程序的输出控制量进行融合，通过动力分配方案将控制输出量转换成每个电机所需的脉冲调制宽度信号，并通过相应的控制量信号输出端口传输到电机控制输出电路；

所述系统单片机嵌入式底层线程用于配置微处理器嵌入式底层资源，包括定时器、中断、硬件协议接口、IO口状态、微处理器时钟频率。

6.根据权利要求1所述的携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，其特征在于：所述机械臂微处理器电路为冗余度机械臂核心处理器部件，完成冗余度机械臂各模块之间的协调控制以及信号传输处理；机械臂传感器信号采集电路完成机械臂关节角度，关节电流以及外界环境信息的获取；机械臂通信收发电路完成与多旋翼飞行器间通信；舵机控制信号输出电路完成对关节舵机执行器的关节角控制；机械臂电源稳压电路完成对电源的电压分配以及模块的稳压供电；其中，机械臂电源稳压电路连接冗余度机械臂电源，通过转接板和稳压电路为各模块提供稳定电压；机械臂传感器信号采集电路中的关节角度，电流传感器采集得到实时传感数据，通过采集电路接口反馈回机械臂微处理器电路，机械臂微处理器电路根据内置冗余度机械臂控制系统程序完成信号的处理，并通过控制程序，解算出所需的舵机关节角，把控制信号输入到舵机控制信号输出电路，实现对每个舵机的控制，从而实现冗余度机械臂控制；机械臂微处理器电路通过单片机嵌入式底层程序完成与各个模块接口协议；机械臂通信收发电路用于连接冗余度机械臂以及多旋翼飞行器，用于完成两个子系统间的通信交流。

7.根据权利要求1所述的携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，其特征在于：所述机械臂微处理器电路采用单片机作为核心，机械臂控制主程序框架根据时序要求以及设计目标调用不同线程实现对冗余度机械臂部分的模块调用与系统操控；其中由于不同线程程序运行时间具有差异，机械臂控制主程序框架通过定时器合理制定调用规则，安排程序调用进程；同时通过合理的时间间隔以及时序要求完成传感器信号读取以及程序计算，提高系统准确度以及效率；

机械臂传感器信号获取线程包括接口程序部分以及传感器信号转化程序；其中接口程序部分包括IIC，SPI，串口模块接口协议程序以及ADC模数转化接口程序，上述接口程序用于完成同各传感器接口的信息读取与交流；传感器信号转化程序为将不同数据结构的传感器信号数据转化为单片机能识别的整型或浮点型数据，实现数据规范化；

所述机械臂信息处理线程包括滤波算法程序以及信号融合程序，用于：由于传感器获取的信号带有噪声以及测量不准确的相关因素，需要对采集到的原始信号进行相应的滤波处理或信号融合，采集电流传感器信号后需进行滤波读取准确的舵机电流值以完成电流反馈、采集关节角角度数据需进行融合处理计算末端执行器位置；

所述机械臂通信收发线程包括通信接口配置程序，接收与发送程序及通信协议编码与解码程序；其中通信接口配置程序完成机械臂同飞行器的通信交流；接收与发送程序采用DMA结合通信协议完成接收；通信协议编码与解码程序将数据与指令转化为相应的通信协议编码或将通信协议编码解码为相应的数据与指令格式；

所述机械臂控制线程包括：关节角转换程序、电流控制程序以及舵机控制程序；所述关节角转换程序将相应的关节角角度映射到舵机控制范围；所述电流控制程序完成对关节舵机的电流监控，当在大负载或碰撞易造成舵机损毁的情况下，关闭机械臂舵机电源以防止系统出错；舵机控制程序负责连接舵机控制信号输出电路，实现舵机控制信号输出；

所述机械臂单片机嵌入式底层线程用于配置微处理器嵌入式底层资源，包括定时器，中断，硬件协议接口，IO口状态，微处理器时钟频率。

8.根据权利要求1所述的携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，其特征在于：步骤1)中的携带冗余度机械臂的多旋翼飞行机器人根据实际任务以及所需实现功能，设计飞行控制算法和机械臂运动规划算法。

9.根据权利要求8所述的携带冗余度机械臂的多旋翼飞行机器人整机系统设计方法，其特征在于：根据实际任务以及所需实现功能，设计飞行控制算法和机械臂运动规划算法的步骤具体包括：

通过数学建模分析飞行器物理模型，根据建模后物理模型设计飞行机器人控制器；根据传感器信号以及目标任务所需要的目标值，通过控制器求解相应电机控制量，完成电机控制；将所需执行冗余度机械臂任务，将冗余度机械臂的运动任务转化为二次型，转化为运动规划求解问题，对相应的运动规划求解问题通过二次规划求解出相应的二次型最优解，得出各个机械臂关节舵机关节角度，控制机械臂完成目标任务。

Images