导航基本算法.docx

导航基本算法 有了位置、高度、地速、空速、姿态，我们就可以根据这些参数来进行计算，控制飞机给出 相 应 舵 面 ， 实 现 导 航 、 定 高 的 目 的 。 最简单的导航算法只考虑当前飞机所在点和目标点。设想一下，飞机在空中飞行时，当前点和目标点的连线指定了飞机应该飞行的航向（即目标航向） ，如果要使飞机朝向目标点飞行，必 须 控 制 飞 机 转 弯 ， 使 飞 机 航 向 与 目 标 航 向 一 致 。 为了准确描述航向，我们定义以正北为 0 度，顺时针起航向与正北的夹角为正， 逆时针起航向与 正北的夹角为负，这样将航向定义在 -180 度到+180 度之间。这样，上例中，飞机的航向为 90 度， 目 标 航 向 为 120 度 左 右 。 好了，当前飞机的航向我们可以从 GPS 航向里提取，我们记其为 Hangxiang 。下面我们怎么根据目标点和当前点的坐标来计算目标航向呢？ 我们把目标航向记为 beta， 于是航向偏差为beta-Hangxiang 。 记当前的经纬度为 CurLongi 、CurLatti ，单位为度；目标点经纬度为 tgtLongi 、tgtLatti，单位为 度。由平面几何的知识，我们计算反正切 atan((tgtLongi-CurLongi)/(tgtLatti-CurLatti)) ，再经过象限处 理 ， 就 可 得 到 目 标 航 向 。 在 实 际 程 序 中 ， 我 们 是 这 样 做 的 ： vectorLatti = vectorLongi = tgtLatti tgtLongi CurLatti; CurLongi; beta = CalAtanVector(); // 计 算 vectorLongi 、 vectorLatti 向 量 与 正 北 的 夹 角 而 CalAtanVector （ ） 函 数 如 下 ： float CalAtanVector(void) //计算 vectorLongi/vectorLatti 的反正切函数，返回范围在 -PI~PI 之间 { float float if(vectorLatti beta; lTemp; > 0) bNorth = 1; else bNorth = 0; if(vectorLongi bEast = > 0) 1; else bEast = 0; vectorLatti vectorLongi = fabsf(vectorLatti); = fabsf(vectorLongi); vectorLongi if(vectorLongi = vectorLongi > * COS_FACTOR; vectorLatti) { lTemp = vectorLongi; vectorLongi = vectorLatti; vectorLatti = lTemp; bGT45 = 1; } else { bGT45 = 0; } if beta= atan(vectorLongi/vectorLatti); //0~PI/4 (vectorLatti!=0) 范 围 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 beta = beta && - PI; //-PI~-3PI/4 范 !bEast) 围 if(!bNorth && bEast) beta = PI - beta; //3PI/4~PI 范 围 return beta; } 其中出现了一个因数 COS_FACTOR，这是当前纬度的余弦值。这是因为两点经度之差要乘以当前纬度的余弦值，才代表了两点水平距离之差。 也就是说，同样的经度差，在赤道附近表征很大的 距 离 ， 而 在 北 极 附 近 表 征 的 距 离 就 相 对 较 小 ， 这 一 点 大 家 应 该 能 理 解 。 好了，现在已经得到了 alpha = beta –Hangxiang ，即为当前航向与目标航向的偏差角， 当不需要压航线进行控制的时候，比如进行所点即所到、临时改变航点的时候，就可以用 alpha 作为 PID 控制的误差输入量，去计算应该给出的方向舵控制量（方向舵转弯时） 或横滚坡度量（副翼转弯 时 ） 了 。