飞控算法从入门到精通 | 085、路径规划：Dijkstra与RRT算法
去年在做一个物流无人机项目，客户要求飞机在仓库内自主飞行到指定货架取货。仓库里堆满了货架，通道狭窄，还有几根立柱。我一开始想当然地用了A算法做全局路径规划，结果在仿真里跑得挺好，一上真机就出问题——飞机在某个拐角处反复横跳，最后撞上了货架边缘。后来排查发现，A生成的路径虽然最短，但路径点过于密集，导致控制器的跟踪误差累积，在狭窄通道里直接跑偏。
那次之后我认真对比了Dijkstra和RRT两种算法，发现它们在实际飞控场景下的适用性差异非常大。今天这篇笔记，就把这两种算法的工程实现细节和踩过的坑写清楚。
Dijkstra：别被“最短路径”骗了
Dijkstra算法本质上是广度优先搜索的加权版本。它维护一个开放列表，每次从列表中取出代价最小的节点进行扩展，直到到达目标点。这个逻辑在理论课上讲得很清楚，但真写到飞控代码里，有几个地方特别容易翻车。
第一个坑是栅格地图的代价设计。很多人直接把栅格距离当作边权，比如相邻栅格代价设为1，对角栅格代价设为1.414。这在平坦地面上没问题，但如果你在三维空间里做路径规划，或者要考虑障碍物距离的安全裕度，这种简单赋值就会出问题。我习惯的做法是：对每个栅格，根据其到最近障碍物的距离，动态调整通过该栅格的代价。比如距离障碍物小于两个机身半径的栅格，代价乘以一个惩罚系数。这个系数不能太大，否则算法会绕远路；也不能太小，否则路径贴着障碍物走，飞控稍微有点偏差就撞上。我一般取1.5到2.0之间，具体值要看飞机的定位精度。
第二个坑是开放列表的数据结
