[发明专利]一种基于逆推法的多无人机协同航迹规划方法

摘要

本发明公开了一种基于逆推法的多无人机协同航迹规划方法，包括以下步骤：步骤一、可飞行路径规划；步骤二、调整可飞行路径参数产生可飞行等长路径；步骤三、任意两条可飞行等长路径碰撞检测；步骤四、逆推法调整可飞行等长路径；步骤五、生成安全可飞行路径。本发明方法简单，采用逆推法从航路终止位姿向起始位姿逆推计算碰撞点，对两条可飞行路径的其中一条在碰撞点进行路径补偿，对另一条在起点位姿进行路径补偿，在保证多无人机安全飞行的同时，极大地减少了航迹生成时间，增加了规划效率，使用效果好。

主权项

1.一种基于逆推法的多无人机协同航迹规划方法，其特征在于，包括以下步骤：步骤一、可飞行路径规划：分别确定n架无人机的起始位姿和终止位姿，为第i架无人机产生连接起始位姿φ_si和终止位姿φ_fi的可飞行路径，n架无人机的可飞行路径均满足曲率约束，其中φ_si表示第i架无人机的起始位姿，φ_fi表示第i架无人机的终止位姿，i＝1,2,...，n,n表示无人机的数量；步骤二、调整可飞行路径参数产生可飞行等长路径：步骤201、选取参考路径：分别计算n架无人机的可飞行路径的路径长度，选择n架无人机的可飞行路径中路径长度最长的一条作为参考路径；步骤202、修正剩余可飞行路径：调整第i条可飞行路径的起始曲率半径ρ_si和终止曲率半径ρ_fi，使L_i＝L_ref，其中L_ref表示参考路径的路径长度，L_i表示第i架无人机的可飞行路径的路径长度；步骤三、任意两条可飞行等长路径碰撞检测：计算第p架无人机和第q架无人机在飞行t时刻时的分离距离d_t‑p,q，若d_t‑p,q≥R_p+R_q，则进入步骤五，否则进入步骤四，其中R_p表示第p架无人机的安全圆半径，R_q表示第q架无人机的安全圆半径，第p架无人机和第q架无人机表示n架无人机中任意两架，p和q均为不大于n的正整数；步骤四、逆推法调整可飞行等长路径：步骤401、逆推碰撞点：从航路终止位姿向起始位姿逆推，计算第p架无人机和第q架无人机第一次发生碰撞的碰撞点；步骤402、碰撞点规避：同时修改第p架无人机的可飞行路径和第q架无人机的可飞行路径，第p架无人机在碰撞点沿绕行圆周绕行一周，第p架无人机的绕行圆周的半径为r_p，第p架无人机的绕行圆周与碰撞点的位姿相切；第q架无人机在起始位姿沿绕行圆周绕行一周，第q架无人机的绕行圆周的半径为r_q，第q架无人机的绕行圆周与第q架无人机的起始位姿相切；步骤403、返回步骤三；步骤五、生成安全可飞行路径。