一种基于地球坐标系和右群状态误差定义的SINS/USBL组合导航方法技术

本发明专利技术属于组合导航技术领域，具体涉及一种基于地球坐标系和右群状态误差定义的SINS/USBL组合导航方法。本发明专利技术利用地球坐标系下的机械编排，使得导航系统具备无切换全球导航能力；利用李群理论定义新的误差并推导相应的误差模型，使得在较大的初始失准角的情况下，采用线性模型和线性滤波器就可以对大初始导航误差进行准确估计，并且在任意载体机动条件下都可以保证姿态误差、速度误差和位置误差的可观测性。本发明专利技术通过建立虚拟观测方程来解决在大初始失准角条件下，由于USBL的数据更新频率低而导致的严重降低收敛速度及估计精度的问题。题。题。

【技术实现步骤摘要】
一种基于地球坐标系和右群状态误差定义的SINS/USBL组合导航方法


[0001]本专利技术属于组合导航
，具体涉及一种基于地球坐标系和右群状态误差定义的SINS/USBL组合导航方法。

技术介绍

[0002]目前已有的捷联惯导/超短基线(SINS/USBL)组合导航系统多是采用基于地理系下线性近似后的误差模型。首先“东
‑
北
‑
天”地理系下的算法只适用于中低纬度地区不适用于极区。此外由于其推导过程中的线性近似使得不具备对大初始导航误差的估计能力。而地球系下的惯导机械编排使得惯导具备无切换全球导航能力，并且这种机械编排计算的导航参数(姿态、速度和位置)构成的动力学模型已被证明满足“群仿射”性质。在这种性质的条件下，利用李群理论定义右群状态误差向量，可以推导一种新的线性SINS/USBL误差模型，能够更准确地描述导航过程中的误差传播。该误差模型最大的优点是能够处理大的初始对准角，在存在大导航误差的情况下利用线性误差模型仍可以对误差进行准确描述，从而准确估计误差。最后，考虑到USBL测量更新频率较低，提出了一种构建虚拟观测方程的方法，以加快算法在大初始对准角下的误差收敛速度。

技术实现思路

[0003]本专利技术的目的在于提供能够应对由于初始对准效果差或初始对准中断导致的大初始姿态误差的情况的一种基于地球坐标系和右群状态误差定义的SINS/USBL组合导航方法。
[0004]一种基于地球坐标系和右群状态误差定义的SINS/USBL组合导航方法，包括以下步骤：
[0005]步骤1：在地球坐标系下对SINS进行纯惯性导航解算，得到导航参数和
[0006]其中，表示从载体坐标系到地球坐标系的姿态矩阵；表示载体相对于惯性坐标系在地球坐标系下的速度；表示载体在地球坐标系下的位置；
[0007]步骤2：获取当前时刻USBL测量结果，并与上一时刻对比；如果当前时刻USBL测量结果更新，则执行步骤3；如果当前时刻USBL测量结果没有更新，则执行步骤4；
[0008]步骤3：利用SINS/USBL组合导航系统模型估算导航误差，得到后，执行步骤5；
[0009]SINS/USBL组合导航系统模型为：
[0010][0011][0012]其中，表示姿态误差角估计值，表示速度误差向量估计值，表示位置误差向量估计值；表示常值漂移，表示常值零偏；
“×”
表示向量叉乘运算；表示向量叉乘运算；和是随机噪声；表示USBL中应答器的位置；为矩阵的转置矩阵，为从载体坐标系到USBL基阵坐标系的姿态变换矩阵；V表示USBL的测量噪声；
[0013]步骤4：利用虚拟观测方程估算导航误差，得到后，执行步骤5；
[0014]虚拟观测方程为：
[0015]X
k
＝F
k,k
‑1X
k
‑1+G
k
‑1W
k
‑1[0016][0017]其中，公式中矩阵向量的下标表示采样次数，当前时刻为t，t＝kT，T为采样间隔，k为当前采样次数；k
‑
i为USBL测量结果上一次更新时对应的采样次数；i为USBL测量结果上一次更新时对应的采样次数；
[0018]矩阵向量
[0019]步骤5：利用导航误差的估计值对导航参数进行校正，输出精确导航参数
[0020][0021][0022][0023]进一步地，所述USBL由应答器和收发器组成，应答器设置在水下或水面母船的底部，它的位置是已知的；所述收发器由一个发射换能器和多个水听器组成，发射换能器用于将声信号发送到应答器，水听器用于接收从应答器返回的声信号。
[0024]进一步地，所述步骤1具体为：
[0025][0026][0027][0028]其中，分别为导航参数的一阶导数；表示陀螺仪输
出的角速度；f
b
表示加速度计输出的加速度；ω
ie
表示地球自转角速度；表示当地重力矢量。
[0029]本专利技术的有益效果在于：
[0030]本专利技术利用地球坐标系下的机械编排，使得导航系统具备无切换全球导航能力；利用李群理论定义新的误差并推导相应的误差模型，使得在较大的初始失准角的情况下，采用线性模型和线性滤波器就可以对大初始导航误差进行准确估计，并且在任意载体机动条件下都可以保证姿态误差、速度误差和位置误差的可观测性。本专利技术通过建立虚拟观测方程来解决在大初始失准角条件下，由于USBL的数据更新频率低而导致的严重降低收敛速度及估计精度的问题。
附图说明
[0031]图1为USBL定位原理及其组成。
[0032]图2为本专利技术中基于虚拟观测方程的误差估计流程图。
[0033]图3为SINS/USBL组合导航系统结构框图。
[0034]图4为本专利技术的实施例中的仿真轨迹图。
[0035]图5为本专利技术的实施例中三种算法的50次蒙特卡罗仿真结果示意图。
[0036]图6为RE
‑
KF和VO
‑
RE
‑
KF在较长测量更新周期下的失准角收敛曲线图。
[0037]图7为本专利技术的总体架构图。
具体实施方式
[0038]下面结合附图对本专利技术做进一步描述。
[0039]首先对常用坐标系进行定义，b表示载体系，e表示地球系，n表示地理系，i表示惯性系，u表示USBL基阵坐标系。
[0040]在仅给定初始的姿态、速度和位置的条件下，捷联惯导仅依靠陀螺仪输出的角速度信息和加速度计输出的加速度信息f
b
就可以实时在线计算导航参数。本专利技术采用地球系(e系)下的惯导系统机械编排算法实时求解姿态、速度和位置：
[0041][0042][0043][0044]式中，
“×”
表示向量叉乘运算。表示陀螺仪输出的角速度，f
b
表示加速度计输出的加速度，表示地球自转角速度表示从b系到e系的姿态矩阵，表示载体相对于惯性系在地球系下的速度，表示地球系下的位置(直角坐标)。g
e
表示当地重力矢量。
[0045]从微分方程(1)
‑
(3)可以看出。e系下惯导机械编排与纬度、子午线或当地大地水准面没有关系，只与惯性器件输出、地球自传角速度和重力分量有关。因此，e系下的机械编排可以提供无切换全球导航能力，在中低纬度和高纬度地区都可以正常工作。此外，e系下
的机械编排具有更高的效率和更低的计算复杂性。
[0046]姿态阵、速度和位置可以构成特殊欧式群中的元素。
[0047][0048]相应的动力学模型可以表示为
[0049][0050]对于(5)所表示的模型，它满足f(χ
A
χ
B
)＝f(χ
A
)χ
B
+χ
A
f(χ
B<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于地球坐标系和右群状态误差定义的SINS/USBL组合导航方法，其特征在于，包括以下步骤：步骤1：在地球坐标系下对SINS进行纯惯性导航解算，得到导航参数和其中，表示从载体坐标系到地球坐标系的姿态矩阵；表示载体相对于惯性坐标系在地球坐标系下的速度；表示载体在地球坐标系下的位置；步骤2：获取当前时刻USBL测量结果，并与上一时刻对比；如果当前时刻USBL测量结果更新，则执行步骤3；如果当前时刻USBL测量结果没有更新，则执行步骤4；步骤3：利用SINS/USBL组合导航系统模型估算导航误差，得到后，执行步骤5；SINS/USBL组合导航系统模型为：SINS/USBL组合导航系统模型为：其中，其中，表示姿态误差角估计值，表示速度误差向量估计值，表示位置误差向量估计值；表示常值漂移，表示常值零偏；表示常值零偏；
“×”
表示向量叉乘运算；表示向量叉乘运算；和是随机噪声；是随机噪声；表示USBL中应答器的位置；为矩阵的转置矩阵，的转置矩阵，为从载体坐标系到USBL基阵坐标系的姿态变换矩阵；V表示USBL的测量噪声；步骤4：利用虚拟观测方程估算导航误差，得到后，执行步骤5；虚拟观测方程为：X
k
＝F
k,k
‑1X
k
‑1+G
k
‑1W
k...

【专利技术属性】
技术研发人员：徐博，郭瑜，赵玉新，丁文婧，张伟，戴晓强，赵强，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：