地空多机器人编队的图搜索规划和分布式轨迹优化方法及系统

本发明公开了一种地空多机器人编队的图搜索规划和分布式轨迹优化方法及系统，首先，对状态空间进行位置和角度的离散化，将得到的离散点映射得到不同的编队构型，构造有效构型的无向图，利用图搜索规划方法，在无向图上搜索从起始构型到目标构型的路径序列；然后利用微分平坦将四旋翼飞行器和移动机器人的动力学模型降维，分别对每个平坦输出进行轨迹规划，综合考虑能量消耗和编队误差指标，构建最优函数，将机器人的轨迹规划转化为最优问题；最后做分布式分段求解，迭代后获得优化的轨迹。本发明方法有效降低单个机器人的计算压力，提高计算效率，更适应于大规模集群的机器人系统规划方法。