一种采用测距激光扫描建图的机器人室内定位系统及方法与流程

本发明涉及移动机器人角度矫正技术领域，具体为一种采用测距激光扫描建图的机器人室内定位系统及方法。

背景技术：

目前机器人室内定位建图有陀螺仪室内定位导航，及360°激光雷达扫描的定位导航方式。采用陀螺仪的室内定位方式，优点是低成本、易安装，缺点是由于轮子打滑及时间的累积误差，导致地图慢慢的出现偏移而无法矫正，最终导致定位失败。360°激光雷达扫描方式定位建图方式，优点是建图成功率较高，能根据激光数据实时矫正地图，缺点是成本高，且对模具的要求较高，激光雷达不易安装，且由于激光雷达内置动作旋转机构，容易损坏。

技术实现要素：

针对现有技术的不足，本发明提供了一种采用测距激光扫描建图的机器人室内定位系统及方法，解决了目前移动机器人角度矫正技术无法长久有效或者成本较高的问题。

为实现上述目的，本发明提供如下技术方案：一种采用测距激光扫描建图的机器人室内定位系统，包括设置于机器人内的mcu，与mcu连接的激光测距模块、陀螺仪、机器人左右行动轮；所述机器人至少一侧面设置激光测距模块，且机器人左右行动轮内分别设有与mcu连接的里程计模块。

为了符合移动机器人工字型运动或者更加准确的扫描侧面障碍物获得坐标，所述机器人两侧面均设置激光测距模块。

为了得到准确的机器人移动后的坐标，所述机器人正前方面设置激光测距模块。

一种采用测距激光扫描建图的机器人室内定位方法，包括以下步骤：

步骤1，开机以当前点为原始坐标建立坐标系，机器人的激光测距模块不断测量障碍物距离，陀螺仪模块不停积分计算角度；

步骤2，根据机器人行走的距离确定机器人当前坐标，机器人直线行走或曲线行走；

具体为机器人直线行走时，t1时刻，机器人的坐标为(x1，y1，θ1)，其中θ1为机器人在t1时刻陀螺仪模块计算出的角度；t2时刻陀螺仪计算出的角度为θ2，根据机器人左右行动轮内的里程计模块获得t1时刻与t2时刻之间的机器人移动的距离d，则t2时刻的坐标(x2,y2,θ2)，且计算公式如下：x2＝x1+d*cos(θ2)，y2＝y1+d*sin(θ2)；若机器人曲线行走，曲线行走是将每一个短距离直线行走的积分过程，只需将多个短距离直线行走的过程积分即可得到曲线行走的机器人当前坐标；

步骤3，根据步骤2中得到的机器人当前角度坐标，机器人侧面激光测距模块测得与障碍物的距离dr、激光测距模块与机器人正前方的夹角θr、机器人的半径r，计算出障碍物的坐标(xr，yr)；并在接下来的运动中获得障碍物的曲线函数；

具体的计算公式为：当前机器人坐标为(x2,y2)，当前角度为θ2，

xr＝x2+(r+dr)*cos(θ2+θr)

yr＝y2+(r+dr)*sin(θ2+θr)

步骤4，机器人在t1时间运动的过程中按照第3步骤计算出的障碍物的集合{(xr1,yr1),(xr2,yr2),(xr3,yr3)…}，得到障碍物的曲线函数y＝f(x)；机器人在t2时间运动的过程中不断的扫描障碍物同样按照第3步骤得到障碍物曲线函数y＝f2(x)；

t2时间，如果机器人回到t1时刻扫描出的障碍物y＝f(x)的附近，且机器人的与障碍物y＝f(x)的距离在激光的测距范围内，则t2时刻的障碍物曲线y＝f2(x)与y＝f(x)可能为同一障碍物，通过最小二乘法对曲线y＝f2(x)和y＝f(x)做曲线拟合，如果曲线拟合成功，则为同一障碍物，可计算出曲线y＝f2(x)与曲线y＝f(x)的偏差，该偏差即为机器人t2时刻坐标与t1时刻坐标的偏差，即可修正机器人t2时刻的坐标，还可计算出障碍物曲线的旋转角度，从而修正t2时刻陀螺仪数据。

作为优选，所述步骤4中机器人的运动方式为水平移动。

作为优选，所述步骤4中机器人的运动为原地旋转；此时机身的坐标(x，y)没有变化，但角度θ产生变化，激光测距模块可按照第3步骤计算出扫描出的障碍物坐标，得到障碍物的坐标曲线函数，通过步骤4两次时间t1,t2的运动，同样用最小二乘法进行曲线拟合判断，并且修正坐标和陀螺仪数据。

进一步的，在只有一侧面的激光测距模块情况下，所述机器人采用弓字行走路线：t1时间直线行走，碰到障碍物，y轴坐标移动一定距离，然后掉头180，再直线行走，碰到障碍物，y轴坐标移动一定距离，然后t2时间掉头180，再直线行走，t2时间的直行路线和t1的直行路线是两条平行线，且就在t1时间直行的附近，根据步骤3、4曲线拟合矫正算法更为有效，更能发挥该算法的矫正精准度。

作为优选，所述机器人两侧均设置激光测距模块。

进一步的，在两侧面都有激光测距模块情况下，所述机器人采用弓字行走路线：t1时间直线行走，碰到障碍物，y轴坐标移动一定距离，然后t2时间掉头180，再直线行走，t2时间的直行路线和t1的直行路线是两条平行线，且就在t1时间直行的附近，根据步骤3、4曲线拟合矫正算法更为有效，更能发挥该算法的矫正精准度；

作为优选，所述机器人正前方面设有激光测距模块，在步骤2中，机器人在直线行走的时候，正前方的激光测距模块测量前部障碍物的距离，通过距离的变化修正里程计的数据，得到准确的当前机器人坐标；

算法如下：t1时间测量到障碍物的距离为s1；t2时间，测量到障碍物的距离为s2，t1到t2时刻里程计测量到的距离为d；则t1到t2时刻机器人移动的距离d21＝d*(1-k)+(s1-s2)*k；其中k为激光测距模块计算值的信任度，值0～1之间；1表明100％信任激光测距模块，0表示不信任测距模块；k的值可以根据卡尔曼滤波算法或二阶滤波算法等滤波算法结合其他传感器数据计算出最优值。

本发明的有益效果：结合陀螺仪导航和360°激光雷达扫描定位方式的优点，采用至少一组固定的测距激光，即可实现机器人的室内定位导航，产品成本比360°激光雷达扫描建图方式低，且易于安装，室内定位导航的效果优于陀螺仪导航，接近360°激光雷达扫描建图的效果；适用于智能扫地机器人的室内导航定位技术。

附图说明

图1为本发明的控制原理图。

图2为本发明的状态图。

图3为本发明的流程图。

图4为发明的实施例2的状态图。

具体实施方式

实施例1

如图1、2、3所示，一种采用测距激光扫描建图的机器人室内定位系统，包括设置于机器人内的mcu，与mcu连接的激光测距模块、陀螺仪、机器人左右行动轮；所述机器人至少一侧面设置激光测距模块，且机器人左右行动轮内分别设有与mcu连接的里程计模块；所述机器人正前方面设置激光测距模块。

4.一种采用测距激光扫描建图的机器人室内定位方法，其特征在于，包括以下步骤：

步骤1，开机以当前点为原始坐标建立坐标系，机器人的激光测距模块不断测量障碍物距离，陀螺仪模块不停积分计算角度；

步骤2，根据机器人行走的距离确定机器人当前坐标，机器人直线行走或曲线行走；

具体为机器人直线行走时，t1时刻，机器人的坐标为(x1，y1，θ1)，其中θ1为机器人在t1时刻陀螺仪模块计算出的角度；t2时刻陀螺仪计算出的角度为θ2，根据机器人左右行动轮内的里程计模块获得t1时刻与t2时刻之间的机器人移动的距离d，则t2时刻的坐标(x2,y2,θ2)，且计算公式如下：x2＝x1+d*cos(θ2)，y2＝y1+d*sin(θ2)；若机器人曲线行走，曲线行走是将每一个短距离直线行走的积分过程，只需将多个短距离直线行走的过程积分即可得到曲线行走的机器人当前坐标；

步骤3，根据步骤2中得到的机器人当前角度坐标，机器人侧面激光测距模块测得与障碍物的距离dr、激光测距模块与机器人正前方的夹角θr、机器人的半径r，计算出障碍物的坐标(xr，yr)；并在接下来的运动中获得障碍物的曲线函数；

具体的计算公式为：当前机器人坐标为(x2,y2)，当前角度为θ2，

xr＝x2+(r+dr)*cos(θ2+θr)

yr＝y2+(r+dr)*sin(θ2+θr)

步骤4，机器人在t1时间运动的过程中按照第3步骤计算出的障碍物的集合{(xr1,yr1),(xr2,yr2),(xr3,yr3)…}，得到障碍物的曲线函数y＝f(x)；机器人在t2时间运动的过程中不断的扫描障碍物同样按照第3步骤得到障碍物曲线函数y＝f2(x)；

t2时间，如果机器人回到t1时刻扫描出的障碍物y＝f(x)的附近，且机器人的与障碍物y＝f(x)的距离在激光的测距范围内，则t2时刻的障碍物曲线y＝f2(x)与y＝f(x)可能为同一障碍物，通过最小二乘法对曲线y＝f2(x)和y＝f(x)做曲线拟合，如果曲线拟合成功，则为同一障碍物，可计算出曲线y＝f2(x)与曲线y＝f(x)的偏差，该偏差即为机器人t2时刻坐标与t1时刻坐标的偏差，即可修正机器人t2时刻的坐标，还可计算出障碍物曲线的旋转角度，从而修正t2时刻陀螺仪数据。

所述步骤4中机器人的运动方式为水平移动；采用弓字行走路线：t1时间直线行走，碰到障碍物，y轴坐标移动一定距离，然后掉头180，再直线行走，碰到障碍物，y轴坐标移动一定距离，然后t2时间掉头180，再直线行走，t2时间的直行路线和t1的直行路线是两条平行线，且就在t1时间直行的附近，根据步骤3、4曲线拟合矫正算法更为有效，更能发挥该算法的矫正精准度。

所述机器人正前方面设有激光测距模块，在步骤2中，机器人在直线行走的时候，正前方的激光测距模块测量前部障碍物的距离，通过距离的变化修正里程计的数据，得到准确的当前机器人坐标；

算法如下：t1时间测量到障碍物的距离为s1；t2时间，测量到障碍物的距离为s2，t1到t2时刻里程计测量到的距离为d；则t1到t2时刻机器人移动的距离d21＝d*(1-k)+(s1-s2)*k；其中k为激光测距模块计算值的信任度，值0～1之间；1表明100％信任激光测距模块，0表示不信任测距模块；k的值可以根据卡尔曼滤波算法或二阶滤波算法等滤波算法结合其他传感器数据计算出最优值。

实施例2

如图1、2、3、4所示，一种采用测距激光扫描建图的机器人室内定位系统，包括设置于机器人内的mcu，与mcu连接的激光测距模块、陀螺仪、机器人左右行动轮；所述机器人至少一侧面设置激光测距模块，且机器人左右行动轮内分别设有与mcu连接的里程计模块；所述机器人两侧面均设置激光测距模块；所述机器人正前方面设置激光测距模块。

一种采用测距激光扫描建图的机器人室内定位方法，包括以下步骤：

步骤1，开机以当前点为原始坐标建立坐标系，机器人的激光测距模块不断测量障碍物距离，陀螺仪模块不停积分计算角度；

步骤2，根据机器人行走的距离确定机器人当前坐标，机器人直线行走或曲线行走；

具体为机器人直线行走时，t1时刻，机器人的坐标为(x1，y1，θ1)，其中θ1为机器人在t1时刻陀螺仪模块计算出的角度；t2时刻陀螺仪计算出的角度为θ2，根据机器人左右行动轮内的里程计模块获得t1时刻与t2时刻之间的机器人移动的距离d，则t2时刻的坐标(x2,y2,θ2)，且计算公式如下：x2＝x1+d*cos(θ2)，y2＝y1+d*sin(θ2)；若机器人曲线行走，曲线行走是将每一个短距离直线行走的积分过程，只需将多个短距离直线行走的过程积分即可得到曲线行走的机器人当前坐标；

步骤3，根据步骤2中得到的机器人当前角度坐标，机器人侧面激光测距模块测得与障碍物的距离dr、激光测距模块与机器人正前方的夹角θr、机器人的半径r，计算出障碍物的坐标(xr，yr)；并在接下来的运动中获得障碍物的曲线函数；

具体的计算公式为：当前机器人坐标为(x2,y2)，当前角度为θ2，

xr＝x2+(r+dr)*cos(θ2+θr)

yr＝y2+(r+dr)*sin(θ2+θr)

步骤4，机器人在t1时间运动的过程中按照第3步骤计算出的障碍物的集合{(xr1,yr1),(xr2,yr2),(xr3,yr3)…}，得到障碍物的曲线函数y＝f(x)；机器人在t2时间运动的过程中不断的扫描障碍物同样按照第3步骤得到障碍物曲线函数y＝f2(x)；

t2时间，如果机器人回到t1时刻扫描出的障碍物y＝f(x)的附近，且机器人的与障碍物y＝f(x)的距离在激光的测距范围内，则t2时刻的障碍物曲线y＝f2(x)与y＝f(x)可能为同一障碍物，通过最小二乘法对曲线y＝f2(x)和y＝f(x)做曲线拟合，如果曲线拟合成功，则为同一障碍物，可计算出曲线y＝f2(x)与曲线y＝f(x)的偏差，该偏差即为机器人t2时刻坐标与t1时刻坐标的偏差，即可修正机器人t2时刻的坐标，还可计算出障碍物曲线的旋转角度，从而修正t2时刻陀螺仪数据。

所述步骤4中机器人的运动方式为水平移动；所述机器人两侧均设置激光测距模块；采用弓字行走路线：t1时间直线行走，碰到障碍物，y轴坐标移动一定距离，然后t2时间掉头180，再直线行走，t2时间的直行路线和t1的直行路线是两条平行线，且就在t1时间直行的附近，根据步骤3、4曲线拟合矫正算法更为有效，更能发挥该算法的矫正精准度。

所述机器人正前方面设有激光测距模块，在步骤2中，机器人在直线行走的时候，正前方的激光测距模块测量前部障碍物的距离，通过距离的变化修正里程计的数据，得到准确的当前机器人坐标；

算法如下：t1时间测量到障碍物的距离为s1；t2时间，测量到障碍物的距离为s2，t1到t2时刻里程计测量到的距离为d；则t1到t2时刻机器人移动的距离d21＝d*(1-k)+(s1-s2)*k；其中k为激光测距模块计算值的信任度，值0～1之间；1表明100％信任激光测距模块，0表示不信任测距模块；k的值可以根据卡尔曼滤波算法或二阶滤波算法等滤波算法结合其他传感器数据计算出最优值。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换或改进等，均应包含在本发明的保护范围之内。