技术领域
[0001] 本
发明属于工业自动化领域，特别涉及一种基于示教的多传感器的
移动机器人定位导航系统及方法。
背景技术
[0002] 随着人
力成本的提高和科学技术的发展，机器人(尤其是
工业机器人)在工业自动化领域越来越广泛的应用。自动导航搬运车(Automated Guided Vehicle以下简称AGV)也被称作搬运机器人，是现代智能物流系统中的重要环节，然而目前主流的AGV导引技术并不能使其在工作中从真正意义上实现完全自主。
[0003] 导航作为核心技术之一，是AGV技术发展的最重要标志。传统导航方式以电磁导航和磁带导航为代表，使用物理路径，环境表示简单，无需自动地图创建技术，定位时只需要通过传感器检测车体与物理路径之间的偏差。随着科技的进步，激光传感器、图像传感器逐步应用到AGV导航技术中。由于具有定位
精度高，无需对地面进行改造，路径柔性高的优点，基于
激光雷达的AGV导航技术即激光导航受到广泛研究并得到成功应用。与传统导航方式相比，激光导航涉及面多，技术复杂。首先，激光导航不存在物理路径，因此无法像传统导航方式那样直接检测车体与物理路径之间的偏差。因此在激光导航中，AGV通过激光雷达实时采集环境数据，通过将环境数据的处理结果与事先建立的环境地图进行对比获得自身在地图中的
位姿估计，这种位姿估计即定位比传统导航方式中的直接偏差计算要复杂。其次，激光导航中使用的地图具有明显的测度信息，需要进行精确绘制，所创建的环境地图精度影响AGV定位精度。
[0004] 在未知环境中，由于环境
感知传感器的探测范围和测量精度限制、环境中
墙壁等物体对传感器探测的遮挡等问题，机器人无法通过一次测量创建全局环境地图。因此，机器人只能在不断地环境探索过程中获取足够的环境感知数据，才能完成全局环境地图的创建工作。对于机器人在环境中各个
位置创建的局部地图而言，只有确切地知道机器人各位置的位姿才能将该局部地图转换为全局地图，即地图的创建依赖于机器人的位姿。然而现实情况是，机器人的位姿往往是通过环境地图得到的，即机器人的位姿依赖于环境地图。位姿和地图估计之间的这种相互依赖的关系给机器人在未知环境下的导航提出了新的课题，称之为“同时定位与地图构建”(Simultaneous Localization and Mapping以下简称SLAM)问题。因此SLAM的最终目的是创建一致的环境地图，是否能够实现正确闭环是方法自身性能的一种体现；而基于闭环消除定位误差的地图创建方法，在通过航迹推测所估计的位姿误差较大的情况下，只能通过辅助方法建立约束条件，即在建立正确闭环的前提下，再通过优化方法校正机器人位姿和创建环境地图。因此slam系统这样的实现需要构建全局一致的地图因此带来更高的计算成本。
发明内容
[0005] 为了克服
现有技术的不足，本发明提出了一种基于基于示教的多传感器的移动机器人定位导航系统及方法。
[0006] 本发明所采用的技术方案为：一种基于基于示教的多传感器的移动机器人定位导航系统包括：上层导航模
块、下层运动控
制模块、
手柄以及计算机；
[0007] 所述手柄与下层运动
控制模块连接，所述下层运动控制模块与上层导航模块连接，所述上层导航模块与计算机连接；
[0008] 所述上层导航模块包括路由器、
里程计、三维激光、车载
电池以及升压模块；
[0009] 所述下层运动控制模块包括：工控机、TTL转USB模块、路由器、自动导航搬运车AGV、光电
编码器、直流减速
电机及
驱动器、嵌入式开发板以及供电电源；
[0010] 所述供电电源与自动导航搬运车AGV连接，所述光电编码器与直流减速电机及驱动器放置在方向轮小车中、所述光电编码器与里程计连接，所述直流减速电机及驱动器与嵌入式开发板连接，所述嵌入式开发板与TTL转USB模块连接，所述TTL转USB模块与工控机连接；所述手柄与工控机连接；
[0011] 所述车载电池分别与三维激光、里程计、路由器以及升压模块连接，所述升压模块还与工控机连接，所述三维激光还与路由器连接，所述里程计还与路由器连接，所述路由器分别与工控机、计算机连接；
[0012] 所述的手柄提供小车运动控制的使能作用；
[0013] 所述的计算机用于监控和调试机器人；
[0014] 所述的TTL转USB模块用于实现嵌入式开发板与工控机的通信；
[0015] 所述的升压模块用于实现将车载电池的
电压转化为工控机的标准电压；
[0016] 所述的供电电源为自动导航搬运车AGV提供
电能；
[0017] 所述的工控机通过路由器与三维激光进行通讯，获取三维激光所生成的三维点
云并计算机器人的位姿；
[0018] 所述的光电编码器用于计算自动导航搬运车AGV轮子转过的圈数；
[0019] 所述的直流减速电机及驱动器为自动导航搬运车AGV的动力驱动装置；
[0020] 所述的嵌入式开发版为自动导航搬运车AGV的底层运动控制模块；
[0021] 所述里程计根据光电编码器的数据推算机器人的里程信息；
[0022] 一种基于示教的多传感器的移动机器人定位导航方法，采用前述的一种基于示教的多传感器的移动机器人定位导航系统实现，包括以下步骤：
[0023] 步骤1、在教学阶段，通过手柄控制将速度指令发送给工控机，工控机将指令发送给TTL转USB模块，TTL转USB模块再将运动指令发送给嵌入式开发板STM32，嵌入式开发板STM32将接收到的指令发送给直流减速机及驱动器以控制机器人运动；在机器人运动的过程中，光电编码器采集速度信息给里程计、三维激光采集点云数据信息，里程计与三维激光将采集的信息通过路由器发送给工控机与计算机、同时也会记录点云数据；教学完成后，停止录制，点云数据会记录在
数据库中；
[0024] 步骤2、在重复阶段，通过手柄按钮将教学过程中的命令再次回放；
[0025] 步骤3、自动导航搬运车读取机器人当前获得的点云，并将其与教学阶段保存的点云进行比较，并计算从读取的三维点云到记录的三维点云的变换，从而计算出机器人的位姿；
[0026] 将机器人的位姿求解问题转换为数学问题，
[0027]
[0028] q＝[qR|qT]T
[0029] 其中Np为点数，xi和pi为匹配的点对，四元数是旋转的得一种表达形式，此处旋转向量qR用四元数表达，qτ是平移向量，qR和qτ共同组成完全坐标变换向量q，R(qR)是旋转向量qR对应的旋转矩阵，f(q)为求解一个q使得匹配得xi与pi之间得距离最小函数。
[0030] 步骤3.1根据当前机器人获得的点云数据目标点集P中的点坐标，在教学阶段存储的点云点集曲面S上搜索相应最近点点集形成参考集X；
[0031] 步骤3.2计算目标点集P和参考点集X的
重心：其中μp、μx分别为点集P和X的重心，Np和Nx分别为点集P和X的点数；
[0032]
[0033]
[0034] 步骤3.3由点集P和X构造协方差矩阵
[0035]
[0036] 步骤3.4由协方差矩阵构造4×4对称矩阵：
[0037] 其中，I3是3x3单位矩阵，tr(∑P，X)是矩阵∑P，X的迹， 矩阵A＝∑P，X+∑P，XT，Aij表示矩阵A的第i行、第j列的元素；
[0038] 步骤3.5计算Q(∑P，X)的特征值和
特征向量，其最大特征值对应的特征向量[q0 q1 q2 q3]T即为最佳旋转向量 其中q0、q1、q2、q3为四元数；
[0039] 步骤3.6计算最佳平移向量
[0040] 其中
[0041]
[0042] 步骤3.7得到 求得
最小均方误差dmin＝f(q)；
[0043] 步骤4、将计算得到的完全坐标变换向量转换为机器人的位姿发送到嵌入式开发板STM32中，使用位置PID
算法补偿机器人当前位姿与教学阶段存储的对应位姿的误差，完成移动机器人的导航与定位；
[0044] 本发明的有益技术效果：本发明系统理论结合实际，构成合理、简洁，方法便捷，运用本发明可以快速地进行室内AGV的导航，非专业人员易上手、操作简单、使用方便。
[0045] 该方法计算效率高，无需
照相机标定，而且可以自主学习和遍历任意形状的路径。在教学阶段，机器人是由一名人类操作者驾驶的，机器人会存储它的速度、激光扫描图和里程计信息。在自主导航中，该方法在二维甚至是三维空间中无需显式机器人定位，只需回放在教学阶段学习的速度，同时还能根据三维激光数据和由里程计信息生成的位置信息对路速度大小和方向进行修正。因此，机器人可以重复地沿着预期的路径自动驾驶而无需人工干预。
附图说明
[0046] 图1为本发明基于示教的多传感器融合的移动机器人定位与导航系统总结构
框图；
[0047] 图2为本发明基于示教的多传感器融合的移动机器人定位与导航系统三维点云生成示意图；
[0048] 图3为本发明基于示教的多传感器融合的移动机器人定位与导航系统
软件框架示意图；
[0049] 图4为本发明基于示教的多传感器融合的移动机器人定位与导航系统
硬件连接示意图；
[0050] 图5为本发明具体实施方式的方法
流程图；
[0051] 图6为本发明基于示教的多传感器融合的移动机器人定位与导航
系统软件定位算法中的核心算法ICP算法流程图；
[0052] 图中，ENCA1、ENCB1、ENCC1、ENCD1是四路PWM作为直流减速电机及驱动器得控制
信号。
具体实施方式
[0053] 下面结合附图和
实施例，对本发明做进一步的解释说明。
[0054] 本实施例的系统总结构框如图1所示，嵌入式开发板的型号为STM32，工控机的型号为占美gk400，TTL转USB模块的型号是CH340G，三维激光的型号是VLP-16，车载电池的型号为YSN-1211000，升压模块的型号为MKX-DC12V～19V,车载电池的型号为凯美威12V100A，供电电源的型号为SKYRC IMAX B6，路由器的型号为MERCURY MW325R，手柄的型号为Sony PS3。本实施例中，各部件的连接关系为：
[0055] 一种基于示教的多传感器的移动机器人定位导航系统：上层导航模块、下层运动控制模块、手柄以及计算机；
[0056] 手柄与下层控制模块连接，下层控制模块与上层导航模块连接，上层导航模块与计算机连接；
[0057] 下层运动控制模块用于实现小车平台的运动控制、显示小车状态信息以及编码器数据的读取与发送，包括工控机、TTL转USB模块、自动导航搬运车AGV、光电编码器、直流减速电机及驱动器、嵌入式开发板以及供电电源；
[0058] 上层导航模块用于采集点云数据，其包括包括路由器、里程计、三维激光、车载电池以及升压模块；
[0059] 供电电源与自动导航搬运车AGV连接，光电编码器与直流减速电机及驱动器设置在方向轮小车中、光电编码器与里程计连接，直流减速电机及驱动器与嵌入式开发板连接，嵌入式开发板与TTL转USB模块连接，TTL转USB模块与工控机连接；手柄与工控机连接；
[0060] 车载电池分别与三维激光、里程计、路由器以及升压模块连接，升压模块还与工控机连接，三维激光还与路由器连接，里程计还与路由器连接，路由器分别与工控机、计算机连接；升压模块MKX-DC12V～19V、三维激光VLP-16、路由器MERCURY MW325R以及里程计均直接连接车载电池凯美威12V100A由其供电。
[0061] 手柄提供小车运动控制的使能作用；
[0062] 计算机用于监控和调试机器人；
[0063] TTL转USB模块用于实现嵌入式开发板与工控机的通信；
[0064] 升压模块用于实现将车载电池的电压转化为工控机的标准电压；
[0065] 供电电源为AGV小车提供电能；
[0066] 工控机通过路由器与三维激光进行通讯，获取三维激光所生成的三维点云并计算机器人的位姿；
[0067] 光电编码器用于计算AGV轮子转过的圈数；
[0068] 直流减速电机及驱动器为AGV的动力驱动装置；
[0069] 嵌入式开发版为AGV的底层运动控制模块；
[0070] 里程计根据光电编码器的数据推算机器人的里程信息；
[0071] 工控机通过路由器与三维激光行通讯，获取三维激光所生成的三维点云并运行后续的算法程序；
[0072] 图4为下层导航单元的硬件连接示意图，搭载Linux系统的占美gk400工控机是整个装置导航单元的大脑，它一方面通过TTL转USB模块CH340G给STM32发送控制指令，使其能够前、后、左、右或自转运动，另一方面通过路由器读取得里程计数据和激光雷达的数据，如图2三维点云生成示意图所示，然后通过算法将这两种
传感器数据的融合实现定位与导航。
[0073] 激光雷达通过螺丝固定在雷达
支撑架上面，雷达支撑架通过螺丝与万向轮小车上层平台连接；激光雷达的供电线接上层车载电池凯美威12V100A，数据线接上层路由器MERCURYMW325R(连接车载电池凯美威12V100A供电)；车载电池12V端口接下层升压模块MKX-DC12V～19V，后转接工控机占美gk400；路由器MERCURY MW325R的两个LAN口，一个接三维激光VLP-16，另外一个接工控机占美gk400；通过路由器的有线连接，确保了激光雷达与工控机之间数据传输的
稳定性、安全性和实时性，为工控机上运行的示教软件实时处理激光雷达发来的大量三维点云数据提供了有效支持。
[0074] 工控机中存储着导航程序，嵌入式开发板STM32存储着运动控制程序，导航程序与运动控制程序均为基于ROS
操作系统，用C++语言编写；包含了上层的导航模块与底层运动控制模块两部分，实现整个系统的完整运行，其中上层导航模块的程序在工控机中运行，底层运动控制模块在嵌入式开发板STM32上运行；
[0075] 本实例的软件框架如图3所示，在教学阶段，用手柄驱动机器人在前、后、左、右和自转运动的
基础上移动任意路径，并将路径点坐标即机器人的位姿和三维激光点云数据和速度控制指令存储在工控机上的数据库中，在重复阶段，上层的导航模块检索数据库，将数据库中保存的路径点坐标参考路径，存储的速度作为参考速度，将数据库中的保存的点云数据与机器人当前获得的点云数据比较，确定机器人当前的位姿并与存储的机器人位姿计算一个偏差作为补偿
叠加到参考速度指令上，然后发送给底层运动控制模块；底层运动控制模块全部在STM32中执行，包括电机运动控制程序和数据读收发程序，其中与工控机的数据收发程序基于SCIP2.0协议编写；下层运动控制模块接收上层导航模块的速度控制指令，借助万向轮的特性，实现了AGV的前、后、左、右和自转运动；四个独立的
车轮光电编码器值通过该tcp/ip通讯程序传送到上层导航模块进行解算，得出小车的里程计信息。上层导航模块基于Linux环境下的ROS操作系统，分为教学和重复两个阶段。
[0076] 如图5所示，基于激光和里程计定位的示教导航方法，具体步骤为：
[0077] 步骤1、在教学阶段，通过手柄控制将速度指令发送给工控机，工控机将指令发送给TTL转USB模块，TTL转USB模块再将运动指令发送给嵌入式开发板STM32，嵌入式开发板STM32将接收到的指令发送给直流减速机及驱动器以控制机器人运动；在机器人运动的过程中，光电编码器采集速度信息给里程计、三维激光采集点云数据信息，里程计与三维激光将采集的信息通过路由器发送给工控机与计算机、同时也会记录点云数据；教学完成后，停止录制，点云数据会记录在数据库中；
[0078] 步骤2、在重复阶段，通过手柄按钮将教学过程中的命令将再次回放；
[0079] 步骤3、运行定位算法模块，自动导航搬运车读取机器人当前获得的点云，并将其与教学阶段保存的点云进行比较，并计算从读取的三维点云到记录的三维点云的变换，从而计算出机器人的位姿；
[0080] 将机器人的位姿求解问题转换为数学问题，
[0081]
[0082] q＝[qR|qT]T
[0083] 其中Np为点数，xi和pi为匹配的点对，四元数是旋转的得一种表达形式，此处旋转向量qR用四元数表达，qτ是平移向量，qR和qτ共同组成完全坐标变换向量q，R(qR)是旋转向量qR对应的旋转矩阵，f(q)为求解一个q使得匹配得xi与pi之间得距离最小函数。
[0084] 步骤3.1如图6所示，根据当前机器人获得的点云数据目标点集P中的点坐标，在教学阶段存储的点云点集曲面S上搜索相应最近点点集形成参考集X；
[0085] 步骤3.2计算目标点集P和参考点集X的重心：其中μp、μx分别为点集P和X的重心，Np和Nx分别为点集P和X的点数；
[0086]
[0087]
[0088] 步骤3.3由点集P和X构造协方差矩阵
[0089]
[0090] 步骤3.4由协方差矩阵构造4×4对称矩阵：
[0091] 其中，I3是3x3单位矩阵，tr(∑FX)是矩阵∑P，X的迹， 矩阵A＝∑P，X+∑P，XT，Aij表示矩阵A的第i行、第j列的元素；
[0092] 步骤3.5计算Q(∑F，X)的特征值和特征向量，其最大特征值对应的特征向量[q0 q1 q2 q3]T即为最佳旋转向量qR＝[q0q1q2q3]T；其中q0、q1、q2、q3为四元数；
[0093] 步骤3.6计算最佳平移向量
[0094] 其中
[0095]
[0096] 步骤3.7得到q＝[qR|qT]r，求得最小均方误差dmin＝f(q)；
[0097] 步骤4、将计算得到的完全坐标变换向量转换为机器人的位姿然后发送到工控机，工控机将控制位置PID算法补偿机器人当前位姿与教学阶段存储的对应位姿的误差，完成移动机器人的导航与定位；
[0098] 本系统的核心算法是基于
迭代最近点(Interative Closet Points，简称ICP)点云匹配算法。
[0099] 三维点云匹配是一个非常重要的中间步骤，它在表面重建、三维
物体识别、相机定位等问题中有着极其重要的应用。对于三维点云匹配问题，研究者提出了很多解决方案，但是应用最广泛的，影响最大的还是ICP算法，它是基于纯粹
几何模型的三维点云匹配算法，由于它的强大功能以及高精确度，很快就成为点云匹配中的主流算法。
[0100] 其中电机运动控制程序如下所示：
[0101] 1.从数据库中检索机器人的当前的位姿qk_expect和3维点云数据，将3维点云数据和机器人当前获得的3维点云数据送到定位算法模块得到机器人的相对于数据库点云的位姿qk_calc，计算PID位置环的输入偏差，其中k表示第k时刻，
[0102] Δqk＝qk_expect-qk_calc；
[0103] 2.依据PID的公式计算位置环的输出，其中upk，Kpp和Kpi分别表示PID位置环的输出，PID的比例和积分系数，
[0104]
[0105] 3.利用在数据库中检索机器人的当前期望的速度vk_expect和通过里程计获得机器人当前真实速度vk_truth，和位置PID环的输出计算其偏差Δvk，其中k表示当前时刻，α表示位置环的输出在偏差Δvk中的权重，
[0106] Δvk＝αupk+vk_expect-vk_truth，
[0107] 4.计算速度环的PID输出，其中uvk，Kvp，Kvd，Kvi分别表示速度环PID输出，比例系数，微分系数和积分系数，
[0108]
[0109] 5.计算最终的输出速度，其中最终的输出速度用Vk表示，
[0110] Vk＝vk_exect+uvk。
