CN113465599A - 定位定向方法、装置及系统 - Google Patents

定位定向方法、装置及系统 Download PDF

Info

Publication number
CN113465599A
CN113465599A CN202110622364.3A CN202110622364A CN113465599A CN 113465599 A CN113465599 A CN 113465599A CN 202110622364 A CN202110622364 A CN 202110622364A CN 113465599 A CN113465599 A CN 113465599A
Authority
CN
China
Prior art keywords
error
strapdown
navigation
coordinate system
model
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110622364.3A
Other languages
English (en)
Other versions
CN113465599B (zh
Inventor
苏中
雷明
赵辉
李擎
付国栋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Information Science and Technology University
Original Assignee
Beijing Information Science and Technology University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Information Science and Technology University filed Critical Beijing Information Science and Technology University
Priority to CN202110622364.3A priority Critical patent/CN113465599B/zh
Publication of CN113465599A publication Critical patent/CN113465599A/zh
Application granted granted Critical
Publication of CN113465599B publication Critical patent/CN113465599B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/166—Mechanical, construction or arrangement details of inertial navigation systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本公开提供了一种定位定向方法、装置及系统。其中，该方法包括：通过惯性传感器采集携带所述惯性传感器的载体的运动状态信息；基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来确定所述载体的位姿误差，其中，所述弱捷联导航误差模型用于描述在所述惯性传感器与所述载体未固联情况下的真实位姿和理论位姿之间的误差变化；基于所确定的位姿误差来解算所述载体的位姿信息，以对所述载体进行定位定向。本公开解决了由于惯性传感器未固联造成的定位精度差的技术问题。

Description

定位定向方法、装置及系统
技术领域
本公开涉及计算机领域，具体而言，涉及一种定位定向方法、装置及系统。
背景技术
随着卫星导航和无线通信技术的发展，通过卫星定位、预置信标和数字地图匹配等方法，可以方便地获取个人或者运动体(统称为载体)位置信息。但对于地下灾害环境下人员位置姿态信息缺失的环境下(例如，无线电信号质量差、WiFi，卫星等通信系统等原因无法使用)，位置和姿态(也称为位姿)信息的获取一直是国内外导航与位置服务的难题和研究热点。
捷联惯性导航技术是自主定位定向的主要技术之一。但由于系统建模误差以及惯性传感器存在随机漂移误差，导航信息经过多重积分产生，航向信息根据陀螺仪解算而来，随着脚步晃动和时间的累积，导航精度随之降低。
相关的行人惯性导航算法存在很多缺点：(1)对于航向误差发散的问题没有得到有效解决；(2)由于地下环境复杂，人员的运动形式也会变化多样，惯性传感器器件与人体不能完全固联，传统捷联惯性导航适用性变差。
为了解决上述问题，相关技术中也有将IMU器件信息和其他高精度传感器信息融合的方案(视觉、RFID、蓝牙、UWB、WIFI)等。尽管融合另外的传感器可提高定位精度，但是这些方法需要先验信息，而且和低成本MENS惯性器件比，不适用于灾害环境的地下遮蔽空间多运动姿态的人体定位定向。
当发生地震灾害时，由于地下环境复杂，人员的运动形式也会变化多样，导致IMU与人体呈现一种弱捷联关系，此时传统的捷联惯性导航误差传播规律已无法表征真实的导航参数误差传播特性，需要揭示弱捷联情形下的惯性导航误差传播机理，以定位定向携带惯性传感器的载体，例如人。
针对上述的问题，目前尚未提出有效的解决方案。
发明内容
本公开实施方式提供了一种定位定向方法、装置及系统，以至少解决由于惯性传感器未固联造成的定位精度差的技术问题。
根据本公开实施方式的一个方面，提供了一种定位定向方法，包括：通过惯性传感器采集携带所述惯性传感器的载体的运动状态信息；基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来估算所述载体的位姿误差，其中，所述弱捷联导航误差模型用于描述在所述惯性传感器与所述载体未固联情况下的真实位姿和理论位姿之间的误差变化；解算所述载体的位姿信息，并基于所估算的位姿误差来补偿所解算出的位姿信息，以对所述载体进行定位定向。
本公开实施方式提供了弱捷联导航误差模型。弱捷联导航误差模型可以有效的表征针对灾难环境下人体复杂运动所带来的捷联惯性导航误差模型所无法估计的误差传播特性，从而提高了对载体的定向定位的准备性。
根据本公开实施方式的另一方面，还提供了一种定位定向装置，包括：采集模块，被配置为通过惯性传感器采集携带所述惯性传感器的载体的运动状态信息；误差估算模块，被配置为基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来估算所述载体的位姿误差，其中，所述弱捷联导航误差模型用于描述在所述惯性传感器与所述载体未固联情况下的真实位姿和理论位姿之间的误差变化；解算模块，被配置为解算所述载体的位姿信息，并基于所估算的位姿误差来补偿所解算出的位姿信息，以对所述载体进行定位定向。
本公开实施方式中的定位定向装置，能够弥补人体复杂运动带来的基准晃动导致传统的捷联惯性导航误差传播规律无法表征真实的导航参数误差传播特性的问题，同时又能够对弱捷联情况下所带来的位姿误差进行估计和补偿，实现在灾害环境地下遮蔽空间的自主定位定向。
根据本公开实施方式的又一方面，还提供了一种定位定向系统，包括惯性传感器和上述定位定向装置。
本公开实施方式的定位定向系统，根据传统的捷联惯性导航误差传播规律，引入人体晃动模型，推导得到弱捷联惯性导航误差模型。根据弱捷联惯性导航误差模型建立单兵捷联惯性导航状态观测器，结合人体特殊运动状态约束，实现单兵位姿的精确估计。
在本公开实施方式中，通过惯性传感器采集携带所述惯性传感器的载体的运动状态信息；基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来估算所述载体的位姿误差，其中，所述弱捷联导航误差模型用于描述在所述惯性传感器与所述载体未固联情况下的真实位姿和理论位姿之间的误差变化；并解算所述载体的位姿信息，并基于所估算的位姿误差来补偿所解算出的位姿信息，以对所述载体进行定位定向，从而实现了在惯性传感器和载体未固联的情况下对载体精确定位定向的技术效果，进而解决了由于惯性传感器未固联造成的定位精度差的技术问题。
附图说明
此处所说明的附图用来提供对本公开的进一步理解，构成本申请的一部分，本公开的示意性实施方式及其说明用于解释本公开，并不构成对本公开的不当限定。在附图中：
图1是根据本公开实施方式的一种定位定向方法的流程图；
图2是根据本公开实施方式的估算载体的位姿误差的方法的流程图；
图3是根据本公开实施方式的一种定位定向装置的结构示意图；
图4是根据本公开实施方式的一种定位定向系统的结构示意图；
图5是根据本公开实施方式的优选的定向定位方法的流程图；
图6是捷联惯性导航下传感器坐标系和载体坐标系的状态示意图；
图7是弱捷联惯性导航下传感器坐标系和载体坐标系的状态示意图；
图8是根据本公开实施方式的另一种定位定向方法的流程图；
图9是根据本公开实施方式的坐标统一方法的流程图；
图10是根据本公开实施方式的实现定位定向方法的计算机设备的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本公开方案，下面将结合本公开实施方式中的附图，对本公开实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式仅仅是本公开一部分的实施方式，而不是全部的实施方式。基于本公开中的实施方式，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式，都应当属于本公开保护的范围。
需要说明的是，本公开的说明书和权利要求书及上述附图中的术语″第一″、″第二″等是用于区别类似的载体，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本公开的实施方式能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语″包括″和″具有″以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
术语定义
捷联是指传感器坐标系和载体坐标系重合。图6示出了捷联惯性导航下传感器坐标系和载体坐标系的状态。
弱捷联是指传感器坐标系和载体坐标系不重合(不能实现固连)，存在随机线位移和随机小角度位移。图7示出了弱捷联状态下传感器坐标系和载体坐标系的状态。
弱捷联导航误差模型是指弱捷联导航位置误差、速度误差和姿态矩阵误差的组合；
捷联导航误差模型是指捷联导航位置误差、速度误差和姿态矩阵误差的组合。
实施方式1
地下灾害环境下，常用的导航手段均会失效或导航性能下降，但惯性导航作为一种强自主性的导航方式，完全契合地下未知环境的精确导航需求。因此，在本公开的实施方式中，采用了惯性导航的方式。
图1是根据本公开实施方式的定位定向方法，如图1所示，该方法包括以下步骤：
步骤S102，通过惯性传感器采集携带所述惯性传感器的载体的运动状态信息。
采集惯性传感器的原始信号，生成采样信号。对采样信号进行预处理，从预处理后的信号中选取一定时间片长度或者一定采样点数目的采样信号，提取所选取信号的时域特征和频域特征，以作为所采集的载体的运动状态信息。
在一个实施方式中，惯性传感器包括加速度计(或加速度传感计)和角速度传感器(陀螺)以及它们的单、双、三轴组合IMU(惯性测量单元)，AHRS(包括磁传感器的姿态参考系统)。MEMS加速度计是利用传感质量的惯性力测量的传感器，通常由标准质量块(传感元件)和检测电路组成。IMU主要由三个MEMS加速度传感器及三个陀螺和解算电路组成。
惯性传感器分为两大类：一类是角速率陀螺；另一类是线加速度计。角速率陀螺又分为：机械式干式、液浮、半液浮、气浮角速率陀螺；挠性角速率陀螺；MEMS硅石英角速率陀螺(含半球谐振角速率陀螺等)；光纤角速率陀螺；激光角速率陀螺等。线加速度计又分为：机械式线加速度计；挠性线加速度计；MEMS硅、石英线加速度计(含压阻、压电线加速度计)；石英挠性线加速度计等。
步骤S104，基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来估算所述载体的位姿误差，其中，所述弱捷联导航误差模型用于描述在所述惯性传感器与所述载体未固联情况下的真实位姿和理论位姿之间的误差变化。
依据传统的捷联惯性导航误差传播模型，引入人体晃动模型，生成弱捷联惯性导航误差传播模型。基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来估算所述载体的位姿误差。其中，位资误差包括速度误差、位置误差、姿态误差。
关于步骤S104的具体实现方式，将在下文描述图2所示出的估算位置误差的方法时详细说明，此处不再赘述。
步骤S106，解算所述载体的位姿信息，并基于所估算的位姿误差来补偿所解算出的位姿信息，以对所述载体进行定位定向。
根据弱捷联惯性误差模型建立单兵捷联惯性导航状态观测器，结合人体特殊运动状态约束，实现单兵位姿的精确估计。其中，位姿信息包括速度信息、位置信息和姿态信息。
图2是根据本公开示例性实施方式的估算载体的位姿误差的方法的流程图，如图2所示，该方法包括以下步骤：
步骤S1042，预先构建弱捷联导航误差模型。
在一个实施方式中，可以通过以下方法构建弱捷联导航误差模型：基于捷联惯性导航误差传播模型和预设的晃动模型，生成所述弱捷联导航误差模型，其中，所述晃动模型表示基于姿态误差角从传感器坐标系到载体坐标系的变换。
例如，基于所述捷联惯性导航误差传播模型和预设的晃动模型，确定随机小角度位移下的第一姿态矩阵误差、第一位置误差以及第一速度误差；和/或基于所述捷联惯性导航误差传播模型和预设的晃动模型，确定随机线位移导致的第二姿态矩阵误差、第二位置误差以及第二速度误差；其中，所述弱捷联导航误差模型包括以下至少之一：所述第一姿态矩阵误差、所述第一位置误差、第一速度误差、所述第二姿态矩阵误差、所述第二位置误差和第二速度误差。
通过上述步骤，弱捷联导航误差模型能够全部描述因为惯性传感器晃动所带来的载体，例如人体，和惯性传感器之间的角误差和线位移误差所带来的系统级误差。
在一个示例性实施方式中，可以基于公式
Figure BDA0003100381000000061
确定所述第一姿态矩阵误差，其中，
Figure BDA0003100381000000062
为惯性传感器角速度误差；
Figure BDA0003100381000000067
为姿态误差角的微分，Υ为姿态误差角向量，
Figure BDA0003100381000000063
为从传感器坐标系到载体坐标系转换的方向余弦矩阵；基于公式
Figure BDA0003100381000000064
确定第一速度误差，其中，
Figure BDA0003100381000000065
为速度误差，fb为载体坐标系的比力，Υ为姿态误差角向量，
Figure BDA0003100381000000066
为从传感器坐标系到载体坐标系转换的方向余弦矩阵，为δfc为传感器系的比力误差；基于公式
Figure BDA0003100381000000071
确定所述第一位置误差，其中，
Figure BDA0003100381000000072
为位置误差的微分，δv为速度误差。
在另一个示例性实施方式中，可以基于公式
Figure BDA0003100381000000073
确定所述第二姿态矩阵误差，其中，
Figure BDA0003100381000000074
为姿态误差角向量微分，
Figure BDA0003100381000000075
为导航坐标系相对于地球坐标系在导航坐标系投影的角速度，
Figure BDA0003100381000000076
为地球坐标系相对于惯性坐标系在导航坐标系投影的角速度；Υ2为姿态误差角向量，
Figure BDA0003100381000000077
为载体坐标系的角速度误差，
Figure BDA0003100381000000078
为从载体坐标系到导航坐标系转换的方向余弦矩阵，δ为误差；基于公式
Figure BDA0003100381000000079
Figure BDA00031003810000000710
确定第二速度误差，其中，
Figure BDA00031003810000000711
为杆臂速度，
Figure BDA00031003810000000712
为地球坐标系相对于传感器坐标系在导航坐标系投影的角速度，rn为导航系的杆臂，
Figure BDA00031003810000000713
为从传感器坐标系到导航坐标系转换的方向余弦矩阵，rc为传感器系的杆臂，
Figure BDA00031003810000000714
为所述惯性传感器的角速度；对第二速度误差积分，获得第二位置误差。
通过上述方法，将弱捷联位置、速度和姿态误差方程作为状态方程，具有可以补偿惯性传感器坐标系和载体坐标系之间的相对角误差和相对线误差的有益效果，从而可以有效克服传统捷联惯性导航定位精度相对较差的问题。
步骤S1042，设定状态约束条件。
在一个实施方式中，状态约束条件可以是人体运动状态约束条件。人体运动状态约束条件可以包括静止状态约束条件和运动状态约束条件。其中，运动状态约束条件又可以包括匀速运动约束条件、加速运动约束条件、减速运动约束条件、直线运动约束条件和曲线运动约束条件等。
其中，步骤S1040和S1042可以是预先设定的，而不需要按顺序执行。
步骤S1044，基于所采集的运动状态信息和预设的状态约束条件，生成扩展卡尔曼滤波器的量测方程。
在本公开的示例性实施方式中，当感知到人体的特殊运动状态时，如静止时，可以建立速度误差的量测方程为：Zk+1＝HXk+1，其中，Zk+1为K时刻观测量，H为观测矩阵，Xk+1为K+1时刻系统的状态向量。
步骤S1046，基于所述弱捷联导航误差模型和捷联惯性导航误差传播模型，生成所述扩展卡尔曼滤波器的状态方程。
例如，根据弱捷联导航误差模型和传统的捷联惯性导航误差传播模型，建立系统的状态方程为：Xk+1＝Fk+1Xk+Gk+1Wk，其中，Xk+1为K+1时刻系统的状态向量，FK+1为K+1时刻状态转移矩阵，GK+1为K+1时刻噪声驱动矩阵，WK为K时刻噪声矩阵。
通过上述方法，引入弱捷联导航误差模型建立卡尔曼滤波器的状态方程，根据不同的运动状态约束条件构建最优误差量测方程(即，量测方程)，并通过状态方程、量测方程构建卡尔曼滤波算法对位姿误差进行估计和补偿，从而进一步提高了对携带惯性传感器的载体的定向定位精度。
需要说明的是，对于前述的各方法实施方式，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本公开并不受所描述的动作顺序的限制，因为依据本公开，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施方式均属于优选实施方式，所涉及的动作和模块并不一定是本公开所必须的。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施方式的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本公开的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网络设备等)执行本公开各个实施方式所述的方法。
实施方式2
根据本公开实施方式，还提供了一种用于实施上述定向定位装置，如图3所示，该定向定位装置300包括：
采集模块32，被配置为通过惯性传感器采集携带所述惯性传感器的载体的运动状态信息。
误差估算模块34，被配置为基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来估算所述载体的位姿误差，其中，所述弱捷联导航误差模型用于描述在所述惯性传感器与所述载体未固联情况下的真实位姿和理论位姿之间的误差变化；
解算模块36，被配置为解算所述载体的位姿信息，并基于所估算的位姿误差来补偿所解算出的位姿信息，以对所述载体进行定位定向。
在本公开的一个实施例性实施方式中，该定位定向装置还包括弱捷联构建模块，被配置为基于捷联惯性导航误差传播模型和预设的晃动模型，生成所述弱捷联导航误差模型，其中，所述晃动模型表示基于姿态误差角从传感器坐标系到载体坐标系的变换。
其中，所述弱捷联构建模块还被配置为以下至少之一：基于所述捷联惯性导航误差传播模型和预设的晃动模型，确定随机小角度位移下的第一姿态矩阵误差、第一位置误差以及第一速度误差；基于所述捷联惯性导航误差传播模型和预设的晃动模型，确定随机线位移导致的第二姿态矩阵误差、第二位置误差以及第二速度误差；其中，所述弱捷联导航误差模型包括以下至少之一：所述第一姿态矩阵误差、所述第一位置误差、第一速度误差、所述第二姿态矩阵误差、所述第二位置误差和第二速度误差。
在本公开的一个示例性实施方式中，所述误差估算模块34可以包括方程生成单元，其被配置为：基于所采集的运动状态信息和预设的状态约束条件，生成扩展卡尔曼滤波器的量测方程；基于所述弱捷联导航误差模型和捷联误差模型，生成所述扩展卡尔曼滤波器的状态方程，其中，所述状态方程包括多维状态量，其中，所述多维状态量包括所述弱捷联导航误差模型的第一速度误差、第一位置误差、第一姿态误差、第二速度误差、第二位置误差以及第二姿态误差；其中，所述误差估算模块被配置为基于所述量测方程和所述状态方程，来估算所述载体的位姿误差。
实施方式3
根据本公开实施方式，还提供了一种用于实施上述定位定向的服务器，如图4所示，该服务器包括一种定位定向系统400，该定位定向系统包括如上所述的定向定位装置300、惯性传感器42和数据库44。
惯性传感器42被载体例如人体40携带，并被配置为采集载体的运动状态信息。
定向定位装置300被配置为获取惯性传感器42所采集的载体的运动状态信息，并基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来估算所述载体的位姿误差，然后，解算所述载体的位姿信息，并基于所估算的位姿误差来补偿所解算出的位姿信息，以对所述载体进行定位定向。其中，定向定位装置300在需要时，可以从数据库44读取所需数据，也可以将所生成的数据存储在数据库44中。
惯性传感器42和定向定位装置300之间可以通过网络连接，例如，使用WiFi、4G、5G、zigbee、蓝牙等无线技术构建的无线网络。
可选地，本实施方式中的具体示例可以参考上述实施方式1和实施方式2和实施方式3中所描述的示例，本实施方式在此不再赘述。
本公开实施方式的定向定位装置300实施了上述实施方式1中的定向定位方法，即，弱捷联单兵惯性自主定位定向方法，其利用定位定向模块中的惯性传感器测量人体的角运动和线运动信息，通过运动感知器实时感知人员的运动状态；根据采集的惯性运动信息，利用捷联惯性导航算法结合人体运动状态约束和弱捷联导航误差模型解算人员的位置、速度和姿态信息。
实施方式4
本公开的实施方式还提供了一种存储介质。可选地，在本实施方式中，上述存储介质可以实施上述实施方式1和实施方式2中所描述的方法。
可选地，在本实施方式中，上述存储介质可以位于惯性导航系统的网络中的多个网络设备中的至少一个网络设备。
可选地，在本实施方式中，上述存储介质可以包括但不限于：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
可选地，在本实施方式中，处理器根据存储介质中已存储的程序代码执行上述实施方式1和2中的方法。
可选地，本实施方式中的具体示例可以参考上述实施方式1和实施方式2中所描述的示例，本实施方式在此不再赘述。
实施方式5
本公开实施方式中的定位定向方法是一种应用于地下遮蔽空间多运动姿态的人体定位定向的弱捷联惯性定位定向误差补偿方法。如图5所示，该方法包括以下步骤：
步骤S502：分析随机小角度位移下的姿态矩阵误差、速度误差和位置误差。
(1)姿态矩阵误差分析(第一姿态矩阵误差分析)
在捷联系统中，传感器坐标系与载体坐标系(b系)重合，b系相对于导航坐标系(n系)的变换可以用方向余弦矩阵
Figure BDA0003100381000000111
表示，计算的姿态矩阵
Figure BDA0003100381000000112
可以表示为：
Figure BDA0003100381000000113
其中，B表示真实姿态矩阵到计算的姿态矩阵的变换矩阵。对于较小的随机误差角，B可以近似表示为
B＝[I-ψ] (2)
其中ψ为姿态误差角构成的斜对称矩阵，I为单位矩阵。
此时估算的方向余弦矩阵可以表示为
Figure BDA0003100381000000114
对上式进行整理可得，
Figure BDA0003100381000000115
对上式求导，并进一步整理可得
Figure BDA0003100381000000121
式(5)就是传统的捷联惯性导航误差传播模型。但在弱捷联的情况下，公式(1)应该表示为：
Figure BDA0003100381000000122
其中，U表示传感器坐标系到载体坐标系的变换。其中：
U＝[I-Υ] (7)
其中，Υ为姿态误差角向量。
Figure BDA0003100381000000123
其中Υ为姿态误差角构成的斜对称矩阵，δ为误差，ac为传感器俯仰角，Bc为传感器偏航角，yc为传感器滚转角。为了更好的表示传感器和人体坐标系的偏差，下面用b代表人体坐标系(载体坐标系)，c代表传感器坐标系。传感器坐标系相对于人体坐标系的变换可以用方向余弦矩阵
Figure BDA0003100381000000124
表示，计算的姿态矩阵
Figure BDA0003100381000000125
可以表示为：
Figure BDA0003100381000000126
则弱捷联误差传播同理可推得：
Figure BDA0003100381000000127
该方程通过求导可得：
Figure BDA0003100381000000128
其中，
Figure BDA0003100381000000129
为从传感器坐标系到载体坐标系转换方向余弦矩阵的微分的转置。
依据捷联惯性导航可知，方向余弦矩阵是作为绝对载体速率和导航坐标系速率的函数进行传播的，其传播方程为：
Figure BDA00031003810000001210
其中，
Figure BDA0003100381000000131
表示测量载体的角速度；
Figure BDA0003100381000000132
为导航坐标系相对于惯性坐标系在导航坐标系投影的角速度。
但对比传统捷联惯性导航可以看出，弱捷联导航误差特性是研究关于从传感器坐标系变换为载体坐标系之间的关系，不存在相对坐标系转动。所以弱捷联的方向余弦矩阵只是作为绝对载体速率的函数进行传播的，可以忽略
Figure BDA0003100381000000133
这一项。同理写出弱捷联方向余弦的传播方程：
Figure BDA0003100381000000134
同样估算矩阵的时间微分方程可以表示为：
Figure BDA0003100381000000135
式中，
Figure BDA0003100381000000136
为传感器的测量角速度。
把(13)和(14)带入(11)，可以得
Figure BDA0003100381000000137
把式(9)中的
Figure BDA0003100381000000138
带入方程(15)得
Figure BDA0003100381000000139
令
Figure BDA00031003810000001310
可以得到下式：
Figure BDA00031003810000001311
通过元素对比后写成向量形式为：
Figure BDA00031003810000001312
其中，其中，
Figure BDA00031003810000001313
为惯性传感器角速度误差；
Figure BDA00031003810000001314
为姿态误差角的微分，γ为姿态误差角向量，
Figure BDA00031003810000001315
为从传感器坐标系到载体坐标系转换的方向余弦矩阵，Υ＝[δαc δβc δγc]T为姿态误差角向量，且有：
Figure BDA00031003810000001316
(2)位置和速度误差分析(第一位置误差和第一速度误差分析)
在传统捷联惯性导航中，速度方程可以表示为
Figure BDA0003100381000000141
式中fb表示加速度计输出在载体坐标系的比力，其中，
Figure BDA0003100381000000142
为导航坐标系相对于地球坐标系在导航坐标系投影的角速度，
Figure BDA0003100381000000143
为地球坐标系相对于惯性坐标系在导航坐标系投影的角速度，v为速度，g1为重力加速度矢量。
同理考虑到传感器坐标器和载体坐标系的相对运动，而非导航坐标系和载体坐标系的相对运动，方程可以简化可得：
Figure BDA0003100381000000144
其中，fc表示加速度计输出在传感器坐标系的比力。
同样估算的速度可以以下式子传播：
Figure BDA0003100381000000145
对于式子(20)和(21)进行差分运算
Figure BDA0003100381000000146
把式
Figure BDA0003100381000000147
带入上式，并且令
Figure BDA0003100381000000148
化简整理可得
Figure BDA0003100381000000149
其中，设定比力在人体坐标系上的投影为
Figure BDA00031003810000001410
然后再次整理可得
Figure BDA00031003810000001411
忽略重力矢量误差的情况下，方程(24)可以简化为
Figure BDA00031003810000001412
最后位置误差传播方式可表示为下式
Figure BDA00031003810000001413
其中，
Figure BDA00031003810000001414
为位置误差的微分，δv为速度误差。
上述步骤S502和S504是依据传统的捷联惯性导航误差传播规律，引入人体晃动模型，推导得到弱捷联惯性误差模型。
步骤S504：分析随机线位移导致的姿态矩阵、位置和速度误差。
(1)位置和速度误差(第二位置误差和第二速度误差)
由于惯性传感器无法完全固联在载体例如人体上，人体和惯性传感器之间存在晃动传感器的质心和质心不重合，导致惯性传感器和人体之间存在杆臂效应。杆臂效应常见于惯性导航系统和其他导航系统的组合中，在弱捷联情况下人体坐标系和传感器坐标系存在相对运动，引起传感器加速度值与载体真实加速度值的大小和方向存在一定的偏差，导致惯性导航系统的导航坐标系速度及位置导航参数存在误差。
因为杆臂效应的存在所以弱捷联速度误差和加速度误差并不能只用失准角误差推导得出，所以弱捷联导航速度误差模型还要针对线速度误差来推导。
假设人体相对地心位置向量为Rb，惯性传感器相对地心位置向量为Rc，人体与惯性传感器之间相对位移向量为r，三者关系为：
Rc＝Rb+r (27)
根据哥氏定理将式(27)对地球e坐标系同时求导，再向n系投影
Figure BDA0003100381000000151
c为传感器坐标系。
令传感器在导航坐标系上的速度为
Figure BDA0003100381000000152
人体在导航系上的速度为
Figure BDA0003100381000000153
因为存在杆臂速度
Figure BDA0003100381000000154
其中，
Figure BDA0003100381000000155
为载体坐标系相对地球坐标系在导航坐标系投影的速度，
Figure BDA0003100381000000156
为传感器坐标系相对地球坐标系在导航坐标系投影的速度，Vr为相对速度，δV为速度误差。由此可得下式：
Figure BDA0003100381000000161
由于地球转动相对较慢，人体和载体位移相对较小，所以
Figure BDA0003100381000000162
所以尽可能简化为
Figure BDA0003100381000000163
其中，
Figure BDA0003100381000000164
为传感器的角速度，
Figure BDA0003100381000000165
为杆臂速度，
Figure BDA0003100381000000166
为地球坐标系相对于传感器坐标系在导航坐标系投影的角速度，rn为导航系的杆臂，
Figure BDA0003100381000000167
为从传感器坐标系到导航坐标系转换的方向余弦矩阵，rc为传感器系的杆臂，其中，
Figure BDA0003100381000000168
可以由传感器输出的导航参数得到。
根据抑制的杆臂r，可计算出杆臂速度
Figure BDA0003100381000000169
并可对传感器的导航系速度进性补偿。
(2)加速度误差分析
同理，根据哥氏定理可以将式(27)两边惯性坐标系对时间求二阶导数，再向c系投影，
Figure BDA00031003810000001610
同理
Figure BDA00031003810000001611
可简化为
Figure BDA00031003810000001612
令
Figure BDA00031003810000001613
由式(30)可得
Figure BDA00031003810000001614
则杆臂加速度为
Figure BDA00031003810000001615
其中
Figure BDA00031003810000001616
为传感器测量加速度，
Figure BDA00031003810000001617
为载体加速度。
(3)姿态矩阵误差分析(第二姿态矩阵误差分析)
导航坐标系的转移速率用公式表示成向量形式为：
Figure BDA0003100381000000171
VN为北向速度；VE为东向速度；Ro为地球半径；L为纬度；h为距离地面的高度，Ve为东向速度。
方向余弦其传播方程为：
Figure BDA0003100381000000172
其中
Figure BDA0003100381000000173
代入得
Figure BDA0003100381000000174
同理设线位移所带来的小失准角为E
E＝[I-Υ2] (36)
可以整理为：
Figure BDA0003100381000000175
同样估算矩阵
Figure BDA0003100381000000176
的时间微分方程表示为：
Figure BDA0003100381000000177
将
Figure BDA0003100381000000178
和
Figure BDA0003100381000000179
代入方程5可得：
Figure BDA00031003810000001710
再次化简为
Figure BDA00031003810000001711
设定
Figure BDA00031003810000001712
并且忽略误差乘积项，可得
Figure BDA0003100381000000181
因为
Figure BDA0003100381000000182
为定值所以
Figure BDA0003100381000000183
用矢量形式表示为
Figure BDA0003100381000000184
且有
Figure BDA0003100381000000185
其中，Υ2为线位移导致的姿态误差角矢量，
Figure BDA0003100381000000186
为姿态误差角向量微分，
Figure BDA0003100381000000187
为导航坐标系相对于地球坐标系在导航坐标系投影的角速度，
Figure BDA0003100381000000188
为地球坐标系相对于惯性坐标系在导航坐标系投影的角速度；Υ2为姿态误差角向量，
Figure BDA0003100381000000189
为载体坐标系的角速度误差，
Figure BDA00031003810000001810
为从载体坐标系到导航坐标系转换的方向余弦矩阵，δ为误差。
步骤S506：建立扩展卡尔曼滤波器的弱捷联误差状态模型和人体姿态约束条件下的量测方程。
把步骤S502和S504中所分析的弱捷联误差模型和传统的捷联惯性导航误差模型作为扩展卡尔曼滤波器的状态方程。本方法采用的18维状态量包含传统捷联惯性导航误差模型和弱捷联导航误差模型的速度误差、位置误差和姿态误差，即，第一速度误差、第二速度误差、第一位置误差、第二位置误差、第一姿态矩阵误差和第二姿态矩阵误差等。在其他的实施方式中，弱捷联导航误差模型还可以包括加速度误差。
然后，根据已知的人体运动状态构建实时人体零速状态的量测方程。
步骤S508：弱捷联单兵惯性自主定位定向误差补偿。
根据弱捷联惯性导航误差传播模型和传统的捷联惯性导航误差传播模型，建立系统的状态方程为：
Xk+1＝Fk+1Xk+Gk+1Wk (43)
其中，Xk+1为K+1时刻系统的状态向量，FK+1为K+1时刻状态转移矩阵，GK+1为K+1时刻噪声驱动矩阵，WK为K时刻噪声矩阵。
当感知到人体的特殊运动状态时，如静止时，可以建立速度误差的量测方程为：
Zk+1＝HXk+1 (44)
其中，Zk+1为K+1时刻观测量，H为观测矩阵，Xk+1为K+1时刻系统的状态向量。
Figure BDA0003100381000000191
导航系统首先进行初始化导航参数和初始对准，然后设置初始状态和协方差矩阵，最后卡尔曼滤波进行时间更新和量测更新。
时间更新为：
Figure BDA0003100381000000192
Figure BDA0003100381000000193
其中，
Figure BDA0003100381000000194
为当前状态的估计值，
Figure BDA0003100381000000195
为上一状态的估计值，f为非线性状态方程，tk-1为K-1时刻的时间值，TS为采样时间，Pk|k-1为K-1时刻到K时刻状态误差协方差矩阵，Pk-1为K-1时刻状态协方差矩阵，Φk|k-1为状态转移矩阵，Qk-1为过程噪声协方差矩阵。
量测更新为：
Figure BDA0003100381000000196
Figure BDA0003100381000000197
Figure BDA0003100381000000198
Figure BDA0003100381000000199
其中，
Figure BDA00031003810000001910
为预测的观测值，h为非线性观测方程，tk为K时刻时间值，Kk为卡尔曼增益，Pk|k-1为K-1时刻到K时刻状态误差协方差矩阵，
Figure BDA00031003810000001911
为观测矩阵，Rk为观测数据的协方差矩阵，zk为当前状态的测量值，Pk为K时刻的状态协方差矩阵，Hk为观测矩阵，
Figure BDA00031003810000001912
为卡尔曼增益的转置，K为第K时刻，T为时间值。
F(tk-1)与Hk为雅克比矩阵和海森矩阵，F(tk-1)的离散形式为Φk|k-1，如下：
Figure BDA0003100381000000201
Figure BDA0003100381000000202
Φk|k-1≈I+F(tk-1)Ts (50)
其中，xT为状态变量，tk-1为K-1时刻的时间值，k为时刻，xT(tk-1)为K-1时间的状态变量，
Figure BDA0003100381000000203
为偏导数，h为非线性观测方程，Ts为采样时间。
EKF对非线性系统函数近似线性化处理，利用泰勒级数展开式，舍去高阶项，保留级数低阶项。
根据上述的状态方程和量测方程，便可以利用卡尔曼滤波算法进行导航误差的估计和补偿，抑制位姿误差的发散，保证定位定向精度。当探测到其他的特殊运动形式时，同样可以采取上述的方法，设置合适的误差量测，进行导航误差的估计。
综上所述，本公开提出的定位定向方法是一种弱捷联单兵定位定向方法，这种方法解决了灾难环境下人体复杂运动带来的基准晃动和IMU器件难以可靠固联导致的传统捷联惯性导航误差传播规律无法表征导航参数误差传播特性的问题。
本公开实施方式的弱捷联单兵惯性自主定位定向方法，是为了解决灾难环境下，人体复杂运动带来的基准晃动和IMU器件难以可靠固联的情况下，引发的传统捷联惯性导航误差传播规律已无法表征真实的导航参数误差传播特性，进而无法补偿真实误差导致定位精度差的问题。本公开研究导航基准随机晃动的建模方法，推导了弱捷联导航误差模型，并提出了一种弱捷联单兵惯性自主定位定向方法。弱捷联导航误差模型可以有效地表征灾难环境下人体复杂运动所带来的捷联惯性导航误差模型所无法估计的误差传播特性。弱捷联单兵惯性自主定位定向方法针对弱捷联误差方程和运动状态约束方程建立扩展卡尔曼滤波器对弱捷联情况下单兵位置、速度和姿态误差进行估计和补偿。
实施方式6
图8是根据本公开实施方式的另一种定位定向方法的流程图，如图8所示，该方法包括以下步骤：
步骤S802，MENS-IMU三轴加速度计、陀螺仪采集运动状态信息，例如加速度a，角速度ω。
步骤S804，基于所采集到的运动状态信息进行运动状态识别。
步骤S806，基于所识别出的运动状态、预先构建的弱捷联误差模型、预设的状态约束条件，生成扩展卡尔曼滤波的状态方程和量测方程。
步骤S808，进行扩展卡尔曼滤波，得到位置误差、速度误差和姿态误差。
步骤S810，基于所得到的位置误差、速度误差和姿态误差，进行捷联结算，得到载体的位置、速度和姿态。
实施方式7
图9是根据本公开实施方式的坐标统一方法的流程图。坐标统一的目的在于对载体进行定位定向，因此，坐标统一方法实际也是一种定位定向方法。如图9所示，该方法包括以下步骤：
步骤S902，初始对准。
把坐标系的原点对准初始绝对地理信息远点，坐标系方向分别对准东北天方向。
步骤S904，弱捷联误差校准。
把传感器坐标系原点对准载体坐标系(例如，人体坐标系)原点，把传感器坐标系对准载体坐标系的滚转、俯仰和偏航。
步骤S906，捷联导航误差校准。
把载体坐标系原点对准当地导航坐标系原点，把载体坐标系对准当地导航坐标系的东北天方向。
步骤S908：得到绝对地理坐标系。
实施方式8
下面参考图10，其示出了适于用来实现本公开实施方式的计算机设备800的结构示意图。图10示出的计算机设备仅仅是一个示例，不应对本公开实施方式的功能和使用范围带来任何限制。
如图10所示，计算机设备800包括中央处理单元(CPU)801，其可以根据存储在只读存储器(ROM)802中的程序或者从存储部分808加载到随机访问存储器(RAM)803中的程序而执行各种适当的动作和处理。在RAM803中，还存储有设备800操作所需的各种程序和数据。CPU801、ROM802以及RAM803通过总线804彼此相连。输入/输出(I/O)接口805也连接至总线804。
以下部件连接至I/O接口805：包括键盘、鼠标等的输入部分806；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分807；包括硬盘等的存储部分808；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分809。通信部分809经由诸如因特网的网络执行通信处理。驱动器810也根据需要连接至I/O接口805。可拆卸介质811，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器810上，以便于从其上读出的计算机程序根据需要被安装入存储部分808。
根据本公开的实施方式，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施方式包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施方式中，该计算机程序可以通过通信部分809从网络上被下载和安装，和/或从可拆卸介质811被安装。在该计算机程序被中央处理单元(CPU)801执行时，执行本公开的方法中限定的上述功能。需要说明的是，本公开的计算机存储介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本公开中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
附图中的流程图和框图，图示了按照本公开各种实施方式的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本公开实施方式中所涉及到的模块或单元可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的模块或单元也可以设置在处理器中，这些模块或单元的名称在某种情况下并不构成对该模块或单元本身的限定。
上述本公开实施方式序号仅仅为了描述，不代表实施方式的优劣。
上述实施方式中的集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在上述计算机可读取的存储介质中。基于这样的理解，本公开的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在存储介质中，包括若干指令用以使得一台或多台计算机设备(可为个人计算机、服务器或者网络设备等)执行本公开各个实施方式所述方法的全部或部分步骤。
在本公开的上述实施方式中，对各个实施方式的描述都各有侧重，某个实施方式中没有详述的部分，可以参见其他实施方式的相关描述。
在本申请所提供的几个实施方式中，应该理解到，所揭露的客户端，可通过其它的方式实现。其中，以上所描述的装置实施方式仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施方式方案的目的。
另外，在本公开各个实施方式中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
以上所述仅是本公开的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本公开原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本公开的保护范围。

Claims (13)

1.一种定位定向方法，包括：
通过惯性传感器采集携带所述惯性传感器的载体的运动状态信息；
基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来估算所述载体的位姿误差，其中，所述弱捷联导航误差模型用于描述在所述惯性传感器与所述载体未固联情况下的真实位姿和理论位姿之间的误差变化；
解算所述载体的位姿信息，并基于所估算的位姿误差来补偿所解算出的位姿信息，以对所述载体进行定位定向。
2.根据权利要求1所述的方法，其中，所述弱捷联导航误差模型是通过以下方法构建的：基于捷联惯性导航误差传播模型和预设的晃动模型，生成所述弱捷联导航误差模型，其中，所述晃动模型表示基于姿态误差角从传感器坐标系到载体坐标系的变换。
3.根据权利要求2所述的方法，其中，基于捷联惯性导航误差传播模型和预设的晃动模型，生成所述弱捷联导航误差模型包括以下至少之一：
基于所述捷联惯性导航误差传播模型和预设的晃动模型，确定随机小角度位移下的第一姿态矩阵误差、第一位置误差以及第一速度误差；
基于所述捷联惯性导航误差传播模型和预设的晃动模型，确定随机线位移导致的第二姿态矩阵误差、第二位置误差以及第二速度误差；
其中，所述弱捷联导航误差模型包括以下至少之一：所述第一姿态矩阵误差、所述第一位置误差、第一速度误差、所述第二姿态矩阵误差、所述第二位置误差和第二速度误差。
4.根据权利要求3所述的方法，其中，基于捷联惯性导航误差传播模型和预设的晃动模型，确定随机小角度位移下的第一姿态矩阵误差、第一位置误差以及第一速度误差包括：
基于公式
Figure FDA0003100380990000021
确定所述第一姿态矩阵误差，其中，
Figure FDA0003100380990000022
为惯性传感器角速度误差；
Figure FDA0003100380990000023
为姿态误差角的微分，Υ为姿态误差角向量，
Figure FDA0003100380990000024
为从传感器坐标系到载体坐标系转换的方向余弦矩阵；
基于公式
Figure FDA0003100380990000025
确定第一速度误差，其中，
Figure FDA0003100380990000026
为速度误差，fb为载体坐标系的比力，Υ为姿态误差角向量，
Figure FDA0003100380990000027
为从传感器坐标系到载体坐标系转换的方向余弦矩阵，为δfc为传感器系的比力误差；
基于公式
Figure FDA0003100380990000028
确定所述第一位置误差，其中，
Figure FDA0003100380990000029
为位置误差的微分，δv为速度误差。
5.根据权利要求3所述的方法，其中，基于捷联惯性导航误差传播模型和预设的晃动模型，确定随机线位移导致的第二姿态矩阵误差、第二位置误差以及第二速度误差包括：
基于公式
Figure FDA00031003809900000210
确定所述第二姿态矩阵误差，其中，
Figure FDA00031003809900000211
为姿态误差角向量微分，
Figure FDA00031003809900000212
为导航坐标系相对于地球坐标系在导航坐标系投影的角速度，
Figure FDA00031003809900000213
为地球坐标系相对于惯性坐标系在导航坐标系投影的角速度；Υ2为姿态误差角向量，
Figure FDA00031003809900000214
为载体坐标系的角速度误差，
Figure FDA00031003809900000215
为从载体坐标系到导航坐标系转换的方向余弦矩阵，δ为误差；
基于公式
Figure FDA0003100380990000031
角定第二速度误差，其中，
Figure FDA0003100380990000032
为杆臂速度，
Figure FDA0003100380990000033
为地球坐标系相对于传感器坐标系在导航坐标系投影的角速度，rn为导航系的杆臂，
Figure FDA0003100380990000034
为从传感器坐标系到导航坐标系转换的方向余弦矩阵，rc为传感器系的杆臂，
Figure FDA0003100380990000035
为所述惯性传感器的角速度；
对第二速度误差积分，获得第二位置误差。
6.根据权利要求1至5中任一项所述的方法，其中，基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来估算所述载体的位姿误差包括：
基于所采集的运动状态信息和预设的状态约束条件，生成扩展卡尔曼滤波器的量测方程；
基于所述弱捷联导航误差模型和捷联误差模型，生成所述扩展卡尔曼滤波器的状态方程，其中，所述状态方程包括多维状态量，其中，所述多维状态量包括所述弱捷联导航误差模型的第一速度误差、第一位置误差、第一姿态误差、第二速度误差、第二位置误差以及第二姿态误差；
基于所述量测方程和所述状态方程，来估算所述载体的位姿误差。
7.根据权利要求6所述的方法，其中，基于所述量测方程和所述状态方程，来估算所述载体的位姿误差包括：
利用扩展卡尔曼滤波算法对所述状态方程进行时间更新，并对所述量测方程进行量测更新，来估算所述载体的位姿误差。
8.一种定位定向装置，其特征在于，包括：
采集模块，被配置为通过惯性传感器采集携带所述惯性传感器的载体的运动状态信息；
误差估算模块，被配置为基于所采集的运动状态信息、预设的状态约束条件和预先构建的弱捷联导航误差模型，来估算所述载体的位姿误差，其中，所述弱捷联导航误差模型用于描述在所述惯性传感器与所述载体未固联情况下的真实位姿和理论位姿之间的误差变化；
解算模块，被配置为解算所述载体的位姿信息，并基于所估算的位姿误差来补偿所解算出的位姿信息，以对所述载体进行定位定向。
9.根据权利要求8所述的装置，其中，所述装置还包括弱捷联构建模块，被配置为基于捷联惯性导航误差传播模型和预设的晃动模型，生成所述弱捷联导航误差模型，其中，所述晃动模型表示基于姿态误差角从传感器坐标系到载体坐标系的变换。
10.根据权利要求9所述的装置，其中，所述弱捷联构建模块还被配置为以下至少之一：
基于所述捷联惯性导航误差传播模型和预设的晃动模型，确定随机小角度位移下的第一姿态矩阵误差、第一位置误差以及第一速度误差；
基于所述捷联惯性导航误差传播模型和预设的晃动模型，确定随机线位移导致的第二姿态矩阵误差、第二位置误差以及第二速度误差；
其中，所述弱捷联导航误差模型包括以下至少之一：所述第一姿态矩阵误差、所述第一位置误差、第一速度误差、所述第二姿态矩阵误差、所述第二位置误差和第二速度误差。
11.根据权利要求8至10中任一项所述的装置，其中，所述误差估算模块包括方程生成单元，被配置为：
基于所采集的运动状态信息和预设的状态约束条件，生成扩展卡尔曼滤波器的量测方程；
基于所述弱捷联导航误差模型和捷联误差模型，生成所述扩展卡尔曼滤波器的状态方程，其中，所述状态方程包括多维状态量，其中，所述多维状态量包括所述弱捷联导航误差模型的第一速度误差、第一位置误差、第一姿态误差、第二速度误差、第二位置误差以及第二姿态误差；
其中，所述误差估算模块被配置为基于所述量测方程和所述状态方程，来估算所述载体的位姿误差。
12.一种定位定向系统，其特征在于，包括
惯性传感器，被载体携带，被配置为采集载体的运动状态信息；
根据权利要求8至10中任一项所述的定位定向装置。
13.一种计算机可读存储介质，其上存储有程序，其特征在于，当所述程序被执行时，使得计算机执行如权利要求1至7中任一项所述的方法。
CN202110622364.3A 2021-06-04 2021-06-04 定位定向方法、装置及系统 Active CN113465599B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110622364.3A CN113465599B (zh) 2021-06-04 2021-06-04 定位定向方法、装置及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110622364.3A CN113465599B (zh) 2021-06-04 2021-06-04 定位定向方法、装置及系统

Publications (2)

Publication Number Publication Date
CN113465599A true CN113465599A (zh) 2021-10-01
CN113465599B CN113465599B (zh) 2023-08-01

Family

ID=77872249

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110622364.3A Active CN113465599B (zh) 2021-06-04 2021-06-04 定位定向方法、装置及系统

Country Status (1)

Country Link
CN (1) CN113465599B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114459469A (zh) * 2022-01-14 2022-05-10 北京信息科技大学 多运动状态导航方法、装置及智能可穿戴设备
CN114463932A (zh) * 2022-01-14 2022-05-10 国网江苏省电力工程咨询有限公司 非接触式施工安全距离主动动态识别预警系统及方法
CN114812546A (zh) * 2022-04-20 2022-07-29 北京信息科技大学 遮蔽空间单兵导航位姿修正方法及装置
CN115183732A (zh) * 2022-06-27 2022-10-14 三一重机有限公司 一种挖掘机的位姿标定方法及其系统、挖掘机

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102192716A (zh) * 2010-12-08 2011-09-21 陕西宝成航空仪表有限责任公司 转台动态指标的激光干涉检测法及检测系统
CN102538792A (zh) * 2012-02-08 2012-07-04 北京航空航天大学 一种位置姿态系统的滤波方法
US20120221244A1 (en) * 2011-02-28 2012-08-30 Trusted Positioning Inc. Method and apparatus for improved navigation of a moving platform
CN103913179A (zh) * 2014-03-19 2014-07-09 中国人民解放军63680部队 舰载单轴旋转调制捷联惯导姿态角修正方法
CN104165638A (zh) * 2014-08-07 2014-11-26 北京理工大学 一种双轴旋转惯导系统多位置自主标定方法
US20180128645A1 (en) * 2016-11-09 2018-05-10 Atlantic Inertial Systems Limited Navigation system
CN108398130A (zh) * 2018-02-22 2018-08-14 北京航空航天大学 挠曲形变测量网络的分布式pos传递对准建模方法和装置
CN110132308A (zh) * 2019-05-27 2019-08-16 东南大学 一种基于姿态确定的usbl安装误差角标定方法
CN110285804A (zh) * 2019-06-26 2019-09-27 南京航空航天大学 基于相对运动模型约束的车辆协同导航方法
US20200039522A1 (en) * 2018-07-31 2020-02-06 Seiko Epson Corporation Posture estimation method, posture estimation device, and vehicle
CN111060140A (zh) * 2019-12-31 2020-04-24 西北工业大学 一种地球椭球模型下的极区惯性导航误差获得方法
CN111678538A (zh) * 2020-07-29 2020-09-18 中国电子科技集团公司第二十六研究所 一种基于速度匹配的动态水平仪误差补偿方法
CN111780753A (zh) * 2020-06-10 2020-10-16 北京航天控制仪器研究所 一种姿态误差反馈修正的提高惯性制导精度的方法
CN111795708A (zh) * 2020-06-16 2020-10-20 湖南跨线桥航天科技有限公司 晃动基座条件下陆用惯性导航系统的自适应初始对准方法

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102192716A (zh) * 2010-12-08 2011-09-21 陕西宝成航空仪表有限责任公司 转台动态指标的激光干涉检测法及检测系统
US20120221244A1 (en) * 2011-02-28 2012-08-30 Trusted Positioning Inc. Method and apparatus for improved navigation of a moving platform
CN102538792A (zh) * 2012-02-08 2012-07-04 北京航空航天大学 一种位置姿态系统的滤波方法
CN103913179A (zh) * 2014-03-19 2014-07-09 中国人民解放军63680部队 舰载单轴旋转调制捷联惯导姿态角修正方法
CN104165638A (zh) * 2014-08-07 2014-11-26 北京理工大学 一种双轴旋转惯导系统多位置自主标定方法
US20180128645A1 (en) * 2016-11-09 2018-05-10 Atlantic Inertial Systems Limited Navigation system
CN108398130A (zh) * 2018-02-22 2018-08-14 北京航空航天大学 挠曲形变测量网络的分布式pos传递对准建模方法和装置
US20200039522A1 (en) * 2018-07-31 2020-02-06 Seiko Epson Corporation Posture estimation method, posture estimation device, and vehicle
CN110132308A (zh) * 2019-05-27 2019-08-16 东南大学 一种基于姿态确定的usbl安装误差角标定方法
CN110285804A (zh) * 2019-06-26 2019-09-27 南京航空航天大学 基于相对运动模型约束的车辆协同导航方法
CN111060140A (zh) * 2019-12-31 2020-04-24 西北工业大学 一种地球椭球模型下的极区惯性导航误差获得方法
CN111780753A (zh) * 2020-06-10 2020-10-16 北京航天控制仪器研究所 一种姿态误差反馈修正的提高惯性制导精度的方法
CN111795708A (zh) * 2020-06-16 2020-10-20 湖南跨线桥航天科技有限公司 晃动基座条件下陆用惯性导航系统的自适应初始对准方法
CN111678538A (zh) * 2020-07-29 2020-09-18 中国电子科技集团公司第二十六研究所 一种基于速度匹配的动态水平仪误差补偿方法

Non-Patent Citations (12)

* Cited by examiner, † Cited by third party
Title
HU HUAFENG等: "An Inner Lever-Arm Calibration Method for Self-Alignment of Rotating IMU", ELECTRONICS OPTICS & CONTROL *
冉昌艳: "非线性滤波方法在AUV导航系统中的应用研究", 中国博士学位论文全文数据库 (信息科技辑) *
刘恒志: "一种12维零速状态更新的智能行人航位推算", 系统仿真学报 *
孙枫;孙伟;: "基于单轴旋转的光纤捷联惯导系统误差特性与实验分析", 宇航学报 *
宋新宇;戈新生;: "挠性航天器动力学模型的非约束模态分析", 力学学报 *
张立强: "基于准静止磁场检测的行人导航算法框架iIEZ+", 传感技术学报 *
梁浩: "初始方位对准及在轨修正技术研究", 中国优秀硕士学位论文全文数据库 (工程科技Ⅱ辑) *
汤奥斐;仇原鹰;米建伟;孔令飞;: "柔索驱动并联机器人变态模型相似性分析", 应用基础与工程科学学报 *
汪宋兵: "基于UWB/MEMS组合的行人室内定位系统关键技术研究", 中国优秀硕士学位论文全文数据库 (工程科技Ⅱ辑) *
王铨: "基于MEMS惯性传感器的井下人员定位系统", 中国优秀硕士学位论文全文数据库 (信息科技辑) *
赵旭: "车联网卫星/微惯性组合导航定位系统研究", 中国优秀硕士学位论文全文数据库 (工程科技Ⅱ辑) *
高哲等: "行人自主导航定位的IEKF-SWCS方法", 系统仿真学报 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114459469A (zh) * 2022-01-14 2022-05-10 北京信息科技大学 多运动状态导航方法、装置及智能可穿戴设备
CN114463932A (zh) * 2022-01-14 2022-05-10 国网江苏省电力工程咨询有限公司 非接触式施工安全距离主动动态识别预警系统及方法
CN114459469B (zh) * 2022-01-14 2023-05-23 北京信息科技大学 多运动状态导航方法、装置及智能可穿戴设备
CN114463932B (zh) * 2022-01-14 2024-05-03 国网江苏省电力工程咨询有限公司 非接触式施工安全距离主动动态识别预警系统及方法
CN114812546A (zh) * 2022-04-20 2022-07-29 北京信息科技大学 遮蔽空间单兵导航位姿修正方法及装置
CN115183732A (zh) * 2022-06-27 2022-10-14 三一重机有限公司 一种挖掘机的位姿标定方法及其系统、挖掘机
CN115183732B (zh) * 2022-06-27 2023-10-20 三一重机有限公司 一种挖掘机的位姿标定方法及其系统、挖掘机

Also Published As

Publication number Publication date
CN113465599B (zh) 2023-08-01

Similar Documents

Publication Publication Date Title
CN113465599B (zh) 定位定向方法、装置及系统
Ludwig et al. Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data
CN109269471B (zh) 一种新型gnss接收机倾斜测量系统及方法
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
CN106225784B (zh) 基于低成本多传感器融合行人航位推算方法
US9541392B2 (en) Surveying system and method
KR101168100B1 (ko) 차량의 위치, 자세 및 헤딩을 추측하는 시스템 및 방법
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN105547288A (zh) 一种煤矿井下移动设备自主定位的方法及系统
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN109916394A (zh) 一种融合光流位置和速度信息的组合导航算法
CN110398245A (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
EP1604173B1 (en) Method for measuring force-dependent gyroscope sensitivity
US20140222369A1 (en) Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method
US11408735B2 (en) Positioning system and positioning method
CN109764870A (zh) 基于变换估计量建模方案的载体初始航向估算方法
CN117289322A (zh) 一种基于imu、gps与uwb的高精度定位算法
Luna et al. An indoor pedestrian positioning system based on inertial measurement unit and wireless local area network
Abdelfatah et al. UAV tracking system using integrated sensor fusion with RTK-GPS
CN115356965B (zh) 一种松散耦合实装数据采集装置及数据处理方法
CN115790585A (zh) 一种视觉辅助的步态特征约束行人导航方法
Sang et al. A self-developed indoor three-dimensional pedestrian localization platform based on MEMS sensors
CN107289935B (zh) 一种适用于可穿戴设备的室内导航算法
Munoz Diaz et al. Geographical and Fingerprinting Data for Positioning and Navigation Systems
Li et al. A multisensor integration approach toward astronaut navigation for landed lunar missions

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant