无人机导航控制算法

i!=0) beta= atan(vectorLongi/vectorLatti); else beta = PI/2; if(bGT45) beta = PI/2 - beta; if(bNorth && bEast) beta = beta; beta = 0 - beta; if(!bNorth && !bEast) beta = beta - PI; if(!bNorth && bEast) beta = PI - beta; return beta; } 其中出现了一个因数 COS_FACTOR，这是当前纬度的余弦值。这是因为两点经度之差要乘以 当前纬度的余弦值，才代表了两点水平距离之差。也就是说，同样的经度差，在赤道附近表征很 大的距离，而在北极附近表征的距离就相对较小，这一点大家应该能理解。 好了，现在已经得到了 alpha = beta – Hangxiang，即为当前航向与目标航向的偏差角， 当不需要压航线进行控制的时候，比如进行所点即所到、临时改变航点的时候，就可以用 alpha 作为 PID 控制的误差输入量， 去计算应该给出的方向舵控制量 （方向舵转弯时） 或横滚坡度量 （副 翼转弯时）了。 当需要压航线进行飞行的时候呢？同样，我们可以根据航线的上一个点（坐标记为 tgtLongi_s，tgtLatti_s）和航线的当前点，按照同上的算法计算出当前航线与正北的夹角，记 为 beta2，如下： vectorLatti = tgtLatti - tgtLatti_s; vectorLongi = tgtLongi - tgtLongi_s; beta2 = CalAtanVector(); //航线与正北的夹角 //3PI/4~PI 范围 //-PI~-3PI/4 范围 //0~PI/4 范围 //-PI/4~0 范围 if(bNorth && !bEast) //PI/4~PI/2 范围 //0~PI/4 范围
然后我们需要再计算一个重要的量——偏航距， 即飞机所在点到航线的距离。 需要压航线时， 飞机需要尽快的回到航线上，而不仅仅是朝着目标点。 我们用一个 newdistance（）函数来计算偏航距。大家可以回忆一下高中数学里的点到直线 的距离公式： float newdistance(void) //计算当前点与当前航线的距离 { float dis,sin_t,cos_t; dis = sqrt( (tgtLatti - tgtLatti_s) * (tgtLatti - tgtLatti_s) + (tgtLongi - tgtLongi_s)
atan((tgtLongi-CurLongi)/(tgtLatti-CurLatti))，再经过象限处理，就可得到目标航向。
在实际程序中，我们是这样做的： vectorLatti = tgtLatti - CurLatti; vectorLongi = tgtLongi - CurLongi; beta = CalAtanVector(); //计算 vectorLongi、vectorLatti 向量与正北的夹角 而 CalAtanVector（）函数如下： float CalAtanVector(void) //计算 vectorLongi/vectorLatti 的反正切函数，返回范围在 -PI~PI 之间 { float beta; float lTemp; if(vectorLatti > 0) bNorth = 1; else bNorth = 0; if(vectorLongi > 0) bEast = 1; else bEast = 0; vectorLatti = fabsf(vectorLatti); vectorLongi = fabsf(vectorLongi); vectorLongi = vectorLongi * COS_FACTOR; if(vectorLongi > vectorLatti) { lTemp = vectorLongi; vectorLongi = vectorLatti; vectorLatti = lTemp; bGT45 = 1; } else { bGT45 = 0;
tempj2 = Hangxiang - beta2; //当前航线-航线航线，用以判断机头是否大致朝向目标点 if (tempj1 > PI) tempj1 = tempj1 - 2*PI; //将其化为-pi 到 pi 之间； if (tempj1 < -PI) tempj1 = 2*PI + tempj1; if (tempj2 > PI) tempj2 = tempj2 - 2*PI; if (tempj2 < -PI) tempj2 = 2*PI + tempj2; if ((fabsf(lDistance) > ParaDataInUse[15])&&(fabsf(tempj2) < 80*PI/180)) //如果飞 机偏航距超过了域值，且大致方向正确，即机头已转过来了 { BP = fabsf(lDistance)*ParaDataInUse[21]/64; //ParaDataInUse 数组为可实时用地面站 调整的参数 if (BP > 60) BP = 60; //航线航向上叠加的修正量限幅 if (tempj1 < 0) //当前点在航线右边 beta = beta2 - BP*PI/180; else } else { beta = beta; } //否则不考虑偏航距 //当前点在航线左边 beta = beta2 + BP*PI/180;
为了准确描述航向，我们定义以正北为 0 度，顺时针起航向与正北的夹角为正，逆时针起航 向与正北的夹角为负，这样将航向定义在-180 度到+180 度之间。这样，上例中，飞机的航向为 90 度，目标航向为 120 度左右。
好了，当前飞机的航向我们可以从 GPS 航向里提取，我们记其为 Hangxiang。下面我们怎么 根据目标点和当前点的坐标来计算目标航向呢？我们把目标航向记为 beta，于是航向偏差为 beta-Hangxiang。 记当前的经纬度为 CurLongi、CurLatti，单位为度；目标点经纬度为 tgtLongi、tgtLatti， 单位为度。由平面几何的知识，我们计算反正切
最后，航向偏差角 alpha = beta – Hangxiang，再将 alpha 作为偏差量输 入到 PID 函数里去，解算出当前的控制量，控制方向舵或副翼进行转弯。
导航基本算法
有了位置、高度、地速、空速、姿态，我们就可以根据这些参数来进行计算，控制飞机给出 相应舵面，实现导航、定高的目的。 最简单的导航算法只考虑当前飞机所在点和目标点。设想一下，飞机在空中飞行时，当前点 和目标点的连线指定了飞机应该飞行的航向（即目标航向），如果要使飞机朝向目标点飞行，必 须控制飞机转弯，使飞机航向与目标航向一致。
* (tgtLongi - tgtLongi_s) * COS_FACTOR * COS_FACTOR ); if (dis!=0) { sin_t = (tgtLatti - tgtLatti_s) / dis; cos_t = (tgtLongi - tgtLongi_s) * COS_FACTOR / dis; } else { sin_t = 0; cos_t = 0; } dis = (CurLongi - tgtLongi) * sin_t * COS_FACTOR + (tgtLatti - CurLatti) * cos_t; dis = fabsf(dis * 110000); return dis; } 最后将 dis 乘以 110000 是因为在赤道上经纬度的 1 度对应 110 公里左右。 有了偏航距，我们需要来判断，当偏航距大于一定域值时，目标航向不再是目标点与当前点 连线与正北的夹角，而是航线与正北的夹角，再附加上一个修正量，这个修正量的目的是是飞机 尽快回到航线上，并且大小与偏航距有关。根据这个逻辑，我们有以下的程序： tempj1 = beta - beta2; //目标航向-航线航线，用以判断飞机在航线的左侧还是右侧