CN113419265B - 一种基于多传感器融合的定位方法、装置与电子设备 - Google Patents

一种基于多传感器融合的定位方法、装置与电子设备 Download PDF

Info

Publication number
CN113419265B
CN113419265B CN202110658126.8A CN202110658126A CN113419265B CN 113419265 B CN113419265 B CN 113419265B CN 202110658126 A CN202110658126 A CN 202110658126A CN 113419265 B CN113419265 B CN 113419265B
Authority
CN
China
Prior art keywords
positioning
value
inertial
attitude
error
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.)
Active
Application number
CN202110658126.8A
Other languages
English (en)
Other versions
CN113419265A (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.)
Hunan Bds Micro Chipset Industry Development Co ltd
Original Assignee
Hunan Bds Micro Chipset Industry Development Co ltd
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 Hunan Bds Micro Chipset Industry Development Co ltd filed Critical Hunan Bds Micro Chipset Industry Development Co ltd
Priority to CN202110658126.8A priority Critical patent/CN113419265B/zh
Publication of CN113419265A publication Critical patent/CN113419265A/zh
Application granted granted Critical
Publication of CN113419265B publication Critical patent/CN113419265B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • 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/165—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 combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/03—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers
    • G01S19/10—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing dedicated supplementary positioning signals
    • G01S19/12—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing dedicated supplementary positioning signals wherein the cooperating elements are telecommunication base stations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Abstract

本发明公开了一种基于多传感器融合定位方法、装置与电子设备，实现多场景下对移动目标的定位跟踪与管理，满足室内外定位无缝切换要求，应用对象为有较高精度定位与跟踪需求的行人或宠物牲畜等，兼顾物流车载等对象的定位功能。特别地，利用惯性系统不依赖外部设备辅助、响应迅速的特点，提高定位对象在信号阻挡环境下的短时短距离定位可靠性。该方法针对特定安装对象利用可用的运动学约束与状态约束，修正其惯导积分定位误差与航向角误差，并实时采集卫星定位信息与蜂窝基站定位信息，使系统设备实现长时间长距离定位。

Description

一种基于多传感器融合的定位方法、装置与电子设备
技术领域
本发明涉及移动定位技术领域，特别涉及一种基于多传感器融合的定位方法、装置与电子设备。
背景技术
随着北斗产业建设日趋完善、新一代蜂窝移动网络技术不断演进，北斗位置增强服务正积极着眼于导航与通信的融合创新。当前，针对室外环境长时长距离目标跟踪与定位的应用，多利用移动蜂窝基站与卫星数据协同进行，处理的方法包括搭建AGPS协助服务、通过蜂窝基站共享卫星轨迹数据库、利用通信基站转发卫星信号、利用蜂窝网络建立卫星的广域增强系统等。
然而，单独使用卫星定位需要着重处理时间同步误差、电离层和对流层延时修正、多普勒频移等影响；而无线通信易受多径效应、NLOS(非视距传播)误差影响，导致此类通信定位方式在桥梁、隧道、树丛、工地、地下车库等区域应用受限，引起由信号阻挡导致的距离测量误差，进而影响定位精度。
发明内容
本发明旨在至少解决现有技术中存在的技术问题。为此，本发明提出一种基于多传感器融合的定位方法、装置与电子设备，结合约束条件、卫星定位数据以及移动蜂窝定位数据对惯性解算结果进行修正，能够解决在复杂地形情况下定位不准确的难题。
第一方面，本发明实施例提供了一种基于多传感器融合的定位方法，包括以下步骤：
获取姿态初始值，并对所述姿态初始值进行校准；
通过所述惯性运动传感器采集惯性导航数据，并基于所述惯性导航数据进行惯性解算；
通过所述惯性运动传感器的安装信息、所述卫星定位模组以及所述移动通信定位模块分别获取约束条件、卫星定位数据以及蜂窝基站定位数据；
构建卡尔曼滤波系统，基于所述惯性导航数据构建所述卡尔曼滤波系统的状态模型，结合所述约束条件、所述卫星定位数据以及所述蜂窝基站定位数据构建所述卡尔曼滤波系统的观测模型，对惯性解算结果进行修正，获取定位结果。
根据本发明的实施例，至少具有如下技术效果：
通过惯性导航数据构建状态模型，结合约束条件、卫星定位数据以及蜂窝基站定位数据构建观测模型，以前述状态模型和观测模型组成的卡尔曼滤波系统，能够降低观测数据中噪声和干扰的影响，提高定位精度；通过结合约束条件、卫星定位数据以及蜂窝基站定位数据共同对惯性解算结果进行修正，能够提高定位的精度和稳定性，适应更多的应用场景。
根据本发明的一些实施例，所述获取姿态初始值，并对所述姿态初始值进行校准，包括步骤：通过前后两帧卫星定位数据获取航向角；进行初始校准和航向角赋值，获取所述姿态初始值；通过惯性导航数据对所述姿态初始值进行补偿。
根据本发明的一些实施例，利用高斯牛顿法或四元数法对所述姿态初始值进行补偿。
根据本发明的一些实施例，所述通过所述惯性运动传感器的安装信息获取所述约束条件，包括步骤：获取所述惯性运动传感器的安装对象和安装位置，若所述惯性运动传感器安装于动物双足，则构建零速约束条件和距离阈值约束条件，若所述惯性运动传感器安装于车载对象，则构建零速约束条件或运动学约束条件。
根据本发明的一些实施例，所述卫星定位数据包括时间戳、经度纬度高度以及相应的位置置信度，其中，所述位置置信度为卫星钟差和接收机钟差得到的定位标准差值。
根据本发明的一些实施例，在结合所述蜂窝基站定位数据构建所述观测模型之前，引入用于判断是否进行惯性解算结果修正的位置差阈值位置差阈值。
根据本发明的一些实施例，所述卡尔曼滤波系统为闭环卡尔曼滤波系统。
第二方面，本发明实施例提供了一种基于多传感器融合的定位装置，包括：
数据获取模块，用于获取惯性导航数据、卫星定位数据以及蜂窝基站定位数据；
主控模块，包括姿态值获取单元、惯性解算单元、修正单元以及定位结果获取单元；所述姿态值获取单元用于获取姿态初始值，并对所述姿态初始值进行校准；所述惯性解算单元用于基于所述惯性导航数据进行惯性解算；所述修正单元用于基于约束条件、所述卫星定位数据以及所述蜂窝基站定位数据对惯性解算结果进行修正；所述定位结果获取单元用于获取定位结果。
根据本发明的一些实施例，还包括：电源模块，用于向其他模块进行供电；通信模块，用于对接所述主控模块与远程服务端，搭建通信链路；人机交互模块，用于在本地或远程实时查看行进轨迹。
第三方面，本发明实施例提供了一种基于多传感器融合的定位电子设备，包括：
存储器，用于存储计算机程序；
处理器，用于执行所述计算机程序时实现前述提供的一种基于多传感器融合的定位方法。
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
图1为本发明实施例提供的一种基于多传感器融合的定位方法的流程示意图；
图2为本发明实施例提供的一种基于多传感器融合的定位方法的具体实施示意图；
图3为本发明实施例提供的一种基于多传感器融合的定位方法的定位终端信息传输示意图；
图4为本发明实施例提供的一种基于多传感器融合的定位装置结构示意图；
图5为本发明实施例提供的计算机设备的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行完整地描述,显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
非另有定义，本发明所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本发明中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本发明。
实施例1
参照图1与图2，本发明实施例提供一种基于多传感器融合的定位方法，包括以下步骤：
S110：获取姿态初始值，并对所述姿态初始值进行校准；
S120：通过所述惯性运动传感器采集惯性导航数据，并基于所述惯性导航数据进行惯性解算；
S130：通过所述惯性运动传感器的安装信息、所述卫星定位模组以及所述移动通信定位模块分别获取约束条件、卫星定位数据以及蜂窝基站定位数据；
S140：构建卡尔曼滤波系统，基于所述惯性导航数据构建所述卡尔曼滤波系统的状态模型，结合所述约束条件、所述卫星定位数据以及所述蜂窝基站定位数据构建所述卡尔曼滤波系统的观测模型，对惯性解算结果进行修正，获取定位结果。
惯性导航是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯导的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。
而姿态初始值是惯性导航中首先要获取的一个数值，在本实施例中，通过卫星定位数据获取航向角，和通过惯性解算获取姿态数据，基于上述两者进行初始对准与航向角赋值，得到姿态初始值，再进行校准。
在本实施例中，惯性运动传感器在不同的安装对象、不同的安装位置上，它的状态和运动轨迹是不一样的。例如人，在不借助交通工具的情况下，移动速度必然不会超过40千米每小时，那么安装在人身上的惯性运动传感器就可以设置约束条件，速度不高于40千米每小时；例如在空中的客机，通常情况下它是不能静止不动的，那么安装在客机上的惯性运动传感器就可以设置为一段时间内定位不能在同一个点。如果惯性解算的结果不满足约束条件，那就赋予这些不合理的结果较低的权值。
由于惯性导航的原理是通过测量载体在惯性参考系的加速度，将它对时间进行积分，如果长时间进行惯性导航，误差会随着积分越来越大，这时候需要借助其他定位方式来辅助惯性导航定位。
在本实施例中，选择基站定位和卫星导航系统定位。基站定位是根据看到的通信铁塔为基准进行的定位。任何一部手机，只要在信号覆盖的地方，随时随地都会接收到几个信号塔的信号，根据信号方向、强弱、信号传递时间等要素，可以交会得出手机的概略位置。显然的，上述信号塔越密集，交会的位置就越准确。目前在发达地区信号塔较密集的地方，定位精度已经可达百米级以下。卫星导航系统定位是手机接收卫星信号通过交付计算进行的定位，手机GPS接收器根据多颗卫星信号的不同方向、信号到达时间，交会计算出自己当前位置的经纬度和海拔高度。
单独使用卫星定位需要着重处理时间同步误差、电离层和对流层延时修正、多普勒频移等影响；而无线通信易受多径效应、NLOS(非视距传播)误差影响，导致此类通信定位方式在桥梁、隧道、树丛、工地、地下车库等区域应用受限，引起由信号阻挡导致的距离测量误差，进而影响定位精度。本发明实施例提供一种基于多传感器融合的定位方法，利用惯性系统不依赖于外部通信，不需要外部设备辅助，响应迅速的特点，提高定位对象在卫星拒止环境下的短时短距离定位可靠性，在应用过程中，针对特定安装对象利用可用的运动学约束与状态约束，修正其惯导积分定位误差与航向角误差，并实时采集卫星定位信息与蜂窝基站定位信息，使系统设备满足长时间长距离定位需求。该一种基于多传感器融合方法，通过惯性导航数据构建状态模型，结合约束条件、卫星定位数据以及蜂窝基站定位数据构建观测模型，以前述状态模型和观测模型组成的卡尔曼滤波系统，能够降低观测数据中噪声和干扰的影响，提高定位精度；通过结合约束条件、卫星定位数据以及蜂窝基站定位数据共同对惯性解算结果进行修正，能够提高定位的精度和稳定性，适应更多的应用场景。
实施例2
本实施例以前述实施例1为基础，进一步细化流程，包括：
在本实施例中，惯性运动传感器、卫星定位模块、以及移动通信定位模块采集数据方式具体包括：
依照
Figure BDA0003114081680000071
数据格式，以100Hz频率采集惯性运动传感器信息，其中，TI表示数据采集时间戳，IDI表示数据标识，AccI表示加速度计三轴输出，GyrI表示陀螺仪三轴输出。
依照(TI,IDI,Sol_Status,Pos,Pos_Std,Sat_num,Sol_Age)数据格式以1Hz频率获取卫星定位请求信息，其中TI表示卫星数据采集时间戳，IDI表示数据标识，Sol_Status表示定位解精度及状态，Pos表示以经度纬度高度为表示方式的坐标值，Pos_Std表示位置精度，Sat_Num表示采集时卫星接收时探测卫星数量，Sol_Age表示解算时间。作为本实施例中的一个优选项，采用SPP(标准单点定位)方式通过事先对导航数据文件解算得到卫星定位结果。其中信号完好性、位置置信度通过对伪距以及位置测量误差模型得到。
基站定位请求信息中包括数据获取时间戳、经纬度数据、获取基站数量以及信号强度信息。可选的，数据采集周期设置为10s。
在本实施例中，获取姿态初始值，并对所述姿态初始值进行校准包括以下实施方法：
定位与航向初始化；
基站定位冷启动响应速度快，根据基站定位结果给定初始位置范围，主控模块等待卫星定位模块启动并接收信号以及进行定位解，航向初始化方法为：
(1)针对前后两帧单点定位解计算出航向角。航向角方向定义应与导航坐标系定义方向一致。可选的，这里航向角的确定可以采用球面正弦方法或者极坐标法，本发明实施例中，航向角定义为“北偏西”为角度正方向。
(2)当设备开机并且主控制器接收惯性传感器数据后，系统立即进行惯性解算输出姿态数据，此时航向角初始值为0度，当完整获得两帧卫星定位数据之后，将航向角计算结果直接赋值为纯惯性解算的初始航向，并以此时姿态作为此时刻后惯性解算的姿态。
惯性姿态解算的校准；
对于惯性姿态解算的校准，可选的，依据惯性运动模块安装位置高动态运动情况，可采取两种方式进行姿态修正：
(1)高斯牛顿法迭代计算姿态量；
首先，由加速度计测量模型
Figure BDA0003114081680000081
构建使用高斯牛顿法对应状态向量的量测模型：
Figure BDA0003114081680000082
其中，facc,k为加速度计读取值，δacc为加速度计测量误差量，
Figure BDA0003114081680000083
为从导航系到载体系四元数姿态表示，
Figure BDA0003114081680000091
为导航坐标系下速度值，g为导航系下重力矢量。然后，以
Figure BDA0003114081680000092
作为状态向量构建非线性方程针对姿态进行高斯牛顿迭代估计，迭代初值为设备初始静止时刻加速度计调平得到的倾斜角姿态值，并设置一次迭代总时间不超过姿态解算周期。
(2)构建误差状态卡尔曼滤波(ESKF)补偿姿态解算误差；
利用陀螺仪积分得到的姿态阵得到载体系下的重力矢量的估计值
Figure BDA0003114081680000093
利用加速度计得到的姿态同样得到载体系下的重力矢量的估计值
Figure BDA0003114081680000094
利用两种估计值矢量构建姿态误差值作为所述设计的误差卡尔曼滤波的观测量，最终由此种误差卡尔曼滤波实时修正姿态估计。此姿态结果用来实时计算后续惯性积分得到速度和位置积分结果。
构建闭环误差状态卡尔曼滤波方式如下：
该误差状态卡尔曼滤波状态向量设置为：
Figure BDA0003114081680000095
状态X中各符号分量依次为失准角(1-3)、速度误差(4-6)、位置误差(7-9)、陀螺漂移(10-12)、加速度计偏值(13-15)、IMU/GNSS不同步时差(16)，其中，括号中的数字代表维度。作为优选，至少考虑前15维状态向量作为基本设置，可选的，可依据实际系统所需精度情况考虑加入其他状态量。其中位置误差定义为
Figure BDA0003114081680000096
即将纬度误差、经度误差、高度误差作为位置误差各项。
线性系统状态方程为
Figure BDA0003114081680000097
由此设计的卡尔曼滤波得到系统矩阵为：
Figure BDA0003114081680000098
系统噪声分布矩阵与系统噪声向量为：
Figure BDA0003114081680000101
这里，系统矩阵Ft中F分块矩阵表示对应状态量之间的微分，
Figure BDA0003114081680000102
表示从载体系转换到导航系的坐标转换矩阵，
Figure BDA0003114081680000103
表示陀螺仪角速率测量白噪声，
Figure BDA0003114081680000104
表示加速度计比力测量白噪声。
在本实施例中，惯性解算过程依据前述组合滤波系统设计，将惯性解算作为组合导航的关键子系统，其中位置信息采用经度、纬度、高度的度量格式进行更新。
参照图3，本发明实施例包括可根据预设的定位目标以及安装对象进行实施，安装情况分为两种情况：
(1)安装于人体双足或类似运动情况的定位对象；
本实施例中，针对此类安装对象，手动调整设备安装对应选项，针对人体或宠物等躯干运动利用安装于双足的惯性模块，进行零速修正与距离阈值约束。
可选的，步行过程中的相对静止状态可以描述为一个假设检验问题，检验统计量可以设为：
Figure BDA0003114081680000105
其中，N为一段时间窗采样数，n为时间窗起始采样点，facc,k为此历元加速度计输出，
Figure BDA0003114081680000106
为时间窗N内加速度计输出均值，γacc为统计量阈值，用于静止判断。本发明实施例中，k+n为当前历元。
当判断满足静止条件时，进行零速更新：
Zzupt,k＝[0-Vn]＝HzuptXk|k-1+νzupt,k
Hzupt＝[03×3 I3×3 03×3 03×7]
Zzupt观测向量，Vn为经计算得到的速度输出值，Hzupt为量测矩阵，ν为量测噪声向量。
然后以此构建误差状态卡尔曼滤波的观测模型对滤波预测过程进行修正解算。
同样的，针对所述距离阈值约束，需要满足：
Figure BDA0003114081680000111
其中，||ΔLk||2和dk代表双足距离，
Figure BDA0003114081680000112
代表第一个惯性运动模块解算得到的位置结果，
Figure BDA0003114081680000113
代表另一个惯性运动模块解算得到的位置结果。γPos为距离阈值。
则针对每一次惯性积分，当判断双模块位置距离大于阈值时，进行位置约束，此时对于第一个惯性运动模块：
Figure BDA0003114081680000114
对于第二个惯性运动模块：
Figure BDA0003114081680000115
Figure BDA0003114081680000116
为对应惯性模块的系统观测向量，
Figure BDA0003114081680000117
为对应模块的当前位置估计值。对应此观测的量测矩阵为：
HPos＝[03×3 03×3 I3×3 03×7]
然后以此构建误差状态卡尔曼滤波的观测模型对滤波预测过程进行修正解算。
(2)安装于物流运输车载对象；
可选的，在车上安装一个惯性运动模块，运用两种运动约束：
第一种如所述采用静止检测和零速修正方式修正惯性积分误差；
第二种为在车辆行驶过程中，将车体行进方向侧向及天向速度为零作为观测值修正惯性运动误差。
接收卫星定位数据时，监测GNSS信号完好性与置信度信息。将此置信度信息作为卫星精度信息通过构造统计量加入观测噪声，对惯性积分得到的位置进行修正，进行滤波修正解算。
当接收到卫星信号时，观测量为：
Figure BDA0003114081680000121
Figure BDA0003114081680000122
其中ZGNSS、ZGNSS、νGNSS为进行卫星位置更新时的观测量、量测矩阵与卫星接收机位置测量白噪声。在本实例中，将GNSS信号完好性作为指标，构建GNSS信号精度标准差σGNSS，加入观测噪声中，即满足当GNSS信号较弱时，νGNSS较大，即：
νGNSS,new＝νGNSS,old+σGNSS
则通过此指标计算量测噪声协方差矩阵，调节卡尔曼滤波增益。然后以此构建误差状态卡尔曼滤波的观测模型对滤波预测过程进行修正解算。
蜂窝基站定位数据接收与融合；
实时接收基站定位数据，依据通信信号强弱信息进行观测协方差矩阵自适应滤波更新。
优选的，每10s获得由通信运营商提供的位置信息，将通过由此位置信息估计的位置观测噪声协方差与预测的估计噪声协方差理论值进行对比，构建统计量TCell,Pos：
Figure BDA0003114081680000131
其中,Hcell、Vcell为进行蜂窝基站位置更新时量测矩阵位置测量白噪声。Rk为当前帧观测噪声协方差矩阵。Pk|k-1为当前帧得到的系统误差协方差矩阵预测值。设计针对统计量TCell,Pos的阈值γCell,Pos，N为设置的数据量滑动窗，表示选择最近N个历元估计量进行计算，k为当前帧历元，tr[·]表示矩阵的迹。
在本实施例中，可选的，当|TCell,Pos|≤γCell,Pos时，则进行针对基站定位的自适应卡尔曼滤波融合。融合时使用所述自适应卡尔曼滤波方法，仅针对蜂窝基站的定位量测噪声协方差进行估计。
此时
Figure BDA0003114081680000132
以及HCell,k＝[03×3 03×3 I3×3 03×6 0]；
其中ZCell为进行蜂窝基站位置更新的观测矩阵。
测量新息为
Figure BDA0003114081680000133
测量噪声协方差估计为
Figure BDA0003114081680000134
其中，Pk|k-1为误差协方差矩阵，dk-1为权值。由此针对解算位置进行修正。
实施例3
本发明第三实施例，如图4所示，一种基于多传感器融合的定位装置，包括：数据获取模块，用于获取惯性导航数据、卫星定位数据以及蜂窝基站定位数据；主控模块，包括姿态值获取单元、惯性解算单元、修正单元以及定位结果获取单元；所述姿态值获取单元用于获取姿态初始值，并对所述姿态初始值进行校准；所述惯性解算单元用于基于所述惯性导航数据进行惯性解算；所述修正单元用于基于约束条件、所述卫星定位数据以及所述蜂窝基站定位数据对惯性解算结果进行修正；所述定位结果获取单元用于获取定位结果。
在本实施例中，数据接收频率和数据同步通过主控模块来处理，主控模块设置在装备内部，执行数据接收、应答、同步、处理工作，主控模块包含至少一个控制器，该控制器可以为中央处理器或微控制器等。
在本实施例中，数据采集模块与三种传感器通信连接，用于采集惯性运动信息与GNSS和移动通信定位的位置信息，其中GNSS和移动通信模块安装于定位目标载体上不受信号阻挡位置即可，惯性运动模块需安装至定位对象相较具有明显运动特征的部位。
根据本发明的一些实施例，一种基于多传感器融合的装置，还包括：电源模块，用于向其他模块进行供电；通信模块，用于对接所述主控模块与远程服务端，搭建通信链路；人机交互模块，用于在本地或远程实时查看行进轨迹，包括但不限于开关机以及启动和关闭监测任务、手动远程连接、远程参数设置、监测报警任务，例如监测到跟踪对象偏离预定轨道或预定范围等。
本发明第四实施例，如图5所示，基于同一发明构思，本发明实施例提供了一种计算机设备401，包括：
至少一个处理器420；以及
存储器410，存储器410存储有可在处理器上运行的计算机程序411，处理器420执行程序时执行如上的任一种基于多传感器融合的定位方法的步骤。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (5)

1.一种基于多传感器融合的定位方法，所述传感器包括惯性运动传感器、卫星定位模组以及移动通信定位模块，其特征在于，包括以下步骤：
获取姿态初始值，并对所述姿态初始值进行校准，包括步骤：通过前后两帧卫星定位数据获取航向角；进行初始校准和航向角赋值，获取所述姿态初始值；利用高斯牛顿法或四元数法通过惯性导航数据对所述姿态初始值进行补偿；其中，通过如下方式利用高斯牛顿法通过惯性导航数据对所述姿态初始值进行补偿：
通过加速度计测量模型构建使用高斯牛顿法对应状态向量的量测模型：
其中，为加速度计读取值，为加速度计测量误差量，为从导航系到载体系四元数姿态表示，为导航坐标系下速度值，为导航系下重力矢量；
将作为状态向量构建非线性方程针对姿态进行高斯牛顿迭代估计，迭代初值为设备初始静止时刻加速度计调平得到的倾斜角姿态值；
通过如下方式利用四元数法通过惯性导航数据对所述姿态初始值进行补偿：
利用陀螺仪积分得到的姿态阵得到载体系下的重力矢量的估计值；
利用加速度计得到的姿态同样得到载体系下的重力矢量的估计值；
利用两种估计值矢量构建姿态误差值作为设计的误差卡尔曼滤波的观测量，最终由此种误差卡尔曼滤波实时修正姿态估计；其中，误差状态卡尔曼滤波状态向量设置为：
状态中各符号分量依次为失准角、速度误差、位置误差、陀螺漂移、加速度计偏值、IMU/GNSS不同步时差，其中，状态中第一维度至第三维度表示所述失准角，状态中第四维度至第六维度表示所述速度误差，状态中第七维度至第九维度表示所述位置误差，状态中第十维度至第十二维度表示所述陀螺漂移，状态中第十三维度至第十五维度表示所述加速度计偏值，状态中第十六维度表示所述IMU/GNSS不同步时差；
通过所述惯性运动传感器采集惯性导航数据，并基于所述惯性导航数据进行惯性解算；
通过所述惯性运动传感器的安装信息、所述卫星定位模组以及所述移动通信定位模块分别获取约束条件、卫星定位数据以及蜂窝基站定位数据；其中，安装信息包括所述惯性运动传感器的安装对象和安装位置，若所述惯性运动传感器安装于人体双足，则构建零速约束条件和距离阈值约束条件，若所述惯性运动传感器安装于车载对象，则构建零速约束条件或运动学约束条件；
构建卡尔曼滤波系统，基于所述惯性导航数据构建所述卡尔曼滤波系统的状态模型，结合所述约束条件、所述卫星定位数据以及所述蜂窝基站定位数据构建所述卡尔曼滤波系统的观测模型，对惯性解算结果进行修正，获取定位结果；在结合所述蜂窝基站定位数据构建所述观测模型之前，引入用于判断是否进行惯性解算结果修正的位置差阈值；所述卫星定位数据包括时间戳、经度纬度高度以及相应的位置置信度，其中，所述位置置信度为卫星钟差和接收机钟差得到的定位标准差值。
2.根据权利要求1所述的一种基于多传感器融合的定位方法，其特征在于：所述卡尔曼滤波系统为闭环卡尔曼滤波系统。
3.一种基于多传感器融合的定位装置，其特征在于，包括：
数据获取模块，用于获取惯性导航数据、卫星定位数据以及蜂窝基站定位数据；
主控模块，包括姿态值获取单元、惯性解算单元、修正单元以及定位结果获取单元；所述姿态值获取单元用于获取姿态初始值，并对所述姿态初始值进行校准，包括步骤：通过前后两帧卫星定位数据获取航向角；进行初始校准和航向角赋值，获取所述姿态初始值；利用高斯牛顿法或四元数法通过惯性导航数据对所述姿态初始值进行补偿；其中，通过如下方式利用高斯牛顿法通过惯性导航数据对所述姿态初始值进行补偿：
通过加速度计测量模型构建使用高斯牛顿法对应状态向量的量测模型：
其中，为加速度计读取值，为加速度计测量误差量，为从导航系到载体系四元数姿态表示，为导航坐标系下速度值，为导航系下重力矢量；
将作为状态向量构建非线性方程针对姿态进行高斯牛顿迭代估计，迭代初值为设备初始静止时刻加速度计调平得到的倾斜角姿态值；
通过如下方式利用四元数法通过惯性导航数据对所述姿态初始值进行补偿：
利用陀螺仪积分得到的姿态阵得到载体系下的重力矢量的估计值；
利用加速度计得到的姿态同样得到载体系下的重力矢量的估计值；
利用两种估计值矢量构建姿态误差值作为设计的误差卡尔曼滤波的观测量，最终由此种误差卡尔曼滤波实时修正姿态估计；其中，误差状态卡尔曼滤波状态向量设置为：
状态中各符号分量依次为失准角、速度误差、位置误差、陀螺漂移、加速度计偏值、IMU/GNSS不同步时差，其中，状态中第一维度至第三维度表示所述失准角，状态中第四维度至第六维度表示所述速度误差，状态中第七维度至第九维度表示所述位置误差，状态中第十维度至第十二维度表示所述陀螺漂移，状态中第十三维度至第十五维度表示所述加速度计偏值，状态中第十六维度表示所述IMU/GNSS不同步时差；所述惯性解算单元用于基于所述惯性导航数据进行惯性解算；所述修正单元用于基于惯性运动传感器的安装信息、约束条件、所述卫星定位数据以及所述蜂窝基站定位数据对惯性解算结果进行修正；其中，安装信息包括所述惯性运动传感器的安装对象和安装位置，若所述惯性运动传感器安装于人体双足，则构建零速约束条件和距离阈值约束条件，若所述惯性运动传感器安装于车载对象，则构建零速约束条件或运动学约束条件；所述定位结果获取单元用于获取定位结果；
卡尔曼滤波系统构建模块，用于基于所述惯性导航数据构建所述卡尔曼滤波系统的状态模型，结合所述约束条件、所述卫星定位数据以及所述蜂窝基站定位数据构建所述卡尔曼滤波系统的观测模型，对惯性解算结果进行修正，获取定位结果；在结合所述蜂窝基站定位数据构建所述观测模型之前，引入用于判断是否进行惯性解算结果修正的位置差阈值；所述卫星定位数据包括时间戳、经度纬度高度以及相应的位置置信度，其中，所述位置置信度为卫星钟差和接收机钟差得到的定位标准差值。
4.根据权利要求3所述的一种基于多传感器融合的定位装置，其特征在于，还包括：电源模块，用于向其他模块进行供电；
通信模块，用于对接所述主控模块与远程服务端，搭建通信链路；
人机交互模块，用于在本地或远程实时查看行进轨迹。
5.一种电子设备，其特征在于，包括：
存储器，用于存储计算机程序；
处理器，用于执行所述计算机程序时实现如权利要求1至2任一所述的一种基于多传感器融合的定位方法。
CN202110658126.8A 2021-06-15 2021-06-15 一种基于多传感器融合的定位方法、装置与电子设备 Active CN113419265B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110658126.8A CN113419265B (zh) 2021-06-15 2021-06-15 一种基于多传感器融合的定位方法、装置与电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110658126.8A CN113419265B (zh) 2021-06-15 2021-06-15 一种基于多传感器融合的定位方法、装置与电子设备

Publications (2)

Publication Number Publication Date
CN113419265A CN113419265A (zh) 2021-09-21
CN113419265B true CN113419265B (zh) 2023-04-14

Family

ID=77788438

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110658126.8A Active CN113419265B (zh) 2021-06-15 2021-06-15 一种基于多传感器融合的定位方法、装置与电子设备

Country Status (1)

Country Link
CN (1) CN113419265B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114001706B (zh) * 2021-12-29 2022-04-29 阿里巴巴达摩院(杭州)科技有限公司 航向角估计方法、装置、电子设备和存储介质

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007064853A (ja) * 2005-08-31 2007-03-15 Hitachi Ltd 複合測位による移動体の測位制御装置、システムおよびプログラム
CN108106635A (zh) * 2017-12-15 2018-06-01 中国船舶重工集团公司第七0七研究所 惯性卫导组合导航系统的长航时抗干扰姿态航向校准方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106324645A (zh) * 2016-08-19 2017-01-11 付寅飞 一种基于惯性导航和卫星差分定位的车辆精准定位方法
CN106595653A (zh) * 2016-12-08 2017-04-26 南京航空航天大学 一种穿戴式行人自主导航系统及其导航方法
CN107655476B (zh) * 2017-08-21 2021-04-20 南京航空航天大学 基于多信息融合补偿的行人高精度足部导航方法
CN109186597B (zh) * 2018-08-31 2020-09-22 武汉大学 一种基于双mems-imu的室内轮式机器人的定位方法
CN109212573B (zh) * 2018-10-15 2020-02-07 东南大学 一种城市峡谷环境下用于测绘车辆的定位系统及方法
CN111947658B (zh) * 2020-06-30 2022-12-27 北京航天控制仪器研究所 一种通信辅助定位的低成本自主导航装置及导航方法
CN112066980B (zh) * 2020-08-31 2022-09-27 南京航空航天大学 一种基于人体四节点运动约束的行人导航定位方法
CN112946713B (zh) * 2021-02-05 2023-09-15 北京森云科技有限公司 一种室内定位方法、设备及系统

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007064853A (ja) * 2005-08-31 2007-03-15 Hitachi Ltd 複合測位による移動体の測位制御装置、システムおよびプログラム
CN108106635A (zh) * 2017-12-15 2018-06-01 中国船舶重工集团公司第七0七研究所 惯性卫导组合导航系统的长航时抗干扰姿态航向校准方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Nannan Wang 等.A Dynamic Weighting Fusion Positioning Method Based on Opportunity Correction for INS and UWB.《2019 International Conference on Cyber-Enabled Distributed Computing and Knowledge Discovery》.2019,317-321. *
任仁凯 等.信息双向融合的机器人协同导航方法.《传感器与微系统》.2016,第35卷(第8期),40-43页. *

Also Published As

Publication number Publication date
CN113419265A (zh) 2021-09-21

Similar Documents

Publication Publication Date Title
US11598638B2 (en) Methods of attitude and misalignment estimation for constraint free portable navigation
CN106255065B (zh) 智能手机室内外无缝定位系统及其方法
Fang et al. Predictive iterated Kalman filter for INS/GPS integration and its application to SAR motion compensation
CN101382431B (zh) 定位系统及其方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN108344415B (zh) 一种组合导航信息融合方法
CN111077549B (zh) 位置数据修正方法、装置和计算机可读存储介质
US10365109B2 (en) Travel distance estimation device
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN112505737B (zh) 一种gnss/ins组合导航方法
US10732299B2 (en) Velocity estimation device
GB2475147A (en) Altitude measurement and user context detection for navigation systems
EP2560024B1 (en) Satellite-based positioning method
CN113074732A (zh) 一种室内外无缝定位系统及其定位方法
CN113295174B (zh) 一种车道级定位的方法、相关装置、设备以及存储介质
Meguro et al. Low-cost lane-level positioning in urban area using optimized long time series GNSS and IMU data
Zhang et al. Integration of INS and un-differenced GPS measurements for precise position and attitude determination
Georgy Advanced nonlinear techniques for low cost land vehicle navigation
CN113419265B (zh) 一种基于多传感器融合的定位方法、装置与电子设备
Park et al. Implementation of vehicle navigation system using GNSS, INS, odometer and barometer
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
CN116558512A (zh) 一种基于因子图的gnss与车载传感器融合定位方法及系统
CN114812554A (zh) 基于滤波的多源融合机器人室内绝对定位方法
KR20140142610A (ko) 위치 측정 장치 및 방법
CN113031040A (zh) 一种用于机场地服车辆的定位方法及系统

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
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20230322

Address after: 410000 room 1913, building 1, microchip technology park, No. 217, Jianshan Road, Changsha high tech Development Zone, Changsha, Hunan Province

Applicant after: HUNAN BDS MICRO CHIPSET INDUSTRY DEVELOPMENT CO.,LTD.

Address before: Room 2203, building 2, Xincheng Science Park, 588 Yuelu West Avenue, Changsha hi tech Development Zone, Changsha City, Hunan Province, 410000

Applicant before: Hunan Beidou microchip Data Technology Co.,Ltd.

GR01 Patent grant
GR01 Patent grant