CN110133702B - 一种基于正交变换的姿态测量方法和设备 - Google Patents

一种基于正交变换的姿态测量方法和设备 Download PDF

Info

Publication number
CN110133702B
CN110133702B CN201910394731.1A CN201910394731A CN110133702B CN 110133702 B CN110133702 B CN 110133702B CN 201910394731 A CN201910394731 A CN 201910394731A CN 110133702 B CN110133702 B CN 110133702B
Authority
CN
China
Prior art keywords
matrix
ambiguity
attitude
attitude matrix
solution
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910394731.1A
Other languages
English (en)
Other versions
CN110133702A (zh
Inventor
纪元法
武月
孙希延
严素清
付文涛
李有明
陈新
甄卫民
陈奇东
张风国
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guilin University of Electronic Technology
Original Assignee
Guilin University of Electronic Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guilin University of Electronic Technology filed Critical Guilin University of Electronic Technology
Priority to CN201910394731.1A priority Critical patent/CN110133702B/zh
Publication of CN110133702A publication Critical patent/CN110133702A/zh
Application granted granted Critical
Publication of CN110133702B publication Critical patent/CN110133702B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude
    • G01S19/54—Determining attitude using carrier phase measurements; using long or short baseline interferometry
    • G01S19/55—Carrier phase ambiguity resolution; Floating ambiguity; LAMBDA [Least-squares AMBiguity Decorrelation Adjustment] method

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

本发明适用于卫星导航领域，提供了一种基于正交变换的姿态测量方法和设备。本发明利用初始的基线配置和正交变换，将基线解算和姿态矩阵解算进行融合，得到一种直接的姿态矩阵解算，消除了两步计算过程中的分步误差，并且保证了姿态矩阵解算结果的正交性，提高了姿态角解算的精度和可靠性。

Description

一种基于正交变换的姿态测量方法和设备
技术领域
本发明属于卫星导航领域，尤其涉及一种基于正交变换的姿态测量方法和设备。
背景技术
近年来，全球导航卫星系统(Global Navigation Satellite System)已经取得了突飞猛进的发展，基于全球导航卫星系统进行的姿态测量已经广泛应用于军事和民用的各个领域。目前主要使用的姿态测量方法是基于载波相位差分技术的，主要包含两种方法，一种是通过姿态矩阵进行解算，通过利用天线接收到的卫星观测值，建立多颗卫星(至少4颗)定位方程组，通过求解超定方程得到基线坐标，利用基线坐标在不同坐标系下的转换关系得到姿态旋转矩阵，进而得到姿态角。但是此过程中需要包含基线坐标以及姿态转换矩阵的解算，使得求得的姿态角中包含两步误差，并且求得的姿态矩阵通常不具备正交性。另一种是直接法，利用已知的天线位置和姿态角之间的关系，直接求解姿态角。既不用计算基线在载体坐标系下的位置也不用计算姿态矩阵，因而不包含分步误差。但是在建立观测模型的过程中，所有天线在载体坐标下Z轴分量为0，使得矩阵不满秩，解得的姿态结果不可靠。综上所述，这两种算法使得姿态解算的结果精度不高。
发明内容
本发明的目的在于提供一种基于正交变换的姿态测量方法、设备和计算机可读存储介质，旨在解决现有技术的方法使得求得的姿态角中包含两步误差，并且求得的姿态矩阵通常不具备正交性，矩阵不满秩，解得的姿态结果不可靠的问题。
第一方面，本发明提供了一种基于正交变换的姿态测量方法，所述方法包括：
S101、在刚体上进行配置后的天线接收导航数据，对导航数据进行预处理得到观测值和星历数据；
S102、根据观测值利用正交变换构建模糊度-姿态矩阵的双差模型；
S103、基于模糊度-姿态矩阵的双差模型根据最小二乘法得到模糊度和姿态矩阵的浮点解
Figure BDA0002057799590000025
和
Figure BDA0002057799590000026
以及对应的方差-协方差矩阵
Figure BDA0002057799590000027
并假定在模糊度已知的情况下，得到姿态矩阵的条件最小二乘浮点解
Figure BDA0002057799590000021
和方差矩阵
Figure BDA0002057799590000022
S104、根据姿态矩阵的条件最小二乘浮点解
Figure BDA0002057799590000024
和方差矩阵
Figure BDA0002057799590000023
建立关于模糊度的目标函数F(Z)以及关于模糊度-姿态矩阵的目标函数F(Z,R)，通过载波相位和伪距观测值引入的方差，对基于目标函数的搜索空间ΩZ和ΩF(Z,R)的包含关系进行对比分析，确定搜索的先后顺序，将非椭球空间的搜索转换为对椭球空间的搜索；
S105、基于模糊度的目标函数F(Z)，直接运用LAMBDA算法的搜索方式得到多组模糊度候选解和对应的搜索阈值
Figure BDA0002057799590000028
S106、利用线性约束下的拉格朗日乘数法代替空间的搜索法对姿态矩阵进行直接求解；
S107、验证姿态矩阵是否满足由初始阈值
Figure BDA0002057799590000029
构成的搜索空间ΩF(Z,R)，若满足，选取使得阈值最小
Figure BDA00020577995900000210
的一组模糊度-姿态矩阵候选值作为最优解，若不满足，重新进行LAMBDA搜索，重复步骤S105；
S108、经过选取得到最优的姿态矩阵，并利用姿态矩阵的转换得到完整的姿态角。
第二方面，本发明提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如所述的基于正交变换的姿态测量方法的步骤。
第三方面，本发明提供了一种基于正交变换的姿态测量设备，包括：
一个或多个处理器；
存储器；以及
一个或多个计算机程序，所述处理器和所述存储器通过总线连接，其中所述一个或多个计算机程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器执行，所述处理器执行所述计算机程序时实现如所述的基于正交变换的姿态测量方法的步骤。
在本发明中，由于利用了基线配置的先验条件，将模糊度-基线矢量的双差模型转换为模糊度-姿态矩阵双差模型，省略了基线坐标解算这一中间步骤，消除了两步误差，并利用了正交约束保证了姿态结果的正交性；又由于利用载波相位和伪距建立双差模型，并通过观测值引入的两种方差完成搜索空间的包含关系分析，将非椭球空间搜索转化为椭球空间的搜索，通过外围椭球搜索空间的搜索得到的候选值和阈值，直接缩小内围非椭球搜索空间内候选值的验证，得到一种效率更高的搜索方式；又由于利用正交约束作为条件建立基于LSE的拉格朗日函数，通过拉格朗日乘数法和Taylor展开约束完成姿态矩阵的直接求解，避免重复的模糊度-姿态矩阵组合值搜索和迭代计算，提高了解算效率和精度；且由于利用搜索空间进行模糊度-姿态矩阵候选值组合的验证，对错误解进行校正，进一步提高了解算结果的可靠性。
附图说明
图1是本发明实施例一提供的基于正交变换的姿态测量方法的流程图。
图2是正交变换下基线坐标转化示意图。
图3是搜索空间ΩZ和ΩF(Z,R)的关系示意图。
图4是LAMBDA搜索方法流程框图。
图5是拉格朗日乘数法姿态解算流程框图。
图6是完整姿态角解算结果示意图。
图7是本发明实施例三提供的基于正交变换的姿态测量设备的具体结构框图。
具体实施方式
为了使本发明的目的、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
为了说明本发明所述的技术方案，下面通过具体实施例来进行说明。
实施例一：
请参阅图1，本发明实施例一提供的基于正交变换的姿态测量方法包括以下步骤：需注意的是，若有实质上相同的结果，本发明的基于正交变换的姿态测量方法并不以图1所示的流程顺序为限。
S101、在刚体上进行配置后的天线接收导航数据，对导航数据进行预处理得到观测值和星历数据。
S102、根据观测值利用正交变换构建模糊度-姿态矩阵的双差模型。
在本发明实施例一中，S102具体为：
根据观测值分别构建载波相位和伪距的双差模型进行组合得到双差方程组，通过已知的天线配置得到基线长度和基线间的相对位置关系，并利用已知的基线配置，通过正交变换完成基线矢量在不同坐标系下的转换得到姿态矩阵，构建正交变换下关于模糊度-姿态矩阵的双差模型。
在刚体上配置3根天线，以其中之一作为主天线，可以组成双基线系统。对于双基线系统同时追踪n+1个卫星的情况：
所述根据观测值分别构建载波相位和伪距的双差模型进行组合得到双差方程组具体通过以下公式得到：
Figure BDA0002057799590000041
Figure BDA0002057799590000051
其中，Y是2n×2观测量矩阵，每一列包含每个基线的双差伪距yρ和载波相位观测值yφ，G为单位视线矢量矩阵,B为3×2实数值基线坐标的矩阵，
Figure BDA0002057799590000052
表示包含载波波长的设计矩阵,In表示n阶单位矩阵，Z为n×2整数值模糊度矢量矩阵，每一列表示每个基线的模糊度矢量，存在高斯误差ε，观测量的离散度用方差-协方差(v-c)矩阵QY表示，Qy用来表示单个基线的观测量的离散度，
Figure BDA0002057799590000053
表示Kronecker积，P用来描述由于共用天线引起的相关性，对于双基线系统,P通常设置为
Figure BDA0002057799590000054
所述通过已知的天线配置得到基线长度和基线间的相对位置关系，并利用已知的基线配置，通过正交变换完成基线矢量在不同坐标系下的转换得到姿态矩阵，构建正交变换下关于模糊度-姿态矩阵的双差模型具体为：
已知的双基线系统中基线长度和基线间的相对位置关系，利用正交变换将本地坐标系下的基线坐标B转化为观测模型中的姿态矩阵R，其线性关系描述为：B＝R·F (2)
其中R为3×2实数值姿态矩阵，并且满足正交关系RTR＝I，F为本地基线转化后坐标，转化坐标系定义为：第一个轴与基线对齐，第二个轴与第一个轴垂直，第三个轴通过形成右手旋转法则得到，如图2所示。对于双基线系统，R和F的乘积定义如下：
Figure BDA0002057799590000055
p为基线数量，ri表示姿态矩阵R中的第i列，正交变换下模糊度-姿态矩阵的双差模型定义为:
Figure BDA0002057799590000056
其中
Figure BDA0002057799590000057
替代基线矢量得到待求姿态矩阵，直接避免了基线坐标计算的中间步骤，消除了两步误差。
由于S102中，原本模糊度-基线矢量构成的双差模型，通过基线配置的已知条件转化为模糊度-姿态矩阵的双差模型，直接消除基线矢量解算这一中间步骤。并且在载波相位差分的基础上加入了伪距差分作为辅助，通过载波和伪距观测值引入的方差有利于后续对搜索空间进行对比分析，得到一种更快的搜索与解算方法。
S103、基于模糊度-姿态矩阵的双差模型根据最小二乘法得到模糊度和姿态矩阵的浮点解
Figure BDA0002057799590000061
和
Figure BDA0002057799590000062
以及对应的方差-协方差矩阵
Figure BDA0002057799590000063
并假定在模糊度已知的情况下，得到姿态矩阵的条件最小二乘浮点解
Figure BDA0002057799590000064
和方差矩阵
Figure BDA0002057799590000065
在本发明实施例一中，S103具体可以为：
定义C＝[H M]T，基于模糊度-姿态矩阵的双差模型根据最小二乘法得到模糊度和姿态矩阵的浮点解
Figure BDA0002057799590000066
以及对应的方差-协方差矩阵
Figure BDA0002057799590000067
Figure BDA0002057799590000068
Figure BDA0002057799590000069
其中，
Figure BDA00020577995900000610
为
Figure BDA00020577995900000611
的协方差阵，
Figure BDA00020577995900000612
表示
Figure BDA00020577995900000613
的协方差阵，
Figure BDA00020577995900000614
与
Figure BDA00020577995900000615
表示
Figure BDA00020577995900000616
和
Figure BDA00020577995900000617
的相关性，在已知模糊度浮点解的条件下，得到姿态矩阵的条件最小二乘浮点解
Figure BDA00020577995900000618
和方差矩阵
Figure BDA00020577995900000619
Figure BDA00020577995900000620
Figure BDA00020577995900000621
S104、根据姿态矩阵的条件最小二乘浮点解
Figure BDA00020577995900000622
和方差矩阵
Figure BDA00020577995900000623
建立关于模糊度的目标函数F(Z)以及关于模糊度-姿态矩阵的目标函数F(Z,R)，通过载波相位和伪距观测值引入的方差，对基于目标函数的搜索空间ΩZ和ΩF(Z,R)的包含关系进行对比分析，确定搜索的先后顺序，将非椭球空间的搜索转换为对椭球空间的搜索，从而降低搜索过程中的计算量。
在本发明实施例一中，S104具体为：
应用整数最小二乘法(ILS)以高效快速的方式求解模型(5)，ILS是对整数约束的线性系统的最小二乘原理的扩展，其目标函数构建为最小化加权(v-c矩阵)残差的平方范数，
Figure BDA0002057799590000071
其中，
Figure BDA0002057799590000072
考虑到模糊度已知下，求解姿态矩阵，加权残差写为几项加权残差的和的形式：
Figure BDA0002057799590000073
其中，
Figure BDA0002057799590000074
表示为最小二乘残差量，在其忽略不计的情况下，得到模糊度的目标函数F(Z)和模糊度-姿态矩阵的目标函数F(Z,R)分别为：
Figure BDA0002057799590000075
Figure BDA0002057799590000076
使得目标函数达到最小，目标函数F(Z)要求整数解与浮点解之间距离的加权平方最小，目标函数F(Z,R)要求模糊度整数解与浮点解距离的加权平方和条件姿态矩阵浮点解与姿态矩阵整数解加权平方之和最小，分别定义目标函数的搜索空间分别为
Figure BDA0002057799590000077
Figure BDA0002057799590000078
ΩF(Z)为椭球搜索空间，ΩF(Z,R)为非椭球搜索空间，χ2为搜索阈值，设置值应该在保证搜索空间足够小的情况下仍然能够包含足够的模糊度和姿态矩阵候选值，单个基线条件下，分别用
Figure BDA0002057799590000079
与
Figure BDA00020577995900000710
表示的载波相位观测量与伪距观测量的方差阵，设M＝[In O]T，H＝[AT AT]T，模糊度浮点解和条件姿态矩阵浮点解的方差阵表示为：
Figure BDA00020577995900000711
Figure BDA0002057799590000081
通常情况下，
Figure BDA0002057799590000082
对于模糊度候选值，可以得到目标函数F(Z,R)中的第一项比第二项要小得多，可以得到F(Z)＜＜F(Z,R)以及搜索空间的包含关系
Figure BDA0002057799590000083
如图3所示。如果直接通过设置F(Z,R)＝χ2来进行ΩF(Z,R)的搜索，那么会包含过多的模糊度候选值使得搜索空间扩大很多，计算量相应增加，为了避免过大的搜索空间以及搜索复杂度，考虑将非椭球空间的搜索转化为椭球空间的搜索，可以先设置较小的
Figure BDA0002057799590000084
满足搜索空间ΩZ，通过LAMBDA算法进行搜索，得到最优的
Figure BDA0002057799590000085
和相应的几组候选值后，再利用已有值对目标函数ΩF(Z,R)进行验证计算，直接减少对目标函数ΩF(Z,R)中每组模糊度-姿态矩阵组合的直接搜索与计算，同时使得ΩF(Z,R)的搜索空间尽可能小。
S105、基于模糊度的目标函数F(Z)，直接运用LAMBDA算法的搜索方式得到多组模糊度候选解和对应的搜索阈值
Figure BDA0002057799590000086
LAMBDA算法是一种较为成熟的搜索算法，在无条件约束情况下，适用于模糊度候选值的直接搜索，效率较高。
在本发明实施例一中，S105具体为：
如图4所示，基于模糊度的目标函数F(Z)，先对模糊度的浮点解与方差矩阵进行LTDL变换，利用初始阈值
Figure BDA0002057799590000087
来确定搜索空间和范围进行模糊度搜索，通过逐渐逼近与整数候选值检验，并不断更新缩小阈值，得到满足条件的多组候选值并记录此时的搜索阈值
Figure BDA0002057799590000088
S106、利用线性约束下的拉格朗日乘数法代替空间的搜索法对姿态矩阵进行直接求解。
在本发明实施例一中，S106具体为：
建立姿态矩阵的目标函数F(R)，利用正交约束条件RTR＝I构建拉格朗日函数L(Z,R)，通过拉格朗日乘数法得到模糊度和姿态矩阵的浮点解与整数解的等式关系，通过Taylor一阶展开约束条件得到拉格朗日算子，在已知最小二乘法模糊度和姿态矩阵浮点解、LAMBDA模糊度候选值与拉格朗日算子的前提下直接计算得到姿态矩阵。不需要进行反复的基线矢量搜索和大量的迭代计算过程，大大降低了计算量，直接省略的基线向量求解步骤消除了两步误差。
在通过LAMBDA算法得到的搜索阈值
Figure BDA00020577995900000911
和模糊度整数候选解的条件下，根据条件姿态矩阵建立姿态矩阵的目标函数
Figure BDA0002057799590000091
设定姿态矩阵为R＝[r11 r21 r31 r12 r22 r32]以及拉格朗日算子λ＝[λ1 λ2 λ3]，根据正交约束条件RTR＝I，可以得到关于姿态矩阵的线性约束函数D(R)和拉格朗日函数L(R,λ)
Figure BDA0002057799590000092
L(R,λ)＝F(R)+2λD(R) (19)
分别对R和λ进行求导可以得到
Figure BDA0002057799590000093
Figure BDA0002057799590000094
其中
Figure BDA0002057799590000095
化简公式(21)可以得到条件姿态矩阵浮点解与整数解之间的关系
Figure BDA0002057799590000096
由于
Figure BDA0002057799590000097
与
Figure BDA0002057799590000098
的对称性，公式(23)可以表达成
Figure BDA0002057799590000099
结合公式
Figure BDA00020577995900000910
可得到模糊度和姿态矩阵的整数解与浮点解之间的等式关系
Figure BDA0002057799590000101
利用姿态矩阵D(R)在R处的Taylor展开建立约束条件与公式(23)构成约束方程组
Figure BDA0002057799590000102
其中，
Figure BDA0002057799590000103
可以得到拉格朗日算子的通解为
Figure BDA0002057799590000104
通过以上求解将得到的
Figure BDA0002057799590000105
结合公式(6)得到的
Figure BDA0002057799590000106
和
Figure BDA0002057799590000107
以及LAMBDA算法下得到的模糊度整数候选解，带入公式(24)中得到拉格朗日算法在正交约束下的姿态矩阵候选解，如图5所示。
S107、验证姿态矩阵是否满足由初始阈值
Figure BDA0002057799590000108
构成的搜索空间ΩF(Z,R)，若满足，选取使得阈值最小
Figure BDA0002057799590000109
的一组模糊度-姿态矩阵候选值作为最优解，若不满足，重新进行LAMBDA搜索，重复步骤S105；
在本发明实施例一中，S107具体为：
得到姿态矩阵候选解以及LAMBDA算法下的搜索阈值
Figure BDA00020577995900001010
验证搜索空间ΩF(Z,R)是否满足，每一组模糊度候选值对应于一个姿态矩阵，同时带入搜索空间进行验证，不需要边计算边验证且直接省略了对搜索范围内大量模糊度-姿态矩阵组合的验证，极大程度的降低了计算量。搜索阈值
Figure BDA00020577995900001011
符合搜索空间的基础上，直接挑选出符合条件最优姿态矩阵，若不能够满足还能够通过重搜索进行校正，降低了解算结果的错误率，提高了姿态角的精度。
如图5所示，得到几组姿态矩阵候选解后，通过F(Z,R)计算每组模糊度-姿态矩阵候选解对应的搜索阈值，验证是否满足由LAMBDA算法得到的
Figure BDA00020577995900001012
构成的搜索空间ΩF(Z,R)，如果存在任何一组模糊度-姿态矩阵候选解不能够满足
Figure BDA0002057799590000111
那么判定得到的候选解组合错误，重新由LAMBDA算法进行再搜索，重复S105和S106。如果每组都满足
Figure BDA0002057799590000112
会从几组候选解组合中选取使得搜索阈值最小
Figure BDA0002057799590000113
的一组模糊度-姿态矩阵解作为最优解。
通过验证姿态矩阵是否满足由初始阈值构成的搜索空间，从而使解算过程中出现的错误解可以进行校正，提高解算结果的可靠性。
S108、经过选取得到最优的姿态矩阵，并利用姿态矩阵的转换得到完整的姿态角。
在本发明实施例一中，S108具体为：
经过验证得到最优的姿态矩阵R后，通过参数转化得到完整的姿态角(俯仰角、航向角、横滚角)，姿态角解算结果如图6所示：
Figure BDA0002057799590000114
实施例二：
本发明实施例二提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如本发明实施例一提供的基于正交变换的姿态测量方法的步骤。
实施例三：
图7示出了本发明实施例三提供的基于正交变换的姿态测量设备的具体结构框图，一种基于正交变换的姿态测量设备100包括：一个或多个处理器101、存储器102、以及一个或多个计算机程序，其中所述处理器101和所述存储器102通过总线连接，所述一个或多个计算机程序被存储在所述存储器102中，并且被配置成由所述一个或多个处理器101执行，所述处理器101执行所述计算机程序时实现如本发明实施例一提供的基于正交变换的姿态测量方法的步骤。
在本发明中，由于利用了基线配置的先验条件，将模糊度-基线矢量的双差模型转换为模糊度-姿态矩阵双差模型，省略了基线坐标解算这一中间步骤，消除了两步误差，并利用了正交约束保证了姿态结果的正交性；又由于利用载波相位和伪距建立双差模型，并通过观测值引入的两种方差完成搜索空间的包含关系分析，将非椭球空间搜索转化为椭球空间的搜索，通过外围椭球搜索空间的搜索得到的候选值和阈值，直接缩小内围非椭球搜索空间内候选值的验证，得到一种效率更高的搜索方式；又由于利用正交约束作为条件建立基于LSE的拉格朗日函数，通过拉格朗日乘数法和Taylor展开约束完成姿态矩阵的直接求解，避免重复的模糊度-姿态矩阵组合值搜索和迭代计算，提高了解算效率和精度；且由于利用搜索空间进行模糊度-姿态矩阵候选值组合的验证，对错误解进行校正，进一步提高了解算结果的可靠性。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取记忆体(RAM，RandomAccess Memory)、磁盘或光盘等。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于正交变换的姿态测量方法，其特征在于，所述方法包括：
S101、在刚体上进行配置后的天线接收导航数据，对导航数据进行预处理得到观测值和星历数据；
S102、根据观测值利用正交变换构建模糊度-姿态矩阵的双差模型；
S103、基于模糊度-姿态矩阵的双差模型根据最小二乘法得到模糊度和姿态矩阵的浮点解
Figure FDA0003830463360000011
和
Figure FDA0003830463360000012
以及对应的方差-协方差矩阵
Figure FDA0003830463360000013
并假定在模糊度已知的情况下，得到姿态矩阵的条件最小二乘浮点解
Figure FDA0003830463360000014
和方差矩阵
Figure FDA0003830463360000015
S104、根据姿态矩阵的条件最小二乘浮点解
Figure FDA0003830463360000016
和方差矩阵
Figure FDA0003830463360000017
建立关于模糊度的目标函数F(Z)以及关于模糊度-姿态矩阵的目标函数F(Z,R)，通过载波相位和伪距观测值引入的方差，对基于目标函数的搜索空间ΩZ和ΩF(Z,R)的包含关系进行对比分析，确定搜索的先后顺序，将非椭球空间的搜索转换为对椭球空间的搜索，其中，Z为n×2整数值模糊度矢量矩阵，R为3×2实数值姿态矩阵；
S105、基于模糊度的目标函数F(Z)，直接运用LAMBDA算法的搜索方式得到多组模糊度候选解和对应的搜索阈值
Figure FDA0003830463360000018
S106、利用线性约束下的拉格朗日乘数法代替空间的搜索法对姿态矩阵进行直接求解；
S107、验证姿态矩阵是否满足由初始阈值
Figure FDA0003830463360000019
构成的搜索空间ΩF(Z,R)，若满足，选取使得阈值最小
Figure FDA00038304633600000110
的一组模糊度-姿态矩阵候选值作为最优解，若不满足，重新进行LAMBDA搜索，重复步骤S105；
S108、经过选取得到最优的姿态矩阵，并利用姿态矩阵的转换得到完整的姿态角。
2.如权利要求1所述的方法，其特征在于，S102具体为：
根据观测值分别构建载波相位和伪距的双差模型进行组合得到双差方程组，通过已知的天线配置得到基线长度和基线间的相对位置关系，并利用已知的基线配置，通过正交变换完成基线矢量在不同坐标系下的转换得到姿态矩阵，构建正交变换下关于模糊度-姿态矩阵的双差模型。
3.如权利要求2所述的方法，其特征在于，在刚体上配置3根天线，以其中之一作为主天线，组成双基线系统；
对于双基线系统同时追踪n+1个卫星的情况：
所述根据观测值分别构建载波相位和伪距的双差模型进行组合得到双差方程组具体通过以下公式得到：
Figure FDA0003830463360000021
Figure FDA0003830463360000022
其中，Y是2n×2观测量矩阵，每一列包含每个基线的双差伪距yρ和载波相位观测值yφ，G为单位视线矢量矩阵，B为3×2实数值基线坐标的矩阵，
Figure FDA0003830463360000023
表示包含载波波长的设计矩阵,In表示n阶单位矩阵，Z为n×2整数值模糊度矢量矩阵，每一列表示每个基线的模糊度矢量，存在高斯误差ε，观测量的离散度用方差-协方差矩阵QY表示，Qy用来表示单个基线的观测量的离散度，
Figure FDA0003830463360000024
表示Kronecker积，P用来描述由于共用天线引起的相关性；
所述通过已知的天线配置得到基线长度和基线间的相对位置关系，并利用已知的基线配置，通过正交变换完成基线矢量在不同坐标系下的转换得到姿态矩阵，构建正交变换下关于模糊度-姿态矩阵的双差模型具体为：
已知的双基线系统中基线长度和基线间的相对位置关系，利用正交变换将本地坐标系下的基线坐标B转化为观测模型中的姿态矩阵R，其线性关系描述为：B＝R·F (2)
其中R为3×2实数值姿态矩阵，并且满足正交关系RTR＝I，F为本地基线转化后坐标，转化坐标系定义为：第一个轴与基线对齐，第二个轴与第一个轴垂直，第三个轴通过形成右手旋转法则得到，对于双基线系统，R和F的乘积定义如下：
Figure FDA0003830463360000031
p为基线数量，ri表示姿态矩阵R中的第i列，f11、f12、和f22分别为本地基线转化后的三个坐标点，正交变换下模糊度-姿态矩阵的双差模型定义为:
Figure FDA0003830463360000032
其中
Figure FDA0003830463360000033
替代基线矢量得到待求姿态矩阵。
4.如权利要求3所述的方法，其特征在于，S103具体为：
定义C＝[H M]T，基于模糊度-姿态矩阵的双差模型根据最小二乘法得到模糊度和姿态矩阵的浮点解
Figure FDA0003830463360000034
以及对应的方差-协方差矩阵
Figure FDA0003830463360000035
Figure FDA0003830463360000036
Figure FDA0003830463360000037
其中，
Figure FDA0003830463360000038
为
Figure FDA0003830463360000039
的协方差阵，
Figure FDA00038304633600000310
表示
Figure FDA00038304633600000311
的协方差阵，
Figure FDA00038304633600000312
与
Figure FDA00038304633600000313
表示
Figure FDA00038304633600000314
和
Figure FDA00038304633600000315
的相关性，在已知模糊度浮点解的条件下，得到姿态矩阵的条件最小二乘浮点解
Figure FDA00038304633600000316
和方差矩阵
Figure FDA00038304633600000317
Figure FDA00038304633600000318
Figure FDA00038304633600000319
5.如权利要求4所述的方法，其特征在于，S104具体为：
应用整数最小二乘法求解模型(5)，ILS是对整数约束的线性系统的最小二乘原理的扩展，其目标函数构建为最小化加权残差的平方范数，
Figure FDA00038304633600000320
其中，
Figure FDA00038304633600000321
考虑到模糊度已知下，求解姿态矩阵，加权残差写为几项加权残差的和的形式：
Figure FDA00038304633600000322
其中，
Figure FDA00038304633600000323
表示为最小二乘残差量，在其忽略不计的情况下，得到模糊度的目标函数F(Z)和模糊度-姿态矩阵的目标函数F(Z,R)分别为：
Figure FDA0003830463360000041
Figure FDA0003830463360000042
使得目标函数达到最小，目标函数F(Z)要求整数解与浮点解之间距离的加权平方最小，目标函数F(Z,R)要求模糊度整数解与浮点解距离的加权平方和条件姿态矩阵浮点解与姿态矩阵整数解加权平方之和最小，分别定义目标函数的搜索空间分别为
Figure FDA0003830463360000043
Figure FDA0003830463360000044
ΩF(Z)为椭球搜索空间，ΩF(Z,R)为非椭球搜索空间，χ2为搜索阈值，单个基线条件下，分别用
Figure FDA0003830463360000045
与
Figure FDA0003830463360000046
表示的载波相位观测量与伪距观测量的方差阵，
Figure FDA0003830463360000047
与
Figure FDA0003830463360000048
分别表示载波相位观测量与伪距观测量，设M＝[In O]T，H＝[AT AT]T，A是模糊度的观测矩阵，模糊度浮点解和条件姿态矩阵浮点解的方差阵表示为：
Figure FDA0003830463360000049
Figure FDA00038304633600000410
先设置小于预设值的
Figure FDA00038304633600000411
满足搜索空间ΩZ，通过LAMBDA算法进行搜索，得到最优的
Figure FDA00038304633600000412
和相应的几组候选值后，再利用已有值对目标函数ΩF(Z,R)进行验证计算。
6.如权利要求1所述的方法，其特征在于，S105具体为：
基于模糊度的目标函数F(Z)，先对模糊度的浮点解与方差矩阵进行LTDL变换，利用初始阈值
Figure FDA00038304633600000413
来确定搜索空间和范围进行模糊度搜索，通过逐渐逼近与整数候选值检验，并不断更新缩小阈值，得到满足条件的多组候选值并记录此时的搜索阈值
Figure FDA0003830463360000051
7.如权利要求4所述的方法，其特征在于，S106具体为：
建立姿态矩阵的目标函数F(R)，利用正交约束条件RTR＝I构建拉格朗日函数L(Z,R)，通过拉格朗日乘数法得到模糊度和姿态矩阵的浮点解与整数解的等式关系，通过Taylor一阶展开约束条件得到拉格朗日算子，在已知最小二乘法模糊度和姿态矩阵浮点解、LAMBDA模糊度候选值与拉格朗日算子的前提下直接计算得到姿态矩阵；
在通过LAMBDA算法得到的搜索阈值
Figure FDA0003830463360000052
和模糊度整数候选解的条件下，根据条件姿态矩阵建立姿态矩阵的目标函数
Figure FDA0003830463360000053
设定姿态矩阵为R＝[r11 r21 r31 r12 r22 r32]以及拉格朗日算子λ＝[λ1 λ2 λ3]，根据正交约束条件RTR＝I，得到关于姿态矩阵的线性约束函数D(R)和拉格朗日函数L(R,λ)
Figure FDA0003830463360000054
L(R,λ)＝F(R)+2λD(R) (19)
分别对R和λ进行求导得到
Figure FDA0003830463360000055
Figure FDA0003830463360000056
其中
Figure FDA0003830463360000057
化简公式(21)得到条件姿态矩阵浮点解与整数解之间的关系
Figure FDA0003830463360000058
由于
Figure FDA0003830463360000059
与
Figure FDA00038304633600000510
的对称性，公式(23)表达成
Figure FDA00038304633600000511
结合公式
Figure FDA0003830463360000061
得到模糊度和姿态矩阵的整数解与浮点解之间的等式关系
Figure FDA0003830463360000062
利用姿态矩阵D(R)在R处的Taylor展开建立约束条件与公式(23)构成约束方程组
Figure FDA0003830463360000063
其中，
Figure FDA0003830463360000064
得到拉格朗日算子的通解为
Figure FDA0003830463360000065
通过以上求解得到的
Figure FDA0003830463360000066
结合公式(6)得到的
Figure FDA0003830463360000067
和
Figure FDA0003830463360000068
以及LAMBDA算法下得到的模糊度整数候选解，带入公式(24)中得到拉格朗日算法在正交约束下的姿态矩阵候选解。
8.如权利要求7所述的方法，其特征在于，S107具体为：
得到几组姿态矩阵候选解后，通过F(Z,R)计算每组模糊度-姿态矩阵候选解对应的搜索阈值，验证是否满足由LAMBDA算法得到的
Figure FDA0003830463360000069
构成的搜索空间ΩF(Z,R)，如果存在任何一组模糊度-姿态矩阵候选解不能够满足
Figure FDA00038304633600000610
那么判定得到的候选解组合错误，重新由LAMBDA算法进行再搜索，重复S105和S106；如果每组都满足
Figure FDA00038304633600000611
从几组候选解组合中选取使得搜索阈值最小
Figure FDA00038304633600000612
的一组模糊度-姿态矩阵解作为最优解；
S108具体为：
经过验证得到最优的姿态矩阵R后，通过参数转化得到完整的姿态角，姿态角包括俯仰角θ、航向角β和横滚角γ：
Figure FDA0003830463360000071
9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至8任一项所述的基于正交变换的姿态测量方法的步骤。
10.一种基于正交变换的姿态测量设备，包括：
一个或多个处理器；
存储器；以及
一个或多个计算机程序，所述处理器和所述存储器通过总线连接，其中所述一个或多个计算机程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器执行，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至8任一项所述的基于正交变换的姿态测量方法的步骤。
CN201910394731.1A 2019-05-13 2019-05-13 一种基于正交变换的姿态测量方法和设备 Active CN110133702B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910394731.1A CN110133702B (zh) 2019-05-13 2019-05-13 一种基于正交变换的姿态测量方法和设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910394731.1A CN110133702B (zh) 2019-05-13 2019-05-13 一种基于正交变换的姿态测量方法和设备

Publications (2)

Publication Number Publication Date
CN110133702A CN110133702A (zh) 2019-08-16
CN110133702B true CN110133702B (zh) 2022-12-27

Family

ID=67573581

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910394731.1A Active CN110133702B (zh) 2019-05-13 2019-05-13 一种基于正交变换的姿态测量方法和设备

Country Status (1)

Country Link
CN (1) CN110133702B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111399020A (zh) * 2020-04-09 2020-07-10 桂林电子科技大学 一种定向测姿系统及方法
CN111650616B (zh) * 2020-05-12 2023-03-24 烟台南山学院 一种高精度北斗导航定位系统导航定位参数计算方法
CN113237628B (zh) * 2021-07-08 2021-09-21 中国空气动力研究与发展中心低速空气动力研究所 一种低速风洞水平自由飞模型姿态测量方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101825717A (zh) * 2010-04-16 2010-09-08 北京航空航天大学 一种基于载波平滑码伪距技术的动态定姿方法
CN105445772A (zh) * 2015-11-19 2016-03-30 南京航空航天大学 多gnss天线组合平台位姿一体化的确定装置及方法
CN106199670A (zh) * 2016-06-28 2016-12-07 北京航空航天大学 一种基于蒙特卡洛采样的gnss单频单历元姿态确定方法
CN107390250A (zh) * 2017-07-14 2017-11-24 重庆重邮汇测通信技术有限公司 一种基于惯性导航系统和双天线gps的定位测姿方法
CN108205151A (zh) * 2018-01-10 2018-06-26 重庆邮电大学 一种低成本gps单天线姿态测量方法
CN108592943A (zh) * 2018-03-16 2018-09-28 东南大学 一种基于opreq方法的惯性系粗对准计算方法
CN108871179A (zh) * 2018-05-07 2018-11-23 桂林电子科技大学 基于载波相位差分动静态融合的形变监测定位方法及装置

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101825717A (zh) * 2010-04-16 2010-09-08 北京航空航天大学 一种基于载波平滑码伪距技术的动态定姿方法
CN105445772A (zh) * 2015-11-19 2016-03-30 南京航空航天大学 多gnss天线组合平台位姿一体化的确定装置及方法
CN106199670A (zh) * 2016-06-28 2016-12-07 北京航空航天大学 一种基于蒙特卡洛采样的gnss单频单历元姿态确定方法
CN107390250A (zh) * 2017-07-14 2017-11-24 重庆重邮汇测通信技术有限公司 一种基于惯性导航系统和双天线gps的定位测姿方法
CN108205151A (zh) * 2018-01-10 2018-06-26 重庆邮电大学 一种低成本gps单天线姿态测量方法
CN108592943A (zh) * 2018-03-16 2018-09-28 东南大学 一种基于opreq方法的惯性系粗对准计算方法
CN108871179A (zh) * 2018-05-07 2018-11-23 桂林电子科技大学 基于载波相位差分动静态融合的形变监测定位方法及装置

Also Published As

Publication number Publication date
CN110133702A (zh) 2019-08-16

Similar Documents

Publication Publication Date Title
CN110133702B (zh) 一种基于正交变换的姿态测量方法和设备
US6753810B1 (en) Fast ambiguity resolution for real time kinematic survey and navigation
Teunissen The affine constrained GNSS attitude model and its multivariate integer least-squares solution
CN113466903B (zh) 一种顾及观测值系统误差的部分模糊度固定算法
EP2479588B1 (en) Method and apparatus for determining the relative position between two receivers of a satellite navigation system
CN110907975B (zh) 一种基于序贯最小二乘的模糊度固定方法
CN102928858A (zh) 基于改进扩展卡尔曼滤波的gnss单点动态定位方法
Park et al. Integer least squares with quadratic equality constraints and its application to GNSS attitude determination systems
CN116819580B (zh) 一种惯性辅助的双天线gnss海上船舶姿态确定方法
CN116540285B (zh) 惯性辅助的gnss双天线定向方法、装置与电子设备
CN110646822B (zh) 一种基于惯导辅助的整周模糊度Kalman滤波算法
CN111399020A (zh) 一种定向测姿系统及方法
CN105403904A (zh) 一种基于天线阵列的卫星导航单频测姿方法
CN115096303A (zh) 一种gnss多天线与ins紧组合定位定姿方法和设备
CN114488235A (zh) 一种双天线卫星定向方法、系统、存储介质和电子设备
CN114740507A (zh) 一种基于短基线的定位定向方法及装置
CN113189541B (zh) 一种定位方法、装置及设备
Li et al. Global navigation satellite system ambiguity resolution with constraints from normal equations
CN114355393A (zh) 一种基于低成本接收机的三天线姿态估计方法
CN116299616A (zh) 一种附加已知基线约束的部分整周模糊度解算方法
Giorgi et al. Integer ambiguity resolution with nonlinear geometrical constraints
CN114895340A (zh) 双天线gnss/ins组合导航系统的定位方法和装置
CN108919315B (zh) 多模接收机系统层几何精度因子最小值计算方法
Teunissen GNSS ambiguity resolution for attitude determination: theory and method
Hu et al. Attitude Determination and RTK Performances Amelioration Using Multiple Low-Cost Receivers with Known Geometry

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant