CN114543794A - 一种视觉惯性里程计与间断性rtk融合的绝对定位方法 - Google Patents

一种视觉惯性里程计与间断性rtk融合的绝对定位方法 Download PDF

Info

Publication number
CN114543794A
CN114543794A CN202111642896.XA CN202111642896A CN114543794A CN 114543794 A CN114543794 A CN 114543794A CN 202111642896 A CN202111642896 A CN 202111642896A CN 114543794 A CN114543794 A CN 114543794A
Authority
CN
China
Prior art keywords
coordinate system
rtk
vio
yaw angle
enu
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.)
Pending
Application number
CN202111642896.XA
Other languages
English (en)
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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical 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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202111642896.XA priority Critical patent/CN114543794A/zh
Publication of CN114543794A publication Critical patent/CN114543794A/zh
Pending legal-status Critical Current

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/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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning 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/20—Instruments for performing navigational calculations
    • 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/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Software Systems (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Operations Research (AREA)
  • Probability & Statistics with Applications (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Computing Systems (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种视觉惯性里程计与间断性RTK融合的绝对定位方法，首先进行RTK‑VIO系统初始化，完成RTK所在地球地理坐标系以及VIO坐标系和ENU坐标系之间的转换；然后获取当前位置的RTK值和VIO值，并转换到ENU坐标系；接下里利用VIO所估计位姿与RTK所估计位姿构建观测残差优化问题；构建滑动窗口，将步骤3所得残差按优质RTK信号发生事件的时间顺序加入到滑动窗口中，进行滚动优化，得到一个最新的校准事件；最终将校准事件平滑化，保证无人机的安全稳定飞行。本发明弥补了视觉惯性里程计误差累积和RTK受间歇性遮挡无法准确定位的缺点，实现了相对地理坐标系切空间的实时鲁棒的绝对定位。

Description

一种视觉惯性里程计与间断性RTK融合的绝对定位方法
技术领域
本发明属于定位技术领域，具体涉及一种融合定位方法。
背景技术
基础设施检测可以帮助相关部门定期评估基础设施的健康状态，及时维护受损设施，预防和防止灾难的发生。传统基础设施检测主要依靠人工携带设备或者人工遥操作的方式，存在人身风险高、作业效率底、成本高等多方面缺点。受益于近年来机器人和人工智能等领域的发展，自主检测基础设施已逐步受到研究者的关注。四旋翼无人机作为一种高灵活性的飞行载具，特别适合基础设施自主检测任务的要求，然而，在传统四旋翼无人机或是依赖GPS定位，或是依赖视觉惯性里程计定位，存在易受遮挡或相对定位的缺点，均无法满足基础设施自主检测任务对于高精度绝对定位的需求。无人机常利用实时动态测量技术(Real Time Kinematic,RTK)来提供精确的绝对定位进而确保鲁棒飞行，然而，当无人机靠近设施进行巡检时，RTK信号容易受到设施自身结构的遮挡而质量变差甚至中断；与此同时，随着无人机远离设施结构，RTK信号质量又会变好。另一方面，视觉惯性里程计(VisualInertial Odometry,VIO)常被用于无人机定位导航，且在无人机靠近基础设施时仍可提供持续的位姿估计，然而，存在多个不可观的自由度(绝对位置、偏航角)，只能实现相对定位，无法完全胜任基础设施自主检测任务(如已知RTK坐标的基础设施重复自主检测)。RTK作为一种高精度绝对定位方案虽然受到设施遮挡等不利因素的影响，但是与VIO具有很好的互补性，如何设计合适的算法融合间断性的RTK信息与视觉惯性里程计实现高精度绝对定位是具有重要意义的研究问题。虽然GPS与视觉惯性里程计融合的受到广泛研究，但是少有专门针对基础设施自主检测场景下间断性RTK信号与视觉惯性里程计通过非线性优化进行融合的研究。
“Intermittent GPS-aided VIO:Online Initialization and Calibration”一文，考虑了间断性GPS与视觉惯性里程计的融合，采用了多状态约束扩展卡尔曼滤波来实现视觉惯性信息的融合，使用带约束最小二乘优化实现视觉惯性里程计与GPS融合的初始化，并在随后利用卡尔曼滤波实现融合更新以及时空校准。然而，由于没有考虑长时间无GPS的情况，因此，在GPS间断时间较长情况下的融合会由于视觉惯性里程计的长时间漂移而使得融合是产生不连续的定位结果，进而影响飞行器的安全性和作业的稳定性。
“A general optimization-based framework for local odometry estimationwith multiple sensors”一文，提出了一个非线性优化框架，使用非线性因子图优化实现了GPS与视觉惯性的紧耦合融合方法，但是由于没有考虑GPS的间断性，且运算量较大，无法对基础设施自主检测场景下的无人机提供实时的绝对定位。
现有的GPS融合视觉惯性里程计的方法的虽然在普通室外场景可以实现较好的精度，但是大都没有考虑间断性GPS的情况，且没有考虑融合过程中位姿的剧烈变化对无人机控制系统稳定性带来的影响，容易在融合过程中导致飞机控制系统的不稳定甚至发散。另外，已有方法所用带约束优化模型实际上可以表示为更加简化的无约束优化模型，因此可以用一种更加统一的框架加以解决。
发明内容
为了克服现有技术的不足，本发明提供了一种视觉惯性里程计与间断性RTK融合的绝对定位方法，首先进行RTK-VIO系统初始化，完成RTK所在地球地理坐标系以及VIO坐标系和ENU坐标系之间的转换；然后获取当前位置的RTK值和VIO值，并转换到ENU坐标系；接下里利用VIO所估计位姿与RTK所估计位姿构建观测残差优化问题；构建滑动窗口，将步骤3所得残差按优质RTK信号发生事件的时间顺序加入到滑动窗口中，进行滚动优化，得到一个最新的校准事件；最终将校准事件平滑化，保证无人机的安全稳定飞行。本发明弥补了视觉惯性里程计误差累积和RTK受间歇性遮挡无法准确定位的缺点，实现了相对地理坐标系切空间的实时鲁棒的绝对定位。
本发明解决其技术问题所采用的技术方案包括如下步骤：
步骤1：RTK-VIO系统初始化；
步骤1-1：RTK以地球地理坐标系作为参考系，以经度、纬度和椭球高度表示；
VIO坐标系为局部笛卡尔坐标系，以无人机起始点为坐标原点，坐标系Z轴与水平面垂直，以三维向量表示位置，以四元数表示姿态；
步骤1-2：选择待检测设施周围任一点作为原点，建立东北天坐标系，即ENU坐标系；将ENU坐标系作为RTK-VIO系统的全局参考坐标系；ENU坐标系的原点在地球地理坐标系中具有固定的经度、纬度和椭球高度；
步骤1-3：使用公式组(1)和(2)将地球地理坐标系下的经度、纬度和椭球高度转化到以地球为中心的地球固定坐标系下，得到地球固定坐标系的坐标(X,Y,Z)T；
Figure BDA0003444260000000031
其中，
Figure BDA0003444260000000032
λ、h分别地球为地理坐标系的经度、纬度和椭球高度；a和b分别是地球的赤道半径和极半径；
Figure BDA0003444260000000033
采用公式(3)将地球固定坐标系的(X,T,Z)T转化为ENU坐标系下的坐标Gp＝(x,y,z)T：
Figure BDA0003444260000000034
其中，(Xr,Yr,Zr)TT为ENU坐标系的原点在地球固定坐标系的坐标，
Figure BDA0003444260000000035
λr分别为ENU坐标系的原点在地球地理坐标系下的经度和纬度；
步骤1-4：设在无人机运动过程中，ENU坐标系下的RTK在n个观测点的位置估计为
Figure BDA0003444260000000036
VIO坐标系下相同时刻的VIO位置估计为
Figure BDA0003444260000000037
则有：
Figure BDA0003444260000000038
其中，GpV为VIO坐标系到ENU坐标系的平移向量，
Figure BDA0003444260000000039
为VIO坐标系到ENU坐标系的旋转矩阵；
Figure BDA00034442600000000310
表示如式(5)：
Figure BDA00034442600000000311
其中，γ为VIO坐标系到ENU坐标系的偏航角；
步骤1-5：定义
Figure BDA00034442600000000312
Figure BDA00034442600000000313
和
Figure BDA00034442600000000314
表示两个相邻观测点i和j的RTK位置估计，
Figure BDA00034442600000000315
和
Figure BDA00034442600000000316
表示两个相邻观测点i和j在VIO坐标系下对应时刻的VIO位置估计；
设置权重因子
Figure BDA0003444260000000041
ΔTj表示第j个采样时间；
根据式(6)计算第i个观测点VIO坐标系到ENU坐标系的偏航角γi的正弦值和余弦值：
Figure BDA0003444260000000042
取无人机开始运动后的最初N1个观测点，得到N1个偏航角的正弦值和余弦值，使用权重因子ηj对N1个偏航角的正弦值和余弦值分别加权平均并归一化，得到估计量sinγ和cosγ，再利用反正切函数得到最终偏航角γ；将γ代入式(5)得到旋转矩阵
Figure BDA0003444260000000043
的估计量
Figure BDA0003444260000000044
再将旋转矩阵
Figure BDA0003444260000000045
的估计量
Figure BDA0003444260000000046
多次代入式(4)，取平均求出GpV的估计值
Figure BDA0003444260000000047
完成RTK-VIO系统的初始化；
步骤2：观测值构建；
获取当前无人机的RTK定位值，使用式(1)到式(3)转换到ENU坐标系中，得到RTK观测量
Figure BDA0003444260000000048
设定滑动窗口长度为N2，将RTK观测量
Figure BDA0003444260000000049
与滑动窗口内时间最早的RTK观测量作差求得位移观测量
Figure BDA00034442600000000410
获取当前无人机的VIO定位值，并利用步骤1得到的
Figure BDA00034442600000000411
将VIO坐标系下的VIO定位值转换到ENU坐标系中，得到VIO观测量
Figure BDA00034442600000000412
再将VIO观测量
Figure BDA00034442600000000413
与滑动窗口内时间最早的VIO观测量作差求得位移观测量
Figure BDA00034442600000000414
步骤3：构建优化问题；
待估计的状态量为
Figure BDA00034442600000000415
由第i次位置观测和位移观测构造残差，如式(7)所示：
Figure BDA00034442600000000416
将所有残差进行组合，构建能量函数E(χ)如式(8)：
Figure BDA00034442600000000417
使用Ceres最小二乘求解器迭代求解χ，对残差进行扰动，推导残差雅可比如式(9)：
Figure BDA0003444260000000051
其中，(·)∧表示求反对称阵算子，Exp(·)＝exp(·)∧，为定义在
Figure BDA0003444260000000052
上的指数映射；δp表示位移扰动，δφ表示旋转扰动；
选择χ的优化初始值为步骤1所得
Figure BDA0003444260000000053
令
Figure BDA0003444260000000054
δφ的更新方式为：
Figure BDA0003444260000000055
其中M的形式如式(10)：
Figure BDA0003444260000000056
步骤4：滚动优化；
在能量函数中加入边缘化先验残差rprior：
Figure BDA0003444260000000057
其中，先验残差rprior分为两项，即姿态变换残差和位置变换残差，定义为：
Figure BDA0003444260000000058
其中，Log(·)＝log(·)∨，为定义在SO(3)上的对数映射；
使用最小二乘优化求解优化问题，按照RTK定位观测点的时间顺序在滑动窗口中计算，得到一个最新的待估计状态量，即一个校准事件；每当有新的状态量被获取，则剔除滑动窗口中时间最旧的状态量；
步骤5：校准事件平滑化；
定义每个采样周期的最小偏航角矫正量ωmin和最小位置矫正量vmin，最大偏航角矫正量ωmax和最大位置矫正量vmax；
当偏航角矫正量和位置矫正量均小于每个采样周期的最小偏航角矫正量ωmin和最小位置矫正量vmin，则直接予以更新；
当偏航角矫正量介于最小和最大偏航角量矫正量之间，位置矫正量介于最小和最大位置矫正量之间，使用式(13)更新矫正量；
Figure BDA0003444260000000061
其中，γcur代表当前的偏航角估计，
Figure BDA0003444260000000062
分别代表估计得到的最新偏航角和上一次估计得到的偏航角，即
Figure BDA0003444260000000063
pcur代表当前的位移量估计，
Figure BDA0003444260000000064
分别代表估计得到的最新位移量和上一次估计得到的位移量；ω表示偏航角矫正量，v表示位置矫正量；
实现校准事件的平滑化，保证无人机的安全稳定飞行。
优选地，所述N1＝30，N2＝50。
本发明的有益效果如下：
本发明提出了一种基于统一的无约束非线性优化的间断性RTK与视觉惯性里程计融合的定位导航方法，具有在RTK信号受遮挡环境下实现实时、连续、稳定的绝对定位的能力，解决了基础设施自主检测等工业场景中无法实时准确获取机器人全局位置信息的问题。由于本发明所提定位方法可以实现绝对定位，不受累积误差的影响，故自主检测机器人可以实现对同一设施的多次重复检测，而无需每次检测时都重新执行勘测环境、线规划路径、结构搜索覆盖等繁杂的工作。针对同一个固定设施这些计算量、时间、人力消耗巨大的任务只需要在第一次检测时一次，大大节约了成本。
本发明弥补了视觉惯性里程计误差累积和RTK受间歇性遮挡无法准确定位的缺点，实现了相对地理坐标系切空间的实时鲁棒的绝对定位；在融合过程中考虑了位姿调整的平滑性以保证飞机的安全性和稳定性；并在仿真环境和真实的无人机桥梁检测环境中验证了所提方法的有效性。
附图说明
图1为本发明方法坐标系转化示意图。
图2为本发明实施例仿真环境下桥梁模型和规划轨迹。
图3为本发明实施例实际环境中桥梁位置和飞行轨迹。
图4为本发明实施例实际检测场景和无人机飞行效果。
具体实施方式
下面结合附图和实施例对本发明进一步说明。
一种视觉惯性里程计与间断性RTK融合的绝对定位方法，包括如下步骤：
步骤1：RTK-VIO系统初始化；
步骤1-1：RTK以地球地理坐标系作为参考系，以经度、纬度和椭球高度表示；
VIO坐标系为局部笛卡尔坐标系，以无人机起始点为坐标原点，坐标系Z轴与水平面垂直，以三维向量表示位置，以四元数表示姿态；
步骤1-2：选择待检测设施周围任一点作为原点，建立东北天坐标系，即ENU坐标系；将ENU坐标系作为RTK-VIO系统的全局参考坐标系；ENU坐标系的原点在地球地理坐标系中具有固定的经度、纬度和椭球高度；
步骤1-3：使用公式组(1)和(2)将地球地理坐标系下的经度、纬度和椭球高度转化到以地球为中心的地球固定坐标系下，得到地球固定坐标系的坐标(X,Y,Z)T；
Figure BDA0003444260000000071
其中，
Figure BDA0003444260000000072
λ、h分别地球为地理坐标系的经度、纬度和椭球高度；a和b分别是地球的赤道半径和极半径；
Figure BDA0003444260000000073
采用公式(3)将地球固定坐标系的(X,Y,Z)T转化为ENU坐标系下的坐标Gp＝(x,y,z)T：
Figure BDA0003444260000000074
其中，(Xr,Yr,Zr)T为ENU坐标系的原点在地球固定坐标系的坐标，
Figure BDA0003444260000000075
λr分别为ENU坐标系的原点在地球地理坐标系下的经度和纬度；
步骤1-4：设在无人机运动过程中，ENU坐标系下的RTK在n个观测点的位置估计为
Figure BDA0003444260000000076
VIO坐标系下相同时刻的VIO位置估计为
Figure BDA0003444260000000077
则有：
Figure BDA0003444260000000081
其中，GpV为VIO坐标系到ENU坐标系的平移向量，
Figure BDA0003444260000000082
为VIO坐标系到ENU坐标系的旋转矩阵；
Figure BDA0003444260000000083
表示如式(5)：
Figure BDA0003444260000000084
其中，γ为VIO坐标系到ENU坐标系的偏航角；
步骤1-5：定义
Figure BDA0003444260000000085
Figure BDA0003444260000000086
和
Figure BDA0003444260000000087
表示两个相邻观测点i和j的RTK位置估计，
Figure BDA0003444260000000088
和
Figure BDA0003444260000000089
表示两个相邻观测点i和j在VIO坐标系下对应时刻的VIO位置估计；
设置权重因子
Figure BDA00034442600000000810
ΔTj表示第j个采样时间；
根据式(6)计算第i个观测点VIO坐标系到ENU坐标系的偏航角γi的正弦值和余弦值：
Figure BDA00034442600000000811
取无人机开始运动后的最初30个观测点，得到30个偏航角的正弦值和余弦值，使用权重因子ηj对30个偏航角的正弦值和余弦值分别加权平均并归一化，得到估计量sinγ和cosγ，再利用反正切函数得到最终偏航角γ；将γ代入式(5)得到旋转矩阵
Figure BDA00034442600000000812
的估计量
Figure BDA00034442600000000813
再将旋转矩阵
Figure BDA00034442600000000814
的估计量
Figure BDA00034442600000000815
多次代入式(4)，取平均求出GpV的估计值
Figure BDA00034442600000000816
完成RTK-VIO系统的初始化；
步骤2：观测值构建；
获取当前无人机的RTK定位值，使用式(1)到式(3)转换到ENU坐标系中，得到RTK观测量
Figure BDA00034442600000000817
设定滑动窗口长度为50，将RTK观测量
Figure BDA00034442600000000818
与滑动窗口内时间最早的RTK观测量作差求得位移观测量
Figure BDA00034442600000000819
获取当前无人机的VIO定位值，并利用步骤1得到的
Figure BDA0003444260000000091
将VIO坐标系下的VIO定位值转换到ENU坐标系中，得到VIO观测量
Figure BDA0003444260000000092
再将VIO观测量
Figure BDA0003444260000000093
与滑动窗口内时间最早的VIO观测量作差求得位移观测量
Figure BDA0003444260000000094
步骤3：构建优化问题；
待估计的状态量为
Figure BDA0003444260000000095
由第i次位置观测和位移观测构造残差，如式(7)所示：
Figure BDA0003444260000000096
将所有残差进行组合，构建能量函数E(χ)如式(8)：
Figure BDA0003444260000000097
使用Ceres最小二乘求解器迭代求解χ，对残差进行扰动，推导残差雅可比如式(9)：
Figure BDA0003444260000000098
其中，(·)∧表示求反对称阵算子，Exp(·)＝exp(·)∧，为定义在
Figure BDA0003444260000000099
上的指数映射；
选择χ的优化初始值为步骤1所得
Figure BDA00034442600000000910
令
Figure BDA00034442600000000911
δφ的更新方式为：
Figure BDA00034442600000000912
其中M的形式如式(10)：
Figure BDA00034442600000000913
步骤4：滚动优化；
在能量函数中加入边缘化先验残差rprior：
Figure BDA00034442600000000914
其中，先验残差rprior分为两项，即姿态变换残差和位置变换残差，定义为：
Figure BDA0003444260000000101
其中，Log(·)＝log(·)∨，为定义在SO(3)上的对数映射；
使用最小二乘优化求解优化问题，按照RTK定位观测点的时间顺序在滑动窗口中计算，得到一个最新的待估计状态量，即一个校准事件；每当有新的状态量被获取，则剔除滑动窗口中时间最旧的状态量；
步骤5：校准事件平滑化；
定义每个采样周期的最小偏航角矫正量ωmin和最小位置矫正量vmin，最大偏航角矫正量ωmax和最大位置矫正量vmax；
当偏航角矫正量和位置矫正量均小于每个采样周期的最小偏航角矫正量ωmin和最小位置矫正量vmin，则直接予以更新；
当偏航角矫正量介于最小和最大偏航角量矫正量之间，位置矫正量介于最小和最大位置矫正量之间，使用式(13)更新矫正量；
Figure BDA0003444260000000102
其中，γcur代表当前的偏航角估计，
Figure BDA0003444260000000103
分别代表估计得到的最新偏航角和上一次估计得到的偏航角，即
Figure BDA0003444260000000104
实现校准事件的平滑化，保证无人机的安全稳定飞行。
具体实施例：
1、RTK-VIO初始化。RTK所获定位信息与VIO所估计定位信息位于不同的参考坐标系内，RTK以地球地理坐标系作为参考系，分别以经度、纬度和椭球高度表示，而VIO坐标系通常是一个局部笛卡尔坐标系，以起始点为坐标原点，坐标系Z轴与水平面垂直，以三维向量来表示位置和以四元数来表示姿态。为了将二者融合，首先需要将之转化到统一参考系下，且要实现绝对定位，必须令所转化坐标系能够固定在地球上某一点，并在不同次运行时保持不变。根据习惯，选择距离待检测设施较近的某一固定经纬度和椭球高度的点作为参考点，并建立东北天(East-North-Up,ENU)坐标系，作为RTK-VIO的全局参考系。如图1所示，首先利用公式组(1)和(2)将大地坐标系下的经度、纬度和椭球高度转化到以地球中心的地球固定坐标系(Earth-centered,Earth-fixed coordinate system,ECEF)下得到坐标(X,Y,X)T
随后利用公式(3)ECEF坐标系下的(X,Y,Z)T被转化为以固定参考点为原点的正切于椭球面的ENU坐标系下的坐标Gp＝(x,y,z)T。
在完成坐标系转化后，要估计VIO坐标系与RTK-VIO坐标系之间的相对变换关系。首先标定得到RTK传感器与IMU的外参，并利用外参将所有RTK与VIO估计的结果转化到Body系，按照REP105规定，将Body系规定为一个固定于机体的局部前-左-上坐标系(Forward-Left-Up，FLU)。设一系列ENU坐标系下的RTK测量结果为
Figure BDA0003444260000000111
VIO坐标系下对应时刻的VIO位置估计为
Figure BDA0003444260000000112
则有：
Figure BDA0003444260000000113
其中，GpV为VIO坐标系到ENU坐标系之间的平移向量，
Figure BDA0003444260000000114
为VIO坐标系到ENU坐标系之间的旋转矩阵。实际上，由于VIO在姿态估计上只有一个偏航角不可观，只需要对偏航角进行对齐即可，无需考虑俯仰和滚转这两个自由度，即
Figure BDA0003444260000000115
可表示如式(5)。
Figure BDA0003444260000000116
通过多对RTK估计位置差(位移)向量和VIO估计位置差(位移)向量可以计算出偏航角γ。取每个采样时间大于0.5s且位移量大于0.5m且信号质量较好的相邻RTK测量信息，并取位移量的(x,y)分量。即
Figure BDA0003444260000000117
则根据式(4)和式(5)可得关于γ的正余弦函数，如式(6)所示。取最初的30个观测点，得到偏航角正余弦量的加权平均值并归一化，得到最终的估计量(sinγ,cosγ)，其中，权重因子η设置为反比于采样时间并正比于位移量的长度，即
Figure BDA0003444260000000118
并利用反正切函数得到最终偏航角γ，代入式(5)得到
Figure BDA0003444260000000119
估计量
Figure BDA00034442600000001110
Figure BDA0003444260000000121
在初始化
Figure BDA0003444260000000122
之后，将其代入式(4)求均值，可求出GpV的估计值
Figure BDA0003444260000000123
至此，RTK-VIO系统完成初始化。
2、观测值构建。设置一独立线程，线程在平时处于休眠状态，基本不占用系统计算资源，当RTK信号质量达标且收敛时线程被唤醒，首先获取当前的RTK定位数值，并将之使用式(1)-(3)转换到初始化时建立的ENU坐标系中，得到RTK观测量
Figure BDA0003444260000000124
并将之与当前保存最老观测求位移观测量
Figure BDA0003444260000000125
同时主线程获取VIO的定位结果，并利用
Figure BDA0003444260000000126
和式(4)，将VIO坐标系下所估计位姿转换到ENU坐标系下，得到VIO观测量
Figure BDA0003444260000000127
同理得到位移观测量
Figure BDA0003444260000000128
3、构建优化问题。待估计的状态量为
Figure BDA0003444260000000129
根据约束关系得第i次位置观测和位移观测构造残差，如式(7)所示，
Figure BDA00034442600000001210
将所有残差进行组合，构建能量函数如下式：
Figure BDA00034442600000001211
使用Ceres最小二乘求解器迭代求解χ。对残差进行扰动，推导残差雅可比如下式：
Figure BDA00034442600000001212
其中，(·)∧表示求反对称阵算子，Exp(·)＝exp(·)∧，为定义在
Figure BDA00034442600000001213
上的指数映射。选择χ的优化初始值为步骤1所得
Figure BDA00034442600000001214
由于VIO在姿态估计上仅有偏航角不可观，在上述优化过程中，计算出的更新量δφ的更新方式为：
Figure BDA0003444260000000131
其中M的形式如下式：
Figure BDA0003444260000000132
4、时序事件滚动优化。在进行最小二乘优化求解状态量时，为了同时保持状态量的估计精度和计算效率，构建了大小为50的滑动窗口，将步骤3所得观测按优质RTK信号发生事件的时间顺序加入到滑动窗口中，进行最小二乘优化，得到一个最新的校准事件。当滑动窗口中的观测量达到50组时，每当有新的观测量被获取，则剔除滑动窗口中最旧的观测。然而，直接剔除会损失精度，因此每次剔除掉一个观测之后，都会在优化问题上保留了一个观测先验这个先验包含了过去被剔除的观测对状态量的估计，具有限制偶然因素导致的状态量突变的效果。加入边缘化先验项之后的能量函数变为：
Figure BDA0003444260000000133
其中，先验残差分为两项，即姿态变换残差和位置变换残差，定义为：
Figure BDA0003444260000000134
其中，Log(·)＝log(·)∨，为定义在SO(3)上的对数映射。
5、校准事件平滑化。由于两次相邻的RTK校准可能间隔时间会比较长，VIO经过较长时间后累积偏移可能较大，此时如果直接用滑窗优化得到的最新状态量估计，会使最终应用于飞控的里程计信息变化产生较大不连续性。因此，每当滑窗得到一个新的估计，主线程不会立即更新飞控所用里程计的估计值，而是将新估计值逐步应用于里程计。同时，需要保证里程计本身的估计值变化得到即时更新。
如图2到图4所示，本实施例将偏航角矫正量和位置矫正量分开进行平滑，设置最小的偏航角矫正速度为π/60rad/s，最小的位置矫正速度为0.1m/s。由于RTK-VIO的估计频率为f＝50hz，因此，每个估计周期的最小偏航角量和最小位置矫正量分别为ωmin＝π/3000rad和vmin＝0.002m。当VIO本身估计值变化较快时，则相对减小矫正量的更新速度，反之，则相对增大更新速度。但是最大更新速度不超过π/12rad/s和0.5m/s，同理可得每个估计周期最大矫正量，设为ωmax,vmax。当矫正量小于每估计周期的最小矫正速度，则直接予以更新。当介于两者之间时则使用式(13)更新矫正量。
Figure BDA0003444260000000141
其中，γcur代表当前的偏航角估计，
Figure BDA0003444260000000142
分别代表估计得到的最新偏航角和上一次估计得到的偏航角，即
Figure BDA0003444260000000143
同理，
Figure BDA0003444260000000144
代表当前的位置偏差估计，这些量都由滚动优化滑窗线程给出；而式(13)中用这些量建立的分式则指明了更新的方向。sigmoid函数起到根据里程计偏航角和位置每周期变化量与最大矫正量的相对大小来控制更新速度幅值的作用。最大的更新速度幅值可以保证不超过每周期的最大允许矫正量。由此，实现了校准事件的平滑化，保证了无人机的安全稳定飞行。

Claims (2)

1.一种视觉惯性里程计与间断性RTK融合的绝对定位方法，其特征在于，包括如下步骤：
步骤1：RTK-VIO系统初始化；
步骤1-1：RTK以地球地理坐标系作为参考系，以经度、纬度和椭球高度表示；
VIO坐标系为局部笛卡尔坐标系，以无人机起始点为坐标原点，坐标系Z轴与水平面垂直，以三维向量表示位置，以四元数表示姿态；
步骤1-2：选择待检测设施周围任一点作为原点，建立东北天坐标系，即ENU坐标系；将ENU坐标系作为RTK-VIO系统的全局参考坐标系；ENU坐标系的原点在地球地理坐标系中具有固定的经度、纬度和椭球高度；
步骤1-3：使用公式组(1)和(2)将地球地理坐标系下的经度、纬度和椭球高度转化到以地球为中心的地球固定坐标系下，得到地球固定坐标系的坐标
Figure FDA0003444259990000018
Figure FDA0003444259990000011
其中，
Figure FDA0003444259990000012
λ、h分别地球为地理坐标系的经度、纬度和椭球高度；a和b分别是地球的赤道半径和极半径；
Figure FDA0003444259990000013
采用公式(3)将地球固定坐标系的
Figure FDA0003444259990000019
转化为ENU坐标系下的坐标
Figure FDA00034442599900000111
Figure FDA00034442599900000110
Figure FDA0003444259990000014
其中，
Figure FDA00034442599900000112
为ENU坐标系的原点在地球固定坐标系的坐标，
Figure FDA0003444259990000015
λr分别为ENU坐标系的原点在地球地理坐标系下的经度和纬度；
步骤1-4：设在无人机运动过程中，ENU坐标系下的RTK在n个观测点的位置估计为
Figure FDA0003444259990000016
VIO坐标系下相同时刻的VIO位置估计为
Figure FDA0003444259990000017
则有：
Figure FDA0003444259990000021
其中，GpV为VIO坐标系到ENU坐标系的平移向量，
Figure FDA0003444259990000022
为VIO坐标系到ENU坐标系的旋转矩阵；
Figure FDA0003444259990000023
表示如式(5)：
Figure FDA0003444259990000024
其中，γ为VIO坐标系到ENU坐标系的偏航角；
步骤1-5：定义
Figure FDA0003444259990000025
Figure FDA0003444259990000026
和
Figure FDA0003444259990000027
表示两个相邻观测点i和j的RTK位置估计，
Figure FDA0003444259990000028
和
Figure FDA0003444259990000029
表示两个相邻观测点i和j在VIO坐标系下对应时刻的VIO位置估计；
设置权重因子
Figure FDA00034442599900000210
ΔTj表示第j个采样时间；
根据式(6)计算第i个观测点VIO坐标系到ENU坐标系的偏航角γi的正弦值和余弦值：
Figure FDA00034442599900000211
取无人机开始运动后的最初N1个观测点，得到N1个偏航角的正弦值和余弦值，使用权重因子ηj对N1个偏航角的正弦值和余弦值分别加权平均并归一化，得到估计量sinγ和cosγ，再利用反正切函数得到最终偏航角γ；将γ代入式(5)得到旋转矩阵
Figure FDA00034442599900000212
的估计量
Figure FDA00034442599900000213
再将旋转矩阵
Figure FDA00034442599900000214
的估计量
Figure FDA00034442599900000215
多次代入式(4)，取平均求出GpV的估计值
Figure FDA00034442599900000216
完成RTK-VIO系统的初始化；
步骤2：观测值构建；
获取当前无人机的RTK定位值，使用式(1)到式(3)转换到ENU坐标系中，得到RTK观测量
Figure FDA00034442599900000217
设定滑动窗口长度为N2，将RTK观测量
Figure FDA00034442599900000218
与滑动窗口内时间最早的RTK观测量作差求得位移观测量
Figure FDA00034442599900000219
获取当前无人机的VIO定位值，并利用步骤1得到的
Figure FDA0003444259990000031
将VIO坐标系下的VIO定位值转换到ENU坐标系中，得到VIO观测量
Figure FDA0003444259990000032
再将VIO观测量
Figure FDA0003444259990000033
与滑动窗口内时间最早的VIO观测量作差求得位移观测量
Figure FDA0003444259990000034
步骤3：构建优化问题；
待估计的状态量为
Figure FDA0003444259990000035
由第i次位置观测和位移观测构造残差，如式(7)所示：
Figure FDA0003444259990000036
将所有残差进行组合，构建能量函数E(χ)如式(8)：
Figure FDA0003444259990000037
使用Ceres最小二乘求解器迭代求解χ，对残差进行扰动，推导残差雅可比如式(9)：
Figure FDA0003444259990000038
其中，(·)∧表示求反对称阵算子，Exp(·)＝exp(·)∧，为定义在
Figure FDA0003444259990000039
上的指数映射；δp表示位移扰动，δφ表示旋转扰动；
选择χ的优化初始值为步骤1所得
Figure FDA00034442599900000310
令
Figure FDA00034442599900000311
δφ的更新方式为：
Figure FDA00034442599900000312
其中M的形式如式(10)：
Figure FDA00034442599900000313
步骤4：滚动优化；
在能量函数中加入边缘化先验残差rprior：
Figure FDA00034442599900000314
其中，先验残差rprior分为两项，即姿态变换残差和位置变换残差，定义为：
Figure FDA0003444259990000041
其中，Log(·)＝log(·)∨，为定义在SO(3)上的对数映射；
使用最小二乘优化求解优化问题，按照RTK定位观测点的时间顺序在滑动窗口中计算，得到一个最新的待估计状态量，即一个校准事件；每当有新的状态量被获取，则剔除滑动窗口中时间最旧的状态量；
步骤5：校准事件平滑化；
定义每个采样周期的最小偏航角矫正量ωmin和最小位置矫正量vmin，最大偏航角矫正量ωmax和最大位置矫正量vmax；
当偏航角矫正量和位置矫正量均小于每个采样周期的最小偏航角矫正量ωmin和最小位置矫正量vmin，则直接予以更新；
当偏航角矫正量介于最小和最大偏航角量矫正量之间，位置矫正量介于最小和最大位置矫正量之间，使用式(13)更新矫正量；
Figure FDA0003444259990000042
其中，γcur代表当前的偏航角估计，
Figure FDA0003444259990000043
分别代表估计得到的最新偏航角和上一次估计得到的偏航角，即
Figure FDA0003444259990000044
pcur代表当前的位移量估计，
Figure FDA0003444259990000045
分别代表估计得到的最新位移量和上一次估计得到的位移量；ω表示偏航角矫正量，v表示位置矫正量；
实现校准事件的平滑化，保证无人机的安全稳定飞行。
2.根据权利要求1所述的一种视觉惯性里程计与间断性RTK融合的绝对定位方法，其特征在于，所述N1＝30，N2＝50。
CN202111642896.XA 2021-12-29 2021-12-29 一种视觉惯性里程计与间断性rtk融合的绝对定位方法 Pending CN114543794A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111642896.XA CN114543794A (zh) 2021-12-29 2021-12-29 一种视觉惯性里程计与间断性rtk融合的绝对定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111642896.XA CN114543794A (zh) 2021-12-29 2021-12-29 一种视觉惯性里程计与间断性rtk融合的绝对定位方法

Publications (1)

Publication Number Publication Date
CN114543794A true CN114543794A (zh) 2022-05-27

Family

ID=81669616

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111642896.XA Pending CN114543794A (zh) 2021-12-29 2021-12-29 一种视觉惯性里程计与间断性rtk融合的绝对定位方法

Country Status (1)

Country Link
CN (1) CN114543794A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116985144A (zh) * 2023-09-26 2023-11-03 珞石（北京）科技有限公司 一种具有c2连续的机器人末端姿态规划方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116985144A (zh) * 2023-09-26 2023-11-03 珞石（北京）科技有限公司 一种具有c2连续的机器人末端姿态规划方法

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
Li et al. LIDAR/MEMS IMU integrated navigation (SLAM) method for a small UAV in indoor environments
CN106289246B (zh) 一种基于位置和姿态测量系统的柔性杆臂测量方法
Madyastha et al. Extended Kalman filter vs. error state Kalman filter for aircraft attitude estimation
CN108036785A (zh) 一种基于直接法与惯导融合的飞行器位姿估计方法
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
Ahn et al. Fast alignment using rotation vector and adaptive Kalman filter
CN105094138A (zh) 一种用于旋翼无人机的低空自主导航系统
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN103900576A (zh) 一种深空探测自主导航的信息融合方法
CN105865452A (zh) 一种基于间接卡尔曼滤波的移动平台位姿估计方法
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN115855062A (zh) 一种室内移动机器人自主建图与路径规划方法
Park et al. Design and performance validation of integrated navigation system based on geometric range measurements and GIS map for urban aerial navigation
Yang et al. Multilayer low-cost sensor local-global filtering fusion integrated navigation of small UAV
Rhudy et al. Wide-field optical flow aided inertial navigation for unmanned aerial vehicles
CN114543794A (zh) 一种视觉惯性里程计与间断性rtk融合的绝对定位方法
Liu et al. Multi-UAV cooperative navigation algorithm based on federated filtering structure
Haotian et al. Accurate attitude estimation of HB2 standard model based on QNCF in hypersonic wind tunnel test
Du et al. A low-cost attitude estimation system for UAV application
Hasan et al. Evaluation of a low-cost MEMS IMU for indoor positioning system
CN114111840B (zh) 一种基于组合导航的dvl误差参数在线标定方法
Lee et al. Excavator posture estimation and position tracking system based on kinematics and sensor network to control mist-spraying robot
CN114719858A (zh) 一种基于imu和楼层高度目标补偿的3维定位方法
Qiu et al. Outlier-Robust Extended Kalman Filtering for Bioinspired Integrated Navigation System

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