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