CN113325454B - 一种基于ArduRover无人车的组合定位方法 - Google Patents

一种基于ArduRover无人车的组合定位方法 Download PDF

Info

Publication number
CN113325454B
CN113325454B CN202110539154.8A CN202110539154A CN113325454B CN 113325454 B CN113325454 B CN 113325454B CN 202110539154 A CN202110539154 A CN 202110539154A CN 113325454 B CN113325454 B CN 113325454B
Authority
CN
China
Prior art keywords
under
carrier
gnss
representing
positioning
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
CN202110539154.8A
Other languages
English (en)
Other versions
CN113325454A (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.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
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 Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN202110539154.8A priority Critical patent/CN113325454B/zh
Publication of CN113325454A publication Critical patent/CN113325454A/zh
Application granted granted Critical
Publication of CN113325454B publication Critical patent/CN113325454B/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
    • 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
    • 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
    • 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/1656—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 passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/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
    • 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

本发明属于导航定位技术领域，公开了一种基于ArduRover无人车的组合定位方法。将VIO定位技术与GNSS定位技术融合，获得在全球地心地固系下的定位结果，以惯性与视觉紧组合的视觉惯性里程计VIO作为基础，将GNSS解算的位置速度作为额外的观测量加入到滤波器之中，作为位置等状态量的绝对约束，通过机器人操作***ROS的运动控制和通讯服务在ArduRover小车上完成定位导航和定位效果的验证。本发明解决了无人小车组合定位效果较差的问题，能够实现高精度定位。

Description

一种基于ArduRover无人车的组合定位方法
技术领域
本发明属于导航定位技术领域，更具体地，涉及一种基于ArduRover无人车的组合定位方法。
背景技术
随着计算机技术与导航技术的发展，各种形态的智能机器人在各种生产活动中逐渐扮演起重要的角色，如物流机器人、服务机器人、无人机，乃至自动驾驶汽车等。对于智能移动机器人而言，如何实现复杂环境下的高精度自主定位导航，是一个需要解决的核心问题。
目前，机器人的自主导航有很多不同的实现手段，但是仍然缺乏成熟的解决方案，可以利用成本相对低廉的传感器在室内外的复杂环境下进行较为稳定的导航定位。传统的高精度GNSS/INS组合导航依赖于良好的硬件和通视环境，而基于单线激光或是视觉的室内机器人导航方案在应用场景方面存在局限性。
全球卫星导航***(GNSS)，可在卫星可视范围内提供快速、精准的定位、授时、导航服务(Positioning Navigation and Timing,PNT)。目前，实时精密单点定位技术相对于实时相对定位(Real-Time Kinematic,RTK)有着全球全天候作业、灵活性强，且不需要建立密集的基准站的特点，而在观测条件好的环境下定位精度首次收敛至厘米级需要的时间也在数历元以内。
惯性导航(Inertial Navigation System,INS)是一门涉及精密机械、计算机技术、微电子、光学、自动控制、材料等多种学科和领域的综合技术，陀螺仪作为惯性导航的核心部件，可实现高精度、高可靠性、低成本、小型化、数字化以及应用领域更加广泛的测量，且随着微机械制造技术的发展，采用微机械陀螺制造的惯性导航***正成为惯性导航的主流趋势。惯性导航通常采用轨迹测量算法，不依靠外部信息，自主式导航***便可获取完整的位置信息，保证测量精度，并可以在信号屏蔽或信号较弱的测绘场合发挥重要作用。
视觉里程计(Visual Odometry)指利用多张相片作为量测值，通过最小二乘优化或卡尔曼滤波等优化框架估计得到量测时刻的位置、速度和姿态。视觉惯性里程计(Visual-Inertial Odometry，VIO)则是在INS的基础上，融合了视觉里程计技术(即在量测部分加入了相片)，目前来说，相片对于姿态估计的贡献最大，可以在很大程度上减小惯性测量单元(Inertial Measurement Unit，IMU)的陀螺零偏发散的速率，大大提升了位姿估计的效果。
然而，无论是视觉里程计(VO)还是视觉惯性里程计(VIO)都为航位推算技术，是在当地的局部坐标系下推算的定位结果，即存在误差累积的问题。
发明内容
本发明通过提供一种基于ArduRover无人车的组合定位方法，解决现有技术中使用视觉惯性里程计算法存在定位发散的问题。
本发明提供一种基于ArduRover无人车的组合定位方法，包括以下步骤：
步骤1、利用全球卫星导航***GNSS定位结果与视觉惯性里程计VIO定位结果进行联合初始对准，获取载体在地心地固系下的运动状态；
步骤2、构建卡尔曼滤波器，利用上一量测时刻到当前时刻之间的惯性测量单元IMU输出的速度增量和姿态增量进行滤波器的时间更新，对新时刻的滤波器的状态向量进行预测，并对滤波器的状态向量的协方差矩阵进行转移；
步骤3、当获取到新时刻的相片时，对滤波器的状态向量进行扩维，将新时刻的相片位置和姿态加入到滤波器的状态向量之中，得到滤波器的总的状态向量；
步骤4、完成所述联合初始对准后，在获取到新的GNSS定位结果时，将GNSS解算输出的位置和速度作为量测值加入到滤波器的量测更新部分，对所述载体在地心地固系下的运动状态进行精对准，得到GNSS/INS/Visual组合定位结果；
步骤5、将所述GNSS/INS/Visual组合定位结果输出到机器人操作***ROS中，通过ROS内置的路径规划服务并结合用户指定的导航目标产生控制指令，控制ArduRover无人车进行定位导航。
优选的，所述步骤1中，所述载体在地心地固系下的运动状态包括：当地系n与世界系w之间的水平偏角
Figure BDA0003071096520000021
以及世界系w在地心地固系e下的绝对位置
Figure BDA0003071096520000022
优选的，所述步骤1中，所述联合初始对准前，VIO输出世界系w下载体的运动状态，GNSS解算不断输出载体在地心地固系e下的位置；
将GNSS输出的载体在地心地固系e下的运动轨迹投影到起点的当地系n下，利用该轨迹与VIO输出轨迹的首尾相对位置的位矢进行水平偏角
Figure BDA0003071096520000023
的计算，如公式(1)所示：
Figure BDA0003071096520000024
其中，
Figure BDA0003071096520000025
表示轨迹终点在当地系n下的坐标，
Figure BDA0003071096520000026
表示轨迹起点在当地系n下的坐标；
Figure BDA0003071096520000031
表示轨迹终点在世界系w下的坐标，
Figure BDA0003071096520000032
表示轨迹起点在世界系w下的坐标；
世界系w在地心地固系e下的绝对位置
Figure BDA0003071096520000033
采用公式(2)计算：
Figure BDA0003071096520000034
其中，
Figure BDA0003071096520000035
表示载体系b在地心地固系e下的绝对位置，
Figure BDA0003071096520000036
表示载体系b在世界系w下的绝对位置，
Figure BDA0003071096520000037
表示当地系n与世界系w之间的水平偏角，
Figure BDA0003071096520000038
表示将世界系w下的坐标转到当地系n下的旋转矩阵，
Figure BDA0003071096520000039
表示将当地系n下的坐标转到地心地固系e下的旋转矩阵,
Figure BDA00030710965200000310
表示将载体系b下的坐标转到世界系w下的旋转矩阵，
Figure BDA00030710965200000311
表示GNSS天线坐标系到载体系原点的偏移量。
优选的，所述步骤1中，所述联合初始对准成功的标准为同时满足第一条件和第二条件；所述第一条件为：载体的直线运动距离超过设定的第一阈值，且位置误差小于设定的第二阈值；所述第二条件为：水平偏角
Figure BDA00030710965200000312
在连续的历元内误差都小于设定的第三阈值。
优选的，所述步骤2中，所述滤波器的状态向量包括当前时刻载体的运动状态、与IMU相关的状态向量。
优选的，所述步骤2中，所述与IMU相关的状态向量表示为：
Figure BDA00030710965200000313
其中，XI表示与IMU相关的状态向量，
Figure BDA00030710965200000314
是载体在世界系w下的姿态，
Figure BDA00030710965200000315
为载体在世界系w下的加速度，
Figure BDA00030710965200000316
为载体在世界系w下的位置，bg和ba分别为IMU的陀螺仪零偏和加速度零偏，
Figure BDA00030710965200000317
和
Figure BDA00030710965200000318
为左相机坐标系与载体系b之间的转换参数，其中，左相机坐标系为以双目相机中左侧相机光心为原点，XYZ轴分别指向右下前方向的坐标系。
优选的，所述步骤3中，与相片相关的状态向量表示为：
Figure BDA00030710965200000319
其中，XC表示与相片相关的状态向量，XCn表示时刻n下与相片相关的状态向量，
Figure BDA00030710965200000320
为第i张相片在世界系w下的姿态，
Figure BDA0003071096520000041
为第i张相片在世界系w下的位置；
所述滤波器的总的状态向量X表示为：
X＝[XI T XC T]T (5)
其中，XC表示与相片相关的状态向量，XI表示与IMU相关的状态向量。
优选的，所述步骤4中，GNSS的位置和速度的观测方程形式如下：
Figure BDA0003071096520000042
其中，zpos和
Figure BDA0003071096520000043
表示载体在地心地固系e下的绝对位置，
Figure BDA0003071096520000044
表示载体在世界系w下的绝对位置，
Figure BDA0003071096520000045
表示当地系n与世界系w之间的水平偏角，
Figure BDA0003071096520000046
表示将世界系w下的坐标转到当地系n下的旋转矩阵，
Figure BDA0003071096520000047
表示将载体系b下的坐标转到世界系w下的旋转矩阵，
Figure BDA0003071096520000048
表示GNSS天线坐标系到载体系原点的偏移量；zvel和
Figure BDA0003071096520000049
为载体在地心地固系e下的绝对速度，
Figure BDA00030710965200000410
表示载体在世界系w下的速度，
Figure BDA00030710965200000411
为改正零偏后的陀螺角速度的叉乘形式。
优选的，所述步骤4中，对所述载体在地心地固系下的运动状态进行精对准时，使用GNSS输出的位置和速度作为滤波器观测值，通过绝对定位，对VIO产生的误差进行修正；
误差方程的表示为：
Figure BDA00030710965200000412
其中，rGNSS为GNSS量测向量，包含zpos和zvel的误差形式；nGNSS为GNSS解算的位置和速度的量测噪声，
Figure BDA00030710965200000413
为残差量对状态量的雅可比矩阵，
Figure BDA00030710965200000414
为状态向量X的误差形式。
优选的，所述步骤4中，所述精对准完成的标准为：航向角标准差小于设定的第四阈值。
本发明中提供的一个或多个技术方案，至少具有如下技术效果或优点：
在发明中，将VIO定位技术与GNSS定位技术融合，获得在全球地心地固系下的定位结果，通过全球坐标系的绝对约束解决了VIO这种相对定位技术的定位发散问题。以惯性与视觉紧组合的视觉惯性里程计VIO作为基础，将全球卫星定位***GNSS解算的位置速度作为额外的观测量加入到滤波器之中，作为位置等状态量的绝对约束，得到更高精度的定位结果，并为无人车***提供在地心地固系下进行定位导航的能力，通过机器人操作***ROS的运动控制和通讯服务在ArduRover小车上完成定位效果的验证，为无人小车自动导航奠定基础。本发明解决了现有技术中无人小车组合定位效果较差的问题，对于高精度无人车的组合定位应用具有重要意义。
附图说明
图1为本发明实施例提供的一种基于ArduRover无人车的组合定位方法的流程示意图；
图2为ArduRover无人车导航流程图；
图3为GNSS和VIO联合初始对准示意图。
具体实施方式
为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。
本实施例提供了一种基于ArduRover无人车的组合定位方法，参见图1，包括以下步骤：
步骤1，利用GNSS定位结果与VIO定位结果进行联合初始对准，获取无人车载体在地心地固系下的运动状态。
本***采用GNSS/VIO联合对准方法对***进行初始对准。在本定位算法中，为了描述载体在地心地固系下的运动状态，对准所要确定的参数为当地系n与世界系w之间的水平偏角
Figure BDA0003071096520000051
(如图3所示)，以及世界系w在地心地固系e下的绝对位置
Figure BDA0003071096520000052
在对准之前，视觉惯性里程计输出世界系w下载体的运动状态，而GNSS解算不断输出载体在e系下的位置，***同时维护这两条不同参考系下载体的运动轨迹。
注意到近似世界系w与当地系n在原则上只相差一个水平偏角
Figure BDA0003071096520000053
本算法将GNSS输出的载体在e系下的运动轨迹投影到起点的当地系n下，利用该轨迹与VIO输出轨迹的首尾相对位置的位矢进行
Figure BDA0003071096520000054
的计算。
Figure BDA0003071096520000055
其中，
Figure BDA0003071096520000056
表示轨迹终点在n系下的坐标，
Figure BDA0003071096520000057
表示轨迹起点在n系下的坐标；
Figure BDA0003071096520000061
表示轨迹终点在w系下的坐标，
Figure BDA0003071096520000062
表示轨迹起点在w系下的坐标。
而世界系w在地心地固系e下的绝对位置
Figure BDA0003071096520000063
可以根据两条轨迹最新时刻的坐标进行近似的计算。
Figure BDA0003071096520000064
其中，
Figure BDA0003071096520000065
表示载体系b在地心地固系e下的绝对位置，
Figure BDA0003071096520000066
表示载体系b在世界系w下的绝对位置，
Figure BDA0003071096520000067
表示当地系n下世界系w的水平偏角，
Figure BDA0003071096520000068
表示将世界系w下的坐标转到当地系n下的旋转矩阵，
Figure BDA0003071096520000069
表示将当地系n下的坐标转到地心地固系e下的旋转矩阵,
Figure BDA00030710965200000610
表示将载体系b下的坐标转到世界系w下的旋转矩阵，
Figure BDA00030710965200000611
表示GNSS天线坐标系到载体系原点的偏移量。
在尝试对准时，算法将会根据以下两个条件判断是否对准成功。条件一：载体直线运动距离超过设定的阈值，且位置误差小于设定的阈值；条件二：水平偏角
Figure BDA00030710965200000614
在连续的历元内误差都小于设定的阈值时，初始对准过程成功。
上述对准算法充分利用了不同传感器的信息，可以在载体有明显水平运动的情况下快速完成粗对准，定向误差在10°以内。
本发明使用了GNSS和VIO联合对准方法，无需额外的辅助信息，通过无人车本身的短时间运动就能得到载体的初始运动状态，从而保证后续组合定位结果的精度。
步骤2，构建卡尔曼滤波器，利用上一量测时刻(或初始时刻)到当前时刻之间的IMU输出值进行滤波器的时间更新，对新时刻的滤波器状态量进行预测，并对状态协方差矩阵进行转移。
构建卡尔曼滤波器，在新的量测时刻到来时，算法利用上一量测时刻(或初始时刻)到当前时刻之间的IMU输出的速度增量和姿态增量进行滤波器的时间更新，对新时刻的状态量进行预测，并对状态协方差矩阵进行转移。
本算法以起点的世界系w作为导航的参考坐标系(称之为世界系是因为参考系的初始水平方位角在没有GNSS量测信息的情况下不可观，该系与真正的ENU当地系n之间相差一个未知的水平偏角。设置世界系w作为参考系是为了便于视觉惯性里程计的推算，同时避免定位模块对于绝对位置的依赖)。从物理含义上讲，这个坐标系与地球是固联的。我们做出如下近似：在一定的运动范围内，载体受到的重力都可以在w系下表示为[0 0 -|g|]T，即忽略了
Figure BDA00030710965200000612
的变化。其中，
Figure BDA00030710965200000613
表示地球自转带来的当地系相对于地心地固系的旋转量。
本算法中，滤波器维护的状态向量包含两部分，一部分是当前时刻载体的运动状态以及与IMU相关的状态
Figure BDA0003071096520000071
其中，XI表示与IMU相关的状态向量，
Figure BDA0003071096520000072
是载体(IMU坐标系)在参考坐标系下的姿态。
Figure BDA0003071096520000073
为载体在参考坐标系下的加速度，
Figure BDA0003071096520000074
载体在参考系下的位置，bg和ba分别为IMU的陀螺仪零偏和加速度零偏，
Figure BDA0003071096520000075
为左相机坐标系与载体坐标系(IMU坐标系)之间的转换参数。其中，左相机坐标系为以双目相机中左侧相机光心为原点，XYZ轴分别指向右下前方向的坐标系。
与一般的VIO***不同的是，为了引入GNSS量测值，本算法额外引入了两个与载体绝对位姿相关的参数，分别是当地系n与世界系w之间的水平偏角
Figure BDA0003071096520000076
以及世界系w在地心地固系e下的绝对位置
Figure BDA0003071096520000077
对于视觉惯性组合而言，这四维参数是不可观测的；而一旦加入GNSS测量值，则可以对这四维参数进行估计。
本发明使用了卡尔曼滤波框架作为状态估计器，扩展了滤波器的状态向量，计算效率高，存储空间耗费低，且可扩展性好，方便引入更多种定位传感器，符合无人车实时导航定位的需求。
步骤3，当获取到新时刻的相片时，对滤波器***状态向量进行扩维，将新时刻的相片位置和姿态加入到滤波器的状态向量之中。
滤波器在加入相片的位置和姿态时，需要扩增状态向量的维度，将相片的位姿引入状态向量中，通过估计得到更精确的相片位姿用以辅助无人车自身的状态估计。
在新相片到来时，将会首先对状态向量进行扩维，将新时刻的相片位姿加入到状态向量之中。任意时刻的相片位姿与载***姿之间存在着近似固联的关系(IMU-相机外参)，新时刻的相片位姿是由时间更新后的IMU状态计算而来。
状态向量的阔维部分是滤波器中维护的过去时刻的相片位姿，用于构建视觉约束
Figure BDA0003071096520000078
其中，XC表示所有时刻下与相片相关状态向量，XCn表示时刻n下与相片相关的的状态向量，
Figure BDA0003071096520000079
为储存的第i张相片在参考坐标系下的姿态，
Figure BDA00030710965200000710
为第i张相片在世界系w下的位置。
总的状态向量可以描述如下
X＝[XI T XC T]T (5)
其中，XC表示与相片相关的状态向量，XI表示与IMU相关的状态向量。
步骤4，在获取到新的GNSS定位结果时，将GNSS解算输出的位置速度作为量测值加入到滤波器的量测更新部分。
在***完成初始对准之后，GNSS解算输出的位置速度将被作为量测值加入到滤波器的量测更新部分。
GNSS位置和速度的观测方程形式如下
Figure BDA0003071096520000081
其中，zpos和
Figure BDA0003071096520000082
表示载体在地心地固系e下的绝对位置，
Figure BDA0003071096520000083
表示载体在世界系w下的位置，
Figure BDA0003071096520000084
表示当地系n下世界系w的水平偏角，
Figure BDA0003071096520000085
表示将世界系w下的坐标转到当地系n下的旋转矩阵，
Figure BDA0003071096520000086
表示将载体系下的坐标转到世界系下的旋转矩阵，
Figure BDA0003071096520000087
表示GNSS天线坐标系到载体系原点的偏移量；zvel和
Figure BDA0003071096520000088
为载体在地心地固系e下的绝对速度，
Figure BDA0003071096520000089
表示载体在世界系w下的速度，
Figure BDA00030710965200000810
为改正零偏后的陀螺角速度的叉乘形式。
误差方程的形式为
Figure BDA00030710965200000811
其中，rGNSS为GNSS量测向量，包含zpos和zvel的误差形式；GNSS为GNSS解算的位置和速度的量测噪声，可以由GNSS解算程序给出；
Figure BDA00030710965200000812
为状态向量X的误差形式；
Figure BDA00030710965200000813
为残差量对状态量的雅可比矩阵，其具体形式在此给出：
Figure BDA00030710965200000814
其中，GNSS解算的位置和速度分别对各状态量的偏导数的具体形式如下，其中，
Figure BDA00030710965200000815
表示zpos对
Figure BDA00030710965200000816
的导数，
Figure BDA00030710965200000817
表示zpos对
Figure BDA00030710965200000818
的导数，
Figure BDA00030710965200000819
表示zpos对
Figure BDA00030710965200000820
的导数，
Figure BDA00030710965200000821
表示将世界系w下的坐标转到载体系b下的旋转矩阵,
Figure BDA0003071096520000091
表示zvel对
Figure BDA0003071096520000092
的导数,
Figure BDA0003071096520000093
表示zvel对
Figure BDA0003071096520000094
的导数,
Figure BDA0003071096520000095
表示zvel对
Figure BDA0003071096520000096
的导数。
Figure BDA0003071096520000097
Figure BDA0003071096520000098
量测更新的步骤即为标准的卡尔曼滤波量测更新，具体见步骤3。
本发明使用了GNSS输出的位置和速度作为滤波器观测值，通过绝对定位，对VIO本身产生的误差进行修正，原因在于在粗对准之后，***会将新的GNSS量测值引入到滤波器之中，进一步对
Figure BDA0003071096520000099
与
Figure BDA00030710965200000910
进行精对准，直到收敛——航向角标准差小于设定的阈值。此后，GNSS量测值将会为定位算法提供可靠的绝对约束。使得无人车在GNSS信号失锁时期间也能长时间维持所需的定位精度。
步骤5，将以上GNSS/INS/Visual组合定位结果输出到ROS中，通过其内置的路径规划服务并结合用户指定的导航目标产生控制指令，并通过ArduRover无人车实际运动状态验证定位效果。
在嵌入式计算机上实现GNSS/INS/Visual组合定位，并结合ROS发出运动控制指令，在ArduRover无人车上完成高精度定位效果的验证。
ROS导航控制模块由核心导航包、导航包所需输入、机器人底盘组成。
核心导航包为ROS***自带开源算法，接收地图服务器提供的地图和GNSS/INS/Visual组合定位方法提供的定位结果作为输入后，可以得到机器人在地图中的位置，便于用户监视和发布目标。
当用户在用户输入导航目标后，导航包将使用内置的算法进行路径规划——包含全局路径规划和局部路径规划。全局路径规划的结果是一条可能的最优路径，局部路径规划是在全局路径规划的基础上通过对机器人当前运动状态以及周围障碍信息综合处理后得到下一时刻的目标速度和姿态，并将其转换为ROS控制消息发送至ArduRover无人车底盘，无人车将自主完成精准的导航任务，参见图2。
整体组合定位流程图见图1。
具体实施时，以上步骤可采用软件方式提供自动运行。
本发明采用的GNSS/INS/Visual组合定位方法以卡尔曼滤波作为框架，融合了以上三种技术的优势，同时互补以削弱各自的弊端，在现有理论框架下对定位结果达到了最优估计。
以下为用于组合定位方法验证的辅助部分介绍，ROS是基于Linux的机器人操作***，可以为本发明的GNSS/INS/Visual组合定位算法提供辅助的运动控制和通讯服务，用于计算导航规划和小车速度控制信息，并于ArduRover无人车上展示最后的实际应用效果。作为最为流行的机器人操作***，ROS 1.0起源于Willow Garage的PR2项目，主要组件包括ROS Master、ROS Node和ROS Service三种，本文的GNSS/INS/Visual组合定位将依赖于这三个组件在ArduRover小车上提供导航服务。ROS Master的主要功能是命名服务，它存储了启动时需要的运行时参数，消息发布上游节点和接收下游节点的连接名和连接方式，和已有ROS服务的连接名。ROS Node节点是真正的执行模块，对收到的消息进行处理，并且发布新的消息给下游节点。ROS Service是一种特殊的ROS节点，它相当于一个服务节点，接受请求并返回请求的结果。ROS通信的流程顺序，首先节点会向master advertise或者subscribe感兴趣的topic。当创建连接时，下游节点会向上游节点TCP Server发布连接请求，等连接创建后，上游节点的消息就会通过连接送至下游节点。
作为本发明定位方法的验证平台，ArduRover是一款功能强大而且完全开源的自动驾驶小车，同时还可以搭配多款地面控制站使用。地面站中可以在线升级固件、调参，使用一套全双工的无线数据传输***在地面站与小车之间建立起一条数据链，即可组成一套无人车自动控制***。
综上，本发明在卡尔曼滤波器框架下，将惯性测量单元IMU的原始输出数据用于滤波器状态的时间更新部分，将GNSS输出的位置速度和工业相机输出的相片作为观测量加入到滤波器之中，提供地心地固系下的绝对约束，得到更高精度的定位结果，并为无人车***提供在地心地固系下进行定位导航的能力，通过ROS的运动控制和通讯服务在ArduRover小车上完成定位效果的验证，为无人小车自动导航奠定基础。
本发明实施例提供的一种基于ArduRover无人车的组合定位方法至少包括如下技术效果：
(1)将VIO定位技术与GNSS定位技术融合，获得在全球地心地固系下的定位结果，通过全球坐标系的绝对约束解决了VIO这种相对定位技术的定位发散问题。
(2)摄像头和微机电类惯性导航单位器件以及GNSS定位模块都较为廉价，市场购买也比较方便，而通过本发明提供的组合算法可以提供高精度的定位服务。
(3)卡尔曼滤波器的计算效率高，存储量低，能胜任在各种复杂环境中的定位和导航计算任务；无论在相机受到遮挡、光照条件不好、小车剧烈运动等的场景下，定位精度都能得到保证。而且本发明已经在ArduRover平台上验证过导航定位效果的鲁棒性，在硬件平台-定位算法-控制算法-导航算法上提供完整的服务链，能够满足不同行业对无人小车平台室内外高精度定位和导航的需求。
最后所应说明的是，以上具体实施方式仅用以说明本发明的技术方案而非限制，尽管参照实例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (4)

1.一种基于ArduRover无人车的组合定位方法，其特征在于，包括以下步骤：
步骤1、利用全球卫星导航***GNSS定位结果与视觉惯性里程计VIO定位结果进行联合初始对准，获取载体在地心地固系下的运动状态；
所述载体在地心地固系下的运动状态包括：当地系n与世界系w之间的水平偏角
Figure FDA0003600361240000011
以及世界系w在地心地固系e下的绝对位置
Figure FDA00036003612400000118
所述联合初始对准前，VIO输出世界系w下载体的运动状态，GNSS解算不断输出载体在地心地固系e下的位置；
将GNSS输出的载体在地心地固系e下的运动轨迹投影到起点的当地系n下，利用该轨迹与VIO输出轨迹的首尾相对位置的位矢进行水平偏角
Figure FDA0003600361240000012
的计算，如公式(1)所示：
Figure FDA0003600361240000013
其中，
Figure FDA0003600361240000014
表示轨迹终点在当地系n下的坐标，
Figure FDA0003600361240000015
表示轨迹起点在当地系n下的坐标；
Figure FDA0003600361240000016
表示轨迹终点在世界系w下的坐标，
Figure FDA0003600361240000017
表示轨迹起点在世界系w下的坐标；
世界系w在地心地固系e下的绝对位置
Figure FDA0003600361240000018
采用公式(2)计算：
Figure FDA0003600361240000019
其中，
Figure FDA00036003612400000110
表示载体系b在地心地固系e下的绝对位置，
Figure FDA00036003612400000111
表示载体系b在世界系w下的绝对位置，
Figure FDA00036003612400000112
表示当地系n与世界系w之间的水平偏角，
Figure FDA00036003612400000113
表示将世界系w下的坐标转到当地系n下的旋转矩阵，
Figure FDA00036003612400000114
表示将当地系n下的坐标转到地心地固系e下的旋转矩阵,
Figure FDA00036003612400000115
表示将载体系b下的坐标转到世界系w下的旋转矩阵，
Figure FDA00036003612400000116
表示GNSS天线坐标系到载体系原点的偏移量；
所述联合初始对准成功的标准为同时满足第一条件和第二条件；所述第一条件为：载体的直线运动距离超过设定的第一阈值，且位置误差小于设定的第二阈值；所述第二条件为：水平偏角
Figure FDA00036003612400000117
在连续的历元内误差都小于设定的第三阈值；
步骤2、构建卡尔曼滤波器，利用上一量测时刻到当前时刻之间的惯性测量单元IMU输出的速度增量和姿态增量进行滤波器的时间更新，对新时刻的滤波器的状态向量进行预测，并对滤波器的状态向量的协方差矩阵进行转移；
步骤3、当获取到新时刻的相片时，对滤波器的状态向量进行扩维，将新时刻的相片位置和姿态加入到滤波器的状态向量之中，得到滤波器的总的状态向量；
步骤4、完成所述联合初始对准后，在获取到新的GNSS定位结果时，将GNSS解算输出的位置和速度作为量测值加入到滤波器的量测更新部分，对所述载体在地心地固系下的运动状态进行精对准，得到GNSS/INS/Visual组合定位结果；
GNSS的位置和速度的观测方程形式如下：
Figure FDA0003600361240000021
Figure FDA0003600361240000022
其中，zpos和
Figure FDA0003600361240000023
表示载体在地心地固系e下的绝对位置，
Figure FDA0003600361240000024
表示载体在世界系w下的绝对位置，
Figure FDA0003600361240000025
表示当地系n与世界系w之间的水平偏角，
Figure FDA0003600361240000026
表示将世界系w下的坐标转到当地系n下的旋转矩阵，
Figure FDA0003600361240000027
表示将载体系b下的坐标转到世界系w下的旋转矩阵，
Figure FDA0003600361240000028
表示GNSS天线坐标系到载体系原点的偏移量；zvel和
Figure FDA0003600361240000029
为载体在地心地固系e下的绝对速度，
Figure FDA00036003612400000210
表示载体在世界系w下的速度，
Figure FDA00036003612400000211
为改正零偏后的陀螺角速度的叉乘形式；
对所述载体在地心地固系下的运动状态进行精对准时，使用GNSS输出的位置和速度作为滤波器观测值，通过绝对定位，对VIO产生的误差进行修正；
误差方程的表示为：
Figure FDA00036003612400000212
其中，rGNSS为GNSS量测向量，包含zpos和zvel的误差形式；nGNSS为GNSS解算的位置和速度的量测噪声，
Figure FDA00036003612400000213
为残差量对状态量的雅可比矩阵，
Figure FDA00036003612400000214
为状态向量X的误差形式；
所述精对准完成的标准为：航向角标准差小于设定的第四阈值；
步骤5、将所述GNSS/INS/Visual组合定位结果输出到机器人操作***ROS中，通过ROS内置的路径规划服务并结合用户指定的导航目标产生控制指令，控制ArduRover无人车进行定位导航。
2.根据权利要求1所述的基于ArduRover无人车的组合定位方法，其特征在于，所述步骤2中，所述滤波器的状态向量包括当前时刻载体的运动状态、与IMU相关的状态向量。
3.根据权利要求2所述的基于ArduRover无人车的组合定位方法，其特征在于，所述步骤2中，所述与IMU相关的状态向量表示为：
Figure FDA0003600361240000031
其中，XI表示与IMU相关的状态向量，
Figure FDA0003600361240000032
是载体在世界系w下的姿态，
Figure FDA0003600361240000033
为载体在世界系w下的加速度，
Figure FDA0003600361240000034
为载体在世界系w下的位置，bg和ba分别为IMU的陀螺仪零偏和加速度零偏，
Figure FDA0003600361240000035
和
Figure FDA0003600361240000036
为左相机坐标系与载体系b之间的转换参数，其中，左相机坐标系为以双目相机中左侧相机光心为原点，XYZ轴分别指向右下前方向的坐标系。
4.根据权利要求1所述的基于ArduRover无人车的组合定位方法，其特征在于，所述步骤3中，与相片相关的状态向量表示为：
Figure FDA0003600361240000037
其中，XC表示与相片相关的状态向量，XCn表示时刻n下与相片相关的状态向量，
Figure FDA0003600361240000038
为第i张相片在世界系w下的姿态，
Figure FDA0003600361240000039
为第i张相片在世界系w下的位置；
所述滤波器的总的状态向量X表示为：
X＝[XI T XC T]T (5)
其中，XC表示与相片相关的状态向量，XI表示与IMU相关的状态向量。
CN202110539154.8A 2021-05-18 2021-05-18 一种基于ArduRover无人车的组合定位方法 Active CN113325454B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110539154.8A CN113325454B (zh) 2021-05-18 2021-05-18 一种基于ArduRover无人车的组合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110539154.8A CN113325454B (zh) 2021-05-18 2021-05-18 一种基于ArduRover无人车的组合定位方法

Publications (2)

Publication Number Publication Date
CN113325454A CN113325454A (zh) 2021-08-31
CN113325454B true CN113325454B (zh) 2022-06-14

Family

ID=77415881

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110539154.8A Active CN113325454B (zh) 2021-05-18 2021-05-18 一种基于ArduRover无人车的组合定位方法

Country Status (1)

Country Link
CN (1) CN113325454B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN107850673A (zh) * 2015-07-27 2018-03-27 高通股份有限公司 视觉惯性测距姿态漂移校准
CN109931926A (zh) * 2019-04-04 2019-06-25 山东智翼航空科技有限公司 一种基于站心坐标系的小型无人机无缝自主式导航算法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及***

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10267924B2 (en) * 2017-01-04 2019-04-23 Qualcomm Incorporated Systems and methods for using a sliding window of global positioning epochs in visual-inertial odometry

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107850673A (zh) * 2015-07-27 2018-03-27 高通股份有限公司 视觉惯性测距姿态漂移校准
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及***
CN109931926A (zh) * 2019-04-04 2019-06-25 山东智翼航空科技有限公司 一种基于站心坐标系的小型无人机无缝自主式导航算法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Joint inversion of GNSS and teleseismic data for the rupture process of the 2017 Mw6.5 Jiuzhaigou, China, earthquake;Qi Li等;《Journal of Seismology》;20180113;第805-814页 *
基于状态变换多状态约束卡尔曼滤波的视觉/惯性组合导航;王茂松 等;《中国惯性技术学报》;20210430;第29卷(第2期);第199-207页 *

Also Published As

Publication number Publication date
CN113325454A (zh) 2021-08-31

Similar Documents

Publication Publication Date Title
CN109885080B (zh) 自主控制***及自主控制方法
CN113884102B (zh) 传感器安装偏差角的标定方法、组合定位***和车辆
CN108225302B (zh) 一种石化工厂巡检机器人定位***和方法
CN111102978B (zh) 一种车辆运动状态确定的方法、装置及电子设备
Xiong et al. G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving
Goel et al. Robust localization using relative and absolute position estimates
US20210199438A1 (en) Heading initialization method for tilt rtk
CN107132563B (zh) 一种里程计结合双天线差分gnss的组合导航方法
CN110702091A (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN110988949A (zh) 定位方法、定位装置、计算机可读存储介质与可移动设备
CN116047565A (zh) 一种多传感器数据融合定位***
Sun et al. Motion model-assisted GNSS/MEMS-IMU integrated navigation system for land vehicle
Niu et al. Camera-based lane-aided multi-information integration for land vehicle navigation
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
CN113325454B (zh) 一种基于ArduRover无人车的组合定位方法
Benz et al. CIOT: Constraint-Enhanced Inertial-Odometric Tracking for Articulated Dump Trucks in GNSS-Denied Mining Environments
WO2022018964A1 (ja) 情報処理装置、情報処理方法およびプログラム
CN114894180A (zh) 基于相对导航信息的多源融合导航方法及***
Wang et al. Low-speed unmanned vehicle localization based on sensor fusion of low-cost stereo camera and IMU
Xu et al. Research on Indoor AGV Fusion Localization Based on Adaptive Weight EKF Using Multi-sensor
Luo et al. An imu/visual odometry integrated navigation method based on measurement model optimization
CN113188539B (zh) 一种巡检机器人的组合定位方法
ZHANG et al. RRVPE: A Robust and Real-Time Visual-Inertial-GNSS Pose Estimator for Aerial Robot Navigation
CN115717901B (zh) 基于滤波的惯性/视觉里程计安装误差估计方法
CN113029130B (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