开源|快速入门和理解并模拟实现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 中的仿真界面实现。如下图：

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