基于碰撞检测模型和人工势场法的AUV局部避障方法与流程

技术特征：
1.一种基于碰撞检测模型和人工势场法的auv局部避障方法，其特征在于：所述的基于碰撞检测模型和人工势场法的auv局部避障方法包括按顺序进行的下列步骤：步骤一：根据全局路径建立三维环境下的碰撞检测模型，用于检测任意两个路径点所构成的路径段是否与三维地形发生碰撞；以两个路径点的三个坐标分量之间距离最大的坐标做路径探测，从起始坐标点开始，每次增加特定步长坐标值，依次向目的坐标点逐渐试探；如果某一次试探出路径点在z轴上的坐标分量不高于路径点所对应三维地形的高度值，则输出两路径点连线与三维地形发生碰撞；如果从始至终路径点在z轴上的坐标分量都保持高于路径点所对应三维地形的高度值，则输出两路径点连线不与三维地形发生碰撞；步骤二：在全局路径已知的情况下，利用碰撞检测模型筛选全局路径中的关键路径点；步骤三：利用关键路径点做为局部子目标点并且在人工势场法的引导下进行局部避障；当auv未进入障碍物的斥力场时，auv沿着全局路径正常行驶；当auv进入障碍物的斥力场时，auv就会以当前路径点为起点，首个未经过的关键路径点为局部子目标点，切换到人工势场法进行局部避障操作直至auv抵达局部子目标点；当auv抵达局部子目标点后，auv继续沿着全局路径正常行驶。2.根据权利要求1所述的基于碰撞检测模型和人工势场法的auv局部避障方法，其特征在于，所述步骤一中三维环境下的碰撞检测模型的具体过程为：假设两路径点坐标分别为p
i
(x
i
,y
i
,z
i
)，p
i 1
(x
i 1
,y
i 1
,z
i 1
)，则得到两路径点连线的线性参数方程为：性参数方程为：公式(1)到公式(2)为两路径点连线的线性参数方程的计算过程；其中t为参数变量，x
i
,y
i
,z
i
分别为路径点p
i
在x轴、y轴、z轴上的坐标分量，x
i 1
,y
i 1
,z
i 1
分别为路径点p
i 1
在x轴、y轴、z轴上的坐标分量，a,b,c为中间变量；则x,y,z都可由参数变量t表示，即：令dz＝|z
i 1
‑
z
i
|，dy＝|y
i 1
‑
y
i
|，dx＝|x
i 1
‑
x
i
|；如果dz等于max{dx,dy,dz}，则用z坐标做探测；如果dy等于max{dx,dy,dz}，则用y坐标做探测；如果dx等于max{dx,dy,dz}，则用x坐标做探测；其中max{dx,dy,dz}表示dx,dy,dz中的最大值；假设dx最大,即采用x坐标做探测，则有以下流程：1.1令x1＝min{x
i 1
，x
i
}，x2＝max{x
i 1
，x
i
}；其中min{x
i 1
，x
i
}表示x
i 1
，x
i
中的最小值，max{x
i 1
，x
i
}表示x
i 1
，x
i
中的最大值；1.2若x1<x2，则x1＝x1 c1并转入步骤1.3，否则试探过程结束，输出两路径点连线不与三维地形发生碰撞；其中，c1为探测步长且为固定常数；1.3利用公式(3)求解x1对应的y1和z1，可得对应的坐标(x1,y1,z1)；如果z1>high(x1,y1)，则转入步骤1.2；如果z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞；其
中，high(x1,y1)表示(x1,y1)所对应三维地形的高度值；假设dy最大,即采用y坐标做探测，则有以下流程：1.4令y1＝min{y
i 1
，y
i
}，y2＝max{y
i 1
，y
i
}；其中min{y
i 1
，y
i
}表示y
i 1
，y
i
中的最小值，max{y
i 1
，y
i
}表示y
i 1
，y
i
中的最大值；1.5若y1<y2，则y1＝y1 c1并转入步骤1.6，否则试探过程结束，输出两路径点连线不与三维地形发生碰撞；其中，c1为探测步长且为固定常数；1.6利用公式(3)求解y1对应的x1和z1，可得对应的坐标(x1,y1,z1)；如果z1>high(x1,y1)，则转入步骤1.5；如果z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞；其中，high(x1,y1)表示(x1,y1)所对应三维地形的高度值；假设dz最大,即采用z坐标做探测，则有以下流程：1.7令z1＝min{z
i 1
，z
i
}，z2＝max{z
i 1
，z
i
}，其中min{z
i 1
，z
i
}表示z
i 1
，z
i
中的最小值，max{z
i 1
，z
i
}表示z
i 1
，z
i
中的最大值；1.8若z1<z2，则z1＝z1 c1并转入步骤1.9，否则试探过程结束，输出两路径点连线不与三维地形发生碰撞；其中，c1为探测步长且为固定常数；1.9利用公式(3)求解z1对应的x1和y1，可得对应的坐标(x1,y1,z1)；如果z1>high(x1,y1)，则转入步骤1.8；如果z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞；其中，high(x1,y1)表示(x1,y1)所对应三维地形的高度值。3.根据权利要求1所述的基于碰撞检测模型和人工势场法的auv局部避障方法，其特征在于，所述步骤二中利用碰撞检测模型筛选全局路径中的关键路径点的具体过程为：假设某条全局路径包含m个路径点，key_path_point为存储的关键路径点的集合；point_i表示全局路径中的第i个路径点，point_j表示全局路径中的第j个路径点；则该条全局路径的关键路径点获取流程如下所示：2.1令i＝1，j＝2；2.2通过碰撞检测模型来判断point_i与point_j所构成的线性路径段是否与三维地形发生碰撞；如果该路径段与三维地形发生了碰撞，则转入步骤2.3；如果该路径段没有与三维地形发生碰撞，则转入步骤2.4；2.3将point_i存入key_path_point中，然后令i＝j
‑
1；转入步骤2.5；2.4令temp＝j 1；转入步骤2.5；其中，temp充当中间变量；2.5判断point_j是否为终点；如果point_j是终点，则先将point_i存入key_path_point中，再将point_j存入key_path_point中，转入步骤2.6；如果point_j不是终点，则令j＝temp，转入步骤2.2；2.6关键路径点寻找完毕，输出key_path_point，流程结束。4.根据权利要求1所述的基于碰撞检测模型和人工势场法的auv局部避障方法，其特征在于，所述步骤三中利用关键路径点做为局部子目标点并且在人工势场法的引导下进行局部避障的具体过程为：3.1判断auv当前所处位置是否位于动态障碍物的斥力场内；如果auv不在动态障碍物的斥力场内，则转入步骤3.2；如果auv在动态障碍物的斥力场内，则转入步骤3.3；3.2 auv沿着全局路径继续行驶至下一路径点；转入步骤3.5；3.3 auv以当前路径点为起点，首个未经过的关键路径点为局部子目标点，切换到人工
势场法，然后行驶至下一路径点；转入步骤3.4；3.4判断auv是否行驶至局部子目标点；如果auv行驶至局部子目标点，则转入步骤3.5；如果auv没有行驶至局部子目标点，则转入步骤3.3；3.5判断auv当前所处位置是否为终点，如果auv当前所处位置不是终点，则转入步骤3.1，如果auv当前所处位置是终点，则流程结束，输出auv成功避开动态障碍物并抵达终点。5.根据权利要求4所述的基于碰撞检测模型和人工势场法的auv局部避障方法，其特征在于，所述步骤3.3中人工势场法的具体执行过程为：3.3.1判断auv是否到达局部子目标点，若auv未到达局部子目标点，则转入步骤3.3.2；若auv已经到达局部子目标点，则流程结束，输出auv已经到达局部子目标点；3.3.2判断auv当前位置是否陷入局部极小值；若auv当前位置已经陷入局部极小值，则采用随机点法使得auv跳出局部极小值，然后转入步骤3.3.1；若auv当前位置未陷入局部极小值，则转入步骤3.3.3；3.3.3计算auv受到局部子目标点产生的引力，计算auv受到动态障碍物产生的斥力，计算auv所受到的合力；转入步骤3.3.4；其中：auv受到局部子目标点产生的引力大小的计算公式如下：|f
att
|＝k1*dis(p,g)
w1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)其中，f
att
为auv受到局部子目标点产生的引力；|f
att
|为auv受到局部子目标点产生的引力的大小；f
att
为矢量；f
att
的大小为|f
att
|且f
att
的方向由auv当前位置指向局部子目标点的位置；k1为引力增益系数且为固定常数；p表示auv当前位置，g表示局部子目标点的位置，dis(p,g)表示auv当前位置与局部子目标点的位置之间的距离；w1为距离权重系数且为固定常数；auv受到动态障碍物产生的斥力大小的计算公式如下：其中，f
rep
为auv受到动态障碍物产生的斥力；|f
rep
|为auv受到动态障碍物产生的斥力的大小；f
rep
为矢量；f
rep
的大小为|f
rep
|且f
rep
的方向由动态障碍物当前位置指向auv当前位置；k2为斥力增益系数且为固定常数；p表示auv当前位置，g表示局部子目标点的位置，o表示动态障碍物当前位置，dis(p,g)表示auv当前位置与局部子目标点的位置之间的距离，dis(p,o)表示auv当前位置与动态障碍物当前位置之间的距离；w2、w3均为距离权重系数且为固定常数；r为动态障碍物的斥力场半径且为固定常数；auv所受到的合力的计算公式如下：其中，f
sum
为auv受到的合力；f
sum
的方向为auv受到的引力与auv受到的所有斥力的矢量和；f
att
为auv受到局部子目标点产生的引力；f
rep
为auv受到动态障碍物产生的斥力；n为三维环境中动态障碍物的数量，i表示第几个动态障碍物；3.3.4auv在合力的方向上按照既定步长移动行驶至下一个路径点；转入步骤3.3.1。6.根据权利要求5所述的基于碰撞检测模型和人工势场法的auv局部避障方法，其特征
在于，所述步骤3.3.2中局部极小值检测的具体过程为：假设auv当前位置为路径中的第k个路径点，i＝n表示路径中的第n个路径点，count为圆内的auv已经经过的路径点的个数；则局部极小值的判定流程如下：3.3.2.1令i＝1，count＝0；3.3.2.2如果第i个路径点到第k个路径点之间的距离小于等于r1，则转入步骤3.3.2.3；如果第i个路径点到第k个路径点之间的距离大于r1，则转入步骤3.3.2.4；3.3.2.3令count＝count 1；如果count的值达到阈值ε2，则流程结束，输出auv当前位置已经陷入局部极小值；如果count的值未达到阈值ε2，则转入步骤3.3.2.4；3.3.2.4令i＝i 1；如果i>k，则流程结束，输出auv当前位置未陷入局部极小值；如果i≤m，则转入步骤3.3.2.2。7.根据权利要求5所述的基于碰撞检测模型和人工势场法的auv局部避障方法，其特征在于，所述步骤3.3.2中采用随机点法使得auv跳出局部极小值的具体过程为：3.3.2.5在三维环境中随机取一个非障碍物坐标点充当局部子目标点，同时暂时摒弃原有的局部子目标点；3.3.2.6由于局部子目标点发生了变化，因此合力的方向也会发生变化；auv沿着新的合力方向按照既定步长移动到下一个路径点；3.3.2.7判断auv当前位置是否陷入局部极小值；如果auv当前位置仍处于局部极小值，则转入步骤3.3.2.5；如果auv当前位置不处于局部极小值，则流程结束，输出auv已经跳出局部极小值，同时把首个未经过的关键路径点做为当前auv的局部子目标点。8.根据权利要求5所述的基于碰撞检测模型和人工势场法的auv局部避障方法，其特征在于，所述步骤3.3.4中auv在合力的方向上按照既定步长移动行驶至下一个路径点的具体过程为：3.3.4.1计算出auv受到的引力f
att
在x轴、y轴、z轴上的各引力分量；在x轴、y轴、z轴上的各引力分量；其中α1、β1、γ1为矢量pg的三个方向角；cosα1、cosβ1、cosγ1为矢量pg的方向余弦；f
attx
、f
atty
、f
attz
分别为f
att
在x轴、y轴、z轴上的各引力分量；px、py、pz分别为auv当前位置p在x轴、y轴、z轴上的坐标分量；gx、gy、gz分别为局部子目标点的位置g在x轴、y轴、z轴上的坐标分量；3.3.4.2计算出auv受到的斥力f
rep
在x轴、y轴、z轴上的各斥力分量；
其中α2、β2、γ2为矢量op的三个方向角；cosα2、cosβ2、cosγ2为矢量op的方向余弦；f
repx
、f
repy
、f
repz
分别为f
rep
在x轴、y轴、z轴上的各斥力分量；px、py、pz分别为auv当前位置p在x轴、y轴、z轴上的坐标分量；ox、oy、oz分别为动态障碍物当前位置o在x轴、y轴、z轴上的坐标分量；3.3.4.3计算出auv受到的合力f
sum
在x轴、y轴、z轴上的各合力分量；其中，f
sumx
、f
sumy
、f
sumz
分别为f
sum
在x轴、y轴、z轴上的各合力分量；n为三维环境中动态障碍物的数量，i表示第几个动态障碍物；3.3.4.4计算出auv的下一个路径点的坐标；3.3.4.4计算出auv的下一个路径点的坐标；其中α3、β3、γ3为矢量f
sum
的三个方向角；cosα3、cosβ3、cosγ3为矢量f
sum
的方向余弦；p1(px1,py1,pz1)为auv的下一个路径点的坐标；p(px,py,pz)为auv的当前路径点的坐标；l为auv的移动步长且为固定常数。9.根据权利要求4所述的基于碰撞检测模型和人工势场法的auv局部避障方法，其特征在于，所述步骤3.4中判断auv是否行驶至局部子目标点的具体过程为：
当auv当前位置与局部子目标点的距离小于某个阈值时，判定auv到达局部子目标点；假设auv当前位置的坐标为p(px,py,pz)，局部子目标点的位置的坐标为g(gx,gy,gz)，则判定公式如下：(gx
‑
px)2 (gy
‑
py)2 (gz
‑
pz)2≤ε1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)其中，ε1为判定阈值且为固定常数；px、py、pz分别为auv当前位置p在x轴、y轴、z轴上的坐标分量；gx、gy、gz分别为局部子目标点的位置g在x轴、y轴、z轴上的坐标分量。

技术总结
一种基于碰撞检测模型和人工势场法的AUV局部避障方法,其包括利用碰撞检测模型筛选全局路径中的关键路径点；利用关键路径点做为局部子目标点并且在人工势场法的引导下进行局部避障等步骤。当AUV未进入障碍物的斥力场时，AUV沿着全局路径正常行驶，当AUV进入障碍物的斥力场时，AUV就会以当前路径点为起点，首个未经过的关键路径点为局部子目标点，切换到人工势场法进行局部避障操作直至AUV抵达局部子目标点。本方法不仅能够使AUV有效的避开各类动态、静态障碍物，而且还能够有效避免由于避障行为导致的AUV能耗突增的现象,使得AUV尽可能地沿着全局路径行驶以保证规划路径的质量同时提高AUV避障的实时性。时提高AUV避障的实时性。时提高AUV避障的实时性。


技术研发人员：郝琨 赵家乐 王贝贝
受保护的技术使用者：天津城建大学
技术研发日：2021.07.27
技术公布日：2021/10/8