typora-root-url: img

6_树莓派机载计算机通过串口指令控制无人机自主飞行教程

电赛飞行器赛题的前世今生

飞行器赛题至出现以来。从大体趋势上来看参赛学生的主流飞控路线主要经历了以下四个发展阶段：

• APM/Pixhawk开源飞控作为飞控板直接控制无人机飞行，赞助商MCU作为导航板处理部分视觉数据、测距数据后，单片机模拟出遥控信号给控制板间接控制无人机

• 赞助商MCU作为唯一的飞控板，赞助商的MCU需要处理姿态检测、飞行控制等核心部分，视觉处理部分可以采用成品模块

• 鼓励使用赞助商MCU作为飞控板，但禁止机载计算机作为处理器设备使用，如树莓派、英伟达板卡等（实际在2020年省赛中就有少量队伍使用机载计算机参赛被默许）

• 使用赞助商MCU作为飞控板已基本成为共识，对机载计算机在无人机赛题中使用限制基本完全放开，进而引入了激光雷达SLAM定位、T265追踪相机、OPENCV机器视觉、ROS机器人操作系统等

  竞赛研讨会议上队对树莓派相关使用限制的讨论

|

年份	飞控、视觉、机载计算机等处理器要求	比赛内容
2013年——四旋翼自主飞行器
飞行控制板的MCU必须为赞助商产品，控制板：瑞萨MCU参与无人机控制就行	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-DjO3lKBJ-1651065268774)(/image-20220426171226814.png)]飞行任务，一键定点起飞、定点降落、投掷物体后返航降落
2014年——四旋翼飞行器	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-KyorDiNu-1651065268775)(/image-20220426172353612.png)]飞行器的姿态检测、飞行控制部分必须为赞助商产品，飞控两大核心板块姿态解算与控制必须有TI芯片处理	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-weTMdMqt-1651065268776)(/image-20220426172305134.png)]飞行任务：起飞、定高、定点降落
2015年——多旋翼自主飞行器	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-FtgjbxN8-1651065268776)(/image-20220426173237909.png)]飞控板任意选择，瑞萨MCU参与无人机数据处理、导航控制	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-wwwsqlZh-1651065268776)(/image-20220426173619581.png)]飞行任务：定高、定点降落、投掷，自主循迹飞行
2016年——四旋翼自主投靶飞行器	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0xdAoe7V-1651065268777)(/image-20220426173956089.png)]飞控板唯一，且主控MCU必须为赞助商TI公司的MCU，以往比赛中使用其它厂家MCU的开源飞控+赞助方的MCU协处理方案直接出局	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0aGhx4GP-1651065268777)(/image-20220426173901471.png)]飞行任务：定高、定点起飞、精准投靶
2017年——四旋翼自主飞行器探测跟踪系统	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-rRiBQzSB-1651065268778)(/image-20220426174953982.png)]和2015年国赛要求一致，飞控板、视觉处理任意选择，瑞萨MCU参与无人机数据处理、导航控制	飞控板、视觉处理任意选择[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-NLx6QlTx-1651065268778)(/image-20220426174809003.png)]飞行任务：飞行中识别底部小车、自动追踪小车
2018年——灭火飞行器	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-oVdl0Vdz-1651065268779)(/image-20220426175619668.png)]进一步明确必须使用赞助商TI公司处理器作为飞行器姿态检测和飞行控制处理器，也是从这一年开始，TI公司作为2018年至2027全国大学生电子设计竞赛的唯一冠名商与赞助商	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-q8bmkUrA-1651065268779)(/image-20220426175241567.png)]飞行任务：定高定点巡航，视觉识别底部光源，飞行过程中通过矩形框，也是从本届开始，无人机自主飞行需要处理底部和空中两个方向的特征信息，以后的比赛都沿用此套路：底部+飞行航线上的视觉特征识别
2019年——巡线无人机	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-QeUsqPP4-1651065268780)(/image-20220426183003931.png)]进一步规范无人机现场启动方式，不得使用无线通信及遥控功能，无人机的机架、电调、电机、螺旋桨、电池动力组件以及视觉处理的OPENMV、K210等全部可以购买现成的模块，其它功能的实现不得采用集成商提供的组件本意是想限定成品集成了飞控板的组件，希望学生自己设计飞控，而实际比赛中并未作任何限制[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ZTkNoJTt-1651065268780)(/image-20220426235712244.png)]	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-uRsJV6uX-1651065268781)(/image-20220426180326898.png)]巡线、识别条形码、二维码、挂负载等
2020年——绕障飞行器	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-dpXsXQR0-1651065268781)(/image-20220426181350015.png)]进一步规范无人机现场启动方式，不得使用无线通信及遥控功能，无人机的机架、电调、电机、螺旋桨、电池动力组件以及视觉处理的OPENMV、K210等全部可以购买现成的模块，其它功能的实现不得采用集成商提供的组件本意是想限定成品集成了飞控板的组件，希望学生自己设计飞控，而实际比赛中并未作任何限制，虽然第6项进行了加粗提示	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2cnnHssE-1651065268781)(/image-20220426181459786.png)]标记点起飞、降落，飞行中绕障碍
2021年——植保飞行器	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-GOawmryg-1651065268782)(/image-20220426212852080.png)]1、删去了前两年要求中必须使用按键控制起飞的方式，去掉了无线通讯及遥控等限制，比赛中使用遥控器操作无人机启动重新被允许。2、对无人机动力配件、视觉模块、机载计算机、飞控板处理器类型等不再有任何限制，会在当前的备赛阶段通知公告，至少有一道赛题将使用竞赛赞助商TI处理器[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-rjVTzorD-1651065268782)(/image-20220426214557551.png)]至于处理器限定是否会出现在接下来的飞行器赛题中，题目未出来前不得而知，组委会这样公布肯定是鼓励大家用TI处理器备赛，在当下TI飞控方案日臻成熟的情况下，没有任何理由不选TI处理器作为飞控去备赛，因为飞行器赛题一旦出现限定必须使用TI处理器，抱着侥幸心态的学生最后只能叹是真的勇，毕竟在2019年前TI作为赞助商的电赛中，无人机赛题处理器无一例外的都做了限定且限制得比瑞萨更加明确。	[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-CV3oWRTa-1651065268782)(/image-20220426212727152.png)]定点起飞降落、航点遍历、识别颜色、绕杆避障、识别条码

上述方案的前三项基本都是基于单片机层面编程，以TI飞控为例，用户可以通过基本飞行支持保障函数与导航控制函数的配合使用去完成某一任务的航点飞行，前面讲的飞行任务实现是通过在飞控程序Developer_Mode.c、Subtask_Demo.c中编写相关任务函数，直接在单片机端编程实现的，还未了解的同学参照下方链接。

5_竞赛无人机搭积木式编程 ——以2021年电赛国奖标准完整复现为例学习 https://www.bilibili.com/read/cv15844252?spm_id_from=333.999.0.0

单片机端编程还包括电赛早期在部分限定处理器的年份，市面上缺少像当下TI飞控这样提供用户SDK任务编程、二次开发高效且完全开源飞控可供学生选择。学生自己独立去开发一套飞控费力耗时稳定性还得不到保障，最后迫不得已只能使用APM或者Pixhawk等国外开源飞控。这类飞控由于代码量大、封装层多、开发环境对新手不友好，最关键的一点是几乎没有任何售后技术支持，使得普通本/专科生一开始直接上手就去改飞控代码，实现具体飞行任务变得几乎不可能。

在这种情况下，学生为了完成飞行器赛题任务又需要满足赛题处理器限定，多采用组委会限定单片机（瑞萨/德州仪器）+开源飞控Pixhawk/APM的方式去比赛。学生不需要去管Pixhawk/APM飞控端代码，只需要在组委会限定单片机平台上，用单片机去编写程序实现模拟出遥控器控制信号PPM/PWM，模拟出的遥控器信号接到飞控遥控信号接口，进而去控制无人机飞行。前些年飞行器赛题内容相对比较简单，采用Pixhawk/APM这类飞控也能完成任务，本方案下文称作方案1。

更有甚者在19年工程机器人大赛无人机赛题中，对于第二、三项中需要无人机自主完成的任务，有一个队伍直接采用大疆Mavic无人机去参赛，然后在自己遥控器杆上加装了舵机等机械装置，可以实现通过程序设定去操作遥控器打杆，进而实现无人机的“自主”飞行，同时在无人机端装了视觉OPENMV和数传，用户利用自己加装的遥控器端的控制器去处理无限得到的视觉端处理数据，进而去决策无人机如何“自主”飞行，本方案下文称作方案2。

使用程序模拟遥控器PPM/PWM信号和舵机等机械装置改变遥控器信号本质上是一样的，都是通过改变飞控收到的遥控器数据，进而改变无人机的位置、速度、姿态期望实现自主飞行。上述打着插边球的参赛方案1和2显然不是组委会的出题初衷，但是人家也确实是满足当时竞赛赛题要求。正式鉴于此前不明确规则中存在的缺失，组委会在后面的无人机赛题中对限定赞助商MCU的处理具体内容部分逐步进行了明确，进而发展到当下放开机载计算机的限制，这一放开使得电赛飞行器向前迈了一大步。

机载计算机限制使用的放开使得依靠激光雷达/视觉SLAM等高精度定位手段在比赛中得以使用，实际2020年省赛中有少量队伍使用机载计算机平台、到了2021年国赛中基本已全面放开此限制，在无人机中使用机载计算机平台已经成为大势所趋，学生能够实现更加高效的二次开发，彻底告别“盲飞”。

无人机的串口指令控制

根据**《零基础学习竞赛无人机搭积木式编程指南》**里面的5讲内容，通过封装好的导航控制函数改变无人机的位置期望可以实现自定义航点飞行，同理我们可以设计一组串口指令控制数据帧用于对导航控制函数的参数进行设置，进而可以实现通过无人机地面站或者机载计算机ROS端发送串口指令数据帧给飞控，进而控制无人机自主飞行。这里的机载计算机ROS端等效于上一节中早期方案的赞助商MCU导航板，串口指令控制数据帧等效于模拟出的遥控器PPM/PWM信号。

二者形式上看视等效，实际内涵已经完全不一样了，赞助商限定MCU飞控从食物链的顶端逐步过渡到了最底部，经过这些年的电赛发展，现在赞助商的处理器TIVA、MSP432、C2000做成的飞控做到了简单易用，代码功能丰富、SDK接口函数调用容易，电赛无人机赛题的学生比赛准备阶段大多数经历，并不是在基于单片机驱动程序、芯片资源使用层面开发，而是基于飞控应用+机器视觉层面，针对具体赛题做二次应用开发、最后以往特别的难点在于赛题是利用有限的定位传感器光流、激光测距、超声波、openmv等结合现场标记特征，实现有效的室内定位功能，最后的这一难点在开放机载计算机使用限制后变得不再困难了，可以预见激光雷达/视觉SLAM方案会逐步作为通用的标准配置在比赛中使用。

首先实现无人机的串口指令控制也需要基本的飞行支持，和**《零基础学习竞赛无人机搭积木式编程指南》里面的4讲一样，必备的定高、定点，另额外加入了航向角速度控制**，相关函数如下：

//基本飞行保障必备子函数——定高、定点、同时加入了偏航指令控制
void ros_flight_support(void)
{Flight.yaw_ctrl_mode=ROTATE;//偏航控制为手动模式if(RC_Data.rc_rpyt[RC_YAW]==0)//无水平遥控量给定){if(ngs_nav_ctrl.cmd_vel_update==1)	Flight.yaw_outer_control_output  =ngs_nav_ctrl.cmd_vel_angular_z;else Flight.yaw_outer_control_output  =RC_Data.rc_rpyt[RC_YAW];//作用期过后，控制权限交给遥控器}else{Flight.yaw_outer_control_output  =RC_Data.rc_rpyt[RC_YAW];ngs_nav_ctrl.cmd_vel_update=0;//只要存在遥控器打杆操作，强制结束本次速度控制ngs_nav_ctrl.cmd_vel_during_cnt=0;		}		OpticalFlow_Control_Pure(0);//SLAM定点控制Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
}

`基于此函数无人机能满足三维空间位置的位置、速度、航向等控制，同时定义了串口控制数据结构体变量如下：

typedef enum 
{BODY_FRAME=0,//机体坐标系MAP_FRAME, //导航坐标系
}Navigation_Frame;typedef enum 
{RELATIVE_MODE=0,//相对模式GLOBAL_MODE,	//全局模式CMD_VEL_MODE,   //速度模式TRANSITION_MODE //过渡模式
}Navigation_Mode;typedef struct{uint16_t number;				//任务编号float x;						//x方向位置/速度期望float y;						//y方向位置/速度期望float z;						//z方向位置/偏航角速度期望uint8_t nav_mode;				//导航模式uint8_t frame_id;				//坐标系类型uint8_t update_flag;			//指令更新标志位uint8_t ctrl_finish_flag;		//控制完毕标志位uint16_t cnt; 					//位置控制完毕判断计数器float dis_cm;					//总位置偏差const float dis_limit_cm;       //位置阈值const float cmd_vel_max;        //最大线速度限制const float cmd_angular_max;    //最大角速度限制uint8_t cmd_vel_update;			//速度控制指令更新标志位float cmd_vel_x;				//期望x方向速度float cmd_vel_y;                //期望y方向速度float cmd_vel_angular_z;		//期望偏航方向角速度uint32_t cmd_vel_during_cnt;    //速度控制计数器uint8_t cmd_vel_suspend_flag;   //控制中止标志位
}nav_ctrl;

ROS端/地面砖发送的串口指令数据帧经过解析后，参数会传给导航控制函数实现期望的改变：

串口控制指令数据帧详情定义如下：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-zQ62rlBx-1651065268235)(/image-20220427103005569.png)]

nav_mode为0x00、0x01时表示位置导航控制，nav_mode为0x02时表示速度控制；位置导航控制的完毕约束条件是三个方向的位置偏差小于某个阈值，速度控制的约束条件是设定的执行时间计数器归0。飞控在收到控制指令已经对应控制执行完毕后，都会返回应答给地面站或者ROS端，用户在机载计算机端开发飞控任务时，可以通过查看应答数据来判断程序的执行情况

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-bVnVQJlx-1651065268236)(/image-20220427104119755.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VRn8EIKH-1651065268236)(/image-20220427121021526.png)]

下面以机载计算机端auto_flight功能包中提供的navigation_ctrl_demo为例，列出在ros端实现串口指令控制无人机自主飞行任务的编程步骤：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-aqLWXcND-1651065268237)(/image-20220427122756126.png)]

上图中圈红部分为串口指令控制部分，其中navigation_ctrl_demo节点发布了话题名为/nav_ctrl_comand的消息，消息数据类型为user_messages::navigation_ctrl，serialport_rplidar节点订阅此消息；同时serialport_rplidar节点发布了/uart_ctrl_response话题给navigation_ctrl_demo节点订阅。

float32 x
float32 y
float32 z
uint8 nav_mode
uint8 frame_id
uint16 num
uint32 execution_time
uint8 update_flaguint8 BODY_FRAME=0
uint8 MAP_FRAME=1
uint8 RELATIVE_MODE=0
uint8 GLOBAL_MODE=1
uint8 CMD_VEL_MODE=2

#include 
#include 
#include 
#include 
#include 
#include "floatbytedecode.h"
#include user_messages::navigation_ctrl cmd;serial::Serial com;
uint8_t read_buf[2048];
_serialport_decode _nclink_buf;
static uint8_t NCLink_Head[2]={0xFC,0xFF};//数据帧头
static uint8_t NCLink_End[2] ={0xA1,0xA2};//数据帧尾//导航控制串口数据帧发送
void navigation_ctrl_send(user_messages::navigation_ctrl *_cmd)
{_nclink_buf=Send_Navigation_Ctrl(_cmd->num,_cmd->x,_cmd->y,_cmd->z,_cmd->nav_mode,_cmd->frame_id,_cmd->execution_time);com.write(_nclink_buf.buf,_nclink_buf.cnt);   
}//导航控制串口应答回调函数
void uart_ctrl_response_callback(const std_msgs::UInt8::ConstPtr& msg)
{uint8_t cmd_check=msg->data;if(cmd_check==0xf2)      std::cout<<"[ INFO] ["<("nav_ctrl_comand",100);//定义导航控制话题发布者ros::Subscriber uart_ctrl_response_sub=n.subscribe("/uart_ctrl_response",100,uart_ctrl_response_callback);//定义导航控制应答话题订阅者ros::Duration(1).sleep();//节点初始化后必要的延时//参数重载std::string serial_port;    int serial_baudrate = 460800; ros::NodeHandle nh_private("~");nh_private.param("serial_port", serial_port, "/dev/ttyAMA0"); nh_private.param("serial_baudrate", serial_baudrate, 460800);try{com.setPort(serial_port);com.setBaudrate(serial_baudrate);serial::Timeout to=serial::Timeout::simpleTimeout(1000);com.setTimeout(to);com.open();}catch (serial::IOException& e){ROS_ERROR_STREAM("NAMELESSTECH unable to open port");return -1;}if(com.isOpen()){ROS_INFO_STREAM("NAMELESSTECH serial port is initialized");}else return -1;cmd.execution_time=1000;//速度控制时的执行时间ros::Rate loop_rate(1);//1hzwhile(ros::ok()){size_t _cnt=com.available();if(_cnt!=0){com.read(read_buf,_cnt);com.write(read_buf,_cnt);}static uint16_t cnt=0;if(cnt==0){//发布航点（0,0,100）cmd.num=0;cmd.x=0;cmd.y=0;cmd.z=100;cmd.frame_id=cmd.MAP_FRAME;cmd.nav_mode=cmd.GLOBAL_MODE;nav_ctrl_cmd_pub.publish(cmd);navigation_ctrl_send(&cmd);}else if(cnt==10){//发布航点（0,100,100）cmd.num=0;cmd.x=0;cmd.y=100;cmd.z=100;cmd.frame_id=cmd.MAP_FRAME;cmd.nav_mode=cmd.GLOBAL_MODE;nav_ctrl_cmd_pub.publish(cmd);navigation_ctrl_send(&cmd);    }else if(cnt==20){//发布航点（100,100,100）cmd.num=0;cmd.x=100;cmd.y=100;cmd.z=100;cmd.frame_id=cmd.MAP_FRAME;cmd.nav_mode=cmd.GLOBAL_MODE;nav_ctrl_cmd_pub.publish(cmd);navigation_ctrl_send(&cmd);}else if(cnt==30){//发布航点（100,0,100）cmd.num=0;cmd.x=100;cmd.y=0;cmd.z=100;cmd.frame_id=cmd.MAP_FRAME;cmd.nav_mode=cmd.GLOBAL_MODE;nav_ctrl_cmd_pub.publish(cmd);navigation_ctrl_send(&cmd);}else if(cnt==40){//发布航点（0,0,100）cmd.num=0;cmd.x=0;cmd.y=0;cmd.z=100;cmd.frame_id=cmd.MAP_FRAME;cmd.nav_mode=cmd.GLOBAL_MODE;nav_ctrl_cmd_pub.publish(cmd);navigation_ctrl_send(&cmd);}cnt++;ros::spinOnce();//监听反馈函数loop_rate.sleep();//执行循环     }return 0;
}

navigation_ctrl_demo例程序中，总共发布了5次串口控制指令消息，设置的主函数刷新频率是1Hz，通过计数器控制每间隔10s发布一次导航控制指令话题，发布的航点坐标依次是ENU（0，0，100）、ENU（0，100，100）、ENU（100，100，100）、ENU（100，0，100）、ENU（0，0，100），如果飞机初始处于地面，执行的任务是先自主起飞到100cm高度，而后依次向正北、正东、正南、正西飞行一个正方形轨迹后，回到起飞点正上方，这里说的东南西北是激光雷达SLAM定位下的等效方位。例程给出的是用时间去约束航点发布的间隔，采用时间间隔发布航点时，需要评估预设的间隔时间上是否足够无人机从当前位置飞到下一航点位置，实际用户二次开发也可以通过监听飞控应答数据来判断是否达到，同时飞控是有实时向无人机广播自身位置、速度、姿态的数据的，可以通过查看数据类型为nav_msgs::Odometry，名称为flight_state的话题来判断无人机的实时位置和姿态。

同时为了用户进一步的了解ROS端串口指令控制无人机的原理，我们提供了cmd_vel_ctrl_demo和cmd_pos_ctrl_demo，分别对无人机的速度与位置进行控制，使用cmd_vel_ctrl_demo例子时可以实现采用电脑键盘来操作无人机运动，类似ROS例程里面发布cmd_vel话题控制无人机x，y向线速度，已经z方向角速度。需要注意的是cmd_pos_ctrl_demo例子中前后左右是相对当前机体坐标系来讲的，这点和实际遥控器操控无人机飞行一致。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ypwTFAxQ-1651065268238)(/image-20220427201737995.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2d41ARaj-1651065268238)(/image-20220427202827895.png)]

掌握了机载计算机和飞控之间串口指令控制基本用法后，把飞控当做一个黑箱，像之前给出的2021年国赛赛题方案飞控端编程一样，我们同样可以在ROS端编程去实现所有任务，并且在ROS里面操作OPENCV，用户处理起来更灵活，数据实施性更高，这部分扩展例程序将会在后续的教程中展开。

d_vel话题控制无人机x，y向线速度，已经z方向角速度。需要注意的是cmd_pos_ctrl_demo例子中前后左右是相对当前机体坐标系来讲的，这点和实际遥控器操控无人机飞行一致。

[外链图片转存中…(img-ypwTFAxQ-1651065268238)]

[外链图片转存中…(img-2d41ARaj-1651065268238)]

掌握了机载计算机和飞控之间串口指令控制基本用法后，把飞控当做一个黑箱，像之前给出的2021年国赛赛题方案飞控端编程一样，我们同样可以在ROS端编程去实现所有任务，并且在ROS里面操作OPENCV，用户处理起来更灵活，数据实施性更高，这部分扩展例程序将会在后续的教程中展开。