开源|快速入门和理解并模拟实现GPS户外机器人的定位与导航

户外机器人的定位导航相对于需要建图的场景来说，是比较简单容易实现的，因为可以借助第三方地图完成定位，并在第三方地图中完成路径规划和下发航点等操作，实现的难题在于如何控制机器人完成步行和转弯。

这些在不引进RTK高精度定位的前提下而言。由于存在干扰，以及传感器的精度可能不高，实际操作中，并不一定能够按期待的动作实现定位和导航。

笔者简单的搭了一个模拟机器人的程序，意在实现地图上的定位和快速导航，这样能快速理解其中的逻辑和一些简单的算法。

先看一下效果：
https://mp.weixin.qq.com/s/c783rHvsMxiH9a6IDtMCNA

废话不多说直接上程序，主要的处理逻辑如下：
while(GLOBAL_SWITCH)
{
if(onceread ==0)
{
onceread =1;
if(is_file_exist(“waypoints.csv”)!=0)//判断航点的文件是否存在
{
GLOBAL_STATUS=STOP_STATUS;
GLOBAL_SWITCH =0 ;
onceread =0;
DEBUG(LOG_ERR,“waypoint file is not exist \n”);
break;
}
}
if((latitude ==0.0)&&(longitude == 0.0))//是否定位成功
{
DEBUG(LOG_ERR,“GPS CANNOT LOCATION PLEASE CHECK \n”);
GLOBAL_STATUS = STOP_STATUS;
GLOBAL_SWITCH = 0;
break;

    }if ((getmillis() - lastSubMillis > SUBSUMPTION_INTERVAL))//定时处理{switch(GLOBAL_STATUS){case STANDBY_STATUS://启动后的初始状态DEBUG(LOG_DEBUG,"STANDBY STATUS \n");lastGPSMillis =0 ;ReadWaypointsFile();//读取航点到队列CalculateHeadingToWaypoint();//计算当前角度和航点的方向CalculateDistanceToWaypoint();//计算当前和航点的距离GLOBAL_STATUS = ROTATE_STATUS ;break;case CACULATE_STATUS://计算状态DEBUG(LOG_DEBUG,"CACULATE STATUS \n");CalculateHeadingToWaypoint();CalculateDistanceToWaypoint();GLOBAL_STATUS = ROTATE_STATUS ;break;case ROTATE_STATUS ://旋转状态DEBUG(LOG_DEBUG,"ROTATE_STATUS   \n");if (abs((int)car_heading_simulation - (int)targetHeading) > 10){RotateDegrees(targetHeading);//这里需要根据求出的角度进行转动，需要旋转的角度位 当前航向角-两点之间的角度，需要做-180~180之间的归一化}else {GLOBAL_STATUS = MOVE_STATUS ;}break;case MOVE_STATUS ://前进状态  DEBUG(LOG_DEBUG,"MOVE_STATUS   \n");if((waypointRange > 200))//大于100m 认为不合法 所以规划路径时需要注意{  DEBUG(LOG_ERR,"distance > 200m \n");break;}  MoveDistance(waypointRange);break;    case WAYPOINTARRIVE_STATUS://读取下一个航点DEBUG(LOG_DEBUG,"WAYPOINTARRIVE_STATUS,currentWaypoint:%d,waypointCount:%d\n",currentWaypoint,waypointCount);if(currentWaypoint < waypointCount-1 ){currentWaypoint ++;DEBUG(LOG_DEBUG,"currentWaypoint ++\n");GLOBAL_STATUS = CACULATE_STATUS ;}else  if(currentWaypoint >= waypointCount-1 ){GLOBAL_STATUS = STOP_STATUS ;DEBUG(LOG_ERR,"currentWaypoint >= waypointCount stop status\n");}break;case STOP_STATUS :break;case MANUAL_STATUS :break;default :break;}//end switch// 必须先运行一次 standby 状态 只有当在目的地附近3米内才会转换状态ret =  isInRange(3, latitude , longitude, waypoints[currentWaypoint].latitude, waypoints[currentWaypoint].longitude);if (ret == 1) //点在圆圈内{   DEBUG(LOG_DEBUG,"arrive into circle scale ,will read next point  \n");GLOBAL_STATUS = WAYPOINTARRIVE_STATUS ;}waypointlongitude =  waypoints[currentWaypoint].longitude;waypointlatitude = waypoints[currentWaypoint].latitude;CalculateHeadingToWaypoint();//计算两点之间的角度int b_heading = abs((int)car_heading_simulation - (int)targetHeading);//获取当前航向和两点之间的差值DEBUG(LOG_DEBUG,"caculate b-heading:%d  ，ret=%d \n",b_heading,ret);if ((b_heading > 10 )&&(ret != 1)  ){GLOBAL_STATUS = ROTATE_STATUS ;}lastSubMillis = getmillis();}//end  sub loop}//end while switch on

}

关于实现整套系统，需要搭建一个web服务器，然后使用MQTT协议实现通信，嵌入式中使用BOA来实现简单的web并包含地图的代码。

模拟程序依赖libmosquitto. 需要安装

sudo apt-get install mosquitto-dev

sudo apt-get install mosquitto-client

sudo find / -name “libmosquitto*.so”

找到后，将libmosquitto.so 替换MQtt文件下lib的libmosquitto.so库。

模拟程序使用c开发，直接 make就可以使用。

BOA搭建些许麻烦，可以使用 www.woyilian.com 中的仿真界面实现。如下图：

该工程的源码在这里下载：
请在链接末尾处下载。