【代码】给定起点位置、距离及姿态角，求另一点位置

ytkz2024-03-172024-09-09

如题，这是现实中遇到的问题的简化版。

已知一个点的经纬度和离另一个点的球面距离，和无人机飞行时刻姿态角，求另一个点的经纬度。

姿态角

首先，得了解什么是姿态角。

姿态角（Attitude angles）：这是一个用于描述物体在空间中的方向和姿态的一组三个角度，通常包括滚动角（Roll）、俯仰角（Pitch）和偏航角（Yaw）。

俯仰角：围绕物体的前后轴线（通常被称为X轴）旋转的角度。
偏航角：围绕物体的左右轴线（通常被称为Y轴）旋转的角度。
滚动角：围绕物体的上下轴线（通常被称为Z轴）旋转的角度。

下面三张动图形象的表示了欧拉角的旋转方式。第一张是绕x轴旋转pitch，第二张绕y轴旋转yaw，第三张是绕z轴旋转roll。

这里写图片描述

简言之，pitch是俯仰角，是“点头“

yaw是偏航角，是‘摇头’

roll是旋转角，是“翻滚”

这里我们要用到的是姿态角中的偏航角。

代码

无人机从一个已知的经纬度（lat1, lon1）飞行一定的球面距离（d）后，假设飞行方向由偏航角（yaw）决定，得出的目标点的经纬度（lat2, lon2）。请看以下代码。

import math

def compute_destination_point(lat1, lon1, yaw, d):
    # 地球半径，单位为千米
    R = 6371.0

    # 将输入的角度和距离转为弧度
    lat1 = math.radians(lat1)
    lon1 = math.radians(lon1)
    yaw = math.radians(yaw)
    d = d / 1000  # 将距离转为千米

    # 计算目标点的纬度
    lat2 = math.asin(math.sin(lat1)*math.cos(d/R) +
                     math.cos(lat1)*math.sin(d/R)*math.cos(yaw))

    # 计算目标点的经度
    lon2 = lon1 + math.atan2(math.sin(yaw)*math.sin(d/R)*math.cos(lat1),
                             math.cos(d/R)-math.sin(lat1)*math.sin(lat2))

    # 将结果转为度数
    lat2 = math.degrees(lat2)
    lon2 = math.degrees(lon2)

    return lat2, lon2

# 测试函数
lat1 = 30.0  # 初始纬度
lon1 = 120.0  # 初始经度
yaw = 0 # 偏航角，以度为单位
d = 111100 # 球面距离，以米为单位

lat2, lon2 = compute_destination_point(lat1, lon1, yaw, d)
print(f"目标点的纬度：{lat2}, 经度：{lon2}")

结果展示

定义测试函数中的数值是有讲究的，偏航角为0即无人机向东飞行。

1度约等于111100米，这里定义球面距离为111100米，所以结果应该是纬度31度，经度为120度。

程序运行结果符合预期，误差在预期范围内。

代码解释

代码的主要步骤如下：

将输入的经纬度、偏航角和距离转换为弧度。弧度是角度的另一种表示方式，适用于数学运算。弧度和角度之间的转换关系为：弧度 = 角度 * π / 180。这里使用math.radians函数进行转换。
计算目标点的纬度（lat2）。这一步使用的是球面三角学的公式，基于当前位置的纬度（lat1）、飞行距离（d）和偏航角（yaw）来计算。
计算目标点的经度（lon2）。这一步同样使用的是球面三角学的公式，基于当前位置的经度（lon1）、纬度（lat1）、飞行距离（d）和偏航角（yaw）来计算。
将计算出的目标点的经纬度转换回度数。这里使用math.degrees函数进行转换。
返回目标点的经纬度。