CN101266150B - 一种无人机侧向领航方法 - Google Patents

Abstract

本发明提供了一种无人机侧向领航方法，该方法根据期望航线以及无人机位置、高度、地速信息，连续实时计算无人机相对于期望航线的侧偏距D_Z及侧偏移速度D_Zd等领航参数，并同无人机姿态运动信息一同输入至侧向控制回路得到舵偏度指令，最终引导无人机沿着期望航线飞行。本发明的领航方法在常值侧风干扰下为控制稳态误差的消除提供前提，并在计算过程中考虑了地球为椭球体，所得到的领航精度高，适用于对于精度要求较高的无人机领航。

Description

一种无人机侧向领航方法

技术领域

本发明属于无人机导航技术领域，具体地说，是指一种无人机侧向领航方法。

背景技术

无人机导航的基本任务是精确确定飞机的空间位置，并使其能够沿期望的轨迹飞行。目前的区域导航方法，能够允许飞机在台基导航设备的基准台覆盖范围内或在自主导航设备能力有限范围内或两者配合下按任何希望的飞行路径飞行。其中的台基导航设备，包括传统的以地面电台为基础的陆基导航设备和卫星导航系统为基础的星基导航设备。在航线结构上，区域导航的航线就是由航路点系列组成的连线，这些航路点是脱离电台台址而自行设定的任何地理位置；在定位方法上，区域导航定的是飞机在地球上的绝对位置；在导航算法上，区域导航按飞机计划转换到航线坐标，计算出向前方航路点飞行的待飞距离和航迹的侧向偏移，所有的计算在大圆航线上进行。

目前无人机侧向领航控制系统的工作原理如图1所示。该侧向领航方法根据期望航线以及由传感器获得的飞机位置、高度、航向角信息，连续实时计算飞机相对于期望航线的侧偏距及航向角偏差等领航参数，并同由姿态运动传感器获得的飞机姿态运动信息一同输入至侧向控制回路得到舵偏度指令，由舵回路根据舵偏度指令控制无人机飞行，最终实现引导无人机沿着期望航线飞行。

上述的领航方法在计算飞机侧偏距时，通常以地球圆球模型进行计算，计算精度在十几到几十米左右。由于地球绕其极轴转动，所以赤道各处的地球半径较其极轴方向的半径长，地球类似于一旋转椭球体。因此现有的这种领航方法对于精度要求较高的无人机是不适用的。另外，由于无人机侧向领航方法给出的是侧偏距及航向角偏差参数，在常值侧风干扰下将会有出现控制稳态偏差，若能够提供侧偏移速度信息则可将控制的稳态误差消除。因此，有必要考虑地球的椭球体模型，研究能够提供高精度的侧偏距信息，并提供侧偏移速度信息的领航方法。

发明内容

本发明的目的是提出一种无人机侧向领航方法，根据无人机的位置信息以及期望航线得到无人机的侧偏距以及侧偏移速度，进而引导无人机沿期望航线飞行。

本发明的无人机侧向领航方法通过以下步骤实现：

步骤一：根据无人机的当前地理纬度B、航线从点(L₀，B₀)的地理纬度B₀、航线到点(L₁，B₁)的地理纬度B₁以及无人机的高度H信息，由式(1)、(2)、(3)计算无人机的地心纬度B_EC、航线从点地心纬度B_EC0、航线到点地心纬度B_EC1：

$B_{\mathrm{EC}}=\mathrm{arctg}[{\left(\frac{H+R_b}{H+R_a}\right)}^2\·\mathrm{tgB}]---\left(1\right)$

$B_{\mathrm{EC}0}=\mathrm{arctg}[{\left(\frac{H+R_b}{H+R_a}\right)}^2\·\mathrm{tg}{B}_0]---\left(2\right)$

$B_{\mathrm{EC}1}=\mathrm{arctg}[{\left(\frac{H+R_b}{H+R_a}\right)}^2\·\mathrm{tg}{B}_1]---\left(3\right)$

上式中依据WGS_84坐标系，地球长半轴R_a＝6378137.0m，地球短半轴R_b＝6356752.3m。

步骤二：根据步骤一中得到的无人机的地心纬度B_EC、航线从点地心纬度B_EC0、航线到点地心纬度B_EC1以及无人机的地理经度L、航线从点(L₀，B₀)地理经度L₀、航线到点(L₁，B₁)地理经度L₁、无人机的高度H信息，由式(4)、(5)、(6)计算无人机当前位置点在WGS_84坐标系中的天向分量k，航线从点在WGS_84坐标系中的天向分量k₀，航线到点在WGS_84坐标系中的天向分量k₁。

$k=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}}\right)\·\cos \left(L\right)\\ \cos \left(B_{\mathrm{EC}}\right)\·\sin \left(L\right)\\ \sin \left(B_{\mathrm{EC}}\right)\end{array}\right]---\left(4\right)$

$k_0=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}0}\right)\·\cos \left(L_0\right)\\ \cos \left(B_{\mathrm{EC}0}\right)\·\sin \left(L_0\right)\\ \sin \left(B_{\mathrm{EC}0}\right)\end{array}\right]---\left(5\right)$

$k_1=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}1}\right)\·\cos \left(L_1\right)\\ \cos \left(B_{\mathrm{EC}1}\right)\·\sin \left(L_1\right)\\ \sin \left(B_{\mathrm{EC}1}\right)\end{array}\right]---\left(6\right)$

步骤三：根据步骤二中得到的天向分量k、k₀、k₁以及无人机当前地理纬度B，由式(7)计算侧偏距D_z。

$D_z=R_a\·\{1+\frac{f^2}{2}\·[\frac{1}{1+(1-f^2)\·\mathrm{tgB}}-1\left]\right\}(\frac{\mathrm{\π}}{2}-\mathrm{\α})---\left(7\right)$

依据WGS_84坐标系，式中的地球椭偏度f＝0.003352811；

$\mathrm{\α}=\arccos [\left(\frac{k_1\×k_0}{|k_1\×k_0|}\right)\·k].$

步骤四：根据无人机当前位置(L，B)、航线从点(L₀，B₀)、航线到点(L₁，B₁)信息，由式(8)、(9)、(10)、计算无人机当前点在当地水平坐标系中的北向分量j，航线从点在当地水平坐标系中的天向分量k₂，航线到点在当地水平坐标系中的天向分量k₃：

$j=\left[\begin{array}{c}-\sin \left(B\right)\·\cos \left(L\right)\\ -\sin \left(B\right)\·\sin \left(L\right)\\ \cos \left(B\right)\end{array}\right]---\left(8\right)$

$k_2=\left[\begin{array}{c}\cos \left(B_0\right)\·\cos \left(L_0\right)\\ \cos \left(B_0\right)\·\sin \left(L_0\right)\\ \sin \left(B_0\right)\end{array}\right]---\left(9\right)$

$k_3=\left[\begin{array}{c}\cos \left(B_1\right)\·\cos \left(L_1\right)\\ \cos \left(B_1\right)\·\sin \left(L_1\right)\\ \sin \left(B_1\right)\end{array}\right]---\left(10\right)$

步骤五：根据步骤四中得到的无人机当前点在当地水平坐标系中的北向分量j，航线从点在当地水平坐标系中的天向分量k₂，航线到点在当地水平坐标系中的天向分量k₃，结合无人机的北向地速V_dn，东向地速V_de，由式(11)计算侧偏移速度D_Zd：

其中，

步骤六：将步骤四及步骤五中得到的飞机的侧偏距D_Z及侧偏移速度D_Zd输出至侧向控制回路得到舵偏度指令，由舵回路根据舵偏度指令控制无人机飞行，最终实现引导飞机沿着期望航线飞行。

本发明提出的无人机侧向领航方法的优点在于：(1)根据无人机的位置信息以及无人机的期望航线得到无人机的侧偏距以及侧偏移速度，能够引导无人机沿期望航线飞行，在常值侧风干扰下为控制稳态误差的消除提供前提；(2)本方法在计算过程中考虑了地球为椭球体，计算结果精度高，适用于对于精度要求较高的无人机领航。

附图说明

图1是现有的无人机侧向领航控制系统工作原理示意图；

图2是本发明的无人机侧向领航控制系统工作原理示意图；

图3是不同垂线和纬度之间的关系示意图；

图4是B₁＞B₀时侧偏距、侧偏移速度及地速的几何关系示意图；

图5是B₁＜B₀时侧偏距、侧偏移速度及地速的几何关系示意图。

具体实施方式

下面结合附图对本发明的无人机侧向领航方法作进一步说明。

本发明的目的是提出一种无人机侧向领航方法，根据无人机的位置信息以及期望航线得到无人机的侧偏距以及侧偏移速度，进而引导无人机沿期望航线飞行。本发明的无人机侧向领航方法控制系统的工作原理如图2所示。本发明的侧向领航方法根据期望航线以及由传感器获得的无人机位置、高度及地速信息，连续实时计算飞机相对于期望航线的侧偏距及侧偏移速度等领航参数，并同由姿态运动传感器获得的飞机姿态运动信息一同输入至侧向控制回路得到舵偏度指令，由舵回路根据舵偏度指令控制无人机飞行，最终实现引导无人机沿着期望航线飞行。

为了描述无人机相对于地球的位置，进行无人机的导航定位，首先需要进行参考椭球系的选取。由于卫星技术和遥测技术的发展，目前已可利用卫星测量的办法取得全球性的大地测量数据，从而拟和出全球大地坐标系。WGS_84坐标系正是于1984年制定的全球大地坐标系，适应于全球定位。选取WGS_84坐标系，即确定了地球长半轴R_a、短半轴R_b和地球椭偏度f参数，依据该坐标系，地球长半轴R_a＝6378137.0m，短半轴R_b＝6356752.3m，地球椭偏度f＝0.003352811。依据如下步骤计算无人机的侧偏距D_Z及侧偏移速度D_Zd：

步骤一：计算无人机的地心纬度B_EC、航线从点地心纬度B_EC0、航线到点地心纬度B_EC1。

根据无人机当前地理纬度B，航线从点地理纬度B₀，航线到点地理纬度B₁以及无人机的高度H，可以计算无人机的地心纬度B_EC、航线从点地心纬度B_EC0、航线到点地心纬度B_EC1，如式(1)、(2)、(3)：

$B_{\mathrm{EC}}=\mathrm{arctg}[{\left(\frac{H+R_b}{H+R_a}\right)}^2\·\mathrm{tgB}]---\left(1\right)$

$B_{\mathrm{EC}0}=\mathrm{arctg}[{\left(\frac{H+R_b}{H+R_a}\right)}^2\·\mathrm{tg}{B}_0]---\left(2\right)$

$B_{\mathrm{EC}1}=\mathrm{arctg}[{\left(\frac{H+R_b}{H+R_a}\right)}^2\·\mathrm{tg}{B}_1]---\left(3\right)$

地球表面某点常用的垂线和纬度有：地理垂线和地理纬度、地心垂线和地心纬度。其中，地理垂线是指参考椭球上某点处的法线，地理垂线与赤道平面的夹角为地理纬度；地心垂线是指参考椭球上某点到地球中心的连线，地心垂线与赤道平面的夹角为地心纬度，如图3所示。设无人机当前位置为C，地心为O，图3中CA为地理垂线，地理纬度为B，CO为地心垂线，地心纬度为B_EC。

步骤二：计算无人机当前位置C在WGS_84坐标系中的天向分量k，航线从点(L₀，B₀)在WGS_84坐标系中的天向分量k₀，航线到点(L₁，B₁)在WGS_84坐标系中的天向分量k₁。

根据无人机的地心纬度B_EC、地理经度L、航线从点地心纬度B_EC0、航线从点地理经度L₀、航线到点地心纬度B_EC1、航线到点地理经度L₁以及无人机的高度H，可以计算得到无人机当前位置C在WGS_84坐标系中的天向分量k，航线从点(L₀，B₀)在WGS_84坐标系中的天向分量k₀，航线到点(L₁，B₁)在WGS_84坐标系中的天向分量k₁，如式(4)、(5)、(6)：

$k=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}}\right)\·\cos \left(L\right)\\ \cos \left(B_{\mathrm{EC}}\right)\·\sin \left(L\right)\\ \sin \left(B_{\mathrm{EC}}\right)\end{array}\right]---\left(4\right)$

$k_0=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}0}\right)\·\cos \left(L_0\right)\\ \cos \left(B_{\mathrm{EC}0}\right)\·\sin \left(L_0\right)\\ \sin \left(B_{\mathrm{EC}0}\right)\end{array}\right]---\left(5\right)$

$k_1=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}1}\right)\·\cos \left(L_1\right)\\ \cos \left(B_{\mathrm{EC}1}\right)\·\sin \left(L_1\right)\\ \sin \left(B_{\mathrm{EC}1}\right)\end{array}\right]---\left(6\right)$

步骤三：计算侧偏距D_z。

根据步骤二中所求得的无人机天向分量k、k₀、k₁，以及无人机当前地理纬度B，计算侧偏距D_z。

$D_z=R_a\·\{1+\frac{f^2}{2}\·[\frac{1}{1+(1-f^2)\·\mathrm{tgB}}-1\left]\right\}(\frac{\mathrm{\π}}{2}-\mathrm{\α})---\left(7\right)$

其中， $\mathrm{\α}=\arccos [\left(\frac{k_1\×k_0}{|k_1\×k_0|}\right)\·k].$

步骤四：计算无人机当前位置C在当地水平坐标系中的北向分量j，航线从点在当地水平坐标系中的天向分量k₂，航线到点在当地水平坐标系中的天向分量k₃。

根据无人机当前地理纬度B、地理经度L、航线从点地理纬度B₀、航线从点地理经度L₀、航线到点地理纬度B₁、航线到点地理经度L₁，可以计算无人机当前点在当地水平坐标系中的北向分量j，航线从点在当地水平坐标系中的天向分量k₂，航线到点在当地水平坐标系中的天向分量k₃：

$j=\left[\begin{array}{c}-\sin \left(B\right)\·\cos \left(L\right)\\ -\sin \left(B\right)\·\sin \left(L\right)\\ \cos \left(B\right)\end{array}\right]---\left(8\right)$

$k_2=\left[\begin{array}{c}\cos \left(B_0\right)\·\cos \left(L_0\right)\\ \cos \left(B_0\right)\·\sin \left(L_0\right)\\ \sin \left(B_0\right)\end{array}\right]---\left(9\right)$

$k_3=\left[\begin{array}{c}\cos \left(B_1\right)\·\cos \left(L_1\right)\\ \cos \left(B_1\right)\·\sin \left(L_1\right)\\ \sin \left(B_1\right)\end{array}\right]---\left(10\right)$

步骤五：计算侧偏移速度D_Zd。

图4及图5分别给出了B₁＞B₀和B₁＜B₀时无人机相对于期望航线的侧偏距、侧偏移速度及地速的几何关系示意图。其中OX_gY_g为当地水平坐标系，其原点O位于无人机质心，OX_g轴指向当地正北，OY_g轴指向当地正东；V_d表示地速；D_z表示侧偏距，定义无人机在期望航线的欲飞方向的右侧为正；D_Zd为侧偏移速度，定义使D_z正向增加时为正；

为当地水平坐标系内，垂直于当前航线，沿侧偏距D_z增大的方向与北方向之间的夹角。

根据无人机当前位置点在当地水平坐标系中的北向分量j，航线从点(L₀，B₀)在当地水平坐标系中的天向分量k₂，航线到点(L₁，B₁)在当地水平坐标系中的天向分量k₃，可以计算夹角

由图4可以看出B₁＞B₀时，侧偏移速度D_Zd与无人机的北向地速V_dn，东向地速V_de，以及夹角

的关系为：

由图5可以看出B₁＜B₀时，侧偏移速度D_Zd与无人机的北向地速V_dn，东向地速V_de，以及夹角

的关系为：

步骤六：将步骤四及步骤五中得到的飞机的侧偏距D_Z及侧偏移速度D_Zd输出至侧向控制回路得到舵偏度指令，由舵回路根据舵偏度指令来控制无人机飞行，最终实现引导无人机沿着期望航线飞行。

本发明提供的侧向领航方法根据期望航线以及由传感器获得的无人机位置、高度、地速信息，连续实时计算无人机相对于期望航线的侧偏距D_Z及侧偏移速度D_Zd等领航参数，并同由姿态运动传感器获得的无人机姿态运动信息一同输入至侧向控制回路得到舵偏度指令，由舵回路根据舵偏度指令来控制无人机飞行，最终实现引导无人机沿着期望航线飞行。本发明的领航方法在常值侧风干扰下为控制稳态误差的消除提供前提，并在计算过程中考虑了地球为椭球体，所得到的领航精度高，适用于对于精度要求较高的无人机领航。

Claims (1)

1.一种无人机侧向领航方法，其特征在于：

步骤一、根据无人机的当前地理纬度B、航线从点(L₀，B₀)的地理纬度B₀、航线到点(L₁，B₁)的地理纬度B₁以及无人机的高度H信息，计算无人机的地心纬度B_EC、航线从点地心纬度B_EC0、航线到点地心纬度B_EC1：

$B_{\mathrm{EC}}=\mathrm{arctg}[{\left(\frac{H+R_b}{H+R_a}\right)}^2\·\mathrm{tgB}]$

$B_{\mathrm{EC}0}=\mathrm{arctg}[{\left(\frac{H+R_b}{H+R_a}\right)}^2\·\mathrm{tg}{B}_0]$

$B_{\mathrm{EC}1}=\mathrm{arctg}[{\left(\frac{H+R_b}{H+R_a}\right)}^2\·\mathrm{tg}{B}_1]$

其中地球长半轴R_a＝6378137.0m，地球短半轴R_b＝6356752.3m；

步骤二、根据步骤一中得到的无人机的地心纬度B_EC、航线从点地心纬度B_EC0、航线到点地心纬度B_EC1结合无人机的地理经度L、航线从点(L₀，B₀)、航线到点(L₁，B₁)、无人机的高度H信息，计算无人机当前位置点在WGS_84坐标系中的天向分量k，航线从点在WGS_84坐标系中的天向分量k₀，航线到点在WGS_84坐标系中的天向分量k₁：

$k=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}}\right)\·\cos \left(L\right)\\ \cos \left(B_{\mathrm{EC}}\right)\·\sin \left(L\right)\\ \sin \left(B_{\mathrm{EC}}\right)\end{array}\right]$

$k_0=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}0}\right)\·\cos \left(L_0\right)\\ \cos \left(B_{\mathrm{EC}0}\right)\·\sin \left(L_0\right)\\ \sin \left(B_{\mathrm{EC}0}\right)\end{array}\right]$

$k_1=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}1}\right)\·\cos \left(L_1\right)\\ \cos \left(B_{\mathrm{EC}1}\right)\·\sin \left(L_1\right)\\ \sin \left(B_{\mathrm{EC}1}\right)\end{array}\right];$

步骤三、根据步骤二中得到的天向分量k、k₀、k₁结合无人机当前地理纬度B，计算侧偏距D_z：

$D_z=R_a\·\{1+\frac{f^2}{2}\·[\frac{1}{1+(1-f^2)\·\mathrm{tgB}}-1\left]\right\}(\frac{\mathrm{\π}}{2}-\mathrm{\α})$

依据WGS_84坐标系，式中的地球椭偏度f＝0.003352811；

步骤四、根据无人机当前位置(L，B)、航线从点(L₀，B₀)、航线到点(L₁，B₁)信息，计算无人机当前点在当地水平坐标系中的北向分量j，航线从点在当地水平坐标系中的天向分量k₂，航线到点在当地水平坐标系中的天向分量k₃：

$j=\left[\begin{array}{c}-\sin \left(B\right)\·\cos \left(L\right)\\ -\sin \left(B\right)\·\sin \left(L\right)\\ \cos \left(B\right)\end{array}\right]$

$k_2=\left[\begin{array}{c}\cos \left(B_0\right)\·\cos \left(L_0\right)\\ \cos \left(B_0\right)\·\sin \left(L_0\right)\\ \sin \left(B_0\right)\end{array}\right]$

$k_3=\left[\begin{array}{c}\cos \left(B_1\right)\·\cos \left(L_1\right)\\ \cos \left(B_1\right)\·\sin \left(L_1\right)\\ \sin \left(B_1\right)\end{array}\right];$

步骤五、根据步骤四中得到的无人机当前点在当地水平坐标系中的北向分量j，航线从点在当地水平坐标系中的天向分量k₂，航线到点在当地水平坐标系中的天向分量k₃，结合无人机的北向地速V_dn，东向地速V_de，计算侧偏移速度D_Zd：

其中，

步骤六：将步骤四及步骤五中得到的飞机的侧偏距D_Z及侧偏移速度D_Zd输出至侧向控制回路得到舵偏度指令，由舵回路根据舵偏度指令控制无人机飞行，最终实现引导飞机沿着期望航线飞行。

Images