机器人车技术开发分享网
根据两点坐标获取航向角和距离的c程序
  • 首页 > 智能小车
  • 作者:lide2017
  • 2019年1月22日 15:10 星期二
  • 浏览:2213
  • 字号:
  • 评论:0
  • #include <stdio.h>
    #include <math.h>
    
    
    #define PI 3.1415926
    #define EARTH_RADIUS 6378.137 //地球近似半径
    
    
    double radian(double d);
    double get_distance(double lat1, double lng1, double lat2, double lng2);
    
    
    // 求弧度
    double radian(double d)
    {
     return d * PI / 180.0; //角度1˚ = π / 180
    }
    
    
    
    
    //计算距离
    double get_distance(double lat1, double lng1, double lat2, double lng2)
    {
     double radLat1 = radian(lat1);
     double radLat2 = radian(lat2);
     double a = radLat1 - radLat2;
     double b = radian(lng1) - radian(lng2);
    
    
     double dst = 2 * asin((sqrt(pow(sin(a / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(b / 2), 2) )));
    
    
     dst = dst * EARTH_RADIUS;
     dst= round(dst * 10000) / 10000;
     return dst;
    }
    int get_bearing(double lat1, double lng1, double lat2, double lng2)
    {
     int distance_North = EARTH_RADIUS*(lat2-lat1);
     int distance_East = EARTH_RADIUS*cos(lat1)*(lng2-lng1);
     // int bearing = modf( atan2(distance_East, distance_North), 2*PI );
    
    
     // return bearing;
    
    
    }
    double toRadian(double point)
    {
     return (point*M_PI)/180.0;
    }
    
    
    double toDegree(double point)
    {
     return point*180.0/ M_PI;
    }
    
    
    int toBearing(double angle)
    {
    
    
     double bearing = toDegree(angle)+ 360.0;
     return fmod(bearing, 360);
    }
    
    
    /*
    
    
     This function will determine the bearing of the path to be travelled
    
    
    */
    
    
    double getBearing(double longtitude_start, double latitude_start, double longtitude_end, double latitude_end)
    {
     double rad_latitude_start = toRadian(latitude_start);
    
    
     double rad_latitude_end = toRadian(latitude_end);
    
    
     double deltaLongtitude = toRadian(longtitude_end)- toRadian(longtitude_start);
    
    
     double x = sin(deltaLongtitude)*cos(rad_latitude_end);
    
    
     double y = cos(rad_latitude_start)*sin(rad_latitude_end)-sin(rad_latitude_start)*cos(rad_latitude_end)*cos(deltaLongtitude);
    
    
     return toBearing(atan2(x, y));
    
    
    }
    double getAngle1(double lat_a, double lng_a, double lat_b, double lng_b)
    {
    
    
     double y = sin(lng_b-lng_a) * cos(lat_b);
     double x = cos(lat_a)*sin(lat_b) - sin(lat_a)* cos(lat_b)* cos(lng_b-lng_a);
     double bearing = atan2(y, x);
    
    
     // bearing = Math.toDegrees(bearing);
     if(bearing < 0){
     bearing = bearing +360;
     }
     return bearing;
    
    
    }
    //heading :车的航向角相对于真北方向
    // bearing: 目的地相对于真北方向的方位角
    //返回:最优的转弯方案:转弯方向,转弯角度,行驶距离
    int HeadingAnalysis(int Heading,int Bearing)
    {
     int headingtmp;
     if((Heading - Bearing)< 0)
     {
    
    
     headingtmp = Heading +360;
     if(headingtmp - Bearing <= 180)
     {
     //左转
     printf("turn left %d\n",headingtmp - Bearing);
     }else if(headingtmp - Bearing > 180){
     printf("turn right %d\n",360- (headingtmp-Bearing));
    
    
     }
     }else if((Heading - Bearing) > 0)
     {
     if(Heading - Bearing < 180)
     {
     printf("turn left %d\n",Heading - Bearing);
     }
     else if(Heading - Bearing > 180)
     {
     printf("turn right %d \n",360-(Heading-Bearing));
     }
     }
    
    
    
    
    }
    int main (int argc, const char * argv[])
    {
    
    
     double lat1 = 39.90759;
     double lng1 = 116.41552;//经度,纬度1
     double lat2 = 39.93804;
     double lng2 = 116.41371;//经度,纬度2
     // insert code here...
     int angle;
     double dst = get_distance(lat1, lng1, lat2, lng2);
    
    
     angle= getBearing( lng1,lat1, lng2 ,lat2);
     printf("dst = %0.3fkm\n", dst); //dst = 9.281km
     // printf("angle = %d 度\n", angle);
     HeadingAnalysis(70,150);
     return 0;

    }


    该程序已经在Linux下验证通过可行,该程序和算法将最终用到GPS定位小车上。

    2AC8DBED-54D9-421b-9367-FDE15C61AEA6.png

    
    
    		
      您阅读这篇文章共花了:  
    二维码加载中...
    本文作者:lide2017      文章标题: 根据两点坐标获取航向角和距离的c程序
    本文地址:http://blog.cvosrobot.com/?post=415
    版权声明:若无注明,本文皆为“机器人车技术开发分享网”原创,转载请保留文章出处。

    返回顶部| 首页| 手气不错| 捐赠支持| 自定义链接| 自定义链接| 自定义链接| 手机版本|后花园

    Copyright © 2014-2017 机器人车技术开发分享网   京ICP备14059411 Copyright 2014-2019 小v工作室 版权所有 All Rights Reserved

    sitemap