导航基本算法.doc

导航基本算法 有了位置、高度、地速、空速、姿态，我们就可以根据这些参数来进行计算，控制飞机给出相应舵面，实现导航、定高的目的。 最简单的导航算法只考虑当前飞机所在点和目标点。设想一下，飞机在空中飞行时，当前点和目标点的连线指定了飞机应该飞行的航向（即目标航向），如果要使飞机朝向目标点飞行，必须控制飞机转弯，使飞机航向与目标航向一致。为了准确描述航向，我们定义以正北为0度，顺时针起航向与正北的夹角为正，逆时针起航向与正北的夹角为负，这样将航向定义在-180度到+180度之间。这样，上例中，飞机的航向为90度，目标航向为120度左右。好了，当前飞机的航向我们可以从GPS航向里提取，我们记其为Hangxiang。下面我们怎么根据目标点和当前点的坐标来计算目标航向呢？我们把目标航向记为beta，于是航向偏差为beta-Hangxiang。 记当前的经纬度为CurLongi、CurLatti，单位为度；目标点经纬度为tgtLongi、tgtLatti，单位为度。由平面几何的知识，我们计算反正切atan(tgtLongi-CurLongi)/(tgtLatti-CurLatti)，再经过象限处理，就可得到目标航向。在实际程序中，我们是这样做的：vectorLatti = tgtLatti - CurLatti;vectorLongi = tgtLongi - CurLongi;beta = CalAtanVector(); /计算vectorLongi、vectorLatti向量与正北的夹角而CalAtanVector（）函数如下：float CalAtanVector(void) /计算vectorLongi/vectorLatti的反正切函数，返回范围在-PIPI之间float beta;float lTemp;if(vectorLatti 0)bNorth = 1;elsebNorth = 0;if(vectorLongi 0)bEast = 1;elsebEast = 0;vectorLatti = fabsf(vectorLatti);vectorLongi = fabsf(vectorLongi);vectorLongi = vectorLongi * COS_FACTOR;if(vectorLongi vectorLatti)lTemp = vectorLongi;vectorLongi = vectorLatti;vectorLatti = lTemp;bGT45 = 1;elsebGT45 = 0;if (vectorLatti!=0)beta= atan(vectorLongi/vectorLatti); /0PI/4范围elsebeta = PI/2;if(bGT45)beta = PI/2 - beta; /PI/4PI/2范围if(bNorth & bEast)beta = beta; /0PI/4范围if(bNorth & !bEast)beta = 0 - beta; /-PI/40范围if(!bNorth & !bEast)beta = beta - PI; /-PI-3PI/4范围if(!bNorth & bEast)beta = PI - beta; /3PI/4PI范围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)tempj2 = tempj2 - 2*PI;if (tempj2 ParaDataInUse15)&(fabsf(tempj2