基于先进路径规划技术的智能清洁机器人及其清洁方法【技术领域】
本发明属于智能车领域，具体涉及一种基于先进路径规划技术的智能清洁机器人及其清洁
方法。
【背景技术】
目前，公知的扫地机器的机身为无线机器，以圆盘型为主。使用充电电池为电源运作，操
作方式以遥控器、机器上的操作面板为主。一般能设定时间预约打扫，自行充电。机器人前方
设置有传感器，可侦测障碍物，如侦测到墙壁或其他障碍物，会自行转弯，依据不同厂商设定，
而按不同的判断机制行驶，有规划的清扫地区。(部分较早期机型可能缺少其中部分功能)。一
般的扫地机器人采用的是随机路径清扫方式，其清扫面积覆盖率低，容易发生部分房间与角落
的漏扫和重复清扫，且清洁耗时长。
【发明内容】
本发明的目的在于克服现有技术中存在的问题，提供一种基于先进路径规划技术的智能清
洁机器人及其清洁方法，该机器人能够自动完成地板清理工作，能自动记录环境地图判断自己
的位置并规划出清洁路径进行清洁。
为了达到上述目的，本发明清洁机器人采用如下技术方案：
包括安装在机器人机身上的外部环境信息探测系统、实时定位系统、主控制系统、电机驱
动模块以及电源，其中：
外部环境信息探测系统用于探测外部环境信息；实时定位系统用于获取机器人自身所在的
实时位置；主控制系统用于获取外部环境信息探测系统探测的外部环境信息，并建立栅格地图，
通过结合栅格地图和实时位置，用先进路径规划算法规划出清扫路径；电机驱动模块用于驱动
机器人按规划出的清扫路径运行并进行清扫；电源用于给主控制系统和电机驱动模块供电。
进一步地，外部环境信息探测系统包括均匀布置在机器人机身前侧的若干个红外反射传感
器，以及均匀布置在机身顶部的若干个超声波测距传感器，超声波测距传感器还与充电基座相
配合。
进一步地，实时定位系统包括用于获取机器人角速度的陀螺仪、用于采集机器人加速度的
三轴加速度计以及用来采集机器人实时速度的增量型旋转编码器。
进一步地，主控制系统还用于调整机器人的运行模式和行驶速度。
进一步地，机器人的机身侧面安装有用来检测障碍的避障传感器，以及用于记录突发碰撞
情况的碰撞传感器。
进一步地，机器人的机身底部安装有用于判断是否脱离地面的红外近距传感器。
进一步地，电机驱动模块包括与主控制器相连的驱动电路，驱动电路同时连接用于控制机
器人运动的第一无刷直流电机和用于控制机器人清洁的第二无刷直流电机。
本发明清洁方法采用如下技术方案，包括以下步骤：
步骤一，对机器人进行初始化；
步骤二，用户对机器人进行功能选择；
步骤三：以充电基座为坐标原点，并在机器人工作平面上建立直角坐标系，通过机器人机
身上均匀分布的若干个超声波测距传感器接收来自充电基座的超声波信号，判断机器人是否位
于充电基座及电量是否充足，在机器人位于充电基座且电量充足时，进入步骤四，否则返回充
电基座并在电量不足时进行充电，然后进入步骤四；
步骤四：探测机器人外部环境信息，建立栅格地图；
步骤五：在栅格地图中将待清洁的区域分割成单元区域，先找出单元区域的边界作为清扫
路径的第一步，然后在整块的矩形区域中横坐标不变，沿着纵坐标从小到大方向移动，到达顶
端后，再沿着水平方向移动一个栅格，然后纵坐标从大到小方向移动，依次进行，直至规划出
清扫路径；
步骤六：按规划出的清扫路径运行，进行清扫，并完成航迹推算定位；
步骤七：在清扫过程中，进行路障扫描，遇到障碍时进行避障同时把障碍物所在位置信息
记录进地图数据中；
步骤八：判断是否打扫完全部可清洁栅格，若清扫完成则进入步骤九；若没有结束，返回
步骤三；
步骤九：结束。
进一步地，步骤六中航迹推算定位采用如下公式：
601、在清扫过程中，每隔Δt的单位时间，通过陀螺仪获取一次机器人的加速度ai，通过
三轴加速度计采集一次机器人的角速度Wi，其中参数角标i表示第i次测量，计算得到：
Via＝0.5(ai-1+ai)Δt(1)；
Sia＝0.5(Vi-1+Vi)Δt(2)；
θia＝0.5(Wi-1+Wi)Δt(3)；
其中，Via表示机器人的实时速度，Sia表示在第i个Δt时间内的位移，θia表示通过式(3)
计算得到的单位时间内机器人所转过的角度；并由以上式(1)～(3)得到：
ΔXia＝Siacosθia(4)
ΔYia＝Siasinθia(5)
其中，ΔXia表示X坐标的增量，ΔYia表示Y坐标的增量；并由以上式(4)～(5)得到机
器人当前坐标(Xa,Ya)：
(Xa,Ya)＝(Xia-1+ΔXia,Yia-1+ΔYia)(6)；
602、机器人行驶的左轮和右轮分别连接第一无刷直流电机，第一无刷直流电机上均安装
有增量型旋转编码器；在清扫过程中，在每个Δt的单位时间内，分别获取左轮侧的增量型旋
转编码器发出的脉冲数nLi和右轮侧的增量型旋转编码器发出的脉冲数nRi，得到单位时间内左
轮所走过的路程dLi和单位时间内右轮所走过的路程dRi：
d L i = 2 π n L i m R - - - ( 7 ) ; d R i = 2 π n R i m R - - - ( 8 ) ; ]]>
其中，m为增量型旋转编码器转一圈所发出的脉冲数，R表示左轮和右轮的半径，从而，
单位时间内左轮和右轮的路程差Δdi＝|dLi-dRi|(9)；
由此， θ i b = arc c o s 2 r 2 - Δd i 2 r 2 - - - ( 10 ) ; ]]>Sib＝0.5(dLi+dRi)(11)；
其中，θib表示通过式(10)计算得到的单位时间内机器人所转过的角度，r为左轮和右轮
之间的距离；Sib为单位时间内左轮和右轮的路程平均值；
同时ΔXib＝Sibcosθib(12)；ΔYib＝Sibsinθib(13)；
结合上式(10)～(13)，得到机器人当前坐标(Xb,Yb)，
(Xb,Yb)＝(Xib-1+ΔXib,Yib-1+ΔYib)(14)；
603、通过步骤601和步骤602所得的坐标(Xa,Ya)和(Xb,Yb)，获得机器人的精确坐标为
(X,Y)＝(AXa+BXb,AYa+BYb)，
其中，A是陀螺仪的置信度值，B是增量型旋转编码器的置信度值，A和B是通过以下方
法分别获得的：
θ a = Σ i = 1 n θ i a - - - ( 15 ) ; θ b = Σ i = 1 n θ i b - - - ( 16 ) ]]>
j＝θ-θa(17)；k＝θ-θb(18)；
A = k j + k - - - ( 19 ) ; B = j j + k - - - ( 20 ) ; ]]>
其中θ为机器人机身上的超声波测距传感器在能够收到来自充电基座的超声波信号范围
内，机器人和充电基座与X轴之间的实时夹角。
进一步地，步骤七中的障碍物的边长为L，包括小型障碍、大型障碍和墙形障碍，其避障
方法如下：
(1)L≤25cm的为小型障碍，机器人遇到小型障碍时，在距离小型障碍5cm处，环绕小
型障碍清扫一圈，继续沿原清扫路线前进；
(2)25＜L≤100cm的为大型障碍，在距离大型障碍5cm处清扫一圈，然后在距离大型障
碍物15cm处清扫一圈；通过航迹推算定位实时获得机器人相对于充电基座的坐标，并记录最
后一圈清扫路径上水平坐标和纵坐标的最大值与最小值，求得水平坐标最大值和最小值的平均
值作为中心点的水平坐标，求得纵坐标的最大值和最小值的平均值，作为中心点的纵坐标，以
过中心点且分别平行于X轴的线段和平行于Y轴的线段，将未清扫区域划分为四个单元域，
进行S型清扫；
(3)L＞100cm的为墙型障碍，在距离墙形障碍1cm处，沿边清扫，直到遇到转角处，
沿原路径返回起始点，继续之前清扫路线。
与现有技术相比，本发明具有以下有益的技术效果：
本发明机器人能够凭借着多传感器信息融合，记录环境地图，并基于先进路径规划技术设
计出最优的清洁路径，实时判断现处的位置并进行避障，可以在清洁地面时选择最优路径来节
省清洁时间，同时实现对其清扫环境的最大覆盖率。
本发明清洁方法中采用修扫边界，制造矩形，平行铺扫，兼顾障碍的先进路径规划算法，
规划出最优清扫路径，实现对其清扫环境的最大覆盖率，有效避免房间与角落的漏扫和重复清
扫，极好地提高清洁效率。
进一步地，本发明通过采用陀螺仪、三轴加速度计和增量型旋转编码器配合来完成航迹推
算定位，同时利用机器人机身上的超声波测距传感器与充电基座的配合，精确的测量出机器人
和充电基座与X轴之间的夹角，并和陀螺仪与旋转编码器多次叠加所得到的夹角比较，从而
得到陀螺仪与增量型旋转编码器的偏差，减小陀螺仪或增量型旋转编码器单独工作的偏差，从
而实现机器人的精确定位，确定其本身在栅格地图中的位置。
【附图说明】
图1为本发明的系统总框图。
图2为本发明的硬件结构图。
图3为本发明的流程图。
图4为本发明的大型避障碍说明图。
【具体实施方式】
下面结合附图对本发明做进一步详细说明。
图1为系统的总框图，整个系统划分为4个主要部分：传感器部分，控制部分，人机交互
部分和电机驱动部分。控制部分中的主控制系统控制智能清洁机器人的两个用于运动的第一无
刷直流电机与两个用于清洁的第二无刷直流电机，并利用多传感器信息融合技术不断调整智能
清洁机器人的运行模式和行驶速度，实现智能清洁机器人的实时调整与控制。再通过人机交互
模块可以实现对智能清洁机器人工作状态和性能的显示，并通过wifi技术可以完成智能清洁
机器人的遥控与预约功能。
图2为硬件结构图，主要包括：
(1)传感器部分：
包括安装在机器人机身上的外部环境信息探测系统和实时定位系统。
其中，外部环境信息探测系统用于探测外部环境信息，包括均匀布置在机器人机身前侧的
十一个红外反射传感器，以及均匀布置在机身顶部的四个超声波测距传感器；超声波测距传感
器还与充电基座相配合。在机器人机身上还安装有用于判断突发碰撞情况的碰撞传感器，以及
用来检测障碍情况的避障传感器。机器人的机身底部安装有用于判断是否脱离地面的红外近距
传感器。
实时定位系统用于获取机器人自身所在的实时位置，包括陀螺仪模块和增量型旋转编码
器；陀螺仪模块中含有用于获取机器人角速度的陀螺仪，以及用于采集机器人加速度的三轴加
速度计；增量型旋转编码器用来采集机器人实时速度，其采集原理是：增量型旋转编码器与第
一无刷直流电机同轴，第一无刷直流电机旋转时，增量型旋转编码器的光栅盘与第一无刷直流
电机同步旋转，经发光二极管等电子元件组成的检测装置检测输出若干脉冲信号，通过计算在
一定时间内的输出脉冲的个数计算出当前第一无刷直流电机的转速，从而采集到与第一无刷直
流电机转速相等的各车轮转速，得出实时速度，增量型旋转编码器与陀螺仪和三轴加速度计相
配合，利于提高航迹精度。
(2)控制部分：控制电路以STM32为核心芯片，配备完善的外围电路。主要通过获取传
感器的反馈信号获取智能清洁机器人运行状态，并依此信息控制电机的运行和实现相关功能。
例如：获取避障传感器，碰撞传感器的反馈信号，将反馈信号经过控制部分的相关处理与运算
对路况进行判断，并传出相应的控制信号；获取机器人机身底部安装的红外近距传感器的反馈
信号，判断机身是否着地(如果机身底部和地面之间的距离超过阈值，则表明机身离地，此时
机器人自动关机)；通过设置开关，判断车体内的集尘盒安装状态，在集尘盒安装上后，会压
住开关使开关闭合使电路导通，此时该电路将高电平信号传给处理器，使处理器收到集尘盒已
安装的信息，若处理器没有接收到高电平信号则故障灯亮；获取主面板的信号从而控制智能清
洁机器人的模式转换。
(3)人机交互部分：该部分主要由无线通信模块，按键，显示屏，蜂鸣器，LED组成。其
中无线通信实现手机与单片机进行信息传输；按键可以供用户选择模式，可以选择清扫模式，
比如自动清扫模式，快速清扫模式，选择速度等等，实现全区域的清洁，避免漏扫；当智能清
洁机器人处于不同模式或工作状态时，点亮不同的LED灯，提醒用户；将电池电量，当前清
扫完成度等信息显示在显示屏上。
(4)驱动电机部分：该部分由电机驱动电路、直流无刷电机、信号检测电路、系统保护电
路组成。
本发明机器人的机体呈扁圆形，在机体前半段设置数个红外反射传感器，用来检测障碍，
进行障碍物的躲避，车底部设置红外近距传感器用于判断是否脱离地面，且呈三角分布两个动
力轮与一个万向轮，动力轮连接第一无刷直流电机，万向轮左右两侧分布动力刷用于清理，动
力刷连接第二无刷直流电机，底部中部设置进污口，机体上方分布按键与取放垃圾装置。利用
布置在机器反射人内部的小型真空吸尘器和布置在机器人前端的双扫刷将地面杂物先吸纳进
入自身的集尘盒，从而完成地面清理功能。利用充电基座发出的超声波信号，和机器人顶部布
置在四角的超声波接收器，确定充电基座的位置并能在电量不足的时候自动回充电基座充电。
在机身顶部的四个角落布置四组超声波测距传感器，互成九十度夹角。
本发明以圆盘机身为载体，利用含三轴加速度计的陀螺仪模块和增量型旋转编码器实现室
内定位、通过布置在机身前侧圆弧的红外反射传感器和机身上部的四个超声波测距传感器感知
外部环境、通过机身上部的超声波测距传感器与充电基座发射的超声波来确定其相对充电机座
的位置，并在电量不足时自动回充电基座充电、利用先进算法自动规划最优清扫路径。本装置
主要用于实现清洁时间最省，覆盖面积最大的室内清洁。
参见图3，本发明主要的工作过程及原理如下：
步骤一：开始，对机器人进行初始化；
步骤二：用户对机器人进行功能选择，如速度和模式等；
步骤三：充电基座位于墙壁拐角处或紧靠墙壁，以充电基座为坐标原点并在机器人工作平
面上建立直角坐标系，判断机器人是否位于充电基座，当机器人位于充电基座时，若电量充足
时直接进入步骤四，若电量不足则充电后进入步骤四；当机器人不在充电基座时，开始沿边搜
索充电基座，回到充电基座后，若电量不足则充电，电量充足则进入步骤四；
步骤四：利用机身前侧成弧线布置的11个红外反射传感器，和布置在机器顶部的四个角
的超声波测距传感器，感知外部环境信息，包括机器人周围障碍物信息和充电基座方位信息。
401、在已知环境地图的情况下，机器人将使用超声波测距传感器和红外反射传感器获得
外部环境信息与已有的环境地图进行对比，当出现不一致之处时则更新环境地图信息，当变动
量超过阀值则重新绘制环境地图。
402、在未知环境地图的情况下，机器人将利用红外反射传感器获取近处障碍物的信息，
并以20CM为边长的正方形作为栅格大小，建立栅格地图。栅格地图的单位域主要包含了以下
信息类型：
栅格的位置坐标；
栅格是否可通行；
栅格在路径规划中的特征标识；
栅格在单元区域分割的序号。
步骤五：在已知环境地图和实时位置时，可以利用先进路径规划算法自动规划出最优清扫
路径。
利用已有的栅格地图规划最优清扫路径，实现清洁时间最省。
地图已知情况下的单元域域内规划遵循的基本原则是:修扫边界，制造矩形，平行铺扫，
兼顾障碍。
即使用效率最高的覆盖方——S型的规整扫地方式扫过地面，即机器人横坐标不变，沿着
纵坐标从小到大方向移动，到达顶端后，机器人沿着水平方向移动一个栅格，再纵坐标从大到
小方向移动，依次进行。这种方式对于矩形区域来说效率是最高，可是任务区域往往并非理想
的矩形，采用这种方法在曲折的边界过多转弯会影响速度，故在栅格地图中将待清洁的区域先
大致分块，分割成单元区域，如图4中的单元域1至单元域4，在具体路径规划过程中优先沿
着单元区域的边界清扫，然后在整块的矩形区域采用S型的规整扫地方式。
步骤六：按规划出的最优清扫路径运行，进行清扫；利用含三轴加速度计的陀螺仪模块与
增量型旋转编码器配合算法来完成航迹推算定位。该方法通过对陀螺仪模块提供的加速度和角
速度和增量型旋转编码器提供的双轮加速度进行二重积分，并依据航迹推算基本算法，来对自
身进行精确定位，确定其本身在栅格地图中的位置。
601、基于陀螺仪的航迹推算公式如下：
S = ∫ 0 t ∫ 0 t a ( t ) d t d t - - - ( 1 ) ]]>
θ = ∫ 0 t W ( t ) d t - - - ( 2 ) ]]>
其中a(t)表示t时刻的加速度，W(t)表示t时刻的角速度，S表示t时刻的位移，θ表示t时刻
转过角度。且S(0)＝0，θ(0)＝0。用数值分析的方法，将利用梯形数值积分公式，化简运算
公式为:
V i a = ( Σ i = 0 n a i - 0.5 a 0 - 0.5 a i ) x Δ t - - - ( 3 ) ]]>
S i a = ( Σ i = 1 n V i - 0.5 V 0 - 0.5 V i ) x Δ t - - - ( 4 ) ]]>
θ i a = ( Σ i = 1 n W i - 0.5 W 0 - 0.5 W i ) x Δ t - - - ( 5 ) ]]>
ai表示第i次测量得到的加速度，Via表示实时速度，Sia表示第i次Δt时间内的位移，Wi表
示第i次测量得到的角速度，θia表示通过式(5)计算得到的单位时间内机器人所转过的角度，
Δt表示测量时间间隔，为单位时间；参数角标i，表示第i次测量得到的数据。其中ai、Via和
Sia都包含水平和竖直两个分量。
由于上一次的积分得到的速度、位置结果中已经包含之前所有运动历史数据的信息，所以
在进行下一次航迹推算时，只需在上一次的速度、位置上进行递推即可。
Via＝0.5(ai-1+ai)Δt(6)
Sia＝0.5(Vi-1+Vi)Δt(7)
θia＝0.5(Wi-1+Wi)Δt(8)
由上可得：
ΔXia＝Siacosθia(9)
ΔYia＝Siasinθia(10)
(Xa,Ya)＝(Xia-1+ΔXia,Yia-1+ΔYia)(11)
其中，ΔXia表示X坐标的增量，ΔYia表示Y坐标的增量。(Xa,Ya)表示用陀螺仪和加速度
计得到的机器人当前坐标；参数角标i，表示第i次测量得到的数据。
602、基于增量型旋转编码器的航迹推算公式如下：
d L i = 2 π n L i m R - - - ( 12 ) ; d R i = 2 π n R i m R - - - ( 13 ) ; ]]>Δdi＝|dLi-dRi|(14)；
θ i b = arc c o s 2 r 2 - Δd i 2 r 2 - - - ( 15 ) ; ]]>Sib＝0.5(dLi+dRi)(16)；
ΔXib＝Sibcosθib(17)；ΔYib＝Sibsinθib(18)；
(Xb,Yb)＝(Xib-1+ΔXib,Yib-1+ΔYib)(19)。
dLi为单位时间内左轮所走过的路程，nLi为左轮侧的增量型旋转编码器单位时间内发出的
脉冲数，m为增量型旋转编码器转一圈所发出的脉冲数，R表示车轮的半径；dRi为单位时间
内右轮所走过的路程，nRi为右轮侧的增量型旋转编码器单位时间内发出的脉冲数；Δdi为单位
时间内左轮和右轮的路程差。
θib表示通过式(17)计算得到的单位时间内机器人所转过的角度，r为左轮和右轮之间的
距离；Sib为单位时间内左轮和右轮的路程平均值；ΔXib表示X坐标的增量，ΔYib表示Y坐标
的增量；(Xb,Yb)表示用增量型旋转编码器得到的机器人当前坐标；参数角标i，表示第i次测
量得到的数据。
603、机器人的精确坐标为：
θ a = Σ i = 1 n θ i a - - - ( 20 ) ; θ b = Σ i = 1 n θ i b - - - ( 21 ) ]]>
j＝θ-θa(22)；k＝θ-θb(23)；
A = k j + k - - - ( 24 ) ; B = j j + k - - - ( 25 ) ]]>
(X,Y)＝(AXa+BXb,AYa+BYb)(26)
A是陀螺仪的置信度值，B是增量型旋转编码器的置信度值；θia是单位时间内用陀螺仪
得到的机器人所转过的角度，θib是单位时间内用增量型旋转编码器得到的机器人所转过的角
度，θa为用陀螺仪得到的当前位置与原点沿X轴的夹角，θb为用增量型旋转编码器得到的当
前位置与原点沿X轴的夹角，j为θa与θ的差值。k为θb与θ的差值。θ为机身上的超声波测
距传感器在能够收到来自基座的超声波信号时，通过超声波经过四个超声波测距传感器的不同
时间，精确的测量出机器人和充电基座与X轴之间的实时夹角。利用这个精确的夹角，来和
陀螺仪与增量型旋转编码器多次叠加所得到的夹角比较，可以得到每个传感器的偏差。当一个
传感器出现错误的时候，其偏差会很大，其置信度就会变得很小，所以本发明可以减少单个传
感器的误差，也增加了传感器系统的容错率；在机身上的超声波测距传感器无法收到来自基座
的超声波信号时，A和B设为最近的一次取值。(X,Y)表示计算得到的机器人精确坐标。
步骤七：在清扫过程中，进行路障扫描，遇到障碍时进入避障程序同时把障碍物所在位置
信息记录进地图数据中。其中遇到障碍物时的避障碍策略分为三种，即墙形障碍策略、小型障
碍策略、大型障碍策略。参见图4，详细说明如下：
当遇到障碍后首先依靠侧翼的红外反射传感器沿着障碍物边沿前进，若障碍物边长小于
25cm则定义为小型障碍，若障碍物边长大于25cm但小于100cm则定义为大型障碍，若障碍
物边长大于100cm则定义为墙型障碍。
墙形障碍策略:首先依靠侧翼的红外反射传感器，在距离墙形障碍1cm处，沿边清扫，直
到遇到明显转角处，此时切出障碍清扫策略，沿原路径返回起始点，继续之前清扫路线。
小型障碍策略:遇到小型障碍时，在距离小型障碍5cm处，环绕小型障碍清扫一圈，继续
沿原清扫路线前进。
大型障碍策略:遇到大型障碍时，在距离大型障碍5cm处清扫一圈，最后在距离大型障碍
物15cm处清扫一圈。在此过程中通过航迹推算法实时获得机器人相对于充电基座的坐标，并
记录最后一圈清扫路径上水平坐标和纵坐标的最大值与最小值。以所记录得到的水平坐标的最
大值和最小值的平均值，作为中心点的水平坐标。以所记录得到的纵坐标的最大值和最小值的
平均值，作为中心点的纵坐标。在得到障碍物的中心点后，过中心点生成一条平行于X轴的
线段和一条平行于Y轴的线段。这两条线段将除去大型障碍物的未清扫区域划分为四个区块。
再以这四个区块为清扫的单元域，逐个执行整齐的S型清扫。
在清扫同时进行红外扫描并对红外近距传感器传回数据检测，检测车体是否离开地面。若
检测到车体离开地面，则停止所有功能进入待机状态。
步骤八：判断是否打扫完全部可清洁栅格，若清扫完成则进入步骤九；若没有结束，返回
步骤三。
步骤九：结束，沿规划路径返回充电基座的超声波信号范围内，直至回到充电基座。
本发明机器人能够凭借着多传感器信息融合技术，自动记录环境地图并实时判断现处的位
置，并基于先进路径规划技术，利用栅格地图与基于航迹的室内定位算法，将人机交互和传感
器的扫描检测部分放在主函数的循环里，而将用于判断是否能运行放在中断里。设计出最优清
洁路径。