一种组合导航方法和系统与流程
本发明涉及导航技术领域，特别涉及一种组合导航方法和系统。
背景技术：
在当前的现有技术中，有卫星导航、惯性导航、地磁导航、地形匹配导航等多种导航方式。由于惯性导航是完全自主的导航方式，而且具有信息相对连续，不易受到干扰，短时间内精度较高等优点，因此，惯性导航在目前的所有导航方式中依然占有重要的地位。捷联惯性导航(SINS)系统是惯性导航中的一种，具有惯性导航的基本特点。但是，捷联惯性导航是一种递推式的计算，其误差将随着时间的增加而迅速增加，不能满足长时间保持一定导航精度的要求。
组合导航的出现解决了目前单一导航方式遇到的部分问题。组合导航是将两种或者两种以上的导航方式结合起来使用，达到取长补短的目的。以最常见的卫星导航为例，卫星导航具有全天候、在较大的空间范围内有效、导航精度较高、误差不随时间积累等优点。然而与捷联惯性导航系统相比，卫星导航系统也具有信息更新频率较低、大动态条件下容易失锁、信号容易被干扰等缺点。而将捷联惯性导航与卫星导航组成组合导航系统，则可以实现长时间高精度的自主导航。
组合导航系统需要用到信息融合技术。目前的融合方法有卡尔曼滤波、鲁棒滤波、粒子滤波等多种方式。卡尔曼滤波是一种线性最小方差滤波，对于白噪声驱动条件下的线性系统，其滤波值是最优的。但是，卡尔曼滤波需要系统噪声协方差阵和观测噪声协方差阵来分别描述时间更新的精度和观测值的精度。然而，现有技术中的系统噪声协方差阵和观测噪声协方差阵都是根据经验设置，因此导致方差阵的设置不够准确合理，从而影响滤波精度。
现有技术中通常采用的是捷联惯性导航和卫星导航的松组合方式，该方法只是对惯性导航的速度位置等变量进行了修正，而并没有考虑陀螺和加速度计的常值漂移和比例项的变化等误差源，这些也是捷联惯性导航误差发散的原因之一；另外，目前的捷联惯性卫星组合导航系统没有对信息进行充分的利用，仅仅是在滤波时刻使用一次后就丢弃。
技术实现要素：
有鉴于此，本发明提供一种组合导航方法和系统，从而可以提高惯导导航精度和优化滤波效果。
本发明的技术方案具体是这样实现的：
一种组合导航方法，该方法包括如下所述步骤：
A、第一计算单元将第一导航系统的数据和第二导航系统的数据进行信息融合，得到融合后的组合导航数据；
B、第二计算单元根据记录的第一导航系统的历史数据和第二导航系统的历史数据，估算得到陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的估计值；对估算得到的各个估计值进行评价，判断各个估计值是否为更准确的参数；根据判断结果将估计值反馈给第一计算单元，对相应的参数进行修正。
较佳的，所述第一导航系统为捷联惯性导航系统SINS，第二导航系统为卫星导航系统。
较佳的，所述步骤A包括如下的步骤：
A1、在第一导航系统的数据中，根据陀螺脉冲数计算角增量，并根据加速度计脉冲数计算速度增量；
A2、根据角增量和速度增量计算下一时刻的姿态、速度和位置；
A3、根据计算得到的姿态、速度和位置以及第二导航系统的数据，进行组合导航信息融合，得到融合后的组合导航数据；
A4、记录每个时刻产生的陀螺脉冲数和加速度计脉冲数，记录捷联惯导系统每次计算得到的速度位置，记录由卫星导航系统获得的每个速度位置，记录滤波值。
较佳的，所述根据记录的第一导航系统的历史数据和第二导航系统的历史数据，估算得到陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的估计值包括如下步骤：
B1、根据记录的第一导航系统的历史数据和第二导航系统的历史数据，设置遗传算法需要的初始种群；
B2、初始种群中的每个个体根据记录的第一导航系统的历史数据和第二导航系统的历史数据进行计算，得到每个个体各自的速度、位置和姿态，同时也得到每个个体滤波后的速度、位置和姿态；
B3、对于每个个体，判断该个体是否满足预设条件；如果有至少一个个体满足预设条件，则认为估计成功，则将该个体对应的零次项、一次项、噪声阵反馈给第一计算单元，对相应的参数进行修正；如果存在多个个体满足所有要求，则选择适应度最好的个体；
如果没有一个个体满足预设条件，则执行步骤B4；
B4、进行遗传操作，生成下一代种群。
较佳的，所述步骤B1包括如下的步骤：
对系统噪声阵的当前值进行编码；
根据历史数据，对陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵中的每个量，均以当前值的1倍为步长，当前值为中间值，产生n个数；
所有待估计的陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的n组数组成A矩阵；
对A矩阵进行均匀化得到矩阵population，将矩阵population作为遗传算法的初始种群。
较佳的，所述步骤B2包括如下的步骤：
设置n个估计子单元，将从第二导航系统获得的位置rg记作标准值；取估计子单元同时刻的位置ri与第二导航系统中的位置rg做差，得到位置误差向量δri；对滤波是否收敛进行判断；当滤波收敛时，取稳态部分的位置误差并计算每个时刻的误差2-范数；当滤波发散时，取最后的50个滤波点的位置误差，计算每个时刻的误差2-范数；将所述位置误差2-范数记为δrnij，其中，j＝1,2,…50；求δrnij的平均值和二阶原点矩，分别记为Eri和Sri；
将每个估计子单元滤波修正后的惯性系统相关项，代入到各自的惯导递推中，从初始时刻开始计算，直到当前时刻；记得到的当前时刻的位置为ri'，与第二导航系统的位置rg做差并计算误差的2-范数，记为
对每个估计子单元，计算其适应度标量fitnessi＝k1δri'+k21Eri+k22Sri；其中，k1、k21、k22是权值，fitnessi是描述第i个估计子单元分配的个体i的适应度标量；将n个个体的适应度标量组成描述种群适应度的适应度向量fitness；
将适应度向量中的最小值对应的个体作为适应性最好的个体：Sopt＝min{fitness}。
较佳的，所述步骤B4包括如下的步骤：
B41、对父代种群population中的个体进行选择得到种群S_population；
B42、对种群S_population中的个体进行交叉操作得到种群C_population；
B43、对种群C_population中的个体进行变异操作得到子代种群N_population。
较佳的，所述步骤B41包括：
设置最优个体有30％的概率被选择到下一代，次优个体有20％的概率被选择到下一代，适应性第三的个体有15％的概率到下一代，适应性第四的个体有10％的概率到下一代，剩余个体有25％的概率到下一代；
产生n个0～1之间的随机数，进行n次选择，对每次选择，如果随机数在0～0.3之间，则选择最优个体；如果随机数在0.3～0.5之间，则选择次优个体，依次类推；如果随机数在0.75～1之间，则从剩余个体中随机的选择一个个体遗传到下一代。
较佳的，所述步骤B42包括：
预先设置交叉点个数c_num，交叉概率Pc；
将交叉概率乘个体数，然后取整，得到进行交叉的个体数目cross_num；
随机地从种群S_population中选取cross_num个个体，随机地确定c_num个交叉点，个体之间两两交叉；
对每个交叉点，具体交叉方式为：
其中a和b是初始值，a'和b'是交叉后得到的值，α和β是0～1之间的随机数。
较佳的，所述步骤B43包括：
预先设置变异概率Pm；
预先设置变异的方差σ；
产生与基因总数gen相同数目的0～1之间的随机数；
如果第i个随机数小于变异概率Pm，则第i个基因进行变异；如果第i个随机数大于变异概率Pm，则不对第i个基因进行操作；其中，i取1,2,3...gen；
具体变异方式为：
c'＝N(c,σ)；
其中c是初始值，c'是变异后得到的值。c'取以c为期望，方差为σ2的一个随机数。
较佳的，所述第一导航系统为捷联惯性导航系统SINS；
第二导航系统为地磁组合导航系统或景象匹配组合导航系统。
本发明还提供了一种组合导航系统，该系统包括：
第一计算单元和第二计算单元；
所述第一计算单元，用于将第一导航系统的数据和第二导航系统的数据进行信息融合，得到融合后的组合导航数据；
所述第二计算单元，用于根据记录的第一导航系统的历史数据和第二导航系统的历史数据，估算得到陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的估计值；对估算得到的各个估计值进行评价，判断各个估计值是否为更准确的参数；根据判断结果将估计值反馈给第一计算单元，对相应的参数进行修正。
较佳的，所述第一计算单元进一步包括：第一数据处理模块、第二数据处理模块、解算模块和滤波模块；
所述第一数据处理模块，用于根据接收到的第一导航系统的数据计算角增量和速度增量，并将角增量和速度增量发送给解算模块；
所述第二数据处理模块，用于将接收到的第二导航系统的数据发送给滤波模块；
所述解算模块，用于根据角增量和速度增量计算下一时刻的姿态、速度和位置，并将计算得到的姿态、速度和位置发送给滤波模块；
所述滤波模块，用于根据接收到的数据，进行组合导航信息融合，得到融合后的组合导航数据。
较佳的，所述第二计算单元进一步包括：计算模块、评价模块和数据存储模块；
所述数据存储模块，用于存储第一导航系统的历史数据和第二导航系统的历史数据；
所述计算模块，用于根据记录的第一导航系统的历史数据和第二导航系统的历史数据，估算得到陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的估计值；
所述评价模块，用于对估算得到的各个估计值进行评价，判断各个估计值是否为更准确的参数；根据判断结果将估计值反馈给第一计算单元，对相应的参数进行修正。
较佳的，所述计算模块进一步包括：种群操作子单元、解算子单元、结果评估子单元和n个估计子单元；
所述种群操作子单元，用于根据记录的第一导航系统的历史数据和第二导航系统的历史数据，设置遗传算法需要的初始种群，并根据结果评估子单元返回的数据进行遗传操作，生成下一代种群；
所述解算子单元，用于根据角增量和速度增量计算下一时刻的姿态、速度和位置，并将计算得到的姿态、速度和位置发送给n个估计子单元；
所述n个估计子单元分别对应于初始种群中的n个个体，用于根据记录的第一导航系统的历史数据和第二导航系统的历史数据进行计算，得到每个个体各自的速度、位置和姿态，同时也得到每个个体滤波后的速度、位置和姿态；
所述结果评估子单元，用于对于每个个体，判断该个体是否满足预设条件；如果有至少一个个体满足预设条件，则认为估计成功，则将该个体对应的零次项、一次项、噪声阵反馈给第一计算单元，对相应的参数进行修正；如果存在多个个体满足所有要求，则选择适应度最好的个体；如果没有一个个体满足预设条件，则将n个估计子单元输出的数据返回给所述种群操作子单元。
较佳的，所述第一数据处理模块将接收到的第一导航系统的数据存储在数据存储模块中；
所述第二数据处理模块将接收到的第二导航系统的数据存储在数据存储模块中。
较佳的，所述第一导航系统数据处理模块为捷联惯性导航系统数据处理模块；
所述第二导航系统数据处理模块为GPS数据处理模块。
如上可见，在本发明中的组合导航方法和系统中，由于可以根据记录的第一导航系统的历史数据和第二导航系统的历史数据，估算得到陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的估计值；对估算得到的各个估计值进行评价，判断各个估计值是否为更准确的参数；根据判断结果将估计值反馈给第一导航系统和第二导航系统，对对应的参数进行修正，因此可以实现以SINS为基础的组合导航，并且更加充分地利用了两个导航系统(例如，SINS和GPS)的历史数据信息，通过对历史数据信息的挖掘和再利用，可以实现对陀螺和加速度计的误差补偿，实现对滤波系统噪声协方差阵和观测噪声协方差阵的优化设置，实现了陀螺常值漂移和比例项、加速度计常值漂移和比例项、滤波模型系统方程噪声阵和滤波模型观测方程的在线估计并反馈给相应的系统，可以对陀螺和加速度计的工具误差进行在线实时修正，因此即使是在没有GPS信息时，也可以使得SINS具有比没有反馈时更高的精度，达到提高惯导导航精度的目的。
另外，在本发明的技术方案中，还进一步地对滤波的系统方程噪声阵和观测方程噪声阵进行了在线实时修正，降低了对滤波器设计时的要求，提高了噪声协方差阵描述相应噪声的准确性，从而提高了滤波精度。对滤波的发散也有一定的抑制效果，更适合长时间滤波。
附图说明
图1为本发明实施例中的组合导航方法的流程示意图。
图2为本发明实施例中的步骤11的具体流程示意图。
图3为本发明实施例中的步骤12的具体流程示意图。
图4为本发明实施例中的组合导航系统的结构示意图。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚明白，以下参照附图并举实施例，对本发明进一步详细说明。
本实施例提供了一种组合导航方法和系统。
图1为本发明实施例中的组合导航方法的流程示意图。如图1所示，本发明实施例中的组合导航方法主要包括如下所述的步骤：
步骤11，第一计算单元将第一导航系统的数据和第二导航系统的数据进行信息融合，得到融合后的组合导航数据。
在本发明的技术方案中，实际上可以设置两个独立的计算单元，进行并行计算，如图2所示。其中，第一个计算单元的功能是进行常规的组合导航，即目前工程上已经使用的组合导航方式，将第一导航系统(例如，SINS)的数据和第二导航系统(例如，GPS)的数据进行信息融合，得到融合后的组合导航数据(即执行上述的步骤11)；而第二个计算单元的功能则是对所需要的参数(如加速度计的零次项等)进行估计等操作(即执行后述的步骤12)。其中，第一个计算单元中的第一数据处理模块用于处理第一导航系统的数据，第二数据处理模块则用于处理第二导航系统的数据。
因此，在本发明的技术方案中，可以将第一个计算单元看作为一个组合导航系统，而将第二个计算单元看作为一个辅助导航系统。该组合导航系统和辅助导航系统可以并行工作；而且，当辅助导航系统损坏或者失去功能之后，组合导航系统仍能正常进行工作。
较佳的，在本发明的具体实施例中，所述第一导航系统为SINS，第二导航系统为卫星导航系统(譬如，GPS系统)，也可以是其它的导航系统。
因此，在本步骤中，可以将SINS数据和GPS数据进行信息融合，得到融合后的组合导航数据，并保证信息更新速率。
在本发明的技术方案中，可以通过多种具体的实现方式来实现上述的步骤11。以下将以其中的一种具体实现方式为例，对本发明的技术方案进行详细地介绍。在该具体的示例中，第一导航系统为SINS，第二导航系统为卫星导航系统(譬如，GPS系统)。第二导航系统为其它导航系统的情况可以依此类推，因此不再赘述。
例如，较佳的，图2为本发明实施例中的步骤11的具体流程示意图，如图2所示，在本发明的具体实施例中，所述步骤11可以包括如下所述的步骤：
步骤111，在第一导航系统(SINS)的数据中，根据陀螺脉冲数计算角增量，并根据加速度计脉冲数计算速度增量。
例如，在本发明的一个较佳具体实施例中，可以使用如下所述的公式来根据陀螺脉冲数计算角增量：
其中，D0x、D0y、D0z为陀螺常值项误差系数，单位：角秒/秒；Dxx、Dxy、Dxz、Dyx、Dyy、Dyz、Dzx、Dzy、Dzz为陀螺常值项误差系数，单位：1；Ngx、Ngy、Ngz为陀螺输出脉冲个数，kg为陀螺当量；T为采样周期。
又例如，在本发明的一个较佳具体实施例中，可以使用如下所述的公式来根据加速度计脉冲数计算速度增量：
其中，E0x、E0y、E0z为加速度计的零次项误差系数，单位为g0z；Exx、Exy、Exz、Eyx、Eyy、Eyz、Ezx、Ezy、Ezz为加速度计的一次项误差系数，单位为g0z；ΔNax、ΔNay、ΔNaz为加速度计的输出脉冲个数；ka为加计当量；g0z是加速度计的相关参数，表示一个标准的重力加速度单位，单位：m/s2；T为采样周期。
步骤112，根据角增量和速度增量计算下一时刻的姿态、速度和位置。
例如，在本发明的一个较佳具体实施例中，可以使用如下所述的步骤计算下一时刻的姿态、速度和位置：
步骤112a，根据角增量计算下一时刻的姿态；
在本步骤中，将首先根据角增量计算下一时刻的姿态。
例如，在本发明的一个较佳具体实施例中，可以使用如下所述的公式来计算下一时刻的姿态：
其中，qk是当前时刻的姿态四元数，qk+1是下一时刻的姿态四元数。
另外，在本发明的技术方案中，姿态四元数qk与表示姿态的方向余弦阵之间可以相互转换且是一一对应，后续将不再说明。
步骤112b，根据速度增量计算下一时刻的速度和位置。
例如，在本发明的一个较佳具体实施例中，视速度增量在载体系的值经姿态转换为导航坐标系的视速度增量，乘以计算周期并加上初值即得到导航坐标系的速度，对速度进行积分得到位置。
由于不同载体所用的导航坐标系不尽相同，而且该导航方法属于本领域的公知技术，因此此处不再赘述。在本发明的一个具体较佳实施例中，所使用的导航坐标系可以是发射惯性系。
通过上述的步骤112a和112b，即可根据角增量和速度增量计算得到下一时刻的姿态、速度和位置。
步骤113，根据计算得到的姿态、速度和位置以及第二导航系统的数据，进行组合导航信息融合，得到融合后的组合导航数据。
例如，在本发明的一个较佳具体实施例中，可以使用卡尔曼滤波方法进行组合导航信息融合，得到融合后的组合导航数据。
举例来说，在本发明的一个较佳具体实施例中，上述的步骤113可以包括如下所述的步骤：
步骤113a，建立连续系统的系统方程和观测方程；
例如，在本发明的一个较佳具体实施例中，所述系统方程和观测方程可以表示为：
Z(t)＝H(t)X(t)+v(t) (5)
其中，
X(t)＝(δVx,δVy,δVz,δrx,δry,δrz,δφx,δφy,δφz,δK0x,δK0y,δK0z,δD0x,δD0y,δD0z)T (6)
Z(t)＝(δVx,δVy,δVz,δrx,δry,δrz)T (8)
v(t)＝(ΔVgx,ΔVgy,ΔVgz,Δrgx,Δrgy,Δrgz)T (9)
其中，状态向量X(t)中δVx,δVy,δVz为速度误差、δrx,δry,δrz为位置误差、δφx,δφy,δφz为姿态角误差、δK0x,δK0y,δK0z为加速度零位误差、δD0x,δD0y,δD0z为陀螺常值误差；系统白噪声w(t)中为加速度计的噪声，εx1,εy1,εz1为陀螺的噪声；观测向量Z(t)中δVx,δVy,δVz为惯性解算速度与卫星测得的速度之差，δrx,δry,δrz为惯性解算位置与卫星测得的位置之差；观测白噪声v(t)中ΔVgx,ΔVgy,ΔVgz为速度精度误差，Δrgx,Δrgy,Δrgz为位置精度误差；系统矩阵F(t)中I3×3为单位矩阵，为载体系到惯性系的方向余弦阵，fi×为视加速度在惯性系里的值组成的反对称矩阵；系统噪声转移阵G(t)和观测矩阵H(t)中元素含义同上。
步骤113b，对系统方程和观测方程进行离散化。
例如，在本发明的一个较佳具体实施例中，对于上述的方程(4)、(5)进行离散化后得到：
Xk+1＝Φk+1,kXk+Γkwk (13)
Zk+1＝Hk+1Xk+1+vk+1 (14)
其中，状态转移阵输入矩阵系统噪声协方差矩阵观测噪声协方差矩阵T为滤波周期，Fk≈F(t)，Gk≈G(t)，q为w(t)的方差强度阵，Hk+1＝H(t)，vk+1＝v(t)。
步骤113c，对离散化后的系统方程和观测方程进行滤波。
例如，在本发明的一个较佳具体实施例中，对于上述的方程(13)、(14)进行滤波，得到：
Pk+1,k+1＝Pk+1,k-Kk+1Hk+1Pk+1,k (19)
通过上述的步骤113a～113c，即可进行组合导航信息融合，得到融合后的组合导航数据。
通过上述的步骤111～113，即可将SINS数据和GPS数据进行信息融合，得到融合后的组合导航数据，并保证信息更新速率。
步骤12，第二计算单元根据记录的第一导航系统的历史数据和第二导航系统的历史数据，估算得到陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的估计值；对估算得到的各个估计值进行评价，判断各个估计值是否为更准确的参数；根据判断结果将估计值反馈给第一计算单元(例如，第一计算单元中的第一数据处理模块和滤波模块)，对相应的参数进行修正。
较佳的，在本发明的具体实施例中，在所述步骤12之前，还可以进一步包括：
记录每个时刻产生的陀螺脉冲数ΔNgx,ΔNgy,ΔNgz和加速度计脉冲数ΔNax,ΔNay,ΔNaz，记录捷联惯导系统每次计算得到的速度位置，记录由卫星导航系统获得的每个速度位置，记录滤波值。
在本步骤中，第二计算单元将根据所记录的第一导航系统的历史数据和第二导航系统的历史数据，对陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵进行估计，并判断所得到的估计值是否为更准确的参数；然后，再根据判断结果将估计值反馈给第一计算单元，对对应的参数进行修正。
在本发明的技术方案中，可以通过多种具体的实现方式来实现上述的步骤12。以下将以其中的一种具体实现方式为例，对本发明的技术方案进行详细地介绍。在该具体的示例中，第一导航系统为SINS，第二导航系统为卫星导航系统(譬如，GPS系统)。
例如，较佳的，图3为本发明实施例中的步骤12的具体流程示意图，如图3所示，在本发明的具体实施例中，所述步骤12中的根据记录的第一导航系统的历史数据和第二导航系统的历史数据，估算得到陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的估计值可以包括如下所述的步骤：
步骤121，根据记录的第一导航系统的历史数据和第二导航系统的历史数据，设置遗传算法需要的初始种群。
在本发明的技术方案中，可以使用遗传算法来进行上述的估计。因此，在本步骤中，将首先设置遗传算法需要的初始种群。
例如，具体来说，在本发明的一个较佳实施例中，所述步骤121可以包括如下所述的步骤：
步骤121a，对系统噪声阵的当前值进行编码。
进行该操作的主要目的是进行无量纲化处理，并使所有参数在数值上不会差别太大。
步骤121b，根据历史数据，对上述陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵中的每个量，均以当前值的1倍为步长，当前值为中间值，产生n个数；
步骤121c，所有待估计的陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的n组数组成A矩阵；
步骤121d，对A矩阵进行均匀化得到矩阵population，将矩阵population作为遗传算法的初始种群。
通过上述的步骤121a～121d，即可得到遗传算法的初始种群。
上述初始种群均匀化后，可以在一定程度上提高遗传算法的收敛速度，减少迭代次数，从而减小计算量，节省计算时间。所以均匀化可以作为遗传算法的优化方法。
另外，本发明中所使用的均匀化方法可以采用常用的均匀设计法，例如，方开泰提供的均匀设计法，具体使用的均匀设计表可参见方开泰的《均匀设计》，在此不再赘述。
步骤122，初始种群中的每个个体根据记录的第一导航系统的历史数据和第二导航系统的历史数据进行计算，得到每个个体各自的速度、位置和姿态，同时也得到每个个体滤波后的速度、位置和姿态。
在本步骤中，可以由初始种群(实际上包含了零次项、比例项)中的每个个体(每个个体可以对应一个下述的估计子单元)利用存储的历史数据(即记录的第一导航系统的历史数据和第二导航系统的历史数据)进行计算，从而得到每个个体各自的速度、位置和姿态，同时也得到每个个体滤波后的速度、位置和姿态(每个个体滤波采用的噪声阵或者叫协方差阵也是各不相同的)。
例如，具体来说，在本发明的具体实施例中，如果所设置的初始种群中有n个个体，则可以先设置n个估计子单元(分别对应于初始种群中的n个个体)，其中的每一个估计子单元实质上都相当于一个SINS/GPS松组合导航系统(即图4中所示的第一计算单元)，都包含惯性递推和滤波两部分；然后，为每个估计子单元分配矩阵population中的一个体S；然后，对所有估计子单元，采用相同的初始时刻和初始值；位置和速度的初始值采用同时刻从卫星导航系统(例如，GPS系统)得到的位置和速度；随后，各个估计子单元对状态量中的零次项等进行滤波修正，得到相应的滤波值；之后，即可根据记录的历史数据，进行递推和滤波计算，重复滤波过程，记录每个估计子单元所有时刻产生的速度Vi和位置ri。
例如，较佳的，在本发明的具体实施例中，所述步骤122可以包括如下的步骤：
步骤122a，设置n个估计子单元，将从卫星导航系统(例如，GPS系统)获得的位置rg记作标准值；取估计子单元同时刻的位置ri与GPS系统中的位置rg做差，得到位置误差向量δri；对滤波是否收敛进行判断；当滤波收敛时，取稳态部分的位置误差并计算每个时刻的误差2-范数；当滤波发散时，取最后的50个滤波点的位置误差，计算每个时刻的误差2-范数；将所述位置误差2-范数记为δrnij，其中，j＝1,2,…50；求δrnij的平均值和二阶原点矩，分别记为Eri和Sri。
步骤122b，将每个估计子单元滤波修正后的惯性系统相关项(例如，加速度计零次项等)，代入到各自的惯导递推中，从初始时刻开始计算，直到当前时刻；记得到的当前时刻的位置为ri'，与卫星导航系统(例如，GPS系统)的位置rg做差并计算误差的2-范数，记为
步骤122c，对每个估计子单元，计算其适应度标量fitnessi＝k1δri'+k21Eri+k22Sri；其中Eri、Sri、δri'如上所述，k1、k21、k22是权值，fitnessi是描述第i个估计子单元分配的个体i的适应度标量；将n个个体的适应度标量组成描述种群适应度的适应度向量fitness。
步骤122d，将适应度向量中的最小值对应的个体作为适应性最好的个体：Sopt＝min{fitness}。
步骤123，对于每个个体，判断该个体是否满足预设条件；如果有至少一个个体满足预设条件，则认为估计成功，则将该个体对应的零次项(例如，包括陀螺常值项和加速度计常值项)、一次项(例如，包括陀螺比例项和加速度计比例项)、噪声阵(例如，包括系统噪声阵和观测噪声阵)反馈给第一计算单元，对相应的参数进行修正；如果存在多个个体满足所有要求，则选择适应度最好的个体；
如果没有一个个体满足预设条件，则执行步骤124；
也就是说，在本步骤中，将判断每个个体中的速度位置是否有跳变、估计子单元中滤波是否收敛、系统状态协方差阵是否正定；将存储的第二导航系统(例如，GPS)的速度位置作为基准，与估计子单元中得到的速度位置做差，按照预设的判断方法进行判断，如果存在至少一个个体满足预设的所有条件，则认为估计(即步骤12)成功，返回对应的零次项、一次项、噪声阵(例如，将其中的零次项和一次项反馈给第一计算单元中的第一数据处理模块，而将噪声阵反馈给第一计算单元中的滤波模块)，如果存在多个个体满足所有要求，则选择适应度最好的个体。如果没有一个个体满足预设条件，则执行步骤124。
其中，所述预设的判断方法为：
遗传算法的适值函数为：
f1(Q,P0)＝k11E(||δVf||2)+k12E(||δrf||2)+k13D(||δVf||2)+k14D(||δrf||2)
其中，k1i是加权系数，i＝1,2,3,4。
遗传算法的优化目标是找到使上式取得极小值的个体：
其中，Ω是遗传算法的搜索空间。
观测值的方差记为速度误差时间序列的平均值为exp(||δV||2)，位置误差时间序列的平均值为exp(||δr||2)。为描述速度误差和位置误差的离散程度，并排除滤波可能会出现有偏的情况，取其二阶原点矩作为描述指标，分别记为cov(||δV||2)和cov(||δr||2)。
限制条件的指标可以写为函数：
f2(Q)＝k21exp(||δV||2)+k22exp(||δr||2)+k23cov(||δV||2)+k24cov(||δr||2)
其中，k2i与k1i相关，i＝1,2,3,4。
为限制Q异常变小，应满足约束条件：
J≥f2(Q)
总结起来说，应该满足f1足够小，同时满足f1大于f2。
步骤124，进行遗传操作，生成下一代种群。
在本步骤中，将进行遗传操作，生成下一代种群。
例如，较佳的，在本发明的具体实施例中，所述步骤124可以包括如下的步骤：
步骤124a，对父代种群population中的个体进行选择得到种群S_population。
例如，较佳的，在本发明的一个具体实施例中，可以设置最优个体有30％的概率被选择到下一代，次优个体有20％的概率被选择到下一代，适应性第三的个体有15％的概率到下一代，适应性第四的个体有10％的概率到下一代，剩余个体有25％的概率到下一代；然后，产生n个0～1之间的随机数，进行n次选择，对每次选择，如果随机数在0～0.3之间，则选择最优个体；如果随机数在0.3～0.5之间，则选择次优个体，依次类推；如果随机数在0.75～1之间，则从剩余个体中随机的选择一个个体遗传到下一代。
步骤124b，对种群S_population中的个体进行交叉操作得到种群C_population。
更进一步的，在本发明较佳的具体实施例中，可以预先设置交叉点个数c_num，交叉概率Pc；将交叉概率乘个体数，然后取整，得到进行交叉的个体数目cross_num；随机地从种群S_population中选取cross_num个个体，随机地确定c_num个交叉点，个体之间两两交叉；对每个交叉点，具体交叉方式为：
其中a和b是初始值，a'和b'是交叉后得到的值，α和β是0～1之间的随机数。
步骤124c，对种群C_population中的个体进行变异操作得到子代种群N_population。
更进一步的，在本发明较佳的具体实施例中，可以预先设置变异概率Pm；由于是实数编码，可预先设置变异的方差σ；然后产生与基因总数gen相同数目的0～1之间的随机数；如果第i个随机数小于变异概率Pm，则第i个基因进行变异；如果第i个随机数大于变异概率Pm，则不对第i个基因进行操作；其中，i取1,2,3...gen；具体变异方式为：
c'＝N(c,σ) (21)
其中c是初始值，c'是变异后得到的值。c'取以c为期望，方差为σ2的一个随机数。
通过上述的步骤124a～124c，即可进行遗传操作，生成下一代种群。
在上述的描述中，均是以第一导航系统为SINS，第二导航系统为卫星导航系统为例进行的介绍。然而，在本发明的技术方案中，也可以选用其它的导航系统作为第二导航系统。例如，所述第二导航系统可以是：地磁组合导航系统或景象匹配组合导航系统等其它导航系统。此时，可以对应地对信息融合方式进行相应的修改，同样可以对融合过程中的某些参数进行估计。
另外，在本发明的技术方案中，除了上述所描述的遗传算法之外，还可以使用其它的估计方法，例如，较佳的，在本发明的具体实施例中，还可以使用粒子群算法等其它智能方法作为估计方法，在此不再赘述。
另外，在本发明的技术方案中，可以设置两个独立的计算单元，进行并行计算。其中，第一个计算单元的功能是进行常规的组合导航，即目前工程上已经使用的组合导航方式，将第一导航系统的数据和第二导航系统的数据进行信息融合，得到融合后的组合导航数据；而第二个计算单元的功能则是对所需要的参数(如加速度计的零次项等)进行估计(即执行上述的步骤12)。
此外，在本发明的技术方案中，还可以设置数据存储单元，用于在线存储第一导航系统和第二导航系统中的相关数据，例如，SINS中的陀螺和加速度计产生的脉冲数，GPS系统中获得的速度位置信息等。
本发明中还提出了一种组合导航系统。图4为本发明实施例中的组合导航系统的结构示意图，如图4所示，本发明实施例中的组合导航系统包括：第一计算单元和第二计算单元；
所述第一计算单元，用于将第一导航系统的数据和第二导航系统的数据进行信息融合，得到融合后的组合导航数据；
所述第二计算单元，用于根据记录的第一导航系统的历史数据和第二导航系统的历史数据，估算得到陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的估计值；对估算得到的各个估计值进行评价，判断各个估计值是否为更准确的参数；根据判断结果将估计值反馈给第一计算单元，对相应的参数进行修正。
更进一步的，较佳的，在本发明的具体实施例中，所述第一计算单元还可以进一步包括：第一数据处理模块、第二数据处理模块、解算模块和滤波模块；
所述第一数据处理模块，用于根据接收到的第一导航系统的数据(例如，SINS的陀螺脉冲数和加速度计脉冲数)计算角增量和速度增量，并将角增量和速度增量发送给解算模块；
所述第二数据处理模块，用于将接收到的第二导航系统的数据(例如，GPS系统的位置和速度数据)发送给滤波模块；
所述解算模块，用于根据角增量和速度增量计算下一时刻的姿态、速度和位置，并将计算得到的姿态、速度和位置发送给滤波模块；
所述滤波模块，用于根据接收到的数据，进行组合导航信息融合，得到融合后的组合导航数据。
另外，较佳的，在本发明的具体实施例中，所述第二计算单元还可以进一步包括：计算模块、评价模块和数据存储模块；
所述数据存储模块，用于存储第一导航系统的历史数据和第二导航系统的历史数据；
所述计算模块，用于根据记录的第一导航系统的历史数据和第二导航系统的历史数据，估算得到陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的估计值；
所述评价模块，用于对估算得到的各个估计值进行评价，判断各个估计值是否为更准确的参数；根据判断结果将估计值反馈给第一计算单元(例如，第一计算单元中的第一数据处理模块和滤波模块)，对相应的参数进行修正。
此外，较佳的，在本发明的一个具体实施例中，所述计算模块还可以进一步包括：种群操作子单元、解算子单元、结果评估子单元和n个估计子单元；
所述种群操作子单元，用于根据记录的第一导航系统的历史数据和第二导航系统的历史数据，设置遗传算法需要的初始种群，并根据结果评估子单元返回的数据进行遗传操作，生成下一代种群；
所述解算子单元，用于根据角增量和速度增量计算下一时刻的姿态、速度和位置，并将计算得到的姿态、速度和位置发送给n个估计子单元；
所述n个估计子单元分别对应于初始种群中的n个个体，用于根据记录的第一导航系统的历史数据和第二导航系统的历史数据进行计算，得到每个个体各自的速度、位置和姿态，同时也得到每个个体滤波后的速度、位置和姿态；
所述结果评估子单元，用于对于每个个体，判断该个体是否满足预设条件；如果有至少一个个体满足预设条件，则认为估计成功，则将该个体对应的零次项、一次项、噪声阵反馈给第一计算单元，对相应的参数进行修正；如果存在多个个体满足所有要求，则选择适应度最好的个体；如果没有一个个体满足预设条件，则将n个估计子单元输出的数据返回给所述种群操作子单元。
更进一步的，在本发明的一个较佳实施例中，所述第一数据处理模块还可将接收到的第一导航系统的数据存储在数据存储模块中；
所述第二数据处理模块还可将接收到的第二导航系统的数据存储在数据存储模块中。
此外，在本发明的一个较佳实施例中，所述第一导航系统数据处理模块可以是SINS数据处理模块，第二导航系统数据处理模块可以是GPS数据处理模块，也可以是其它导航系统的数据处理模块。
综上可知，在本发明中的组合导航方法和系统中，由于可以根据记录的第一导航系统的历史数据和第二导航系统的历史数据，估算得到陀螺常值项和比例项、加速度计常值项和比例项、滤波所用的系统噪声阵和观测噪声阵的估计值；对估算得到的各个估计值进行评价，判断各个估计值是否为更准确的参数；根据判断结果将估计值反馈给第一导航系统和第二导航系统，对对应的参数进行修正，因此可以实现以SINS为基础的组合导航，并且更加充分地利用了两个导航系统(例如，SINS和GPS)的历史数据信息，通过对历史数据信息的挖掘和再利用，可以实现对陀螺和加速度计的误差补偿，实现对滤波系统噪声协方差阵和观测噪声协方差阵的优化设置，实现了陀螺常值漂移和比例项、加速度计常值漂移和比例项、滤波模型系统方程噪声阵和滤波模型观测方程的在线估计并反馈给相应的系统，可以对陀螺和加速度计的工具误差进行在线实时修正，因此即使是在没有GPS信息时，也可以使得SINS具有比没有反馈时更高的精度，达到提高惯导导航精度的目的。
另外，在本发明的技术方案中，还进一步地对滤波的系统方程噪声阵和观测方程噪声阵进行了在线实时修正，降低了对滤波器设计时的要求，提高了噪声协方差阵描述相应噪声的准确性，从而提高了滤波精度。对滤波的发散也有一定的抑制效果，更适合长时间滤波。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。
- 还没有人留言评论。精彩留言会获得点赞！