CN110207692B - 一种地图辅助的惯性预积分行人导航方法 - Google Patents

一种地图辅助的惯性预积分行人导航方法 Download PDF

Info

Publication number
CN110207692B
CN110207692B CN201910393891.4A CN201910393891A CN110207692B CN 110207692 B CN110207692 B CN 110207692B CN 201910393891 A CN201910393891 A CN 201910393891A CN 110207692 B CN110207692 B CN 110207692B
Authority
CN
China
Prior art keywords
time
speed
zero
navigation
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
CN201910393891.4A
Other languages
English (en)
Other versions
CN110207692A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201910393891.4A priority Critical patent/CN110207692B/zh
Publication of CN110207692A publication Critical patent/CN110207692A/zh
Application granted granted Critical
Publication of CN110207692B publication Critical patent/CN110207692B/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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

本发明公开了一种地图辅助的惯性预积分行人导航方法。周期采集k时刻加速度计数据和陀螺仪数据；利用获取的惯性传感器测量数据预积分预测k时刻载体的导航信息；判断导航***是否初始化，若已经初始化，则进行地图匹配检测，若匹配成功，则进行联合地图、惯性误差进行位姿全局优化，并输出载体导航信，若匹配失败，则进行零速检测，若为非零速，则进行惯性预积分求解位姿，若存在零速，则联合惯性误差、零速约束优化求解位姿，输出载体导航信。本发明利用惯性传感器测量值进行预积分优化求解输出行人导航信息，并进行零速约束校正优化，且引入地图信息对全局轨迹进行优化，消除累积误差。

Description

一种地图辅助的惯性预积分行人导航方法
技术领域
本发明属于行人导航技术领域，特别涉及了一种惯性预积分行人导航方法。
背景技术
行人导航技术是当今社会与生活中导航技术领域中的一大研究与应用热点。惯性传感器可以敏感自身转动角速度与加速度信息，具有极高自主性与抗干扰性，可以应用在各种复杂环境中，是导航领域中具有广阔发展前景与较大应用潜质的技术。
目前大多行人导航技术都基于卡尔曼滤波极其衍生的技术，解算过程求取为当前时刻的最优解，但并不是全局最优的情况。同时由于惯性器件为纯自主传感器，无法接受外界信号修正航向，在室内环境中导航时，往往可以获取当前环境的先验地图消息而进行航向的修正，基于卡尔曼滤波技术的行人导航对于航向的修正仅为简单的误差平均，无法反应真实情况，从而对于误差的修正效果不佳，造成精度下降。
发明内容
为了解决上述背景技术提到的技术问题，本发明提出了一种地图辅助的惯性预积分行人导航方法，利用预积分技术结合图优化框架将累计误差准确估计，得到全局最优导航信息。
为了实现上述技术目的，本发明的技术方案为：
一种地图辅助的惯性预积分行人导航方法，将惯性传感器固定于行人足部载体，所述惯性传感器包括陀螺仪和加速度计，其特征在于，包括以下步骤：
(1)周期采集k时刻加速度计数据和陀螺仪数据；
(2)利用步骤(1)获取的惯性传感器测量数据预积分预测k时刻载体的导航信息，得到预积分预测值；
(3)判断导航***是否初始化，若未初始化，则进行初始化，得到惯性传感器的偏差和重力分量的估计值，并跳转至步骤(1)；若已经初始化，则进入步骤(4)；
(4)进行地图匹配检测，若匹配成功，则进行联合地图、惯性误差进行位姿全局优化，并跳转至步骤(6)；若匹配失败则进入步骤(5)；
(5)进行零速检测，若为非零速，则进行惯性预积分求解位姿；若存在零速，则联合惯性误差、零速约束优化求解位姿；
(6)输出载体导航信息，并跳转至步骤(1)。
进一步地，所述载体的导航信息包括姿态四元数、三维位置和三维速度信息；所述姿态四元数用于表示包括横滚角、俯仰角和偏航角在内的三个姿态角，其中，横滚角为载体绕导航坐标系的Y轴方向的转动角度，俯仰角为载体绕导航坐标系的X轴方向的转动角度，偏航角为载体绕导航坐标系的Z轴方向的转动角度，其旋转方向均满足右手定则；所述三维位置为载***置在导航系各轴中的投影，三维速度为载体速度在导航系各轴中的投影。
进一步地，定义各坐标系：
以当前时刻载体的位置为原点构建机体坐标系b，其X轴、Y轴与Z轴分别与当前时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建惯性坐标系i，其X轴、Y轴与Z轴分别与初始时刻载体的右向、前向和当地水平面的天向重合；以与惯性系相同的规则构建导航坐标系n。
进一步地，在步骤(2)中，获得预积分预测值的方法如下：
在惯性传感器中，基于重力不变假设，其测量模型如下：
Figure BDA0002057526980000031
Figure BDA0002057526980000032
上式中，
Figure BDA0002057526980000033
和fn(k)分别为加速度计的测量值和真值，
Figure BDA0002057526980000034
和
Figure BDA0002057526980000035
分别为陀螺仪的测量值和真值，ηa(k)为k时刻加速度计的随机噪声，ηg(k)为k时刻陀螺仪的随机噪声，gn为导航系下的重力，
Figure BDA0002057526980000036
为k时刻导航坐标系到机体坐标系的旋转矩阵，ba(k)为k时刻的加速度计量测偏差，bg(k)为k时刻的陀螺仪量测偏差，则对ba(k)和bg(k)有：
Figure BDA0002057526980000037
Figure BDA0002057526980000038
其中，
Figure BDA0002057526980000039
和
Figure BDA00020575269800000310
为ba(k)和bg(k)的微分，ηba为加速度计白噪声，ηbg为陀螺仪白噪声；
对惯性测量信息进行预积分的过程为，采用两组不同间隔对时间进行编号，其中以离散周期Δt下的编号为k及k(·)，以n个离散周期nΔt下的编号为i，即i时刻与k(i)时刻表示为同一时刻，有k(i)-k(i-1)＝n，经过时间t(i)-t(i-1)＝k(i)Δt-(k(i)-n)Δt，t(i)为到当前i时刻经过的总时间，则：
Figure BDA00020575269800000311
Figure BDA00020575269800000312
Figure BDA00020575269800000313
上式中，ΔRi-1,i为从i时刻到i-1时刻的旋转矩阵，Δvi-1,i为i-1时刻到i时刻的速度变化量，Δpi-1,i为从i-1时刻到i时刻的位置变化量，Ri为i时刻从机体系至导航系的旋转矩阵，
Figure BDA0002057526980000041
为i-1时刻从导航系至机体系的旋转矩阵，vi为i时刻在导航系中的速度，vi-1为i-1时刻在导航系中的速度，Δtk(i-1),i为从k(i-1)时刻到i时刻的时间周期，ΔRk(i-1),k为从k时刻到k(i-1)时刻的旋转矩阵，pi为i时刻在导航系中的位置，pi-1为i-1时刻在导航系中的位置；
Figure BDA0002057526980000042
Exp:R3→SO(3)，代表指数投影映射，将一个姿态向量
Figure BDA0002057526980000043
投影为李群中的旋转矩阵；其中，
Figure BDA0002057526980000044
为任一三维向量，‖‖表示被限制在其它任意一个连续的2π范围内，
Figure BDA0002057526980000045
为向量
Figure BDA0002057526980000046
的反对称矩阵，I为单位阵，R3为代数空间，SO(3)为李群空间。
进一步地，在步骤(3)中，初始化的过程如下：
(301)开始时刻静止k(i)时刻，获取第i＝1时刻的所有惯性测量信息并进行惯性预积分，并以开始时刻的机体系作为导航系；
(302)利用静止时刻的零速假设，通过优化预积分预测与静止假设旋转差值，计算陀螺仪的初始偏差，在初始的零速区间内，将陀螺仪的测量值进行预积分：
Figure BDA0002057526980000047
其中，静忽略止时刻中的连续偏差变化，bg(1)＝bg(2)＝…＝bg(k(1)-1)，
Figure BDA0002057526980000048
表示陀螺仪的误差为bg(1)时k(1)时刻到1时刻旋转矩阵
Figure BDA0002057526980000049
的偏导数，
Figure BDA00020575269800000410
为k时刻旋转矩阵
Figure BDA0002057526980000051
相对于陀螺仪误差bg(1)导数；
由于为静止状态，所以姿态变化量测为单位阵I，优化上述函数得到初始化陀螺仪偏差；
(303)利用重力估计确定当前横滚俯仰在导航系中的分量：
由于零速假设，当加速度计的量测值全部为重力在导航系中的投影，得到重力分量的初值
Figure BDA0002057526980000052
gb为重力在机体系下的投影；
(304)将重力表达为：
gb＝RbiExp(δθ)gi
≈Rbigi-Rbi(gi)×δθ
其中，Rbi为惯性系与机体系的变化关系，(·)×为取反对称矩阵，δθ＝[δθx δθy 0]为姿态的扰动，δθx为绕x轴旋转的角度扰动，δθy为绕y轴旋转的角度扰动，gi为惯性系下的重力，将导航系与惯性系对齐，使得导航系中重力分量与当地水平面垂直；
将旋转矩阵转换为姿态四元数，转换过程如下：
Figure BDA0002057526980000053
Figure BDA0002057526980000054
Figure BDA0002057526980000055
Figure BDA0002057526980000056
Figure BDA0002057526980000057
其中，q0、q1、q2、q3为姿态四元数，rij为旋转矩阵中第i行第j列的元素。
进一步地，在步骤(4)中，地图匹配检测的方法如下：
利用提前获取的当前场景二维地图，设置高程标记点，其坐标在设定初始位置与航向后将投影至惯性坐标系下，将载体在惯性系下的高程变化与角度变化进行检测，检测方程如下：
在前一个运动区间内，如果其高程变化最高值超过提前设定的阈值εh，即如果存在k时刻的高度zk相对于k-1时刻的变化|zk-zk-1|＞εh，则在此运动区间后的第一个零速点，位置为
Figure BDA0002057526980000061
作为地图匹配矫正点，并利用当前时刻的速度vk的x,y分量补偿位置偏差，
Figure BDA0002057526980000062
为补偿后位置，vi-1(x,y,0)为在i-1时刻(x,y,0)处的速度，vi(x,y,0)为在i时刻(x,y,0)处的速度，消除高程速度影响，即如下式所示：
Figure BDA0002057526980000063
进一步地，在步骤(4)中，联合地图、惯性误差进行位姿全局优化的方法如下：
将当前时刻的载体导航位置信息与地图点匹配的位置信息的误差加入整体误差进行优化，从而进行全局的位姿优化；优化目标为从上一次经过匹配成功的地图点到现在检测的地图匹配点之间的所有q个零速间隔内惯性预积分预测状态值χ:[x1,x2…xq],
Figure BDA0002057526980000064
其中，
Figure BDA0002057526980000065
为q时刻从机体系到导航系的旋转矩阵，
Figure BDA0002057526980000066
为q时刻机体系在导航系中的位置，
Figure BDA0002057526980000067
为q时刻机体系在导航系中的速度，
Figure BDA0002057526980000068
为q时陀螺仪误差，
Figure BDA0002057526980000069
为q时加速度计误差，EIMU(q,q+1)为检测成功时刻及下一刻的位姿估计优化函数：
Figure BDA00020575269800000610
其中：
Figure BDA0002057526980000071
Figure BDA0002057526980000072
Figure BDA0002057526980000073
eb＝b(q+1)-b(q)
Figure BDA0002057526980000074
上式中，
Figure BDA0002057526980000075
为当前检测匹配值的位置，ΣI与ΣR为信息矩阵，用于描述惯性误差之间的关系，ρ为鲁棒核函数，用于抵制错误点带来的误差，eR为惯性在姿态中的误差，ev为惯性在速度中的误差，ep为惯性在位置中的误差，eb为惯性在误差转移上的变化，ΔRq,q+1(bg(q))为陀螺仪误差为bg(q)下从q+1时刻至q时刻旋转矩阵，Δvq,q+1(bg(q),ba(q))为陀螺仪误差为bg(q)、加速度计误差为ba(q)下从q时刻至q+1时刻速度变化，
Figure BDA0002057526980000076
为q+1时刻从惯性系到机体系的旋转矩阵，δbg(q)为q时刻陀螺仪误差变化量，δba(q)为q时刻加速度计误差变化量；
Figure BDA0002057526980000077
代表对数投影映射，将旋转矩阵变换为一个旋转角
Figure BDA0002057526980000078
与一个旋转轴单位矢量a，R为任意旋转矩阵，上标“∨”表示取逆反对称矩阵；
eR在优化过程中仅优化通过陀螺仪量测的流型积分与重力估计的横滚与俯仰角之差；
在进行联合地图匹配优化后，利用速度对位置与偏航进行重新递推，
Figure BDA0002057526980000079
从而对全局位置与偏航角进行优化；
利用牛顿-列温伯格迭代方法进行求解上述优化函数，当得到最优解或迭代次数达到最大迭代次数时输出当前全局状态量的求解结果。
进一步地，在步骤(5)中，零速检测的方法如下：
利用广义似然比检验法的零速更新进行零速检测，对行人处在站立相和行走相的概率进行估计，从而对零速状态T(zn)进行估计：
Figure BDA0002057526980000081
其中，N为滑动窗口区间，Ωn＝{k∈N:n≤k≤N-1}，
Figure BDA0002057526980000082
为滑动窗口区间内加速度计输出的平均值，
Figure BDA0002057526980000083
为其二范数模长，
Figure BDA0002057526980000084
与
Figure BDA0002057526980000085
为加速度计与陀螺仪的噪声方差，g表示当地重力分量；
当检测到T(zn)小于阈值γ'，则认为进入站立静止相则为零速状态，其中，γ'＝-2ln(γ)/N，γ根据实际情况设定。
进一步地，在步骤(5)中，联合惯性误差、零速约束优化求解位姿的方法如下：
在检测到零速后，则将当前载体预积分姿态值与零速进行比较得出误差，从而优化对惯性误差的估计；
以当前零速间隔为q，在当前零速q与上一零速q-1之间增加零速约束，优化目标为
Figure BDA0002057526980000086
优化函数如下：
Figure BDA0002057526980000087
采用牛顿-列温伯格迭代方法进行求解上述优化函数，当得到最优解或迭代次数达到最大迭代次数时输出当前状态量的求解结果。
采用上述技术方案带来的有益效果：
本发明能够在室内环境中利用地图有效修正行人导航中的航向及位置误差，获得高可靠性及高精度的行人导航信息，具有良好的应用前景。
附图说明
图1是本发明的方法流程图。
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
本发明设计了一种地图辅助的惯性预积分行人导航方法，如图1所示，将惯性传感器固定于行人足部载体，所述惯性传感器包括陀螺仪和加速度计，其特征在于，步骤如下：
步骤1：周期采集k时刻加速度计数据和陀螺仪数据；
步骤2：利用步骤1获取的惯性传感器测量数据预积分预测k时刻载体的导航信息，得到预积分预测值；
步骤3：判断导航***是否初始化，若未初始化，则进行初始化，得到惯性传感器的偏差和重力分量的估计值，并跳转至步骤1；若已经初始化，则进入步骤4；
步骤4：进行地图匹配检测，若匹配成功，则进行联合地图、惯性误差进行位姿全局优化，并跳转至步骤6；若匹配失败则进入步骤5；
步骤5：进行零速检测，若为非零速，则进行惯性预积分求解位姿；若存在零速，则联合惯性误差、零速约束优化求解位姿；
步骤6：输出载体导航信息，并跳转至步骤1。
在本实施例中，所述载体的导航信息包括姿态四元数、三维位置和三维速度信息；所述姿态四元数用于表示包括横滚角、俯仰角和偏航角在内的三个姿态角，其中，横滚角为载体绕导航坐标系的Y轴方向的转动角度，俯仰角为载体绕导航坐标系的X轴方向的转动角度，偏航角为载体绕导航坐标系的Z轴方向的转动角度，其旋转方向均满足右手定则；所述三维位置为载***置在导航系各轴中的投影，三维速度为载体速度在导航系各轴中的投影。
在本实施例中，定义各坐标系：
以当前时刻载体的位置为原点构建机体坐标系b，其X轴、Y轴与Z轴分别与当前时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建惯性坐标系i，其X轴、Y轴与Z轴分别与初始时刻载体的右向、前向和当地水平面的天向重合；以与惯性系相同的规则构建导航坐标系n。
在本实施例中，步骤2采用如下优选方案实现：
获得预积分预测值的方法如下：
在惯性传感器中，基于重力不变假设，其测量模型如下：
Figure BDA0002057526980000101
Figure BDA0002057526980000102
上式中，
Figure BDA0002057526980000103
和fn(k)分别为加速度计的测量值和真值，
Figure BDA0002057526980000104
和
Figure BDA0002057526980000105
分别为陀螺仪的测量值和真值，ηa(k)为k时刻加速度计的随机噪声，ηg(k)为k时刻陀螺仪的随机噪声，gn为导航系下的重力，
Figure BDA0002057526980000106
为k时刻导航坐标系到机体坐标系的旋转矩阵，ba(k)为k时刻的加速度计量测偏差，bg(k)为k时刻的陀螺仪量测偏差，则对ba(k)和bg(k)有：
Figure BDA0002057526980000107
Figure BDA0002057526980000108
其中，
Figure BDA0002057526980000109
和
Figure BDA00020575269800001010
为ba(k)和bg(k)的微分，ηba为加速度计白噪声，ηbg为陀螺仪白噪声；
对惯性测量信息进行预积分的过程为，对惯性测量信息进行预积分的过程为，采用两组不同间隔对时间进行编号，其中以离散周期Δt下的编号为k及k(·)，以n个离散周期nΔt下的编号为i，即i时刻与k(i)时刻表示为同一时刻，有k(i)-k(i-1)＝n，经过时间t(i)-t(i-1)＝k(i)Δt-(k(i)-n)Δt，t(i)为到当前i时刻经过的总时间，则：
Figure BDA0002057526980000111
Figure BDA0002057526980000112
Figure BDA0002057526980000113
上式中，ΔRi-1,i为从i时刻到i-1时刻的旋转矩阵，Δvi-1,i为i-1时刻到i时刻的速度变化量，Δpi-1,i为从i-1时刻到i时刻的位置变化量，Ri为i时刻从机体系至导航系的旋转矩阵，
Figure BDA0002057526980000114
为i-1时刻从导航系至机体系的旋转矩阵，vi为i时刻在导航系中的速度，vi-1为i-1时刻在导航系中的速度，Δtk(i-1),i为从k(i-1)时刻到i时刻的时间周期，ΔRk(i-1),k为从k时刻到k(i-1)时刻的旋转矩阵，pi为i时刻在导航系中的位置，pi-1为i-1时刻在导航系中的位置；
Figure BDA0002057526980000115
Exp:R3→SO(3)，代表指数投影映射，将一个姿态向量
Figure BDA0002057526980000116
投影为李群中的旋转矩阵；其中，
Figure BDA0002057526980000117
为任一三维向量，‖‖表示被限制在其它任意一个连续的2π范围内，
Figure BDA0002057526980000121
为向量
Figure BDA0002057526980000122
的反对称矩阵，I为单位阵，R3为代数空间，SO(3)为李群空间。
在本实施例中，步骤3采用如下优选方案实现：
初始化的过程如下：
301、开始时刻静止k(i)时刻，获取第i＝1时刻的所有惯性测量信息并进行惯性预积分，并以开始时刻的机体系作为导航系；
302、利用静止时刻的零速假设，通过优化预积分预测与静止假设旋转差值，计算陀螺仪的初始偏差，在初始的零速区间内，将陀螺仪的测量值进行预积分：
Figure BDA0002057526980000123
其中，静忽略止时刻中的连续偏差变化，bg(1)＝bg(2)＝…＝bg(k(1)-1)，
Figure BDA0002057526980000124
表示陀螺仪的误差为bg(1)时k(1)时刻到1时刻旋转矩阵
Figure BDA0002057526980000125
的偏导数，
Figure BDA0002057526980000126
为k时刻旋转矩阵
Figure BDA0002057526980000127
相对于陀螺仪误差bg(1)导数；
由于为静止状态，所以姿态变化量测为单位阵I，优化上述函数得到初始化陀螺仪偏差；
303、利用重力估计确定当前横滚俯仰在导航系中的分量：
由于零速假设，当加速度计的量测值全部为重力在导航系中的投影，得到重力分量的初值
Figure BDA0002057526980000128
gb为重力在机体系下的投影；
304、将重力表达为：
gb＝RbiExp(δθ)gi
≈Rbigi-Rbi(gi)×δθ
其中，Rbi为惯性系与机体系的变化关系，(·)×为取反对称矩阵，δθ＝[δθx δθy 0]为姿态的扰动，δθx为绕x轴旋转的角度扰动，δθy为绕y轴旋转的角度扰动，gi为惯性系下的重力，将导航系与惯性系对齐，使得导航系中重力分量与当地水平面垂直；
将旋转矩阵转换为姿态四元数，转换过程如下：
Figure BDA0002057526980000131
Figure BDA0002057526980000132
Figure BDA0002057526980000133
Figure BDA0002057526980000134
Figure BDA0002057526980000135
其中，q0、q1、q2、q3为姿态四元数，rij为旋转矩阵中第i行第j列的元素。
在本实施例中，步骤4采用如下优选方案实现：
地图匹配检测的方法如下：
利用提前获取的当前场景二维地图，设置高程标记点，其坐标在设定初始位置与航向后将投影至惯性坐标系下，将载体在惯性系下的高程变化与角度变化进行检测，检测方程如下：
在前一个运动区间内，如果其高程变化最高值超过提前设定的阈值εh，即如果存在k时刻的高度zk相对于k-1时刻的变化|zk-zk-1|＞εh，则在此运动区间后的第一个零速点，位置为
Figure BDA0002057526980000136
作为地图匹配矫正点，并利用当前时刻的速度vk的x,y分量补偿位置偏差，
Figure BDA0002057526980000137
为补偿后位置，vi-1(x,y,0)为在i-1时刻(x,y,0)处的速度，vi(x,y,0)为在i时刻(x,y,0)处的速度，消除高程速度影响，即如下式所示：
Figure BDA0002057526980000141
如果在楼梯处，则存在高度的变化|zk-zk-1|＞εh，在楼梯的最后一阶台阶可作为地图标志点，此时高度变化从有到无，即|zk-zk-1|＜εh，将此时楼梯的位置作为地图匹配位置点。
联合地图、惯性误差进行位姿全局优化的方法如下：
将当前时刻的载体导航位置信息与地图点匹配的位置信息的误差加入整体误差进行优化，从而进行全局的位姿优化；优化目标为从上一次经过匹配成功的地图点到现在检测的地图匹配点之间的所有q个零速间隔内惯性预积分预测状态值χ:[x1,x2...xq],
Figure BDA0002057526980000142
其中，
Figure BDA0002057526980000143
为q时刻从机体系到导航系的旋转矩阵，
Figure BDA0002057526980000144
为q时刻机体系在导航系中的位置，
Figure BDA0002057526980000145
为q时刻机体系在导航系中的速度，
Figure BDA0002057526980000146
为q时陀螺仪误差，
Figure BDA0002057526980000147
为q时加速度计误差，EIMU(q,q+1)为检测成功时刻及下一刻的位姿估计优化函数：
Figure BDA0002057526980000148
其中：
Figure BDA0002057526980000149
Figure BDA00020575269800001410
Figure BDA00020575269800001411
eb＝b(q+1)-b(q)
Figure BDA0002057526980000151
上式中，
Figure BDA0002057526980000152
为当前检测匹配值的位置，ΣI与ΣR为信息矩阵，用于描述惯性误差之间的关系，ρ为鲁棒核函数，用于抵制错误点带来的误差，eR为惯性在姿态中的误差，ev为惯性在速度中的误差，ep为惯性在位置中的误差，eb为惯性在误差转移上的变化，ΔRq,q+1(bg(q))为陀螺仪误差为bg(q)下从q+1时刻至q时刻旋转矩阵，Δvq,q+1(bg(q),ba(q))为陀螺仪误差为bg(q)、加速度计误差为ba(q)下从q时刻至q+1时刻速度变化，
Figure BDA0002057526980000153
为q+1时刻从惯性系到机体系的旋转矩阵，δbg(q)为q时刻陀螺仪误差变化量，δba(q)为q时刻加速度计误差变化量；
Figure BDA0002057526980000154
代表对数投影映射，将旋转矩阵变换为一个旋转角
Figure BDA0002057526980000155
与一个旋转轴单位矢量a，R为任意旋转矩阵，上标“∨”表示取逆反对称矩阵；
eR在优化过程中仅优化通过陀螺仪量测的流型积分与重力估计的横滚与俯仰角之差；
在进行联合地图匹配优化后，利用速度对位置与偏航进行重新递推，
Figure BDA0002057526980000156
从而对全局位置与偏航角进行优化；
利用图优化开源库G2O、GTSAM、Ceres等牛顿-列温伯格迭代方法进行求解上述优化函数，当得到最优解或迭代次数达到最大迭代次数时输出当前全局状态量的求解结果。
在本实施例中，步骤5采用如下优选方案实现：
零速检测的方法如下：
利用广义似然比检验法的零速更新进行零速检测，对行人处在站立相和行走相的概率进行估计，从而对零速状态T(zn)进行估计：
Figure BDA0002057526980000161
其中，N为滑动窗口区间，Ωn＝{k∈N:n≤k≤N-1}，
Figure BDA0002057526980000162
为滑动窗口区间内加速度计输出的平均值，
Figure BDA0002057526980000163
为其二范数模长，
Figure BDA0002057526980000164
与
Figure BDA0002057526980000165
为加速度计与陀螺仪的噪声方差，g表示当地重力分量；
当检测到T(zn)小于阈值γ'，则认为进入站立静止相则为零速状态，其中，γ'＝-2ln(γ)/N，γ根据实际情况设定。
联合惯性误差、零速约束优化求解位姿的方法如下：
在检测到零速后，则将当前载体预积分姿态值与零速进行比较得出误差，从而优化对惯性误差的估计；
以当前零速间隔为q，在当前零速q与上一零速q-1之间增加零速约束，优化目标为
Figure BDA0002057526980000166
优化函数如下：
Figure BDA0002057526980000167
利用图优化开源库G2O、GTSAM、Ceres等牛顿-列温伯格迭代方法进行求解上述优化函数，当得到最优解或迭代次数达到最大迭代次数时输出当前状态量的求解结果。
实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (7)

1.一种地图辅助的惯性预积分行人导航方法，将惯性传感器固定于行人足部载体，所述惯性传感器包括陀螺仪和加速度计，其特征在于，包括以下步骤：
(1)周期采集k时刻加速度计数据和陀螺仪数据；
(2)利用步骤(1)获取的惯性传感器测量数据预积分预测k时刻载体的导航信息，得到预积分预测值；
获得预积分预测值的方法如下：
在惯性传感器中，基于重力不变假设，其测量模型如下：
Figure FDA0002780115100000011
Figure FDA0002780115100000012
上式中，
Figure FDA0002780115100000013
和fn(k)分别为加速度计的测量值和真值，
Figure FDA0002780115100000014
和
Figure FDA0002780115100000015
分别为陀螺仪的测量值和真值，ηa(k)为k时刻加速度计的随机噪声，ηg(k)为k时刻陀螺仪的随机噪声，gn为导航系下的重力，
Figure FDA0002780115100000016
为k时刻导航坐标系到机体坐标系的旋转矩阵，ba(k)为k时刻的加速度计量测偏差，bg(k)为k时刻的陀螺仪量测偏差，则对ba(k)和bg(k)有：
Figure FDA0002780115100000017
Figure FDA0002780115100000018
其中，
Figure FDA0002780115100000019
和
Figure FDA00027801151000000110
为ba(k)和bg(k)的微分，ηba为加速度计白噪声，ηbg为陀螺仪白噪声；
对惯性测量信息进行预积分的过程为，采用两组不同间隔对时间进行编号，其中以离散周期Δt下的编号为k及k(·)，以n个离散周期nΔt下的编号为i，即i时刻与k(i)时刻表示为同一时刻，有k(i)-k(i-1)＝n，经过时间t(i)-t(i-1)＝k(i)Δt-(k(i)-n)Δt，t(i)为到当前i时刻经过的总时间，则：
Figure FDA0002780115100000021
Figure FDA0002780115100000022
Figure FDA0002780115100000023
上式中，ΔRi-1,i为从i时刻到i-1时刻的旋转矩阵，Δvi-1,i为i-1时刻到i时刻的速度变化量，Δpi-1,i为从i-1时刻到i时刻的位置变化量，Ri为i时刻从机体系至导航系的旋转矩阵，
Figure FDA0002780115100000024
为i-1时刻从导航系至机体系的旋转矩阵，vi为i时刻在导航系中的速度，vi-1为i-1时刻在导航系中的速度，Δti-1,i为从i-1时刻到i时刻的时间周期，ΔRk(i-1),k为从k时刻到k(i-1)时刻的旋转矩阵，pi为i时刻在导航系中的位置，pi-1为i-1时刻在导航系中的位置；
Figure FDA0002780115100000025
Exp:R3→SO(3)，代表指数投影映射，将一个姿态向量
Figure FDA0002780115100000026
投影为李群中的旋转矩阵；其中，
Figure FDA0002780115100000027
为任一三维向量，‖‖表示被限制在其它任意一个连续的2π范围内，
Figure FDA0002780115100000028
为向量
Figure FDA0002780115100000029
的反对称矩阵，I为单位阵，R3为代数空间，SO(3)为李群空间；
(3)判断导航***是否初始化，若未初始化，则进行初始化，得到惯性传感器的偏差和重力分量的估计值，并跳转至步骤(1)；若已经初始化，则进入步骤(4)；
初始化的过程如下：
(301)开始时刻静止k(i)时刻，获取第i＝1时刻的所有惯性测量信息并进行惯性预积分，并以开始时刻的机体系作为导航系；
(302)利用静止时刻的零速假设，通过优化预积分预测与静止假设旋转差值，计算陀螺仪的初始偏差，在初始的零速区间内，将陀螺仪的测量值进行预积分：
Figure FDA0002780115100000031
其中，忽略静止时刻中的连续偏差变化，bg(1)＝bg(2)＝…＝bg(k(1)-1)，
Figure FDA0002780115100000032
Figure FDA0002780115100000033
表示陀螺仪的误差为bg(1)时，k(1)时刻到1时刻旋转矩阵
Figure FDA0002780115100000034
的偏导数，
Figure FDA0002780115100000035
为k时刻旋转矩阵
Figure FDA0002780115100000036
相对于陀螺仪误差bg(1)导数；
由于为静止状态，所以姿态变化量测为单位阵I，优化上述函数得到初始化陀螺仪偏差；
(303)利用重力估计确定当前横滚俯仰在导航系中的分量：
由于零速假设，当加速度计的量测值全部为重力在导航系中的投影，得到重力分量的初值
Figure FDA0002780115100000037
gb为重力在机体系下的投影；
(304)将重力表达为：
gb＝RbiExp(δθ)gi
≈Rbigi-Rbi(gi)×δθ
其中，Rbi为惯性系与机体系的变化关系，(·)×为取反对称矩阵，δθ＝[δθx δθy 0]为姿态的扰动，δθx为绕x轴旋转的角度扰动，δθy为绕y轴旋转的角度扰动，gi为惯性系下的重力，将导航系与惯性系对齐，使得导航系中重力分量与当地水平面垂直；
将旋转矩阵转换为姿态四元数，转换过程如下：
Figure FDA0002780115100000041
Figure FDA0002780115100000042
Figure FDA0002780115100000043
Figure FDA0002780115100000044
Figure FDA0002780115100000045
其中，q0、q1、q2、q3为姿态四元数，rij为旋转矩阵中第i行第j列的元素；
(4)进行地图匹配检测，若匹配成功，则进行联合地图、惯性误差进行位姿全局优化，并跳转至步骤(6)；若匹配失败则进入步骤(5)；
(5)进行零速检测，若为非零速，则进行惯性预积分求解位姿；若存在零速，则联合惯性误差、零速约束优化求解位姿；
(6)输出载体导航信息，并跳转至步骤(1)。
2.根据权利要求1所述地图辅助的惯性预积分行人导航方法，其特征在于，所述载体的导航信息包括姿态四元数、三维位置和三维速度信息；所述姿态四元数用于表示包括横滚角、俯仰角和偏航角在内的三个姿态角，其中，横滚角为载体绕导航坐标系的Y轴方向的转动角度，俯仰角为载体绕导航坐标系的X轴方向的转动角度，偏航角为载体绕导航坐标系的Z轴方向的转动角度，其旋转方向均满足右手定则；所述三维位置为载***置在导航系各轴中的投影，三维速度为载体速度在导航系各轴中的投影。
3.根据权利要求1所述地图辅助的惯性预积分行人导航方法，其特征在于，定义各坐标系：
以当前时刻载体的位置为原点构建机体坐标系b，其X轴、Y轴与Z轴分别与当前时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建惯性坐标系i，其X轴、Y轴与Z轴分别与初始时刻载体的右向、前向和当地水平面的天向重合；以与惯性系相同的规则构建导航坐标系n。
4.根据权利要求1所述地图辅助的惯性预积分行人导航方法，其特征在于，在步骤(4)中，地图匹配检测的方法如下：
利用提前获取的当前场景二维地图，设置高程标记点，其坐标在设定初始位置与航向后将投影至惯性坐标系下，将载体在惯性系下的高程变化与角度变化进行检测，检测方程如下：
在前一个运动区间内，如果其高程变化最高值超过提前设定的阈值εh，即如果存在k时刻的高度zk相对于k-1时刻的变化|zk-zk-1|＞εh，则在此运动区间后的第一个零速点，位置为
Figure FDA0002780115100000051
作为地图匹配矫正点，并利用当前时刻的速度vk的x,y分量补偿位置偏差，
Figure FDA0002780115100000052
为补偿后位置，vi-1(x,y,0)为在i-1时刻(x,y,0)处的速度，vi(x,y,0)为在i时刻(x,y,0)处的速度，消除高程速度影响，即如下式所示：
Figure FDA0002780115100000053
5.根据权利要求4所述地图辅助的惯性预积分行人导航方法，其特征在于，在步骤(4)中，联合地图、惯性误差进行位姿全局优化的方法如下：
将当前时刻的载体导航位置信息与地图点匹配的位置信息的误差加入整体误差进行优化，从而进行全局的位姿优化；优化目标为从上一次经过匹配成功的地图点到现在检测的地图匹配点之间的所有q个零速间隔内惯性预积分预测状态值χ:[x1,x2...xq],
Figure FDA0002780115100000054
其中，
Figure FDA0002780115100000055
为q时刻从机体系到导航系的旋转矩阵，
Figure FDA0002780115100000056
为q时刻机体系在导航系中的位置，
Figure FDA0002780115100000057
为q时刻机体系在导航系中的速度，
Figure FDA0002780115100000061
为q时陀螺仪误差，
Figure FDA0002780115100000062
为q时加速度计误差，EIMU(q,q+1)为检测成功时刻及下一刻的位姿估计优化函数：
Figure FDA0002780115100000063
其中：
Figure FDA0002780115100000064
Figure FDA0002780115100000065
Figure FDA0002780115100000066
eb＝b(q+1)-b(q)
Figure FDA0002780115100000067
上式中，
Figure FDA0002780115100000068
为当前检测匹配值的位置，ΣI与ΣR为信息矩阵，用于描述惯性误差之间的关系，ρ为鲁棒核函数，用于抵制错误点带来的误差，eR为惯性在姿态中的误差，ev为惯性在速度中的误差，ep为惯性在位置中的误差，eb为惯性在误差转移上的变化，ΔRq,q+1(bg(q))为陀螺仪误差为bg(q)下从q+1时刻至q时刻旋转矩阵，Δvq,q+1(bg(q),ba(q))为陀螺仪误差为bg(q)、加速度计误差为ba(q)下从q时刻至q+1时刻速度变化，
Figure FDA0002780115100000069
为q+1时刻从惯性系到机体系的旋转矩阵，δbg(q)为q时刻陀螺仪误差变化量，δba(q)为q时刻加速度计误差变化量；
Figure FDA00027801151000000610
代表对数投影映射，将旋转矩阵变换为一个旋转角
Figure FDA00027801151000000611
与一个旋转轴单位矢量a，R为任意旋转矩阵，上标“∨”表示取逆反对称矩阵；
eR在优化过程中仅优化通过陀螺仪量测的流型积分与重力估计的横滚与俯仰角之差；
在进行联合地图匹配优化后，利用速度对位置与偏航进行重新递推，
Figure FDA0002780115100000071
从而对全局位置与偏航角进行优化；
利用牛顿-列温伯格迭代方法进行求解上述优化函数，当得到最优解或迭代次数达到最大迭代次数时输出当前全局状态量的求解结果。
6.根据权利要求5所述地图辅助的惯性预积分行人导航方法，其特征在于，在步骤(5)中，零速检测的方法如下：
利用广义似然比检验法的零速更新进行零速检测，对行人处在站立相和行走相的概率进行估计，从而对零速状态T(zn)进行估计：
Figure FDA0002780115100000072
其中，N为滑动窗口区间，Ωn＝{k∈N:n≤k≤N-1}，
Figure FDA0002780115100000073
为滑动窗口区间内加速度计输出的平均值，
Figure FDA0002780115100000074
为其二范数模长，
Figure FDA0002780115100000075
与
Figure FDA0002780115100000076
为加速度计与陀螺仪的噪声方差，g表示当地重力分量；
当检测到T(zn)小于阈值γ'，则认为进入站立静止相则为零速状态，其中，γ'＝-2ln(γ)/N，γ根据实际情况设定。
7.根据权利要求5所述地图辅助的惯性预积分行人导航方法，其特征在于，在步骤(5)中，联合惯性误差、零速约束优化求解位姿的方法如下：
在检测到零速后，则将当前载体预积分姿态值与零速进行比较得出误差，从而优化对惯性误差的估计；
以当前零速间隔为q，在当前零速q与上一零速q-1之间增加零速约束，优化目标为
Figure FDA0002780115100000081
优化函数如下：
Figure FDA0002780115100000082
采用牛顿-列温伯格迭代方法进行求解上述优化函数，当得到最优解或迭代次数达到最大迭代次数时输出当前状态量的求解结果。
CN201910393891.4A 2019-05-13 2019-05-13 一种地图辅助的惯性预积分行人导航方法 Active CN110207692B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910393891.4A CN110207692B (zh) 2019-05-13 2019-05-13 一种地图辅助的惯性预积分行人导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910393891.4A CN110207692B (zh) 2019-05-13 2019-05-13 一种地图辅助的惯性预积分行人导航方法

Publications (2)

Publication Number Publication Date
CN110207692A CN110207692A (zh) 2019-09-06
CN110207692B true CN110207692B (zh) 2021-03-30

Family

ID=67787129

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910393891.4A Active CN110207692B (zh) 2019-05-13 2019-05-13 一种地图辅助的惯性预积分行人导航方法

Country Status (1)

Country Link
CN (1) CN110207692B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111241983B (zh) * 2020-01-07 2023-09-26 京东科技信息技术有限公司 一种姿势检测方法、装置、***、电子设备及存储介质
CN111487973B (zh) * 2020-04-26 2023-09-05 河南科技大学 一种针对导航信号低频刷新的导航方法及***
CN112729301A (zh) * 2020-12-10 2021-04-30 深圳大学 一种基于多源数据融合的室内定位方法
CN112985392B (zh) * 2021-04-19 2021-07-30 中国人民解放军国防科技大学 基于图优化框架的行人惯性导航方法和装置
CN115685292B (zh) * 2023-01-05 2023-03-21 中国人民解放军国防科技大学 一种多源融合导航***的导航方法和装置
CN116481535B (zh) * 2023-02-02 2024-06-25 中国科学院力学研究所 一种利用惯导数据修正飞行弹道数据的计算方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108492316A (zh) * 2018-02-13 2018-09-04 视辰信息科技(上海)有限公司 一种终端的定位方法和装置

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104061934B (zh) * 2014-06-10 2017-04-26 哈尔滨工业大学 基于惯性传感器的行人室内位置跟踪方法
KR102440358B1 (ko) * 2017-10-12 2022-09-05 한화디펜스 주식회사 관성 기반 항법 장치 및 상대사전적분에 따른 관성 기반 항법 방법
CN107869989B (zh) * 2017-11-06 2020-02-07 东北大学 一种基于视觉惯导信息融合的定位方法及***
CN108007456A (zh) * 2017-12-06 2018-05-08 深圳市致趣科技有限公司 一种室内导航方法、装置及***
CN109029448B (zh) * 2018-06-28 2021-11-12 东南大学 单目视觉惯性定位的imu辅助跟踪模型
CN108827315B (zh) * 2018-08-17 2021-03-30 华南理工大学 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN109631894A (zh) * 2018-12-11 2019-04-16 智灵飞（北京）科技有限公司 一种基于滑动窗口的单目视觉惯性紧耦合方法
CN109465832A (zh) * 2018-12-18 2019-03-15 哈尔滨工业大学（深圳） 高精度视觉和imu紧融合定位方法与***

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108492316A (zh) * 2018-02-13 2018-09-04 视辰信息科技(上海)有限公司 一种终端的定位方法和装置

Also Published As

Publication number Publication date
CN110207692A (zh) 2019-09-06

Similar Documents

Publication Publication Date Title
CN110207692B (zh) 一种地图辅助的惯性预积分行人导航方法
CN110207697B (zh) 基于角加速度计/陀螺/加速度计的惯性导航解算方法
CN108362282B (zh) 一种基于自适应零速区间调整的惯性行人定位方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN107588769B (zh) 一种车载捷联惯导、里程计及高程计组合导航方法
CN108007477B (zh) 一种基于正反向滤波的惯性行人定位***误差抑制方法
CN106168485B (zh) 步行航迹数据推算方法及装置
US20150362330A1 (en) Method and System for Varying Step Length Estimation Using Nonlinear System Identification
CN106153069B (zh) 自主导航***中的姿态修正装置和方法
CN111024070A (zh) 一种基于航向自观测的惯性足绑式行人定位方法
CN110715659A (zh) 零速检测方法、行人惯性导航方法、装置及存储介质
CN104613965B (zh) 一种基于双向滤波平滑技术的步进式行人导航方法
CN113175933B (zh) 一种基于高精度惯性预积分的因子图组合导航方法
CN110207693B (zh) 一种鲁棒立体视觉惯性预积分slam方法
CN109931955A (zh) 基于状态相关李群滤波的捷联惯性导航***初始对准方法
CN109959374B (zh) 一种行人惯性导航全时全程逆向平滑滤波方法
CN112362057B (zh) 基于零速修正与姿态自观测的惯性行人导航算法
CN112562077A (zh) 一种融合pdr和先验地图的行人室内定位方法
CN112066980B (zh) 一种基于人体四节点运动约束的行人导航定位方法
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
CN116007620A (zh) 一种组合导航滤波方法、***、电子设备及存储介质
CN113514064A (zh) 一种鲁棒因子图多源容错导航方法
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
Tang et al. Exploring the accuracy potential of IMU preintegration in factor graph optimization
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法

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