CN113639722A - 连续激光扫描配准辅助惯性定位定姿方法 - Google Patents

连续激光扫描配准辅助惯性定位定姿方法 Download PDF

Info

Publication number
CN113639722A
CN113639722A CN202111208988.7A CN202111208988A CN113639722A CN 113639722 A CN113639722 A CN 113639722A CN 202111208988 A CN202111208988 A CN 202111208988A CN 113639722 A CN113639722 A CN 113639722A
Authority
CN
China
Prior art keywords
inertial
error
data
inertial navigation
registration
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
CN202111208988.7A
Other languages
English (en)
Other versions
CN113639722B (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.)
Shenzhen University
Original Assignee
Shenzhen 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 Shenzhen University filed Critical Shenzhen University
Priority to CN202111208988.7A priority Critical patent/CN113639722B/zh
Publication of CN113639722A publication Critical patent/CN113639722A/zh
Application granted granted Critical
Publication of CN113639722B publication Critical patent/CN113639722B/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
    • G01C15/00—Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00
    • G01C15/002—Active optical surveying means
    • 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
    • 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/20—Instruments for performing navigational calculations

Landscapes

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

Abstract

本发明公开了连续激光扫描配准辅助惯性定位定姿方法，其中，所述方法包括获得惯性运动数据和环境扫描数据；将所述惯性运动数据带入发明的惯性导航短时间误差模型推算，获得惯性导航相对位置误差数据；通过对所述惯性运动数据、所述惯性导航相对位置误差数据和所述环境扫描数据进行融合后，获得惯性导航误差；通过卡尔曼滤波计算对所述惯性导航误差进行修正，获得高精度位姿数据。本申请公开的连续激光扫描配准辅助惯性定位定姿方法通过激光辅助配合惯性导航技术减缓系统误差的发散速度，实现持续激光扫描辅助惯导持续定位，获得高精度位姿数据，对载体进行高精度定位定姿，弥补了传统定位系统的测量精度不足的问题。

Description

连续激光扫描配准辅助惯性定位定姿方法
技术领域
本发明涉及三维测绘技术领域，特别是涉及连续激光扫描配准辅助惯性定位定姿方法。
背景技术
在测绘领域，动态精密工程测量是一种通过移动测量平台进行快速三维测量的前沿测绘技术，近些年来在公路、铁路等多个行业推广应用，极大地提高了三维空间数据的采集效率。动态精密工程测量是指在测量过程中设备不断变化位置进行测量的测量方法，运动的设备按照预定的扫描时间间隔自动采集数据进行观测，从而可以实时地确定采样点的空间位置。根据动态精密工程测量的特点，目前人们更趋向于在封闭或半封闭的人造场景中使用这种动态测量的方法，如大型建筑内部空间，复杂工业厂房，地铁隧道，地面立体交通结构，城市峡谷等，在这些环境中的施工、建造都需要高精度三维空间数据的支撑，正是在这些场所、设施的运营和管理中，人们需要用到动态精密工程测量技术来实现精确测量；另外，随着人类活动足迹的进一步拓展，很多复杂自然场景如森林、洞穴、外太空探索活动也需要大量高效的三维测量技术。
定位定姿是动态精密工程测量的关键支撑技术，其为环境测量数据提供统一的空间位置、姿态参考，直接影响最终数据成果质量。动态精密工程测量常用的定位定姿方法是卫星导航定位（GNSS，Global Navigation System）和惯性导航定位(INS，InertialNavigation System)的组合定位方法，这种定位方法对应的系统简称为POS系统（Positionand Orientation System）。POS系统中GNSS和INS之间具有很强的互补的关系。惯性导航系统利用高频测量的角速度和加速度积分得到姿态、位置和速度，但单纯惯性导航定位由于其积分定位原理，定位误差会随时间不断累积，直至发散。GNSS可以提供稳定的绝对位置、速度等观测值，修正惯性导航系统的误差。因此，POS系统可以实现组合导航系统的持续定位定姿，在开放场景中定位精度可达厘米级。
不过，现有的动态精密工程测量的测量过程中非常依赖GNSS，一旦发生卫星信号丢失则需要重新进行初始化，测绘工作不得不从头开始，测绘数据容易失去连贯性。在无GNSS信号环境或GNSS信号被干扰区域（弱GNSS环境)的动态精密工程测量成为难题，如果人们在封闭或者半封闭的环境中使用这种动态测量的方法，GNSS信号在复杂环境中，容易受到影响，可能产生多路径效应或信号遮挡，使得GNSS的定位误差显著增大甚至出现粗差，不能对惯性导航误差进行有效纠正，导致误差增大或发散，会使得POS系统产生测量精度不足的问题。
因此，现有技术还有待于改进和发展。
发明内容
鉴于上述现有技术的不足，本发明的目的在于提供连续激光扫描配准辅助惯性定位定姿方法，旨在解决定位定姿系统在卫星导航定位信号弱的环境中测量精度不足的问题。
本发明的技术方案如下：
一种连续激光扫描配准辅助惯性定位定姿方法，其中，包括：
获得坐标系中的惯性运动数据和环境扫描数据；
将所述惯性运动数据带入预设的惯性导航短时间误差模型推算，获得惯性导航相对位置误差数据；
通过对所述惯性运动数据、所述惯性导航相对位置误差数据和所述环境扫描数据进行融合后，获得惯性导航误差；
通过卡尔曼滤波计算对所述惯性导航误差进行修正，获得高精度位姿数据。
所述的连续激光扫描配准辅助惯性定位定姿方法，其中，所述通过卡尔曼滤波计算对所述惯性导航误差进行修正，获得高精度位姿数据的步骤之后，还包括：
通过平滑算法对所述高精度位姿数据进行反向平滑计算，获得平滑后的位姿数据。
所述的连续激光扫描配准辅助惯性定位定姿方法，其中，所述惯性运动数据通过惯性导航系统测量，所述环境扫描数据通过激光扫描仪测量；所述惯性导航系统和所述激光扫描仪集成在一起。
所述的连续激光扫描配准辅助惯性定位定姿方法，其中，所述预设的惯性导航短时间误差模型建立在坐标系中，所述坐标系包括载体坐标系、局部水平坐标系和轨迹坐标系；
所述载体坐标系与载体固连，中心为载体中心，x轴指向右向，y轴指向前向，z轴沿铅锤方向指向上；
所述局部水平坐标系中心为载体中心，x轴指向东方向，y轴指向北方向，z轴沿铅锤方向指向上，所述载体坐标系的姿态在所述局部水平坐标系中进行表达；
所述轨迹坐标系的中心为载体中心，坐标轴姿态为导航系统计算得到的姿态，与所述局部水平坐标系存在一个误差角，记为
Figure 105206DEST_PATH_IMAGE002
。
所述的连续激光扫描配准辅助惯性定位定姿方法，其中，所述预设的惯性导航短时间误差模型为：
Figure 287925DEST_PATH_IMAGE003
其中，
Figure 598821DEST_PATH_IMAGE005
为
Figure 134976DEST_PATH_IMAGE006
时刻载体在所述轨迹坐标系的位置误差，
Figure 214927DEST_PATH_IMAGE007
为
Figure 568548DEST_PATH_IMAGE006
时刻所述轨迹坐标系的速度误差，
Figure 976527DEST_PATH_IMAGE008
为测量时刻与
Figure 441006DEST_PATH_IMAGE006
时刻的时间间隔，
Figure 641043DEST_PATH_IMAGE009
为
Figure 775353DEST_PATH_IMAGE006
时刻所述轨迹坐标系因姿态角误差造成的重力分解误差，
Figure 795261DEST_PATH_IMAGE011
为未补偿的加速度计零偏，
Figure 63432DEST_PATH_IMAGE012
为预积分量，
Figure 727762DEST_PATH_IMAGE013
为惯性导航姿态矩阵，表示系统从所述载体坐标系到所述轨迹坐标系的转换关系。
所述的连续激光扫描配准辅助惯性定位定姿方法，其中，所述通过对所述惯性运动数据、所述惯性导航相对位置误差数据和所述环境扫描数据进行融合后，获得惯性导航误差的步骤，具体包括：
利用所述惯性运动数据对所述环境扫描数据进行转换，获得所述轨迹坐标系中的扫描点云的坐标；
对扫描点云进行配准，得到不同时间点扫描点云重叠部分的配准点对的坐标；
融合所述配准点对的坐标和所述惯性导航相对位置误差数据，计算出所述配准点对之间的距离；
根据所述配准点对之间的距离计算配准误差矢量；
通过利用最小二乘法的原理进行估计，融合所述配准误差矢量，计算出惯性导航误差。
所述的连续激光扫描配准辅助惯性定位定姿方法，其中，所述对扫描点云进行配准，得到不同时间点扫描点云重叠部分的配准点对的坐标的步骤，具体包括：
通过最邻近匹配方法或者特征匹配方法构建扫描点云的配准点对。
所述的连续激光扫描配准辅助惯性定位定姿方法，其中，所述通过卡尔曼滤波计算对所述惯性导航误差进行修正，获得高精度位姿数据的步骤，具体包括：
构建所述惯性运动数据的状态向量；通过点云配准估计的所述轨迹坐标系中的速度误差、重力分解误差和加速度零偏误差构建观测值；
构建所述观测值与所述状态向量之间的函数关系，获得测量模型；
在所述测量模型中利用序列扫描配准激光点云估计并修正所述惯性导航误差，以持续激光扫描辅助关系导航系统持续定位，获得高精度位姿数据。
本申请还公开了一种连续激光扫描配准辅助惯性定位定姿系统，其中，包括移动载体、惯性导航装置、激光扫描仪和处理模块，所述惯性导航装置设置于所述移动载体上，用于获得惯性运动数据；所述激光扫描仪设置于所述移动载体上，用于获得环境扫描数据；所述处理模块与所述惯性导航装置和所述激光扫描仪连接，用于收集所述惯性运动数据和所述环境扫描数据，并执行如上任一所述的连续激光扫描配准辅助惯性定位定姿方法。
所述的连续激光扫描配准辅助惯性定位定姿系统，其中，所述系统还包括平滑滤波器。
与现有技术相比，本发明实施例具有以下优点：
本申请公开的连续激光扫描配准辅助惯性定位定姿方法通过预先构建惯性导航短时间误差模型来描述短时间内惯性导航系统内的误差变化规律；在进行测绘的时候同时获取惯性运动数据和环境扫描数据，先将惯性运动数据带入预设的惯性导航短时间误差模型进行推算，获得短时间内的惯性导航相对位置误差数据；然后，通过融合环境扫描数据、惯性运动数据和惯性导航相对位置误差数据，得到短时间内的惯性导航误差，即通过激光获得的环境扫描数据辅助惯性导航技术提高了定位定姿结果的准确度；最后还通过卡尔曼滤波算法修正，得到高精度位姿数据，有效降低了惯性导航系统误差的发散速度，从而可在没有卫星信号辅助的情况下获得高精度的定位定姿数据，弥补了传统定位系统受环境影响，测量精度不足的问题。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明中连续激光扫描配准辅助惯性定位定姿方法的流程图；
图2为本发明中连续激光扫描配准辅助惯性定位定姿系统的结构示意图；
图3为本发明中连续激光扫描配准辅助惯性定位定姿系统的另一结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
参阅图1，本申请公开的一种连续激光扫描配准辅助惯性定位定姿方法，其中，包括：
S100、获得坐标系中的惯性运动数据和环境扫描数据；
S200、将所述惯性运动数据带入预设的惯性导航短时间误差模型推算，获得惯性导航相对位置误差数据；
S300、通过对所述惯性运动数据、所述惯性导航相对位置误差数据和所述环境扫描数据进行融合后，获得惯性导航误差；
S400、通过卡尔曼滤波计算对所述惯性导航误差进行修正，获得高精度位姿数据。
本申请公开的连续激光扫描配准辅助惯性定位定姿方法通过预先构建惯性导航短时间误差模型来描述短时间内惯性导航系统内的误差变化规律；在进行测绘的时候同时获取惯性运动数据和环境扫描数据，先将惯性运动数据带入预设的惯性导航短时间误差模型进行推算，获得短时间内的惯性导航相对位置误差数据；然后，通过融合环境扫描数据、惯性运动数据和惯性导航相对位置误差数据，得到短时间内的惯性导航误差，即通过激光获得的环境扫描数据辅助惯性导航技术提高了定位定姿结果的准确度；最后还通过卡尔曼滤波算法修正，得到高精度位姿数据，有效降低了惯性导航系统误差的发散速度，从而可在没有卫星信号辅助的情况下获得高精度的定位定姿数据，弥补了传统定位系统受环境影响，测量精度不足的问题。
具体的，作为本实施例的一种实现方式，公开了所述预设的惯性导航短时间误差模型是建立在坐标系中的，所述坐标系包括载体坐标系、局部水平坐标系和轨迹坐标系；
所述载体坐标系与载体固连，中心为载体中心，x轴指向右向，y轴指向前向，z轴沿铅锤方向指向上；
所述局部水平坐标系中心为载体中心，x轴指向东方向，y轴指向北方向，z轴沿铅锤方向指向上，所述载体坐标系的姿态在所述局部水平坐标系中进行表达；
所述轨迹坐标系的中心为载体中心，坐标轴姿态为导航系统计算得到的姿态，与所述局部水平坐标系存在一个误差角，记为
Figure 423186DEST_PATH_IMAGE015
。
通过定义坐标系来表示载体的位置，而且在运动过程中通过载体坐标系、局部水平坐标系和轨迹坐标系中的坐标来表达测绘数据，可以达到灵活准确地表达定位定姿数据的效果。
为了方便记载，将载体坐标系记为b系，局部水平坐标系记为n系，轨迹坐标系记为p系。
具体的，作为本实施例的另一种实现方式，公开了所述惯性运动数据通过惯性导航系统测量，所述环境扫描数据通过激光扫描仪测量；所述惯性导航系统和所述激光扫描仪集成在一起。
本实施例公开的连续激光扫描配准辅助惯性定位定姿方法通过环境扫描数据辅助惯性运动数据进行定位定姿的工作，所以将惯性导航系统和激光扫描仪集成在一起可以同时测量这两个数据，便于获得准确的测量结果。
为了刻画纯惯性导航在短时间内的误差发散规律，本发明构建了一种短时间导航误差模型。选取短时间窗口记为
Figure 195970DEST_PATH_IMAGE017
，根据惯性递推原理可知，某时刻
Figure 609109DEST_PATH_IMAGE018
的惯导位置姿态误差来源可分为三部分，即：a)、惯导系统初始时刻
Figure 783738DEST_PATH_IMAGE019
状态误差，包括导航状态误差和器件零偏误差，短时间内可将零偏误差建模为随机常值；b)、惯性传感器测量噪声积分引起的误差；c)、惯导积分递推的时间间隔
Figure 384484DEST_PATH_IMAGE020
。因此，短时间内的惯性递推轨迹误差其可以概括表示为下式：
Figure 254351DEST_PATH_IMAGE021
（1）
其中
Figure 864324DEST_PATH_IMAGE022
为t刻的惯性递推所得轨迹的误差，
Figure 627881DEST_PATH_IMAGE023
分别表示t时刻惯导姿态误差和位置误差，
Figure 540473DEST_PATH_IMAGE024
表示
Figure 22270DEST_PATH_IMAGE019
时刻系统状态误差引起的轨迹误差，
Figure 170354DEST_PATH_IMAGE025
为
Figure 929363DEST_PATH_IMAGE019
时刻的系统状态误差，
Figure 137490DEST_PATH_IMAGE027
为时间间隔，
Figure 841004DEST_PATH_IMAGE028
为随机噪声积分误差。
将
Figure 668146DEST_PATH_IMAGE019
时刻计算机的导航坐标系，记为
Figure 406295DEST_PATH_IMAGE029
系，
Figure 785324DEST_PATH_IMAGE031
系与
Figure 117079DEST_PATH_IMAGE032
系之间存在一个误差角记为
Figure 606966DEST_PATH_IMAGE033
角。在
Figure 74988DEST_PATH_IMAGE034
系中，定位系统位置、姿态初始误差为零。在
Figure 624918DEST_PATH_IMAGE029
系中简化导航方程如下式所示:
Figure 303024DEST_PATH_IMAGE035
(2)
其中
Figure 737547DEST_PATH_IMAGE036
为惯导姿态矩阵，表示系统所在载体坐标系
Figure 184709DEST_PATH_IMAGE038
系到导航坐标系
Figure 639961DEST_PATH_IMAGE040
系的转换关系，
Figure 960957DEST_PATH_IMAGE042
为系统在导航坐标系中的速度，
Figure 792647DEST_PATH_IMAGE043
为惯导在导航坐标系中的位置。
Figure 625474DEST_PATH_IMAGE044
为惯性传感器测量的真实的角速度，
Figure 126993DEST_PATH_IMAGE046
表示向量的反对称矩阵，
Figure 779692DEST_PATH_IMAGE048
为惯性传感器测量的真实的比力。式中的中间变量定义如下：
Figure 680651DEST_PATH_IMAGE049
（3）
其中，
Figure 712192DEST_PATH_IMAGE051
表示姿态从n系到p系的转换关系，
Figure 40406DEST_PATH_IMAGE053
表示姿态从b系到p系的转换关系。
Figure 914821DEST_PATH_IMAGE055
表示计算得到的姿态从b系到n系的转换关系，由于受误差角
Figure 229259DEST_PATH_IMAGE057
的影响，和实际
Figure 771098DEST_PATH_IMAGE059
存在误差
Figure 4634DEST_PATH_IMAGE061
为p系下的重力加速度矢量。
将短时间窗口内陀螺零偏
Figure 241711DEST_PATH_IMAGE063
建模为随机常值，则短时间窗口内p系的姿态误差可表示为:
Figure 218894DEST_PATH_IMAGE064
（4）
将短时间窗口内加速度零偏
Figure 615241DEST_PATH_IMAGE065
建模为常值，则p坐标系中速度和位置可表示为:
Figure 895043DEST_PATH_IMAGE066
（5）
其中
Figure 9630DEST_PATH_IMAGE067
，
Figure 524925DEST_PATH_IMAGE069
积分量，预积分指的是在下一个关键帧到来之前先对惯性测量单元（IMU）数据积分，把关键帧之间的IMU测量值积分成相对运动的约束，避免了因为初始条件变化造成的重复积分，从而极大优化了计算效率。具体计算方法如式（6）所示。
Figure 651144DEST_PATH_IMAGE070
（6）
式中
Figure 492061DEST_PATH_IMAGE072
为惯导的比力测量值，
Figure 828364DEST_PATH_IMAGE073
为加速度计测量噪声，
Figure 22716DEST_PATH_IMAGE074
表示
Figure 128076DEST_PATH_IMAGE076
的时间间隔。
对式（5）进行扰动分析，最终可得短时间惯导相对位置误差的时变模型:
Figure 139894DEST_PATH_IMAGE077
（7）
其中，
Figure 570351DEST_PATH_IMAGE078
为
Figure 958607DEST_PATH_IMAGE079
时刻p系的位置误差，
Figure 918472DEST_PATH_IMAGE080
为
Figure 710979DEST_PATH_IMAGE082
时刻p系的速度误差，
Figure 21875DEST_PATH_IMAGE083
为
Figure 948242DEST_PATH_IMAGE082
时刻p系因姿态角误差造成的重力分解误差，
Figure 637981DEST_PATH_IMAGE084
为未补偿的加速度计零偏，
Figure 257181DEST_PATH_IMAGE086
为预积分量。
从式（7）可得短时间内局部位置误差主要由初始速度误差、水平角误差以及加速度计零偏和噪声造成。具体地，水平姿态角误差造成重力分解误差，引起的位置误差项随时间成二次方增长；速度误差引起的位置误差随时间成线性增长；加速度零偏误差引起的位置误差项随时间成二次方增长。
通过将惯性运动数据带入到上述短时间惯导相对位置误差的时变模型中，得到惯性导航相对位置误差数据之后，具体的，作为本实施例的另一种实现方式，公开了所述步骤S300，具体包括：
S301、利用所述惯性运动数据对所述环境扫描数据进行转换，获得所述轨迹坐标系中的扫描点云的坐标；
S302、对扫描点云进行配准，得到不同时间点扫描点云重叠部分的配准点对的坐标；
S303、融合所述配准点对的坐标和所述惯性导航相对位置误差数据，计算出所述配准点对之间的距离；
S304、根据所述配准点对之间的距离计算配准误差矢量；
S305、通过利用最小二乘法的原理进行估计，融合所述配准误差矢量，计算出惯性导航误差。
具体的，作为本实施例的另一种实现方式，公开了所述步骤S302，具体包括：通过最邻近匹配方法或者特征匹配方法构建扫描点云的配准点对。
通过环境扫描数据与惯性导航相对位置误差数据融合，获得更准确的关系导航误差。具体推算步骤如下：
移动激光扫描是利用定位定姿系统得到的位姿
Figure 789794DEST_PATH_IMAGE087
，对激光扫描仪测量的
Figure 395218DEST_PATH_IMAGE088
进行转换，得到轨迹坐标系中的点云坐标
Figure 64097DEST_PATH_IMAGE090
：
Figure 588619DEST_PATH_IMAGE091
(8)
式中
Figure 749473DEST_PATH_IMAGE093
为计算得到的导航系中的坐标值，
Figure 752064DEST_PATH_IMAGE095
为相应的导航系中的坐标误差。
Figure 541029DEST_PATH_IMAGE097
，
Figure 111819DEST_PATH_IMAGE099
为计算得到的位姿，
Figure 619023DEST_PATH_IMAGE101
为计算得到的载体系中的坐标值，
Figure 425305DEST_PATH_IMAGE102
为载体系中的坐标误差，
Figure 209722DEST_PATH_IMAGE103
表示误差角。根据式(8)可知，移动扫描点云的误差主要由轨迹的误差（位置和姿态误差）引起。一般地，短时间内移动扫描激光点云存在着重叠，可以用来构建相对约束。对于测量型惯导，其在短时间内产生的姿态误差可以忽略不记，因此对于扫描点云进行配准后，得到点云重叠部分的配准点对
Figure 76047DEST_PATH_IMAGE105
，其匹配误差可表示为点对
Figure 70547DEST_PATH_IMAGE106
)的点云坐标
Figure 555887DEST_PATH_IMAGE108
和
Figure 319443DEST_PATH_IMAGE109
的差值，如式（9）所示:
Figure 91090DEST_PATH_IMAGE110
(9)
式（9）表达了点云配准误差与惯导误差之间的函数关系，式中
Figure 451183DEST_PATH_IMAGE112
和
Figure 864847DEST_PATH_IMAGE114
表示测量的点云坐标值，
Figure 482910DEST_PATH_IMAGE116
和
Figure 566404DEST_PATH_IMAGE118
为相应的坐标误差。进一步将式（7）带入式（9），将坐标误差表示为惯导误差
Figure 535497DEST_PATH_IMAGE120
，
Figure 221693DEST_PATH_IMAGE121
，
Figure 835208DEST_PATH_IMAGE122
的函数。通过式（9）可知移动平台的位置误差是点云配准误差的主要来源。为了估计位置误差模型参数，通过最邻近匹配或特征匹配方法构建点云匹配对之后， 配准点对之间的距离可表示为式（10）：
Figure 214237DEST_PATH_IMAGE123
（10）
其中，
Figure 405047DEST_PATH_IMAGE125
表示点云中第
Figure DEST_PATH_IMAGE126
个点，
Figure 301458DEST_PATH_IMAGE127
为点
Figure DEST_PATH_IMAGE128
处的法向量。每个匹配点的权重由点云面片的厚度和激光的测距误差进行确定，如式所示：
Figure DEST_PATH_IMAGE130
（11）
最终的轨迹误差修正参数通过最小化短时间内重叠点云误差进行估计，利用最小二乘原理最小化误差，即待优化目标函数为：
Figure 566218DEST_PATH_IMAGE131
（12）
其中
Figure DEST_PATH_IMAGE132
为配准误差矢量，且
Figure DEST_PATH_IMAGE134
；
Figure DEST_PATH_IMAGE136
为配准约束权值，且
Figure 53831DEST_PATH_IMAGE137
；利用最小二乘方法可以精确计算
Figure 607303DEST_PATH_IMAGE139
参数及其协方差
Figure 635302DEST_PATH_IMAGE141
。
本实施例中通过融合了激光扫描获得的环境扫描数据与惯性导航系统中得到的惯性导航相对位置误差数据，使计算出的误差数据更接近真实误差，提高惯性定位的精度。
具体的，得到短时间惯性误差模型参数后，作为本实施例的另一种实现方式，公开了所述通过步骤S400，具体包括：
S401、构建所述惯性运动数据的状态向量；通过点云配准估计的所述轨迹坐标系中的速度误差、重力分解误差和加速度零偏误差构建观测值；
S402、构建所述观测值与所述状态向量之间的函数关系，获得测量模型；
S403、在所述测量模型中利用序列扫描配准激光点云估计并修正所述惯性导航误差，以持续激光扫描辅助关系导航系统持续定位，获得高精度位姿数据。
通过卡尔曼滤波算法对INS系统的状态误差进行修正，对下一次惯导递推的结果误差进行控制，进而达到提高相对定位精度的目的。本实施例采用卡尔曼滤波算法对惯性导航数据进行处理，卡尔曼滤波是实时移动定位求解当前测量时刻最优定位结果的算法框架，因此需要构建其状态向量、动态方程和测量方程，其中状态向量可以表示为式（13），动态方程采用传统的
Figure 348043DEST_PATH_IMAGE143
角误差方程，本实施例中公开的方法将通过点云配准估计的
Figure 944240DEST_PATH_IMAGE144
系中速度误差
Figure 109643DEST_PATH_IMAGE145
、重力分解误差
Figure 206912DEST_PATH_IMAGE146
以及加速度零偏误差
Figure 381016DEST_PATH_IMAGE147
作为观测值，可表示为式（14）。
Figure 538328DEST_PATH_IMAGE148
（13）
Figure 191027DEST_PATH_IMAGE149
（14）
其中，
Figure 436194DEST_PATH_IMAGE151
表示激光扫描仪的观测值，具体为速度误差
Figure 123527DEST_PATH_IMAGE153
、重力分解误差
Figure 186161DEST_PATH_IMAGE154
以及加速度零偏
Figure 935943DEST_PATH_IMAGE155
，
Figure 375014DEST_PATH_IMAGE156
为测量噪声，
Figure 916854DEST_PATH_IMAGE158
符合高斯分布，即
Figure 25756DEST_PATH_IMAGE160
。
Figure 387467DEST_PATH_IMAGE162
表示噪声的协方差矩阵。
为了实现对状态向量的估计，还需要构建观测值与状态向量之间的函数关系，即测量模型。为了得到测量值与初始时刻扩展卡尔曼滤波状态的之间的关系，构建测量模型。结合式（5）对
Figure 364650DEST_PATH_IMAGE163
进行扰动分析可得：
Figure 636362DEST_PATH_IMAGE164
（15）
其中，
Figure 40799DEST_PATH_IMAGE165
。
进一步的，对
Figure 889806DEST_PATH_IMAGE167
进行扰动分析，可得：
Figure 546047DEST_PATH_IMAGE168
（16）
又有
Figure 796899DEST_PATH_IMAGE169
，使得
Figure 372237DEST_PATH_IMAGE171
的z量恒等于0， 且
Figure DEST_PATH_IMAGE173
角的z量对
Figure DEST_PATH_IMAGE175
的x，y无影响，因此方位角误差是不可观测的。
综合式（15），(16），可以得到连续激光扫描配准辅助惯性定位的测量模型：
Figure DEST_PATH_IMAGE177
（17）
其中：
Figure 787169DEST_PATH_IMAGE178
根据式（17），可以不断利用序列扫描配准激光点云估计并修正纯惯性导航误差，有效减缓整个系统的误差发散速度。
在惯性导航系统中，时间越长，定位定姿的数据就越容易发散，本实施例通过卡尔曼滤波算法对惯性导航误差进行进一步的修正，最终得到的修正后的状态误差发散速度慢，从而获取高精度位姿数据。
具体的，作为本实施例的一种实现方式，公开了所述步骤S400之后，还包括：
S500、通过平滑算法对所述高精度位姿数据进行反向平滑计算，获得平滑后的位姿数据。
在组合导航系统的运动状态估计方法中，滤波是利用当前时刻以及以前时刻的所有量测信息对当前状态进行估计，而平滑算法除了利用滤波所用的量测信息外还利用了当前时刻以后的部分或所有量测信息。平滑算法由于利用了更多的观测信息，使其比滤波算法具有更精确的状态估计效果，能够获得优于滤波的估计精度。本实施例中使用RTS平滑算法（Rauch-Tung-Striebel）进行计算。RTS平滑算法包括前向滤波和后向递推两个部分。其中，前向滤波采用卡尔曼滤波算法，计算并存储每一时刻的状态值和相应的估计误差协方差阵，以及系统的状态转移阵；后向递推则是将前向过程的存储值作为输入量，利用后向递推公式获得最优的平滑估计结果；本实施方式中通过对卡尔曼滤波计算的中间数据，例如滤波状态、滤波状态的协方差矩阵、预测状态的协方差矩阵等等，进行反向RTS平滑，得到平滑后的轨迹，可以进一步提高定位定姿的精度。在一些精密测量应用中本实施例公开的方法具有广大应用前景，特别是在一些无卫星导航信号的环境中。当然了，本实施例公开的连续激光扫描配准辅助惯性定位定姿方法也为室内测图、室内机器人自动驾驶等技术提供了位置和姿态基准。
参阅图2，作为本申请的另一实施例，还公开了一种连续激光扫描配准辅助惯性定位定姿系统，其中，包括移动载体10、惯性导航装置20、激光扫描仪30和处理模块40，所述惯性导航装置20设置于所述移动载体10上，用于获得惯性运动数据；所述激光扫描仪30设置于所述移动载体10上，用于获得环境扫描数据；所述处理模块40与所述惯性导航装置20和所述激光扫描仪30连接，用于收集所述惯性运动数据和所述环境扫描数据，并执行如上任一实施例所述的连续激光扫描配准辅助惯性定位定姿方法。
所述连续激光扫描配准辅助惯性定位定姿系统的工作方法、定位定姿原理在上述实施例中已经详细阐述，在此不再赘述。
参阅图3，具体的，作为本实施例的一种实现方式，公开了所述系统还包括平滑滤波器50。平滑滤波器50用于实现平滑滤波算法，进一步提高连续激光扫描配准辅助惯性定位定姿系统的精度。
综合来说，本申请公开了一种连续激光扫描配准辅助惯性定位定姿方法和系统，其中，所述方法包括：
S100、获得坐标系中的惯性运动数据和环境扫描数据；
S200、将所述惯性运动数据带入预设的惯性导航短时间误差模型推算，获得惯性导航相对位置误差数据；
S300、通过对所述惯性运动数据、所述惯性导航相对位置误差数据和所述环境扫描数据进行融合后，获得惯性导航误差；
S400、通过卡尔曼滤波计算对所述惯性导航误差进行修正，获得高精度位姿数据；
所述系统包括移动载体10、惯性导航装置20、激光扫描仪30和处理模块40，所述惯性导航装置20设置于所述移动载体10上，用于获得惯性运动数据；所述激光扫描仪30设置于所述移动载体10上，用于获得环境扫描数据；所述处理模块40与所述惯性导航装置20和所述激光扫描仪30连接，用于收集所述惯性运动数据和所述环境扫描数据，并执行如上所述的连续激光扫描配准辅助惯性定位定姿方法。
本申请公开的连续激光扫描配准辅助惯性定位定姿方法通过预先构建惯性导航短时间误差模型来描述短时间内惯性导航系统内的误差变化规律；在进行测绘的时候同时获取惯性运动数据和环境扫描数据，先将惯性运动数据带入预设的惯性导航短时间误差模型进行推算，获得短时间内的惯性导航相对位置误差数据；然后，通过融合环境扫描数据、惯性运动数据和惯性导航相对位置误差数据，得到短时间内的惯性导航误差，即通过激光获得的环境扫描数据辅助惯性导航技术提高了定位定姿结果的准确度；最后还通过卡尔曼滤波算法修正，得到高精度位姿数据，有效降低了惯性导航系统误差的发散速度，从而可在没有卫星信号辅助的情况下获得高精度的定位定姿数据，弥补了传统定位系统受环境影响，测量精度不足的问题。
应当理解的是，本发明并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本发明的范围仅由所附的权利要求来限制。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种连续激光扫描配准辅助惯性定位定姿方法，其特征在于，包括：
获得坐标系中的惯性运动数据和环境扫描数据；
将所述惯性运动数据带入预设的惯性导航短时间误差模型推算，获得惯性导航相对位置误差数据；
通过对所述惯性运动数据、所述惯性导航相对位置误差数据和所述环境扫描数据进行融合后，获得惯性导航误差；
通过卡尔曼滤波计算对所述惯性导航误差进行修正，获得高精度位姿数据。
2.根据权利要求1所述的连续激光扫描配准辅助惯性定位定姿方法，其特征在于，所述通过卡尔曼滤波计算对所述惯性导航误差进行修正，获得高精度位姿数据的步骤之后，还包括：
通过平滑算法对所述高精度位姿数据进行反向平滑计算，获得平滑后的位姿数据。
3.根据权利要求1所述的连续激光扫描配准辅助惯性定位定姿方法，其特征在于，所述惯性运动数据通过惯性导航系统测量，所述环境扫描数据通过激光扫描仪测量；所述惯性导航系统和所述激光扫描仪集成在一起。
4.根据权利要求1所述的连续激光扫描配准辅助惯性定位定姿方法，其特征在于，所述预设的惯性导航短时间误差模型建立在坐标系中，所述坐标系包括载体坐标系、局部水平坐标系和轨迹坐标系；
其中，所述载体坐标系与载体固连，中心为载体中心，x轴指向右向，y轴指向前向，z轴沿铅锤方向指向上；
所述局部水平坐标系中心为载体中心，x轴指向东方向，y轴指向北方向，z轴沿铅锤方向指向上，所述载体坐标系的姿态在所述局部水平坐标系中进行表达；
所述轨迹坐标系的中心为载体中心，坐标轴姿态为导航系统计算得到的姿态，与所述局部水平坐标系存在一个误差角，记为
Figure 585769DEST_PATH_IMAGE002
。
5.根据权利要求4所述的连续激光扫描配准辅助惯性定位定姿方法，其特征在于，所述预设的惯性导航短时间误差模型为：
Figure 691129DEST_PATH_IMAGE003
其中，
Figure 578313DEST_PATH_IMAGE004
为
Figure 136334DEST_PATH_IMAGE006
时刻载体在所述轨迹坐标系的位置误差，
Figure 524590DEST_PATH_IMAGE008
为
Figure 359821DEST_PATH_IMAGE006
时刻所述轨迹坐标系的速度误差，
Figure 276962DEST_PATH_IMAGE010
为测量时刻与
Figure 853437DEST_PATH_IMAGE006
时刻的时间间隔，
Figure 386662DEST_PATH_IMAGE012
为
Figure 201034DEST_PATH_IMAGE006
时刻所述轨迹坐标系因姿态角误差造成的重力分解误差，
Figure DEST_PATH_IMAGE013
为未补偿的加速度计零偏，
Figure 961180DEST_PATH_IMAGE014
为预积分量，
Figure 759371DEST_PATH_IMAGE016
为惯性导航姿态矩阵，表示系统从所述载体坐标系到所述轨迹坐标系的转换关系。
6.根据权利要求4所述的连续激光扫描配准辅助惯性定位定姿方法，其特征在于，所述通过对所述惯性运动数据、所述惯性导航相对位置误差数据和所述环境扫描数据进行融合后，获得惯性导航误差的步骤，具体包括：
利用所述惯性运动数据对所述环境扫描数据进行转换，获得所述轨迹坐标系中的扫描点云的坐标；
对扫描点云进行配准，得到不同时间点扫描点云重叠部分的配准点对的坐标；
融合所述配准点对的坐标和所述惯性导航相对位置误差数据，计算出所述配准点对之间的距离；
根据所述配准点对之间的距离计算配准误差矢量；
通过利用最小二乘法的原理进行估计，融合所述配准误差矢量，计算出惯性导航误差。
7.根据权利要求6所述的连续激光扫描配准辅助惯性定位定姿方法，其特征在于，所述对扫描点云进行配准，得到不同时间点扫描点云重叠部分的配准点对的坐标的步骤，具体包括：
通过最邻近匹配方法或者特征匹配方法构建扫描点云的配准点对。
8.根据权利要求4所述的连续激光扫描配准辅助惯性定位定姿方法，其特征在于，所述通过卡尔曼滤波计算对所述惯性导航误差进行修正，获得高精度位姿数据的步骤，具体包括：
构建所述惯性运动数据的状态向量；通过点云配准估计的所述轨迹坐标系中的速度误差、重力分解误差和加速度零偏误差构建观测值；
构建所述观测值与所述状态向量之间的函数关系，获得测量模型；
在所述测量模型中利用序列扫描配准激光点云估计并修正所述惯性导航误差，以持续激光扫描辅助关系导航系统持续定位，获得高精度位姿数据。
9.一种连续激光扫描配准辅助惯性定位定姿系统，其特征在于，包括：
移动载体；
惯性导航装置，设置于所述移动载体上，用于获得惯性运动数据；
激光扫描仪，设置于所述移动载体上，用于获得环境扫描数据；以及
处理模块，与所述惯性导航装置和所述激光扫描仪连接，用于收集所述惯性运动数据和所述环境扫描数据，并执行如权利要求1至8任意一项所述的连续激光扫描配准辅助惯性定位定姿方法。
10.根据权利要求9所述的连续激光扫描配准辅助惯性定位定姿系统，其特征在于，所述系统还包括平滑滤波器。
CN202111208988.7A 2021-10-18 2021-10-18 连续激光扫描配准辅助惯性定位定姿方法 Active CN113639722B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111208988.7A CN113639722B (zh) 2021-10-18 2021-10-18 连续激光扫描配准辅助惯性定位定姿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111208988.7A CN113639722B (zh) 2021-10-18 2021-10-18 连续激光扫描配准辅助惯性定位定姿方法

Publications (2)

Publication Number Publication Date
CN113639722A true CN113639722A (zh) 2021-11-12
CN113639722B CN113639722B (zh) 2022-02-18

Family

ID=78427300

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111208988.7A Active CN113639722B (zh) 2021-10-18 2021-10-18 连续激光扫描配准辅助惯性定位定姿方法

Country Status (1)

Country Link
CN (1) CN113639722B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115127548A (zh) * 2022-06-28 2022-09-30 苏州精源创智能科技有限公司 一种惯导与激光点阵融合导航系统
CN116315189A (zh) * 2023-05-25 2023-06-23 澄瑞电力科技(上海)股份公司 一种基于数据融合的电池包热失控预测方法和系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104977002A (zh) * 2015-06-12 2015-10-14 同济大学 基于sins/双od的惯性组合导航系统及其导航方法
CN105628026A (zh) * 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和系统
CN110673115A (zh) * 2019-09-25 2020-01-10 杭州飞步科技有限公司 雷达与组合导航系统的联合标定方法、装置、设备及介质

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104977002A (zh) * 2015-06-12 2015-10-14 同济大学 基于sins/双od的惯性组合导航系统及其导航方法
CN105628026A (zh) * 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和系统
CN110673115A (zh) * 2019-09-25 2020-01-10 杭州飞步科技有限公司 雷达与组合导航系统的联合标定方法、装置、设备及介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
金峰: "机器人IMU与激光扫描测距传感器数据融合", 《机器人》 *
陈兴邦: "基于LiDAR/INS组合导航的室内移动机器人定位研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115127548A (zh) * 2022-06-28 2022-09-30 苏州精源创智能科技有限公司 一种惯导与激光点阵融合导航系统
CN116315189A (zh) * 2023-05-25 2023-06-23 澄瑞电力科技(上海)股份公司 一种基于数据融合的电池包热失控预测方法和系统
CN116315189B (zh) * 2023-05-25 2023-08-11 澄瑞电力科技(上海)股份公司 一种基于数据融合的电池包热失控预测方法和系统

Also Published As

Publication number Publication date
CN113639722B (zh) 2022-02-18

Similar Documents

Publication Publication Date Title
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN105509739B (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN109933056B (zh) 一种基于slam的机器人导航方法以及机器人
WO2019136714A1 (zh) 一种基于3d激光的地图构建方法及系统
CN113639722B (zh) 连续激光扫描配准辅助惯性定位定姿方法
CN112987065B (zh) 一种融合多传感器的手持式slam装置及其控制方法
CN107490378B (zh) 一种基于mpu6050与智能手机的室内定位与导航的方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN115479598A (zh) 基于多传感器融合的定位与建图方法及紧耦合系统
CN110412596A (zh) 一种基于图像信息和激光点云的机器人定位方法
CN107702712A (zh) 基于惯性测量双层wlan指纹库的室内行人组合定位方法
CN104613966B (zh) 一种地籍测量事后数据处理方法
CN107063251A (zh) 一种基于WiFi室内定位的导航推车系统及定位方法
CN114485643B (zh) 一种煤矿井下移动机器人环境感知与高精度定位方法
CN111060099A (zh) 一种无人驾驶汽车实时定位方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
Deschênes et al. Lidar scan registration robust to extreme motions
WO2024109002A1 (zh) 仿生视觉多源信息智能感知无人平台
CN116338719A (zh) 基于b样条函数的激光雷达-惯性-车辆融合定位方法
CN116202509A (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
GR01 Patent grant
GR01 Patent grant