CN113515114A - 导航避障小车*** - Google Patents

Abstract

本发明提供一种导航避障小车***，包括车体、单片机、光流模块、测距模块、陀螺仪、WIFI模块、电机驱动模块、电源、安卓应用、上位机。其特征在于，所述测距模块由两个左右对称，向中旋转一定角度放置的超声波模块、放置超声波模块的平台、两个与超声波模块连接的舵机和一个与平台连接的舵机组成；所述安卓应用通过调用手机Context.WIFI_SERVICE***服务获取RSSI信息，发送给上位机进行建图；上位机采用WIFI指纹算法融合对天光流Lucas‑Kanade算法所得相对位移对小车进行室内定位，融合BUG2算法和A*算法进行动态路径规划，控制小车导航与避障。

Description

导航避障小车***

技术领域

本发明涉及智能机器人技术领域，具体涉及超声波避障装置、避障算法、WIFI指纹室内定位。

背景技术

小车在运动过程中，需要对障碍物进行侦测，否则将无法触发后续的避开障碍物等操作，存在风险。主要用于小车侦测障碍物的技术有很多，如激光雷达、超声波、深度相机等，超声波传感器是成本较低的方案。

现有的采用超声波侦测障碍物的方案有二，其一，小车的运动方向上设置一个超声波模块，超声波模块发出超声波，在接触障碍物后发生反射，并被模块接收。在波速已知的情况下，根据回波的时间，便可以算出小车与障碍物的距离。由于超声波模块的测量角度约15度，若采用该方案，小车将无法对测量角度外，但会干扰小车工作的障碍物做出反应，造成不良后果。

其二，采取大量超声波模块以覆盖所有角度，可实现对所有方向距离的侦测，但功耗较大，且导致***工作延时提高，不利于在动态环境中的工作。

Bug2算法是是常用的机器人避障算法。该算法的机制为：在当前位置与目标位置之间画一条直线，小车从左侧或右侧绕行障碍，当小车运动到直线上任意一点时，则认为避障完成。Bug2算法对小型障碍有较好的通过性，但对大型障碍，及在复杂环境中的处理均表现不佳。

无线接入点（access point，下称AP）发射信号时，电磁波内会包含许多信息，其中配置信息主要有4个，SSID（描述AP名称），BSSID（可以理解成AP的MAC地址），NETWORKID（AP数字ID），RSSI（官方称为level，描述WIFI信号强弱的值，是定位的关键信息）。

WIFI定位是应用广泛的室内定位技术，其主要算法有二，三角形与指纹位置识别。三角形算法即根据小车所接受到的来自AP的信号强度（RSSI），判断与AP的距离。当AP在三个以上时，小车可获知自身大致位置。指纹位置识别算法即小车读取AP的信号强度，并存入数据库，作为该位置的指纹，然后前往下一位置，直到该区域所有位置的指纹都存入数据库。工作时，小车读取AP的信号强度，与数据库中的指纹对比，从而获知当前位置。

对于这两种算法，AP越多，误差越小。三角形算法需要获取AP的位置，而指纹位置识别不需要，所以后者是更好的选择，但其平均误差也有1-2m，且需要投入大量的时间用于指纹匹配，不利于在动态环境中的工作。

发明内容

本发明提供一种导航避障小车***。可工作于用于超市、仓库等室内环境，具有室内定位、导航、避障等功能。优化传统超声波测距方案，超声波侦测障碍的范围更广；对障碍物分类，避障路径更短；融合WIFI定位和对天光流，室内定位更精准，速度更快。

本发明提供的导航避障小车***，包括。

车体，包括底板，顶板以及设置于底座侧面的麦克纳姆轮。

电源模块，设置于底板尾部，与单片机相连，用于供电。

单片机，设置于底板中部，与各传感器及WIFI模块相连，根据各传感器数据以及上位机指令驱动小车移动。

WIFI模块，设置于顶板尾部，与单片机相连，用于单片机与上位机间的通信。

测距模块，设置于顶板头部，与单片机相连，用于获取障碍物和车体间的距离。由两个左右对称，向中旋转一定角度放置的超声波模块，放置超声波模块的平台，两个与超声波模块连接的舵机和一个与平台连接的舵机组成，舵机可控制与其连接的物体旋转。

电机驱动模块，设置于底板头部，与单片机和电机相连，接收来自单片机的PWM信号，控制电机转速。

电机，设置于底板两侧，共四个，带动麦克纳姆轮，实现全向移动。

光流模块，设置于顶板尾部模块的摄像头朝向上方，与单片机相连，用于获取小车的位移；

陀螺仪，设置于顶板中部，与单片机相连，用于获取小车的加速度。

安卓应用，与上位机通过路由通信，用于建图，以及向上位机发送小车的期望位置。

上位机，与小车和安卓通过路由通信，存放场景地图，根据小车当前位置和来自安卓应用的期望位置进行路径规划，获得路径，并发送给小车。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。

图1为本发明导航避障小车***中小车整车结构示意图。

图2为本发明导航避障小车***中小车顶板结构示意图。

图3为本发明导航避障小车***中小车底板结构示意图。

图4 为本发明导航避障小车***中小车各部分连接图。

图5为本发明导航避障小车***中小车的测距模块示意图。

图6为本发明导航避障小车***中的测距方案与一种现有测距方案的比较示意图。

图7为本发明导航避障小车***中小车避障（导航）部分的tag模块程序流程图。

图8为本发明导航避障小车***中小车避障（导航）部分的mode模块程序流程图。

图9为本发明导航避障小车***中小车避障（导航）部分的move模块程序流程图。

图10为本发明导航避障小车***中小车基于bug2算法进行避障的过程示意图。

图11为本发明导航避障小车***中小车的优化避障方案和原方案比较示意图。

图12为本发明导航避障小车***中使用安卓应用进行建图的流程图。

图13为本发明导航避障小车***中室内定位流程图。

图14为本发明导航避障小车***中路径规划流程图。

具体实施方式

下面将结合本发明实施例中，对本发明实施例中的技术方案进行清楚、完整地描述。

显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

优化的，所述小车组成及连接方式见图1、图2、图3、图4。

测距模块1，设置于顶板5头部，与单片机7相连，用于侦测障碍物。

WIFI模块2，设置于顶板5尾部，与单片机7相连，用于单片机与上位机的交互。

光流模块3，设置于顶板5尾部，与单片机7相连，用于获取小车位移。

陀螺仪4，设置于顶板中部，与单片机7相连，用于获取小车加速度。

顶板5，用于放置测距模块1、WIFI模块2、光流模块3、陀螺仪4。

电源6，设置于底板尾部，与单片机7及电机驱动模块8相连，用于供电。

单片机7，设置于底板中部，与各传感器及WIFI模块2相连，根据各传感器数据以及上位机指令驱动小车移动。

电机驱动模块8，设置于底板头部，与单片机7和电机9相连，接收来自单片机7的PWM信号，控制电机转速。

电机9，设置于底板两侧，共四个，带动麦克纳姆轮10，实现全向移动。

麦克纳姆轮10，与电机9相连，用于移动。

底板11，用于放置电源6，单片机7，电机驱动模块8，电机9。

铜柱12，用于连接底板11与顶板5。

优化的，所述测距模块如图5，包括。

超声波模块1-1，用于测量小车与障碍物之间的距离。

平台1-2，用于放置超声波模块。

舵机1-3，用于控制转向。两侧的舵机可控制超声波模块转向，中央的舵机可控制平台转向。

所述测距模块中，两侧的舵机向中偏转。以运动方向为y轴，设车体的最大长度为x，用户期望的安全距离为y，则左侧舵机偏转角度为arctan（y/x）,右侧舵机偏转角度为arctan（180-y/x）。在运动方向改变时，中部的舵机控制平台转向，从而实现全方位侦测障碍物。

图6所示，所述测距方案对于车体运动方向上侧面的障碍物有更好的侦测效果，图左为本发明中的测距方案，图右为一个现有方案。图左，超声波模块1的测量范围2覆盖了位于车体3范围内的障碍物4，图右超声波模块1的测量范围2未能覆盖了位于车体3范围内的障碍物4。

所述测距方案较传统方案的优势在于：减少视野盲区，可对运动方向上侧面的障碍进行侦测；可通过超声波模块测得距离值确定障碍物二维坐标，便于接下来的避障操作。缺陷在于：在运动方向上，中部的量程较小。

在上述实施例中，避障程序分为tag模块、mode模块和move模块。

Tag模块用于粗略判断障碍，见图7。

Tag=0表示无障碍物，tag=1表示一侧有障碍物，tag=2表示两侧都有障碍物。为防止测距模块对正前方障碍物的量程小所带来的错误，当两侧超声波测距都小于安全距离时，令小车进行横移，重复测量一次。

Mode模块根据tag值，获取工作状态，见图8。

Mode=0表示可正常通行，mode=1表示可从障碍右侧通行，mode=2表示可从障碍左侧通行，mode=3表示未能确定通行方向。

对tag=1，***判定为存在可绕行路线，将在2s内不断重复测距，若在2s内障碍消失，则判定为可正常通行，减少了小车额外移动；若障碍持续存在，则判定为需要避障。

Move模块根据mode值，控制电机工作，见图9。

上述避障程序，目的为避开地图中没有的，临时出现的障碍物。以超市环境为例，路过的顾客或者临时安置的货物属于临时出现的障碍物，货架等无法通过的区域属于地图中已知障碍，在路径规划中已经避开。

对侧方，本发明基于Bug2算法进行避障。OA_right()和OA_left为绕行障碍的函数。由于本发明的测距装置的独特设计，可控制两超声波模块指向任意方向。以左侧绕行为例，绕行过程见图10。

测距模块利用舵机，使得两个超声波模块1分别指向运动方向和障碍4方向.小车的移动方向与速度由两超声波模块所测距离算出，故对曲面障碍也可采用此方案。

见图11，由于bug2算法的缺陷，对于未能确定通行方向的障碍（mode=3），小车在由起点5前往终点6的过程中，由于出现障碍物4，可能会绕开地图中的已知障碍，导致移动路径7较远。该情况下，小车将障碍的二维坐标通过路由上传，上位机进行路径规划后将路径8发给小车，小车根据路径进行移动，达到避障的目的。

所述实施例通过安卓应用进行建图，原理为在开启手机中的WIFI_STATE权限，然后在主活动的java代码中获取Context.WIFI_SERVICE的***服务，再调用getScanResult方法返回一个包含类ScanResult的List容器。这个类中包含了附近可扫描到的wifi的各种信息。

见图12，安卓应用工作建图流程如下。

设场景中有三个AP，记为AP1，AP2和AP3。当收到来自上位机的建图指令时（该指令由标志位和一个二维坐标（x，y）组成），重新扫描当前点附近的wifi信号，再次返回ScanResult，在与先前所选择的AP名称验证后，获取选定的所有wifi的RSSI值。这个扫描时间会持续30s，每秒获取一次RSSI并存入LIST容器中，在30秒扫描结束后，对这30个值取平均值，获得该点的wifi指纹（SS1，SS2，SS3）。

指纹获取结束后，可设定该位置的通过难度。通过难度设定为一个整数，从0~9。0表示完全无障碍，可以通过。9表示完全无法通过。0和9之间的整数表示可通行，但具有不同的通过难度。实际工作环境中，难度较大的对应人流密集的区域，难度较小的对应人流稀少的区域。

当前点的所有工作完成后，将位置指纹，位置坐标，通过难度集合成一数组，通过路由发送给上位机，然后进行下一个位置点的建立，直到所有位置都已存储在地图中，建图完成。

所述实施例采用wifi指纹结合Lucas-Kanade光流算法进行室内定位，具体流程见图13。

设场景中有三个AP，记为AP1，AP2和AP3。在工作状态下，上位机每隔一段时间向小车发送获取RSSI的请求，在接受到上位机的请求后，单片机读取三个AP的RSSI值，记为数组（SS1，SS2，SS3），多次读取取平均值。光流模块利用Lucas-Kanade光流算法，将位置偏移（offset）发送给单片机，单片机将偏移加入数组，即（SS1，SS2，SS3，OS），最后加入状态位，设置导航模式为状态2，即（SS1，SS2，SS3，OS，2）然后利用WiFi模块，将该组数据发送给上位机，小车和上位机间通过路由交互。

上位机存放场景地图，应用mysql数据库进行存储。地图中每一点具有的信息为：RSSI指纹信息、二维坐标（x，y）、通过难度。上位机接受到来自小车的数组（SS1，SS2，SS3，OS，2）后，首先判断上一周期位置是否为空，若不为空，则利用OS值计算出一个坐标A（x1，x2），然后将（SS1，SS2，SS3）与以A为中点1-2m范围内所有点的RSSI指纹信息作比较，采用加权选出间距最小的点B（x2，x3），然后利用A（x1，y1）与B（x2，y2）计算出当前位置C（x3，y3），计算方法为：C=（K*A+B）/（K+1）。

式中K为比例系数，K越大表示越信任光流所得的位置，K越小表示越信任wifi指纹所得位置。由于光照条件、信号强度的不同，小车在不同的环境下工作，可能位置A更为精确，可能位置B更为精确，可在上位机更改K值进行校准。

若上一周期位置为空，表明当前周期为***的第一个周期，或存在数据丢包，则将（SS1，SS2，SS3）与地图中所有的点对比，选出间距最小的点定义为当前位置C。

所述实施例采用A*算法进行路径规划，具体流程见图14。

计算出当前位置C后，由于状态位为2，确定为导航模式。在导航中定义两个位置，最终期望位置EXP与短期期望位置EXP0。EXP可从手机端获取，也可在上位机设置。

首先判断EXP是否已设置，若EXP为空，则向小车发出待机指令；若EXP不为空，判断C与EXP间距是否可接受。

若可接受，则判定小车任务完成，向小车发出待机指令；若C与EXP间距不可接受，判断EXP0是否已设置。

若EXP0为空，则利用A*算法计算得到最优路径点集，一个以EXP为末项的二阶数组[(x,y)]。上位机将该数组的第一组坐标发送给小车，并将该坐标设置为短期期望位置EXP0；若EXP0不为空，则判断C与EXP0的间距是否可接受，若不可接受，则重新利用A*算法获取EXP0.若C与EXP0的间距可接受，则更新EXP0为数组[(x,y)]的第二项，然后删除数组第一项，将剩余项前移一位。

最后，上位机将EXP0发送给小车，小车根据接收到的数据进行移动，导航部分完成。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (10)

1.一种导航避障小车***，其特征在于，包括：

车体，包括底板，顶板以及设置于底座侧面的麦克纳姆轮；

单片机，设置于底板中部，与各传感器相连，根据各传感器数据以及上位机指令驱动小车移动；

电机驱动模块，设置于底板头部，与单片机和电机相连，接收来自单片机的PWM信号，控制电机转速；

电机，设置于底板两侧，共四个，带动麦克纳姆轮，实现全向移动；

陀螺仪，设置于顶板中部，与单片机相连，用于获取小车的加速度；

测距模块，设置于顶板头部，与单片机相连，用于获取障碍物和车体间的距离；

电源模块，设置于底板尾部，与单片机相连，用于供电。

2.如权利要求1所述的导航避障小车***，其特征在于，所述测距模块由两个左右对称，向中旋转一定角度放置的超声波模块、放置超声波模块的平台、两个与超声波模块连接的舵机以及一个与平台连接的舵机组成。

3.如权利要求1所述的导航避障小车***，其特征在于，还包括：

WIFI模块，设置于顶板尾部，与单片机相连，用于单片机与上位机间的通信；

光流模块，设置于顶板尾部模块的摄像头朝向上方，与单片机相连，用于获取小车的位移；

安卓应用，与上位机通过路由通信，用于建图，以及向上位机发送小车的期望位置；

上位机，与小车和安卓通过路由通信，存放场景地图，根据小车当前位置和来自安卓应用的期望位置进行路径规划，获得路径，并发送给小车。

4.如权利要求3所述的导航避障小车***，其特征在于，所述上位机存放WIFI指纹地图，地图中每一点具有RSSI指纹、二维坐标和通过难度。

5.如权利要求4所述的导航避障小车***，其特征在于，所述上位机可设定期望位置，上位机基于A*算法进行动态路径规划，实时更新小车的期望位移。

6.如权利要求5所述的导航避障小车***，其特征在于，所述上位机可通过安卓应用获取期望位置。

7.如权利要求3所述的导航避障小车***，其特征在于，所述安卓应用通过CONTEXT.WIFI_SERVICE***服务获取RSSI值，并发送给上位机，建立WIFI指纹地图。

8.如权利要求3所述的导航避障小车***，其特征在于，所述单片机基于bug2算法，控制小车独立进行部分小型障碍物的规避；和上位机进行交互，基于A*算法进行大型障碍物的规避。

9.如权利要求3所述的导航避障小车***，其特征在于，所述上位机根据光流模块所得数据及上一周期小车位置获得光流位置，并将光流位置作为中心进行数据RSSI值匹配，获取WIFI指纹位置。

10.如权利要求3所述的导航避障小车***，其特征在于，所述上位机根据光流位置及WIFI指纹位置加权获得当前位置。

Images