简介这份压缩包提供无人机飞行走廊规划的完整B样条连接轨迹优化实现面向无人机算法研究、路径规划与自主导航开发的工程师。B样条曲线通过控制点调整路径形状可生成连续平滑的飞行轨迹减少剧烈机动并提升安全性算法兼顾灵活性与计算效率适合实时控制系统。包内共133个文件核心包含50个hpp与24个cpp源码、13个头文件、6个so动态链接库另有cfg参数配置、md说明文档、msg/srv通信定义、rviz可视化配置等整体仅9.7MB结构清晰便于按模块查阅。算法覆盖B样条生成与控制点优化、状态估计与路径跟踪、基于传感器数据的动态避障并提供控制系统接口与仿真测试支撑开发者可结合自身场景修改定制。已有213人学习下载适合需要提升复杂环境下飞行稳定性与自主性的研究者。1. 无人机飞行走廊规划与 B 样条连接轨迹这份源码包到底解决什么问题如果你的无人机路径规划还停留在离线 A* 出折线、再让飞控把折线飞成钝角的阶段那真该花一个下午看看这份资源。飞行走廊规划的思路很简单先从碰撞地图里抠出一条由若干凸多边形组成的无碰撞通道再用 B 样条把通道的骨架连成一条连续且可微的轨迹最后交给优化器收紧平滑性和速度约束。压缩包里的 b_traj_node.cpp、collision_map.cpp 和 MOSEK 求解器配置就是把这三个环节串成一条可复现流水线的完整实现。对做无人机三维路径规划数学模型、写毕业论文或做飞控仿真验证的同学这正是那种拿到就能跑的参考工程能省下大量从零造轮子的时间。2. 从碰撞地图到飞行走廊collision_map 的构建逻辑与三个关键参数先想清楚一件事B 样条优化器并不是直接在原始点云里找路径的。它需要一个“安全信封”这个信封就是飞行走廊。没有走廊避障约束要写成“轨迹上的每一点距离所有障碍物都大于某阈值”这是一个高度非线性的约束而一旦有了走廊约束被改写为“轨迹上的每一点都在某个凸多边形内部”就变成了一组线性不等式。这一步简化才是整个 B 样条优化能在毫秒级跑完的前提。包里的 collision_map.cpp 和 tagged_object_collision_map.cpp就是负责把地图转换成这种线性约束的入口。这一节我会先把两个碰撞地图模块的职责拆清楚再给出走廊剖分的参考实现最后用一个安全指标表说明怎么判断生成的走廊是否合格。2.1 地图输入与碰撞对象标签从文件名就能看出来tagged_object_collision_map.cpp 是在 collision_map.cpp 之上做了“对象标签化”处理。普通栅格地图只告诉你哪个栅格有障碍物tagged 版本则会为每个障碍物对象分配一个 id并给出它的类型、外接半径和类别属性。这样做的直接好处是静态障碍物和动态障碍物可以在同一个碰撞地图里共存B 样条优化器看到静态障碍物时做硬约束看到动态障碍物时则会额外检查时间标签。// 碰撞对象标签的简化数据结构包内实现大概率与此同构 struct ObjectTag { enum Type { STATIC, DYNAMIC, BOUNDARY }; int id; // 标签编号地图内唯一 Type type; // 对象类型 float radius; // 膨胀半径单位与地图一致 float x, y, z; // 障碍物中心位置三维场景下才有意义 }; bool load_obstacles(const std::string cfg, std::vectorObjectTag objs) { // 从 cfg 文件读取对象列表 // 例如 2_300.cfg 可理解为二维地图、300 个碰撞对象 return parse_and_validate(objs); }这段代码里的 id 是后续所有查表的钥匙。collision_map.cpp 里维护的碰撞查询接口比如bool is_collide(float x, float y)内部通常就是拿着坐标遍历所有有效的 ObjectTag比较距离和 radius。动态对象和静态对象的差别在于动态对象的包围球半径除了物理尺寸还要额外叠加它的最大速度乘以规划时间否则轨迹在时间轴上很容易擦到移动障碍物的残影。type 字段在中后期优化里的作用更大。BOUNDARY 类型的对象用来约束走廊边界它不参与避障计算但会参与走廊剖分时的裁剪DYNAMIC 对象在时间分配阶段被重新读取用于检查轨迹在每个时刻是否和它预期位置冲突。如果你只关心静态地图把 type 全部设为 STATIC 也能跑通但就浪费了包内这套标签机制。实际跑的时候还有个小技巧动态障碍物如果数量超过 300遍历查询的时间会线性增长。包内 2_500.cfg 就是 500 个对象的配置如果你在 2_500 上跑每次查询都卡顿可以考虑把地图栅格化先对静态障碍物做一次欧氏距离场动态障碍物再单独用 ObjectTag 数组维护。距离场建立后单次碰撞查询变成了查表加插值耗时和地图分辨率相关和障碍物数量无关这是从包内源码延伸出来最常见的性能优化手段。2.2 走廊剖分与连接顺序有了碰撞对象下一步是把自由空间切成一串凸多边形。最稳的做法是先生成一条参考路径可以是 A* 结果也可以是 RRT 的粗略路径然后把参考路径按固定窗口长度切段对每一段内的路径点集合求凸包。// 走廊剖分参考实现沿参考路径逐段生成凸多边形 std::vectorPolygon build_corridor( const GridMap map, const std::vectorPoint ref_path, float inflation) { std::vectorPolygon corridor; int seg_len 8; // 每段包含的路径点数 for (size_t i 0; i seg_len ref_path.size(); i seg_len) { Polygon poly convex_hull(ref_path.begin() i, ref_path.begin() i seg_len); // 如果凸包与膨胀后的障碍物相交缩短窗口重来 while (intersect(map, poly, inflation)) { seg_len std::max(2, seg_len - 1); } corridor.push_back(poly); // 下一段从上一段末尾重新开始保证相邻多边形有重叠 i i seg_len - 1; } return corridor; }这里有两个参数决定了走廊质量。第一个是 seg_len也就是窗口长度。窗口太长凸包容易包进障碍物程序只能靠 while 循环反复缩短窗口耗时明显窗口太短走廊被切得细碎后续 B 样条控制点数量被迫增加。我一般会以地图分辨率为参考让 seg_len 等于路径总长度除以期望的走廊段数期望段数在 5 到 15 之间比较合理。第二个是 inflation也就是障碍物膨胀半径。它必须大于无人机半径加控制余量但又不能大到把相邻走廊的重叠区域挤没否则第 5 章的不可行问题就会找上门。剖分顺序决定了走廊能否覆盖完整路径。注意代码里i i seg_len - 1这一行它让相邻两个凸包共享一个路径点这样两个多边形一定有交集。B 样条从一个多边形过渡到下一个多边形时这段交集就是天然的缓冲区。很多第一次上手的人会问为什么不直接用矩形走廊矩形走廊当然能做但凸多边形的自由度更高。凸多边形可以由一组线性不等式表达为 A x ≤ bB 样条轨迹的每个采样点都能直接套进这一组不等式MOSEK 求解时不需要任何非线性逼近。矩形走廊则需要更大面积的膨胀导致可行空间被压缩在狭窄场景里特别吃亏。2.3 走廊安全性的量化间隙、重叠和覆盖生成走廊后不要急着丢给 B 样条。先用三个指标检查一下很多隐形问题都藏在走廊内部。指标计算方法经验判据最小净距障碍物到多边形各边的最短距离减去自身半径不低于 0.3 米或一个栅格相邻重叠相邻两个凸多边形交集的内切圆半径不小于控制点间距的一半端点覆盖起点、终点是否落在首尾多边形内部必须完全包含最小净距太小时说明走廊已经贴着障碍物边缘走优化器为了安全会把轨迹强推到一侧结果可能是轨迹出现局部折角。相邻重叠过小时多边形的交集地带非常窄B 样条经过这里时约束太紧MOSEK 会报告不可行。端点覆盖是最容易被忽略的因为很多地图的起点和终点本身就在障碍物边界上多边形如果不做外扩起点根本没被包进去轨迹一开始就越界。提示检查走廊时把每个多边形的顶点输出成 CSV在 MATLAB 里用 patch 画一遍比盯着数字判断直观得多。这也是后面第 6 章验证流程的一部分。走廊质量直接决定 B 样条优化是否收敛数值上不可行和物理上穿墙的根源都在这一层。下面进入轨迹表示层看 B 样条如何把走廊变成一条能飞的曲线。3. 用 B 样条连接走廊控制点初始化、节点向量与时间分配走廊建好还要有一条平滑曲线穿过这些多边形。B 样条作为轨迹表示层有三个参数决定成败阶数、控制点数量和节点向量。理解了这三个参数的取值逻辑后面看 b_traj_node.cpp 的优化代码就会非常顺。3.1 B 样条比贝塞尔强在哪局部支撑与控制点B 样条曲线本质上是控制点的加权和权重由基函数决定。三次 B 样条基函数只在节点向量的局部区间非零所以移动一个控制点只影响附近几段曲线这个性质叫局部支撑。贝塞尔曲线则相反每个控制点都对整条曲线有影响当你面临走廊里 10 个多边形需要局部调整时贝塞尔改一个点等于重新生成整条轨迹。无人机路径规划里的实际收益是优化器每次迭代只需要更新局部控制点就能把轨迹拉回走廊边界内速度比全局调整快了一个量级。包内的 b_traj_node.cpp 把 B 样条作为轨迹表示层正是看中了这个优势。另外三次 B 样条天然保证位置、速度、加速度都连续也就是 C2 连续这意味着对飞控来说参考加速度没有跳变姿态环不会因为一个突兀的期望加速度而猛打舵。在数学模型上B 样条曲线写为 p(u)Σ N_{i,k}(u)·P_i其中节点向量 u 随弧长或时间单调递增。不需要把基函数展开成多么复杂的多项式只要记住阶数 k 决定连续阶控制点 P_i 是优化变量节点向量 u 决定曲线在哪段参数区间的自由度更大。一个很容易踩的细节是端点性质。普通均匀节点向量生成的 B 样条起点和终点不会精确经过首尾控制点而是有一段收敛过渡区。飞行走廊要求轨迹起点就是无人机当前位置终点就是目标点所以节点向量两端要让 k 重复 degree1 次强制形成 clamped 曲线。很多初学的同学把曲线画出来发现起点偏了一截就是漏了这个端点钳制。3.2 控制点初始化与节点向量选取控制点从走廊中线提取。把每个走廊多边形的中心点连成一条中线再按等距采样得到初始控制点。// 根据走廊中线初始化 B 样条参考 b_traj_node 中的做法 BSpline init_bspline_from_corridor( const std::vectorPoint midline, float min_width, int degree 3) { // 对中线降采样间隔按照走廊最小宽度的 1.5 倍控制 std::vectorPoint ctrl downsample(midline, min_width * 1.5); // clamped 节点向量两端重复 degree1 次 std::vectordouble knots; knots.reserve(ctrl.size() degree 1); for (int i 0; i degree; i) knots.push_back(0); for (size_t i 1; i ctrl.size() - degree; i) knots.push_back((double)i); for (int i 0; i degree; i) knots.push_back((double)(ctrl.size() - degree)); return BSpline(knots, ctrl, degree); }代码里的 min_width 是第 2 章走廊最小宽度的统计值因为控制点间隔不能小于走廊能提供的横向自由度。downsample 间隔取 min_width 的 1.5 倍是一个相对稳妥的经验值控制点数量不会过多轨迹抖动风险低如果间隔太密控制点之间的间距小于障碍物膨胀半径优化器很难把轨迹从狭缝里拉出来。节点向量用 clamped 均匀分布是最省事的。均匀节点向量下每个控制点的影响力范围一致QP 问题的 Hessian 矩阵结构比较整齐MOSEK 求解也更快。非均匀节点向量通常用在障碍物非常密集的区域需要在那里密布控制点但代价是约束矩阵稀疏性变差。第一次跑通时别折腾非均匀等看到实际轨迹哪里不够再局部插节点。需要注意的是控制点数量和走廊段数并不一定相等。一个走廊段内部至少需要两到三个控制点才能表达曲线弧度所以初始化时先根据走廊段数估算一个下限再按间距要求加密最后统一做一次降采样。这个过程没有绝对标准我一般会打印控制点数与走廊段数的比值控制在 2.5 到 3.5 之间低于 2 则轨迹偏直高于 4 则开始出现可察觉的波浪。3.3 时间分配与速度加速度可行性B 样条只给了几何形状无人机真正需要的是“什么时刻到达哪个位置”。因此要给曲线加时间参数。常用做法是假设轨迹近似匀速把总长度按控制点间隔分配时间再根据速度加速度限制做一次缩放。// 初始时间分配按速度和加速度约束的保守估计 std::vectordouble allocate_time( const std::vectorPoint ctrl_pts, double vmax, double amax) { std::vectordouble T(ctrl_pts.size() - 1); double segment_len 0; for (size_t i 1; i ctrl_pts.size(); i) segment_len norm(ctrl_pts[i] - ctrl_pts[i-1]); int n int(ctrl_pts.size()) - 1; double t_vel segment_len / (n * vmax); double t_acc std::sqrt(segment_len / (n * amax)); for (int i 0; i n; i) T[i] std::max(t_vel, t_acc); return T; }这段给出的 T 只是初始猜测真正优化会把时间缩放变量一并交给 MOSEK。时间缩放变量是一个标量 τ把 B 样条曲线表达式里的 u 替换成 t/τ速度约束变成线性不等式加速度约束则在二阶锥里处理。细节处注意vmax 和 amax 都要取飞行控制器的保守值不要按电机的最大能力给否则优化出的轨迹虽然数学可行飞控却很可能在急转弯处触到姿态限制。到这里B 样条轨迹已经完成了从几何到时间上的描述下一步才是真正的数值优化。4. MOSEK 优化求解代价函数、凸化处理与配置文件怎么选轨迹的所有自由参数已经明确剩下的问题是如何让它在满足走廊约束、速度加速度约束的同时尽量平滑。MOSEK 是这个流水线的核心求解器包里出现的 libmosek64.so.8.1 正是 MOSEK 8.1 版本的运行库。4.1 把轨迹优化写成凸二次规划凸性的意义如果目标函数和所有约束都是凸的全局最优解可以高效求得。B 样条的控制点作为变量走廊约束是线性不等式速度和加速度约束也可以线性化所以整个问题适合表达成二次规划QP。MOSEK 对大规模稀疏 QP 的处理效率极高这也是为什么本地跑 2_300.cfg 时优化耗时通常只有几十到几百毫秒而不是秒级。目标函数通常包含三项平滑项、净距项和可行性项。平滑项用控制点的二阶差分平方和衡量即 Σ||P_{i2}-2P_{i1}P_i||²它鼓励轨迹不要剧烈扭动同时抑制高频抖动。这一项在数学上是标准二次型展开后每个控制点的系数构成一个五对角矩阵稀疏性很好。净距项用控制点到走廊边界距离的线性惩罚实现距离越近罚值越大。该项与严格约束不同它允许轨迹在极端情况下略微触边给数值求解留出缓冲。惩罚函数一般做成分段一次函数走廊内部罚值为零越过边界后罚值随越界深度线性增长这两段拼起来依然是凸函数。可行性项指的是速度、加速度上下界它们被写成线性不等式或二阶锥约束。速度约束可以直接用控制点差分除以时间缩放变量表示加速度约束涉及二阶差分在 SOCP 形式下更稳当。MOSEK 对混合 QP 和 SOCP 支持得很好无需把二阶锥手动展开成大量线性约束。用 MOSEK 建模时将控制点 P_i 的所有坐标看作一个长度为 n_ctrl×D 的变量向量目标矩阵就是根据 B 样条基函数计算出的稀疏矩阵。每帧优化时只有障碍物位置和无人机当前位置会变矩阵结构不变可以预先分配好稀疏结构只更新数值这一招能把单帧优化时间再压缩一半。4.2 从配置到解MOSEK 求解流程与配置文件怎么选包里 test.cfg、benchmark.cfg 和 2_150.cfg、2_300.cfg、2_500.cfg 的分工很清晰。test.cfg 是快速冒烟配置专门用于确认环境能跑通benchmark.cfg 是正式对比实验配置2_150.cfg、2_300.cfg、2_500.cfg 则代表不同规模的地图场景。实际运行一般是先 test.cfg 确认环境再按需要把地图规模文件传进去。// 求解器调用流程的示意代码MOSEK C 接口风格 MSKenv_t env; MSK_makeenv(env, NULL); MSK_maketask(env, 0, n_vars, task); MSK_put_mobj(task, 0, n_terms, csubj, csubj, val); // 二次目标 MSK_put_linear_constraint(task, n_con, aptr, asub, aval); // A x b MSK_optimizetrm(task); MSK_getsolution(task, MSK_SOL_ITR, NULL, solx, NULL, NULL);这里的 n_vars 是控制点坐标变量加上时间缩放变量的总数n_con 包括走廊不等式约束、速度约束和加速度约束。注意 MOSEK 默认处理的是最小化问题目标函数系数矩阵必须是对称半正定的否则求解器会直接报错。配置文件决定求解精度和速度的平衡。我一般会把 test.cfg 里的迭代上限设到 1000容差稍微放宽到 1e-4跑通后再切到 benchmark.cfg 把容差收紧到 1e-6。具体对照配置典型用途建议迭代上限终止容差test.cfg环境验证、流程冒烟10001e-4benchmark.cfg正式轨迹生成50001e-62_150.cfg稀疏场景10001e-52_300.cfg中等场景30001e-62_500.cfg密集场景60001e-7这些数值不是死的但如果 test.cfg 都跑不出 optimal就不要急着上 2_500.cfg先回头检查走廊。压缩包里同名配置文件的作用就是让你在不同规模之间快速切换而不是手动改代码重编。4.3 数值问题缩放、容差与求解器日志MOSEK 报出 near feasible 或者 unknown大多不是算法本身的问题而是目标函数各项量纲差距过大。平滑项通常是 10⁻³ 数量级安全项可能是 10³ 数量级直接放到同一个目标函数里数值消元很痛苦。解决办法有两种。一种是统一变量量纲控制点坐标都以米为单位时间缩放变量以秒为单位速度约束换算成 1/s 的尺度目标函数各项分别乘以权重 λ1、λ2、λ3让它们在平衡状态贡献相近。另一种是开启 MOSEK 的自动缩放但不建议完全依赖毕竟自动缩放会让约束的物理含义变模糊。求解时也留意日志里的 scaling factor 和 condition number。条件数超过 1e12 时可以尝试减少控制点或者把走廊约束拆成更多条小约束让矩阵更稀疏。当 MOSEK 日志出现连续多行需要额外预处理的提示时基本可以断定数值病态第一反应是简化模型而不是硬调求解器参数。到这一节为止整个规划与优化的主链路已经完整。下面这部分是最容易让人卡住的地方全是实际操作里的坑。5. 避坑指南从缺失共享库到轨迹振荡的五条踩坑记录5.1 libmosek64.so.8.1 找不到现象在 Linux 终端执行./b_traj_node 2_300.cfg立即报错error while loading shared libraries: libmosek64.so.8.1: cannot open shared object file。原因压缩包里的 MOSEK 8.1 运行库没有在系统动态链接器搜索路径里。常见于把源码包解压后直接运行而 libmosek64.so.8.1 和 b_traj_node 不在同一个可执行文件搜索路径下也没有用 LD_LIBRARY_PATH 声明。解决先把所有 so 库放在同一个目录再导出环境变量并用 ldd 确认依赖。export LD_LIBRARY_PATH$LD_LIBRARY_PATH:$(pwd)/lib ldd b_traj_node # 期望输出里每一行都是 found如果 ldd 显示 not found检查目录路径是否写对以及 so 文件是否真的存在。MOSEK 8.1 老版本对 glibc 版本有要求如果在新版系统上起不来最省事的办法是换成 MOSEK 官方许可库重新编译或者用容器装一个老一点的环境。5.2 libcilkrts.so.5 缺失现象处理完 libmosek 后运行又提示libcilkrts.so.5: cannot open shared object file。原因libcilkrts 是 Intel Cilk 并行运行时库老版本 Intel 编译器会把 Cilk 依赖编译进程序。新系统默认不带这个库。解决一种方式是安装 Intel oneAPI 的运行时把 libcilkrts.so.5 加入 LD_LIBRARY_PATH另一种是不装 Intel 全家桶直接找对应版本的 libcilkrts.so.5 放到 b_traj_node 同目录再按 5.1 的方式导出路径。后者更快但要注意版本位数必须匹配32 位和 64 位混用会直接段错误。5.3 控制点过多导致轨迹蛇形振荡现象优化求解成功但输出轨迹明显在走廊内左右蛇形摆动曲率比手动规划的路径大很多。原因控制点数量过多且分布间隔小于走廊横向裕度。B 样条有足够自由度去复现参考路径的抖动优化器在平滑项权重不大时就会保留这些高频分量。解决把第 3 章初始化函数里的 downsample 间隔从 1.5 倍走廊最小宽度提高到 2.5 倍同时把平滑项权重 λ1 调大。如果两根相邻控制点之间轨迹仍有 S 形检查走廊生成时是否用了过大的膨胀半径走廊宽度不均匀也会让控制点分布失真。5.4 走廊过窄导致 MOSEK 报告不可行现象MOSEK 返回Primal infeasible任务输出里没有轨迹。原因走廊剖分时膨胀半径取得太大或者相邻多边形重叠区域太小使得 B 样条无法同时满足“所有点在走廊内”和“平滑项”的约束。例如 inflation 取 0.8 米走廊净宽只剩 0.2 米B 样条两侧几乎贴着墙壁数值上找不到可行解。解决先降低 inflation 到 0.3 到 0.4 米重新生成走廊看能否可行。如果仍不可行检查首尾端点是否被包进多边形再适当放宽与速度约束相关的变量上限给优化器更多裕度。还有一个容易被忽略的点走廊重叠区域的内切圆半径如果小于控制点间距的一半B 样条在两个多边形之间过渡时必然越界这个在第 2 章的安全指标表里已经提醒过。5.5 修改地图后标签过期现象换了新的地图文件但轨迹依然避开旧地图的障碍物位置甚至直接穿过新地图里的障碍物。原因collision_map.cpp 维护的碰撞对象数据缓存没有被刷新或者 tagged_object_collision_map.cpp 里新增对象没有调用刷新接口。地图文件变了内存里的静态对象列表还是旧版。解决每次换 cfg 后按固定顺序重新加载先调用地图加载函数再重建对象标签再重生成走廊最后才做 B 样条优化。把这一整套流程封装成一个脚本不要手动一步步执行脚本里的校验逻辑可以检查对象计数是否和新 cfg 一致。提示在代码里判断标签是否过期最简单的方式是打印每个 cfg 的对象数和地图文件名核对无误后再进入优化。6. 进阶技巧把二维走廊扩到三维并用 MATLAB 复现轨迹6.1 从 2_xxx.cfg 到三维走廊二维和三维的 B 样条控制点结构几乎一样只是控制点从 (x,y) 变成 (x,y,z)目标函数的平滑差分每维各算一次。压缩包里 2_150.cfg、2_300.cfg、2_500.cfg 的命名方式从我的经验看更像是“2 维地图 对象数量”的组合真正要跑三维任务时需要自己做一层扩展。扩展方法不难二维走廊仍然定义 X-Y 方向的凸多边形约束Z 方向单独加上下界 z_min 和 z_max通常把所有走廊段的 Z 上下限设为同一个地表高度和限飞高度即可。控制点变量数变为原来的 1.5 倍MOSEK 任务里约束条目和稀疏矩阵的列数跟着调整求解核心逻辑完全不用改。真正麻烦的是地图里的障碍物高度信息。如果障碍物是竖直柱体沿用二维膨胀半径就能覆盖如果是悬空的低空物体碰撞检查就要按 Z 高度对障碍物做一次视野裁剪低于轨迹高度的物体可以直接忽略这样能减少大量无效约束。6.2 用 MATLAB 验证输出轨迹建议把 B 样条控制点输出成 CSV再用 MATLAB 做可视化。这一步能同时验证走廊几何是否符合预期、轨迹是否平滑、以及在每个时刻是否贴近走廊中心线。data csvread(traj_ctrl.csv); x data(:,1); y data(:,2); if size(data,2) 3 z data(:,3); plot3(x,y,z,linewidth,1.5); grid on; hold on; else plot(x,y,linewidth,1.5); grid on; hold on; end % 把走廊多边形顶点也画出来 patch(corridor_x, corridor_y, g, FaceAlpha, 0.1); xlabel(X); ylabel(Y);这段脚本里 corridor_x、corridor_y 是第 2 章输出的走廊顶点数组。检查原则很简单轨迹应完全落在多边形包络内部且相邻控制点连线的方向变化平稳。如果轨迹贴着某条边线走说明净距项权重偏低如果穿越了走廊边界说明走廊本身没有闭合或者约束矩阵少了一行。6.3 最小验证清单与我的习惯换新机器后先跑ldd b_traj_node确认 libmosek64.so.8.1 和 libcilkrts.so.5 都能解析再进入算法验证。先跑 2_150.cfg 的 test 配置确认返回状态是 optimal再上 2_300.cfg 和 2_500.cfg。每次迭代先看走廊三项安全指标不看轨迹图指标不对后面全白做。对动态障碍物时间分配后检查每个时刻 t 的预测位置是否与走廊段匹配不匹配时优先调整时间缩放变量。我记得有一次直接从 2_500.cfg 开始结果前三次都因为 Cilk 运行时缺失崩溃后来才意识到不是代码问题而是环境没有预先验证。从那以后我每次换环境都强制先跑 test.cfg 加 ldd再上大规模如果规划结果不贴合走廊就先查走廊再查控制点间隔最后才动求解器参数。这份资源给我的最大价值不是现成的 B 样条优化实现而是一条能自己定位问题的完整链路。希望帮到你。本文还有配套的精品资源点击获取