CN103019249A - 一种应用在无人机上的提高导航解算精度的方法 - Google Patents

Abstract

本发明公开了一种应用在无人机上的提高导航解算精度的方法，属于航空自动化控制领域，具体包括以下几个步骤：步骤一：获取无人机的从点、到点与当前位置信息；步骤二：将从点、到点和当前位置的40位浮点类型的经度、纬度、高度数据进行拆分，得到40位浮点类型数据的三角函数方程；步骤三：根据步骤一得到的无人机的从点、到点与当前位置信息，获取侧偏距和侧偏移速度，根据侧偏距和侧偏移速度对无人机跟踪航线进行纠偏；本发明运用40位浮点类型的导航解算可以减小运算过程中产生的截断误差，提高计算精度。当DGPS的定位精度为0.1m时，40位浮点类型的导航解算的侧偏距输出精度能达到0.3m，能满足精确制导、起飞和着陆控制。

Description

一种应用在无人机上的提高导航解算精度的方法

技术领域

本发明属于航空自动化控制领域，具体涉及一种应用在无人机上的提高导航解算精度的方法。

背景技术

DSP芯片，即数字信号处理芯片，也称信号处理器，是一种特别适合于进行数字信号处理运算的微处理器，其主要应用是实时快速地实现各种数字信号处理算法。最成功的DSP芯片当数美国德州仪器公司（Texas Instruments，简称TI）的一系列产品。TI公司已经成为世界上最大的DSP芯片供应商，其DSP市场占有量占全球总额的近50%。

因为TI公司的TMS320系列通用DSP芯片具有接口方便、编程方便、稳定性好、精度高、可重复性好、集成方便等优点，数字信号处理能力、速度都能满足无人机飞行控制的要求，成为无人机机载飞行控制与管理计算机的首选。

TMS320系列的提供32位单精度浮点格式和40位扩展精度浮点格式两种浮点格式。由于用的最多的是单精度浮点格式，TMS320系列DSP开发平台的库函数仅适用于32位单精度浮点格式，而40位扩展精度浮点格式的仅是处理器支持的基本四则运算。

但是，当DGPS的定位精度为0.1m时，使用32位浮点类型库函数进行导航解算侧偏距时仅能达到3m左右的灵敏度。因此，为提高无人机起飞着陆精度，需要考虑利用40位浮点类型来进行导航解算。

发明内容

本发明的目的是为了解决上述问题，提出一种基于TMS320系列DSP开发平台应用于无人机上的提高导航解算精度的方法。导航解算输出侧偏距和侧偏移速度，根据侧偏距和侧偏移速度的进行纠偏控制，达到控制无人机跟踪航线的目的。侧偏距的计算精度影响航线跟踪的精准度，而侧偏移速度的计算精度影响航线跟踪的快速性与稳定性。

一种应用在无人机上的提高导航解算精度的方法，包括以下几个步骤：

步骤一：获取无人机的从点、到点与当前位置信息；

飞机的从点、到点、当前位置各包含经度、纬度与高度信息，通过导航设备，获取无人机的从点、到点与当前位置信息，信息为40位浮点类型的数据信息，40位浮点类型简称为Double40类型；

步骤二：将从点、到点和当前位置的40位浮点类型的经度、纬度、高度数据进行拆分，得到40位浮点类型数据的三角函数方程；

DSP芯片中32位浮点类型标识为double，40位浮点类型标识为longdouble，将32位浮点类型的数值标识为Double32，将40位浮点类型标识为Double40；

设经度或者纬度或者高度的40位浮点类型数据为x，按x＝x₀+dx将数据拆分成小数点5位以前的数与小数点5位后的数，其中x₀表示小数点5位以前的数，为Double32类型，dx表示小数点5位后的数，由Double40类型转存储为Double32；

则有：

f(x)＝f(x₀)+f′(x₀)dx      （4）

其中，f(x)表示Double40类型参数x的函数，x₀为Double32类型参数，f(x₀)表示Double32类型参数x₀的函数，f(x₀)、f′(x₀)直接使用Double32类型库函数，实现Double40数据类型函数f(x)的计算；

当f(x)为三角函数时，根据式（4），由Double32类型三角函数计算Double40类型三角函数，具体为：

cos(x)＝(Double40)(cos((Double32)x₀))-dx×(Double40)sin((Double32)x₀)

sin(x)＝(Double40)(sin((Double32)x₀))+dx×(Double40)cos((Double32)x₀)

tg(x)＝(Double40)(tg((Double32)x₀))+dx/temp                                 (5)

arctg(x)＝(Double40)arctg((Double32)x₀)+dx/(1.0+x₀×x₀)

arccos(x)＝(Double40)arccos((Double32)x₀)-dx/((Double40)sqrt((Double32)(1.0-x₀×x₀)))

步骤三：根据步骤一得到的无人机的从点、到点与当前位置信息，获取侧偏距和侧偏移速度，根据侧偏距和侧偏移速度对无人机跟踪航线进行纠偏；

根据步骤一得到的无人机的从点、到点与当前位置信息，步骤二得到从点、到点和当前位置的40位浮点类型的经度、纬度、高度数据的三角函数方程，导航解算出侧偏距和侧偏移速度，根据侧偏距和侧偏移速度对无人机跟踪航线进行纠偏。

本发明的优点在于：

本发明运用40位浮点类型的导航解算可以减小运算过程中产生的截断误差，提高计算精度。在当DGPS的定位精度为0.1m时，40位浮点类型的导航解算的侧偏距输出精度能达到0.3m，从而能满足精确制导、起飞和着陆控制。

附图说明

图1为本发明的侧偏距示意图；

图2为本发明的方法流程图。

具体实施方式

下面结合附图和实施例对本说明进行详细说明。

无人机飞行航线由若干个航点连接而成，当无人机从航点m飞向航点n时，称无人机在航段m→n上飞行，航点m为当前无人机的从点，航点n为当前无人机的到点。如图1所示，从点与到点为当前无人机的从点和到点在水平面上的投影，那么当前飞机位置在水平面的投影与从点→到点的连线的垂直距离，称为侧偏距，飞机位于航线右侧时侧偏距为正。

一般导航解算方法中包含大量的三角和反三角运算，现有使用的TMS320系列DSP提供32位单精度浮点格式和40位扩展精度浮点格式两种浮点格式，其开发平台提供了32位浮点类型三角库函数，而对于40位浮点类型只能做简单的四则运算。如果直接采用DSP开发平台提供的32位浮点类型三角库函数进行导航解算，截断误差较大，导致导航精度仅能达到3m左右。为提高导航解算的精度，根据单变量函数的泰勒级数展开原理，在32位浮点类型库函数基础上，推算出40位浮点类型数据的三角和反三角运算的近似值。

本发明的一种应用在无人机上的提高导航解算精度的方法，流程如图2所示，包括以下几个步骤：

步骤一：获取无人机的从点、到点与当前位置信息；

从点与到点的连线为飞机的应飞航线，预先装订存储在DSP上。飞机的当前位置由导航设备（如DGPS等）实施测量输出。飞机的从点、到点、当前位置各包含经度、纬度与高度信息，获取无人机的从点、到点与当前位置信息，信息为Double40类型（40位浮点类型）的数据信息。由图1可知，从点、到点、当前位置一起构成计算侧偏距的完全信息。

步骤二：将从点、到点和当前位置的40位浮点类型的经度、纬度、高度数据进行拆分，得到40位浮点类型数据的三角函数方程。

本发明利用32位浮点类型三角库函数得到40位浮点类型三角库函数，具体过程为：

根据泰勒级数展开原理，单变量函数的泰勒公式：如果函数f(x)满足条件（f(x)表示Double40类型参数x的函数，）：(i)在点a的某邻域|x-a|＜δ内有定义，δ表示任意小的正数，(ii)在此邻域内有一直到n-1阶的导数f'(x),…,f^n-1(x)，n表示大于1的正整数，(iii)在点a处有n阶导数fⁿ(a)，那么f(x)在点a的邻域内可表示成以下各种形式：

$f\left(x\right)=f(a+h)=f\left(a\right)+f^{\′}\left(a\right)h+\frac{1}{2!}{f}^{\′\′}\left(a\right)h^2+\·\·\·+\frac{1}{n!}{f}^{\left(n\right)}\left(a\right)h^n+o\left(h^n\right)---\left(2\right)$

$=\underset{k=0}{\overset{n}{\mathrm{\Σ}}}\frac{1}{k!}{f}^{\left(k\right)}\left(a\right)h^k+o\left(h^n\right)$ （当h→0）

DSP芯片中32位浮点类型标识为double，40位浮点类型标识为longdouble。为了容易区分，本发明中将32位浮点类型的数值标识为Double32，将40位浮点类型标识为Double40。

步骤一中获取了无人机的从点、到点与当前位置中，经度、纬度与高度的Double40类型数据，设经度或者纬度或者高度的40位浮点类型数据为x，按x＝x₀+dx将数据拆分成小数点5位以前的数与小数点5位后的数，其中x₀表示小数点5位以前的数，为Double32类型参数，dx表示小数点5位后的数。由Double40类型转存储为Double32，也不会有精度损失或截断。

例如：假设x=1.234567891，则取x₀=1.23456，dx=0.000007891。

由式（2），可得：

$f\left(x\right)=f(x_0+\mathrm{dx})=f\left(x_0\right)+f^{\′}\left(x_0\right)\mathrm{dx}+\frac{1}{2!}{f}^{\′\′}\left(x_0\right){\mathrm{dx}}^2+\·\·\·+\frac{1}{n!}{f}^{\left(n\right)}\left(x_0\right){\mathrm{dx}}^n+o\left({\mathrm{dx}}^n\right)---\left(3\right)$

其中，由于dx为小数点5位后的有效数值，那么从计算精度的需要来说dx²,…,dxⁿ都是极小量，可忽略不计，因此根据公式(3)推理Double40类型参数x的函数f(x)时直接将第三项和第三项之后忽略。

式（3）简化为：

f(x)＝f(x₀+dx)＝f(x₀)+f′(x₀)dx        （4）

在式（4）中，x₀是Double32类型参数，f(x₀)表示Double32类型参数x₀的函数，故f(x₀)、f'(x₀)都可直接使用Double32类型库函数，从而实现Double40数据类型函数f(x)的计算。

根据高等数学，三角函数都是连续可微的，符合函数f(x)的条件。根据式（4），可由Double32类型三角函数计算Double40类型三角函数。那么，Double40类型的参数x的余弦cos、正弦sin、正切tg和反三角运算反正切arctg、反余弦arccos可表示：

cos(x)＝(Double40)(cos((Double32)x₀))-dx×(Double40)sin((Double32)x₀)

sin(x)＝(Double40)(sin((Double32)x₀))+dx×(Double40)cos((Double32)x₀)

tg(x)＝(Double40)(tg((Double32)x₀))+dx/temp                                  (5)

arctg(x)＝(Double40)arctg((Double32)x₀)+dx/(1.0+x₀×x₀)

arccos(x)＝(Double40)arccos((Double32)x₀)-dx/((Double40)sqrt((Double32)(1.0-x₀×x₀)))

步骤三：根据步骤一得到的无人机的从点、到点与当前位置信息，获取侧偏距和侧偏移速度，根据侧偏距和侧偏移速度对无人机跟踪航线进行纠偏；

根据步骤一得到的无人机的从点、到点与当前位置信息，步骤二得到从点、到点和当前位置的40位浮点类型的经度、纬度、高度数据的三角函数方程，导航解算出侧偏距和侧偏移速度，根据侧偏距和侧偏移速度对无人机跟踪航线进行纠偏。

其中，侧偏距的一种获取方法为：

获取侧偏距Dz的方法具体为：

式中：地球长半轴R_长＝6378137.01，B表示当前位置的纬度，地球椭偏度E_D＝0.003352811(1/298.25722)，

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

其中：

k₁——折算为地心纬度的到点在地球坐标系中的天向分量；

k₁＝[k₁[0] k₁[1] k₁[2]]，k₁[0]、k₁[1]、k₁[2]为k₁在地球坐标系中的三轴坐标值；

k₀——折算为地心纬度的从点在地球坐标系中的天向分量；

k₀＝[k₀[0] k₀[1] k₀[2]]，k₀[0]、k₀[1]、k₀[2]为k₀在地球坐标系中的三轴坐标值；

k——折算为地心纬度的当前点在地球坐标系中的天向分量；

k＝[k[0]k[1]k[2]]，k[0]、k[1]、k[2]为k在地球坐标系中的三轴坐标值；

k₁[0]＝cos(B_EC1)·cos(L₁)

k₁[1]＝cos(B_EC1)·sin(L₁)

k₁[2]＝sin(B_EC1)

k₀[0]＝cos(B_EC0)·cos(L₀)

k₀[1]＝cos(B_EC0)·sin(L₀)

k₀[2]＝sin(B_EC0)

k[0]＝cos(B_EC)·cos(L)

k[1]＝cos(B_EC)·sin(L)

k[2]＝sin(B_EC)

其中：L₁、L₀、L分别为到点、从点、当前位置的经度。

式中：H表示当前位置的地理高度，B₁、B₀、B分别为到点、从点、当前位置的纬度。

B_EC——当前点地心纬度，rad；

B_EC0——从点地心纬度，rad；

B_EC1——到点地心纬度，rad；

R_短——地球短半轴，大小为6356752.3m。

由式（1）可知，侧偏距计算过程包含三角运算余弦cos、正弦sin、正切tg和反三角运算反正切arctg、反余弦arccos。

式（1）中所有三角函数运算和四则运算都是Double40类型的运算，三角函数的Double40参数都经过步骤二的拆分，按式（5）实现。将Double40类型的三角函数和反三角函数引入公式（1），计算侧偏距的全部运算过程是Double40类型之间的混合运算，能有效减小截断误差，达到最佳解算精度。

侧偏移速度D_Zd的一种获取方法为：

式中：

V_dn——当前北向地速；

V_de——当前东向地速；

其中

k₃——到点在地球坐标系中的天向分量；

k₃＝[k₃ [0] k₃[1] k₃[2]]，k₃[0]、k₃[1]、k₃[2]为k₃在地球坐标系中的三轴坐标值；

k₂——从点在地球坐标系中的天向分量；

k₂＝[k₂[0] k₂[1] k₂[2]]，k₂[0]、k₂[1]、k₂[2]为k₂在地球坐标系中的三轴坐标值；

j——当前点在地球坐标系中的北向分量；

j＝[j[0] j[1] j[2]]，j[0]、j[1]、j[2]为j在地球坐标系中的三轴坐标值；

k₂[0]＝cosB₀·cosL₀

k₂[1]＝cosB₀·sinL₀

k₂[2]＝sinB₀

k₃[0]＝cosB₁·cosL₁

k₃[1]＝cosB₁·sinL₁

k₃[2]＝sinB₁

j[0]＝-sinB·cosL

j[1]＝-sinB·sinL

j[2]＝cosB

由式（2）可知，侧偏移速度计算过程包含三角运算余弦cos、正弦sin、反余弦arccos。

与侧偏距解算类似，式（2）中所有三角函数运算和四则运算都是Double40类型的运算，三角函数的Double40参数都经过步骤二的拆分，按式（5）实现。将Double40类型的三角函数和反三角函数引入公式（2），计算侧偏移速度的全部运算过程是Double40类型之间的混合运算，力图得到最佳解算精度。

导航解算输出侧偏距和侧偏移速度，根据侧偏距和侧偏移速度的进行纠偏控制，达到控制无人机跟踪航线的目的。通过本发明提出的导航解算方法使解算精度达到0.3米，进而使无人机能够自动准确跟踪航线。

Claims (3)

1.一种应用在无人机上的提高导航解算精度的方法，其特征在于，包括以下几个步骤：

步骤一：获取无人机的从点、到点与当前位置信息；

飞机的从点、到点、当前位置各包含经度、纬度与高度信息，通过导航设备，获取无人机的从点、到点与当前位置信息，信息为40位浮点类型的数据信息，40位浮点类型简称为Double40类型；

步骤二：将从点、到点和当前位置的40位浮点类型的经度、纬度、高度数据进行拆分，得到40位浮点类型数据的三角函数方程；

DSP芯片中32位浮点类型标识为double，40位浮点类型标识为longdouble，将32位浮点类型的数值标识为Double32，将40位浮点类型标识为Double40；

设经度或者纬度或者高度的40位浮点类型数据为x，按x＝x₀+dx将数据拆分成小数点5位以前的数与小数点5位后的数，其中x₀表示小数点5位以前的数，为Double32类型，dx表示小数点5位后的数，由Double40类型转存储为Double32；

则有：

f(x)＝f(x₀)+f′(x₀)dx    （4）

其中，f(x)表示Double40类型参数x的函数，x₀为Double32类型参数，f(x₀)表示Double32类型参数x₀的函数，f(x₀)、f′(x₀)直接使用Double32类型库函数，实现Double40数据类型函数f(x)的计算；

当f(x)为三角函数时，根据式（4），由Double32类型三角函数计算Double40类型三角函数，具体为：

cos(x)＝(Double40)(cos((Double32)x₀))-dx×(Double40)sin((Double32)x₀)

sin(x)＝(Double40)(sin((Double32)x₀))+dx×(Double40)cos((Double32)x₀)

tg(x)＝(Double40)(tg((Double32)x₀))+dx/temp                                  (5)

arctg(x)＝(Double40)arctg((Double32)x₀)+dx/(1.0+x₀×x₀)

arccos(x)＝(Double40)arccos((Double32)x₀)-dx/((Double40)sqrt((Double32)(1.0-x₀×x₀)))

步骤三：根据步骤一得到的无人机的从点、到点与当前位置信息，获取侧偏距和侧偏移速度，根据侧偏距和侧偏移速度对无人机跟踪航线进行纠偏；

根据步骤一得到的无人机的从点、到点与当前位置信息，步骤二得到从点、到点和当前位置的40位浮点类型的经度、纬度、高度数据的三角函数方程，导航解算出侧偏距和侧偏移速度，根据侧偏距和侧偏移速度对无人机跟踪航线进行纠偏。

2.根据权利要求1所述的一种应用在无人机上的提高导航解算精度的方法，其特征在于，所述的步骤三中，获取侧偏距的方法为：

获取侧偏距Dz的方法具体为：

式中：R_长表示地球长半轴，E_D表示地球椭偏度，B表示当前位置的纬度；

R_长＝6378137.0m，E_D表示地球椭偏度E_D＝1/298.25722；

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

其中：

k₁——折算为地心纬度的到点在地球坐标系中的天向分量；

k₁＝[k₁[0] k₁[1] k₁[2]]，k₁[0]、k₁[1]、k₁[2]为k₁在地球坐标系中的三轴坐标值；

k₀——折算为地心纬度的从点在地球坐标系中的天向分量；

k₀＝[k₀[0]k₀[1]k₀[2]]，k₀[0]、k₀[1]、k₀[2]为k₀在地球坐标系中的三轴坐标值；

k——折算为地心纬度的当前点在地球坐标系中的天向分量；

k＝[k[0]k[1]k[2]]，k[0]、k[1]、k[2]为k在地球坐标系中的三轴坐标值；

k₁[0]＝cos(B_EC1)·cos(L₁)

k₁[1]＝cos(B_EC1)·sin(L₁)

k₁[2]＝sin(B_EC1)

k₀[0]＝cos(B_EC0)·cos(L₀)

k₀[1]＝cos(B_EC0)·sin(L₀)

k₀[2]＝sin(B_EC0)

k[0]＝cos(B_EC)·cos(L)

k[1]＝cos(B_EC)·sin(L)

k[2]＝sin(B_EC)

其中：L₁、L₀、L分别为到点、从点、当前位置的经度；

其中：H表示当前位置的地理高度，B₁、B₀、B分别为到点、从点、当前位置的纬度；

B_EC——当前点地心纬度，单位rad；

B_EC0——从点地心纬度，单位rad；

B_EC1——到点地心纬度，单位rad；

R_短——地球短半轴，大小为6356752.3m；

式（1）中所有三角函数运算和四则运算都是Double40类型的运算，三角函数的Double40参数经过步骤二的拆分，按式（5）实现；将Double40类型的三角函数和反三角函数引入公式（1），获取侧偏距。

3.根据权利要求1所述的一种应用在无人机上的提高导航解算精度的方法，其特征在于，所述的步骤三中，获取侧偏移速度的方法为：

获取侧偏移速度D_Zd的方法具体为：

式中：B₁、B₀、B分别为到点、从点、当前位置的纬度；

V_dn——当前北向地速；

V_de——当前东向地速；

其中：

k₃——到点在地球坐标系中的天向分量；

k₃＝[k₃[0] k₃[1] k₃[2]]，k₃[0]、k₃[1]、k₃[2]为k₃在地球坐标系中的三轴坐标值；

k₂——从点在地球坐标系中的天向分量；

k₂＝[k₂[0] k₂[1] k₂[2]]，k₂[0]、k₂[1]、k₂[2]为k₂在地球坐标系中的三轴坐标值；

j——当前点在地球坐标系中的北向分量；

j＝[j[0] j[1] j[2]]，j[0]、j[1]、j[2]为j在地球坐标系中的三轴坐标值；

k₂[0]＝cosB₀·cosL₀

k₂[1]＝cosB₀·sinL₀

k₂[2]＝sinB₀

k₃[0]＝cosB₁·cosL₁

k₃[1]＝cosB₁·sinL₁

k₃[2]＝sinB₁

j[0]＝-sinB·cosL

j[1]＝-sinB·sinL

j[2]＝cosB

L₁、L₀、L分别为到点、从点、当前位置的经度；

式（2）中所有三角函数运算和四则运算都是Double40类型的运算，三角函数的Double40参数都经过步骤二的拆分，按式（5）实现；将Double40类型的三角函数和反三角函数引入公式（2），获取侧偏移速度。

Images