坐标原点，扫地机的运动方向为X轴正向，运动方向的左侧为1轴正向，与扫地机中心垂直地面向上为z轴正向。
[0027]S3:实时获取里程计信息和陀螺仪信息。
[0028]本实施例中对里程计的采集频率为250HZ，对陀螺仪的采集频率为100Hz，可以陀螺仪的采集频率为准，即每隔10ms采集一次陀螺仪和里程计的信息，因此可认为数据采集模块是实时采集里程计信息和陀螺仪信息(本实施例中，里程计信息为左右轮的位移，陀螺仪信息为导航角度)，并通过通信模块(如串口通信模块)传输至扫地机的MCU模块。其中，里程计信息包括扫地机的位移变化量和角度变化量，陀螺仪信息包括扫地机的角度变化量。
[0029]S4:对里程计信息和陀螺仪信息进行融合后以得到扫地机当前定位信息？08^并保存。
[0030]MCU模块将里程计信息作为测量值，将陀螺仪信息作为观测值进行扩展卡尔曼滤波算法后得到扫地机当前的定位信息p0set，然后保存至存储模块，进而实现了扫地机的实时定位。
[0031]S5:扫地机每运动预定距离或预定时间后，将此时记为时刻t’ i，通过双目摄像机获取扫地机在t’ i时刻周围环境的两帧图像信息。
[0032]图像获取是立体视觉的物质基础，即利用摄像机获取3D物体的二维图像，在获取立体图像时不但要满足应用要求，而且要考虑视点差异、光照条件、摄像机性能以及景物特点等因素的影响。本实施例中，扫地机的运动距离可以通过里程计实时监测，在运动预定距离(如0.5m)时或预定时间(如2s)，图像采集模块采集双目摄像机的左右摄像头在t’ i (i为自然数)时刻的两帧图像，然后经通信模块传输至MCU模块，也就是说，图像采集模块会周期性的采集双目摄像机的图片信息并传输MCU模块。
[0033]S6:对双目摄像机的两帧图像进行矫正。
[0034]对采集到的两帧图像进行畸变矫正与极线校正，以修复失真图像。
[0035]S7:对两帧图像进行特征点提取和立体匹配。
[0036]利用双目立体视觉，根据视差恢复目标物体深度信息的关键是确定场景中同一物点在两个图像中的对应关系，解决该问题的方法之一是选择合适的图像特征并进行匹配。良好的匹配特征应该具有可区分性、不变性、稳定性、唯一性以及有效解决歧义匹配的能力，立体匹配是指根据对所选特征的计算，并建立特征之间的对应关系，将同一个空间点在不同图像中的投影点对应起来。本实施例中运用FAST角点检测算法提取当前两帧图像中的特征信息，建立角点特征间的数据关联，得到位姿估计模型；在两帧图像的特征点进行匹配过程中采用结合扩展卡尔曼滤波的一点RANSAC算法进行误匹配剔除。
[0037]S8:计算环境特征点的三维坐标。
[0038]通过立体匹配后，即可根据成像模型中视差与深度的几何关系，对目标的深度进行计算，获得其环境特征点的三维坐标。
[0039]S9:修订扫地机在t’ i时刻的定位信息。
[0040]根据步骤S3、S4可知道，在t’ i时刻，存储模块中保存的扫地机的定位信息为poset，i，此时将p0Set，i作为SLAM算法的预测值，结合步骤S8中获得的环境特征点的三维坐标进行一次SLAM算法，进而得到扫地机在t’ i时刻的经修订后的定位信息poset，i modo
[0041]需要说明的是，扫地机的在任意时刻t的定位信息pose(t) =poset.1 mod+poset0+poset - pose^i,其中，i表示在t时刻扫地机已进行了 i次SLAM计算，poset，i mcid表示扫地机在ti时刻的经修订的定位信息，pose t。表示扫地机初始位置信息，p0set表示扫地机在t时刻经融合里程计信息和陀螺仪信息得到的定位信息，pose 表示扫地机在ti时刻经融合里程计信息和陀螺仪信息得到的定位信息。由于本实施例中将poset。设为(0，0, 0)，因此扫地机在任意时刻t的定位信息为pose (t)= pose t>i mod +poset -pose,l0例如，假设SLAM计算的间隔时间为，里程计与陀螺仪使用一个频率，时间间隔为，为的η倍(η为彡2的正整数);t0时刻扫地机的定位信息为poset。;在t0至tl时间间隔之间，扫地机默认储存里程计与陀螺仪计算的定位信息与to时刻的扫地机定位信息的坐标之和，即 poset0+pose t0+At2、poset0+pose t0+2*At2等;tl 时刻进行第一次 SLAM 算法，其中，tl=t0+Λ tl，里程计与陀螺仪得到的扫地机的定位信息为p0Setl，输入SLAM算法，得到经修订后的tl时刻扫地机的定位信息posetl _，然后储存；在tl至t2时间间隔之间，扫地机默认储存tl时刻经SLAM修订后的扫地机定位信息poSetl nod,再加上里程计与陀螺仪计算的定位信息与tl时刻的里程计与陀螺仪计算的定位信息，即posetl mod+ pose tl+At2 - posetl、
pOS6tl mo(j+ pOSe tl+2*At2
posetl# ;以此类推。
[0042]以上是本发明的较佳实施例的详细说明，不认定本发明只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下所作出的等同替代或明显变形，且性能或用途相同，都应当视为本发明所提交的权利要求书确定的保护范围内。
【主权项】
1.一种移动机器人的实时定位方法，其特征在于，所述移动机器人包括里程计模块、陀螺仪模块和双目摄像机模块，所述方法包括: 51:标定双目摄像机t吴块的相关参数； 52:设定移动机器人的初始定位信息posetQ; 53:实时获取里程计信息和陀螺仪信息； 54:对里程计信息和陀螺仪信息进行融合后以得到移动机器人当前定位信息？08^并保存； 55:移动机器人每运动预定距离或每运动预定时间后，将此时记为时刻t’ i，通过双目摄像机获取移动机器人在t’ i时刻周围环境的两帧图像信息； 56:对双目摄像机的两帧图像进行矫正； 57:对两帧图像进行环境特征点提取和立体匹配； 58:计算环境特征点的三维坐标； 59:通过SLAM算法修订移动机器人在t’ i时刻的定位信息pose^ _并保存。2.如权利要求1所述的移动机器人的实时定位方法，其特征在于，所述移动机器人在任意时刻t的定位信息poseUhposen mod+poset0+ poset - poset，i，其中，i表示在t时刻扫地机已进行了 i次SLAM计算，poset，i mcid表示扫地机在ti时刻的经SLAM算法修订后的定位信息，poset。表示扫地机初始位置信息，pose t表示扫地机在t时刻经融合里程计信息和陀螺仪信息得到的定位信息，posen表示扫地机在ti时刻经融合里程计信息和陀螺仪信息得到的定位信息。3.如权利要求1所述的移动机器人的实时定位方法，其特征在于，所述步骤S5中的预定距离为0.5m。4.如权利要求1所述的移动机器人的实时定位方法，其特征在于，所述步骤S5中的预定时间为2s。5.一种移动机器人，其特征在于，包括MCU模块、双目摄像机模块、图像采集模块、里程计模块、陀螺仪模块、数据采集模块、运动模块、通信模块和存储模块；所述MCU模块用于标定双目摄像机模块的相关参数、设定移动机器人的初始定位信息poset。、对里程计信息及陀螺仪信息进行融合以得到移动机器人的当前定位信息p0set、对图像进行环境特征点提取和立体匹配、计算环境特征点的三维坐标、运行SLAM算法计算移动机器人的修订后定位信息poSet 所述图像采集模块用于采集双目摄像机模块的图像信息并传输至MCU模块；所述数据采集模块用于采集里程计模块数据信息及陀螺仪模块数据信息；所述运动模块用于将移动机器人驱动至指定位置；所述通信模块用于将数据采集模块采集到的里程计信息及陀螺仪信息传输至MCU模块和将MCU模块的指令传输至运动模块；所述存储模块用于存储定位信息。6.如权利要求1所述的移动机器人，其特征在于，所述MCU模块为ARM模块。7.如权利要求1所述的移动机器人，其特征在于，所述通信模块为串口通信模块。8.如权利要求1所述的移动机器人，其特征在于，所述数据采集模块为STM32。9.如权利按要求5至8之一所述的移动机器人，其特征在于: 所述移动机器人为扫地机，所述运动模块为扫地机的左右驱动轮，所述里程计模块和陀螺仪模块分别用于测量左右驱动轮的位移变量和角度变量。10.如权利按要求9所述的移动机器人，其特征在于:所述扫地机还避障模块，所述避障模块用于检测扫地机周围障碍并通过数据采集模块传输至MCU模块。
【专利摘要】本发明揭示了一种移动机器人的实时定位方法，该方法采用将里程计信息与陀螺仪信息相融合得到移动机器人的定位信息，同时将该定位信息作为SLAM算法的预测值，结合双目摄像机获得的场景信息进行SLAM算法，进而实现定位。该方法可实时矫正移动机器人的定位信息，并可有效地降低里程计的累计误差，提高了定位精度，减小系统的累计误差。本发明还揭示了一种采用上述方法的移动机器人，特别是一种清洁地面的扫地机。
【IPC分类】G01C21/20, A47L11/40
【公开号】CN105411490
【申请号】CN201510696663
【发明人】不公告发明人
【申请人】曾彦平
【公开日】2016年3月23日
【申请日】2015年10月26日