CN111766603B - 基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法、系统、介质及设备 - Google Patents
基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法、系统、介质及设备 Download PDFInfo
- Publication number
- CN111766603B CN111766603B CN202010595256.7A CN202010595256A CN111766603B CN 111766603 B CN111766603 B CN 111766603B CN 202010595256 A CN202010595256 A CN 202010595256A CN 111766603 B CN111766603 B CN 111766603B
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- pose
- tag code
- april tag
- camera unit
- 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 40
- 238000010276 construction Methods 0.000 claims abstract description 21
- 238000004590 computer program Methods 0.000 claims description 14
- 238000013507 mapping Methods 0.000 claims description 14
- 238000004364 calculation method Methods 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 5
- 230000004807 localization Effects 0.000 claims description 3
- 238000009825 accumulation Methods 0.000 abstract description 7
- 230000009286 beneficial effect Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 11
- 230000006870 function Effects 0.000 description 11
- 238000001514 detection method Methods 0.000 description 6
- 230000000007 visual effect Effects 0.000 description 6
- 230000007613 environmental effect Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000003708 edge detection Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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/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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法、系统、介质及设备,在移动机器人搭载的视觉相机同一水平线上的障碍物表面打上AprilTag码,移动机器人在定位与地图构建过程中,通过视觉单元实时采集一定距离内的AprilTag码图像,然后搭载在移动机器人内部的微型计算机实时计算移动机器人在地图构建过程中的位姿信息,从而确定移动机器人的视觉偏移量。该方法有利于移动机器人在环境中长时间的建图,减少移动机器人的里程计误差积累,提高移动机器人的建图精度。
Description
技术领域
本发明属于机器人技术领域,特别涉及一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法、系统、介质及设备。
背景技术
AprilTag作为一种视觉定位库,被广泛应用于机器人领域。相关论文由EdwinOlson于2011年发表在IEEE International Conference on Robotics and Automation(ICRA)。定位功能作为AprilTag库的最关键方法,其作用是通过特定的AprilTag码进行标记,由深度相机快速识别该AprilTag码,与库函数对比进行快速的解码并计算其相对于相机的位置。由于每一张AprilTag码携带的信息不同,使用AprilTag码作为视觉辅助定位,可以有助于解决移动机器人在定位与地图构建过程中里程计误差积累的问题,可以提高移动机器人的定位精度、保证移动机器人建图的高效性。
SLAM(simultaneous localization and mapping)被称为即时定位与建图,是移动机器人实现智能化的关键技术,对于移动机器人走向自主式发展具有重要的意义。移动机器人SLAM主要是解决“定位”与“环境地图构建”这两个问题。它能根据当前时刻移动机器人所在世界坐标系中的位置与姿态信息来估计障碍物所在周围环境中的位置信息。移动机器人SLAM技术被广泛应用于餐饮、医护、防疫、无人车、运输、救援等行业,是近年来非常热门的行业之一。因此,移动机器人SLAM技术有着十分重要的意义及应用价值。
然而,现有技术中移动机器人进行SLAM过程中,容易存在定位误差。
发明内容
本发明提供了一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法、系统、介质及设备,其目的在于通过当前时刻里程计系统测量移动机器人的位姿信息,根据移动机器人深度相机检测到AprilTag码的位置与姿态,从而修正里程计系统的定位误差,完成高精度的地图构建。
本发明提供的技术方案如下:
一方面,一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法,包括以下步骤:
步骤一、在环境空间中的目标物体上张贴AprilTag码,AprilTag码与移动机器人的相机单元在同一水平线上呈平行设置;
所述移动机器人的底盘中心设置有IMU单元,移动机器人底盘中心线两侧均设有连接在一起的驱动轮、无刷直流电机和编码器,移动机器人沿前进方向的中心线两端设置有支撑轮,沿机器人前进方向的底盘上方固定相机单元,且沿相机单元坐标系的y轴方向,移动机器人的前进端的支撑轮上方设置激光雷达传感器;
相机可跟随着移动机器人在环境空间中进行旋转、左右、前后移动,移动机器人会带着激光雷达在环境空间中进行定位与建图;
步骤二、记录初始时刻时移动机器人的初始位姿为R0(0,0,0)以及AprilTag码的初始位姿;
步骤三、依据t时刻相机单元采集到的AprilTag码的位姿,获得AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt);
步骤四、利用AprilTag码的偏差位姿,获得移动机器人t时刻的偏差位姿ΔRt(ΔXRt,ΔYRt,ΔβRt),并对移动机器人的t时刻位姿信息进行校正;
所述移动机器人当前的偏差位姿如下:
其中,(XRt,YRt,βRt)表示移动机器人t时刻的位姿,通过里程计计算获得;
步骤五、将修正的里程计位姿信息通过ROS机器人操作系统的节点进行发布出去,由SLAM定位与建图功能包订阅里程计位姿信息,通过激光雷达传感器和里程计单元对环境空间进行地图构建,直到移动机器人将所在的环境空间地图构建完毕。
进一步地,当移动机器人上的相机单元在t时刻采集到一张AprilTag码的位姿时,所述AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt)按以下公式计算:
其中,(XAt,YAt,βAt)表示移动机器人上的相机单元在t时刻采集到的AprilTag码的位姿,(XA0,YA0,βA0)表示AprilTag码的初始位姿。
进一步地,当移动机器人上的相机单元在t时刻采集到两张AprilTag码的位姿时,所述AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt)按以下公式计算:
其中,(ΔXA1t,ΔYA1t,ΔβA1t)和(ΔXA2t,ΔYA2t,ΔβA2t)分别为第一张和第二张AprilTag码的偏差位姿,每张AprilTag码的偏差位姿均为当前时刻采集的位姿与初始时刻采集的位姿差值。
每一张AprilTag码都是固定张贴在目标物体上的,当移动机器人在建图过程中相机单元再次采集到AprilTag码位姿信息时,移动机器人会根据AprilTag码位姿信息修正里程计的误差。
所述的移动机器人位姿是通过里程计单元计算获得的,里程计单元由光电编码器和IMU(惯性测量单元组成),光电编码器获得移动机器人任意时刻的位置信息,IMU获得移动机器人任意时刻的姿态角信息,两者组合即为里程计位姿,表示当前时刻移动机器人的位姿。
进一步地,在移动机器人进行环境空间地图构建过程中,移动机器人的线速度设置为在1m/s、角速度设置为0.2rad/s。
进一步地,所述相机单元为深度相机,所述相机单元通过USB串口与微型计算机相连接。
深度相机采集到的AprilTag码位姿信息快速传回给微型计算机。
进一步地,在步骤五中,所述激光雷达传感器为单线激光雷达传感器,所述激光雷达传感器通过USB串口转RS422模块与微型计算机相连接,激光雷达传感器实时扫描障碍物在全局坐标系中的位置,通过ROS系统构建二维栅格地图。
进一步地,所述的移动机器人的里程计算单元计算得到的t时刻移动机器人的位姿信息Rt(Xt,Yt,βt)所采用的计算公式为:
其中,Xt,Yt,βt分别为t时刻移动机器人在世界坐标系的二维坐标和移动机器人朝向角度,Xt-1,Yt-1,βt-1分别为t-1时刻移动机器人在世界坐标系的二维坐标和移动机器人朝向角度,rt,lt分别为t-1到t时刻移动机器人左右轮的移动距离,B为左右轮的中心距,r为移动半径。
所述的移动机器人的位姿为里程计系统在当前时刻的位姿。
另一方面,一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM系统,包括以下步骤:
移动机器人设置单元:移动机器人的底盘中心设置有IMU单元,移动机器人底盘中心线两侧均设有连接在一起的驱动轮、无刷直流电机和编码器,移动机器人沿前进方向的中心线两端设置有支撑轮,沿机器人前进方向的底盘上方固定相机单元,且沿相机单元坐标系的y轴方向,移动机器人的前进端的支撑轮上方设置激光雷达传感器;
AprilTag码设置单元:通过在环境空间中的目标物体上张贴AprilTag码,并使得AprilTag码与移动机器人的相机单元在同一水平线上呈平行设置;
初始位姿记录单元:记录初始时刻时移动机器人的初始位姿以及AprilTag码的初始位姿;
AprilTag码的偏差位姿获取单元:依据t时刻相机单元采集到的AprilTag码的位姿,获得AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt);
移动机器人位姿校正单元:利用AprilTag码的偏差位姿,获得移动机器人t时刻的偏差位姿ΔRt(ΔXRt,ΔYRt,ΔβRt),并对移动机器人的t时刻位姿信息进行校正;
所述移动机器人当前的偏差位姿如下:
其中,(XRt,YRt,βRt)表示移动机器人t时刻的位姿,通过里程计计算获得;
SLAM创建单元:将校正后的移动机器人位姿信息通过ROS机器人操作系统的节点进行发布出去,由SLAM定位与建图功能包订阅里程计位姿信息,并通过激光雷达传感器和里程计单元对环境空间进行地图构建,直到移动机器人将所在的环境空间地图构建完毕。
再一方面,一种计算机可读存储介质,包括计算机程序,所述计算机程序被处理器执行时实现所述的一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法。
再一方面,一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM设备,包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时,以使所述基于AprilTag码视觉辅助定位的移动机器人激光SLAM设备实现所述的一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法。
有益效果
与当前的技术相比,本发明主要有以下的几处优势:
(1)本发明采用了AprilTag码作为移动机器人建图的辅助定位对象,AprilTag码具有实时性强,定位精度高的特点,通过对AprilTag码进行边缘检测、四边形检测、编码与解码的步骤完成辅助定位信息采集。
(2)本发明降低了移动机器人里程计系统随着时间的推移位置估计误差及不确定性,使得移动机器人在全局坐标系中的定位变得更加准确。
(3)本发明采用激光雷达传感器作为环境信息采集的单元,使得移动机器人在建图时,拥有更快、更准确的采集效率,构建高质量的地图。
(4)本发明所提出的AprilTag码视觉辅助定位与里程计系统定位相结合,提高了移动机器人SLAM的效率,加快了移动机器人实现自主式发展。
附图说明
图1为本发明实例中移动机器人定位校正流程示意图;
图2为本发明实例中移动机器人三维结构图;
图3为本发明实例中AprilTag码位姿采集示意图;
图4为本发明实例中移动机器人底盘结构示意图;
图5为本发明实例中激光雷达传感器三角测距示意图;
图6为本发明实例中移动机器人不同时刻位姿修正示意图。
具体实施方式
下面将结合附图和实施例对本发明做进一步的说明。
请参阅图1所示,本发明提供一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法,包括如下步骤:
步骤一、在环境空间中的目标物体1上张贴AprilTag码2,AprilTag码2与相机单元3在同一水平线上,同时在固定位置上张贴的AprilTag码2与水平面保持平行状态请参阅图2和3所示;
具体的,环境空间是移动机器人进行定位与建图的工作场景,是一个三维空间,可以是办公区域、展览厅、医院科室、餐厅等。环境空间中的目标物体1可以是桌子办公桌、展台、病床、餐桌等。目标物体1上有相应的平整侧面,平整的侧面主要用于张贴AprilTag码2。AprilTag码2打印出来呈正方形,将AprilTag码2中心位置与移动机器人装载的相机单元3在同一平面上,同时対齐地面,固定AprilTag码2在目标物体的侧面,即可完成AprilTag码2的张贴。每一张AprilTag码2都是不同的,在本实例中,包含两张不同的AprilTag码,相机单元3通过检测AprilTag码2中黑白方块的排列方式来采集不同的位姿信息。
请参阅图3所示,在移动机器人的底盘4上面与移动机器人前进的方向上安装相机3,相机3可跟随着移动机器人在环境空间中进行旋转、左右、前后移动;
具体的,相机单元3通过支架6固定安装在移动机器人的底盘4上面,相机单元3的朝向是与移动机器人前进的方向是一致的。相机单元3为深度相机,支架6可进行收缩。
为了使相机单元3采集到AprilTag码2的位姿信息更加准确,在使用并安装相机单元3之前,对相机单元3采用棋盘格标定法进行标定,以校正镜头在生产和组装过程中发生畸变;同时,也可以得到标定后相机单元3的参数,从而建立相机单元3成像的几何模型,使获得的深度距离信息变得更加准确。将标定后的相机单元3固定安装在支架6上,从而使相机单元3在跟随移动机器人进行图像采集时,其产生的运动畸变大幅度的降低。相机单元3采集到AprilTag码2的视觉信息,将反馈到AprilTag库并调用OpenCV库函数进行编码、解码,即可完成对AprilTag码2的位姿检测。
请参阅图4所示,IMU单元安装在移动机器人底盘的中心位置,左右两个编码器与无刷直流电机连接在一起分别安装在移动机器人底盘中心线的两侧;
具体的,所述的移动机器人底盘4由前支撑轮7、左驱动轮8、左无刷直流电机9、左编码器10、IMU单元11、右驱动轮12、右无刷直流电机13、右编码器14、后支撑轮15组成。所述的左驱动轮8和右驱动轮12为移动机器人实现前进、后退、旋转功能,当移动机器人两轮速度V=(Vl+Vr)/2时,移动机器人实现前进或后退功能;当移动机器人Vl>Vr时,移动机器人实现右前进或右后退功能;当移动机器人Vl<Vr时,移动机器人实现左前进或左后退功能。所述的前支撑轮7和后支撑轮15为无动力万向轮,为移动机器人提供平衡性。所述的左无刷直流电机9和右无刷直流电机13分别为移动机器人的左驱动轮8、右驱动轮12提供动力。所述的左编码器10和右编码器14为移动机器人提供实时的位置信息,编码器均为光电编码器。其主要由光栅盘和光电检测装置构成,在直流无刷电机发生转动时,光栅盘与直流电动机同轴转动,直流无刷电动机旋转的同时也带动光栅盘的旋转,再经光电检测装置输出若干个脉冲信号,根据该信号的每秒脉冲数便可计算当前直流无刷电动机的转速。因此,在时间已知的情况下,便可计算出移动机器人的位置信息。所述的IMU单元11为移动机器人姿态检测传感器,可以实时计算移动机器人所在环境空间中的姿态角。
进一步说明,由移动机器人的光电编码器提供实时位置信息及IMU单元11提供实时姿态角信息,便能实时定位出移动机器人在环境空间中的位姿信息。然而,随着移动机器人在环境空间中地图构建的时间积累,其编码器和IMU单元11组成的里程计系统误差的积累也随着增加,最终会导致移动机器人构建的环境地图出现畸变、或重影等现象。因此,基于AprilTag码2的视觉辅助定位降低了里程计系统的误差积累,改善了移动机器人的定位精度,提高了移动机器人的建图效率。
请参阅图2和5所示,在相机3坐标系的-y轴方向上,移动机器人前支撑轮7的上方,移动机器人的前进方向上安装激光雷达传感器5,移动机器人会带着激光雷达传感器5在环境空间中进行定位与建图;
具体的,所述的激光雷达传感器5安装方式如图2所示,其通过图5中的激光发射端16发射激光点,激光点打在目标物体1的障碍物18上,光线经反射回到接收端17。其中α、d、β已知,h的垂直距离即为激光雷达传感器5检测到障碍物18的距离。因此,由tanα=h/d1,tanβ=h/d2,即d1=h/tanα,d2=h/tanβ。所以,由d=d1+d2=hsin(α+β)/sinαsinβ,得h=dsinαsinβ/sin(α+β),从而求出当前时刻激光雷达传感器5与障碍物18的距离。
步骤二、;记录初始时刻时移动机器人的初始位姿为R0(0,0,0)以及AprilTag码2的初始位姿;
具体的,初始时刻是指t=0的时刻,初始位姿是指t=0时刻的位姿信息,移动机器人的初始位姿为(0,0,0),AprilTag码2的初始位姿为(XA0,YA0,βA0)。
步骤三、在tx1时刻通过计算机视觉系统识别第一张AprilTag码2的位姿(XAt,YAt,βAt),同一时刻通过里程计系统计算出当前时刻移动机器人的位姿信息(XRt,YRt,βRt),根据计算机视觉系统测量结果修正移动机器人的位姿偏差(ΔXRt,ΔYRt,ΔβRt),请参阅图6所示;
具体的,如图3所示,所述的t0时刻移动机器人的初始位姿为(0,0,0),AprilTag码2的初始位姿为相机单元3测得的(XA0,YA0,βA0)。移动机器人获得初始位姿之后,经过一段时间的环境地图创建,当移动机器人的里程计系统出现误差积累时,相机单元3将采集到的AprilTag码2位姿信息快速与里程计系统的位姿进行匹配,修正当前时刻移动机器人的位姿偏差(ΔXRt,ΔYRt,ΔβRt)。
步骤四、在tx2时刻通过计算机视觉识别系统获取下一张AprilTag码2的位姿信息,将当前时刻获取的AprilTag码2位姿作为基准位姿,再次修正当前时刻移动机器人的位姿,完成移动机器人里程计的微调定位;
具体的,所述的tx2时刻为移动机器人完成微调定位之后的准确位姿。所述的计算机视觉识别系统由AprilTag库和OpenCV库组成,相机单元3将获取的AprilTag码2视觉信息根据梯度检测出图像中的各种边缘,之后将进行四边形检测,最后与AprilTag码库进行匹配编码,完成解码的过程。如图6所示,在tx2时刻将获得的AprilTag码2的定位信息与移动机器人进行位姿匹配,完成移动机器人里程计的微调定位。
当移动机器人上的相机单元在t时刻采集到一张AprilTag码的位姿时,所述AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt)按以下公式计算:
其中,(XAt,YAt,βAt)表示移动机器人上的相机单元在t时刻采集到的AprilTag码的位姿,(XA0,YA0,βA0)表示AprilTag码的初始位姿。
进一步地,当移动机器人上的相机单元在t时刻采集到两张AprilTag码的位姿时,所述AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt)按以下公式计算:
其中,(ΔXA1t,ΔYA1t,ΔβA1t)和(ΔXA2t,ΔYA2t,ΔβA2t)分别为第一张和第二张AprilTag码的偏差位姿,每张AprilTag码的偏差位姿均为当前时刻采集的位姿与初始时刻采集的位姿差值。
每一张Apriltag码2都是固定张贴在目标物体1上的,当移动机器人在建图过程中相机单元3再次采集到Apriltag码2位姿信息时,移动机器人会根据Apriltag码2位姿信息修正里程计的误差;
具体的,移动机器人在工作环境中进行定位与建图,当Apriltag码2不在移动机器人携带的相机单元3的视觉范围内时,移动机器人的定位信息来自于里程计系统,误差的积累此时没有得到修正。当Apriltag码2出现在移动机器人携带的相机单元3的视觉范围内时,相机将继续采集Apriltag码2的位姿信息,从而完成移动机器人里程计的位姿修正。因此,在环境空间中的不同区域的目标物体1上固定张贴两张Apriltag码2,相机单元3会时刻采集到Apriltag码2的位姿信息来修正移动机器人里程计的误差积累。
步骤五、将修正的里程计位姿信息通过ROS系统的节点进行发布出去,由SLAM定位与建图功能包订阅里程计位姿信息,通过激光雷达传感器5和里程计系统对环境空间进行地图构建;
所述的ROS系统为机器人操作系统,其为移动机器人进行定位与建图提供功能包,移动机器人的里程计定位信息与激光雷达传感器5获取的环境信息作为输入,环境地图作为输出,通过粒子滤波算法完成环境地图的构建。
在本实例中,将移动机器人的线速度设置为在1m/s、角速度设置为0.2rad/s,重复步骤五-八,直到移动机器人将所在的环境空间地图构建完毕。
具体的,所述的移动机器人线速度为1m/s,所述的角速度为0.2rad/s,适合移动机器人进行定位与建图的线速度及角速度使得移动机器人的工作效率更优越。
所述的移动机器人的里程计算单元计算得到的t时刻移动机器人的位姿信息Rt(Xt,Yt,βt)所采用的计算公式为:
其中,Xt,Yt,βt分别为t时刻移动机器人在世界坐标系的二维坐标和移动机器人朝向角度,Xt-1,Yt-1,βt-1分别为t-1时刻移动机器人在世界坐标系的二维坐标和移动机器人朝向角度,rt,lt分别为t-1到t时刻移动机器人左右轮的移动距离,B为左右轮的中心距,r为移动半径。
基于上述方法,本发明实施例还提供一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM系统,包括以下步骤:
移动机器人设置单元:移动机器人的底盘中心设置有IMU单元,移动机器人底盘中心线两侧均设有连接在一起的驱动轮、无刷直流电机和编码器,移动机器人沿前进方向的中心线两端设置有支撑轮,沿机器人前进方向的底盘上方固定相机单元,且沿相机单元坐标系的y轴方向,移动机器人的前进端的支撑轮上方设置激光雷达传感器;
AprilTag码设置单元:通过在环境空间中的目标物体上张贴AprilTag码,并使得AprilTag码与移动机器人的相机单元在同一水平线上呈平行设置;
初始位姿记录单元:记录初始时刻时移动机器人的初始位姿以及AprilTag码的初始位姿;
AprilTag码的偏差位姿获取单元:依据t时刻相机单元采集到的AprilTag码的位姿,获得AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt);
移动机器人位姿校正单元:利用AprilTag码的偏差位姿,获得移动机器人t时刻的偏差位姿ΔRt(ΔXRt,ΔYRt,ΔβRt),并对移动机器人的t时刻位姿信息进行校正;
所述移动机器人当前的偏差位姿如下:
其中,(XRt,YRt,βRt)表示移动机器人t时刻的位姿,通过里程计计算获得;
SLAM创建单元:将校正后的移动机器人位姿信息通过ROS机器人操作系统的节点进行发布出去,由SLAM定位与建图功能包订阅里程计位姿信息,并通过激光雷达传感器和里程计单元对环境空间进行地图构建,直到移动机器人将所在的环境空间地图构建完毕。
应当理解,本发明各个实施例中的功能单元模块可以集中在一个处理单元中,也可以是各个单元模块单独物理存在,也可以是两个或两个以上的单元模块集成在一个单元模块中,可以采用硬件或软件的形式来实现。
本发明实施例还提供一种计算机存储介质,包括计算机程序,所述程序被处理器执行时实现所述的一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法,其有益效果参见方法部分的有益效果,在此不再赘述。
本发明实施例还提供一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM设备,包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序,其特征在于,所述处理器执行所述程序时,以使所述基于AprilTag码视觉辅助定位的移动机器人激光SLAM设备实现所述的一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法。
本领域内的技术人员应明白,本申请的实施例可提供为方法、系统、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
最后应当说明的是:以上实施例仅用以说明本发明的技术方案而非对其限制,尽管参照上述实施例对本发明进行了详尽的说明,所属领域的普通技术人员应当理解,上述实施例仅仅是对本发明的示意性实现方式的解释,实施例中的细节并不构成对本发明范围的限制,在不背离本发明的精神和范围的情况下,任何基于本发明技术方案的等效变换、简单替换等显而易见的改变,均落在本发明保护范围之内。
Claims (10)
1.一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法,其特征在于,包括以下步骤:
步骤一、在环境空间中的目标物体上张贴AprilTag码,AprilTag码与移动机器人的相机单元在同一水平线上呈平行设置;
所述移动机器人的底盘中心设置有IMU单元,移动机器人底盘中心线两侧均设有连接在一起的驱动轮、无刷直流电机和编码器,移动机器人沿前进方向的中心线两端设置有支撑轮,沿机器人前进方向的底盘上方固定相机单元,且沿相机单元坐标系的y轴方向,移动机器人的前进端的支撑轮上方设置激光雷达传感器;
步骤二、记录初始时刻时移动机器人的初始位姿为R0(0,0,0)以及AprilTag码的初始位姿;
步骤三、依据t时刻相机单元采集到的AprilTag码的位姿,获得AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt);
步骤四、利用AprilTag码的偏差位姿,获得移动机器人t时刻的偏差位姿ΔRt(ΔXRt,ΔYRt,ΔβRt),并对移动机器人的t时刻偏差位姿信息进行校正;
所述移动机器人当前的偏差位姿如下:
其中,(XRt,YRt,βRt)表示移动机器人t时刻的位姿,通过里程计计算获得;
步骤五、将修正的里程计位姿信息通过ROS机器人操作系统的节点进行发布出去,由SLAM定位与建图功能包订阅里程计位姿信息,通过激光雷达传感器和里程计单元对环境空间进行地图构建,直到移动机器人将所在的环境空间地图构建完毕。
2.根据权利要求1所述的方法,其特征在于,当移动机器人上的相机单元在t时刻采集到一张AprilTag码的位姿时,所述AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt)按以下公式计算:
其中,(XAt,YAt,βAt)表示移动机器人上的相机单元在t时刻采集到的AprilTag码的位姿,(XA0,YA0,βA0)表示AprilTag码的初始位姿。
3.根据权利要求2所述的方法,其特征在于,当移动机器人上的相机单元在t时刻采集到两张AprilTag码的位姿时,所述AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt)按以下公式计算:
其中,(ΔXA1t,ΔYA1t,ΔβA1t)和(ΔXA2t,ΔYA2t,ΔβA2t)分别为第一张和第二张AprilTag码的偏差位姿,每张AprilTag码的偏差位姿均为当前时刻采集的位姿与初始时刻采集的位姿差值。
4.根据权利要求1所述的方法,其特征在于,在移动机器人进行环境空间地图构建过程中,移动机器人的线速度设置为在1m/s、角速度设置为0.2rad/s。
5.根据权利要求1或2所述的方法,其特征在于,所述相机单元为深度相机,所述相机单元通过USB串口与微型计算机相连接。
6.根据权利要求1所述的方法,其特征在于,在步骤五中,所述激光雷达传感器为单线激光雷达传感器,所述激光雷达传感器通过USB串口转RS422模块与微型计算机相连接,激光雷达传感器实时扫描障碍物在全局坐标系中的位置,通过ROS系统构建二维栅格地图。
7.根据权利要求1所述的方法,其特征在于,所述的移动机器人的里程计算单元计算得到的t时刻移动机器人的位姿信息Rt(Xt,Yt,βt)所采用的计算公式为:
其中,Xt,Yt,βt分别为t时刻移动机器人在世界坐标系的二维坐标和移动机器人朝向角度,Xt-1,Yt-1,βt-1分别为t-1时刻移动机器人在世界坐标系的二维坐标和移动机器人朝向角度,rt,lt分别为t-1到t时刻移动机器人左右轮的移动距离,B为左右轮的中心距,r为移动半径。
8.一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM系统,其特征在于,包括以下步骤:
移动机器人设置单元:移动机器人的底盘中心设置有IMU单元,移动机器人底盘中心线两侧均设有连接在一起的驱动轮、无刷直流电机和编码器,移动机器人沿前进方向的中心线两端设置有支撑轮,沿机器人前进方向的底盘上方固定相机单元,且沿相机单元坐标系的y轴方向,移动机器人的前进端的支撑轮上方设置激光雷达传感器;
AprilTag码设置单元:通过在环境空间中的目标物体上张贴AprilTag码,并使得AprilTag码与移动机器人的相机单元在同一水平线上呈平行设置;
初始位姿记录单元:记录初始时刻时移动机器人的初始位姿以及AprilTag码的初始位姿;
AprilTag码的偏差位姿获取单元:依据t时刻相机单元采集到的AprilTag码的位姿,获得AprilTag码的偏差位姿ΔAt(ΔXAt,ΔYAt,ΔβAt);
移动机器人位姿校正单元:利用AprilTag码的偏差位姿,获得移动机器人t时刻的偏差位姿ΔRt(ΔXRt,ΔYRt,ΔβRt),并对移动机器人的t时刻位姿信息进行校正;
所述移动机器人当前的偏差位姿如下:
其中,(XRt,YRt,βRt)表示移动机器人t时刻的位姿,通过里程计计算获得;
SLAM创建单元:将校正后的移动机器人位姿信息通过ROS机器人操作系统的节点进行发布出去,由SLAM定位与建图功能包订阅里程计位姿信息,并通过激光雷达传感器和里程计单元对环境空间进行地图构建,直到移动机器人将所在的环境空间地图构建完毕。
9.一种计算机可读存储介质,包括计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1-7任一项所述的一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法。
10.一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM设备,包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序,其特征在于,所述处理器执行所述程序时,以使所述基于AprilTag码视觉辅助定位的移动机器人激光SLAM设备实现如权利要求1-7任一项所述的一种基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010595256.7A CN111766603B (zh) | 2020-06-27 | 2020-06-27 | 基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法、系统、介质及设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010595256.7A CN111766603B (zh) | 2020-06-27 | 2020-06-27 | 基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法、系统、介质及设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111766603A CN111766603A (zh) | 2020-10-13 |
CN111766603B true CN111766603B (zh) | 2023-07-21 |
Family
ID=72722314
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010595256.7A Active CN111766603B (zh) | 2020-06-27 | 2020-06-27 | 基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法、系统、介质及设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111766603B (zh) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112833898B (zh) * | 2020-12-30 | 2023-03-21 | 清华大学 | 一种面向ros的无人车倒退方法 |
CN113310484B (zh) * | 2021-05-28 | 2022-06-24 | 杭州艾米机器人有限公司 | 一种移动机器人定位方法和系统 |
CN113671523A (zh) * | 2021-08-18 | 2021-11-19 | Oppo广东移动通信有限公司 | 机器人定位方法、装置、存储介质及机器人 |
CN114111791B (zh) * | 2021-11-22 | 2024-05-17 | 国网江苏省电力有限公司信息通信分公司 | 一种智能机器人室内自主导航方法、系统及存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106444750A (zh) * | 2016-09-13 | 2017-02-22 | 哈尔滨工业大学深圳研究生院 | 一种基于二维码定位的智能仓储移动机器人系统 |
CN109211251A (zh) * | 2018-09-21 | 2019-01-15 | 北京理工大学 | 一种基于激光和二维码融合的即时定位与地图构建方法 |
CN109579824A (zh) * | 2018-10-31 | 2019-04-05 | 重庆邮电大学 | 一种融入二维码信息的自适应蒙特卡诺定位方法 |
CN110488838A (zh) * | 2019-08-29 | 2019-11-22 | 四川阿泰因机器人智能装备有限公司 | 一种室内自主导航机器人精确重复定位方法 |
CN110673612A (zh) * | 2019-10-21 | 2020-01-10 | 重庆邮电大学 | 一种自主移动机器人二维码引导控制方法 |
-
2020
- 2020-06-27 CN CN202010595256.7A patent/CN111766603B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106444750A (zh) * | 2016-09-13 | 2017-02-22 | 哈尔滨工业大学深圳研究生院 | 一种基于二维码定位的智能仓储移动机器人系统 |
CN109211251A (zh) * | 2018-09-21 | 2019-01-15 | 北京理工大学 | 一种基于激光和二维码融合的即时定位与地图构建方法 |
CN109579824A (zh) * | 2018-10-31 | 2019-04-05 | 重庆邮电大学 | 一种融入二维码信息的自适应蒙特卡诺定位方法 |
CN110488838A (zh) * | 2019-08-29 | 2019-11-22 | 四川阿泰因机器人智能装备有限公司 | 一种室内自主导航机器人精确重复定位方法 |
CN110673612A (zh) * | 2019-10-21 | 2020-01-10 | 重庆邮电大学 | 一种自主移动机器人二维码引导控制方法 |
Non-Patent Citations (1)
Title |
---|
基于视觉伺服的无人机自主着陆仿真系统设计;杨建业;戚国庆;盛安冬;;电子设计工程(第11期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN111766603A (zh) | 2020-10-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111766603B (zh) | 基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法、系统、介质及设备 | |
US10796151B2 (en) | Mapping a space using a multi-directional camera | |
CN109975792B (zh) | 基于多传感器融合矫正多线激光雷达点云运动畸变的方法 | |
WO2021189468A1 (zh) | 激光雷达的姿态校正方法、装置和系统 | |
EP3011362B1 (en) | Systems and methods for tracking location of movable target object | |
Prusak et al. | Pose estimation and map building with a time-of-flight-camera for robot navigation | |
CN110345937A (zh) | 一种基于二维码的导航定姿定位方法和系统 | |
CN103412565B (zh) | 一种具有全局位置快速估计能力的机器人定位方法 | |
CN111624995B (zh) | 移动机器人高精度导航定位的方法 | |
CN110176038B (zh) | 校准车辆的摄像头的方法、系统和存储介质 | |
JP6827642B2 (ja) | 車輪付き車両用自己較正センサシステム | |
Ernst et al. | Camera calibration for lane and obstacle detection | |
CN110673608A (zh) | 一种机器人导航方法 | |
Schneider et al. | On the accuracy of dense fisheye stereo | |
CN115014338A (zh) | 一种基于二维码视觉和激光slam的移动机器人定位系统及方法 | |
US11514588B1 (en) | Object localization for mapping applications using geometric computer vision techniques | |
WO2016157428A1 (ja) | 計測装置、計測方法、及び、プログラム | |
Fang et al. | Ground-texture-based localization for intelligent vehicles | |
Yang et al. | Simultaneous estimation of ego-motion and vehicle distance by using a monocular camera | |
CN113030960A (zh) | 一种基于单目视觉slam的车辆定位方法 | |
Wang et al. | Measurement error correction model of TOF depth camera | |
CN116500595A (zh) | 激光雷达、相机和惯性传感器相互约束的外参标定方法 | |
Hedi et al. | A system for vehicle surround view | |
Chang et al. | Reconstruction of 3D contour with an active laser‐vision robotic system | |
Hoang et al. | Planar motion estimation using omnidirectional camera and laser rangefinder |
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 |