[发明专利]一种基于拉格朗日方程的无人机编队队形控制算法

摘要

本发明公开了一种基于拉格朗日方程的无人机编队队形控制算法，在编队无人机路径规划方面采用改进的人工势能场；在队形保持方面将编队无人机运动看作一个约束动力学系统。在建立约束动力学系统时，首先利用多个无人机相对于惯性坐标系的空间几何关系，根据需要的编队形状建立空间队形约束方程，然后引入拉格朗日乘子建立含有队形约束的编队无人机的约束动力学方程组，最后结合鲍姆加特稳定性采用直接消除拉格朗日乘子方法并求解拉格朗日乘子。通过约束方程建立多无人机在空间的几何关系，对含有约束条件的编队运动学方程进行求解得出编队无人机在保持期望队形情况下的飞行轨迹。在解决无人机编队飞行时队形保持和变换有较强的灵活性和有效性。

主权项

一种基于拉格朗日方程的无人机编队队形控制算法，其特征在于，包括以下步骤：1)对无人机编队的运行环境进行简化和抽象，包括将无人机编队飞行三维惯性坐标系简化到二维惯性平面坐标系，将地面标志物抽象为障碍物；2)对所述无人机编队进行受力分析，包括求解在当前位置时目标区域对所述无人机编队中各个无人机产生的引力，以及所述障碍物对所述无人机编队产生的斥力；3)求解所述无人机编队中各个无人机受到的合力，以及整个所述无人机编队收受到的合力；4)引入拉格朗日乘子,建立含有队形约束的无人机编队约束动力学方程组：q·=v---(8)]]>Mq··=F(q,v,t,FTotal)-J(q)Tλ---(9)]]>g＝g(q,t)＝0                  (10)其中，q为2n维的编队无人机位置矩阵；λ为拉格朗日乘子；M为2n*2n编队无人机的权重矩阵；F(q,v,t,FTotal)为2n维的外力矩阵；为无人机当前速度；t为时间；FTotal为合外力向量，g为队形约束方程；为队形约束函数的雅可比矩阵；5)采用直接消除拉格朗日乘子方法，并结合鲍姆加特稳定性对编队无人机约束动力学方程组进行求解,得到拉格朗日乘子方程:λ=[J(q)M-1JT(q)]-1[J(q)M-1f(q,v,t,FTotal)+J(q·)q·+∂2g∂t2++αg·+δg]---(17)]]>其中,α、δ为比例因子；6)通过判断所述无人机编队受到的合力是否为零，来判断编队无人机是否运行到目标区域：如果所述无人机编队受到的合力为零，则输出所述无人机编队飞行轨迹，同时输出误差曲线，解算结束；如果所述无人机编队受到的合力不为零，则返回步骤2)。