本公开涉及导航定位技术领域，尤其涉及一种机器人自主导航定位方法及系统。
背景技术
对于在室内地面上运动的机器人，能够自主导航是一项非常重要的功能。目前常用的方法非常消耗运算资源，难以应用到目前机器人的嵌入式平台上。
例如，目前已有报道采用视觉、惯导和里程计进行导航定位，如slam等，但主要是通过视觉特征点解算机器人的运动，非常复杂、要进行大量的运算、对计算机硬件平台的要求很高，目前难以运用到扫地机器人等运算量有限的嵌入式平台上。
技术实现要素：
(一)要解决的技术问题
本公开提供了一种机器人自主导航定位方法及系统，以至少部分解决以上所提出的技术问题。
(二)技术方案
根据本公开的一个方面，提供了一种机器人自主导航定位方法，该机器人上布置有视觉传感器、单轴惯性传感器和里程计，所述视觉传感器的主光轴和单轴惯性传感器的测量轴均沿机器人转动轴向设置，该机器人自主导航定位方法包括以下步骤中的至少之一：
对机器人转弯运动的航向误差进行校正的步骤，具体包括：当机器人作出转弯运动时进行一次视觉测量，利用视觉传感器分别拍摄对应于开始转弯运动和结束转弯运动时机器人所处环境的两帧第一图像，从该两帧第一图像中获取机器人视觉的第一航向变化量，结合单轴惯性传感器的测量数据，获取机器人转弯运动的航向误差并进行校正；
对机器人直线运动的航向误差进行校正的步骤，具体包括：当机器人作出直线运动时每隔设定距离时进行一次视觉测量，利用视觉传感器分别拍摄对应于第m个设定距离的起点和终点处机器人所处环境的两帧第二图像，该设定距离根据里程计测量得到，从该两帧第二图像中获取机器人视觉的第二航向变化量，结合单轴惯性传感器的测量数据，获取在该第m个设定距离内机器人直线运动的航向误差并进行校正，其中m≥1；
对机器人直线运动的里程误差进行校正的步骤，具体包括：利用所述两帧第二图像获取第m个设定距离内机器人视觉的直线运动量，结合里程计的测量结果，获取该第m个设定距离内机器人直线运动的里程误差并进行校正。
在本公开的一些实施例中，所述机器人视觉的第一航向变化量通过以下步骤得到：
确定所述两帧第一图像中的n组相同特征点，n≥1；
将该n组相同特征点的图像坐标代入式1和式2中，通过最小二乘法解算出机器人视觉的第一航向变化量：
其中，(x1，n，y1，n)和(x2，n，y2，n)分别为第n组相同特征点在两帧第一图像中的坐标，n＝1、2...、n，f为视觉传感器的焦距，dx和dy均为像素尺寸，zc为视觉传感器与被拍摄物体之间的距离，hvis，r为机器人视觉的第一航向变化量，(tx，ty)为机器人视觉的相对位移量。
在本公开的一些实施例中，所述获取机器人转弯运动的航向误差进行校正的步骤包括：
利用式3计算机器人转弯运动的航向误差：
hdrift，r＝hins，r-hvis，r，式3；
利用式4计算经校正后t时刻的机器人转弯运动的航向：
其中，hdrift，r为机器人完成转弯运动所产生的航向误差，hins，r为单轴惯性传感器测量的机器人完成转弯运动的航向变化量，hvis，r为机器人视觉的第一航向变化量，ht，r’为经校正后t时刻的机器人转弯运动的航向，ht，r为t时刻单轴惯性传感器解算的航向，tr为机器人完成转弯运动所需时间，δtr为转弯运动开始的时刻与t时刻之间的间隔时间。
在本公开的一些实施例中，所述n组相同特征点通过surf方法进行匹配确定。
在本公开的一些实施例中，所述机器人视觉的第二航向变化量通过以下步骤得到：
确定所述两帧第二图像中的n’组相同特征点，n’≥1；
将该n’组相同特征点的图像坐标代入式5和式6中，通过最小二乘法解算出机器人视觉的第二航向变化量：
其中，(x1，n’，y1，n’)和(x2，n’，y2，n’)分别为第n’组相同特征点在两帧第二图像中的坐标，n’＝1、2...、n’，f为视觉传感器的焦距，dx和dy均为像素尺寸，zc为视觉传感器与被拍摄物体之间的距离，hvis，l为机器人视觉的第二航向变化量，(tx，m’，ty，m’)为机器人视觉的相对位移量。
在本公开的一些实施例中，所述获取第m个设定距离内机器人直线运动的航向误差并进行校正的步骤包括：
利用式7计算在第m个设定距离内机器人直线运动的航向误差：
hdrift，l＝hins，l-hvis，l，式7；
利用式8计算经校正后t时刻的机器人直线运动航向：
其中，hdrift，l为第m个设定距离内机器人作直线运动所产生的航向误差，hins，l为单轴惯性传感器测量第m个设定距离内机器人作直线运动时的航向变化量，hvis，l为机器人视觉的第二航向变化量，ht，l’为经校正后t时刻的机器人直线运动的航向，ht，l为t时刻单轴惯性传感器解算的航向，tl为机器人完成第m个设定距离的直线运动所需时间，δtl为第m个设定距离的起点对应的时刻与t时刻之间的间隔时间。
在本公开的一些实施例中，所述机器人视觉的直线运动量为
在本公开的一些实施例中，所述获取该第m个设定距离内机器人直线运动的里程误差并进行校正的步骤包括：
利用式9计算第m个设定距离内机器人视觉的直线运动量与里程计的测量结果的比值rm，判断若rm/rm-1超过阈值范围，则判断在第m个设定距离内发生打滑：
其中，tg，m为里程计的测量结果，(tx，m’，ty，m’)为机器人视觉的相对位移量；
在发生打滑时利用式10对里程计在第m个设定距离内的测量结果进行校正，并得到此时自t-δt时刻至t时刻里程计测量结果的修正值为
其中，tg，m’为在第m个设定距离内对里程计测量结果的修正值，m为介于2和5之间的正整数。
在本公开的一些实施例中，所述阈值范围为90％～110％。
在本公开的一些实施例中，所述n’组相同特征点通过surf方法进行匹配确定。
在本公开的一些实施例中，所述机器人自主导航定位方法还包括在对所述机器人转弯运动的航向误差、直线运动的航向误差和直线运动的里程误差中的至少之一进行校正后，利用式11和式12对机器人轨迹进行解算：
xt＝cos(ht’)·dt’+xt-δt，式11；
yt＝sin(ht′)·dt′+yt-δt，式12；
其中，ht′为经校正后t时刻的机器人转弯运动或直线运动的航向，dt′为自t-δt时刻至t时刻机器人转弯运动时里程计的测量结果、或机器人直线运动时无打滑发生时里程计的测量结果或发生打滑时里程计测量结果的修正值，(xt，yt)为t时刻机器人的坐标，(xt-δt，yt-δt)为t-δt时刻机器人的坐标。
在本公开的一些实施例中，在航向误差修正过程中，当里程计每累计40～60米时，使机器人停1～1.5秒，采集此时机器人的角速度测量值，在后续单轴惯性传感器解算航向的过程中以该角速度测量值为新零点。
在本公开的一些实施例中，所述机器人为轮式机器人，所述里程计为轮式里程计。
在本公开的一些实施例中，所述单轴惯性传感器为单轴mems惯性传感器。
在本公开的一些实施例中，所述视觉传感器为广角摄像头。
根据本公开的另一个方面，提供一种机器人自动导航定位系统，包括：
视觉传感器，布置于机器人上，其主光轴沿机器人转动轴向设置，用于拍摄机器人所处环境的照片；
单轴惯性传感器，布置于机器人上，其测量轴沿机器人转动轴向设置，用于测量机器人的航向；
里程计，布置于机器人上，用于测量机器人直线运动和转弯运动的运动量；
数据处理及控制模块，电性连接至所述视觉传感器、单轴惯性传感器以及里程计，用于根据单轴惯性传感器、里程计以及视觉传感器输出的数据，执行如上所述的机器人自主导航定位方法。
(三)有益效果
从上述技术方案可以看出，本公开机器人自主导航定位方法及系统至少具有以下有益效果其中之一或其中一部分：
(1)本公开利用单轴惯性传感器和里程计对机器人进行定位运算，并结合视觉测量消除运算过程中的航向误差和里程误差，充分利用了惯导、视觉、和里程计的信息互补性，具有运算量非常小的优点。
(2)定位过程无需传统视觉导航所用到的双目视觉或深度摄像头，仅需要单轴惯导、普通的摄像头和轮式里程计，导航系统的硬件非常简洁。
(3)与例如视觉slam等导航定位方法不同，本公开的导航定位方法无需采集大量的特征点而造成需要大量的运算，在绝大多数时间只用到单轴惯性传感器和里程计的数据，此时每次运算仅有几次乘法和加法，因此适合运用到扫地机器人等目前常用的机器人嵌入式平台上。
(4)视觉测量尽在校正时刻启动，使用次数少，不会给机器人的嵌入式系统造成太大的运算压力。
附图说明
图1为本公开一实施例机器人自主导航定位方法的示意图；
图2为本公开一实施例机器人自助导航定位系统的示意图；
图3为本公开实验1中利用图1所示方法解算的机器人行驶轨迹；
图4为本公开实验2中利用图1所示方法解算的机器人行驶轨迹；
图5为本公开实验2中未进行校正时解算的机器人行驶轨迹。
【附图中本公开实施例主要元件符号说明】
10-机器人小车；
11-轮式里程计；12-单轴惯性传感器；13-视觉传感器；
20-数据处理及控制模块。
具体实施方式
本公开利用式(1)和(2)所示的策略解算机器人的轨迹、获得机器人的坐标，所需的运算量很少，能够在机器人嵌入式系统中快速运算，且考虑到机器人运动在地面上，因此采用简单的单轴惯性传感器即可测定航向，采用里程计测定行驶距离：
xt＝cos(ht)·dt+xt-δt，式(1)
yt＝sin(ht)·dt+yt-δt，式(2)
其中，ht为机器人行驶时依靠惯性传感器解算的航向，dt为依靠里程计记录自t-δt时刻至t时刻行走的距离，(xt，yt)对应于t时刻机器人的坐标，(xt-δt，yt-δt)对应于t-δt时刻机器人的坐标。
但这一运算过程存在误差，包括航向误差、里程误差，本公开的核心就是消除这两种影响的机器人导航定位的误差、准确解算出机器人在每一时刻的位置、获取行驶轨迹。
其中，“航向误差”是指传感器测量的机器人行驶的航向不准确，这种误差主要是由于惯性传感器中陀螺的漂移或里程计航向测量结果的漂移造成。本公开将该航向误差分为转弯运动航向误差和直线运动航向误差。“里程误差”是指机器人打滑等情况发生时，导致安装在机器人轮子上的编码器无法准确测量轮子的转动情况，进而造成里程测量不准确。
据上所述，本公开提供了机器人自主导航定位方法及系统，将视觉传感器、惯性传感器和里程计融合到一起，构建机器人的自主导航定位系统，设计了一种能够在嵌入式系统中运行的机器人导航方法，具有运算量少、精度高的优点。
为使本公开的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本公开进一步详细说明。
图1为本公开一实施例机器人自主导航定位方法的示意图。如图1所示，本实施例一种机器人自主导航定位方法，在该机器人上布置有视觉传感器、单轴惯性传感器和里程计，所述视觉传感器的主光轴和单轴惯性传感器的测量轴均沿机器人转动轴向设置，该机器人自主导航定位方法包括：
步骤a：对机器人转弯运动的航向误差进行校正，具体地，当机器人作出转弯运动时进行一次视觉测量，利用视觉传感器分别拍摄对应于开始转弯运动和结束转弯运动时机器人所处环境的两帧第一图像，结合从该两帧第一图像中获取的机器人视觉的第一航向变化量，以及单轴惯性传感器测量的航向变化量，对机器人转弯运动的航向误差进行校正。
具体地，当机器人转弯运动开始时，其转动角速度从接近于0的数值突然增大到每秒几十度，单轴惯性传感器的角速度测量结果会发生阶跃式的上升，此时立即启动视觉传感器的拍摄功能，拍摄第一帧第一图像；当机器人转弯运动结束时，机器人的角速度会立即降到接近于0的状态，单轴惯性传感器的角速度测量结果也会发生突变为0值的阶跃式的下降，此时立即启动视觉传感器的拍摄功能，拍摄第二帧第一图像。
本实施例的视觉传感器为广角摄像头，其拍摄的该第一帧第一图像和第二帧第一图像例如为天花板的图像。
本实施例中采用surf方法寻找和匹配两帧第一图像中的相同特征点，当然并不局限于此，其他的特征点匹配方法亦可，利用相同特征点通过视觉解算出航向的变化hvis，r以及拍摄两帧第一图像时机器人视觉的相对位移量(tx，ty)，可将多组相同特征点代入式(3)和式(4)中，通过最小二乘小进行解算：
其中，(x1，n，y1，n)和(x2，n，y2，n)分别为第n组相同特征点在两帧第一图像中的坐标，n＝1、2...、n，f为视觉传感器的焦距，dx和dy均为像素尺寸，zc为视觉传感器与被拍摄物体之间的距离，摄像头向上拍摄天花板上的特征点时，可以认为所有特征点的高度zc基本相等，hvis，r为机器人视觉的第一航向变化量，(tx，ty)为机器人视觉的相对位移量。
利用前述的视觉测量结果结合式(5)和式(6)对转弯运动的航向进行校正：
hdrift，r＝hins，r-hvis，r，式(5)；
其中，hdrift，r为机器人完成转弯运动所产生的航向误差，hins，r为单轴惯性传感器测量的机器人完成转弯运动的航向变化量，hvis，r为机器人视觉的第一航向变化量，ht，r’为经校正后t时刻的机器人转弯运动的航向，这里“’”符号作为区别标记，用于区分测量值和校正值，ht，r为t时刻单轴惯性传感器解算的航向，tr为机器人完成转弯运动所需时间，δtr为转弯运动开始的时刻与t时刻之间的间隔时间。
步骤b：对机器人直线运动的航向误差进行校正，具体地，当机器人作出直线运动时每隔设定距离时进行一次视觉测量，利用视觉传感器分别拍摄对应于第m个设定距离的起点和终点处机器人所处环境的两帧第二图像，该设定距离根据里程计测量得到，结合从该两帧第二图像中获取机器人视觉的第二航向变化量，对在该第m个设定距离内机器人直线运动的航向误差进行校正，其中m≥1。
本步骤中，在第二图像拍摄的过程中，与步骤a不同之处在于，是在直线运动过程中，每当里程计的测量值增加一设定距离，即启动摄像头。与步骤a类似，同样采用surf方法寻找和匹配两帧第二图像中的相同特征点，利用相同特征点通过视觉解算出在第m个设定距离内航向的变化hvis，l以及拍摄两帧第二图像时机器人视觉的相对位移量(tx，m’，ty，m’)，将多组相同特征点代入式(7)和式(8)中通过最小二乘小进行解算：
其中，(x1，n’，y1，n’)和(x2，n’，y2，n’)分别为第n’组相同特征点在两帧第一图像中的坐标，n’＝1、2...、n’，f为视觉传感器的焦距，dx和dy均为像素尺寸，zc为视觉传感器与被拍摄物体之间的距离，摄像头向上拍摄天花板上的特征点时，可以认为所有特征点的高度zc基本相等，hvis，l为机器人视觉的第二航向变化量，(tx，m’，ty，m’)为机器人视觉的相对位移量(即视觉观测中的机器人的直线运动量，单位：像素点)，这里“’”符号作为区别标记，用于区分第一图像和第二图像获取的特征值。
利用前述的视觉测量结果结合式(9)和式(10)对第m个设定距离内机器人直线运动的航向误差进行校正：
hdrift，l＝hins，l-hvis，l，式(9)；
其中，hdrift，l为第m个设定距离内机器人作直线运动所产生的航向误差，hins，l为单轴惯性传感器测量第m个设定距离内机器人作直线运动时的航向变化量，hvis，l为机器人视觉的第二航向变化量，ht，l’为经校正后t时刻的机器人直线运动的航向，这里“’”符号作为区别标记，用于区分测量值和校正值，ht，l为t时刻单轴惯性传感器解算的航向，tl为机器人完成第m个设定距离的直线运动所需时间，δtl为第m个设定距离的起点对应的时刻与t时刻之间的间隔时间。
前述的设定距离并没有严格的限制，可依据机器人所处环境的大小以及运算量进行灵活设置，例如可以是1.5米。
在步骤a和步骤b的航向误差修正过程中，为了更好地修正惯性传感器中陀螺的漂移，可设计当里程计每累计40～60米时，机器人停1～1.5秒，采集此时机器人的角速度测量值，在后续单轴惯性传感器解算航向的过程中以该角速度测量值为新零点。
步骤c：对机器人直线运动的里程误差进行校正，具体地从前述两帧第二图像中获取第m个设定距离内机器人视觉的直线运动量，结合里程计的测量结果，获取该第m个设定距离内机器人直线运动的里程误差并进行校正；
本步骤中，对于通常的室内结构房间，每个房间的高度通常是一个定值，因此对于相隔距离不是很大时拍摄的两帧第二图像，包含了许多重叠的景物，利用上述式(7)和式(8)解算出的机器人视觉的运动量，即
利用式(11)判断若rm/rm-1超过阈值范围，则判断在第m个设定距离内发生打滑，例如当rm/rm-1超过110％或低于90％，则判断发生打滑。此时，可利用式(12)对里程计在第m个设定距离内的测量结果进行校正，并得到此时自t-δt时刻至t时刻里程计测量结果的修正值为
其中，tg，m’为经校正后在第m个设定距离内对里程计测量结果的修正值，m为介于2和5之间的正整数。
当机器人在不同屋子中运动时，如果间隔1.5米的两帧分别拍摄的是两间屋子内的景物，此时视觉中拍摄的景物无法匹配，利用这种现象可以判断出机器人运动到了另一间屋子、并与打滑区分开，有效避免了打滑的误判断。
步骤d：在对机器人转弯运动的航向误差、直线运动的航向误差和直线运动的里程误差进行校正后对机器人的轨迹进行解算。
本步骤中，利用式(13)和式(14)对机器人轨迹进行解算：
xt＝cos(ht’)·dt’+xt-at，式(13)；
yt＝sin(ht′)·dt′+yt-δt，式(14)；
其中，ht′为经校正后t时刻的机器人转弯运动的航向ht，r’或直线运动的航向ht，l’，dt′为自t-δt时刻至t时刻机器人转弯运动时里程计的测量结果、或机器人直线运动时无打滑发生时里程计的测量结果、或发生打滑时里程计测量结果的修正值
至此，本实施例机器人自主导航定位方法介绍完毕。以下对本实施例机器人自主导航定位系统作详细说明。
图2为本公开一实施例机器人自助导航定位系统的示意图。如图2所示，本实施例机器人自动导航定位系统，包括：
视觉传感器13，布置于机器人小车10上，其主光轴沿机器人小车转动轴向设置，用于拍摄机器人所处环境的照片；
单轴惯性传感器12，布置于机器人小车10上，其测量轴沿机器人小车转动轴向设置，用于测量机器人小车的航向；
轮式里程计11，布置于机器人小车10上，用于测量机器人直线运动和转弯运动的运动量；
数据处理及控制模块20，电性连接至视觉传感器13、单轴惯性传感器12以及轮式里程计11，用于根据单轴惯性传感器12、轮式里程计11以及视觉传感器13输出的数据，执行如前述步骤a至步骤d所述的机器人自主导航定位方法。
其中，视觉传感器11在本实施例中为广角摄像头；单轴惯性传感器12可为简单的单轴mems惯性传感器；本实施例中机器人小车10为轮式机器人，可以理解还可为其他形式的机器人，此时可选用其他本领域中常用的里程计进行机器人运动量的测量。
至此，本实施例机器人自主导航定位系统介绍完毕。
目前本公开的方法已可应用到以树莓派嵌入式系统为核心的机器人小车上，能够快速准确的实时解算出机器人的行驶轨迹。以下通过两个实验对本公开的效果作进一步说明，在这两个实验中，将本公开的机器人自主导航定位方法运行在该机器人小车的树莓派嵌入式系统中。
实验1
如图3所示，机器人在两间屋子及连接它们的小走廊中循环行驶了三圈，可以实时解算出机器人的行驶轨迹，对机器人任意时刻的位置进行定位，定位误差小于0.2米。
实验2
如图4和5所示，机器人在更大范围内行驶，实验区域为一层楼，尺寸约为40米×20米，包括一条l型长走廊和两个房间。在部分区域机器人按照扫地机器人常采用的“弓”字型轨迹前进，图中实线为前进时解算出来的轨迹结果、虚线为返程时解算出来的轨迹结果，结果显示，图4所示解算出的轨迹与实际轨迹更为接近。
至此，已经结合附图对本公开实施例进行了详细描述。需要说明的是，在附图或说明书正文中，未绘示或描述的实现方式，均为所属技术领域中普通技术人员所知的形式，并未进行详细说明。此外，上述对各元件和方法的定义并不仅限于实施例中提到的各种具体结构、形状或方式，本领域普通技术人员可对其进行简单地更改或替换。
综上所述，本公开提供一种机器人自主导航定位方法及系统，利用单轴惯性传感器和里程计对机器人进行定位运算，并结合视觉测量消除运算过程中的航向误差和里程误差，充分利用了惯导、视觉、和里程计的信息互补性，具有运算量非常小、硬件简洁的优点。
还需要说明的是，附图中各部件的形状和尺寸不反映真实大小和比例，而仅示意本公开实施例的内容。另外，在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。
再者，单词“包含”不排除存在未列在权利要求中的元件或步骤。说明书与权利要求中所使用的序数例如“第一”、“第二”、“第三”等的用词，以修饰相应的元件，其本身并不意味着该元件有任何的序数，也不代表某一元件与另一元件的顺序、或是制造方法上的顺序，该些序数的使用仅用来使具有某命名的一元件得以和另一具有相同命名的元件能做出清楚区分。
此外，除非特别描述或必须依序发生的步骤，上述步骤的顺序并无限制于以上所列，且可根据所需设计而变化或重新安排。并且上述实施例可基于设计及可靠度的考虑，彼此混合搭配使用或与其他实施例混合搭配使用，即不同实施例中的技术特征可以自由组合形成更多的实施例。
本公开可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。本公开的各个部件实施例可以以硬件实现，或者以在一个或者多个处理器上运行的软件模块实现，或者以它们的组合实现。本领域的技术人员应当理解，可以在实践中使用微处理器或者数字信号处理器(dsp)来实现根据本公开实施例的相关设备中的一些或者全部部件的一些或者全部功能。本公开还可以实现为用于执行这里所描述的方法的一部分或者全部的设备或者装置程序(例如，计算机程序和计算机程序产品)。这样的实现本公开的程序可以存储在计算机可读介质上，或者可以具有一个或者多个信号的形式。这样的信号可以从因特网网站上下载得到，或者在载体信号上提供，或者以任何其他形式提供。
以上所述的具体实施例，对本公开的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本公开的具体实施例而已，并不用于限制本公开，凡在本公开的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。