CN113325454B - 一种基于ArduRover无人车的组合定位方法 - Google Patents
一种基于ArduRover无人车的组合定位方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 24
- 230000033001 locomotion Effects 0.000 claims abstract description 34
- 230000000007 visual effect Effects 0.000 claims abstract description 22
- 239000013598 vector Substances 0.000 claims description 57
- 238000005259 measurement Methods 0.000 claims description 34
- 239000011159 matrix material Substances 0.000 claims description 23
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000003287 optical effect Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 16
- 230000000694 effects Effects 0.000 abstract description 13
- 238000004891 communication Methods 0.000 abstract description 5
- 238000012795 verification Methods 0.000 abstract description 5
- 238000004364 calculation method Methods 0.000 description 5
- 238000001914 filtration Methods 0.000 description 3
- 238000011144 upstream manufacturing Methods 0.000 description 3
- 239000000306 component Substances 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000003860 storage Methods 0.000 description 2
- 240000005574 Eucalyptus neglecta Species 0.000 description 1
- 241000124033 Salix Species 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 239000008358 core component Substances 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000004377 microelectronic Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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无人车的组合定位方法。
背景技术
随着计算机技术与导航技术的发展,各种形态的智能机器人在各种生产活动中逐渐扮演起重要的角色,如物流机器人、服务机器人、无人机,乃至自动驾驶汽车等。对于智能移动机器人而言,如何实现复杂环境下的高精度自主定位导航,是一个需要解决的核心问题。
目前,机器人的自主导航有很多不同的实现手段,但是仍然缺乏成熟的解决方案,可以利用成本相对低廉的传感器在室内外的复杂环境下进行较为稳定的导航定位。传统的高精度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中,所述联合初始对准前,VIO输出世界系w下载体的运动状态,GNSS解算不断输出载体在地心地固系e下的位置;
其中,表示载体系b在地心地固系e下的绝对位置,表示载体系b在世界系w下的绝对位置,表示当地系n与世界系w之间的水平偏角,表示将世界系w下的坐标转到当地系n下的旋转矩阵,表示将当地系n下的坐标转到地心地固系e下的旋转矩阵,表示将载体系b下的坐标转到世界系w下的旋转矩阵,表示GNSS天线坐标系到载体系原点的偏移量。
优选的,所述步骤1中,所述联合初始对准成功的标准为同时满足第一条件和第二条件;所述第一条件为:载体的直线运动距离超过设定的第一阈值,且位置误差小于设定的第二阈值;所述第二条件为:水平偏角在连续的历元内误差都小于设定的第三阈值。
优选的,所述步骤2中,所述滤波器的状态向量包括当前时刻载体的运动状态、与IMU相关的状态向量。
优选的,所述步骤2中,所述与IMU相关的状态向量表示为:
其中,XI表示与IMU相关的状态向量,是载体在世界系w下的姿态,为载体在世界系w下的加速度,为载体在世界系w下的位置,bg和ba分别为IMU的陀螺仪零偏和加速度零偏,和为左相机坐标系与载体系b之间的转换参数,其中,左相机坐标系为以双目相机中左侧相机光心为原点,XYZ轴分别指向右下前方向的坐标系。
优选的,所述步骤3中,与相片相关的状态向量表示为:
所述滤波器的总的状态向量X表示为:
X=[XI T XC T]T (5)
其中,XC表示与相片相关的状态向量,XI表示与IMU相关的状态向量。
优选的,所述步骤4中,GNSS的位置和速度的观测方程形式如下:
其中,zpos和表示载体在地心地固系e下的绝对位置,表示载体在世界系w下的绝对位置,表示当地系n与世界系w之间的水平偏角,表示将世界系w下的坐标转到当地系n下的旋转矩阵,表示将载体系b下的坐标转到世界系w下的旋转矩阵,表示GNSS天线坐标系到载体系原点的偏移量;zvel和为载体在地心地固系e下的绝对速度,表示载体在世界系w下的速度,为改正零偏后的陀螺角速度的叉乘形式。
优选的,所述步骤4中,对所述载体在地心地固系下的运动状态进行精对准时,使用GNSS输出的位置和速度作为滤波器观测值,通过绝对定位,对VIO产生的误差进行修正;
误差方程的表示为:
优选的,所述步骤4中,所述精对准完成的标准为:航向角标准差小于设定的第四阈值。
本发明中提供的一个或多个技术方案,至少具有如下技术效果或优点:
在发明中,将VIO定位技术与GNSS定位技术融合,获得在全球地心地固系下的定位结果,通过全球坐标系的绝对约束解决了VIO这种相对定位技术的定位发散问题。以惯性与视觉紧组合的视觉惯性里程计VIO作为基础,将全球卫星定位***GNSS解算的位置速度作为额外的观测量加入到滤波器之中,作为位置等状态量的绝对约束,得到更高精度的定位结果,并为无人车***提供在地心地固系下进行定位导航的能力,通过机器人操作***ROS的运动控制和通讯服务在ArduRover小车上完成定位效果的验证,为无人小车自动导航奠定基础。本发明解决了现有技术中无人小车组合定位效果较差的问题,对于高精度无人车的组合定位应用具有重要意义。
附图说明
图1为本发明实施例提供的一种基于ArduRover无人车的组合定位方法的流程示意图;
图2为ArduRover无人车导航流程图;
图3为GNSS和VIO联合初始对准示意图。
具体实施方式
为了更好的理解上述技术方案,下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。
本实施例提供了一种基于ArduRover无人车的组合定位方法,参见图1,包括以下步骤:
步骤1,利用GNSS定位结果与VIO定位结果进行联合初始对准,获取无人车载体在地心地固系下的运动状态。
本***采用GNSS/VIO联合对准方法对***进行初始对准。在本定位算法中,为了描述载体在地心地固系下的运动状态,对准所要确定的参数为当地系n与世界系w之间的水平偏角(如图3所示),以及世界系w在地心地固系e下的绝对位置
在对准之前,视觉惯性里程计输出世界系w下载体的运动状态,而GNSS解算不断输出载体在e系下的位置,***同时维护这两条不同参考系下载体的运动轨迹。
其中,表示载体系b在地心地固系e下的绝对位置,表示载体系b在世界系w下的绝对位置,表示当地系n下世界系w的水平偏角,表示将世界系w下的坐标转到当地系n下的旋转矩阵,表示将当地系n下的坐标转到地心地固系e下的旋转矩阵,表示将载体系b下的坐标转到世界系w下的旋转矩阵,表示GNSS天线坐标系到载体系原点的偏移量。
上述对准算法充分利用了不同传感器的信息,可以在载体有明显水平运动的情况下快速完成粗对准,定向误差在10°以内。
本发明使用了GNSS和VIO联合对准方法,无需额外的辅助信息,通过无人车本身的短时间运动就能得到载体的初始运动状态,从而保证后续组合定位结果的精度。
步骤2,构建卡尔曼滤波器,利用上一量测时刻(或初始时刻)到当前时刻之间的IMU输出值进行滤波器的时间更新,对新时刻的滤波器状态量进行预测,并对状态协方差矩阵进行转移。
构建卡尔曼滤波器,在新的量测时刻到来时,算法利用上一量测时刻(或初始时刻)到当前时刻之间的IMU输出的速度增量和姿态增量进行滤波器的时间更新,对新时刻的状态量进行预测,并对状态协方差矩阵进行转移。
本算法以起点的世界系w作为导航的参考坐标系(称之为世界系是因为参考系的初始水平方位角在没有GNSS量测信息的情况下不可观,该系与真正的ENU当地系n之间相差一个未知的水平偏角。设置世界系w作为参考系是为了便于视觉惯性里程计的推算,同时避免定位模块对于绝对位置的依赖)。从物理含义上讲,这个坐标系与地球是固联的。我们做出如下近似:在一定的运动范围内,载体受到的重力都可以在w系下表示为[0 0 -|g|]T,即忽略了的变化。其中,表示地球自转带来的当地系相对于地心地固系的旋转量。
本算法中,滤波器维护的状态向量包含两部分,一部分是当前时刻载体的运动状态以及与IMU相关的状态
其中,XI表示与IMU相关的状态向量,是载体(IMU坐标系)在参考坐标系下的姿态。为载体在参考坐标系下的加速度,载体在参考系下的位置,bg和ba分别为IMU的陀螺仪零偏和加速度零偏,为左相机坐标系与载体坐标系(IMU坐标系)之间的转换参数。其中,左相机坐标系为以双目相机中左侧相机光心为原点,XYZ轴分别指向右下前方向的坐标系。
与一般的VIO***不同的是,为了引入GNSS量测值,本算法额外引入了两个与载体绝对位姿相关的参数,分别是当地系n与世界系w之间的水平偏角以及世界系w在地心地固系e下的绝对位置对于视觉惯性组合而言,这四维参数是不可观测的;而一旦加入GNSS测量值,则可以对这四维参数进行估计。
本发明使用了卡尔曼滤波框架作为状态估计器,扩展了滤波器的状态向量,计算效率高,存储空间耗费低,且可扩展性好,方便引入更多种定位传感器,符合无人车实时导航定位的需求。
步骤3,当获取到新时刻的相片时,对滤波器***状态向量进行扩维,将新时刻的相片位置和姿态加入到滤波器的状态向量之中。
滤波器在加入相片的位置和姿态时,需要扩增状态向量的维度,将相片的位姿引入状态向量中,通过估计得到更精确的相片位姿用以辅助无人车自身的状态估计。
在新相片到来时,将会首先对状态向量进行扩维,将新时刻的相片位姿加入到状态向量之中。任意时刻的相片位姿与载***姿之间存在着近似固联的关系(IMU-相机外参),新时刻的相片位姿是由时间更新后的IMU状态计算而来。
状态向量的阔维部分是滤波器中维护的过去时刻的相片位姿,用于构建视觉约束
总的状态向量可以描述如下
X=[XI T XC T]T (5)
其中,XC表示与相片相关的状态向量,XI表示与IMU相关的状态向量。
步骤4,在获取到新的GNSS定位结果时,将GNSS解算输出的位置速度作为量测值加入到滤波器的量测更新部分。
在***完成初始对准之后,GNSS解算输出的位置速度将被作为量测值加入到滤波器的量测更新部分。
GNSS位置和速度的观测方程形式如下
其中,zpos和表示载体在地心地固系e下的绝对位置,表示载体在世界系w下的位置,表示当地系n下世界系w的水平偏角,表示将世界系w下的坐标转到当地系n下的旋转矩阵,表示将载体系下的坐标转到世界系下的旋转矩阵,表示GNSS天线坐标系到载体系原点的偏移量;zvel和为载体在地心地固系e下的绝对速度,表示载体在世界系w下的速度,为改正零偏后的陀螺角速度的叉乘形式。
误差方程的形式为
其中,rGNSS为GNSS量测向量,包含zpos和zvel的误差形式;GNSS为GNSS解算的位置和速度的量测噪声,可以由GNSS解算程序给出;为状态向量X的误差形式;为残差量对状态量的雅可比矩阵,其具体形式在此给出:
其中,GNSS解算的位置和速度分别对各状态量的偏导数的具体形式如下,其中,表示zpos对的导数,表示zpos对的导数,表示zpos对的导数,表示将世界系w下的坐标转到载体系b下的旋转矩阵,表示zvel对的导数,表示zvel对的导数,表示zvel对的导数。
量测更新的步骤即为标准的卡尔曼滤波量测更新,具体见步骤3。
本发明使用了GNSS输出的位置和速度作为滤波器观测值,通过绝对定位,对VIO本身产生的误差进行修正,原因在于在粗对准之后,***会将新的GNSS量测值引入到滤波器之中,进一步对与进行精对准,直到收敛——航向角标准差小于设定的阈值。此后,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定位结果进行联合初始对准,获取载体在地心地固系下的运动状态;
所述联合初始对准前,VIO输出世界系w下载体的运动状态,GNSS解算不断输出载体在地心地固系e下的位置;
其中,表示载体系b在地心地固系e下的绝对位置,表示载体系b在世界系w下的绝对位置,表示当地系n与世界系w之间的水平偏角,表示将世界系w下的坐标转到当地系n下的旋转矩阵,表示将当地系n下的坐标转到地心地固系e下的旋转矩阵,表示将载体系b下的坐标转到世界系w下的旋转矩阵,表示GNSS天线坐标系到载体系原点的偏移量;
所述联合初始对准成功的标准为同时满足第一条件和第二条件;所述第一条件为:载体的直线运动距离超过设定的第一阈值,且位置误差小于设定的第二阈值;所述第二条件为:水平偏角在连续的历元内误差都小于设定的第三阈值;
步骤2、构建卡尔曼滤波器,利用上一量测时刻到当前时刻之间的惯性测量单元IMU输出的速度增量和姿态增量进行滤波器的时间更新,对新时刻的滤波器的状态向量进行预测,并对滤波器的状态向量的协方差矩阵进行转移;
步骤3、当获取到新时刻的相片时,对滤波器的状态向量进行扩维,将新时刻的相片位置和姿态加入到滤波器的状态向量之中,得到滤波器的总的状态向量;
步骤4、完成所述联合初始对准后,在获取到新的GNSS定位结果时,将GNSS解算输出的位置和速度作为量测值加入到滤波器的量测更新部分,对所述载体在地心地固系下的运动状态进行精对准,得到GNSS/INS/Visual组合定位结果;
GNSS的位置和速度的观测方程形式如下:
其中,zpos和表示载体在地心地固系e下的绝对位置,表示载体在世界系w下的绝对位置,表示当地系n与世界系w之间的水平偏角,表示将世界系w下的坐标转到当地系n下的旋转矩阵,表示将载体系b下的坐标转到世界系w下的旋转矩阵,表示GNSS天线坐标系到载体系原点的偏移量;zvel和为载体在地心地固系e下的绝对速度,表示载体在世界系w下的速度,为改正零偏后的陀螺角速度的叉乘形式;
对所述载体在地心地固系下的运动状态进行精对准时,使用GNSS输出的位置和速度作为滤波器观测值,通过绝对定位,对VIO产生的误差进行修正;
误差方程的表示为:
所述精对准完成的标准为:航向角标准差小于设定的第四阈值;
步骤5、将所述GNSS/INS/Visual组合定位结果输出到机器人操作***ROS中,通过ROS内置的路径规划服务并结合用户指定的导航目标产生控制指令,控制ArduRover无人车进行定位导航。
2.根据权利要求1所述的基于ArduRover无人车的组合定位方法,其特征在于,所述步骤2中,所述滤波器的状态向量包括当前时刻载体的运动状态、与IMU相关的状态向量。
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)
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)
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 |
-
2021
- 2021-05-18 CN CN202110539154.8A patent/CN113325454B/zh active Active
Patent Citations (4)
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)
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 |