【STM32无人车之GPS导航经纬度坐标计算前视距离和方位角】

是 否 +2 论坛币

k人 参与回答

经管之家送您一份

应届毕业生专属福利!

求职就业群

赵安豆老师微信：zhaoandou666

经管之家联合CDA

送您一个全额奖学金名额~ !

立即领取

感谢您参与论坛问题回答

经管之家送您两个论坛币！

+2 论坛币

通过经纬度数据可以计算出两点之间的方位角。

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     计算从第一个点到第二个点的方位角
// 参数说明     latitude1       第一个点的纬度
// 参数说明     longitude1      第一个点的经度
// 参数说明     latitude2       第二个点的纬度
// 参数说明     longitude2      第二个点的经度
// 返回参数     double          返回方位角（0至360）
// 使用示例     get_two_points_azimuth(latitude1_1, longitude1, latitude2, longitude2);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
double get_two_points_azimuth (double latitude1, double longitude1, double latitude2, double longitude2)
{
    latitude1 = ANGLE_TO_RAD(latitude1);
    latitude2 = ANGLE_TO_RAD(latitude2);
    longitude1 = ANGLE_TO_RAD(longitude1);
    longitude2 = ANGLE_TO_RAD(longitude2);

    double x = sin(longitude2 - longitude1) * cos(latitude2);
    double y = cos(latitude1) * sin(latitude2) - sin(latitude1) * cos(latitude2) * cos(longitude2 - longitude1);
    double angle = RAD_TO_ANGLE(atan2(x, y));
    return angle;  //((angle > 0) ? angle : (angle + 360));将GPS的的方位角限制在[0,360]之间
}

利用相同的经纬度信息，还能够测算两点间的前视距离。

//-------------------------------------------------------------------------------------------------------------------
// 函数简介 计算从第一个点到第二个点的距离
// 参数说明 latitude1 第一个点的纬度
// 参数说明 longitude1 第一个点的经度
// 参数说明 latitude2 第二个点的纬度
// 参数说明 longitude2 第二个点的经度
// 返回参数 double 返回两点距离
// 使用示例 get_two_points_distance(latitude1_1, longitude1, latitude2, longitude2);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
double get_two_points_distance (double latitude1, double longitude1, double latitude2, double longitude2)
{
const double EARTH_RADIUS = 6378137; // 地球半径(单位：m)
double rad_latitude1;
double rad_latitude2;
double rad_longitude1;
double rad_longitude2;
double distance;
double a;
double b;
rad_latitude1 = ANGLE_TO_RAD(latitude1);                                    // 根据角度计算弧度
rad_latitude2 = ANGLE_TO_RAD(latitude2);
rad_longitude1 = ANGLE_TO_RAD(longitude1);
rad_longitude2 = ANGLE_TO_RAD(longitude2);

a = rad_latitude1 - rad_latitude2;
b = rad_longitude1 - rad_longitude2;

distance = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(rad_latitude1) * cos(rad_latitude2) * pow(sin(b / 2), 2)));   // google maps 里面实现的算法
distance = distance * EARTH_RADIUS;

return distance;
}

扫码加我 拉你入群

请注明：姓名-公司-职位

以便审核进群资格，未注明则拒绝

分享0 收藏0 回帖

关键词：GPS导航 无人车 经纬度 方位角 GPS