CN106569222A - 一种基于测距原理的方位角测量方法 - Google Patents

Abstract

本发明公开了一种基于测距原理的飞行器方位角测量方法，属于飞行器定位技术领域。该方法具体为：1)令A，B为飞行器检测点，A点在飞行器头部，B点在飞行器尾部；P1,P2,P3为均匀分布飞行器四周的为地面随机基准点；测量三个已知点与两个未知点之间的距离，构建关于距离与检测点的方程组：2)求解方程组，得到A，B点相对于发射系的坐标；3)根据A、B点坐标计算飞行器相对于发射坐标系的方位角。本发明测量过程高度自动化，测量周期短，简化了复杂度，提高了精度。

Description

一种基于测距原理的方位角测量方法

技术领域

本发明属于飞行器定位技术领域，具体涉及一种基于测距原理的飞行器方位角测量方法，特别适用于飞行器飞行启动时的初始方位定位。

背景技术

飞行器的导航建立在导航坐标系上，而导航坐标系的建立需要知道飞行器的初始方位角。方位角的误差将引起导航误差，降低飞行器的飞行轨迹精度，所以飞行器的初始方位角对飞行器的飞行过程很重要。

传统的方位角测量方式：工作人员通过目测与棱镜检测来自飞行器上特定点的反射光，通过反射光方向与理论射向夹角，推算飞行器方位角。在烈日等强烈光照的背景下，人眼对光发射的识别力降低，导致人工检测反射光方向的时间存在不确定性。

发明内容

针对现有技术的以上缺陷或改进需求，本发明的目的在于提供一种基于测距原理的飞行器方位角测量方法，测量过程高度自动化，测量周期短，简化了复杂度，提高了精度。

一种基于测距原理的飞行器方位角测量方法，具体为：

1)令A，B为飞行器检测点，A点在飞行器头部，B点在飞行器尾部；P1,P2,P3为均匀分布飞行器四周的为地面随机基准点；三个已知点P1,P2,P3相对于发射坐标系的位置坐标为P₁(x₁,y₁,z₁)、P₂(x₂,y₂,z₂)、P₃(x₃,y₃,z₃)；定义两个未知点A，B相对于发射系坐标分别表示为A(x_a,y_a,z_a)、B(x_b,y_b,z_b)，测量得到的三个已知点与两个未知点之间的距离分别表示为|P₁A|＝r₁、|P₂A|＝r₂、|P₃A|＝r₃、|P₁B|＝r₄、|P₂B|＝r₅、|P₃B|＝r₆，构建方程组(1)和(2)：

2)求解步骤1)构建的方程组，得到A，B点相对于发射系的坐标A(x_a,y_a,z_a)、B(x_b,y_b,z_b)；

3)根据A、B点坐标A(x_a，y_a，z_a)、B(x_b，y_b，z_b)计算飞行器相对于发射坐标系的方位角。

进一步地，所述步骤(2)计算A，B点相对于发射系的坐标A(x_a，y_a，z_a)、B(x_b，y_b，z_b)的方式相同；所述步骤(2)计算A点相对于发射系的坐标A(x_a，y_a，z_a)的具体过程为：

构建(x_a，y_a，z_a)的表达式：

其中：为A点的位置估计值，初值为(0,0,0)；(Δx_a,Δya,Δz_a)为估计值的偏离；

将方程组(1)的单一方程表示为：

其中，i＝1,2,3；

利用A点的坐标估计值计算出一个近似距离：

将公式(3)代入公式(4)，得：

将公式(6)在处泰勒展开，得：

为了消除非线性项，将上述展开公式(7)中截去了一阶偏导数以后的项，各项偏导数为：

将式(5)和式(8)代入式(7)，得到：

令：

将式(11)代入式(10)，得：

Δρ_i＝a_xiΔx_a+a_yiΔy_a+a_ziΔz_a (12)

根据三个已知点构建如下方程组(13)：

将式(13)写成矩阵形式：

即：

Δρ＝HΔx (15)

式(15)的解为：

Δx＝H^-1Δρ (16)

根据用户对精度的要求，为Δx设定门限值：如果Δx超过了门限值，则即以当前计算得到的点坐标(x_a,y_a,z_a)作为新的估计值重复迭代上述过程，直至Δx在门限值以内，得到满足精度要求的A点坐标值，停止迭代过程。

进一步地，所述步骤(3)根据A、B点坐标分别为A(x_a,y_a,z_a)、B(x_b,y_b,z_b)计算飞行器相对于发射坐标系的偏航角ψ为：

设理论射向为Α0，飞行器方位角Α1为：

Α1＝Α0-ψ。

总体而言，通过本发明所构思的以上技术方案与现有技术相比降低对天气环境的苛刻的要求，和工作人员测量技能的高要求。新方法简单快捷，极大地缩小了测试时间。

附图说明

图1是本发明基准点与检测点分布示意图。

图2是本发明涉及到的各角度关系示意图(图中，O为发射点；Ox1为弹体坐标系x轴；Oxg为发射坐标系x轴；ON为地理坐标系的北向；OE为地理坐标系的东向；A0为发射坐标系理论射向；A1为发射坐标系飞行器方位角；ψ为发射坐标系飞行器偏航角)。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明提供的测量方法实现原理介绍如下：

图1中A，B为飞行器检测点；P1,P2,P3为均匀分布飞行器四周的为地面随机基准点.通过表1数据，可推算出飞行器的位置与姿态。

表1已知数据

1位置坐标计算

三个已知点P1,P2,P3相对于发射坐标系的位置坐标为P₁(x₁,y₁,z₁)、P₂(x₂,y₂,z₂)、P₃(x₃,y₃,z₃)，设两个未知点A，B相对于发射系坐标分别为A(x_a,y_a,z_a)、B(x_b,y_b,z_b)，测量得到的三个已知点与两个未知点之间的距离分别为|P₁A|＝r₁、|P₂A|＝r₂、|P₃A|＝r₃、|P₁B|＝r₄、|P₂B|＝r₅、|P₃B|＝r₆，则可得如下方程：

方程组(1)包含A(x_a,y_a,z_a)位置坐标的三个未知数，有三个非线性方程；方程组(2)包含B(x_b,y_b,z_b)位置坐标的三个未知数，有三个非线性方程。

下面介绍使用基于线性化的迭代技术解方程组(1)的方法，方程组(2)的解法同方程组(1)相同。

设

其中：为A点的估计值，初值为(0,0,0)；

(Δx_a,Δy_a,Δz_a)为估计值的偏离。

将方程组(1)的单一方程表示为：

其中，i＝1,2,3。

利用A的估计值可以计算出一个近似距离：

将公式(3)代入公式(4)，得：

将公式(6)在处泰勒展开，得：

为了消除非线性项，上述展开公式中截去了一阶偏导数以后的项。各项偏导数为：

将式(5)和式(8)代入式(7)，得到：

令：

将式(11)代入式(10)，得：

Δρ_i＝a_xiΔx_a+a_yiΔy_a+a_ziΔz_a (12)

因为有三个已知点，所以有如下三个方程：

将式(13)写成矩阵形式：

即：

Δρ＝HΔx (15)

式(15)的解为：

Δx＝H^-1Δρ (16)

根据用户对精度的要求，设定Δx门限值：如果Δx超过了门限值，则重新迭代上述过程，即以算出的点坐标(x_a,y_a,z_a)作为新的估计值重新计算Δx，直至Δx在门限值以内，得到满足精度要求的A点坐标值，停止迭代过程。

2方位角计算

根据上一章位置坐标计算可得A、B点坐标分别为(x_a,y_a,z_a)、(x_b,y_b,z_b)，设A点在飞行器头部，B点在飞行器尾部，理论射向为Α0为已知量。

飞行器相对于发射坐标系的偏航角ψ为：

飞行器方位角Α1为：

Α1＝Α0-ψ。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于测距原理的飞行器方位角测量方法，其特征在于，具体为：

1)令A，B为飞行器检测点，A点在飞行器头部，B点在飞行器尾部；P1,P2,P3为均匀分布飞行器四周的为地面随机基准点；三个已知点P1,P2,P3相对于发射坐标系的位置坐标为P₁(x₁,y₁,z₁)、P₂(x₂,y₂,z₂)、P₃(x₃,y₃,z₃)；定义两个未知点A，B相对于发射系坐标分别表示为A(x_a,y_a,z_a)、B(x_b,y_b,z_b)，测量得到的三个已知点与两个未知点之间的距离分别表示为|P₁A|＝r₁、|P₂A|＝r₂、|P₃A|＝r₃、|P₁B|＝r₄、|P₂B|＝r₅、|P₃B|＝r₆，构建方程组(1)和(2)：

$\begin{array}{c}{r}_1=\sqrt{{(x_1-x_a)}^2+{(y_1-y_a)}^2+{(z_1-z_a)}^2}\\ r_2=\sqrt{{(x_2-x_a)}^2+{(y_2-y_a)}^2+{(z_2-z_a)}^2}\\ r_3=\sqrt{{(x_3-x_a)}^2+{(y_3-y_a)}^2+{(z_3-z_a)}^2}\end{array}---\left(1\right)$

$\begin{array}{c}{r}_4=\sqrt{{(x_1-x_b)}^2+{(y_1-y_b)}^2+{(z_1-z_b)}^2}\\ r_5=\sqrt{{(x_2-x_b)}^2+{(y_2-y_b)}^2+{(z_2-z_b)}^2}\\ r_6=\sqrt{{(x_3-x_a)}^2+{(y_3-y_b)}^2+{(z_3-z_b)}^2}\end{array}---\left(2\right)$

2)求解步骤1)构建的方程组，得到A，B点相对于发射系的坐标A(x_a,y_a,z_a)、B(x_b,y_b,z_b)；

3)根据A、B点坐标A(x_a,y_a,z_a)、B(x_b,y_b,z_b)计算飞行器相对于发射坐标系的方位角。

2.根据权利要求1所述的基于测距原理的飞行器方位角测量方法，其特征在于，所述步骤(2)计算A，B点相对于发射系的坐标A(x_a,y_a,z_a)、B(x_b,y_b,z_b)的方式相同；所述步骤(2)计算A点相对于发射系的坐标A(x_a,y_a,z_a)的具体过程为：

构建(x_a,y_a,z_a)的表达式：

$\begin{array}{c}{x}_a={\hat{x}}_a+{\mathrm{\Δx}}_a\\ y_a={\hat{y}}_a+{\mathrm{\Δy}}_a\\ z_a={\hat{z}}_a+{\mathrm{\Δz}}_a\end{array}---\left(3\right)$

其中：为A点的位置估计值，初值为(0,0,0)；(Δx_a,Δy_a,Δz_a)为估计值的偏离；

将方程组(1)的单一方程表示为：

$\begin{array}{c}{r}_i=\sqrt{{(x_i-x_a)}^2+{(y_i-y_a)}^2+{(z_i-z_a)}^2}\\ =f(x_a,y_a,z_a)\end{array}---\left(4\right)$

其中，i＝1,2,3；

利用A点的坐标估计值计算出一个近似距离：

$\begin{array}{c}{\hat{r}}_i=\sqrt{{(x_i-{\hat{x}}_a)}^2+{(y_i-{\hat{y}}_a)}^2+{(z_i-{\hat{z}}_a)}^2}\\ =f({\hat{x}}_a,{\hat{y}}_a,{\hat{z}}_a)\end{array}---\left(5\right)$

将公式(3)代入公式(4)，得：

$f(x_a,y_a,z_a)=f({\hat{x}}_a+{\mathrm{\Δx}}_a,{\hat{y}}_a+{\mathrm{\Δy}}_a,{\hat{z}}_a+{\mathrm{\Δz}}_a)---\left(6\right)$

将公式(6)在处泰勒展开，得：

$\begin{array}{c}f({\hat{x}}_a+{\mathrm{\Δx}}_a,{\hat{y}}_a+{\mathrm{\Δy}}_a,{\hat{z}}_a+{\mathrm{\Δz}}_a)=f({\hat{x}}_a,{\hat{y}}_a,{\hat{z}}_a)+\\ \frac{\∂f({\hat{x}}_a,{\hat{y}}_a,{\hat{z}}_a)}{\∂{\hat{x}}_a}{\mathrm{\Δx}}_a+\frac{\∂f({\hat{x}}_a,{\hat{y}}_a,{\hat{z}}_a)}{\∂{\hat{y}}_a}{\mathrm{\Δy}}_a+\frac{\∂f({\hat{x}}_a,{\hat{y}}_a,{\hat{z}}_a)}{\∂{\hat{z}}_a}{\mathrm{\Δz}}_a\end{array}---\left(7\right)$

为了消除非线性项，将上述展开公式(7)中截去了一阶偏导数以后的项，各项偏导数为：

$\left\{\begin{array}{c}\frac{\∂f({\hat{x}}_a,{\hat{y}}_a,{\hat{z}}_a)}{\∂{\hat{x}}_a}=-\frac{x_i-{\hat{x}}_a}{{\hat{r}}_i}\\ \frac{\∂f({\hat{x}}_a,{\hat{y}}_a,{\hat{z}}_a)}{\∂{\hat{y}}_a}=-\frac{y_i-{\hat{y}}_a}{{\hat{r}}_i}\\ \frac{\∂f({\hat{x}}_a,{\hat{y}}_a,{\hat{z}}_a)}{\∂{\hat{z}}_a}=-\frac{z_i-{\hat{z}}_a}{{\hat{r}}_i}\end{array}\right.---\left(8\right)$

将式(5)和式(8)代入式(7)，得到：

$r_i={\hat{r}}_i-\frac{x_i-{\hat{x}}_a}{{\hat{r}}_i}{\mathrm{\Δx}}_a-\frac{y_i-{\hat{y}}_a}{{\hat{r}}_i}{\mathrm{\Δy}}_a-\frac{z_i-{\hat{z}}_a}{{\hat{r}}_i}{\mathrm{\Δz}}_a---\left(9\right)$

${\hat{r}}_i-r_i=\frac{x_i-{\hat{x}}_a}{{\hat{r}}_i}{\mathrm{\Δx}}_a+\frac{y_i-{\hat{y}}_a}{{\hat{r}}_i}{\mathrm{\Δy}}_a+\frac{z_i-{\hat{z}}_a}{{\hat{r}}_i}{\mathrm{\Δz}}_a---\left(10\right)$

令：

$\left\{\begin{array}{c}{\mathrm{\Δ\ρ}}_i={\hat{r}}_i-r\\ a_{xi}=\frac{x_i-{\hat{x}}_a}{{\hat{r}}_i}\\ a_{yi}=\frac{y_i-{\hat{y}}_a}{{\hat{r}}_i}\\ a_{zi}=\frac{z_i-{\hat{z}}_a}{{\hat{r}}_i}\end{array}\right.---\left(11\right)$

将式(11)代入式(10)，得：

Δρ_i＝a_xiΔx_a+a_yiΔy_a+a_ziΔz_a (12)

根据三个已知点构建如下方程组(13)：

$\left\{\begin{array}{c}{\mathrm{\Δ\ρ}}_1=a_{x1}{\mathrm{\Δx}}_a+a_{y1}{\mathrm{\Δy}}_a+a_{z1}{\mathrm{\Δz}}_a\\ {\mathrm{\Δ\ρ}}_2=a_{x2}{\mathrm{\Δx}}_a+a_{y2}{\mathrm{\Δy}}_a+a_{z2}{\mathrm{\Δz}}_a\\ {\mathrm{\Δ\ρ}}_3=a_{x3}{\mathrm{\Δx}}_a+a_{y3}{\mathrm{\Δy}}_a+a_{z3}{\mathrm{\Δz}}_a\end{array}\right.---\left(13\right)$

将式(13)写成矩阵形式：

$\mathrm{\Δ}\mathrm{\ρ}=\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\ρ}}_1\\ {\mathrm{\Δ\ρ}}_2\\ {\mathrm{\Δ\ρ}}_3\end{array}\right],H=\left[\begin{array}{ccc}{a}_{x1}& a_{y1}& a_{z1}\\ a_{x2}& a_{y2}& a_{z2}\\ a_{x3}& a_{y3}& a_{z3}\end{array}\right],\mathrm{\Δ}x=\left[\begin{array}{c}\mathrm{\Δ}{x}_a\\ {\mathrm{\Δy}}_a\\ {\mathrm{\Δz}}_a\end{array}\right]---\left(14\right)$

即：

Δρ＝HΔx (15)

式(15)的解为：

Δx＝H^-1Δρ (16)

根据用户对精度的要求，为Δx设定门限值：如果Δx超过了门限值，则即以当前计算得到的点坐标(x_a,y_a,z_a)作为新的估计值重复迭代上述过程，直至Δx在门限值以内，得到满足精度要求的A点坐标值，停止迭代过程。

3.根据权利要求1所述的基于测距原理的飞行器方位角测量方法，其特征在于，所述步骤(3)根据A、B点坐标分别为A(x_a,y_a,z_a)、B(x_b,y_b,z_b)计算飞行器相对于发射坐标系的偏航角ψ为：

$\mathrm{\ψ}=-\arcsin \left(\frac{z_a-z_b}{\sqrt{{(x_a-x_b)}^2+{(y_a-y_b)}^2+{(z_a-z_b)}^2}}\right)$

设理论射向为Α0，飞行器方位角Α1为：

Α1＝Α0-ψ。