链上镖师 / Chain Biaoshi
TRC|密集环境多车轨迹规划：优先级搜索与安全走廊优化
SMART把多车交互顺序放在行为层处理，再用时空搜索和安全走廊SQP生成可执行轨迹
这篇TRC论文提出SMART框架，用学习引导的优先级搜索、时空Hybrid A*和安全走廊SQP，把密集场景下的多车避碰规划拆成可扩展的交互排序与连续轨迹细化问题。
- 01：研究对象是连续空间中的多车轨迹规划，车辆需要满足非完整运动学约束，同时避开静态障碍和其他车辆。
- 02：核心难点来自车辆数增长后成对避碰约束的二次增长，密集环境中的优先通过、等待和绕行顺序会直接影响可行性。
- 03：SMART在50 m随机障碍地图中25辆车仍达到90.00%成功率，在100 m地图中50辆车成功率保持95.00%以上，并完成实车交叉口验证。
研究背景
多车轨迹规划要同时解决起终点到达、车辆运动学约束、静态障碍避让和车车避碰。论文讨论的场景包括停车场、交叉口、窄通道和室内样式地图，车辆密度升高后，每一辆车的让行、等待和绕行动作都会影响其他车辆的可行空间。
来源：Fig. 1。图中给出地图尺寸、静态障碍、车辆起终点配置，以及所有车辆无碰撞到达目标的一组可行轨迹。
已有方法大致包括集中优化、分布式协调、采样规划、顺序凸化、安全走廊和MAPF启发式搜索。它们分别在建模严谨性、局部反应、路径探索和离散交互推理上有优势，密集连续车辆规划仍然需要同时兼顾可扩展交互排序、运动学可行性和后端轨迹细化。
论文把问题抓在“交互顺序”上：同一组起终点和障碍，车辆先后关系不同，会诱导不同的让行、等待、重规划和死锁风险。
核心问题
论文定义的MVTP任务是在连续二维工作空间中，为M辆前轮转向车辆计算无碰撞、运动学可行且尽量缩短总完成时间的轨迹。车辆状态包含位置、航向角和转向角，控制量包含纵向速度和转向速率，到达目标后车辆保持在目标位置直到总体任务结束。
01目标函数是最小化所有车辆中最晚到达车辆的完成时间。
02约束包括起终点边界条件、车辆动力学、车辆与静态障碍不相交、任意两车占用区域不相交。
03车辆数量增加后，车车避碰约束按二次规模增长，联合优化的可行域变得高度耦合且非凸。
SMART的核心设计是分层求解。前端先用优先级搜索推理车辆之间的交互关系，低层用时空Hybrid A*给每辆车找满足高优先级轨迹避让的可行初始轨迹，后端再通过安全走廊和顺序二次规划把轨迹变得更平滑、更可执行。
方法框架
前端采用Priority-Based Search。根节点从空优先级约束开始，若发现冲突，就对冲突车辆对施加相反的优先级关系并扩展子节点。新约束加入后，算法只对低优先级车辆及其依赖集合做拓扑排序和选择性重规划，以减少重复计算。
来源：Algorithm 1。流程展示APRIL热启动、STHA*逐车规划、拓扑排序、受影响车辆重规划和不可行节点剪枝。
来源：Fig. 2。STHA*在状态-时间联合空间扩展节点，用f=g+h排序候选节点，并剪枝与静态障碍或高优先级车辆轨迹冲突的后继。
1行为层：学习引导的优先级估计给PBS根节点提供更好的车辆排序。
2搜索层：STHA*为每辆低优先级车辆生成避开高优先级轨迹的可行初始解。
3优化层：插值初始轨迹、搜索邻域车辆、构造安全走廊，并用SQP平滑轨迹。
来源：Fig. 3。车辆被两个半径为rv的圆覆盖，膨胀信任域用于判断哪些车辆可能相互影响，从而削减后端优化中的冗余碰撞约束。
案例或实验设置
仿真实验覆盖50 m×50 m和100 m×100 m两类地图尺寸，每类包含无障碍、随机障碍和室内地图。车辆数量逐步增加以制造更高密度，每个地图类型和车辆数组合生成60个随机实例，总计2100个测试案例。
来源：Fig. 4。实验地图包含50 m随机障碍、100 m随机障碍和室内样式地图，用于测试不同障碍密度和通道约束下的规划能力。
车辆被建模为相同的前轮转向汽车，最小转弯半径为3 m，车宽1 m，轴距2 m，前后悬各1 m，最大转向角0.3218 rad，最大速度2.0 m/s，最大转向速率0.07 s^-1。这些设置让测试保留真实车辆的非完整约束。
来源：Fig. 6。实车平台配备32线LiDAR、单目相机、组合导航、车载工业计算机、4G路由和云端通信模块，并设置中高难度交叉口冲突场景。
结果分析
在50 m随机障碍地图中，SMART在10、15、20、25辆车时成功率分别为95.00%、98.33%、96.67%和90.00%，对应求解时间为0.10 s、0.22 s、0.25 s和0.90 s。相比之下，SCC在25辆车时成功率降至05.00%，PP和FOTP均为51.67%。
来源：Table 1。表格比较SCC、PP、FOTP和SMART在成功率、求解时间和任务完成时间上的表现，显示SMART在密集小中规模场景中保持较高成功率。
来源：Fig. 5。轨迹示例展示车辆数量增加后，SMART仍能生成交织、绕行和通过的无碰撞路径形态。
来源：Table 2。100 m随机障碍地图中，SMART在25至50辆车成功率保持93.33%到100.00%区间，60辆车降至65.00%，90辆车为1.67%。
50 m随机障碍25辆车
90.00%
SMART成功率，求解时间0.90 s
100 m无障碍50辆车
98.33%
SMART成功率，求解时间4.89 s
管理启示
实车实验把算法放进中高难度交叉口冲突场景。中难度20 m×20 m场景中，车辆按A≺B≺C的优先顺序执行：A基本直行通过，B在冲突点附近向右短暂绕行，C在交叉点附近减速等待A和B通过后继续前进。
来源：Fig. 7。图中展示SMART在中高难度场景下的时间序列执行轨迹，以及与CL-CBS在规划轨迹和执行轨迹上的对比。
在真实中难度场景中，CL-CBS规划耗时0.332 s、任务完成时间15.5 s，SMART规划耗时0.014 s、任务完成时间18.1 s。在高难度场景中，CL-CBS规划耗时10.075 s、任务完成时间17.4 s，SMART规划耗时0.623 s、任务完成时间20.3 s。
✓密集车队调度可以先确定可解释的优先关系，再把局部轨迹交给连续优化处理。
✓V2I通信、路侧感知和共享状态信息是SMART实车部署的关键系统条件。
✓论文结果支持将SMART用于需要快速生成安全协同轨迹的封闭园区、测试场和受控交叉口场景。
这篇论文的贡献在于把多车密集交互从一次性联合求解中拆出来，用优先级搜索保留全局协调结构，再用安全走廊SQP获得可执行轨迹，从而在成功率、规划时间和实车可解释性之间取得可验证的平衡。
论文：SMART: Scalable multi-agent reasoning and trajectory planning in dense environments
作者：Heye Huang，Yibin Yang，Wang Chen，Tiantian Chen，Jinxin Liu，Sikai Chen
期刊：Transportation Research Part C
链上镖师、先卜交通、先卜运筹通用论文ID
保存此推文 ID，可在粉丝群内获取相关材料，未来更多功能敬请期待
先卜科技团队：常文婕、张巧巧、王一航、夏子帆、张晓乐、祁园园、张仁义
