实现智能型吸尘器自动归位功能　MCU动态坐标精确计算

市面上已有居家小帮手吸尘器相关产品，其中较高等级产品具有自动归位充电功能，也就是当小帮手吸尘器工作至电量不足时，会自动寻找原点的充电位置以完成自动归位与充电功能，如此对于居家生活有实质帮助。主人可于外出或上班前设定小帮手吸尘器工作模式，即可自动完成居家的吸尘工作并归位完成自动充电，该功能对于忙碌现代人而言帮助非常大，故有其市场价值。

然市场上号称具有自动归位功能的小帮手吸尘器，深入了解为利用靠右墙壁，以循壁方式逆时针寻找直到回到原位为止，但该法有使用盲点，若小帮手吸尘器开始要归位时刚好在原位的右边墙壁，则小帮手吸尘器须绕一大圈才能回到原位，若运气不好，可能因距离太远而在电量耗尽前还无法找到原位。另一个缺点为若小帮手吸尘器开始要归位时，刚好遇见如箱柜四面均靠墙，则小帮手吸尘器会绕着该障碍物箱柜顺时针旋转，永远无法找到原位。

居家小帮手若用于室内，应不能使用全球卫星定位系统(GPS)为定位功能，因为室内环境无法定位，须于室外见天环境才可定位。另也有考虑用无线射频辨识系统(RFID)定位，虽然可达到精确定位，但须于使用时对居家环境作RFID标签设定，对用户来说并不实用。若使用红外线定位亦是可行方法，但遇到红外线被环境地物遮蔽时，小帮手吸尘器则无法顺利归位。

基于上述说明，本文拟提出坐标定位法记录小帮手吸尘器的动态坐标，包含X、Y轴坐标与小帮手吸尘器的方向角，据此即可达成最短距离自动归位和提高归位成功率。

动态坐标计算方法介绍

小帮手吸尘器除具自动吸尘功能外，若要方便用户使用，最重要具备的功能为当小帮手吸尘器电量不足须充电时，要能自动归位充电，且归位动作须具有快速与准确特性，且考虑商品化须有低成本优点。本文即以此为重点聚焦于小帮手吸尘器自动归位功能的动态坐标法则设计。

图1为智能型居家小帮手吸尘器工作示意图，小帮手吸尘器由原点坐标(0,0)出发，小帮手吸尘器的正前方定义为正X轴方向，及初始时小帮手吸尘器方向角为0度，接着依小帮手吸尘器工作模式(不在本文讨论范围)先以θ1角度前进至P1点，接着转至θ2角度前进至P2点，再转θ3角度前进至P3点，最后到达Pn点，假设到达Pn点后小帮手吸尘器电量呈现不足，须直接由Pn点回到原点充电。

图1 智能型居家小帮手吸尘器工作示意图

要由Pn点直接往原点坐标(0,0)归位，小帮手吸尘器须计算其行走至每一点之坐标值及其方向角，才能正确判断原点方向。当然在小帮手吸尘器归位路径上也许会有障碍物阻挡小帮手吸尘器前进，此时须设计避开障碍物功能，此情形并不在本文讨论范围。

图1中定义原点坐标为(0,0)，正X轴方向为小帮手吸尘器正前方，假设点P1、P2、P3、…Pi、…Pn的坐标分别为(X1,Y1)、(X2,Y2)、(X3,Y3)、…(Xi,Yi)、…与(Xn,Yn)，而小帮手吸尘器方向角转动角度为θ1、θ2、θ3、…θi、…与θn，由原点至P1点距离为Z1、由P1点至P2点距离为Z2、由P2点至P3点距离为Z3以此类推，其须计算每一点坐标值与方位角度。以图1为例，假设矩阵Tyi(y,d)、Tzi(z,θ)与Txi(x,a)功能为将行走一段距离转换为原坐标，矩阵Tyi(y,d)处理Y轴距离，矩阵Tzi(z,θ)处理坐标旋转角度θi，与矩阵Txi(x,a)处理X轴距离，定义矩阵Tyi(y,d)、Tzi(z,θ)与Txi(x,a)如方程式(1)～(3)所示。

则Pi点对应至原坐标系统坐标值如(4)式所示。坐标(Pxi,Pyi)即为Pi点对应至原坐标系统之坐标值。

[Pix Piy 0 1]T=Tyi(y,d)Tzi(z,θ)Txi(x,a)[0 0 0 1]T ……………………………(4)

小帮手吸尘器坐标改变可归纳三种模式，直线行走、原地旋转与行走转弯模式。直线行走只改变小帮手吸尘器坐标位置并不影响其方向角，原地旋转只改变小帮手吸尘器方向角并不影响其坐标位置，行走转弯同时会改变小帮手吸尘器坐标位置与其方向角，此三种模式改变小帮手吸尘器坐标位置与其方向角其计算方法如下。   

采直线行走模式计算坐标位置  

图2为直线行走模式坐标位置计算方法示意图，小帮手吸尘器具有左右轮，其左右轮之间距离为Dw，左右轮之半径为Rw，因为直线行走模式故左右轮转动方式与速度相同，此模式可使小帮手吸尘器前进或后退，只要计算出小帮手吸尘器前进或后退之距离带入(1)～(4)式即可求出新坐标，而小帮手吸尘器前进或后退距离可由式(5)求出。

图2 直线行走模式坐标位置计算方法示意图

Zi=2π*Rw*Cw ………………………………(5)

其中Cw为轮子转动圈数。

藉原地旋转模式计算方向角

若小帮手吸尘器原地旋转及左右轮以相同速度但方向相反转动，则其旋转角度计算方法如式(6)所示。

θi=(2π*Rw*Cw/(Dw*π))*360=(2Rw*Cw/Dw)*360………………………………(6)

Cw为小帮手左右轮以相同速度但方向相反转动之转动圈数。式(6)角度值以度数(Degree)表示。

行走转弯模式坐标位置/方向角计算解析

图3为行走转弯模式坐标位置与其方向角计算方法示意图，若左轮与右轮均个别以等速前进，且右轮速度高于左轮，则小帮手会往左转弯如图3所示，此时不但坐标位置改变，其方向角亦随之改变。

图3 行走转弯模式坐标位置/方向角计算方法示意图

计算小帮手吸尘器之旋转角度如式(7)所示。

θ=360*( Rw*Cwr)/(Dw+Dw/(Cwr/Cwl-1))*2π……………………………………(7)

其中Cwr为右轮转动的圈数，Cwl为左轮转动的圈数。而小帮手新坐标计算方法为：

Ｒc=Dw/[(Cwr/Cwl)-1]………………………(8)

其中Ｒc为小帮手转弯内半径，则新坐标为

x=[2π*Rw*(Cwr+Cwl)/2*cos(θ)……………(9)

y=[2π*Rw*(Cwr+Cwl)/2*sin(θ)…………(10)

计算小帮手吸尘器坐标之方法步骤依序为判断小帮手吸尘器工作模式；若为直线前进或后退模式时，计算前进或后退之行走距离，利用坐标转换计算新坐标；若为原地旋转模式时，计算小帮手之新方向角；若为前进或后退转弯模式时，计算小帮手的新方向角及前进或后退的行走距离；若须归位回到原点时，计算最短距离方向以规画为行进方向，若遇障碍物时，则以左弯或右弯以避开障碍物回到原点。

硬件架构面面观

图4为智能型居家小帮手核心控制芯片电路图，本系统以HT46R24微控制器(MCU)为控制核心，因HT46R24具有高性能与低价位优势，且稳定度佳具工业规格认证。经由MCU计算与分析左右轮行走距离，可得到小帮手吸尘器坐标位置与其方向角，虽然误差会造成归位误差，但程序设计小帮手吸尘器回到原位附近时，若无法实时找到Home的位置，可设计于附近寻找Home的位置，此时可加装传感器以确认Home确实位置。

{$page$}

图4 智能型居家小帮手核心控制芯片电路图

图5为智能型居家小帮手左右轮马达控制电路图，采单独控制方式并加装传感器感以感测左右轮个别转动圈数。小帮手吸尘器由左右两颗马达驱动，依驱动马达正反转即可控制小帮手前进、后退、左弯与右弯，又依其马达正反转、速度高低即可轻易控制小帮手运动行为。经实际测试确实可达上述功能，若将小帮手设计为圆形且左右驱动马达置于其直径上时，小帮手即可原地左右转。

图5 智能型居家小帮手左右轮马达控制电路图

应用动态坐标计算法　小帮手吸尘器渐成主流

图6为智能型居家小帮手实际实验行走路线图，系统启动后MCU开始计算由编码器计算马达转动角度，再使用齐次坐标(Homogeneous Coordinate)计算出智能型居家小帮手行走的路线距离与角度，并计算其坐标位置。智能型居家小帮手由A原点出发，18.45度直行3.16公尺至B点，接着转向52度执行至C点，再由C点至D点、E点、F点、G点，至G点后，若智能型居家小帮手接收到归位命令，MCU会计算归位原点方向角与距离，计算核心会先控制智能型居家小帮手转至面对原点的方向，接着控制直行归位至原点的距离，如此智能型居家小帮手即可成功归位。

图6 智能型居家小帮手实际行走路线图

图7为智能型居家小帮手动态坐标定位系统计算的程序流程图，系统启动后，MCU开始实时计算由编码器送来的讯号，以计算出智能型居家小帮手当下的坐标位置。当归位模式开始执行后，MCU计算原点方向与归位时智能型居家小帮手须转动的角度，接着控制马达并由编码器量测是否已转到归位方向的角度，当方向正确后，再直线前进以达成归位功能，归位途中若遇到障碍物，将启动更换路线模式以寻求另一归位路径。

图7 智能型居家小帮手归位程序流程图

图8为智能型居家小帮手实验用实体照相图，智能型居家小帮手具有五个超音波测距模块用来量测障碍物的位置与距离。前方设有防碰撞机构用来确保智能型居家小帮手吸尘时不会因碰撞而受损，而底部有红外线传感器，用来保护小帮手不会因掉落而损坏，智能型居家小帮手还有一组射频(RF)遥控器，可以方便使用者选择自己想要的工作模式。

图8 智能型居家小帮手实验用实体照相图

本文以HT46R24芯片为核心，利用动态坐标法计算小帮手吸尘器的实时坐标位置与其方向角，并利用此坐标位置与其方向角数据计算小帮手吸尘器归位的最佳路径与方向。相信本文所提方法具有高可靠度、低成本与具有市场性的居家特色，非常适合用于小帮手吸尘器自动归位充电用，且适合任何场所应用，相信应用动态坐标计算法可使小帮手吸尘器逐渐成为吸尘器市场主流。