源码主线 04｜A* 的路线如何经 LOS 变成飞控速度

规划器输出的是空间路线，飞控需要的是此刻可执行的运动目标。项目的 planner_astar.py、nav_los.py、mavlink_velocity.py把这三个阶段分得很清楚：图搜索找路线，几何制导生成速度，协议层把速度交给GUIDED。理解接口，比把“A*＋LOS”当成一个整体算法更有用。

地图与搜索：最优指的是哪一个问题

load_occupancy_map读取YAML边界、障碍和分辨率，并进行障碍膨胀；AStar3DPlanner使用26邻域与欧氏距离代价、欧氏启发式。膨胀相当于把机器人的尺寸从运动物体搬到障碍物上，再把机器人近似为点。这个版本从静态地图规划，A*是可选路径，配置默认关闭。关闭时航点仍可进入LOS，因此导航并不以运行搜索为前提。

$$\text{f(n)=g(n)+h(n)； h(n)=‖p\_goal−p\_n‖}$$

在以欧氏边长计费的该图中，直线距离不大于任何绕行折线长度，因此是合适的下界。A*得到的最短，是这个离散图及其代价下的最短；它不是连续空间中任意曲线的全局最短，也没有自动考虑推进器制动能力。分辨率0.5 m时，轴向、平面对角、空间对角边长分别为0.5、0.707、0.866 m。

LOS 不追节点本身，而追前方一点

$$\text{t = clip(((p−a)ᵀ(b−a))/‖b−a‖²,0,1)； p\_look = a + min(1,t+L/‖b−a‖)(b−a)}$$

当前路径段起点a=(0,0,0)、终点b=(4,0,0)，机器人p=(1,0.6,0)，前视L=1 m。投影参数t=0.25，前视点为(2,0,0)，指向前视点的向量为(1,−0.6,0)。以0.3 m/s沿它运动，速度约为(0.257,−0.154,0)m/s：既向前走，也消除横向误差。前视点到终点截断，可避免制导目标越过当前线段。

较大L更平顺，但转弯更容易切内侧；较小L更积极消除横向误差，却增加方向变化。LOSNavigator.update还负责到点接受半径、临近目标降速、分量限幅与总速度约束；纯深度航点采用更紧的接受半径，避免尚未完成下潜就跳到下一点。

从NED速度到GUIDED分支

当前导航配置示例为10 Hz、默认0.3 m/s、L=1 m；水平分量上限0.35 m/s、Z上限0.1 m/s、总速度上限0.4 m/s。MavlinkVelocitySender发送SET_POSITION_TARGET_LOCAL_NED，frame为LOCAL_NED，mask为0x0DC7。当前GCS_MAVLINK_Sub的GUIDED接收分支采用同一契约，并把NED向下速度变为内部向上爬升符号。

因此速度目标必须按北、东、下解释，不能只因为机器人要“前进”便直接把机体前速填进北速。若上层计算的是body速度，应先根据姿态旋转到LOCAL_NED，或者使用一个双方明确支持的body接口。frame与mask属于控制语义，不只是通讯字段。

从路径安全走向运动安全

可以把安全条件分成三层：节点是否在自由空间；相邻节点之间的整条边是否可通过；LOS加上机器人实际动态形成的连续轨迹是否可通过。对角线连接两个空格仍可能擦过障碍角；前视切弯也可能离开规划折线。统一处理方法是对边和跟踪轨迹做扫掠碰撞，并用机器人尺寸和定位误差确定裕度。

$${\text{d}}_{\text{stop}}\text{≈}\frac{{\text{v}}^{\text{2}}}{\text{2}{\text{a}}_{\text{brake}}}\text{+ vτ}$$

以v=0.3 m/s、可用减速度0.15 m/s²、响应延迟0.2 s作教学估算，制动距离约0.30+0.06=0.36 m。速度虽然很低，接近障碍时仍不能只看当前位置。急转弯所需横向加速度约v²/r，转弯半径越小，同一速度越难实现。这样就能把矩阵分配能力、控制延迟与上层导航速度联系起来。

一个完整的小型复现

在二维切片中放置L形障碍，把当前位置显式作为首段起点，再规划两个航点。先画A*折线，再用上述LOS公式推进一个有加速度上限的质点。分别改变前视长度与接受半径，画出实际轨迹和膨胀障碍。学习目标不是只看到一条漂亮路线，而是解释路线、参考运动和实际运动为什么不同。