多水下机器人协同路径规划方法

摘要

本发明涉及一种多水下机器人协同路径规划方法，包括：采用旋转坐标系下离散化处理的方法对规划空间进行建模；给定目标函数以及约束条件，对各水下机器人的路径规划进行最优化建模；根据规划空间模型以及单水下机器人路径规划的最优化模型，求取各水下机器人的最优化路径；根据各水下机器人的最优化路径，确定多水下机器人协同约束条件下的最优指标函数，求取多水下机器人的最优化路径。本发明通过基于改进的最小一致性理论来初步规划各水下机器人的航行路径，在此基础上重新规划各水下机器人路径以满足协同约束条件，本发明的多水下机器人路径规划方法简单可行，路径平滑、优化时间短、效率高。

说明书

技术领域

本发明属于水下机器人导航制导与控制技术领域，尤其涉及一种多水下机器人协同路径规划方法。

背景技术

水下机器人路径规划问题，是指依据已知或实时探测的环境信息，选定优化目标(如路径长度最短、消耗能量最优、或者航行时间最少等)，规划一条从起点到终点的最优路径，且该路径可躲避各类障碍物或威胁，确保航行安全。现有的水下机器人路径规划方法主要借鉴了地面或空中机器人的方法，并结合水下环境特点与机器人性能约束而不断深入，主要包括路标图法、空间分解法、随机规划法、数学规划法、人工势场法、导引法(如几何导引)、行为法(如模糊选择左转/右转等机动行为)等，以及各种方法的组合。其中，空间分解法可处理航行区域内的不规则障碍物，使用灵活方便，它主要包括空间建模与优化求解两步：首先对规划空间进行处理(如构建栅格地图)，从而将路径规划问题建模为典型的优化问题，然后采用数学优化方法(如动态规划法等)或智能优化方法(如遗传算法、粒子群优化等)，寻找一系列连通的单元组成最优路径。然而，现有的空间分解法仍具有路径不平滑、优化时间长等缺陷，需进一步从空间建模或优化求解等角度进行改进。

同时，单个水下机器人的作业能力往往有限，而多个水下机器人系统通过行为协同可显著提升整体的作业效率，但这也增加了水下机器人路径规划问题的难度，因此如何在上述单个水下机器人路径规划问题的基础上结合多水下机器人的协同约束，将是更具有挑战性的研究工作。然而，现阶段在水下机器人领域关于多水下机器人协同路径规划的研究较少，因此，有必要提供一种针对多水下机器人的协同路径规划方法，以规划多水下机器人航行路径。

发明内容

本发明在水下机器人单机路径规划不足的基础上提供了一种多水下机器人协同路径规划方法，基于改进的最小一致性理论来初步规划得到各水下机器人最优路径，在此基础上对各水下机器人最优路径重新规划，以满足多机器人航行时的协同约束条件。

为了实现上述目的，本发明提供了一种多水下机器人协同路径规划方法，包括以下步骤：

(S1)建立规划空间模型：采用旋转坐标系下离散化处理的方法对规划空间进行建模；

(S2)建立单水下机器人路径规划最优化模型：给定目标函数以及约束条件，对各水下机器人的路径规划进行最优化建模；

(S3)根据规划空间模型以及单水下机器人路径规划的最优化模型，求取各水下机器人的最优化路径；

(S4)根据各水下机器人的最优化路径，确定多水下机器人协同约束条件下的最优指标函数，求取多水下机器人的最优化路径。

优选的，步骤(S1)采用旋转坐标系下离散化处理的方法对规划空间进行建模的方法为：

以水下机器人的起点P_s＝(x_s,y_s)与终点P_d＝(x_d,y_d)的连线P_sP_d为x′轴，以P_s点为原点，构建旋转坐标系o′-x′y′；

将连线P_sP_d等分为M+1份，以Δx′＝|P_s-P_d|/(M+1)，构建垂线集合{L_s,L₁,…,L_m,…,L_M,L_d}；

将每条垂线进一步等分为N+1份，将规划空间离散化为旋转坐标系o′-x′y′下的(N+2)×(M+2)个路径点，其中N>>M。

优选的，步骤(S2)给定目标函数以及约束条件，对各水下机器人的路径规划进行最优化建模的方法为：

以minT_r为目标函数，s.t.θ<θ_max,为约束条件，以+∞作为不满足约束条件的罚项，以为罚函数，对各水下机器人的路径规划进行最优化建模；其中，P_m表示离散后的规划空间的任意路径点，θ为水下机器人的转弯角，即任意相邻路径段之间的夹角<P_m-₁P_m,P_mP_m+1>，θ＝<P_m-1P_m,P_mP_m+1>,R_r表示第r艘水下机器人的所有可能路径集合，表示第r艘水下机器人的最优路径，对应最优解为T_r^*。

优选的，步骤(S3)求取各水下机器人的最优化路径的方法为：

基于BMC最小一致性理论对各水下机器人进行路径规划，构建节点图与状态转移方程s_i+1＝f(s_i,u_i)；

通过迭代计算各节点的控制输入值u_i来更新其状态值s_i，使系统状态趋于稳定，获得各水下机器人的最优化路径。

优选的，求取各水下机器人的最优化路径的方法具体为：

在离散后的规划空间(N+2)×(M+2)个路径点中，选取可行域的路径点作为图节点V(满足)，舍弃非可行域S_F内路径点，构建节点图；

将各节点的状态值s_i定义为从当前节点i到终点P_d的目标函数；

以终点P_d为Leader节点且满足s_i(0)＝0，以其他节点为Follower节点且初始状态值设置为大于0的任意值；

定义状态转移方程为：

迭代计算，确定节点i的控制输入变量u_i以及相应的状态值s_i；其中V₁、V₂分别表示Leader节点和Follower节点，N(i)为节点的邻居节点集合，w_ij表示节点i与相邻节点j之间的状态偏差值；

根据公式:

确定到达稳定状态时该水下机器人的最优化目标函数

从起点P_s开始，依次寻找最小的邻居节点作为下一个路径点，直至到达终点P_d，确定该水下机器人的最优化路径

优选的，当起点P_s的本次迭代状态值与上次迭代状态值的绝对差小于设定的阈值时，则判断系统到达稳定状态，停止迭代计算。

优选的，步骤(S4)根据各水下机器人的最优化路径，确定多水下机器人协同约束条件下的最优指标函数，求取多水下机器人的最优化路径的方法为：

根据规划的各水下机器人的最优化路径，确定各水下机器人的最优目标值的集合其中r∈{1,...,N_u}，N_u表示水下机器人数量；

从选取基准值，以最大值为基准值，以为最优指标函数；

根据公式：

修正各水下机器人的最优化规划路径表示重新规划的路径，即可得到多水下机器人的最优化路径。

优选的，修正各水下机器人的最优化路径的方法为：

基于BMC最小一致性理论，对各水下机器人最优化路径重新规划，构建各水下机器人各节点的状态转移方程s'_i+1＝f(s'_i,u'_i)；

将各节点的状态值s’_i定义为从当前节点i到终点P_d的第r艘水下机器人最优目标值s_r^*与第k艘水下机器人最优目标值的差值；

以终点P_d为Leader节点且满足s_i’(0)＝0，以其他节点为Follower节点且初始状态值设置为大于0的任意值；

定义状态转移方程为：

其中，表示|s’_j+w_ij|最小的邻居节点；

迭代计算，确定节点i的控制输入变量u'_i；

根据公式：

确定到达稳定状态时该水下机器人的最优化路径对应的状态值

从起点P_s开始，依次寻找最小的邻居节点作为下一个路径点，直至到达终点P_d，即可将该水下机器人最优化路径重新规划为进而得到多水下机器人的最优化路径。

与现有技术相比，本发明的优点和积极效果在于：

本发明针对水下机器人单机路径规划的不足，提供了一种多水下机器人协同路径规划方法，基于改进的最小一致性理论来初步规划各水下机器人的航行路径，在此基础上重新规划各水下机器人路径以满足协同约束条件。

(1)相比于传统的栅格建模方式，本发明采取坐标旋转、离散化处理的方法对规划空间进行建模，可有效提高后续规划路径的质量。

(2)本发明采取改进的最小一致性理论(BMC)来求解最优化问题，该方法具有很好的环境适应性与鲁棒性，即总能找到最优化问题的最优解，获得单艘水下机器人的最优化路径。

(3)本发明在对各水下机器人的最优化路径规划的基础上，再次根据多水下机器人协同约束条件，采用改进的最小一致性理论(BMC)对各水下机器人的最优化路径修正，得到多水下机器人的最优化路径。本发明的路径规划方法简单可行，路径平滑、优化时间短。

附图说明

图1为本发明的规划空间与路径规划建模示意图；

图2为本发明的邻居节点示意图；

图3为实施例1的规划方法流程图；

图4为上述实施例中单水下机器人的路径规划结果；

图5a为不考虑同时到达的多水下机器人路径规划结果；

图5b为考虑同时到达的多水下机器人的路径规划结果。

具体实施方式

以下，结合附图对本发明的具体实施方式进行进一步的描述。

本发明针对海洋科学研究与海洋工程作业的实时观测需求，改进水下机器人单机路径规划的不足，提供了一种多水下机器人协同路径规划方法，在对规划空间与路径规划问题进行建模的基础上，结合改进的最小一致性理论进行问题求解，初次规划各水下机器人的最优化路径，然后根据多水下机器人的协同约束条件，修正各水下机器人的最优化规划路径，得到多水下机器人协同工作时的最优化路径。

一种多水下机器人协同路径规划方法，包括以下步骤：

(S1)建立规划空间模型：采用旋转坐标系下离散化处理的方法对规划空间进行建模，具体参考图1所示。

①以水下机器人的起点P_s＝(x_s,y_s)与终点P_d＝(x_d,y_d)的连线P_sP_d为x′轴，以P_s点为原点，构建旋转坐标系o′-x′y′。

②将连线P_sP_d等分为M+1份，以Δx′＝|P_s-P_d|/(M+1)，构建垂线集合{L_s,L₁,…,L_m,…,L_M,L_d}，因此只需优化纵坐标y′即可，将大大降低后续算法的复杂度。

③假设纵轴范围等于横轴范围，将每条垂线进一步等分为N+1份，以垂线L_m为例，该垂线可离散化为一系列路径点{P_m,0,P_m,1,...,P_m,N,P_m,N+1}，则该垂线上的最优路径点P_m将从节点集合{P_m,0,P_m,1,...,P_m,N,P_m,N+1}中选择，因此水下机器人的规划空间离散化o′-x′y′坐标系下的(N+2)×(M+2)个路径点。此外，为提高规划路径的平滑度或质量，要求纵轴上的离散采样间隔远远小于横轴，即N>>M。

相比于传统的栅格建模方式，采用旋转坐标、离散化处理的方法对规划空间进行建模，可有效提高后续规划路径的质量。

(S2)建立单水下机器人路径规划最优化模型：给定目标函数以及约束条件，对各水下机器人的路径规划进行最优化建模。

由于水下机器人航行空间内往往存在岩礁、海洋生物等各类型规则或不规则的障碍物，它们统一组成禁航区S_F，即S_F为非可行域，规划路径必须在禁航区域的外部，因此可直接用无穷大的数值作为碰撞情形下的惩罚项。此外，主要考虑最大转弯角这一性能约束，要求水下机器人转弯角θ(即任意相邻路径段间的夹角<P_m-1P_m,P_mP_m+1>)小于最大转弯角θ_max，此时也直接用无穷大的数值作为不满足性能约束情形下的惩罚项。

即：以minT_r为目标函数，s.t.θ<θ_max,为约束条件，以+∞作为不满足约束条件的罚项，以为罚函数，对各水下机器人的路径规划进行最优化建模；其中，P_m表示离散后的规划空间的任意路径点，θ为水下机器人的转弯角，即任意相邻路径段之间的夹角<P_m-1P_m,P_mP_m+1>，θ＝<P_m-1P_m,P_mP_m+1>,R_r表示第r艘水下机器人的所有可能路径集合，表示第r艘水下机器人的最优路径，对应最优解为

(S3)根据规划空间模型以及单水下机器人路径规划的最优化模型，求取各水下机器人的最优化路径。即：基于BMC最小一致性理论对各水下机器人进行路径规划，构建节点图与状态转移方程s_i+1＝f(s_i,u_i)；通过迭代计算各节点的控制输入值u_i来更新其状态值s_i，使系统状态趋于稳定，获得各水下机器人的最优化路径。

具体的：

①在离散后的规划空间(N+2)×(M+2)个路径点中，选取可行域的路径点作为图节点V(满足)，舍弃非可行域S_F内路径点，构建节点图。

②参考图2所示，将各节点的状态值s_i定义为从当前节点i到终点P_d的目标函数，某垂线上的节点的邻居节点分布在下一条垂线上且应满足性能约束，以垂线L_m上的某节点P_m,n为例，它的邻居节点{P_m+1,n-Q,...,P_m+1,n,...,P_m+1,n+Q}均在垂线L_m+1上。以终点P_d为Leader节点且满足s_i(0)＝0，以其他节点为Follower节点且初始状态值设置为大于0的任意值。

③定义状态转移方程为：

迭代计算，确定节点i的控制输入变量u_i以及相应的状态值s_i；其中V₁、V₂分别表示Leader节点和Follower节点，N(i)为节点的邻居节点集合，w_ij表示节点i与相邻节点j之间的状态偏差值。

当起点P_s的本次迭代状态值与上次迭代状态值的绝对差小于设定的阈值时，则判断系统到达稳定状态，停止迭代计算。系统到达稳定状态时，地图上各个节点i的状态值均到达稳定。

④根据公式:

确定到达稳定状态时最优化目标函数

⑥从起点P_s开始，依次寻找最小的邻居节点作为下一个路径点，直至到达终点P_d，确定该水下机器人的最优化路径此时表示起始节点P_s的状态值。进而可确定出各个水下机器人的最优化路径。

(S4)根据各水下机器人的最优化路径，确定多水下机器人协同约束条件下的最优指标函数，求取多水下机器人的最优化路径。

①根据规划的各水下机器人的最优化路径，确定各水下机器人的最优目标值的集合其中r∈{1,...,N_u}，N_u表示水下机器人数量。

②从选取基准值，即以最大值为基准值，以为最优指标函数。

③根据公式：

修正各水下机器人的最优化规划路径表示重新规划的路径，即可得到多水下机器人的最优化路径。

具体的，进一步采用BMC最小一致性理论求解上式，对各水下机器人最优化路径重新规划，以第r艘水下机器人为例，构建该水下机器人维护的地图各节点的状态转移方程s'_i+1＝f(s'_i,u'_i)。将各节点的状态值s_i’定义为从当前节点i到终点P_d的第r艘水下机器人最优目标值s_r^*与第k艘水下机器人最优目标值的差值；以终点P_d为Leader节点且满足s’_i(0)＝0，以其他节点为Follower节点且初始状态值设置为大于0的任意值。定义状态转移方程为：

其中，表示|s'_j+w_ij|最小的邻居节点。

迭代计算，确定节点i的控制输入变量u'_i；

根据公式：

确定到达稳定状态时该水下机器人的最优化路径对应的状态值

从起点P_s开始，依次寻找最小的邻居节点作为下一个路径点，直至到达终点P_d，即可将该水下机器人最优化路径重新规划为进而得到多水下机器人的最优化路径。

实施例1：

在多水下机器人进行编队航行或执行饱和攻击等任务前，往往需要它们同时到达(集结)期望目标点，此时需考虑同时到达这一时间协同约束以规划多机器人的路径。本实施例将考虑各水下机器人同时到达终点情况时多水下机器人路径规划问题，将上述路径规划方法具体应用到本实施例中，对规划路径进行调整，使得各水下机器人的预计到达时间趋近一致。

参考图3所示，假设共有N_u艘水下机器人协同工作，具体以第r艘水下机器人的路径规划问题为例，以最小化航行时间min>r为指标函数，以θ<θ_max,为约束条件，以+∞作为不满足约束条件的罚项，将单艘水下机器人的路径规划问题建模为最优化问题。

由于洋流场往往会影响水下机器人的运动速度，假设洋流场模型分辨率小于规划空间的建模分辨率，即在任意路径段下的洋流速度保持恒定。水下机器人沿路径段的绝对速度V_a等于机器人的相对速度V_r与洋流速度V_c的矢量和，根据余弦定理可求解|V_a|：

|V_a|²+|V_c|²-2|V_a||V_c|cos<V_c,V_a>＝|V_r|²>

其中<V_c,V_a>为路径段向量与洋流速度向量的夹角，假设水下机器人推力恒定且已知|V_r|＝c，洋流速度V_c已知。因此，第r艘水下机器人沿其规划路径航行所需时间为：

其中P₀表示起点P_s，P_M+1表示终点P_d。因此，第r艘水下机器人的路径规划问题可建模为最优化问题，即寻找具有最短航行时间min>r的路径：

R_r表示第r艘机器人的所有可能路径集合，表示第r艘机器人的最优路径且最短航行时间为T_r^*。

然后采用改进的最小一次性理论(BMC)求解最优化路径，将s_i定义为从当前节点i到终点P_d的航行时间T_i，边(i,j)的连接权重w_ij表示该水下机器人沿该边航行所需的时间。利用公式(1)迭代计算各节点的控制输入值并更新状态值，则系统的各节点最终将到达稳定状态：

其中表示从节点i到终点的最短航行时间，则起点的稳定状态值即为从起点到终点的最短航行时间T_r^*。最后，从起点开始，依次寻找下一步最优节点作为下一个路径点，直至到达终点，即完成第r艘机器人的最优路径规划。同理，对N_u艘水下机器人均进行最优路径规划。

图4表示分别采取改进的最小一致性理论(BMC)、快速扩展随机树(RRT)、粒子群优化(PSO)等方法的水下机器人路径规划结果，虽然三种方法均能规划出避开障碍物的安全路径，但BMC方法规划的路径更平滑，且路径形状在一定程度上随着洋流方向变化，且水下机器人航行时间更短(采取三种方法的航行时间分别为：811s,941s,899s)。

为满足多机间的同时到达终点的约束，需重新规划各水下机器人的路径以减少各机航行时间的差距。本实施例中，根据规划的各水下机器人的最优化路径，确定所有水下机器人的最短航行时间T_r^*的集合{T_r^*}。从{T_r^*}选取最大值为基准，将考虑同时到达的多机协同路径规划问题简化为以最小化机间航行时间差为指标的最优化问题。在此基础上，为满足多机间的同时到达约束，需对最短航行时间为最大值之外的其他水下机器人(r≠k)的路径重新规划，以减少各机航行时间的差距：即

然后利用改进的最小一致性理论(BMC)来求解上述最优化问题，重新定义图节点的状态值与各边权重值。以第r艘水下机器人为例，假设规划路径分为M+1段，将边(i,j)的连接权重w_ij定义为水下机器人沿该边航行的实际时间|P_i-P_j|/|V_a|与基准时间T_k^*/(M+1)的差值：

其中P_i,P_j分别表示节点i,j的坐标。

对各水下机器人最优化路径重新规划，然后结合公式(4)、(5)、(10)、(11),可实现机间航行时间差的最小化即从起点P_s开始，依次寻找最小的邻居节点作为下一个路径点，直至到达终点P_d，即可将该水下机器人最优化路径重新规划为进而得到考虑各水下机器人同时到达终点情况时多水下机器人最优化路径。

图5a与图5b表示某海域下的仿真场景，图5a为如果不考虑同时到达约束时3艘水下机器人的路径；图5b为考虑同时到达约束时3艘水下机器人的路径。由图5a、图5b可以看出，未考虑同时到达时，3艘水下机器人的最优路径之间存在较大时间偏差(64.12h,61.03h,62.87h)，而考虑同时到达约束后重新规划的路径航行时间将趋于一致(64.12h,64.14h,64.09h)。

由此可知，本实施例通过采取改进的最小一致性理论来求解最优化问题，总能找到最优化问题的最优解，获得单艘机器人的航行时间最短路径；在各水下机器人航行时间最短路径规划的基础上，再次采用改进的最小一致性理论来重新规划各机器人的路径，从而使得各机器人的航行时间趋于一致，且方法简单，优化时间短、精确度高。

需要注意的是，本发明中的多水下机器人协同路径规划方法并非仅限于各水下机器人同时达到情况下的协同路径规划，目标函数也不仅限于航行时间最少的最优路径，可根据实际需要，拓展到例如距离最短路径等其他优化目标的最优化路径规划问题。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。