首页 新闻 会员 周边 捐助

路线导航如何判断当前处于哪个阶段

0
悬赏园豆:10 [待解决问题]
       [
          {
            instruction: '向东步行84米左转',
            action: '左转',
            polyline: [
              [121.585851, 31.198207],
              [121.586424, 31.198312],
              [121.586424, 31.198312],
              [121.586727, 31.198364],
            ],
          },
          {
            instruction: '向北步行27米右转',
            action: '右转',
            polyline: [
              [121.586727, 31.198364],
              [121.58668, 31.198611],
            ],
          },
          {
            instruction: '沿晨晖路向东步行26米左转',
            action: '左转',
            polyline: [
              [121.586675, 31.198611],
              [121.586784, 31.198633],
              [121.586944, 31.198659],
            ],
          },
          {
            instruction: '沿松涛路向北步行333米右转',
            action: '右转',
            polyline: [
              [121.586944, 31.198659],
              [121.586671, 31.200204],
              [121.586671, 31.200204],
              [121.586589, 31.20066],
              [121.586589, 31.20066],
              [121.586506, 31.201137],
              [121.586506, 31.201137],
              [121.586437, 31.201502],
              [121.586437, 31.201502],
              [121.586411, 31.201615],
              [121.586389, 31.201641],
            ],
          },
          {
            instruction: '沿祖冲之路向东步行97米到达目的地',
            action: '',
            polyline: [
              [121.586385, 31.201641],
              [121.586814, 31.201719],
              [121.586814, 31.201719],
              [121.587248, 31.201793],
              [121.587248, 31.201793],
              [121.5874, 31.201819],
            ],
          },
        ]

像上面的一个导航规划的路线,给定一个坐标,如何判断当前处于哪个阶段,有没有好的算法啊

__小灰的主页 __小灰 | 初学一级 | 园豆:192
提问于:2024-09-14 11:52

将经纬度转换为平面坐标后,可以判断上一次位置到当前位置是以什么方式接近目标位置的,最好写一个计算算法,比如从X方向靠近还是Y方向靠近,如果从X方向靠近,则判断该方位目标位置,但是只适合数据量小的,如果是数据量比较大的,则需要考虑聚类(数据可能不是一条线,有误差情况下可能呈现出发散状态),通过一次次聚合来进行判断路径(中心点)

把月亮偷进小木盒 3个月前
< >
分享
所有回答(2)
0

判断点在哪个线段上就行了吧. 点在首尾之间,点在线上.
如果点不一定完全在线上. 那就点在首尾之间,点到线的距离最短.

www378660084 | 园豆:1192 (小虾三级) | 2024-09-14 15:20
0

你好,我并没有太了解你的意思,你可以看看这个是不是你想要的。

import numpy as np


def distance_point_to_segment(px, py, ax, ay, bx, by):
    """计算点(px, py)到线段(ax, ay)和(bx, by)的最短距离"""
    # 线段的长度
    ab = np.array([bx - ax, by - ay])
    ap = np.array([px - ax, py - ay])
    bp = np.array([px - bx, py - by])

    # 线段的长度的平方
    ab_squared = np.dot(ab, ab)

    # 计算投影比例
    if ab_squared == 0:
        return np.linalg.norm(ap)

    t = np.clip(np.dot(ap, ab) / ab_squared, 0, 1)  # 计算t值并限制范围
    projection = np.array([ax, ay]) + t * ab  # 求投影点
    return np.linalg.norm(np.array([px, py]) - projection)  # 返回距离


def find_current_stage(coordinate, route):
    """返回当前阶段"""
    current_x, current_y = coordinate
    closest_stage = None
    min_distance = float('inf')  # 用于追踪最小距离

    for index, stage in enumerate(route):
        start = stage['polyline'][0]
        end = stage['polyline'][-1]

        # 计算当前坐标到该阶段的线段的距离
        dist = distance_point_to_segment(current_x, current_y, start[0], start[1], end[0], end[1])

        # 输出调试信息
        print(f"阶段 {index + 1} 的距离: {dist:.2f} 米")

        # 如果距离小于最小距离和判断阈值,更新当前阶段
        if dist < min_distance and dist < 10:  # 修改阈值为10米
            min_distance = dist
            closest_stage = (index + 1, stage['instruction'])

    return closest_stage if closest_stage else (None, None)


# 示例坐标
current_coordinate = (121.585851, 31.198207)
route = [
    {
        'instruction': '向东步行84米左转',
        'action': '左转',
        'polyline': [
            [121.585851, 31.198207],
            [121.586424, 31.198312],
            [121.586424, 31.198312],
            [121.586727, 31.198364],
        ],
    },
    {
        'instruction': '向北步行27米右转',
        'action': '右转',
        'polyline': [
            [121.586727, 31.198364],
            [121.58668, 31.198611],
        ],
    },
    {
        'instruction': '沿晨晖路向东步行26米左转',
        'action': '左转',
        'polyline': [
            [121.586675, 31.198611],
            [121.586784, 31.198633],
            [121.586944, 31.198659],
        ],
    },
    {
        'instruction': '沿松涛路向北步行333米右转',
        'action': '右转',
        'polyline': [
            [121.586944, 31.198659],
            [121.586671, 31.200204],
            [121.586671, 31.200204],
            [121.586589, 31.20066],
            [121.586589, 31.20066],
            [121.586506, 31.201137],
            [121.586506, 31.201137],
            [121.586437, 31.201502],
            [121.586437, 31.201502],
            [121.586411, 31.201615],
            [121.586389, 31.201641],
        ],
    },
    {
        'instruction': '沿祖冲之路向东步行97米到达目的地',
        'action': '',
        'polyline': [
            [121.586385, 31.201641],
            [121.586814, 31.201719],
            [121.586814, 31.201719],
            [121.587248, 31.201793],
            [121.587248, 31.201793],
            [121.5874, 31.201819],
        ],
    },
]

current_stage, instruction = find_current_stage(current_coordinate, route)
print(f"当前阶段: {current_stage}, 指令: {instruction}")
谁是月光谁离城 | 园豆:516 (小虾三级) | 2024-09-14 15:22
清除回答草稿
   您需要登录以后才能回答,未注册用户请先注册