对无人机高度环的思考和第一次炸机体验

 前两天学长布置了一个任务，大体就是在无人机起飞一段时间后再次改变飞行高度。一开始我觉得挺简单，随便改改就行了，所以今天才开始写，然后。。。炸机了。。。。先说说第一次炸机的体验。。就是看到无人机炸机的疯狂往屋顶上窜的时候，大脑先是一阵空白，然后我也没想起来要赶快离的远点，就是满脑子的  “为什么会这样？” “这TM跟我想的不对啊”，大概过了三四秒才反应过来，使劲向下拉油门，其实拉油门也没什么了，毕竟我程序写的是在5S后一键降落。然后我又看着飞机疯狂向下坠地。很庆幸，当时有两个同学在写程序，没有伤到同学。。总之炸机确实非常危险，要尽量避免事故的发生。我使用的是匿名的拓空者，这里贴上匿名飞控高度环和自动起飞任务的代码部分（未修改之前的）：

void Auto_Take_Off_Land_Task(u8 dT_ms)//
{static u16 take_off_ok_cnt;one_key_take_off_task(dT_ms);//如果已经解锁if(flag.unlock_sta){if(flag.taking_off){   if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL){flag.auto_take_off_land = AUTO_TAKE_OFF;       }}}else{auto_taking_off_speed = 0; flag.auto_take_off_land = AUTO_TAKE_OFF_NULL;  }
if(flag.auto_take_off_land ==AUTO_TAKE_OFF){//设置最大起飞速度s16 max_take_off_vel = LIMIT(Ano_Parame.set.auto_take_off_speed,20,200);//take_off_ok_cnt += dT_ms;//这个地方是自动起飞速度（一键起飞，一键降落）的关键auto_taking_off_speed = AUTO_TAKE_OFF_KP *(Ano_Parame.set.auto_take_off_height - wcz_hei_fus.out);//计算自动起飞速度auto_taking_off_speed = LIMIT(auto_taking_off_speed,0,max_take_off_vel);//退出起飞流程条件1，满足高度或者流程时间大于5000毫秒。if(take_off_ok_cnt>=5000 || (Ano_Parame.set.auto_take_off_height - loc_ctrl_2.exp[Z] <2))//(auto_ref_height>AUTO_TAKE_OFF_HEIGHT){//代表此时已经飞行稳定，加入积分控制flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH;}//退出起飞流程条件2，2000毫秒后判断用户正在控制油门。if(take_off_ok_cnt >2000 && ABS(fs.speed_set_h_norm[Z])>0.1f)// 一定已经taking_off,如果还在推杆，退出起飞流程{//代表此时已经飞行稳定，加入积分控制flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH;}}else {take_off_ok_cnt = 0;if(flag.auto_take_off_land ==AUTO_TAKE_OFF_FINISH){auto_taking_off_speed = 0;}}//调用自动降落后直接给予z方向上下降的速度if(flag.auto_take_off_land == AUTO_LAND){//设置自动下降速度auto_taking_off_speed = -(s16)LIMIT(Ano_Parame.set.auto_landing_speed,20,200);}
}
//高度环控制
void Alt_2level_Ctrl(float dT_s)
{//自动起飞任务Auto_Take_Off_Land_Task(1000*dT_s);//设置起飞速度fs.alt_ctrl_speed_set = fs.speed_set_h[Z] + auto_taking_off_speed;//计算出期望高度然后限幅loc_ctrl_2.exp[Z] += fs.alt_ctrl_speed_set *dT_s;loc_ctrl_2.exp[Z] = LIMIT(loc_ctrl_2.exp[Z],loc_ctrl_2.fb[Z]-200,loc_ctrl_2.fb[Z]+200);//loc_ctrl_2.fb[Z] = (s32)wcz_hei_fus.out;/if(fs.alt_ctrl_speed_set != 0){flag.ct_alt_hold = 0;}else{if(ABS(loc_ctrl_1.exp[Z] - loc_ctrl_1.fb[Z])<20){flag.ct_alt_hold = 1;}}if(flag.taking_off == 1){PID_calculate( dT_s,            //周期（单位：秒）0,               //前馈值loc_ctrl_2.exp[Z],             //期望值（设定值）loc_ctrl_2.fb[Z],           //反馈值（）&alt_arg_2, //PID参数结构体&alt_val_2,  //PID数据结构体100,//积分误差限幅0         //integration limit，积分限幅                                 );}else{loc_ctrl_2.exp[Z] = loc_ctrl_2.fb[Z];alt_val_2.out = 0;}alt_val_2.out  = LIMIT(alt_val_2.out,-150,150);
}

在自动起飞任务里匿名调用了一键起飞任务，当调用一键起飞或者手动推油门起飞时，则会执行该函数。又因为飞机起飞时数据偏差可能会很大，影响积分控制，所以在起飞的一段时间里会取消积分控制，执行PD控制。而当程序退出起飞流程后或者说飞行稳定后，则加入积分控制。而飞机初始的起飞高度，又由该指令控制

auto_taking_off_speed = AUTO_TAKE_OFF_KP *(Ano_Parame.set.auto_take_off_height - wcz_hei_fus.out);

这里使用了一个比例控制，该式右边第一项为KP项，一个比例系数。括号内第一项为自动起飞高度，也就是飞机在执行自动起飞时最后达到的高度。括号内第二项为当前高度。第一项也可以看作是一个期望高度。也只有当起飞高度=期望高度时，自动起飞速度才会置0，自动起飞速度在高度环会被调用作为起飞速度的一项。而在自动起飞期望高度该代码中该项被置零，所以我们只需要将自动起飞期望高度设置成我们第一次想要达到的高度即可。（这里修改起飞高度好像要用匿名上位机写入flash？）

最重要的是第二步，在飞机初始高度保持一段时间后在改变飞机的飞行高度。于是头脑简单的我直接给高度环里的期望高度粗暴的加上50，就像这样：

loc_ctrl_2.exp[Z] += fs.alt_ctrl_speed_set *dT_s+50;

然后就。。。医院wifi挺好的。。。
为什么不能直接这样粗暴的加上50呢？
我后来分析了炸机原因，发现高度环是每11ms调用一次的，也就是说每11ms，不管竖直方向上的速度，这个高度环的期望值都会自增50，这代表什么概念呢，大概就是要使反馈高度跟上这样的期望高度，飞机要每11ms竖直飞行50cm才可以达到。总的来说这里的问题出在反馈高度的增加值远远不如期望高度增加的快，所以导致飞机疯狂向屋顶上冲导致炸机。这里，在退出起飞流程后，Z轴上的速度只取决于起飞速度的大小，而起飞速度的大小则会取决于油门。所以，在这里我觉得可以通过改变起飞速度的值来达到再次改变高度的目的。

//这是分割线

在经历了昨天的炸机之后，今天我自己又写了写程序，效果不错。可以完成无人机在起飞流程结束后完成高度改变，所以最后我将无人机高度变化的操作封装了成了一个函数，可以在匿名的拓空者（试过了没问题）代码中直接调用，在这里分享给大家。

void Auto_height_change(float height,float time_s)
{//定义速度和相对高度float speed,rela_height;//只有退出起飞流程后才能执行该函数if(flag.auto_take_off_land == AUTO_TAKE_OFF_FINISH){//计算出相对高度，无人机在该基础上应该改变多少高度rela_height=height-wcz_hei_fus.out;//时间必须大于0，否则无效if(time_s>0){//计算出应该Z轴具有的速度speed = rela_height / time_s;//赋值z轴速度Program_Ctrl_User_Set_Zcmps(speed);//将标志位置1进入计时状态change_flag=1;}}
}

void Auto_height_change_task(float height,float time_s)
{double true_time;Auto_height_change(height,time_s);if(change_flag){//      cnt_height_change++;//计算出当前的真实时间//当开始计时后每次执行该函数真实时间增加0.05s（即50ms）true_time += 0.05;}//    true_time = cnt_height_change*0.05;//如果时间一旦达到规定的时间if(true_time==time_s){//将真实时间清0，以便于下次再次计算true_time=0;//将标志位清0退出计时change_flag=0;//z轴方向不在赋予速度，以达到定高的效果Program_Ctrl_User_Set_Zcmps(0);}
}

我比较菜，代码也就写成这样了。这俩函数的第一个参数就是你想要达到的高度，你可以随便设置。第二个参数就是无人机从起飞完成的高度到你设置高度的时间，如果你想要较快的变化，可以将时间设置的短一些；想慢慢观察可以将时间设置的长一些。最后一个函数我是将其写在轮询任务9中，也就是50ms就会调用一次该函数。总的来说，最终的控制效果确实可以达到，但是会产生高度的误差。一方面可能是高度环和该函数调用周期不同而产生的差值，另一方面可能是数据融合高度不稳定造成的。嗯，就这样。

对无人机高度环的思考和第一次炸机体验相关推荐

让人又爱又恨的氮化镓器件 --- 记录人生第一次炸机
宽禁带器件也不是什么新鲜词儿了,我们常用的宽禁带电力半导体有两类 - 氮化镓GaN器件和碳化硅SiC器件. 之前尝试过用SiC器件研发了一款图腾柱PFC整流器,别的不说,这SiC是真的耐造.从头到尾, ...
ACfly的Ctrl_Attitude.cpp的代码（可以看到在角速度环和高度环用ADRC）
ACfly的Ctrl_Attitude.cpp的代码 Attitude是姿势的意思我看有个姿态ESO,实际应该是角速度的,还有个高度ESO 在高度上用ADRC可能是为了高度无参自适应,你每次改变无人 ...
基于数据融合和串级PID的小型四旋翼无人机高度测量与控制系统
作者:Tomy 摘要:简单介绍了一种基于 Mahony 算法无人机姿态解算算法,并在这个的基础上设计了一种融合惯导加速度计和气压计的小型无人机高度测量系统, 同时利用该高度测量系统设计了基于串级 ...
倾斜摄影测量无人机炸机的八大原因及“炸机”预防措施
引言采用无人机航测的方式代替人工完成相应的测绘工作不仅效率高,其精度也有保证.但飞行事故却是一个不容忽视的重要潜在危险源.航测设备的价格往往不低,每一个事故的发生都会给客户造成巨大损失.有鉴于此,睿 ...
记一次拆机除尘换硅脂的经历，第一次拆机幸好没拆坏，心真大-_-!
**************注意****************** 拆机有风险,请不要随便乱拆,有需要找专业人士本文是分享经历的文章,不是教程... *********************** ...
MIUI10刷入Magisk模块——记小白的第一次刷机
MIUI10刷入Magisk模块--记小白的第一次刷机前言之前双十一没啥可做的,全国人民都在为淘宝破那多少个亿做准备,于是在这个氛围中突然萌生一种给自己一个奖励的冲动,然后决定奖励自己一次让手机变 ...
小白第一次注册机编写，c/bat/易某言（不好意思说2333）
小白第一次注册机编写,c/bat/易某言(不好意思说出口2333) 零.视频教程一.免责声明二.软件相关信息三.查壳四.分析软件注册流程五.IDA载入六.OD载入 (一).找关键跳转 1. ...
HTC G8 wildfire第一次刷机
10年8月份买的HTC G8水货机,wifi(wlan)可用,一直没觉得什么不妥,只是有一次不知什么原因,智能键盘出现的是不知是什么字的键盘,无法存储人,无法发送短消息,总之一句话,就基本上就是一家庭 ...
从零开始学安卓笔记：记第一次真机调试IDEA
从几个月前一时兴起开始学安卓的知识,一直都是用的模拟器调试的,但是这就有一个问题,模拟器有时候很慢,而且经常出现没有响应的状况. 今天想用真机测试一下,但是发现不会操作,在搜索一圈之后发现大家都说的在 ...

对无人机高度环的思考和第一次炸机体验

对无人机高度环的思考和第一次炸机体验相关推荐

最新文章

热门文章