CN110058594A - 基于示教的多传感器的移动机器人定位导航***及方法 - Google Patents
基于示教的多传感器的移动机器人定位导航***及方法 Download PDFInfo
- Publication number
- CN110058594A CN110058594A CN201910348071.3A CN201910348071A CN110058594A CN 110058594 A CN110058594 A CN 110058594A CN 201910348071 A CN201910348071 A CN 201910348071A CN 110058594 A CN110058594 A CN 110058594A
- Authority
- CN
- China
- Prior art keywords
- navigation
- connect
- module
- personal computer
- industrial personal
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 21
- 230000004807 localization Effects 0.000 title claims abstract description 18
- 230000033001 locomotion Effects 0.000 claims abstract description 37
- 230000000694 effects Effects 0.000 claims abstract description 5
- 238000012544 monitoring process Methods 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000009466 transformation Effects 0.000 claims description 9
- 238000011161 development Methods 0.000 claims description 8
- 230000005484 gravity Effects 0.000 claims description 6
- 238000013519 translation Methods 0.000 claims description 6
- 238000013459 approach Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 239000003638 chemical reducing agent Substances 0.000 claims description 3
- 238000004891 communication Methods 0.000 claims description 3
- 238000003780 insertion Methods 0.000 claims 1
- 230000037431 insertion Effects 0.000 claims 1
- 230000005540 biological transmission Effects 0.000 abstract description 2
- 230000007613 environmental effect Effects 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 7
- 230000004927 fusion Effects 0.000 description 6
- 241001269238 Data Species 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000037361 pathway Effects 0.000 description 4
- QSHDDOUJBYECFT-UHFFFAOYSA-N mercury Chemical compound [Hg] QSHDDOUJBYECFT-UHFFFAOYSA-N 0.000 description 3
- 229910052753 mercury Inorganic materials 0.000 description 3
- 238000009987 spinning Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000008901 benefit Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 210000004556 brain Anatomy 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000000465 moulding Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000004321 preservation Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于示教的多传感器的移动机器人定位导航***及方法，包括上层导航模块、下层运动控制模块、手柄、计算机；所述的手柄提供小车运动控制的使能作用；所述的计算机用于监控和调试机器人；所述下层运动控制模块用于实现小车平台的运动控制、显示小车状态信息以及编码器数据的读取与发送，所述上层导航模块用于采集点云数据与速度数据，本发明构成合理、简洁，方法便捷，运用本发明可以快速地进行室内AGV的导航，非专业人员易上手、操作简单、使用方便。其***的方法计算效率高，而且可以自主学习和遍历任意形状的路径，同时还能根据三维激光数据和由里程计信息提供的位置信息对路速度大小和方向进行修正。
Description
技术领域
本发明属于工业自动化领域，特别涉及一种基于示教的多传感器的移动机器人定位导航***及方法。
背景技术
随着人力成本的提高和科学技术的发展，机器人(尤其是工业机器人)在工业自动化领域越来越广泛的应用。自动导航搬运车(Automated Guided Vehicle以下简称AGV)也被称作搬运机器人，是现代智能物流***中的重要环节，然而目前主流的AGV导引技术并不能使其在工作中从真正意义上实现完全自主。
导航作为核心技术之一，是AGV技术发展的最重要标志。传统导航方式以电磁导航和磁带导航为代表，使用物理路径，环境表示简单，无需自动地图创建技术，定位时只需要通过传感器检测车体与物理路径之间的偏差。随着科技的进步，激光传感器、图像传感器逐步应用到AGV导航技术中。由于具有定位精度高，无需对地面进行改造，路径柔性高的优点，基于激光雷达的AGV导航技术即激光导航受到广泛研究并得到成功应用。与传统导航方式相比，激光导航涉及面多，技术复杂。首先，激光导航不存在物理路径，因此无法像传统导航方式那样直接检测车体与物理路径之间的偏差。因此在激光导航中，AGV通过激光雷达实时采集环境数据，通过将环境数据的处理结果与事先建立的环境地图进行对比获得自身在地图中的位姿估计，这种位姿估计即定位比传统导航方式中的直接偏差计算要复杂。其次，激光导航中使用的地图具有明显的测度信息，需要进行精确绘制，所创建的环境地图精度影响AGV定位精度。
在未知环境中，由于环境感知传感器的探测范围和测量精度限制、环境中墙壁等物体对传感器探测的遮挡等问题，机器人无法通过一次测量创建全局环境地图。因此，机器人只能在不断地环境探索过程中获取足够的环境感知数据，才能完成全局环境地图的创建工作。对于机器人在环境中各个位置创建的局部地图而言，只有确切地知道机器人各位置的位姿才能将该局部地图转换为全局地图，即地图的创建依赖于机器人的位姿。然而现实情况是，机器人的位姿往往是通过环境地图得到的，即机器人的位姿依赖于环境地图。位姿和地图估计之间的这种相互依赖的关系给机器人在未知环境下的导航提出了新的课题，称之为“同时定位与地图构建”(Simultaneous Localization and Mapping以下简称SLAM)问题。因此SLAM的最终目的是创建一致的环境地图，是否能够实现正确闭环是方法自身性能的一种体现；而基于闭环消除定位误差的地图创建方法，在通过航迹推测所估计的位姿误差较大的情况下，只能通过辅助方法建立约束条件，即在建立正确闭环的前提下，再通过优化方法校正机器人位姿和创建环境地图。因此slam***这样的实现需要构建全局一致的地图因此带来更高的计算成本。
发明内容
为了克服现有技术的不足，本发明提出了一种基于基于示教的多传感器的移动机器人定位导航***及方法。
本发明所采用的技术方案为：一种基于基于示教的多传感器的移动机器人定位导航***包括：上层导航模块、下层运动控制模块、手柄以及计算机；
所述手柄与下层运动控制模块连接，所述下层运动控制模块与上层导航模块连接，所述上层导航模块与计算机连接；
所述上层导航模块包括路由器、里程计、三维激光、车载电池以及升压模块；
所述下层运动控制模块包括：工控机、TTL转USB模块、路由器、自动导航搬运车AGV、光电编码器、直流减速电机及驱动器、嵌入式开发板以及供电电源；
所述供电电源与自动导航搬运车AGV连接，所述光电编码器与直流减速电机及驱动器放置在方向轮小车中、所述光电编码器与里程计连接，所述直流减速电机及驱动器与嵌入式开发板连接，所述嵌入式开发板与TTL转USB模块连接，所述TTL转USB模块与工控机连接；所述手柄与工控机连接；
所述车载电池分别与三维激光、里程计、路由器以及升压模块连接，所述升压模块还与工控机连接，所述三维激光还与路由器连接，所述里程计还与路由器连接，所述路由器分别与工控机、计算机连接；
所述的手柄提供小车运动控制的使能作用；
所述的计算机用于监控和调试机器人；
所述的TTL转USB模块用于实现嵌入式开发板与工控机的通信；
所述的升压模块用于实现将车载电池的电压转化为工控机的标准电压；
所述的供电电源为自动导航搬运车AGV提供电能；
所述的工控机通过路由器与三维激光进行通讯，获取三维激光所生成的三维点云并计算机器人的位姿；
所述的光电编码器用于计算自动导航搬运车AGV轮子转过的圈数；
所述的直流减速电机及驱动器为自动导航搬运车AGV的动力驱动装置；
所述的嵌入式开发版为自动导航搬运车AGV的底层运动控制模块；
所述里程计根据光电编码器的数据推算机器人的里程信息；
一种基于示教的多传感器的移动机器人定位导航方法，采用前述的一种基于示教的多传感器的移动机器人定位导航***实现，包括以下步骤：
步骤1、在教学阶段，通过手柄控制将速度指令发送给工控机，工控机将指令发送给TTL转USB模块，TTL转USB模块再将运动指令发送给嵌入式开发板STM32，嵌入式开发板STM32将接收到的指令发送给直流减速机及驱动器以控制机器人运动；在机器人运动的过程中，光电编码器采集速度信息给里程计、三维激光采集点云数据信息，里程计与三维激光将采集的信息通过路由器发送给工控机与计算机、同时也会记录点云数据；教学完成后，停止录制，点云数据会记录在数据库中；
步骤2、在重复阶段，通过手柄按钮将教学过程中的命令再次回放；
步骤3、自动导航搬运车读取机器人当前获得的点云，并将其与教学阶段保存的点云进行比较，并计算从读取的三维点云到记录的三维点云的变换，从而计算出机器人的位姿；
将机器人的位姿求解问题转换为数学问题，
q＝[qR|qT]T
其中Np为点数，xi和pi为匹配的点对，四元数是旋转的得一种表达形式，此处旋转向量qR用四元数表达，qτ是平移向量，qR和qτ共同组成完全坐标变换向量q，R(qR)是旋转向量qR对应的旋转矩阵，f(q)为求解一个q使得匹配得xi与pi之间得距离最小函数。
步骤3.1根据当前机器人获得的点云数据目标点集P中的点坐标，在教学阶段存储的点云点集曲面S上搜索相应最近点点集形成参考集X；
步骤3.2计算目标点集P和参考点集X的重心：其中μp、μx分别为点集P和X的重心，Np和Nx分别为点集P和X的点数；
步骤3.3由点集P和X构造协方差矩阵
步骤3.4由协方差矩阵构造4×4对称矩阵：
其中，I3是3x3单位矩阵，tr(∑P，X)是矩阵∑P，X的迹，矩阵A＝∑P，X+∑P，X T，Aij表示矩阵A的第i行、第j列的元素；
步骤3.5计算Q(∑P，X)的特征值和特征向量，其最大特征值对应的特征向量[q0 q1q2 q3]T即为最佳旋转向量其中q0、q1、q2、q3为四元数；
步骤3.6计算最佳平移向量
其中
步骤3.7得到求得最小均方误差dmin＝f(q)；
步骤4、将计算得到的完全坐标变换向量转换为机器人的位姿发送到嵌入式开发板STM32中，使用位置PID算法补偿机器人当前位姿与教学阶段存储的对应位姿的误差，完成移动机器人的导航与定位；
本发明的有益技术效果：本发明***理论结合实际，构成合理、简洁，方法便捷，运用本发明可以快速地进行室内AGV的导航，非专业人员易上手、操作简单、使用方便。
该方法计算效率高，无需照相机标定，而且可以自主学习和遍历任意形状的路径。在教学阶段，机器人是由一名人类操作者驾驶的，机器人会存储它的速度、激光扫描图和里程计信息。在自主导航中，该方法在二维甚至是三维空间中无需显式机器人定位，只需回放在教学阶段学习的速度，同时还能根据三维激光数据和由里程计信息生成的位置信息对路速度大小和方向进行修正。因此，机器人可以重复地沿着预期的路径自动驾驶而无需人工干预。
附图说明
图1为本发明基于示教的多传感器融合的移动机器人定位与导航***总结构框图；
图2为本发明基于示教的多传感器融合的移动机器人定位与导航***三维点云生成示意图；
图3为本发明基于示教的多传感器融合的移动机器人定位与导航***软件框架示意图；
图4为本发明基于示教的多传感器融合的移动机器人定位与导航***硬件连接示意图；
图5为本发明具体实施方式的方法流程图；
图6为本发明基于示教的多传感器融合的移动机器人定位与导航***软件定位算法中的核心算法ICP算法流程图；
图中，ENCA1、ENCB1、ENCC1、ENCD1是四路PWM作为直流减速电机及驱动器得控制信号。
具体实施方式
下面结合附图和实施例，对本发明做进一步的解释说明。
本实施例的***总结构框如图1所示，嵌入式开发板的型号为STM32，工控机的型号为占美gk400，TTL转USB模块的型号是CH340G，三维激光的型号是VLP-16，车载电池的型号为YSN-1211000，升压模块的型号为MKX-DC12V～19V,车载电池的型号为凯美威12V100A，供电电源的型号为SKYRC IMAX B6，路由器的型号为MERCURY MW325R，手柄的型号为SonyPS3。本实施例中，各部件的连接关系为：
一种基于示教的多传感器的移动机器人定位导航***：上层导航模块、下层运动控制模块、手柄以及计算机；
手柄与下层控制模块连接，下层控制模块与上层导航模块连接，上层导航模块与计算机连接；
下层运动控制模块用于实现小车平台的运动控制、显示小车状态信息以及编码器数据的读取与发送，包括工控机、TTL转USB模块、自动导航搬运车AGV、光电编码器、直流减速电机及驱动器、嵌入式开发板以及供电电源；
上层导航模块用于采集点云数据，其包括包括路由器、里程计、三维激光、车载电池以及升压模块；
供电电源与自动导航搬运车AGV连接，光电编码器与直流减速电机及驱动器设置在方向轮小车中、光电编码器与里程计连接，直流减速电机及驱动器与嵌入式开发板连接，嵌入式开发板与TTL转USB模块连接，TTL转USB模块与工控机连接；手柄与工控机连接；
车载电池分别与三维激光、里程计、路由器以及升压模块连接，升压模块还与工控机连接，三维激光还与路由器连接，里程计还与路由器连接，路由器分别与工控机、计算机连接；升压模块MKX-DC12V～19V、三维激光VLP-16、路由器MERCURY MW325R以及里程计均直接连接车载电池凯美威12V100A由其供电。
手柄提供小车运动控制的使能作用；
计算机用于监控和调试机器人；
TTL转USB模块用于实现嵌入式开发板与工控机的通信；
升压模块用于实现将车载电池的电压转化为工控机的标准电压；
供电电源为AGV小车提供电能；
工控机通过路由器与三维激光进行通讯，获取三维激光所生成的三维点云并计算机器人的位姿；
光电编码器用于计算AGV轮子转过的圈数；
直流减速电机及驱动器为AGV的动力驱动装置；
嵌入式开发版为AGV的底层运动控制模块；
里程计根据光电编码器的数据推算机器人的里程信息；
工控机通过路由器与三维激光行通讯，获取三维激光所生成的三维点云并运行后续的算法程序；
图4为下层导航单元的硬件连接示意图，搭载Linux***的占美gk400工控机是整个装置导航单元的大脑，它一方面通过TTL转USB模块CH340G给STM32发送控制指令，使其能够前、后、左、右或自转运动，另一方面通过路由器读取得里程计数据和激光雷达的数据，如图2三维点云生成示意图所示，然后通过算法将这两种传感器数据的融合实现定位与导航。
激光雷达通过螺丝固定在雷达支撑架上面，雷达支撑架通过螺丝与万向轮小车上层平台连接；激光雷达的供电线接上层车载电池凯美威12V100A，数据线接上层路由器MERCURYMW325R(连接车载电池凯美威12V100A供电)；车载电池12V端口接下层升压模块MKX-DC12V～19V，后转接工控机占美gk400；路由器MERCURY MW325R的两个LAN口，一个接三维激光VLP-16，另外一个接工控机占美gk400；通过路由器的有线连接，确保了激光雷达与工控机之间数据传输的稳定性、安全性和实时性，为工控机上运行的示教软件实时处理激光雷达发来的大量三维点云数据提供了有效支持。
工控机中存储着导航程序，嵌入式开发板STM32存储着运动控制程序，导航程序与运动控制程序均为基于ROS操作***，用C++语言编写；包含了上层的导航模块与底层运动控制模块两部分，实现整个***的完整运行，其中上层导航模块的程序在工控机中运行，底层运动控制模块在嵌入式开发板STM32上运行；
本实例的软件框架如图3所示，在教学阶段，用手柄驱动机器人在前、后、左、右和自转运动的基础上移动任意路径，并将路径点坐标即机器人的位姿和三维激光点云数据和速度控制指令存储在工控机上的数据库中，在重复阶段，上层的导航模块检索数据库，将数据库中保存的路径点坐标参考路径，存储的速度作为参考速度，将数据库中的保存的点云数据与机器人当前获得的点云数据比较，确定机器人当前的位姿并与存储的机器人位姿计算一个偏差作为补偿叠加到参考速度指令上，然后发送给底层运动控制模块；底层运动控制模块全部在STM32中执行，包括电机运动控制程序和数据读收发程序，其中与工控机的数据收发程序基于SCIP2.0协议编写；下层运动控制模块接收上层导航模块的速度控制指令，借助万向轮的特性，实现了AGV的前、后、左、右和自转运动；四个独立的车轮光电编码器值通过该tcp/ip通讯程序传送到上层导航模块进行解算，得出小车的里程计信息。上层导航模块基于Linux环境下的ROS操作***，分为教学和重复两个阶段。
如图5所示，基于激光和里程计定位的示教导航方法，具体步骤为：
步骤1、在教学阶段，通过手柄控制将速度指令发送给工控机，工控机将指令发送给TTL转USB模块，TTL转USB模块再将运动指令发送给嵌入式开发板STM32，嵌入式开发板STM32将接收到的指令发送给直流减速机及驱动器以控制机器人运动；在机器人运动的过程中，光电编码器采集速度信息给里程计、三维激光采集点云数据信息，里程计与三维激光将采集的信息通过路由器发送给工控机与计算机、同时也会记录点云数据；教学完成后，停止录制，点云数据会记录在数据库中；
步骤2、在重复阶段，通过手柄按钮将教学过程中的命令将再次回放；
步骤3、运行定位算法模块，自动导航搬运车读取机器人当前获得的点云，并将其与教学阶段保存的点云进行比较，并计算从读取的三维点云到记录的三维点云的变换，从而计算出机器人的位姿；
将机器人的位姿求解问题转换为数学问题，
q＝[qR|qT]T
其中Np为点数，xi和pi为匹配的点对，四元数是旋转的得一种表达形式，此处旋转向量qR用四元数表达，qτ是平移向量，qR和qτ共同组成完全坐标变换向量q，R(qR)是旋转向量qR对应的旋转矩阵，f(q)为求解一个q使得匹配得xi与pi之间得距离最小函数。
步骤3.1如图6所示，根据当前机器人获得的点云数据目标点集P中的点坐标，在教学阶段存储的点云点集曲面S上搜索相应最近点点集形成参考集X；
步骤3.2计算目标点集P和参考点集X的重心：其中μp、μx分别为点集P和X的重心，Np和Nx分别为点集P和X的点数；
步骤3.3由点集P和X构造协方差矩阵
步骤3.4由协方差矩阵构造4×4对称矩阵：
其中，I3是3x3单位矩阵，tr(∑FX)是矩阵∑P，X的迹，矩阵A＝∑P，X+∑P，X T，Aij表示矩阵A的第i行、第j列的元素；
步骤3.5计算Q(∑F，X)的特征值和特征向量，其最大特征值对应的特征向量[q0 q1q2 q3]T即为最佳旋转向量qR＝[q0q1q2q3]T；其中q0、q1、q2、q3为四元数；
步骤3.6计算最佳平移向量
其中
步骤3.7得到q＝[qR|qT]r，求得最小均方误差dmin＝f(q)；
步骤4、将计算得到的完全坐标变换向量转换为机器人的位姿然后发送到工控机，工控机将控制位置PID算法补偿机器人当前位姿与教学阶段存储的对应位姿的误差，完成移动机器人的导航与定位；
本***的核心算法是基于迭代最近点(Interative Closet Points，简称ICP)点云匹配算法。
三维点云匹配是一个非常重要的中间步骤，它在表面重建、三维物体识别、相机定位等问题中有着极其重要的应用。对于三维点云匹配问题，研究者提出了很多解决方案，但是应用最广泛的，影响最大的还是ICP算法，它是基于纯粹几何模型的三维点云匹配算法，由于它的强大功能以及高精确度，很快就成为点云匹配中的主流算法。
其中电机运动控制程序如下所示：
1.从数据库中检索机器人的当前的位姿qk_expect和3维点云数据，将3维点云数据和机器人当前获得的3维点云数据送到定位算法模块得到机器人的相对于数据库点云的位姿qk_calc，计算PID位置环的输入偏差，其中k表示第k时刻，
Δqk＝qk_expect-qk_calc；
2.依据PID的公式计算位置环的输出，其中upk，Kpp和Kpi分别表示PID位置环的输出，PID的比例和积分系数，
3.利用在数据库中检索机器人的当前期望的速度vk_expect和通过里程计获得机器人当前真实速度vk_truth，和位置PID环的输出计算其偏差Δvk，其中k表示当前时刻，α表示位置环的输出在偏差Δvk中的权重，
Δvk＝αupk+vk_expect-vk_truth，
4.计算速度环的PID输出，其中uvk，Kvp，Kvd，Kvi分别表示速度环PID输出，比例系数，微分系数和积分系数，
5.计算最终的输出速度，其中最终的输出速度用Vk表示，
Vk＝vk_exect+uvk。
Claims (3)
1.一种基于示教的多传感器的移动机器人定位导航***，其特征在于，包括上层导航模块、下层运动控制模块、手柄以及计算机；
所述手柄与下层运动控制模块连接，所述下层运动控制模块与上层导航模块连接，所述上层导航模块与计算机连接；
所述上层导航模块包括路由器、里程计、三维激光、车载电池以及升压模块；
所述车载电池分别与三维激光、里程计、路由器以及升压模块连接，所述升压模块还与工控机连接，所述三维激光还与路由器连接，所述里程计还与路由器连接，所述路由器分别与工控机、计算机连接；
所述下层运动控制模块包括：工控机、TTL转USB模块、路由器、自动导航搬运车AGV、光电编码器、直流减速电机及驱动器、嵌入式开发板以及供电电源；
所述供电电源与自动导航搬运车AGV连接，所述光电编码器与直流减速电机及驱动器放置在方向轮小车中、所述光电编码器与里程计连接，所述直流减速电机及驱动器与嵌入式开发板连接，所述嵌入式开发板与TTL转USB模块连接，所述TTL转USB模块与工控机连接；所述手柄与工控机连接；
所述车载电池分别与三维激光、里程计、路由器以及升压模块连接，所述升压模块还与工控机连接，所述三维激光还与路由器连接，所述里程计还与路由器连接，所述路由器分别与工控机、计算机连接；
所述的手柄提供小车运动控制的使能作用；
所述的计算机用于监控和调试机器人；
所述的TTL转USB模块用于实现嵌入式开发板与工控机的通信；
所述的升压模块用于实现将车载电池的电压转化为工控机的标准电压；
所述的供电电源为自动导航搬运车AGV提供电能；
所述的工控机通过路由器与三维激光进行通讯，获取三维激光所生成的三维点云并计算机器人的位姿；
所述的光电编码器用于计算自动导航搬运车AGV轮子转过的圈数；
所述的直流减速电机及驱动器为自动导航搬运车AGV的动力驱动装置；
所述的嵌入式开发版为自动导航搬运车AGV的底层运动控制模块；
所述里程计根据光电编码器的数据推算机器人的里程信息。
2.一种基于示教的多传感器的移动机器人定位导航方法，采用权利要求1所述的一种基于示教的多传感器的移动机器人定位导航***实现，其特征在于，包括以下步骤：
步骤1、在教学阶段，通过手柄控制将速度指令发送给工控机，工控机将指令发送给TTL转USB模块，TTL转USB模块再将运动指令发送给嵌入式开发板STM32，嵌入式开发板STM32将接收到的指令发送给直流减速机及驱动器以控制机器人运动；在机器人运动的过程中，光电编码器采集速度信息给里程计、三维激光采集点云数据信息，然后将里程计与三维激光采集的信息通过路由器发送给工控机与计算机、同时也会记录点云数据；教学完成后，停止录制，点云数据会记录在数据库中；
步骤2、在重复阶段，通过手柄按钮将教学过程中的命令再次回放；
步骤3、自动导航搬运车读取机器人当前获得的点云，并将其与教学阶段保存的点云进行比较，并计算从读取的三维点云到记录的三维点云的变换，从而计算出机器人的位姿；
q＝[qR|qT]T
其中Np为点数，xi和pi为匹配的点对，四元数是旋转的得一种表达形式，此处旋转向量qR用四元数表达，qT是平移向量，qR和qT共同组成完全坐标变换向量q，R(qR)是旋转向量qR对应的旋转矩阵，f(q)为求解一个q使得匹配得xi与pi之间得距离最小函数；
步骤3.1根据当前机器人获得的点云数据目标点集P中的点坐标，在教学阶段存储的点云点集曲面S上搜索相应最近点点集形成参考集X；
步骤3.2计算目标点集P和参考点集X的重心：其中μp、μx分别为点集P和X的重心，Np和Nx分别为点集P和X的点数；
步骤3.3由点集P和X构造协方差矩阵
步骤3.4由协方差矩阵构造4×4对称矩阵：
其中，l3是3x3单位矩阵，tr(∑F，x)是矩阵∑P，X的迹，矩阵A＝∑P，X+∑P，X T,Aij表示矩阵A的第i行、第j列的元素；
步骤3.5计算Q(∑P，X)的特征值和特征向量，其最大特征值对应的特征向量[q0 q1 q2q3]T即为最佳旋转向量其中q0、q1、q2、q3为四元数；
步骤3.6计算最佳平移向量
其中
步骤3.7得到求得最小均方误差dmin＝f(q)；
步骤4、将计算得到的完全坐标变换向量转换为机器人的位姿发送到嵌入式开发板STM32中，使用位置PID算法补偿机器人当前位姿与教学阶段存储的对应位姿的误差，完成移动机器人的导航与定位。
3.根据权利要求1所述的一种基于激光和里程计定位的示教导航***，其特征在于，所述的算法程序基于ROS操作***，采用C++语言编写。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910348071.3A CN110058594A (zh) | 2019-04-28 | 2019-04-28 | 基于示教的多传感器的移动机器人定位导航***及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910348071.3A CN110058594A (zh) | 2019-04-28 | 2019-04-28 | 基于示教的多传感器的移动机器人定位导航***及方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110058594A true CN110058594A (zh) | 2019-07-26 |
Family
ID=67319577
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910348071.3A Pending CN110058594A (zh) | 2019-04-28 | 2019-04-28 | 基于示教的多传感器的移动机器人定位导航***及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110058594A (zh) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110234029A (zh) * | 2019-07-31 | 2019-09-13 | 上海商汤视觉科技有限公司 | 多传感器数据的播放处理方法和装置 |
| CN110568848A (zh) * | 2019-09-10 | 2019-12-13 | 东风商用车有限公司 | 清扫车的示教自动驾驶作业*** |
| CN112179346A (zh) * | 2020-09-15 | 2021-01-05 | 国营芜湖机械厂 | 一种无人小车的室内导航***及其使用方法 |
| CN113946156A (zh) * | 2021-12-20 | 2022-01-18 | 广州朗国电子科技股份有限公司 | 一种轮式机器人的运动路径示教控制方法及控制*** |
| CN114148836A (zh) * | 2021-11-08 | 2022-03-08 | 中国科学院自动化研究所 | 一种机器人自主乘梯方法及装置 |
| US20220353492A1 (en) * | 2019-06-25 | 2022-11-03 | Sony Group Corporation | Information processing device and method |
| US11636612B2 (en) | 2020-09-25 | 2023-04-25 | Industrial Technology Research Institute | Automated guided vehicle navigation device and method thereof |
| CN114265375B (zh) * | 2021-11-25 | 2024-06-04 | 云南昆船智能装备有限公司 | 一种agv对平板卡车货物存取的***和方法 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100013153A1 (en) * | 2003-02-26 | 2010-01-21 | Silverbrook Research Pty Ltd | Game System With Robotic Game Pieces |
| CN104359464A (zh) * | 2014-11-02 | 2015-02-18 | 天津理工大学 | 基于立体视觉的移动机器人定位方法 |
| CN104657981A (zh) * | 2015-01-07 | 2015-05-27 | 大连理工大学 | 一种移动机器人运动中三维激光测距数据动态补偿方法 |
| CN108317953A (zh) * | 2018-01-19 | 2018-07-24 | 东北电力大学 | 一种基于无人机的双目视觉目标表面3d检测方法及*** |
| CN108919810A (zh) * | 2018-07-26 | 2018-11-30 | 东北大学 | 基于视觉示教的移动机器人定位与导航*** |
| CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
| CN109358340A (zh) * | 2018-08-27 | 2019-02-19 | 广州大学 | 一种基于激光雷达的agv室内地图构建方法及*** |
-
2019
- 2019-04-28 CN CN201910348071.3A patent/CN110058594A/zh active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20100013153A1 (en) * | 2003-02-26 | 2010-01-21 | Silverbrook Research Pty Ltd | Game System With Robotic Game Pieces |
| CN104359464A (zh) * | 2014-11-02 | 2015-02-18 | 天津理工大学 | 基于立体视觉的移动机器人定位方法 |
| CN104657981A (zh) * | 2015-01-07 | 2015-05-27 | 大连理工大学 | 一种移动机器人运动中三维激光测距数据动态补偿方法 |
| CN108317953A (zh) * | 2018-01-19 | 2018-07-24 | 东北电力大学 | 一种基于无人机的双目视觉目标表面3d检测方法及*** |
| CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
| CN108919810A (zh) * | 2018-07-26 | 2018-11-30 | 东北大学 | 基于视觉示教的移动机器人定位与导航*** |
| CN109358340A (zh) * | 2018-08-27 | 2019-02-19 | 广州大学 | 一种基于激光雷达的agv室内地图构建方法及*** |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220353492A1 (en) * | 2019-06-25 | 2022-11-03 | Sony Group Corporation | Information processing device and method |
| US11991348B2 (en) * | 2019-06-25 | 2024-05-21 | Sony Group Corporation | Information processing device and method |
| CN110234029A (zh) * | 2019-07-31 | 2019-09-13 | 上海商汤视觉科技有限公司 | 多传感器数据的播放处理方法和装置 |
| CN110234029B (zh) * | 2019-07-31 | 2021-12-17 | 上海商汤临港智能科技有限公司 | 多传感器数据的播放处理方法、装置、设备和存储介质 |
| CN110568848A (zh) * | 2019-09-10 | 2019-12-13 | 东风商用车有限公司 | 清扫车的示教自动驾驶作业*** |
| CN110568848B (zh) * | 2019-09-10 | 2022-09-23 | 东风商用车有限公司 | 清扫车的示教自动驾驶作业*** |
| CN112179346A (zh) * | 2020-09-15 | 2021-01-05 | 国营芜湖机械厂 | 一种无人小车的室内导航***及其使用方法 |
| CN112179346B (zh) * | 2020-09-15 | 2024-02-27 | 国营芜湖机械厂 | 一种无人小车的室内导航***及其使用方法 |
| US11636612B2 (en) | 2020-09-25 | 2023-04-25 | Industrial Technology Research Institute | Automated guided vehicle navigation device and method thereof |
| CN114148836A (zh) * | 2021-11-08 | 2022-03-08 | 中国科学院自动化研究所 | 一种机器人自主乘梯方法及装置 |
| CN114265375B (zh) * | 2021-11-25 | 2024-06-04 | 云南昆船智能装备有限公司 | 一种agv对平板卡车货物存取的***和方法 |
| CN113946156A (zh) * | 2021-12-20 | 2022-01-18 | 广州朗国电子科技股份有限公司 | 一种轮式机器人的运动路径示教控制方法及控制*** |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110058594A (zh) | 基于示教的多传感器的移动机器人定位导航***及方法 | |
| CN112484725B (zh) | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 | |
| CN112518739B (zh) | 履带式底盘机器人侦察智能化自主导航方法 | |
| Royer et al. | Outdoor autonomous navigation using monocular vision | |
| CN109599945A (zh) | 一种智慧电厂自主巡检机器人巡检***及方法 | |
| Pfaff et al. | Towards mapping of cities | |
| CN104914865A (zh) | 变电站巡检机器人定位导航***及方法 | |
| CN109460052A (zh) | 一种可拼组飞行器的控制方法 | |
| CN112734841B (zh) | 一种用轮式里程计-imu和单目相机实现定位的方法 | |
| CN115200588B (zh) | 移动机器人的slam自主导航方法及其装置 | |
| CN113189613B (zh) | 一种基于粒子滤波的机器人定位方法 | |
| CN108919810A (zh) | 基于视觉示教的移动机器人定位与导航*** | |
| CN110039536A (zh) | 室内地图构造和定位的自导航机器人***及图像匹配方法 | |
| CN108759822A (zh) | 一种移动机器人3d定位*** | |
| CN113093756A (zh) | 树莓派平台下基于激光slam的室内导航机器人 | |
| Basiuk et al. | Mobile Robot Position Determining Using Odometry Method | |
| CN110160528A (zh) | 一种基于角度特征识别的移动装置位姿定位方法 | |
| CN118020038A (zh) | 两轮自平衡机器人 | |
| CN114879699A (zh) | 针对埋地管道野外巡检机器人的自主导航***及方法 | |
| Lamon et al. | Mapping with an autonomous car | |
| CN117075158A (zh) | 基于激光雷达的无人变形运动平台的位姿估计方法及*** | |
| Stachniss et al. | How to learn accurate grid maps with a humanoid | |
| Chi et al. | Low-latency Visual-based High-Quality 3D Reconstruction using Point Cloud Optimization | |
| CN115373404A (zh) | 室内静态物品识别与自主建图的移动机器人及工作方法 | |
| Zhu et al. | Indoor Localization Method of Mobile Educational Robot Based on Visual Sensor |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190726 |