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

基于碰撞检测模型和人工势场法的auv局部避障方法
技术领域
1.本发明属于auv局部避障领域，特别是涉及一种基于碰撞检测模型和人工势场法的auv局部避障方法。


背景技术：

2.近些年来，随着科技的不断发展，人们对海洋的利用和认知正在不断的增加。做为执行水下任务的重要工具，自主式水下潜航器(autonomous underwater vehicle，auv)具备高度的灵活性和自主性。因此auv已经被广泛的应用于海洋资源勘探、海洋环境监测、水下军事行动等各类任务中。由于海洋环境的复杂性、不确定性和未知性，只考虑静态环境下的全局路径规划是远远不够的。海洋中存在的大量静态障碍物、动态障碍物以及突发性障碍物都会对auv事先规划好的全局路径造成极大威胁。因此这就需要我们采取一种措施去规避这些静态障碍物、动态障碍物以及突发性障碍物。
3.传统的避障方式是采用剩余路径重规划策略。即当auv预测到即将与障碍物发生碰撞时，auv会以当前位置为起点，预测到的碰撞点为静态障碍物，终点不变，重新规划一条绕开碰撞点的路径。但是此类避障方式需要耗费大量的时间重新规划一条路径，实时性不强，难以适应地形复杂、动态障碍物多的海洋环境。避障方法首先应当考虑的是实时性。在传统避障方式的基础上，有许多学者提出了不同的避障策略。例如，许凯波等人将碰撞的类型分为正面碰撞侧面、侧面碰撞、追及碰撞等类型。针对不同的碰撞方式采用局部路径重规划、等待、转向、倒退等避障策略。这类避障策略都拥有较高的实时性。但是这类碰撞策略不一定能将所有的碰撞类型都考虑完整，可能会存在碰撞类型考虑不全等情况，这就会导致潜在的碰撞风险。此外这类碰撞策略是建立在能够预测到动态障碍物的运动轨迹的基础上的。对于无规律运动的动态障碍物，这类碰撞策略将完全失效。还有些学者采用全局路径规划算法和局部路径规划算法相结合的方式进行局部避障。例如liang等人采用遗传算法与a*算法相结合的方式进行局部避障。先利用遗传算法生成全局路径，当auv遇到动态障碍物时，再切换到a*算法生成后续路径进行避障处理。但是该类方法也有一定的缺陷：第一，算法的切换只能由全局路径规划算法向局部路径规划算法单向切换。这种操作尽管能够避开动态障碍物，但是其规划路径的质量(包括路径长度、路径平滑度、路径安全性、auv能耗)将大幅降低。第二，对于无规律运动的动态障碍物，这种操作未必能够起作用。
4.auv避障策略需要auv在能够避开静态障碍物、动态障碍物以及突发性障碍物的前提下，使得auv尽可能地沿着全局路径行驶以保证规划路径的质量同时提高auv避障的实时性。


技术实现要素：

5.为了解决上述问题，本发明的目的在于提供一种基于碰撞检测模型和人工势场法的auv局部避障方法。其包括利用碰撞检测模型筛选全局路径中的关键路径点；利用关键路径点做为局部子目标点并且在人工势场法的引导下进行局部避障等步骤。
6.为了达到上述目的，本发明是采用下述技术方案实现的：
7.一种基于碰撞检测模型和人工势场法的auv局部避障方法，包括按顺序进行的下列步骤：
8.步骤一：根据全局路径建立三维环境下的碰撞检测模型，用于检测任意两个路径点所构成的路径段是否与三维地形发生碰撞；以两个路径点的三个坐标分量之间距离最大的坐标做路径探测，从起始坐标点开始，每次增加特定步长坐标值，依次向目的坐标点逐渐试探；如果某一次试探出路径点在z轴上的坐标分量不高于路径点所对应三维地形的高度值，则输出两路径点连线与三维地形发生碰撞；如果从始至终路径点在z轴上的坐标分量都保持高于路径点所对应三维地形的高度值，则输出两路径点连线不与三维地形发生碰撞；
9.步骤二：在全局路径已知的情况下，利用碰撞检测模型筛选全局路径中的关键路径点；关键路径点是指用于支撑起全局路径的不可或缺的核心路径节点。这意味着在全局路径中仅仅通过依次连接关键路径点就可以构造出一条高质量的可行路径，而非关键路径点的一些普通路径节点仅仅起到填充路径的作用。
10.步骤三：利用关键路径点做为局部子目标点并且在人工势场法的引导下进行局部避障；其设计思路是通过三维环境下的碰撞检测模型筛选出已知的全局路径中的关键路径点。当auv未进入障碍物的斥力场时，auv沿着全局路径正常行驶。当auv进入障碍物的斥力场时，auv就会以当前路径点为起点，首个未经过的关键路径点为局部子目标点，切换到人工势场法进行局部避障操作直至auv抵达局部子目标点。当auv抵达局部子目标点后，auv继续沿着全局路径正常行驶。
11.上述技术方案中，所述步骤一中三维环境下的碰撞检测模型的具体过程为：
12.假设两路径点坐标分别为p
i
(x
i
,y
i
,z
i
)，p
i 1
(x
i 1
,y
i 1
,z
i 1
)，则可以得到两路径点连线的线性参数方程为
[0013][0014][0015]
公式(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表示，即：
[0016][0017]
令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中的最大值。
[0018]
假设dx最大,即采用x坐标做探测，则有以下流程：
[0019]
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
中的最大值。
[0020]
1.2若x1<x2，则x1＝x1 c1并转入步骤1.3，否则试探过程结束，输出两路径点连线
不与三维地形发生碰撞。其中，c1为探测步长且为固定常数。
[0021]
1.3利用公式(3)求解x1对应的y1和z1，可得对应的坐标(x1,y1,z1)。如果z1>high(x1,y1)，则转入步骤1.2。如果z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞。其中，high(x1,y1)表示(x1,y1)所对应三维地形的高度值。
[0022]
假设dy最大,即采用y坐标做探测，则有以下流程：
[0023]
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
中的最大值。
[0024]
1.5若y1<y2，则y1＝y1 c1并转入步骤1.6，否则试探过程结束，输出两路径点连线不与三维地形发生碰撞。其中，c1为探测步长且为固定常数。
[0025]
1.6利用公式(3)求解y1对应的x1和z1，可得对应的坐标(x1,y1,z1)。如果z1>high(x1,y1)，则转入步骤1.5。如果z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞。其中，high(x1,y1)表示(x1,y1)所对应三维地形的高度值。
[0026]
假设dz最大,即采用z坐标做探测，则有以下流程：
[0027]
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
中的最大值。
[0028]
1.8若z1<z2，则z1＝z1 c1并转入步骤1.9，否则试探过程结束，输出两路径点连线不与三维地形发生碰撞。其中，c1为探测步长且为固定常数。
[0029]
1.9利用公式(3)求解z1对应的x1和y1，可得对应的坐标(x1,y1,z1)。如果z1>high(x1,y1)，则转入步骤1.8。如果z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞。其中，high(x1,y1)表示(x1,y1)所对应三维地形的高度值。
[0030]
从碰撞检测模型的流程可以看出，碰撞检测模型的实质是从起始坐标点开始，每次增加c1坐标值，依次向目的坐标点逐渐试探。如果某一次试探出z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞。如果从始至终z1都保持z1>high(x1,y1)，则输出两路径点连线不与三维地形发生碰撞。
[0031]
上述技术方案中，所述步骤二中利用碰撞检测模型筛选全局路径中的关键路径点的具体过程为：
[0032]
假设某条全局路径包含m个路径点，key_path_point为存储的关键路径点的集合。point_i表示全局路径中的第i个路径点，point_j表示全局路径中的第j个路径点。则该条全局路径的关键路径点获取流程如下所示：
[0033]
2.1令i＝1，j＝2。
[0034]
2.2通过碰撞检测模型来判断point_i与point_j所构成的线性路径段是否与三维地形发生碰撞。如果该路径段与三维地形发生了碰撞，则转入步骤2.3。如果该路径段没有与三维地形发生碰撞，则转入步骤2.4。
[0035]
2.3将point_i存入key_path_point中，然后令i＝j
‑
1。转入步骤2.5。
[0036]
2.4令temp＝j 1。转入步骤2.5。其中，temp充当中间变量。
[0037]
2.5判断point_j是否为终点。如果point_j是终点，则先将point_i存入key_path_point中，再将point_j存入key_path_point中，转入步骤2.6。如果point_j不是终点，则令j＝temp，转入步骤2.2。
[0038]
2.6关键路径点寻找完毕，输出key_path_point，流程结束。
[0039]
上述技术方案中，所述步骤三中利用关键路径点做为局部子目标点并且在人工势场法的引导下进行局部避障的具体过程为：
[0040]
3.1判断auv当前所处位置是否位于动态障碍物的斥力场内。如果auv不在动态障碍物的斥力场内，则转入步骤3.2。如果auv在动态障碍物的斥力场内，则转入步骤3.3。
[0041]
3.2auv沿着全局路径继续行驶至下一路径点。转入步骤3.5。
[0042]
3.3auv以当前路径点为起点，首个未经过的关键路径点为局部子目标点，切换到人工势场法，然后行驶至下一路径点。转入步骤3.4。
[0043]
3.4判断auv是否行驶至局部子目标点。如果auv行驶至局部子目标点，则转入步骤3.5。如果auv没有行驶至局部子目标点，则转入步骤3.3。
[0044]
3.5判断auv当前所处位置是否为终点。如果auv当前所处位置不是终点，则转入步骤3.1。如果auv当前所处位置是终点，则流程结束，输出auv成功避开动态障碍物并抵达终点。
[0045]
上述技术方案中，所述步骤3.3中人工势场法的具体执行过程为：
[0046]
3.3.1判断auv是否到达局部子目标点。若auv未到达局部子目标点，则转入步骤3.3.2。若auv已经到达局部子目标点，则流程结束，输出auv已经到达局部子目标点。
[0047]
3.3.2判断auv当前位置是否陷入局部极小值。若auv当前位置已经陷入局部极小值，则采用随机点法使得auv跳出局部极小值，然后转入步骤3.3.1。若auv当前位置未陷入局部极小值，则转入步骤3.3.3。
[0048]
3.3.3计算auv受到局部子目标点产生的引力，计算auv受到动态障碍物产生的斥力，计算auv所受到的合力。转入步骤3.3.4。其中：auv受到局部子目标点产生的引力大小的计算公式如下：
[0049]
|f
att
|＝k1*dis(p,g)
w1
ꢀꢀꢀ
(4)
[0050]
其中，f
att
为auv受到局部子目标点产生的引力。|f
att
|为auv受到局部子目标点产生的引力的大小。f
att
为矢量。f
att
的大小为|f
att
|且f
att
的方向由auv当前位置指向局部子目标点的位置。k1为引力增益系数且为固定常数。p表示auv当前位置，g表示局部子目标点的位置，dis(p,g)表示auv当前位置与局部子目标点的位置之间的距离。w1为距离权重系数且为固定常数。
[0051]
auv受到动态障碍物产生的斥力大小的计算公式如下：
[0052][0053]
其中，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为动态障碍物的斥力场半径且为固定常数。
[0054]
auv受到的合力的计算公式如下：
[0055][0056]
其中，f
sum
为auv受到的合力。f
sum
的方向为auv受到的引力与auv受到的所有斥力的矢量和。f
att
为auv受到局部子目标点产生的引力。f
rep
为auv受到动态障碍物产生的斥力。n为三维环境中动态障碍物的数量，i表示第几个动态障碍物。
[0057]
3.3.4auv在合力的方向上按照既定步长移动行驶至下一个路径点。转入步骤3.3.1。
[0058]
上述技术方案中，所述步骤3.3.2中局部极小值的定义及局部极小值检测的具体过程为：
[0059]
局部极小值是指在合力的作用下，auv在某一小范围区域内反复震荡从而导致auv无法到达或者延迟到达局部子目标点的现象。局部极小值的判定标准如下：以auv当前位置为圆心，r1为半径做圆。如果在该圆内的auv已经经过的路径点的个数达到某个阈值ε2，则可判定auv陷入局部极小值。否则，可以判定auv未陷入局部极小值。假设auv当前位置为路径中的第k个路径点，i＝n表示路径中的第n个路径点，count为圆内的auv已经经过的路径点的个数。则局部极小值的判定流程如下：
[0060]
3.3.2.1令i＝1，count＝0。
[0061]
3.3.2.2如果第i个路径点到第k个路径点之间的距离小于等于r1，则转入步骤3.3.2.3。如果第i个路径点到第k个路径点之间的距离大于r1，则转入步骤3.3.2.4。
[0062]
3.3.2.3令count＝count 1。如果count的值达到阈值ε2，则流程结束，输出auv当前位置已经陷入局部极小值。如果count的值未达到阈值ε2，则转入步骤3.3.2.4。
[0063]
3.3.2.4令i＝i 1。如果i>k，则流程结束，输出auv当前位置未陷入局部极小值。如果i≤m，则转入步骤3.3.2.2。
[0064]
上述技术方案中，所述步骤3.3.2.2中采用随机点法使得auv跳出局部极小值的具体过程为：
[0065]
3.3.2.5在三维环境中随机取一个非障碍物坐标点充当局部子目标点，同时暂时摒弃原有的局部子目标点。
[0066]
3.3.2.6由于局部子目标点发生了变化，因此合力的方向也会发生变化。auv沿着新的合力方向按照既定步长移动到下一个路径点。
[0067]
3.3.2.7判断auv当前位置是否陷入局部极小值。如果auv当前位置仍处于局部极小值，则转入步骤3.3.2.5。如果auv当前位置不处于局部极小值，则流程结束，输出auv已经跳出局部极小值，同时把首个未经过的关键路径点做为当前auv的局部子目标点。
[0068]
上述技术方案中，所述步骤3.3.4中auv在合力的方向上按照既定步长移动行驶至下一个路径点的具体过程为：
[0069]
3.3.4.1计算出auv受到的引力f
att
在x轴、y轴、z轴上的各引力分量。
[0070][0071][0072]
其中α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轴上的坐标分量。
[0073]
3.3.4.2计算出auv受到的斥力f
rep
在x轴、y轴、z轴上的各斥力分量。
[0074][0075][0076]
其中α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轴上的坐标分量。
[0077]
3.3.4.3计算出auv受到的合力f
sum
在x轴、y轴、z轴上的各合力分量。
[0078][0079]
其中，f
sumx
、f
sumy
、f
sumz
分别为f
sum
在x轴、y轴、z轴上的各合力分量。n为三维环境中动态障碍物的数量，i表示第几个动态障碍物。
[0080]
3.3.4.4计算出auv的下一个路径点的坐标。
[0081][0082][0083]
其中α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的移动步长且通常为固定常数。
[0084]
上述技术方案中，所述步骤3.4中判断auv是否行驶至局部子目标点的具体过程为：
[0085]
当auv当前位置与局部子目标点的距离小于某个阈值时，可判定auv到达局部子目标点。假设auv当前位置的坐标为p(px,py,pz)。局部子目标点的位置的坐标为g(gx,gy,gz)。则判定公式如下：
[0086]
(gx
‑
px)2 (gy
‑
py)2 (gz
‑
pz)2≤ε1
ꢀꢀꢀ
(14)
[0087]
其中，ε1为判定阈值且通常为固定常数。px、py、pz分别为auv当前位置p在x轴、y轴、z轴上的坐标分量。gx、gy、gz分别为局部子目标点的位置g在x轴、y轴、z轴上的坐标分量。
[0088]
本发明提供的基于碰撞检测模型和人工势场法的auv局部避障方法具有如下有益效果：第一，本发明利用关键路径点做为局部子目标点并且在人工势场法的引导下进行局部避障。使得auv既能够避开静态障碍物、突发性障碍物、有规律运动的动态障碍物，也能够避开无规律运动的动态障碍物。本发明在复杂的水下环境中避障效果较好；第二，本发明利用碰撞检测模型筛选全局路径中的关键路径点并把关键路径点做为引导点，使得auv能够在全局路径和局部避障方法之间灵活切换，这样既可以保证auv能够成功躲避障碍物，又可以保证auv行驶路径的质量(包括路径长度、路径平滑度、路径安全性、auv能耗)；第三，由于本发明采用空间向量计算合力方向上的下一个路径点，相较于传统的对合力角度进行分类讨论然后计算合力方向上的下一个路径点的做法，本发明在避障实时性上也有很大提升；第四，本发明提出了一种用于检测auv是否与水下三维地势发生碰撞的碰撞检测模型，该模型可以防止auv在避障的同时与水下三维地势发生碰撞；第五，本发明对传统的人工势场法进行了改进。其改进工作包括在斥力公式中加入距离因子、重新定义了局部极小值点、提出了auv跳出局部极小值点的方法。这些改进工作可以显著降低auv陷入死锁的可能性。
附图说明
[0089]
图1为本发明提供的基于碰撞检测模型和人工势场法的auv局部避障方法的工作
原理示意图。
具体实施方式
[0090]
下面结合附图和具体实施例对本发明提供的基于碰撞检测模型和人工势场法的auv局部避障方法进行详细说明。
[0091]
如图1所示，本发明提供的基于碰撞检测模型和人工势场法的auv局部避障方法包括按顺序进行的下列步骤：
[0092]
步骤一：根据全局路径建立三维环境下的碰撞检测模型；碰撞检测模型是指用于检测任意两个路径点所构成的路径段是否与三维地形发生碰撞的一种方法。以两个路径点的三个坐标分量之间距离最大的坐标做路径探测，从起始坐标点开始，每次增加特定步长坐标值，依次向目的坐标点逐渐试探；如果某一次试探出路径点在z轴上的坐标分量不高于路径点所对应三维地形的高度值，则输出两路径点连线与三维地形发生碰撞；如果从始至终路径点在z轴上的坐标分量都保持高于路径点所对应三维地形的高度值，则输出两路径点连线不与三维地形发生碰撞；
[0093]
所述步骤一中三维环境下的碰撞检测模型的具体过程为：
[0094]
假设两路径点坐标分别为p
i
(x
i
,y
i
,z
i
)，p
i 1
(x
i 1
,y
i 1
,z
i 1
)，则可以得到两路径点连线的线性参数方程为
[0095][0096][0097]
公式(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表示，即：
[0098][0099]
令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中的最大值。
[0100]
假设dx最大,即采用x坐标做探测，则有以下流程：
[0101]
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
中的最大值。
[0102]
1.2若x1<x2，则x1＝x1 c1并转入步骤1.3，否则试探过程结束，输出两路径点连线不与三维地形发生碰撞。其中，c1为探测步长且为固定常数。
[0103]
1.3利用公式(3)求解x1对应的y1和z1，可得对应的坐标(x1,y1,z1)。如果z1>high(x1,y1)，则转入步骤1.2。如果z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞。其中，high(x1,y1)表示(x1,y1)所对应三维地形的高度值。
[0104]
假设dy最大,即采用y坐标做探测，则有以下流程：
[0105]
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
中的最大值。
[0106]
1.5若y1<y2，则y1＝y1 c1并转入步骤1.6，否则试探过程结束，输出两路径点连线不与三维地形发生碰撞。其中，c1为探测步长且为固定常数。
[0107]
1.6利用公式(3)求解y1对应的x1和z1，可得对应的坐标(x1,y1,z1)。如果z1>high(x1,y1)，则转入步骤1.5。如果z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞。其中，high(x1,y1)表示(x1,y1)所对应三维地形的高度值。
[0108]
假设dz最大,即采用z坐标做探测，则有以下流程：
[0109]
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
中的最大值。
[0110]
1.8若z1<z2，则z1＝z1 c1并转入步骤1.9，否则试探过程结束，输出两路径点连线不与三维地形发生碰撞。其中，c1为探测步长且为固定常数。
[0111]
1.9利用公式(3)求解z1对应的x1和y1，可得对应的坐标(x1,y1,z1)。如果z1>high(x1,y1)，则转入步骤1.8。如果z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞。其中，high(x1,y1)表示(x1,y1)所对应三维地形的高度值。
[0112]
从碰撞检测模型的流程可以看出，碰撞检测模型的实质是从起始坐标点开始，每次增加c1坐标值，依次向目的坐标点逐渐试探。如果某一次试探出z1≤high(x1,y1)，则输出两路径点连线与三维地形发生碰撞。如果从始至终z1都保持z1>high(x1,y1)，则输出两路径点连线不与三维地形发生碰撞。
[0113]
步骤二：在全局路径已知的情况下，利用碰撞检测模型筛选全局路径中的关键路径点；关键路径点是指用于支撑起全局路径的不可或缺的核心路径节点。这意味着在全局路径中仅仅通过依次连接关键路径点就可以构造出一条高质量的可行路径，而非关键路径点的一些普通路径节点仅仅起到填充路径的作用。
[0114]
所述步骤二中利用碰撞检测模型筛选全局路径中的关键路径点的具体过程为：
[0115]
假设某条全局路径包含m个路径点，key_path_point为存储的关键路径点的集合。point_i表示全局路径中的第i个路径点，point_j表示全局路径中的第j个路径点。则该条全局路径的关键路径点获取流程如下所示：
[0116]
2.1令i＝1，j＝2。
[0117]
2.2通过碰撞检测模型来判断point_i与point_j所构成的线性路径段是否与三维地形发生碰撞。如果该路径段与三维地形发生了碰撞，则转入步骤2.3。如果该路径段没有与三维地形发生碰撞，则转入步骤2.4。
[0118]
2.3将point_i存入key_path_point中，然后令i＝j
‑
1。转入步骤2.5。
[0119]
2.4令temp＝j 1。转入步骤2.5。其中，temp充当中间变量。
[0120]
2.5判断point_j是否为终点。如果point_j是终点，则先将point_i存入key_path_point中，再将point_j存入key_path_point中，转入步骤2.6。如果point_j不是终点，则令j＝temp，转入步骤2.2。
[0121]
2.6关键路径点寻找完毕，输出key_path_point，流程结束。
[0122]
步骤三：利用关键路径点做为局部子目标点并且在人工势场法的引导下进行局部避障；其设计思路是通过三维环境下的碰撞检测模型筛选出已知的全局路径中的关键路径
点。当auv未进入障碍物的斥力场时，auv沿着全局路径正常行驶。当auv进入障碍物的斥力场时，auv就会以当前路径点为起点，首个未经过的关键路径点为局部子目标点，切换到人工势场法进行局部避障操作直至auv抵达局部子目标点。当auv抵达局部子目标点后，auv继续沿着全局路径正常行驶。
[0123]
所述步骤三中利用关键路径点做为局部子目标点并且在人工势场法的引导下进行局部避障的具体过程为：
[0124]
3.1判断auv当前所处位置是否位于动态障碍物的斥力场内。如果auv不在动态障碍物的斥力场内，则转入步骤3.2。如果auv在动态障碍物的斥力场内，则转入步骤3.3。
[0125]
3.2auv沿着全局路径继续行驶至下一路径点。转入步骤3.5。
[0126]
3.3auv以当前路径点为起点，首个未经过的关键路径点为局部子目标点，切换到人工势场法，然后行驶至下一路径点。转入步骤3.4。
[0127]
3.4判断auv是否行驶至局部子目标点。如果auv行驶至局部子目标点，则转入步骤3.5。如果auv没有行驶至局部子目标点，则转入步骤3.3。
[0128]
3.5判断auv当前所处位置是否为终点。如果auv当前所处位置不是终点，则转入步骤3.1。如果auv当前所处位置是终点，则流程结束，输出auv成功避开动态障碍物并抵达终点。
[0129]
上述实施过程中，所述步骤3.3中人工势场法的具体执行过程为：
[0130]
3.3.1判断auv是否到达局部子目标点。若auv未到达局部子目标点，则转入步骤3.3.2。若auv已经到达局部子目标点，则流程结束，输出auv已经到达局部子目标点。
[0131]
3.3.2判断auv当前位置是否陷入局部极小值。若auv当前位置已经陷入局部极小值，则采用随机点法使得auv跳出局部极小值，然后转入步骤3.3.1。若auv当前位置未陷入局部极小值，则转入步骤3.3.3。
[0132]
3.3.3计算auv受到局部子目标点产生的引力，计算auv受到动态障碍物产生的斥力，计算auv所受到的合力。转入步骤3.3.4。其中：
[0133]
auv受到局部子目标点产生的引力大小的计算公式如下：
[0134]
|f
att
|＝k1*dis(p,g)
w1
ꢀꢀꢀ
(4)
[0135]
其中，f
att
为auv受到局部子目标点产生的引力；|f
att
|为auv受到局部子目标点产生的引力的大小；f
att
为矢量；f
att
的大小为|f
att
|且f
att
的方向由auv当前位置指向局部子目标点的位置；k1为引力增益系数且为固定常数；p表示auv当前位置，g表示局部子目标点的位置，dis(p,g)表示auv当前位置与局部子目标点的位置之间的距离；w1为距离权重系数且为固定常数；
[0136]
auv受到动态障碍物产生的斥力大小的计算公式如下：
[0137][0138]
其中，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为动态障碍物的斥力场半径且为固定常数；
[0139]
auv所受到的合力的计算公式如下：
[0140][0141]
其中，f
sum
为auv受到的合力；f
sum
的方向为auv受到的引力与auv受到的所有斥力的矢量和；f
att
为auv受到局部子目标点产生的引力；f
rep
为auv受到动态障碍物产生的斥力；n为三维环境中动态障碍物的数量，i表示第几个动态障碍物；
[0142]
3.3.4auv在合力的方向上按照既定步长移动行驶至下一个路径点。转入步骤3.3.1。
[0143]
上述实施过程中，所述步骤3.3.2中局部极小值的定义及局部极小值检测的具体过程为：
[0144]
局部极小值是指在合力的作用下，auv在某一小范围区域内反复震荡从而导致auv无法到达或者延迟到达局部子目标点的现象。局部极小值的判定标准如下：以auv当前位置为圆心，r1为半径做圆。如果在该圆内的auv已经经过的路径点的个数达到某个阈值ε2，则可判定auv陷入局部极小值。否则，可以判定auv未陷入局部极小值。假设auv当前位置为路径中的第k个路径点，i＝n表示路径中的第n个路径点，count为圆内的auv已经经过的路径点的个数。则局部极小值的判定流程如下：
[0145]
3.3.2.1令i＝1，count＝0。
[0146]
3.3.2.2如果第i个路径点到第k个路径点之间的距离小于等于r1，则转入步骤3.3.2.3。如果第i个路径点到第k个路径点之间的距离大于r1，则转入步骤3.3.2.4。
[0147]
3.3.2.3令count＝count 1。如果count的值达到阈值ε2，则流程结束，输出auv当前位置已经陷入局部极小值。如果count的值未达到阈值ε2，则转入步骤3.3.2.4。
[0148]
3.3.2.4令i＝i 1。如果i>k，则流程结束，输出auv当前位置未陷入局部极小值。如果i≤m，则转入步骤3.3.2.2。
[0149]
上述实施过程中，所述步骤3.3.2中采用随机点法使得auv跳出局部极小值的具体过程为：
[0150]
3.3.2.5在三维环境中随机取一个非障碍物坐标点充当局部子目标点，同时暂时摒弃原有的局部子目标点。
[0151]
3.3.2.6由于局部子目标点发生了变化，因此合力的方向也会发生变化。auv沿着新的合力方向按照既定步长移动到下一个路径点。
[0152]
3.3.2.7判断auv当前位置是否陷入局部极小值。如果auv当前位置仍处于局部极小值，则转入步骤3.3.2.5。如果auv当前位置不处于局部极小值，则流程结束，输出auv已经跳出局部极小值同时把首个未经过的关键路径点做为当前auv的局部子目标点。
[0153]
上述实施过程中，本发明采用空间向量计算合力方向上的下一个路径点，所述步骤3.3.4中auv在合力的方向上按照既定步长移动行驶至下一个路径点的具体过程为：
[0154]
3.3.4.1计算出auv受到的引力f
att
在x轴、y轴、z轴上的各引力分量。
[0155][0156][0157]
其中α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轴上的坐标分量。
[0158]
3.3.4.2计算出auv受到的斥力f
rep
在x轴、y轴、z轴上的各斥力分量。
[0159][0160][0161]
其中α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轴上的坐标分量。
[0162]
3.3.4.3计算出auv受到的合力f
sum
在x轴、y轴、z轴上的各合力分量。
[0163][0164]
其中，f
sumx
、f
sumy
、f
sumz
分别为f
sum
在x轴、y轴、z轴上的各合力分量。n为三维环境中动态障碍物的数量，i表示第几个动态障碍物。
[0165]
3.3.4.4计算出auv的下一个路径点的坐标。
[0166][0167][0168]
其中α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的移动步长且通常为固定常数。
[0169]
上述实施过程中，所述步骤3.4中判断auv是否行驶至局部子目标点的具体过程为：
[0170]
当auv当前位置与局部子目标点的距离小于某个阈值时，可判定auv到达局部子目标点。假设auv当前位置的坐标为p(px,py,pz)。局部子目标点的位置的坐标为g(gx,gy,gz)。则判定公式如下：
[0171]
(gx
‑
px)2 (gy
‑
py)2 (gz
‑
pz)2≤ε1
ꢀꢀꢀ
(14)
[0172]
其中，ε1为判定阈值且通常为固定常数。px、py、pz分别为auv当前位置p在x轴、y轴、z轴上的坐标分量。gx、gy、gz分别为局部子目标点的位置g在x轴、y轴、z轴上的坐标分量。