2021年国赛植保无人机实现思路_2021电赛题目植保无人机

一.序言

2025年全国电赛临近，我这边刚刚考完期末就得进实验室备赛。由于25年是国赛，加上去年由于实习与比赛时间冲突的原因没能参加，所以今年压力还是很大的。

之前一直在往控制类的方向靠，今年基于各种原因进无人机组了，相比控制类的话它对硬件方面的要求会低很多。

说明：

对于大致框架与启动代码，我的飞控厂家无名创新已有提供，但是由于注释的篇幅原因我个人认为它的注释比较晦涩，如果基础不好还是很难上手，对于想通过厂家代码直接学会怎么用的同学还是不太友好，所以本文只针对与和我一样使用盘古飞控TIDronePilot 的同学，同时整个代码涉及侵权无法提供，只是针对该飞控给出2021年植保题我整理后的实现思路。

1.题目回顾

先来回顾一下题目：

2.配件介绍

我的无人机大部分配件都是无名创新的，包括飞控、树莓派、TOFSense模块等重要配件，至于机甲、电机电调大家随意选择即可，我的是牛牌的。

这里重点介绍飞控，芯片型号是TI官方芯片TM4C123GH6PZ，与我之前常用的stm32F1系列不同，但代码执行逻辑都大同小异。

硬件连接不多做介绍，b站很多视频。需要注意的是测试过程中一定不要给飞控12v！！！我们组就是这个原因把电源板的12v短暂的接到了飞控上，立马烧了。如果是在临近比赛烧坏那基本相当于白做了。

这里我拍的很丑，具体成品参考淘宝无名的厂家即可。

二.基础部分

对二次开发来说，底层部分无名厂家已经做好了，重点要关注整个过程的逻辑，把动作拆分即可。

这里的基本要求共四个步骤：

首先要明确电赛过程是禁止使用遥控器的，所以我们先得思考自动起飞——基础部分——发挥部分的切换怎么实现。这很简单，整个大框架的选择可以在Developer_Mode.c执行，先实现宏观选择，然后将不同的模式设置为不同的case，在选择SDK模式后，即可实现我们想要的功能。后面的编写也是一样，分文件编写有利于我们对整个过程的进一步理解。

接下来进入Subtask.c的介绍，也就是我们整个过程的具体实现逻辑。

先来看看在整个过程中起着至关重要作用的几个全局变量：

我们整个过程的实现实际就是无数个小动作的集合，那么我们如何在一个动作结束之后进入另一个动作的线程？这就用到了第一个变量——flight_subtask_cnt，将其作为每个线程的if判断逻辑，在我们每个线程结束后，自加即可进入下一线程。

flight_global_cnt的作用就没有那么直观了，“全局计数器，用于判断是否到达“其实是一个很抽象的概念。其实，它就是我们用于判断是否达到我们目标点的一个变量，用于减小误差，有着十分重要的作用，因为整个飞行过程的不可控因素太多了，我们不能简单的认为当前位置==目标位置就实现了要求。它需要和以下两个常量配合使用。

在每一个执行逻辑里面，我们都可以通过这种逻辑实现，误差距离小于阈值（dis_cm <= Waypoint_Fix_CM1 ）” 一定时长（累计计数达到 Waypoint_Fix_Cnt1 对应次数，对应代码里的 0.05s 逻辑），避免单次 / 瞬时满足条件就判定到位的误判，让航点 “到达并稳定” 的状态判定更精准，符合实际飞行控制中需要稳定在目标点的需求。

execute_time_ms则用于给子线程设置执行时间，可以通过它的自减有效控制每一个子线程的运行时长，避免卡死与超时。

这三个全局变量起着支架的作用决定整个过程的执行，接下来进入正式的实现环节。

对于基础部分，实现思路是起飞——飞到A点——遍历除最后一个点外的所有点——遍历最后一个点——返回起飞点——进一步确定是否到达起飞点——降落

由于最后一个点之后飞向原点，执行时间与其他点不一致，所以处理需要额外考虑。

const int16_t work_waypoints_table[2][32]={{0,1,1,2,2,3,3,4,5,6,7,7,6,5,4,4,5,6,7,7,6,5,4,4,5,6,7,7,6,5,4,0},{0,4,5,5,4,4,5,5,5,5,5,4,4,4,4,3,3,3,3,2,2,2,2,1,1,1,1,0,0,0,0,0}};//坐标系建立

建好虚拟坐标系以后，分工好我们每个位置的执行时间， 接着根据题目要求——每个模块50cm，建立好我们实际的work_waypoint_generate即可。

每个线程的具体逻辑都大同小异，即是否稳定到达当前航点——稳定后确定目标点坐标——决定是否撒药并进入下一线程，关键就是一定要确保稳定后再进行相应操作，这里给出遍历的实现代码：

if(flight_subtask_cnt[n]<31)//遍历{basic_auto_flight_support();//基本飞行支持软件//判断是否到达目标航点位置if(flight_global_cnt[n]<Waypoint_Fix_Cnt1)//时间要求是否达到？？{float dis_cm=pythagorous2(OpticalFlow_Pos_Ctrl_Err.x,OpticalFlow_Pos_Ctrl_Err.y);if(dis_cm<=Waypoint_Fix_CM1)//距离是否超过误差？？？ flight_global_cnt[n]++;else flight_global_cnt[n]/=2;}else//满足要求，即到达目标航点位置{execute_time_ms[n]=work_time_gap[flight_subtask_cnt[n]]/flight_subtask_delta;//子任务执行时间target_position.x=base_position.x+work_waypoints[0][flight_subtask_cnt[n]];target_position.y=base_position.y+work_waypoints[1][flight_subtask_cnt[n]];target_position.z=base_position.z;Horizontal_Navigation(target_position.x,target_position.y,target_position.z,GLOBAL_MODE,MAP_FRAME);flight_subtask_cnt[n]++;flight_global_cnt[n]=0;if(Opv_Top_View_Target.trust_flag==1)//到达目标航点后，判断底部是否为农作物{laser_light_1.reset=1;laser_light_1.times=1;//闪烁1次}}}

对于我的飞控来说，PD1连接激光笔实现打点操作，laser_light_1可对其进行控制。

还需要注意的是，这里的Opv_Top_View_Target.trust_flag==1实际是视觉模块openmv的作用，具体实现需要在树莓派执行，树莓派与飞控协作，SLAM建图、判断航电、扫描二维码等视觉操作在树莓派执行，串口通信后，飞控判断完再给出具体的执行要求。

整个基础部分并不难，我认为难点就在任务的拆分，以及对实际过程中是否精确到达的处理，还有与视觉部分的协作分工，4天的时间，一定不要着急上手，对整个过程有着清晰的子线程拆分后再测试。

三.发挥部分

基础部分实现不再赘述，在原来的基础上，接下来——降低高度——判断是否到达都很容易想到，但是我们最后的点位在边沿区域，雷达很容易将外界物体识别为目标，如何解决？

这里有一个很聪明的做法，先飞到14号位置，再进行操作，即可避免失误，即

飞到14号区域——偏航运动使得朝向塔杆——识别二维码——闪烁——间隔——闪烁

创新部分最难的就是对杆部分的处理。首先是偏航角，这里要用到PID算法的P部分：比例项。

控制无人机的偏航角度，通常有一个目标值（期望达到的角度）和一个实际值（当前实际的角度）。系统的任务就是通过某种控制手段，让实际值尽可能快速、准确地接近目标值。对于比例控制（P 控制）的核心思想是：控制量与偏差成正比。对于无人机系统，以target_yaw_err作为我们实际航偏角的偏差，差值越大，说明无人机偏离目标偏航角度越远，需要的调整力度也就越大。我们设expect_yaw_gyro（期望偏航角速度），它即是比例控制器的输出结果，它会被传递给无人机的飞行控制系统，用于驱动无人机的相关执行机构，从而让无人机按照期望的角速度转动，逐渐减小偏航角度误差，达到期望的偏航角度。

接下来控制偏航角的控制模式设置和输出限制即可，这里需要知道ROTATE模式，在此模式下，无人机将根据yaw_outer_control_output的值调整偏航角，实现对准操作。同时由于电机的最大负载限制，所以我们一定要用constrain_float函数将输入值限制，否则可能导致失控。接下来按照偏航角的误差在一定范围内的条件进行对准操作即可。

float expect_yaw_gyro=Total_Controller.Yaw_Angle_Control.Kp*target_yaw_err;//期望偏航角速度Flight.yaw_ctrl_mode=ROTATE;Flight.yaw_outer_control_output=constrain_float(expect_yaw_gyro,-10,10);if(ABS(target_yaw_err)<=5.0f)//只有当偏航角度比较小时，才靠近杆{float dis_err=min_dis_cm-50;//设定杆到无人机中心的距离，非摄像头到杆距离，可根据实际轴距、视觉处理模块安装位置以及识别情况适当调整// dis_err=constrain_float(dis_err,-20,20);// OpticalFlow_Y_Vel_Control(Total_Controller.Optical_Position_Control.Kp*dis_err);// OpticalFlow_X_Vel_Control(0); 光流实现速度控制--替换为SLAM实现速度控制 // 1. 计算Y方向目标速度（对应原光流Y方向控制） float target_vy = Total_Controller.Optical_Position_Control.Kp * dis_err; // 2. X方向目标速度设为0（对应原光流X方向控制） float target_vx = 0; // 3. 将目标速度赋值给SLAM速度控制器 Total_Controller.Latitude_Speed_Control.Expect = target_vy; // Y方向速度期望 Total_Controller.Longitude_Speed_Control.Expect = target_vx; // X方向速度期望 // 4. 调用SLAM位置控制函数执行速度控制（假设SLAM数据结构体为slam_ins） slam_control_poshold(&uav_ins);header_fix_flag=1;}else//否则原地悬停{// OpticalFlow_Control_Pure(header_fix_flag);// header_fix_flag=0; 光流悬停 // 若需要悬停，可将目标速度设为0后调用SLAM控制 Total_Controller.Latitude_Speed_Control.Expect = 0; Total_Controller.Longitude_Speed_Control.Expect = 0; slam_control_poshold(&uav_ins); header_fix_flag=0;}Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制if(barcode_flag==1)//只要识别到了二维码，提前结束对准塔杆、靠近任务{flight_subtask_cnt[n]=36;flight_global_cnt[n]=0;//识别到条形码后，重新回退到14区块，避免直接返航过程中撞到杆target_position.x=base_position.x+4*Scale_Param;target_position.y=base_position.y+3*Scale_Param;target_position.z=Barcode_Height_CM;Horizontal_Navigation(target_position.x,target_position.y,target_position.z,GLOBAL_MODE,MAP_FRAME);}

要说明的是我的无人机没有光流，所以原先通过光流控制水平移动的代码我换成了SLAM的，但实现目的都是一致的。

为什么要退回14号区域？代码注释加上前面坐标图已经很清楚了，直接返回可能引发撞杆。

接下来的部分就简单了，实现闪烁——间隔——再闪烁即可，唯一需要注意的是，关于预留时间部分：execute_time_ms[n]=(barcode_id*1000+3000)/flight_subtask_delta。

然后回到原点下方格子——对准机头——设立目标点并降落即可。这里的对准机头需要引入AZIMUTH模式，我的理解是绝对角度控制模式，在该模式下，无人机的控制目标是让机头精确指向某个绝对航向角（如正北方向、预设角度等），而非像ROTATE模式那样控制旋转角速度。

给出开始对准机头的代码，后面再利用AZIMUTH模式类似。

else if(flight_subtask_cnt[n]==40)//开始对准机头，赋值只执行一次{Flight.yaw_ctrl_mode=AZIMUTH;//直接控制航向角模式Flight.yaw_ctrl_start=1;//开始控制使能Flight.yaw_outer_control_output =0;//目标角度，等效正北和初始机头朝向一致//OpticalFlow_Control_Pure(0);//光流定点控制slam_control_poshold(&uav_ins);//替换为SLAM定点Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制flight_subtask_cnt[n]=41;}

最后还是感谢无名创新对本校电赛设备的支持。