一种组合导航系统的高精度定姿方法
【专利摘要】本发明公开了一种组合导航系统的高精度定姿方法，它包括以下步骤：根据陀螺仪角速度信号和当前状态信息解算当前时刻方向余弦矩阵；计算该矩阵当前列向量对前面列向量的投影，在最优的条件下，对当前列进行归一化处理；根据正交归一化处理后的新的方向余弦矩阵求解姿态角。本发明通过四元数法求解当前时刻方向余弦矩阵，计算量小；在得到当前时刻方向余弦矩阵后，采用施密特正交法对当前时刻方向余弦矩阵进行归一化处理，大大降低了后续姿态信息解算的误差，提高了定姿精度；在最优条件下，即基于误差的二范数的代价函数最小，对当前时刻方向余弦矩阵采用施密特正交方法进行归一化处理，能得到稳定、高效的解析解。
【专利说明】一种组合导航系统的高精度定姿方法
【技术领域】
[0001]本发明涉及一种组合导航系统的高精度定姿方法。
【背景技术】
[0002]从惯性导航(INS)的工作原理和误差分析可以看出，惯导系统的自主性很强，它可以连续的提供包括基准在内的全部导航参数，并且具有非常好的短期精度和稳定性，在许多精确制导武器中得到了广泛的应用。但是由于导航系统使用的惯性传感器的精度不高，且误差积累而无法单独长期工作。
[0003]卫星导航(GNSS)虽然具有定位和测速精度高，定位误差与时间无关，并能全天候导航的优点，但是单GNSS不能提供载体的姿态信息，且易受干扰。由于GNSS和INS的组合导航系统能去其短而取其长，完成较高精度、长时间的导航任务，在近几年获得了广泛的应用。在组合导航系统中，载体的姿态角解算是一步核心工作。
[0004]现有姿态测量的方法有很多，如采用加速度计测出的重力分量解算姿态角信息，该测量方法虽然成本低，但其动态性能却比较差；如专利申请号:201210487249.0所述的一种基于GPS载波双差方程的姿态角直接求解方法，通过建立载波双差方程，将载体姿态信息引入到载波双差方程中进行解算，该方法能减小误差，但可靠性及稳定性差。
[0005]其他的还有通过方向余弦矩阵解算姿态信息，但由于计算过程中的舍入误差，由基于四元数的姿态传递方程解算后的方向余弦矩阵其正交性不能满足，容易对后续姿态解算造成较大的误差。
【发明内容】
[0006]本发明的目的在于克服现有技术的不足，提供高可靠度、高稳定性、计算量小的一种组合导航系统的高精度定姿方法，它在最优的条件下对方向余弦矩阵进行正交归一化，大大降低了姿态解算的误差，提高了定姿精度。
[0007]本发明的目的是通过以下技术方案来实现的:一种组合导航系统的高精度定姿方法，它包括以下步骤:
51:根据陀螺仪角速度信号和当前状态信息解算当前时刻方向余弦矩阵；
52:根据方向余弦矩阵求解姿态角。
[0008]所述的当前时刻方向余弦矩阵解算包括以下子步骤:
511:计算载体系相对于惯性系的角速度在导航系中的投影；
512:计算地球系相对于惯性系的角速度在导航系中的投影；
513:计算导航系相对于地球的角速度；
514:计算载体相对于导航系的角速度并用四元数表示；
515:根据四元数姿态传递方程求解当前时刻方向余弦矩阵C。
[0009]所述的根据方向余弦矩阵求解姿态角包括如下子步骤:
S21:根据方向余弦矩阵元素C (3，2)和C (3,3)的反正切求解横滚角； 522:根据方向余弦矩阵元素C (3,1)的反正弦求解俯仰角；
523:根据方向余弦矩阵元素C (2，I)和C (1，I)的反正切求解方位角。
[0010]在根据方向余弦矩阵求解姿态角之前它还包括如下步骤:在最优条件下，对当前时刻方向余弦矩阵采用施密特正交化方法进行正交归一化处理；
所述的采用施密特正交化方法进行正交归一化处理包括如下子步骤:
531:分别计算当前列向量对前面的列向量的投影；
532:当前列向量减去投影向量得到新向量；
533:计算新向量的二范数；
534:新向量各元素除以其二范数得到归一化后的新向量；
S35:得到新的方向余弦矩阵；
536:判断新的方向余弦矩阵的所有列是否均已进行正交归一化处理；
537:若是，跳转到步骤S2 ;若否，则跳转到步骤S31，依次循环。
[0011]进一步的，所述的最优条件指基于误差的二范数的代价函数最小。
[0012]本发明的有益效果是:通过四元数法求解方向余弦矩阵，计算量小；在根据当前时刻方向余弦矩阵解算姿态信息之前先对当前时刻方向余弦矩阵采用施密特正交法进行归一化处理，大大降低了姿态信息解算的误差，提高了定姿精度；在最优条件下(即基于误差的二范数的代价函数最小)对当前时刻方向余弦矩阵采用施密特正交方法进行归一化处理，能得到稳定、高效的解析解。
【专利附图】
【附图说明】
[0013]图1为本发明的计算方法流程图。
【具体实施方式】
[0014]下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于以下所述。
[0015]如图1所示，一种组合导航系统的高精度定姿方法，它包括以下步骤:
S1:根据陀螺仪角速度信号和当前状态信息解算当前时刻方向余弦矩阵；
S2:根据方向余弦矩阵求解姿态角。
[0016]所述的当前时刻方向余弦矩阵解算包括以下子步骤:
511:计算载体系相对于惯性系的角速度在导航系中的投影Wib ；
512:计算地球系相对于惯性系的角速度在导航系中的投影Wie ；
513:计算导航系相对于地球的角速度Wen ；
514:计算载体相对于导航系的角速度Wnb并用四元数表示；
515:根据四元数姿态传递方程求解当前时刻方向余弦矩阵C。
[0017]所述的根据方向余弦矩阵求解姿态角包括如下子步骤:
521:根据方向余弦矩阵元素C (3，2)和C (3,3)的反正切求解横滚角；
522:根据方向余弦矩阵元素C (3,1)的反正弦求解俯仰角；
523:根据方向余弦矩阵元素C (2，I)和C (1，I)的反正切求解方位角。
[0018]在根据方向余弦矩阵求解姿态角之前它还包括如下步骤:在最优条件下，对当前时刻方向余弦矩阵采用施密特正交化方法进行正交归一化处理。
[0019]所述的采用施密特正交化方法进行正交归一化处理包括如下子步骤:
531:分别计算当前列向量对前面的列向量的投影；
532:当前列向量减去投影向量得到新向量；
533:计算新向量的二范数；
534:新向量各元素除以其二范数得到归一化后的新向量；
535:得到新的方向余弦矩阵；
536:判断新的方向余弦矩阵的所有列是否均已进行正交归一化处理；
537:若是，跳转到步骤S2 ;若否，则跳转到步骤S31，依次循环。
[0020]进一步的，所述的最优条件指基于误差的二范数的代价函数最小。
【权利要求】
1.一种组合导航系统的高精度定姿方法，它包括以下步骤: S1:根据陀螺仪角速度信号和当前状态信息解算当前时刻方向余弦矩阵； S2:根据方向余弦矩阵求解姿态角； 所述的当前时刻方向余弦矩阵解算包括以下子步骤: 511:计算载体系相对于惯性系的角速度在导航系中的投影； 512:计算地球系相对于惯性系的角速度在导航系中的投影； 513:计算导航系相对于地球的角速度； 514:计算载体相对于导航系的角速度并用四元数表示； 515:根据四元数姿态传递方程求解当前时刻方向余弦矩阵C ； 所述的根据方向余弦矩阵求解姿态角包括如下子步骤: 521:根据方向余弦矩阵元素C (3，2)和C (3,3)的反正切求解横滚角； 522:根据方向余弦矩阵元素C (3,1)的反正弦求解俯仰角； 523:根据方向余弦矩阵元素C (2，I)和C (1，I)的反正切求解方位角； 其特征在于:在根据方向余弦矩阵求解姿态角之前它还包括如下步骤:在最优条件下，对当前时刻方向余弦矩阵采用施密特正交化方法进行正交归一化处理； 所述的采用施密特正交化方法进行正交归一化处理包括如下子步骤: 531:分别计算当前列向量对前面的列向量的投影； 532:当前列向量减去投影向量得到新向量； 533:计算新向量的二范数； 534:新向量各元素除以其二范数得到归一化后的新向量； 535:得到新的方向余弦矩阵； 536:判断新的方向余弦矩阵的所有列是否均已进行正交归一化处理； 537:若是，跳转到步骤S2 ;若否，则跳转到步骤S31，依次循环。
2.根据权利要求1所述的一种组合导航系统的高精度定姿方法，其特征在于:所述的最优条件指基于误差的二范数的代价函数最小。
【文档编号】G01C21/20GK104180807SQ201310197914
【公开日】2014年12月3日 申请日期:2013年5月25日 优先权日:2013年5月25日
【发明者】罗明刚 申请人:成都国星通信有限公司