1.一种基于CAPT算法改进的无人船集群任务规划方法,其特征在于,该方法包含以下步骤:A、导入地图和同构无人船的起点终点,定义无人船位置状态向量X(t)和目标状态向量G;
B、定义无人船目标分配矩阵并初始化,该目标分配矩阵用来将N艘无人船从各自起点分配到N个目标位置;
C、计算从任意第i艘船的起点到任意第j个终点的轨迹距离yi,j或di,j得到N艘无人船分配轨迹的距离矩阵D,其中,i=1,2…N;j=1,2…N,矩阵D的第i行第j列元素Di,j∈RN*N;
D、设置无人船避碰约束条件,并根据步骤C中无人船分配距离矩阵D求解最优无人船任务分配矩阵φ*;
E、按步骤D中求出的最优分配矩阵φ*进行任务分配时,计算所有无人船行驶的终止时间tf和最终轨迹y*(t);
步骤C的方法如下:
(1)根据sight(i,j)函数依次判断第i艘船的起点和第j个终点之间是否有障碍物,具体过程如下:
1)先计算第i艘船的起点和第j个终点坐标之间直线的斜率k=(y1-y2)/(x1-x2);
2)若|k|<1,则从起点坐标开始横坐标x依次递减或递增1个单位,即在步骤A中的地图上每隔1个单位取一个点ak,直至到达终点坐标;若|k|>1,则从起点坐标开始纵坐标y依次递减或递增1个单位,即每隔1个单位取一个点ak,直至到达终点坐标,依次判断点ak处的值是否为0;
3)若第2)步中所有的点ak值都为0,则起点i和终点j之间无障碍物,sight()函数返回值为0;若第2)步中存在点ak的值为1,则起点i和终点j之间有障碍物,sight()函数返回值为1;
(2)根据(1)中sight(i,j)函数的返回值,计算第i艘无人船到第j个终点的轨迹距离:当无人船i的起点和终点j之间无障碍物时,直接根据起点和终点间的直线计算欧氏距离,即直线轨迹距离为:di,j=||xi(0)-gj||2,当起点i和终点j之间有障碍物时,根据Theta*算法在二值图中规划出由起点i到达终点j的最优无碰撞轨迹,返回由过程点构成的轨迹yi,j,并计算轨迹距离和为||yi,j||;
其中,根据signt(i,j)函数判断出无人船起点和终点中途有障碍物,返回值为1时,根据Theta*算法求取无人船路径规划轨迹的步骤如下:设当前无人船路径规划的起点为start,目标点为goal,无人船从起始点经过中途任意节点n到达目标点的估价函数为f(n),无人船由起始点到任意节点n的真实距离为g(n),无人船由节点n到目标点过程中的预估距离为h(n);OPEN LIST:将未检查过的节点保存至该列表中;CLOSE LIST:将已检查过的节点保存至该列表中;
1)从无人船的起点start开始,把start作为第一个待检查的节点,加入OPEN LIST列表中;
2)将无人船起点start周围可以到达的路径节点放入OPEN LIST列表中,并将节点start设置为这些节点的父节点,周围可以到达的路径节点最多八个,若有障碍物节点,则不可达;
3)将起点start从OPEN LIST列表中取出,并放入CLOSE LIST列表;
4)计算无人船当前所在节点的每个周围节点的f(n)值,即:
f(n)=g(n)+h(n)
5)从OPEN LIST列表中选择出f(n)值最低的节点a,将节点a从OPEN LIST列表中取出,并放入CLOSE LIST列表;
6)检查当前无人船节点a所有的周边可达节点,排除障碍物节点和已经在CLOSE LIST列表中的节点,搜寻下一个相邻候选节点并考虑以下几点:a)若节点a的周边节点还未加入OPEN LIST列表中,将它们加入OPEN LIST列表中,并计算它们的f(n)、g(n)、h(n)值,并将节点a设置为它们的父节点;
b)如果节点a的某相邻节点c已经在OPEN LIST集合中,计算无人船从起点start经由节点a到达节点c的新的路径的g(n)值,根据该g(n)值判断无人船是否需要更新节点:若无人船从起点start经由节点a到达节点c的新路径的g(n)值比节点c的原g(n)值小,则修改无人船的父节点为节点a,重新计算f(n)值,h(n)不变;若新路径的g(n)值比节点c的原g(n)大,则g(n)不作改变;
7)循环重复第4)、5)、6)步;
8)结束判断:当无人船的终点goal出现在OPEN LIST中时,说明无人船由起点start到终点goal的最短路径已经找到,从终点goal开始,每个节点沿着父节点移动至无人船的起点,形成无人船的路径,返回无人船的轨迹yi,j以及轨迹距离和‖yi,j‖;当OPEN LIST为空时,说明没有找到合适路径;
即得出N*N的距离费用矩阵中的元素di,j为:
N*N的距离矩阵D为:
2.根据权利要求1所述的一种基于CAPT算法改进的无人船集群任务规划方法,其特征在于,步骤A的方法如下:(1)加载二值图格式的有障碍物的地图,将该二值图转换为只包含0和1的矩阵,有障碍物的点为1,无障碍物的点为0,设定无人船的船长为L,也即最小避碰半径,集群无人船规模与无人船的目标位置数量相同为N,需要完成的无人船集群规划任务是寻找将N艘同构无人船从N个起点分配到N个终点的最优分配矩阵和每艘无人船的无碰撞轨迹,并且保证各无人船之间、无人船和障碍物之间无碰撞;读取N艘同构无人船的N个起点和需要分配到的N个终点的坐标,定义第i艘无人船在t=0时刻的初始位置为xi(0)∈Rn,第j个目标位置为gj∈Rn;
(2)定义在t时刻的无人船位置状态向量X(t)和目标状态向量G,设无人船在轨迹行驶过程中t时刻的系统状态向量X(t)=[x1(t)T,x2(t)T,…,xN(t)T],系统目标状态向量G=[g1T,g2T,…,gNT],其中,xi(t)为t时刻第i艘船的位置坐标,T为矩阵转置符号。
3.根据权利要求2所述的一种基于CAPT算法改进的无人船集群任务规划方法,其特征在于,步骤B的方法如下:定义目标分配矩阵将N艘无人船从各自起点分配到N个目标位置:其中目标分配矩阵的第i行第j列的元素表示:如果无人船i从起点xi被分配到第j个目标位置gj,则为1,否则为0;将初始化为大小为N*N的单位矩阵;即:上述矩阵一共有N个1;
定义增广分配矩阵其中,In为N*N的单位矩阵,代表求取克罗内克积。
4.根据权利要求1所述的一种基于CAPT算法改进的无人船集群任务规划方法,其特征在于,步骤D的方法如下:定义δ(t)为任意两艘无人船xi和xj在任意t时刻的最小间距:
其中,i≠j∈N,t∈[t0,tf],t0为起始时间,tf为终止时间,inf函数为取最小值的下界函数,L为无人船模型的长度;为了保证所有无人船的避碰,要求δ(t)始终满足约束条件,即间隙始终大于零:δ(t)>0,t∈[t0,tf]
在无人船集群任务规划中CAPT算法目的是找到能使特定代价函数最小化时的最优轨迹y*(t),即:其中,C(y(t))函数为无人船轨迹的距离代价;
CAPT算法将无人船集群任务规划这一无标号问题看做最优化问题进行求解,确定最优的分配方案和轨迹,最优化目标函数为:其中,Φ为步骤B中的增广分配矩阵,为xi(t)的一阶导数,y(t)为各无人船的轨迹,即寻找最优分配矩阵和最优轨迹,使得各艘无人船的总轨迹距离最低,问题的解由直线轨迹和Theta*规划的中途点连线轨迹组成,且各无人船走过的距离的平方之和最小,根据步骤C中的无人船轨迹距离矩阵求出最佳分配,使得总路径长度最低,所以最优分配方案为:使用匈牙利算法解决最优距离平方分配矩阵φ*的求解问题,具体如下:
匈牙利算法根据无人船距离费用矩阵D求解最优距离平方分配矩阵φ*的步骤为:
1)对N艘无人船任务分配的距离费用矩阵D=(dij)N*N先做行变换,将每一行减去该行的最小值,再做列变换,将每一列减去该列的最小值,出现值为零的元素;
2)用最少的水平线或垂直线将1)中变换后的矩阵D中所有的零元素覆盖;
3)若2)中的水平线和垂直线的总数k比N小,则找到未被覆盖的最小元素a,将未覆盖的每行减去a,被覆盖的每列加上a;
4)重复步骤2)和3),直到k=N;
5)根据零元素所在的位置确定最优任务分配,求解出最佳分配矩阵φ*。
5.根据权利要求4所述的一种基于CAPT算法改进的无人船集群任务规划方法,其特征在于,步骤E的方法如下:所有无人船都是在一定时间内到达目标位置,到达最终位置的终止时间tf为:上式中,vmax为行驶最快的无人船的速度,定义关于时间的多项式函数满足δ(0)=0,δ(tf)=1,其中,k为整数且k>2;
其中,参数α为:
使速度平方积分最小的轨迹是速度恒定且满足始终端约束条件的轨迹:y*(t)=(1-δ(t))X(0)+δ(t)ΦG;
其中,X(0)为初始t=0时刻N艘无人船的位置状态向量。