基于多项式优化的高精度快速惯导解算方法和系统

1.本发明涉及传感器技术领域，具体地，涉及一种基于多项式优化的高精度快速惯导解算方法和系统。


背景技术：

2.惯性导航是根据陀螺与加速度计的输出(角速度/角增量与加速度/速度增量)，以航迹推算的方式积分求解载体的姿态、速度和位置。惯性导航的误差源主要包括传感器误差、地球重力误差和解算误差。通常人们认为基于传统圆锥补偿的多子样惯导算法精度已经足够高。但是，随着超高精度冷原子惯导器件以及军事上更高动态应用的出现，惯导解算的精度需要进一步提高，以充分发挥惯性器件的潜能。
3.专利文献cn109724597a(申请号：cn201811558766.6)公开了一种基于函数迭代积分的惯性导航解算方法及系统，包括：拟合步骤；姿态迭代解算步骤；速度/位置迭代解算步骤；获取姿态/速度/位置步骤。基于函数迭代积分和切比雪夫多项式近似的技术，根据陀螺和加速度计测量信息拟合重建角速度和比力加速度，并利用函数迭代积分方法实现姿态、速度和位置运动学方程的精确积分计算。
4.最近提出的基于切比雪夫多项式函数迭代的高精度惯导解算方法，可以将惯性导航解算中的误差降低到计算机截断误差水平，彻底消除了传统惯导算法中的不可交换性误差。但是，函数迭代方法相对于传统惯导解算方法计算量大，限制了其在低成本处理器上的应用。因此，如何在能够消除不可交换性误差的前提下，降低算法计算量是一个需要解决的问题。
5.目前，几乎所有类型惯性导航系统的解算算法都存在两大缺陷：1)原理性缺陷：惯导算法对理论方程采用了近似，如姿态算法使用了简化的旋转向量；速度算法使用了姿态的一阶近似等；2)算法性缺陷：惯导算法参数在特殊运动情形下设计，如圆锥或划摇运动，因此在实际运动中不能保证最优性。


技术实现要素：

6.针对现有技术中的缺陷，本发明的目的是提供一种基于多项式优化的高精度快速惯导解算方法和系统。
7.根据本发明提供的基于多项式优化的高精度快速惯导解算方法，包括：
8.步骤1：构建惯性导航在地球坐标系下的表达式，对陀螺和加速度计观测插值，获得切比雪夫点处的角速度与比力，拟合出角速度和比力的切比雪夫多项式函数；
9.步骤2：求解线性最小二乘，得到姿态四元数增量的切比雪夫多项式；求解非线性最小二乘，得到姿态罗德里格斯增量的切比雪夫系数；
10.步骤3：求解非线性最小二乘，获得速度的系数；迭代求解线性最小二乘，获得速度系数；
11.步骤4：根据得到的姿态/速度/位置的切比雪夫多项式系数以及对应的切比雪夫
多项式，计算得到对应时间区间上的姿态/速度/位置信息；
12.惯性导航在地球坐标系下的表述为：
[0013][0014][0015][0016]
其中，上标/下标b表示载体坐标系，为b系；e表示地球坐标系，为e系；i表示惯性坐标系，为i系；n表示导航坐标系，为n系；
[0017]
分别表示b系相对于e系的姿态四元数和姿态旋转矩阵；表示陀螺测量的角速度；表示地球自转角速度；ve表示在e系下载体相对于地球的速度；表示加速度计测量的比力；γe与pe分别表示e系下的地球重力向量以及载体在e系下的位置；符号ο表示四元数的乘法。
[0018]
在当前采样时间窗口内，若有陀螺测量的角速度或者角增量观测加速度计测量的比力或者速度增量其中，k＝0,1,
…
nm，则使用仿射变换，将时间窗口映射到τ∈[-1 1]，表达式为：
[0019][0020]
首先对陀螺和加速度计观测插值，以获得切比雪夫点处的角速度与比力，分别表示为其中，切比雪夫点定义为：τj＝-cos(jπ/n),j＝0,1,
…
,n，n是切比雪夫多项式的阶数；τ为映射后的时间自变量。
[0021]
进行基于多项式优化的姿态求解，在一个解算时间窗口内，姿态四元数的增量及其导数使用切比雪夫多项式表示为：
[0022][0023]
其中，为区间内切比雪夫增量；nq为切比雪夫的阶数；li为待求的切比雪夫系数，ti与为第i阶重构之后的切比雪夫多项式及其导数，重构的切比雪夫多项式ti定义如下：
[0024][0025]
其中，fi为原始的切比雪夫多项式，其循环定义如下：
[0026]
f0(τ)＝1,f1(τ)＝τ,
[0027]fi 1
(τ)＝2τfi(τ)-f
i-1
(τ)for i≥1
[0028]
通过求解如下线性最小二乘问题，得到姿态四元数增量的切比雪夫系数，表达式为：
[0029][0030]
其中，||
·
||表示向量的2-范数；为待求的切比雪夫系数矩阵；
[0031][0032][0033][0034]
其中，对于任意的四元数四元数操作符号定义为：
[0035][0036]
其中，s与η分别表示四元数的标量与矢量部分；(
·
)
×
表示三维向量对应的反对称矩阵；in表示n维单位矩阵；
[0037]
通过求解上述线性最小二乘问题求得l的估计，然后归一化获得区间内的四元数增量，表达式为：
[0038][0039]
通过求解区间内罗德里格斯向量增量的方法获得区间内的姿态增量，表达式为：
[0040][0041]
其中，与分别表示为区间内姿态罗德里格斯增量及其导数，使用重构切比雪夫多项式表示如下：
[0042][0043]
其中，是待求的罗德里格斯增量的切比雪夫系数；ng是切比雪夫阶数；为区间初始时刻的姿态旋转矩阵；将罗德里格斯向量增量转化为旋转矩阵；
[0044]
求解上述非线性最小二乘问题，并将获得的罗德里格斯增量转变为区间内的四元数增量，表达式为：
[0045][0046]
最后求得区间内的姿态四元数，表达式为：
[0047][0048]
其中，为区间内的初始姿态四元数。
[0049]
进行基于多项式优化的速度和位置求解，将区间内的速度和位置以及速度的导数表示为重构切比雪夫多项式：
[0050][0051][0052][0053]
其中，nv为切比雪夫多项式的阶数；ki为待求的切比雪夫多项式系数，其中，分别表示区间初始时刻的速度和位置；是i阶重构切比雪夫多项式的积分，定义如下：
[0054][0055]
其中，为i阶切比雪夫多项式的积分，表达式为：
[0056][0057]
使用上述切比雪夫多项式表示速度和位置，其系数通过求解如下非线性最小二乘获得：
[0058][0059]
其中，是待求的切比雪夫系数；表示由姿态算法求出的姿态旋转矩阵；e系下重力γe是关于位置的函数；
[0060]
通过假设区间内的重力为常数，然后求解如下线性最小二乘，获得上述算法的初始值，表达式为：
[0061][0062]
其中：
[0063][0064]
[0065][0066][0067]
其中，常量重力通过使用区间初始时刻的位置获得。
[0068]
通过迭代求解上述线性最小二乘，获得区间内速度的切比雪夫系数，步骤如下：
[0069]
步骤3.1：使用区间初始时刻的已知位置，计算重力，并把整个区间的重力当作常数；
[0070]
步骤3.2：求解上述线性最小二乘，获得速度的切比雪夫系数，进而求得区间内的速度和位置；
[0071]
步骤3.3：使用步骤3.2得到的位置，计算重力在切比雪夫点处的值；
[0072]
步骤3.4：重复执行步骤3.2和步骤3.3，直到连续两次循环得到的切比雪夫系数之差小于给定的阈值或者达到最大迭代次数。
[0073]
根据本发明提供的基于多项式优化的高精度快速惯导解算系统，包括：
[0074]
模块m1：构建惯性导航在地球坐标系下的表达式，对陀螺和加速度计观测插值，获得切比雪夫点处的角速度与比力，拟合出角速度和比力的切比雪夫多项式函数；
[0075]
模块m2：求解线性最小二乘，得到姿态四元数增量的切比雪夫多项式；求解非线性最小二乘，得到姿态罗德里格斯增量的切比雪夫系数；
[0076]
模块m3：求解非线性最小二乘，获得速度的系数；迭代求解线性最小二乘，获得速度系数；
[0077]
模块m4：根据得到的姿态/速度/位置的切比雪夫多项式系数以及对应的切比雪夫多项式，计算得到对应时间区间上的姿态/速度/位置信息；
[0078]
惯性导航在地球坐标系下的表述为：
[0079][0080][0081][0082]
其中，上标/下标b表示载体坐标系，为b系；e表示地球坐标系，为e系；i表示惯性坐标系，为i系；n表示导航坐标系，为n系；
[0083]
分别表示b系相对于e系的姿态四元数和姿态旋转矩阵；表示陀螺测量的角速度；表示地球自转角速度；ve表示在e系下载体相对于地球的速度；表示加速度计测量的比力；γe与pe分别表示e系下的地球重力向量以及载体在e系下的位置；符号ο表示四元数的乘法。
[0084]
在当前采样时间窗口内，若有陀螺测量的角速度或者角增量观测加速度计测量的比力或者速度增量其中，k＝0,1,
…
nm，则使用仿射变换，将时间窗口映射到τ∈[-1 1]，表达式为：
[0085][0086]
首先对陀螺和加速度计观测插值，以获得切比雪夫点处的角速度与比力，分别表示为其中，切比雪夫点定义为：τj＝-cos(jπ/n),j＝0,1,
…
,n，n是切比雪夫多项式的阶数；τ为映射后的时间自变量。
[0087]
进行基于多项式优化的姿态求解，在一个解算时间窗口内，姿态四元数的增量及其导数使用切比雪夫多项式表示为：
[0088][0089]
其中，为区间内切比雪夫增量；nq为切比雪夫的阶数；li为待求的切比雪夫系数，ti与为第i阶重构之后的切比雪夫多项式及其导数，重构的切比雪夫多项式ti定义如下：
[0090][0091]
其中，fi为原始的切比雪夫多项式，其循环定义如下：
[0092]
f0(τ)＝1,f1(τ)＝τ,
[0093]fi 1
(τ)＝2τfi(τ)-f
i-1
(τ)for i≥1
[0094]
通过求解如下线性最小二乘问题，得到姿态四元数增量的切比雪夫系数，表达式为：
[0095][0096]
其中，||
·
||表示向量的2-范数；为待求的切比雪夫系数矩阵；
[0097][0098][0099][0100]
其中，对于任意的四元数四元数操作符号定义为：
[0101][0102]
其中，s与η分别表示四元数的标量与矢量部分；(
·
)
×
表示三维向量对应的反对称矩阵；in表示n维单位矩阵；
[0103]
通过求解上述线性最小二乘问题求得l的估计，然后归一化获得区间内的四元数增量，表达式为：
[0104][0105]
通过求解区间内罗德里格斯向量增量的方法获得区间内的姿态增量，表达式为：
[0106][0107]
其中，与分别表示为区间内姿态罗德里格斯增量及其导数，使用重构切比雪夫多项式表示如下：
[0108][0109]
其中，是待求的罗德里格斯增量的切比雪夫系数；ng是切比雪夫阶数；为区间初始时刻的姿态旋转矩阵；将罗德里格斯向量增量转化为旋转矩阵；
[0110]
求解上述非线性最小二乘问题，并将获得的罗德里格斯增量转变为区间内的四元数增量，表达式为：
[0111][0112]
最后求得区间内的姿态四元数，表达式为：
[0113][0114]
其中，为区间内的初始姿态四元数。
[0115]
进行基于多项式优化的速度和位置求解，将区间内的速度和位置以及速度的导数表示为重构切比雪夫多项式：
[0116][0117][0118][0119]
其中，nv为切比雪夫多项式的阶数；ki为待求的切比雪夫多项式系数，其中，分别表示区间初始时刻的速度和位置；是i阶重构切比雪夫多项式的积分，定义如下：
[0120]
[0121]
其中，为i阶切比雪夫多项式的积分，表达式为：
[0122][0123]
使用上述切比雪夫多项式表示速度和位置，其系数通过求解如下非线性最小二乘获得：
[0124][0125]
其中，是待求的切比雪夫系数；表示由姿态算法求出的姿态旋转矩阵；e系下重力γe是关于位置的函数；
[0126]
通过假设区间内的重力为常数，然后求解如下线性最小二乘，获得上述算法的初始值，表达式为：
[0127][0128]
其中：
[0129][0130][0131][0132][0133]
其中，常量重力通过使用区间初始时刻的位置获得。
[0134]
通过迭代求解上述线性最小二乘，获得区间内速度的切比雪夫系数，包括：
[0135]
模块m.1：使用区间初始时刻的已知位置，计算重力，并把整个区间的重力当作常数；
[0136]
模块m.2：求解上述线性最小二乘，获得速度的切比雪夫系数，进而求得区间内的速度和位置；
[0137]
模块m.3：使用模块m.2得到的位置，计算重力在切比雪夫点处的值；
[0138]
模块m.4：重复调用模块m.2和模块m.3，直到连续两次循环得到的切比雪夫系数之差小于给定的阈值或者达到最大迭代次数。
[0139]
与现有技术相比，本发明具有如下的有益效果：
[0140]
本发明提出了一种基于切比雪夫多项式优化的非线性惯性导航算法，可以将惯性导航解算误差降低到计算机截断误差水平，且运行效率明显高于其它高精度惯性导航算法。
附图说明
[0141]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0142]
图1为本发明方法流程图。
具体实施方式
[0143]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
[0144]
实施例：
[0145]
如图1，根据本发明提供的基于多项式优化的高精度快速惯导解算方法，包括：
[0146]
步骤1：构建惯性导航在地球坐标系下的表达式，对陀螺和加速度计观测插值，拟合出角速度和比力的切比雪夫多项式函数，获得切比雪夫点处的角速度与比力；
[0147]
步骤2：求解线性最小二乘，得到姿态四元数增量的切比雪夫多项式；求解非线性最小二乘，得到姿态罗德里格斯增量的切比雪夫系数；
[0148]
步骤3：求解非线性最小二乘，获得速度的系数；迭代求解线性最小二乘，获得速度系数；
[0149]
步骤4：根据得到的姿态/速度/位置的切比雪夫多项式系数以及对应的切比雪夫多项式，计算得到对应时间区间上的姿态/速度/位置信息；
[0150]
其中，所述惯性导航方程在地球坐标系下的表述为：
[0151][0152][0153][0154]
其中，上标/下标b表示载体坐标系(b系)，e表示地球坐标系(e系)，i表示惯性坐标系(i系)，n表示导航坐标系(n系)。不失一般性，本发明定义导航坐标系为北-天-东。分别表示b系相对于e系的姿态四元数和姿态旋转矩阵，表示陀螺测量的角速度，表示地球自转角速度，ve表示载体相对于地球的速度表示在e系下(又称为地速度)，表示加速度计测量的比力，γe与pe分别表示e系下的地球重力向量以及载体在e系下的位置，符号ο表示四元数的乘法。
[0155]
假设在当前采样时间窗口内，有陀螺测量的角速度或者角增量观测加速度计测量的比力或者速度增量其中，k＝0,1,
…
nm，使用仿射变换，将时间窗口映射到τ∈[-1 1]，则有：
[0156][0157]
为了后续计算的需要，本发明首先对陀螺和加速度计观测插值，以获得切比雪夫
点处的角速度与比力，分别表示为其中，切比雪夫点定义为τj＝-cos(jπ/n),j＝0,1,
…
,n，n是切比雪夫多项式的阶数；τ为映射后的时间自变量。
[0158]
1)基于多项式优化的姿态求解算法
[0159]
在一个解算时间窗口内，姿态四元数的增量及其导数使用切比雪夫多项式表示为：
[0160][0161]
其中，为区间内切比雪夫增量，nq为切比雪夫的阶数，li为待求的切比雪夫系数，其中，ti与为第i阶重构之后的切比雪夫多项式及其导数，重构的切比雪夫多项式ti定义如下：
[0162][0163]
其中，fi为原始的切比雪夫多项式，其循环定义如下：
[0164]
f0(τ)＝1,f1(τ)＝τ,
[0165]fi 1
(τ)＝2τfi(τ)-f
i-1
(τ)for i≥1
[0166]
基于以上模型，姿态四元数增量的切比雪夫系数可通过求解如下线性最小二乘问题得到：
[0167][0168]
其中，||
·
||表示向量的2-范数，为待求的切比雪夫系数矩阵；
[0169][0170][0171][0172]
其中，对于任意的四元数四元数操作符号定义为：
[0173][0174]
其中，s与η分别表示四元数的标量与矢量部分，(
·
)
×
表示三维向量对应的反对称矩阵，in表示n维单位矩阵。通过求解上述线性最小二乘问题求得l的估计，然后归一化获得区间内的四元数增量：
[0175][0176]
或者，区间内的姿态增量还可以通过求解区间内罗德里格斯向量增量的方法获得：
[0177][0178]
其中，与分别表示为区间内姿态罗德里格斯增量及其导数，使用重构切比雪夫多项式表示如下：
[0179][0180]
其中，是待求的罗德里格斯增量的切比雪夫系数，ng是切比雪夫阶数，为区间初始时刻的姿态旋转矩阵，将罗德里格斯向量增量转化为旋转矩阵。求解上述非线性最小二乘问题，并将获得的罗德里格斯增量转变为区间内的四元数增量，即：
[0181][0182]
最后，区间内的姿态四元数由下式求得：
[0183][0184]
其中，为区间内的初始姿态四元数。
[0185]
2)基于多项式优化的速度/位置求解算法
[0186]
类似姿态求解算法，区间内的速度和位置以及速度的导数可以表示为重构切比雪夫多项式：
[0187][0188][0189][0190]
其中，nv为切比雪夫多项式的阶数，ki为待求的切比雪夫多项式系数，其中，分别表示区间初始时刻的速度和位置。是i阶重构切比雪夫多项式的积分，定义如下：
[0191]
[0192]
其中，为i阶切比雪夫多项式的积分；
[0193][0194]
使用上述切比雪夫多项式表示速度/位置，其系数可以通过求解如下非线性最小二乘获得：
[0195][0196]
其中，是待求的切比雪夫系数，表示由姿态算法求出的姿态旋转矩阵，e系下重力γe是关于位置的函数。上述算法的初值可以通过假设区间内的重力为常数，然后通过求解如下线性最小二乘获得：
[0197][0198]
其中：
[0199][0200][0201][0202][0203]
其中，常量重力可以通过使用区间初始时刻的位置获得。
[0204]
或者，本发明还能通过循环调用上述线性最小二乘获得速度的切比雪夫系数k，进而求得速度与位置。
[0205]
将重力在区间内当作已知量，迭代求解线性最小二乘，步骤如下：
[0206]
步骤3.1：使用区间初始时刻的已知位置，计算重力，并把整个区间的重力当作常数；
[0207]
步骤3.2：求解上述线性最小二乘，获得速度的切比雪夫系数，进而求得区间内的速度和位置；
[0208]
步骤3.3：使用步骤3.2得到的位置，计算重力在切比雪夫点处的值；
[0209]
步骤3.4：重复执行步骤3.2和步骤3.3，直到连续两次循环得到的切比雪夫系数之差小于给定的阈值或者达到最大迭代次数。
[0210]
针对惯性导航的姿态与速度/位置求解，本发明提出了基于切比雪夫多项式优化的非线性最小二乘与线性最小二乘惯导解算方法。为了简化表示，本发明将使用非线性最小二乘的姿态算法和使用非线性最小二乘的速度/位置算法称作基于切比雪夫优化的非线性惯性导航算法。将使用线性最小二乘的姿态算法和迭代线性最小二乘的速度/位置算法称作基于切比雪夫优化的线性惯性导航算法。
[0211]
本发明提出的基于切比雪夫多项式优化的非线性惯性导航算法的优点是：可以将惯性导航解算误差降低到计算机截断误差水平，且运行效率明显高于其它高精度惯性导航算法。
[0212]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0213]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。