一种导航方法和导航装置与流程

本申请无人机领域，尤其涉及一种用卫星导航和本机惯性测量手段导航的方法和装置。

背景技术：

每种单一导航系统都有各自的独特性能和局限性。小型光纤或微机械惯性导航装置有成本低、体积小、重量轻的优势，可以自主的为载体提供姿态、位置、速度信息；缺点是导航误差随时间增长快，特别是mems惯性器件漂移、零偏重复性等指标差，难于通过预先标校达到提高惯性导航精度的目标。

北斗卫星导航系统是我国自行研制的卫星导航系统，它可以在gps、glonass等国外导航系统关闭的情况下为我国用户提供导航、定位、授时服务。目前北斗系统已经形成了5geo+5igso+5meo的星座框架，可以实现亚太地区的定位、导航和授时，定位精度10m、测速精度0.2m/s、授时精度50ns。卫星导航有全天候、导航精度高的优势，但受使用环境的制约，在干扰和遮挡条件下可用性差，并且不能提供载体姿态信息用于载体飞行控制。

因此，需要设计一种组合导航系统实现方法和系统，将微型惯性测量单元(mimu)与北斗卫星接收机组合构成一种可以提供完整的载体运动参数、导航准确度更高的系统。

技术实现要素：

本发明公开一种导航方法和装置，解决未标定条件下利用微惯性测量单元(mimu)导航的问题。

本申请实施例提供一种导航方法，用于无人机，所述无人机包含微惯性测量单元、卫星接收机，包括以下步骤：

在初始时刻，用所述卫星接收机采集无人机位置和速度，分别作为位置初始值和速度初始值；

从所述初始时刻到校准时刻，用所述微惯性测量单元按测量周期采集无人机加速度瞬时值、陀螺瞬时值；根据加速度瞬时值、陀螺瞬时值计算姿态参数瞬时值；所述校准时刻和所述初始时刻之间为滤波周期，包含多个所述测量周期；

根据所述位置初始值、速度初始值、姿态参数瞬时值，用捷联惯性导航算法得出无人机在每一个测量周期的位置计算值、速度计算值和姿态参数计算值；

在校准时刻，用所述卫星接收机采集无人机位置和速度，分别作为位置校准值和速度校准值；

根据校准时刻的位置计算值、速度计算值、位置校准值、速度校准值，用离散卡尔曼滤波方法计算姿态参数误差、速度误差、位置误差、陀螺误差、加速度误差；所述离散卡尔曼滤波方法使用的误差变量包含：俯仰角误差、横滚角误差、航向角误差、东向速度误差、北向速度误差、精度误差、纬度误差、x、y、z方向陀螺误差、x、y方向加速度误差共12个参数；其中，x、y、z构成直角坐标系，x方向指向无人机右侧方，y方向沿无人机纵轴方向指向前。

进一步地，以所述校准时刻作为新的初始时刻，循环执行所述步骤。

本申请的最佳实施例还包含以下步骤：用所述加速度误差对下一滤波周期的加速度瞬时值进行修正；用所述陀螺误差对下一滤波周期的陀螺瞬时值进行修正。

本申请实施例还提供一种导航装置，用于无人机，包含微惯性测量单元、卫星接收机、处理器；所述微惯性测量单元，用于从初始时刻开始，按照测量周期产生陀螺瞬时值、加速度瞬时值；所述卫星接收机，用于接收卫星定位信息，在初始时刻输出位置初始值和速度初始值；从初始时刻起按照滤波周期输出位置校准值和速度校准值；所述处理器，用于根据所述位置初始值、速度初始值、陀螺瞬时值、加速度瞬时值，用捷联惯性导航算法得出无人机在每一个测量周期的位置计算值、速度计算值和姿态参数计算值；根据校准时刻的位置计算值、速度计算值、位置校准值、速度校准值，用离散卡尔曼滤波方法计算姿态参数误差、速度误差、位置误差、陀螺误差、加速度误差；所述离散卡尔曼滤波方法使用的误差变量包含：俯仰角误差、横滚角误差、航向角误差、东向速度误差、北向速度误差、精度误差、纬度误差、x、y、z方向陀螺误差、x、y方向加速度误差共12个参数；其中，x、y、z构成直角坐标系，x方向指向无人机右侧方，y方向沿无人机纵轴方向指向前。

进一步地，所述处理器进一步包含导航参数初始化单元、捷联惯性导航计算单元、误差估计滤波器；所述导航参数初始化单元，用于接收加速度瞬时值、陀螺瞬时值、根据加速度瞬时值、陀螺瞬时值计算姿态参数瞬时值；所述捷联惯性导航导航单元，用于接收所述位置初始值、速度初始值、姿态参数瞬时值，用捷联惯性导航算法得出无人机在每一个测量周期的位置计算值、速度计算值和姿态参数计算值；所述误差估计滤波器，用离散卡尔曼滤波方法计算姿态参数误差、速度误差、位置误差、陀螺误差、加速度误差。

优选地，所述导航装置进一步包含输出校正单元；所述输出校正单元，用所述姿态参数误差对所述姿态参数计算值进行修正。

在本发明任意一项实施例中，优选地，所述导航装置进一步包含输入校正单元，用所述加速度误差对下一滤波周期的加速度瞬时值进行修正、用所述陀螺误差对下一滤波周期的陀螺瞬时值进行修正，所述导航参数初始化单元的输入。

本申请实施例采用的上述至少一个技术方案能够达到以下有益效果：

对比其他组合导航系统实现方法，本设计具有完备的数据处理过程，采用本发明可以很好地实现基于微惯性测量单元(mimu)、北斗卫星接收机的组合导航。

本发明微惯性器件误差不需要预先标定，一方面周期性的利用北斗卫星接收机输出的位置、速度以及速度航向初始化惯性导航计算参数，另一方面误差估计滤波器实时估计惯性导航误差并进行修正，最终可以得到精度可用的组合导航结果。

本发明采用小型光纤或微机械惯性测量单元与传统的激光或光纤惯性单元相比成本更低、体积更小、重量更轻；本发明采用北斗卫星接收机可使组合导航系统在其他卫星导航系统(gps、glonass等)关闭的情况下仍正常工作。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本发明导航方法的实施例流程图；

图2为本发明导航方法包含循环校准的实施例流程图；

图3为用离散卡尔曼滤波方法进行误差估计的实施例流程图；

图4为本发明导航装置的实施例框图；

图5为本发明导航装置进一步优化的实施例框图；

图6为本发明导航装置进行输入、输出校准的实施例示意图。

具体实施方式

为使本申请的目的、技术方案和优点更加清楚，下面将结合本申请具体实施例及相应的附图对本申请技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

在本申请全部实施例中，x、y、z构成直角坐标系，x方向指向无人机右侧方，y方向沿无人机纵轴方向指向前。

以下结合附图，详细说明本申请各实施例提供的技术方案。

图1为本发明导航方法的实施例流程图。具体包含以下步骤：

步骤11、在初始时刻，用所述卫星接收机采集无人机位置和速度，分别作为位置初始值和速度初始值；

步骤12、从所述初始时刻到校准时刻，用所述微惯性测量单元按测量周期采集无人机加速度瞬时值、陀螺瞬时值；根据加速度瞬时值、陀螺瞬时值计算姿态参数瞬时值；所述校准时刻和所述初始时刻之间的时长为一个滤波周期，包含多个所述测量周期；

其中，在第一个初始时刻，所述姿态参数瞬时值的计算方法为：

pitch0＝arcsin(ay/g0)公式1.1

roll0＝-arcsin(ax/g0)公式1.2

式中pitch0、roll0为初始俯仰角、横滚角；ax、ay为加速度计x、y方向输出值；g0为重力加速度。

例如，以测量周期t1(20ms)采集微惯性测量单元(mimu)的陀螺、加速度计信息。

需要说明的是，惯性导航输出载体姿态用于载体姿态控制，要求实时性高，因此设计惯性导航为短周期t1(为20ms)计算；随计算时间增长惯性导航输出结果(姿态、位置、速度)误差增大，需要周期性利用北斗卫星接收机的信息辅助以提高精度，本方法实施例采用滤波周期t2(为1s)。

在步骤12中，例如可以用北斗卫星接收机的经度、纬度、高度、地速航向和所述初始俯仰角、初始横滚角，计算无人机姿态、速度、位置初值；

步骤13、根据所述位置初始值、速度初始值、姿态参数瞬时值，用捷联惯性导航算法得出无人机在每一个测量周期的位置计算值、速度计算值和姿态参数计算值；

位置参数包含经度、纬度、高度，速度参数包含东向速度、北向速度、天向速度，姿态参数包含航向、俯仰角、横滚角；

步骤14、按所述滤波周期，在校准时刻，用所述卫星接收机采集无人机位置和速度，分别作为位置校准值和速度校准值；

例如，以滤波周期t2(1s)采集北斗卫星接收机的信息(经度、纬度、高度、东向速度、北向速度、天向速度、地速航向)。

步骤15、根据校准时刻的位置计算值、速度计算值、位置校准值、速度校准值，用离散卡尔曼滤波方法计算姿态参数误差、速度误差、位置误差、陀螺误差、加速度误差，所述离散卡尔曼滤波方法使用的误差变量包含：俯仰角误差、横滚角误差、航向角误差、东向速度误差、北向速度误差、精度误差、纬度误差、x、y、z方向陀螺误差、x、y方向加速度误差共12个参数。

步骤16、使用12个参数的误差变量对所述位置计算值、速度计算值和姿态参数计算值进行校准；

例如当卫星接收机参与定位卫星数较多，几何精度因子小于阈值时，组合导航天向速度、高度直接输出卫星接收机天向速度、高度；否则组合导航输出微惯性测量单元(mimu)导航天向速度、高度。

本申请各项实施例中，所述卫星接收机接收导航信号，具体地，应用于北斗卫星、gps卫星或其他导航用卫星。当应用北斗卫星时，所述卫星接收机为北斗文星接收机。

图2为本发明导航方法包含循环校准的实施例流程图。

本申请的最佳实施例还包含以下步骤：

步骤17、用所述加速度误差对下一滤波周期的加速度瞬时值进行修正；用所述陀螺误差对下一滤波周期的陀螺瞬时值进行修正。每个卡尔曼滤波周期(t2)得到的陀螺、加速度计误差估计值对下一周期陀螺、加速度计原始数据修正。

进一步地，失锁重捕时需要重新初始化，重复步骤11～17。

当所述卫星接收机连续跟踪卫星定位信号时，进一步地，以所述校准时刻作为新的初始时刻，循环执行所述步骤13～17。

经过上述过程，微惯性器件误差无需预先标定，周期性的利用北斗卫星接收机输出的位置、速度以及速度航向初始化惯性导航计算参数，以及设计误差估计滤波器实时估计惯性导航误差并进行修正，可以最终得到精度可用的导航结果。

图3为用离散卡尔曼滤波方法进行误差估计的实施例流程图。采用误差估计滤波器进行惯性导航误差估计，考虑微惯性测量单元(mimu)特点和运算时间，滤波器选择12参数的误差状态变量，具体的，误差状态变量x的元素由以下物理量组成：俯仰角误差δφe、横滚角误差δφn、航向角误差δφu；东向速度误差δve、北向速度误差δvn；经度误差δλ、纬度误差δl；x、y、z方向陀螺误差εbx，εby，εbz；x、y方向加速度计误差即

步骤21、根据惯导系统误差、基本导航参数之间的关系建立所述12参数误差状态模型。具体地，误差状态模型为

其中f为状态转移矩阵，维数为12×12，非零项为：

f(0,1)＝ωiesinl；f(0,2)＝-ωiecosl；f(1,0)＝-ωiesinl；f(1,5)＝-ωiesinl；f(2,0)＝ωiecosl；f(3,1)＝-g；f(4,0)＝g；

式中，ωie为地球自转角速度，l为纬度，r为地球半径，为姿态矩阵(维数为3×3)，ve、vn为东向速度、北向速度，g为重力加速度，τgyro为陀螺相关时间，τacc为加速度计相关时间，在本发明技术方案中，陀螺、加速度计相关时间取值5s。

惯导系统误差来源有很多种，如位置误差、速度误差、初始姿态误差、陀螺偏差、加速度计偏差、刻度系数误差等，各项误差又可分为固定偏差和噪声项。惯导系统误差、基本导航参数之间存在近似的对应关系，固定采样间隔内惯导系统误差的变化可以表示为系数乘以误差项加上噪声项的形式。表达式中每一个误差变化量对应的误差项系数是确定的，可以由惯性导航误差传递关系得出。根据选择的误差状态变量不同，误差状态模型写成的表达式形式不同。具体的可以建立公式2形式的误差状态模型。

误差状态模型中，g为噪声矩阵，维数为12×1，满足

其中，wgyro＝[wgxwgywgz]^t，wacc＝[waxwaywaz]^t，wgx、wgy、wgz为陀螺仪测量噪声，wax、way、waz为加速度计测量噪声。

误差估计滤波器按卡尔曼滤波方法计算，计算周期为所述滤波周期t2(例如1s)，每个滤波周期计算得到状态一步估计值：

式中，分别为俯仰角误差、横滚角误差、航向角误差估计值；分别为东向速度误差估计值、北向速度误差估计值；分别为经度、纬度误差估计值；分别为x、y、z方向陀螺误差估计值；分别为x、y方向加速度计误差估计值。

步骤22、用状态一步估计值修正误差，更新姿态矩阵。姿态修正模型为

式中，为当前周期姿态矩阵，为修正后姿态矩阵，用于下一周期s5计算。

当前周期姿态矩阵经姿态偏差修正可以得到修正后的姿态矩阵用于后续捷联惯导计算。姿态修正按照航向角误差、俯仰角误差、横滚角误差的顺序进行。按照上述角度旋转顺序可以得到修正后姿态矩阵。具体的修正后姿态矩阵与当前周期姿态矩阵、系数矩阵关系表达式如圈出部分所示。

步骤23、根据速度、位置修正模型，使用状态一步估计值对微惯性测量单元输出的东向速度、北向速度、纬度、经度进行修正。

建立误差估计滤波器观测模型如下式：

公式6中建立的误差观测模型采用速度位置差作为观测量，微惯性测量单元(mimu)输出的速度、位置与北斗卫星接收机输出的位置速度差值可以表示为惯导误差加测量噪声的形式。

其中ve_ins、vn_ins、lins、λins为根据微惯性测量单元输出得到速度计算值和位置计算值的东向速度、北向速度、纬度、经度ve_bd、vn_bd、lbd、λbd为北斗卫星接收机输出的东向速度、北向速度、纬度、经度；wve、wvn为速度测量噪声；wλ、wl为位置测量噪声。wve、wvn为北斗接收机速度测量噪声，由接收机速度精度参数决定，wλ、wl为北斗接收机位置测量噪声，由接收机位置精度参数决定。

从公式6中看只能直接求出四个惯导误差参数，但是从公式2的状态模型f矩阵关系可以看出这四个误差参数的变化与其余惯导误差参数有关。通过卡尔曼滤波可以解出全部误差参数。包括：姿态参数(航向、俯仰角、横滚角)、位置参数(经度λins、纬度lins、高度)、速度参数(东向速度ve_ins、北向速度vn_ins、天向速度)。

利用状态一步估计值，使用速度、位置修正模型

式中ve、vn、l、λ为修正后的东向速度、北向速度、纬度、经度导航结果。

图4为本发明导航装置的实施例框图。本申请实施例还提供一种导航装置，用于无人机，包含微惯性测量单元100、卫星接收机200、处理器300；所述微惯性测量单元，用于从初始时刻开始，按照测量周期产生陀螺瞬时值、加速度瞬时值；所述卫星接收机，用于接收卫星定位信息，在初始时刻输出位置初始值和速度初始值；从初始时刻起按照滤波周期输出位置校准值和速度校准值；所述处理器，用于根据所述位置初始值、速度初始值、陀螺瞬时值、加速度瞬时值，用捷联惯性导航算法得出无人机在每一个测量周期的位置计算值、速度计算值和姿态参数计算值；根据校准时刻的位置计算值、速度计算值、位置校准值、速度校准值，用离散卡尔曼滤波方法计算姿态参数误差、速度误差、位置误差、陀螺误差、加速度误差；所述离散卡尔曼滤波方法使用的误差变量包含：俯仰角误差、横滚角误差、航向角误差、东向速度误差、北向速度误差、精度误差、纬度误差、x、y、z方向陀螺误差、x、y方向加速度误差共12个参数；其中，x、y、z构成直角坐标系，x方向指向无人机右侧方，y方向沿无人机纵轴方向指向前。

图5为本发明导航装置进一步优化的实施例框图。进一步地，所述处理器300进一步包含导航参数初始化单元301、捷联惯性导航计算单元302、误差估计滤波器303；所述导航参数初始化单元，用于接收加速度瞬时值、陀螺瞬时值、根据加速度瞬时值、陀螺瞬时值计算姿态参数瞬时值(如公式1.1～1.2)；所述捷联惯性导航计算单元，用于接收所述位置初始值、速度初始值、姿态参数瞬时值，用捷联惯性导航算法得出无人机在每一个测量周期的位置计算值、速度计算值和姿态参数计算值；所述误差估计滤波器，用离散卡尔曼滤波方法计算姿态参数误差、速度误差、位置误差、陀螺误差、加速度误差共12个变量(如公式2～5)。

图6为本发明导航装置进行输入、输出校准的实施例示意图。所述导航装置进一步包含输出校正单元400；所述输出校正单元，用所述姿态参数误差对所述姿态参数计算值进行修正，得到修正后的位置、速度、姿态参数。在本发明任意一项实施例中，优选地，所述导航装置进一步包含输入校正单元500，用所述加速度误差对下一滤波周期的加速度瞬时值进行修正、用所述陀螺误差对下一滤波周期的陀螺瞬时值进行修正，所述导航参数初始化单元的输入。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。