CN111649746A - 一种融合惯导测量和ArUco标记的定位与导航方法 - Google Patents
一种融合惯导测量和ArUco标记的定位与导航方法 Download PDFInfo
- Publication number
- CN111649746A CN111649746A CN202010526692.9A CN202010526692A CN111649746A CN 111649746 A CN111649746 A CN 111649746A CN 202010526692 A CN202010526692 A CN 202010526692A CN 111649746 A CN111649746 A CN 111649746A
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- coordinate system
- camera
- world coordinate
- robot
- 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.)
- Granted
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/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种融合惯导测量和ArUco标记的定位与导航方法,属于计算机视觉定位、移动机器人导航技术领域。本发明在ArUco地图环境下,统一了惯导元件和相机的坐标系,克服了因误识别或漏识别ArUco标记而导致移动机器人当前姿态丢失;一定程度上克服了相机视野有限的问题,提升了机器人的姿态感知能力,使机器人在导航中可以完成更复杂的姿态调整。
Description
技术领域
本发明属于计算机视觉定位、移动机器人导航技术领域,涉及一种室内定位与导航的方法,更具体地,涉及一种融合惯导测量和ArUco(Augmented Reality fromUniversity of Cordoba,科尔多瓦大学增强现实库)标记的定位与导航方法,可用于室内环境下移动机器人、无人机等智能移动设备的自主定位与导航。
背景技术
在计算机、传感器等领域的推动下,移动机器人技术得到了飞速发展,并且在医疗、军事、工业和太空等国防科技及经济生活领域得到了广泛地应用。定位导航是机器人运动控制和动作操作的基础性技术,尤其是在室内缺少卫星导航信号下,利用环境信息实现室内机器人的定位成为常用的实现方式。
室内定位和自主导航方法可分为相对估计方法和寻迹方法。相对估计方法是利用惯性导航元件或测距元件测算当前位置和相对移动距离,但缺点是元件测量数据存在漂移,而且精密仪器价格昂贵。寻迹方法需要布置轨道或路标,通过机器人搭载的电磁传感器或图像传感器识别定位,当机器人的任务发生变化时,需要重新布置路径。此外出现了利用传统二维码信息提供导航定位信息的方式,这种方式通过在地面布置二维码,机器人识别二维码信息从而判断自身位姿,并引导其运行。但是传统的二维码扫描距离近,在实际使用的时候,机器人不能很好地识别,有较大的局限性。而且此方法一般需要棋盘格状布置,布置前需要测量二维码的布置位置,布置成本较高。另外,此方法机器人只能在有二维码的路径上行进,限制了机器人的运动范围,面临一定的柔性不足。
为克服人工地标布置成本高、柔性不足等问题,本发明提出一种融合惯导测量和ArUco标记的室内定位导航方法。利用ArUco标记构建室内地图,以ArUco标记为导航信标实现机器人定位解算,同时,将惯导测量元件融合到基于ArUco标记的定位中,利用惯性测量组件的自主性和稳定性,在直角拐弯、ArUco标记丢失等情况下提供姿态输出,保证机器人位姿获取的稳定性,提升了导航鲁棒性。
本发明提供的方法利用ArUco标记建图实现机器人室内定位导航,同时,融合惯导测量信息克服导航在路径转角处无法识别ArUco标记和识别歧义性导致的姿态错误问题,增强导航***稳定性。
发明内容
针对现有技术中存在的上述技术问题,本发明的主要目的在于:针对传统室内导航方法布置成本高,柔性不足等问题,提供一种融合惯导测量和ArUco标记的定位与导航方法,以降低成本,提升***的柔性和鲁棒性。
为了实现上述目的,本发明采用如下技术方案:
一种融合惯导测量和ArUco标记的定位与导航方法,采用惯导元件和相机,包括以下步骤:
步骤1:建立ArUco地图;
步骤2:根据任务需要建立路径,路径由一系列坐标点表示,并设定任务目标点Pset;
步骤3:固定惯导元件和相机的相对位姿,计算得到惯性元件坐标系与相机坐标系之间的旋转关系;
步骤4:惯导元件开始解算姿态;
步骤5:机器人按固定的角速度旋转,直到相机观测到ArUco标记,并成功获得相机在世界坐标系下的姿态,设定当前时刻为k时刻,记录当前相机在世界坐标系中的姿态和惯导元件的自解算姿态;
步骤6:根据步骤5所得到的相机在世界坐标系中的姿态和惯导元件的自解算姿态,计算惯导元件在世界坐标系中的姿态;
步骤7:设定下一时刻为k+1时刻,根据步骤6所得到的惯导元件在世界坐标系中的姿态,计算得到k+1时刻惯导元件在世界坐标系中的姿态;
步骤8:根据步骤7所得到的k+1时刻惯导元件在世界坐标系中的姿态,计算得到k+1时刻相机在世界坐标系中的姿态;
步骤9:通过ArUco标记得到机器人在世界坐标系下的位置Pcurrent,并计算得到设定路径上距离机器人最近的一点Pnearest,Pnearest沿设定路径向Pset方向的第n个点记为Paim;
步骤10:计算Pnearest到Paim的向量Vaim,Vaim即为机器人的方向向量;
步骤11:根据k+1时刻机器人的姿态向量Vrobot和方向向量Vaim,计算机器人的偏航角α和俯仰角β;根据α和β的大小,控制机器人的运动角速度,根据Paim和设定点Pset之间的距离控制机器人的运动线速度;
步骤12:判断器人的当前位置Pcurrent是否等于Pset,若不相等则跳转步骤7,若相等则结束导航。
优选地,在步骤3中,
惯导元件和相机采用正交固定的方法,能够直接推导出相机坐标系和惯导坐标系之间的旋转关系;
惯导元件和相机采用联合标定的方法,能够标定出惯导元件和相机之间的具体坐标关系。
优选地,在步骤4中,
惯导元件开始解算的姿态的四元数初值一般设定为[1 0 0 0]。
优选地,在步骤5中,相机通过识别ArUco标记获得在世界坐标系下的姿态后,取一段时间内的姿态平均数据,以增加定位精度;
优选地,在步骤6中,根据公式(1),计算惯导元件在世界坐标系中的姿态:
式中,q为四元数,为四元数表示旋时转的运算,k表示相机通过识别ArUco标记获得在世界坐标系下的姿态的时刻,为惯导元件在初始时刻即0时刻在世界坐标系中的姿态,为k时刻相机在世界坐标系中的姿态,为k时刻相机相对惯导元件0时刻坐标系的旋转;
优选地,在步骤7中,根据公式(3),计算k+1时刻惯导元件在世界坐标系中的姿态:
优选地,在步骤8中,根据公式(4),计算k+1时刻相机在世界坐标系中的姿态:
本发明所带来的有益技术效果:
(1)在ArUco地图环境下,统一了惯导元件和相机的坐标系,克服了因误识别或漏识别ArUco标记而导致移动机器人当前姿态丢失。
(2)一定程度上克服了相机视野有限的问题,提升了机器人的姿态感知能力,使机器人在导航中可以完成更复杂的姿态调整。
附图说明
图1为室内ArUco标记布置示意图。
图2为ArUco地图和任务路径效果示意图。
图3为导航***坐标系关系示意图。
图4为AGV初始姿态获取流程图。
图5为AGV导航控制示意图。
图6为融合惯导测量和ArUco标记的视觉定位与导航实现流程图。
具体实施方式
下面结合附图以及具体实施方式对本发明作进一步详细说明:
以AGV(Automated Guided Vehicle,自动导引车)导航为例,附图和实施例说明如下。
步骤1:如图1所示,在室内布置ArUco标记,标记由ArUco官方库生成,大小统一,布置于墙面等相机可识别范围内。指定第26号标记为参考标记,并完成视觉建图,ArUco地图如图2所示。另外,采用张正友标定法标定相机内参数。
步骤2:根据任务需要建立导航路径,如图2所示。根据26号标记坐标系建立数学方程,并离散成一系列点,离散密度为1000个/m。本例中导航路径为一条带有直角转弯的路径,路径方程为:
步骤3:如图3所示本发明中各个坐标系关系示意图,本发明利用相机和ArUco标记获得相机在世界坐标系下的位置信息,利用惯导元件(IMU,Inertial Measurement Unit)获得相机在世界坐标系下的姿态信息。图3中世界坐标系为ArUco地图所指定的ArUco标记坐标系,记作w,X-Y-Z构成右手坐标系。相机坐标系与IMU坐标系关系固定,且IMU与相机正交安装。相机坐标系记作c,X轴为AGV的前进方向,Y轴垂直车体运动平面向下,Z轴即相机光轴,水平指向车体前进方向的左侧。IMU的坐标系记作b,X轴水平指向车体前进方向的左侧,与相机的Z轴方向相同,Y轴与AGV前进方向相反,Z轴垂直车体运动平面向上。
将旋转用四元数形式表示:
步骤5:AGV按一定的角速度旋转,识别模式为FAST模式,以增加标记的识别速度,直到相机观测到ArUco标记,识别模式切换为NORMAL模式,以增加识别精度。相机成功识别ArUco标记之后,获得标记角点的像素坐标,通过PnP算法得到相机在世界坐标系下的位置和姿态,取一秒内的姿态平均数据,以增加定位精度,平均姿态的求取方法如下:
1)构造如下姿态代价函数:
对于任意的3阶正交矩阵A,有:
tr{ATA}=3
因此:
于是,求马氏距离最小值问题即可转化为求取迹的最大值问题:
步骤9:通过ArUco标记得到机器人在世界坐标系下的位置Pcurrent,如图5所示,计算得到设定路径上距离机器人最近的一点Pnearest,沿设定路径的向目标方向的第n个点记为Paim,n的取值正比于AGV的最大运行速度和路径点密度。
步骤10:计算Pnearest到Paim的向量Vaim,Vaim即为机器人的目标方向。
步骤11:根据k+1时刻机器人的姿态向量Vrobot和方向向量Vaim,计算机器人的偏航角α。根据α大小,控制机器人的角速度;根据Paim和设定点Pset之间的距离控制机器人的线速度。
步骤12:判断器人的当前位置Pcurrent是否等于Pset,若不相等跳转步骤(7),若相等则结束导航。
当然,上述说明并非是对本发明的限制,本发明也并不仅限于上述举例,本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换,也应属于本发明的保护范围。
Claims (7)
1.一种融合惯导测量和ArUco(标记的定位与导航方法,其特征在于:采用惯导元件和相机,包括以下步骤:
步骤1:建立ArUco地图;
步骤2:根据任务需要建立路径,路径由一系列坐标点表示,并设定任务目标点Pset;
步骤3:固定惯导元件和相机的相对位姿,计算得到惯导元件坐标系与相机坐标系之间的旋转关系;
步骤4:惯导元件开始解算姿态;
步骤5:机器人按固定的角速度旋转,直到相机观测到ArUco标记,并成功获得相机在世界坐标系下的姿态,设定当前时刻为k时刻,记录当前相机在世界坐标系中的姿态和惯导元件的自解算姿态;
步骤6:根据步骤5所得到的相机在世界坐标系中的姿态和惯导元件的自解算姿态,计算惯导元件在世界坐标系中的姿态;
步骤7:设定下一时刻为k+1时刻,根据步骤6所得到的惯导元件在世界坐标系中的姿态,计算得到k+1时刻惯导元件在世界坐标系中的姿态;
步骤8:根据步骤7所得到的k+1时刻惯导元件在世界坐标系中的姿态,计算得到k+1时刻相机在世界坐标系中的姿态;
步骤9:通过ArUco标记得到机器人在世界坐标系下的位置Pcurrent,并计算得到设定路径上距离机器人最近的一点Pnearest,Pnearest沿设定路径向Pset方向的第n个点记为Paim;
步骤10:计算Pnearest到Paim的向量Vaim,Vaim即为机器人的方向向量;
步骤11:根据t+Δt时刻机器人的姿态向量Vrobot和方向向量Vaim,计算机器人的偏航角α和俯仰角β;根据α和β的大小,控制机器人的运动角速度,根据Paim和设定点Pset之间的距离控制机器人的运动线速度;
步骤12:判断器人的当前位置Pcurrent是否等于Pset,若不相等则跳转步骤7,若相等则结束导航。
2.根据权利要求1所述的融合惯导测量和ArUco标记的定位与导航方法,其特征在于:在步骤3中;
惯导元件和相机采用正交固定的方法,能够直接推导出相机坐标系和惯导坐标系之间的旋转关系;
惯导元件和相机采用联合标定的方法,能够标定出惯导元件和相机之间的具体坐标关系。
3.根据权利要求1所述的融合惯导测量和ArUco标记的定位与导航方法,其特征在于:在步骤4中,
惯导元件开始解算的姿态的四元数初值一般设定为[1 0 0 0]。
4.根据权利要求1所述的融合惯导测量和ArUco标记的定位与导航方法,其特征在于:在步骤5中,相机通过识别ArUco标记获得在世界坐标系下的姿态后,取一段时间内的姿态平均数据,以增加定位精度。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010526692.9A CN111649746B (zh) | 2020-06-11 | 2020-06-11 | 一种融合惯导测量和ArUco标记的定位与导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010526692.9A CN111649746B (zh) | 2020-06-11 | 2020-06-11 | 一种融合惯导测量和ArUco标记的定位与导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111649746A true CN111649746A (zh) | 2020-09-11 |
CN111649746B CN111649746B (zh) | 2023-03-28 |
Family
ID=72352846
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010526692.9A Active CN111649746B (zh) | 2020-06-11 | 2020-06-11 | 一种融合惯导测量和ArUco标记的定位与导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111649746B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113917927A (zh) * | 2021-10-26 | 2022-01-11 | 沈阳航天新光集团有限公司 | 一种基于Leap Motion交互的仿生机器鱼控制*** |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106679648A (zh) * | 2016-12-08 | 2017-05-17 | 东南大学 | 一种基于遗传算法的视觉惯性组合的slam方法 |
CN106705965A (zh) * | 2017-01-12 | 2017-05-24 | 苏州中德睿博智能科技有限公司 | 场景三维数据配准方法及导航***误差校正方法 |
CN109063703A (zh) * | 2018-06-29 | 2018-12-21 | 南京睿悦信息技术有限公司 | 基于标识识别和惯性测量单元融合的增强现实定位算法 |
CN109374003A (zh) * | 2018-11-06 | 2019-02-22 | 山东科技大学 | 一种基于ArUco码的移动机器人视觉定位与导航方法 |
CN109443389A (zh) * | 2018-11-28 | 2019-03-08 | 电子科技大学 | 动作捕获***中基于惯性传感器单步标定的姿态融合方法 |
US20190259176A1 (en) * | 2018-01-19 | 2019-08-22 | TuSimple | Method and device to determine the camera position and angle |
-
2020
- 2020-06-11 CN CN202010526692.9A patent/CN111649746B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106679648A (zh) * | 2016-12-08 | 2017-05-17 | 东南大学 | 一种基于遗传算法的视觉惯性组合的slam方法 |
CN106705965A (zh) * | 2017-01-12 | 2017-05-24 | 苏州中德睿博智能科技有限公司 | 场景三维数据配准方法及导航***误差校正方法 |
US20190259176A1 (en) * | 2018-01-19 | 2019-08-22 | TuSimple | Method and device to determine the camera position and angle |
CN109063703A (zh) * | 2018-06-29 | 2018-12-21 | 南京睿悦信息技术有限公司 | 基于标识识别和惯性测量单元融合的增强现实定位算法 |
CN109374003A (zh) * | 2018-11-06 | 2019-02-22 | 山东科技大学 | 一种基于ArUco码的移动机器人视觉定位与导航方法 |
CN109443389A (zh) * | 2018-11-28 | 2019-03-08 | 电子科技大学 | 动作捕获***中基于惯性传感器单步标定的姿态融合方法 |
Non-Patent Citations (1)
Title |
---|
苏昂等: "未知环境下飞行器视觉/惯导组合测速测高方法", 《国防科技大学学报》 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113917927A (zh) * | 2021-10-26 | 2022-01-11 | 沈阳航天新光集团有限公司 | 一种基于Leap Motion交互的仿生机器鱼控制*** |
Also Published As
Publication number | Publication date |
---|---|
CN111649746B (zh) | 2023-03-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110673115B (zh) | 雷达与组合导航***的联合标定方法、装置、设备及介质 | |
US20210333797A1 (en) | Marker-combined simultaneous localization and mapping navigation method, device and system | |
Chen et al. | Qualitative vision-based path following | |
Yang et al. | Autonomous landing of MAVs on an arbitrarily textured landing site using onboard monocular vision | |
KR102627453B1 (ko) | 위치 추정 장치 및 방법 | |
CN106153008B (zh) | 一种基于视觉的旋翼无人机三维目标定位方法 | |
CN111426320A (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
Ranft et al. | 3d perception for autonomous navigation of a low-cost mav using minimal landmarks | |
Yang et al. | Onboard monocular vision for landing of an MAV on a landing site specified by a single reference image | |
CN111649746B (zh) | 一种融合惯导测量和ArUco标记的定位与导航方法 | |
Niu et al. | Camera-based lane-aided multi-information integration for land vehicle navigation | |
Shmatko et al. | Estimation of rotation measurement error of objects using computer simulation | |
CN112862818A (zh) | 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法 | |
Burdziakowski | Towards precise visual navigation and direct georeferencing for MAV using ORB-SLAM2 | |
KR20210010309A (ko) | 항공사진을 이용하여 3차원 지도를 생성하는 장치 및 방법 | |
CN111521996A (zh) | 一种激光雷达的安装标定方法 | |
US20240077880A1 (en) | Slope location correction method and apparatus, robot and readable storage medium | |
CN114326765B (zh) | 一种用于无人机视觉着陆的地标跟踪控制***及方法 | |
CN112904883B (zh) | 四足机器人地形感知方法、运动控制方法及*** | |
CN112857367B (zh) | 一种基于机器视觉和惯性导航的掘进机位姿检测方法 | |
US11348278B2 (en) | Object detection | |
CN114003041A (zh) | 一种多无人车协同探测*** | |
Wang et al. | Indoor visual navigation system based on paired-landmark for small UAVs | |
CN111258306B (zh) | 基于成像设备的车辆定位方法及装置 | |
Rao et al. | Experiments in robot control from uncalibrated overhead imagery |
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 |