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

---

typora-root-url: img

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

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

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

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

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

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

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

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

|

上述方案的前三项基本都是基于单片机层面编程，以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);//高度控制
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

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

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;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

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_flag

uint8 BODY_FRAME=0
uint8 MAP_FRAME=1
uint8 RELATIVE_MODE=0
uint8 GLOBAL_MODE=1
uint8 CMD_VEL_MODE=2
1
2
3
4
5
6
7
8
9
10
11
12
13
14

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <user_messages/navigation_ctrl.h>
#include <serial/serial.h>
#include "floatbytedecode.h"
#include <std_msgs/UInt8.h>

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] ["<<ros::Time::now()<<"]: SERIALPORT_RPLIDAR：飞控返回校验，位移控制指令写入成功"<<std::endl;
    else if(cmd_check==0xf6) std::cout<<"[ INFO] ["<<ros::Time::now()<<"]: SERIALPORT_RPLIDAR：飞控返回校验，导航控制指令写入成功"<<std::endl;
    else if(cmd_check==0xf7) std::cout<<"[ INFO] ["<<ros::Time::now()<<"]: SERIALPORT_RPLIDAR：飞控返回校验，导航控制完毕，已达到目标点"<<std::endl;
    else if(cmd_check==0xf8) std::cout<<"[ INFO] ["<<ros::Time::now()<<"]: SERIALPORT_RPLIDAR：飞控返回校验，SLAM复位重置"<<std::endl;
    else                     std::cout<<"[ INFO] ["<<ros::Time::now()<<"]: SERIALPORT_RPLIDAR：飞控返回校验，表征含义不明"<<std::endl;
}


int main(int argc,char **argv)
{
    ros::init(argc,argv,"navigation_ctrl_demo");//初始化节点名称
    ros::NodeHandle n;//初始化节点句柄  
    ros::Publisher nav_ctrl_cmd_pub=n.advertise<user_messages::navigation_ctrl>("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<std::string>("serial_port", serial_port, "/dev/ttyAMA0"); 
    nh_private.param<int>("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);//1hz
    while(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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138

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，用户处理起来更灵活，数据实施性更高，这部分扩展例程序将会在后续的教程中展开。