第六届华中地区大学生数学建模邀请赛问题1.飞行器空间坐标修正

第六届华中地区大学生数学建模邀请赛

题目：飞行器空间坐标修正

【摘要】

飞行器的导航问题越来越受到人们的重视，它对精度的要求非常之高，我国通过激光引导和北斗导航系统来提高导弹等的定位导航精度然而由于噪声干扰和仪器精度问题使得飞行器的坐标观测值存在一定误差，随着时间的推移，误差逐渐积累会变得越来越大，这会严重影响飞行器的导航精度，因此修正坐标误差非常必要。

本文在分析了各物理量的关系的基础上，经过严密地推导，得出各个轴方向上的位移与其对应速度的修正之后的函数关系，据此建立了相关的数学模型并通过评价说明了模型的合理性和科学性。

对于问题一：噪声信号的干扰对飞行器空间坐标的观测值造成误差，通常采用联邦卡尔曼滤波的方法或者通过坐标变换用雅各比行列式来修正噪声干扰带来的误差。

对于问题二：另外观测数据的仪器误差可以均值法或者迭代均值法来消减。

关键词：联邦卡尔曼，坐标变换，雅各比行列式，迭代均值

目录

一问题重述 (3)

1.1问题背景 (3)

1.2问题提出 (3)

二模型假设 (4)

三符号说明与名词解释 (4)

四问题分析 (4)

4.1问题一分析 (4)

4.2问题二分析 (6)

五模型建立与求解 (7)

5.1问题一模型 (7)

5.2问题二模型 (11)

六模型评价 (13)

七模型的改进 (13)

参考文献 (14)

一问题重述

1.1问题背景

飞行器的导航精度问题一直是航空航天领域研究的重要课题，惯性导航系统是一种不依赖于任何外部信息的自主式导航系统，在航空航天领域起着越来越重要的作用。由于其系统结构误差、惯性测量部件误差、标度系数误差等因素的影响，惯性导航系统的积累误差随着时间的推移而逐渐增大，这一问题严重影响到航空航天技术的发展。目前关于定位精度的研究成果主要是从物理技术(例如红外测距)方面来提高定位的精度，近年来，围绕定位坐标精度问题的相关研究也渐渐展开。因此进一步研究飞行器空间坐标修正方法有重要的理论意义和应用价值。本题的目标是利用数学的方法对飞行器的误差进行修正，并利用结果进行飞行器的仿真。

1.2问题提出

某一观测站测得飞行器空间位置(假设观测站为坐标原点)X(x、y、z)，飞行器的飞行速度V(x轴、y轴、z轴),飞行器与观测站之间的偏向角α，俯仰角θ以及观测数据的时间间隔t。所给的各项数据均含有一定的误差，其中观测站的坐标 (0,0,0)不含误差，飞行器的坐标（观测值）可能含有较大误差。请根据所给数据进行如下工作：问题一：飞行器坐标的数据为观测值，由于电子仪器的精度和噪声干扰等，含有一定的误差波动，建立数学模型对飞行器坐标观测值的随机波动误差进行修正。

问题二：由于观测数据的仪器误差，飞行器坐标在长时间的飞行中，坐标数据的观测值由于误差的累积发生漂移，建立数学模型，对飞行器的坐标的这种误差进行修正。(提示：在短时间内，可以视为飞行器坐标含有一定的常量误差，或者飞行器的这种误差是线性变化的)。

问题三：结合具体的飞行器给出误差修正方案。

二模型假设

假设1.假设噪声的干扰造成的误差可以通过卡尔曼滤波器或者其他方法完全消除。

假设2.观测站的坐标与机体的的坐标系存在有相对运动，而且是球面作用（后边有解释page.11）。

假设3.不考虑在传输过程中的时间延迟效应。

假设4.在使用速度拟合位移曲线的时候假设中间时间间隔做匀速运动。

三符号说明与名词解释

符号解释说明

x,y,h 飞行器空间位置坐标

vx,vy,vh 飞行器沿各个轴方向上的速度

t 观测数据的时间间隔

alpha 飞行器与观测站之间的偏向角

.theta 飞行器与观测站之间的俯仰角

x’,y’，h’坐标变换后飞行器的空间位置坐标

i,j,k 机体采用的坐标系的三个基向量

I’,j’,k’观测站采用的坐标系的三个基向量

四问题分析

4.1问题一分析

首先，惯性导航系统是不依赖于任何外部信息的自主导航系统，明确了数据

vx,vy,vh均是由机体导航系统提供，而x，y，z，以及偏向角（alpha），俯仰角（theta）均由观测站测的

对于问题一：电子仪器精度和噪声的干扰：

2

4

6

800

1000

1200

1400

t

x

x - t 图像

02

46

800

1000

1200

1400

t

y

y - t 图像

1

2

34

5

6

510

510.1510.2510.3

510.4

510.5t

h

h - t 图像

在绘制的x-t ，y-t ，h-t 图像中可以很明显的看出噪声信号对x 和y 的值基本上没

有什么影响，不需要对x,y 进行噪音修正。而h 的值随着t 的变化呈现出上下波动的情况，说明噪声对h 的值影响较大，需要进行修正。消除噪声的干扰可以采用联邦卡尔曼滤波的方法或者使用坐标变换的方法来修正。

4.2问题二分析

观测仪器误差的积累：

246500

1000

1500t x

x - t 图像

2

46

6565.5

66t v x

vx - t 图像

246500

1000

1500t y

y - t 图像

2

46

7070.5

71t v y

vy - t 图像

02

46510

510.5

t

h

h - t 图像

2

46

-1-0.5

0t

v z

vz - t 图像

题设：飞行器的坐标（观测值）可能有较大的误差。

从x-t ，vx-t 图象和y-t ，vy-t 图象中可以看出在x 轴和y 轴方向上都是做类似匀速的运动，速度v 都为正值，不过都有小幅度的波动，说明可能测量过程中存在很小的误差，x ，y 的值都有一定的偏差。

从vz-t 图像中可以看出vz 一直为负值，而且也有小幅度的变化，理论上h 应随着t 时间的推移而呈现出递增趋势，然而根据数据绘制的h-t 图象确实在上下波动的，说明数据中存在偏差，更大可能是所选坐标系不同，导致测量结果的符号不同，需要进行修正。

五模型的建立与求解

5.1问题一模型

针对问题一：由于仪器精度造成的误差无法消除，在这里我们只考虑噪声的干扰，可以采用坐标变换的方法来进行修正。

首先根据所给的数据绘制出x-t ，y-t ，h-t 的图象如下图所示

012

3456

800

10001200

1400r

x

x-t 函数关系图

012

3456

800

10001200

1400t

y

y - t 函数关系图

1

2

34

5

6

510

510.05510.1510.15510.2510.25510.3510.35510.4510.45510.5t h

h-t 折线图

1

2

34

5

6

510510.05510.1510.15510.2510.25510.3

510.35510.4510.45510.5

t/s

h /m

t-h 散点图

data 2

由于仪器精度造成的误差无法消除，在这里我们只考虑噪声的干扰，可以采用迭代均值法，其中（t，x）（t，y）线性拟合后其近似关系很好.

所以第一题的工作重点是对（t，h）进行修正，排除数据传输过程中由于噪音等原因造成的误差，对于这类误差的过滤，卡尔曼过滤器是很好的方法，但是数据较少，用起来误差并不能的到很好的消减.。

于是我们采用以下程序：

设temp：取正如果h>均值，取负如果h<均值（matlab中可用mean指令直接得出）aver则是一个类似于标准差的定义，即误差绝对值的平均数；

由于二者是元素个数相同的向量故用（.*）

H’=h-temp.*aver;

其matlab中函数的实现代码如下

function [ h2] = nihe( h )

%UNTITLED4 Summary of this function goes here

% Detailed explanation goes here

N=41;

temp=(h-mean(h))./abs(h-mean(h));

aver=(sum(abs(h-mean(h))))/N;

h2=h-temp.*aver;

End

然后对新产生的H值绘图，二者对比效果很明显，误差大大的降低了

01

2

34

5

6

510510.05

510.1510.15510.2510.25510.3

510.35510.4510.45510.5t/s

h /s

修正前t-h 关系图修正后t-h 关系图

上图为采用公式迭代一次后结果的的折线图，为了更加精确的过滤误差值，可以采用多次迭代的方法，下图为迭代三次的结果图：

1

2

34

5

6

510510.05510.1510.15510.2510.25510.3510.35510.4

510.45510.5时间t/s

h 即拟合值/h /h 2/h 3/h 4

h h2h3h4

5.2问题二模型

根据模型假设知，（x ，y ，z ）与（vx ，vy ，vh ）的测量，是在两个有相对运动的坐标系中测量的，

可以假想一下观测站的坐标系即为一个静止的球，而观测站在球心处，而机体就在一个球面上但是他的坐标系永远是面朝外的，而且随着它的相对于观测站的运动不断的变换自己的坐标使得总能保持它面朝外（此处的面朝外可以理解为坐标系里的（1，1，1）向量总是与圆心共线只想背离球心的方向） 可以用下式子表示

3（i+j+k ）/3=)' sin j' sin cos i' cos *(cos k αβαβα

（其中(i,j,k ）为机体所采用坐标系三个基向量（i ’,j ’ ,k ’）是观测站采用的坐标系的三个基向量，βα 分别为从观测站对机体的俯仰角，偏向角，前提是两个z 轴始终共面）

我们可以采用三维坐标中的坐标变换----雅各比行列式设 i=theta ，j=alpha (其中雅各比行列式如下)

sin(i)*cos(j) sin(i)*cos(j) cos(i) ; J= cos(i)*cos(j) cos(i)*sin(j) -sin(i); -sin(j) cos(j) 0

而后用v=[vx vy vh]将三个方向上的速度化为一个各时刻个方向的3×41矩阵 V ’=J*V （V ’即为还算过得速度公式） 利用 ?S=V ’*

t 得出单位时间内机体分别在（x ，y ，z ）三个方向上的位移，利用

cumsum 函数对?S 的三个行向量进行累加运算，得出另一个（X ’,Y ’,H ’）

然后用此组数据对题中给定的（x ，y ，h ）进行减法运算发现二者的差值符合三个与t 有关的一次函数

校正方程 ?x=0.648299491856981*t -0.165130488213802 X=x （测）+?x

012

3456

-0.5

00.511.522.5

33.5

4时间t/s

二者误差(X '-x )

x 的误差校正函数

012

3456

123456

78

9时间t/s

误差值（Y '-y ）

(Y-y)的误差误差校正函数

校正方程 ?y=1.406880980638946*t+ 0.083924870933847 Y=y （测）+?y

六模型评价

本个模型对于问题一主要采用均值迭代法来对声音信号的干扰来进行修正，修正后h 的值随时间的波动明显减小，效果相对较好，基本上可以很大程度消减噪声的干扰，如果想进一步消除噪声影响可以采用联邦卡尔曼滤波的方法进行修正。

对于问题二主要采用vx,vy的坐标变换法，对（vx，vy，vh）进行变换，得到的结果误差符合对时间的严格一次函数，所以只需要简单的修正即可很大程度地消减它们的误差。而由于h变化比较复杂，vz全部为负值而h却呈现出上下波动的现象，所以h

的误差相对较大，可以采用将vz与h加权的方法进行修正，能够达到不错的修正效果。不过此模型未能很好的修正仪器精度带来的误差。

七模型的改进

本个模型对误差的修正并没有达到真正最优的效果，因为数据的组数据的组数较多，加之对卡尔曼过滤器的原理没有很好的了解，如果能在问题一中采用联邦卡尔曼滤波消除噪声影响，加上后面的坐标变换进行修正效果会更好些。

参考文献

[1].周建兴、岂兴明、MATLAB从入门到精通，北京：人民邮电出版社，2008年；

[2].马知恩、王绵森，数学分析基础（第二版上册、下册），[M],北京：高等教育出版社，2006年；

[3].王正东，数学软件与数学实验，北京：科学出版社，2004年；

[4].姜启源、谢金星、叶俊，数学模型（第三版）[M]，北京：高等教育出版社，2003年。