技术领域
[0001] 本
发明涉及一种AUV航路规划方法，特别是一种改进人工势场
优化算法的AUV航路规划方法。
背景技术
[0002] 当前，无人系统已在空中、陆地、
水面、水下迅速发展，并逐渐地在我们的社会生活中发挥重要作用，例如：地面的无人车、
机器人，天上的卫星、无人机，海里的无人船、潜航器，每一项无人系统的出现带动社会的发展，全维度的无人系统正在席卷人类的生产和生活领域。自20世纪90年代以来，随着计算机
软件、
硬件、
电子设备技术和智能算法的迅速发展，水下航行器脱颖而出，脱离了缆绳束缚的AUV，在水下执行任务过程中显得更为灵活。目前，AUV更多的被用来执行预报预警、远程搭载、收集信息、采集海洋数据、科学研究和水下资源勘察等任务。
[0003] 任何任务的执行，都需要能够在工作环境中安全的航行。因此为使AUV能够在复杂环境下安全地顺利完成任务，需要涉及到自
定位与目标识别、任务规划、避碰、路径规划和对信息的处理技术。因此，无论出于理论意义还是现实需求，对航路规划的研究都显得意义重大。航路规划问题就是使AUV在有障碍物的
工作空间内无碰撞地到达目标点。AUV想要完成多个不同
位置的任务，就需要在多种工作环境中有效避开障碍物并顺利到达目标点。
[0004] 基于AUV自身特性，一般是在深海区域执行任务，AUV在航行过程中可能会和暗礁、沉船、鱼群等发生碰撞，未知环境的不安全性会对AUV的安全有很大的威胁。探测障碍物需要对AUV建立环境模型。由于障碍物的不确定性，很可能出现目标不可达和局部极小值问题。
发明内容
[0005] 针对上述
现有技术，本发明要解决的技术问题是提供一种能够解决目标不可达和局部极小值问题的改进人工势场优化算法的AUV航路规划方法。
[0006] 为解决上述技术问题，本发明一种改进人工势场优化算法的AUV航路规划方法，包括以下步骤：
[0007] 步骤1：对于AUV在海洋环境中，初始化AUV执行任务的个数、执行任务的顺序、任务类型、人工势场的引
力势场系数、斥力势场系数、步长和
迭代系数；
[0008] 步骤2：在未知的环境中，AUV在航行过程中，计算AUV与目标点的引力势场函数；
[0009] 步骤3：计算AUV和障碍物的斥力势场函数；
[0010] 步骤4：把步骤2和步骤3的相加值作为合力势场函数；
[0011] 步骤5：在计算当前位置后，再计算下一步位置；
[0012] 步骤6：判断AUV是否陷入局部最小值，如果AUV陷入局部最小值，则给AUV一个(0～kπ/2)
角度偏移量，k∈(0,1)，返回步骤2；如果AUV没有陷入局部最小值，执行步骤7；
[0013] 步骤7：判断是否有目标不可达的情况，如果AUV有目标不可达的情况，则构建一个新的斥力势场函数，返回步骤2；如果没有目标不可达的情况，执行步骤8；
[0014] 步骤8：判断AUV是否到达了目标点，如果AUV到达了目标点，则AUV停止航行；否则返回步骤5。
[0015] 本发明一种改进人工势场优化算法的AUV航路规划方法，还包括：
[0016] 1.步骤2中引力势场函数为：
[0017]
[0018]
[0019] ||Wv-Wa||＝||v(x1,y1,z1)-a(x2,y2,z2)||
[0020] 若在三维空间S中，有静止目标点的坐标位置Wa为a(x2,y2,z2)和AUV的坐标位置Wv为v(x1,y1,z1)，vrob为AUV的速度矢量，vtar为目标点的速度矢量，m,n为大于0的常数，α为引力增益系数，m为任意大于零的正数，若在二维空间，把z的值设为零，降低空间的维度，当0＜m≤1时，引力表现为锥形分布，而且每一个位置点都会表现出引力恒定；当m≥2时，环境中产生的
势能最低点为目标点。
[0021] 2.步骤3中的斥力势场模型为：
[0022]
[0023]
[0024] 其中，Ua(Wv)为斥力势场函数， 为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离，ρ＝‖Wv-Wo‖，β为斥力增益系数。
[0025] 3.步骤4中的合力势场函数为：
[0026] U(W)＝Ua(W)+Uo(W)
[0027]
[0028]
[0029]
[0030] 式中：U(W)为总势场，为合力，Ua(W)为目标对机器人的引力场， 为引力，下标a表示目标点，Uo(W)为障碍物对机器人的斥力场， 为斥力，下标o表示障碍物。
[0031] 4.步骤7中所述新的斥力势场函数满足：
[0032]
[0033] 其中ρa＝‖Wv-Wa‖，ρa表示AUV与目标点的相对距离，n是大于零的整数，求出AUV的所对应的斥力的方向和大小，即：
[0034]
[0035]
[0036] Uo(Wv)为斥力势场函数， 为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离为ρ＝‖Wv-Wo‖，β为斥力增益系数。
[0037] 5.步骤6中当AUV陷入极小值的时候，给与AUV在(0-π/2)任意的旋转角度，将旋转kπ/2角度后的方向作为AUV下一步的航行方向，其中，k∈(0，1)，在k值不断递增的过程，AUV会逐步搜索，再次计算AUV所受的合力是不是为零，如果为零，则AUV仍然处于局部极小值点，改变k值；如果不为零，AUV继续向前航行，继续改变偏转角，重复操作，直到AUV跳出极小值点为止，此时AUV的航行方向为最后一次给定偏转角方向。
[0038] 6.通过多波束声呐来获取障碍物的位置信息，过程为：
[0039] 声呐用发射机制造电
信号，再用换能器将
电信号转换为
声音信号，发出声信号在海洋中进行传递，遇到障碍物、鱼类或者潜艇时被反射回来，反射回来的
声波被换能器转换为电信号，经过
数据处理，在显示屏上显示或者在
耳机中变成声音；
[0040] 建立声呐波束矩阵index(i,j){(i＝1，2，3),(j＝1，2，…，80)}，假设在大地
坐标系下，AUV的质心坐标为(xc,yc,zc)，前视声纳的坐标为(xs,ys,zs)，可得AUV质心与前视声纳的间距为lcs＝xs-xc，则在AUV坐标系下，AUV的质心坐标为(xc,yc,zc)＝(0,0,0)，前视声纳的坐标为(xs,ys,zs)＝(lcs,0,0)，障碍物的坐标为(xbs,ybs,zbs)，则前视声呐的视域范围为：
[0041]
[0042] 若AUV在航行过程中，在前视声呐视域范围内的K处，第index(i,j)的波束与障碍物的某一处有交点，在AUV坐标系下，障碍物的位置为(xbs,ybs,zbs)，其计算公式为：
[0043]
[0044] 经过坐标变换把在AUV坐标系下的障碍物位置信息转换为在大地坐标系下的障碍物位置信息，在大地坐标系下，障碍物的位置信息为(x'bs,y'bs,z'bs)，(x,y,z)为所得位置转换为大地坐标的位置，转换公式为：
[0045]
[0046] 本发明有益效果：
[0047] 在海洋未知环境下，为了解决AUV在有障碍物的情况下无碰撞的完成任务，本发明引入新的斥力场函数，帮助AUV改进目标不可达的现象。引入任意的(0-π/2)旋转角度，帮助AUV逃离局部极小值的现象。将这二种方法结合在一起，对AUV的航路规划有很好的寻优性。这二处改进点综合提高了算法的寻优性和鲁棒性，降低算法的复杂性。
附图说明
[0049] 图2为多波束声呐探测图；
[0050] 图3为AUV航路规划流程；
[0051] 图4为AUV在环境中的受力示意图；
[0052] 图5(a)为引力势场分布图；
[0053] 图5(b)为引力势场分布图；
[0054] 图6(a)为斥力势场分布图；
[0055] 图6(b)为斥力势场分布图；
[0056] 图7为目标不可达示意图；
[0057] 图8为局部极小值示意图；
[0058] 图9为目标不可达航行轨迹；
[0059] 图10为目标可达航路轨迹；
[0060] 图11为陷入极小值点的航行轨迹；
[0061] 图12为改进极小值后的航路轨迹；
[0062] 图13(a)为X-Y轴方向AUV航路规划；
[0063] 图13(b)为Y-Z轴方向AUV航路规划；
[0064] 图13(c)为X-Z轴方向AUV航路规划；
[0065] 图13(d)为X-Y-Z轴方向AUV航路规划。
具体实施方式
[0066] 本发明一种改进人工势场优化算法的AUV航路规划方法，下面将以三维环境下为海洋任务环境，1艘携带侦察
传感器模
块的AUV、3个侦察任务、4个障碍物为例。说明该改进算法的实现方式。本发明方法流程如图1所示：
[0067] S1：需要建立在海洋未知环境下的环境模型，通过多波束声呐来获取障碍物的位置信息。该声呐的水平开角为α＝120°，3层波束，垂直开角为β＝17°，80条波束，故多波束声呐共有240条波束。探测距离为R＝120m，工作
频率是2Hz。
[0068] S2：初始化工作：主要包括水面工作站初始化、使命任务下载、人工势场法的相关参数设置、海域面积设置。设置参数为引力系数为1，斥力系数为2，步长为0.1，障碍物影响距离为100，迭代次数为200。
[0069] S3：通过水面
工作台加载AUV及其任务序列的位置信息如表1所示：
[0070] 表1加载的AUV和任务位置信息
[0071]
[0072]
[0073] 为了便于使障碍物归一化展示，在三维环境下，将障碍物都是归一化为球体的形式进行显示，水面工作台加载的障碍物的信息如下表2所示：
[0074] 表2加载的障碍物信息
[0075]
[0076] S4：在未知的环境中，AUV在航行过程中，计算AUV与目标点的引力势场函数。
[0077] S5：计算AUV和障碍物的斥力势场函数。
[0078] S6：把步骤2和步骤3的相加值就是合力势场函数。
[0079] S7：在计算当前位置后，再计算下一个位置。
[0080] S8：如果AUV陷入局部最小值就给AUV一个(0～kn/2)角度偏移量，返回步骤2。
[0081] 步骤9：如果AUV没有陷入局部最小值，就执行步骤8；
[0082] 步骤10：如果AUV有目标不可达的情况，就构建一个新的斥力势场函数，返回步骤2；
[0083] 步骤11：如果没有目标不可达的情况，就执行步骤10；
[0084] 步骤12：如果AUV到达了目标点，AUV就停止航行。
[0085] 步骤13：如果AUV到达目标点，返回步骤5；
[0086] 由于在三维环境下，通过X-Y、Y-Z、X-Z、X-Y-Z方向来显示AUV的航路。AUV航路规划如下图13(a)-图13(d)。其中，图13(a)图为X-Y轴方向，图13(b)Y-Z轴方向，图13(c)X-Z轴方向，图13(d)X-Y-Z轴方向。
[0087] AUV遇到障碍物时可通过图13(a)-图13(d)看出其运动方向了发生改变，且很好的规避障碍物且航路非常光滑。进一步验证了改进人工势场优化算法的有效性和寻优性。
[0088] 综上所述，在海洋未知环境下，本发明提出的改进人工势场优化算法有效提高了算法运行在航路优化的能力，规划出来的航路是非常平滑，且能安全的避过障碍。
[0089] 本发明具体实施方式还包括，包括以下步骤：
[0090] 步骤1：对于AUV在海洋环境中，初始化AUV执行任务的个数、执行任务的顺序、任务类型，人工势场的引力势场系数、斥力势场系数、步长和迭代系数。
[0091] 步骤2：在未知的环境中，AUV在航行过程中，计算AUV与目标点的引力势场函数。
[0092] 步骤3：计算AUV和障碍物的斥力势场函数。
[0093] 步骤4：把步骤2和步骤3的相加值就是合力势场函数。
[0094] 步骤5：在计算当前位置后，再计算下一个位置。
[0095] 步骤6：如果AUV陷入局部最小值就给AUV一个(0～kn/2)角度偏移量，返回步骤2；
[0096] 步骤7：如果AUV没有陷入局部最小值，就执行步骤8；
[0097] 步骤8：如果AUV有目标不可达的情况，就构建一个新的斥力势场函数，返回步骤2；
[0098] 步骤9：如果没有目标不可达的情况，就执行步骤10；
[0099] 步骤10：如果AUV到达了目标点，AUV就停止航行。
[0100] 步骤11：如果AUV到达目标点，返回步骤5；
[0101] 步骤2中引力势场函数模型为：
[0102]
[0103]
[0104] ‖Wv-Wa‖＝‖v(x1,y1,z1)-a(x2,y2,z2)||
[0105] 若在三维空间S中，有静止目标点的坐标位置Wa为a(x2,y2,z2)和AUV的坐标位置Wv为v(x1,y1,z1)。vrob为AUV的速度矢量，vtar为目标点的速度矢量，m,n为大于0的常数。α为引力增益系数，m为任意大于零的正数。若在二维空间，把z的值设为零，降低空间的维度。当0＜m≤1时，引力表现为锥形分布，而且每一个位置点都会表现出引力恒定。同理，当m≥2时，环境中产生的势能最低点就是目标点，有可能导致引力趋于无穷大的现象导致AUV在目标点来回抖动。引力势场分布图如图5(a)和图5(b)所示。
[0106] 步骤3中的斥力势场模型为：
[0107]
[0108]
[0109] 其中，Ua(Wv)为斥力势场函数， 为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离为ρ＝||Wv-Wo||，β为斥力增益系数；斥力势场分布图如图6(a)和图6(b)所示。
[0110] 步骤4中的合力势场模型为：
[0111] 在构建势场时，AUV利用斥力与引力的合力共同作用，其合势场的数学表达式为：
[0112] U(W)＝Ua(W)+Uo(W)
[0113]
[0114]
[0115]
[0116] 式中：U(W)为总势场， 为合力，Ua(W)为目标对机器人的引力场， 为引力，下标a表示目标点。Uo(W)为障碍物对机器人的斥力场， 为斥力，下标o表示障碍物。说明AUV是顺着势场的负梯度方向进行航向，最终达到目标点。
[0117] 步骤8中的目标不可达问题，是指AUV航行离目标点的附近，有一个或者多个障碍物也在目标点的附近，并且AUV和目标点都在障碍物能够影响的范围内，根据斥力势场函数和引力势场函数分析，AUV和目标点的距离越来越近，AUV所受的引力就会越来越小。而AUV和障碍物的距离越来越近，AUV所受的斥力就会越来越大。当斥力大于引力的合力作用，AUV就会远离目标点或者在目标点附近不断徘，导致AUV不能到达目标点的位置。目标不可达示意图如图7、目标不可达航行轨迹图9及其目标可达航路轨迹如图10所示。
[0118] 针对此问题，进一步的考虑AUV与目标点之间的相对距离。构建一个新的斥力势场函数为
[0119]
[0120] 其中ρa＝||Wv-Wa||，ρa表示AUV与目标点的相对距离，n是大于零的整数。可以求出AUV的所对应的斥力的方向和大小，即：
[0121]
[0122]
[0123] 其中，Uo(Wv)为构建一个新的斥力势场函数， 为势场力，ρa＝||Wv-Wa||，ρa表示AUV与目标点的相对距离，n是大于零的整数。
[0124] 步骤7中的局部极小值问题，是AUV在航行的过程中，在还没到达目标点的情况下，AUV受到一个障碍物的斥力或者多个障碍物的受到的合力与AUV受到目标点的引力，二者是大小相等，方向相反。在还没到达目标的时候，航行过程中的合力为零，会出现停滞不前或者来回震荡的现象。AUV停止航行的点就是全局势能最小点。
[0125] 本发明给AUV引入了在一定范围内的随机偏转角。当AUV陷入极小值的时候，这时给与AUV任意的旋转角度，它的范围在(0-π/2)，将旋转kπ/2角度后的方向作为AUV下一步的航行方向。其中，K∈(0，1)，在K值不断递增的过程，AUV会逐步搜索。再次计算AUV所受的合力是不是为零，如果为零，说明AUV仍然处于局部极小值点，改变K值。如果不为零，AUV仍然向前航行，继续改变偏转角，重复操作，直到AUV能够跳出极小值点为止，此时AUV的航行方向就是最后一次给定偏转角方向。局部极小值示意图如图8、陷入极小值点的航行轨迹如图11及其改进极小值后的航路轨迹如图12所示。
[0126] 本发明通过多波束声呐来获取障碍物的位置信息。声呐先用发射机制造电信号，再用换能器(电信号转换为声音信号)发出声信号在海洋中进行传递，如果遇到障碍物、鱼类或者潜艇时，就会被反射回来，反射回来的声波会被换能器转换为电信号，经过数据处理，会在显示屏上显示或者在耳机中变成声音。
[0127] 建立声呐波束矩阵index(i,j){(i＝1，2，3),(j＝1，2，...，80)}。假设在大地坐标系下，AUV的质心坐标为(xc,yc,zc)，前视声纳的坐标为(xs,ys,zs)，可得AUV质心与前视声纳的间距为lcs＝xs-xc，多波束声呐探测图如图2所示。则在AUV坐标系下，AUV的质心坐标为(xc,yc,zc)＝(0,0,0)，前视声纳的坐标为(xs,ys,zs)＝(lcs,0,0)。障碍物的坐标为(xbs,ybs,zbs)，则前视声呐的视域范围为
[0128]
[0129] 若AUV在航行过程中，在前视声呐视域范围内的K处，第index(i,j)的波束与障碍物的某一处有交点。此时，在AUV坐标系下，障碍物的位置为(xbs,ybs,zbs)，其计算公式为：
[0130]
[0131] 经过坐标变换可以把在AUV坐标系下的障碍物位置信息转换为在大地坐标系下的障碍物位置信息。在大地坐标系下，障碍物的位置信息为(x'bs,y'bs,z'bs)。(x,y,z)为所得位置转换为大地坐标的位置。其计算公式为：
[0132]
[0133] 通过建立多波束声呐的数学模型，可以得到AUV在航行过程中获得的障碍物的位置信息。由上所述，AUV在航行过程中是基于前视声纳探测范围的扇形模型来做
基础来实现未知环境模型。
[0134] 本发明具体实施方式还包括：
[0135] 具体的操作步骤如下：
[0136] 步骤1：建立在海洋未知环境下的环境模型，通过多波束声呐来获取障碍物的位置信息。对于在海洋环境中，AUV的位置信息是通过自身定位系统来获取，对于AUV在航行过程中，如果前视声呐探测到障碍物的信息可通过坐标转换为大地坐标系下的障碍物信息。
[0137] 步骤2：将改进人工势场法运用在AUV的航路规划上，建立人工势场算法模型。其中包括的元素有斥力势场函数、引力势场函数、合力势场函数、航向角度。
[0138] 步骤3：对于AUV在海洋环境中，初始化AUV执行任务的个数、执行任务顺序及其类型。
[0139] 步骤4：在未知的环境中，AUV的前视声纳在航行过程对航行路径进行探测，探测到没有障碍物，将计算引力势场函数作为合力势场函数作为最终势场函数来引导AUV的航行。
[0140] 步骤5：如果有障碍物，将判断在AUV航行的安全距离内，有几个障碍物，计算多个障碍物的斥力势场函数，将引力势场函数与斥力势场函数的综合作为合力势场函数来引导AUV的航行。
[0141] 步骤6：如果AUV在航行中陷入局部极小，将给AUV随机在90度的角度，再更新合力函数，直至不再陷入局部极小值点。
[0142] 步骤7：如果AUV在航行过程中有目标不可达问题，将构建新的斥力势场函数，再更新合力函数，直至不再有目标不可达问题。
[0143] 步骤8：当AUV完成所有的任务，即说明AUV到达目标终点，AUV停止航行。
[0144] 引力势场模型为：
[0145]
[0146]
[0147] ||Wv-Wa||＝||v(x1,y1,z1)-a(x2,y2,z2)
[0148] 若在三维空间S中，Ua(Wv)为引力势场函数， 为斥力。静止目标点的坐标位置Wa为a(x2,y2,z2)，AUV的坐标位置Wv为v(x1,y1,z1)。vrob为AUV的速度矢量，vtar为目标点的速度矢量，m,n为大于0的常数，α为引力增益系数。当0＜m≤1时，引力表现为锥形分布，而且每一个位置点都会表现出引力恒定。同理，当m≥2时，环境中产生的势能最低点就是目标点，有可能导致引力趋于无穷大的现象导致AUV在目标点来回抖动。引力势场分布图如图5(a)和图5(b)所示。
[0149] 斥力势场模型为
[0150]
[0151]
[0152] 其中，Uo(Wv)为斥力势场函数， 为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离为ρ＝||Wv-Wo||，β为斥力增益系数；斥力势场分布图如图6(a)和图6(b)所示。
[0153] 合力势场模型为
[0154] 在构建势场时，AUV利用斥力与引力的合力共同作用，其合势场的数学表达式为：
[0155] U(W)＝Ua(W)+Uo(W)
[0156]
[0157]
[0158]
[0159] 式中：U(W)为总势场，为合力，Ua(W)为目标对机器人的引力场， 为引力，下标a表示目标点。Uo(W)为障碍物对机器人的斥力场， 为斥力，下标o表示障碍物。说明AUV是顺着势场的负梯度方向进行航向，最终达到目标点。
[0160] 关于目标不可达问题，是指AUV航行离目标点的附近，有一个或者多个障碍物也在目标点的附近，并且AUV和目标点都在障碍物能够影响的范围内，根据斥力势场函数和引力势场函数分析，AUV和目标点的距离越来越近，AUV所受的引力就会越来越小。而AUV和障碍物的距离越来越近，AUV所受的斥力就会越来越大。当斥力大于引力的合力作用，AUV就会远离目标点或者在目标点附近不断徘，导致AUV不能到达目标点的位置。目标不可达示意图如图7、目标不可达航行轨迹图9及其目标可达航路轨迹如图10所示。
[0161] 针对此问题，进一步的考虑AUV与目标点之间的相对距离。构建一个新的斥力势场模型为
[0162]
[0163]
[0164] Uo(Wv)为斥力势场函数， 为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离为ρ＝||Wv-Wo||，β为斥力增益系数，其中ρa＝||Wv-Wa||，ρa表示AUV与目标点的相对距离，n是大于零的整数。
[0165] 关于局部极小值问题，是AUV在航行的过程中，在还没到达目标点的情况下，AUV受到一个障碍物的斥力或者多个障碍物的受到的合力与AUV受到目标点的引力，二者是大小相等，方向相反。在还没到达目标的时候，航行过程中的合力为零，会出现停滞不前或者来回震荡的现象。AUV停止航行的点就是全局势能最小点。
[0166] 本发明给AUV引入了在一定范围内的随机偏转角。当AUV陷入极小值的时候，这时给与AUV任意的旋转角度，它的范围在(0-π/2)，将旋转kπ/2角度后的方向作为AUV下一步的航行方向。其中，K∈(0，1)，在K值不断递增的过程，AUV会逐步搜索。再次计算AUV所受的合力是不是为零，如果为零，说明AUV仍然处于局部极小值点，改变K值。如果不为零，AUV仍然向前航行，继续改变偏转角，重复操作，直到AUV能够跳出极小值点为止，此时AUV的航行方向就是最后一次给定偏转角方向。局部极小值示意图如图8、陷入极小值点的航行轨迹如图11及其改进极小值后的航路轨迹如图12所示。
[0167] 本发明通过多波束声呐来获取障碍物的位置信息。声呐先用发射机制造电信号，再用换能器(电信号转换为声音信号)发出声信号在海洋中进行传递，如果遇到障碍物、鱼类或者潜艇时，就会被反射回来，反射回来的声波会被换能器转换为电信号，经过数据处理，会在显示屏上显示或者在耳机中变成声音。
[0168] 建立声呐波束矩阵index(i,j){(i＝1，2，3),(j＝1，2，…，80)}。假设在大地坐标系下，AUV的质心坐标为(xc,yc,zc)，前视声纳的坐标为(xs,ys,zs)，可得AUV质心与前视声纳的间距为lcs＝xs-xc，多波束声呐探测图如图2所示。则在AUV坐标系下，AUV的质心坐标为(xc,yc,zc)＝(0,0,0)，前视声纳的坐标为(xs,ys,zs)＝(lcs,0,0)。障碍物的坐标为(xbs,ybs,zbs)，则前视声呐的视域范围为
[0169]
[0170] 若AUV在航行过程中，在前视声呐视域范围内的K处，第index(i,j)的波束与障碍物的某一处有交点。此时，在AUV坐标系下，障碍物的位置为(xbs,ybs,zbs)，其计算公式为：
[0171]
[0172] 经过坐标变换可以把在AUV坐标系下的障碍物位置信息转换为在大地坐标系下的障碍物位置信息。在大地坐标系下，障碍物的位置信息为(x'bs,y'bs,z'bs)。(x,y,z)为所得位置转换为大地坐标的位置。其计算公式为：
[0173]
[0174] 通过建立多波束声呐的数学模型，可以得到AUV在航行过程中获得的障碍物的位置信息。由上所述，AUV在航行过程中是基于前视声纳探测范围的扇形模型来做基础来实现未知环境模型。
