CN104992466A

CN104992466A - 一种三维场景的即时寻径方法

Info

Publication number: CN104992466A
Application number: CN201510362453.3A
Authority: CN
Inventors: 金虎; 陈宇; 陈念伟
Original assignee: SICHUAN SAIDIZHI SCIENCE & TECHNOLOGY Co Ltd
Current assignee: Sichuan Track Point Technology Co., Ltd.
Priority date: 2015-06-26
Filing date: 2015-06-26
Publication date: 2015-10-21
Anticipated expiration: 2035-06-26
Also published as: CN104992466B

Abstract

本发明公开了一种三维场景的即时寻径方法，包括：（1）地形数据按三角线框模型表达，初始化地形数据；（2）射线法检测障碍；（3）对行进位置点预计算；（4）验证预计算点是否是满足约束的有效位置；（5）锋面扩散计算；（6）更新运动坐标；（7）目标可达的停止方式。本发明针对三维场景中障碍物状态复杂性、地表数据的不规则性和区域范围广，以及应用场景对即时或全局寻径的不同需求，充分利用了三角形线框表达模型的特点，并结合了三维碰撞检测技术与锋面传播方法，优化了三维场景即时、快速、有效的寻径方式，使得寻径的过程具有智能性和自适应性，满足自动寻径的需求，并降低了计算失效风险和计算成本。

Description

一种三维场景的即时寻径方法

技术领域

本发明涉及一种三维场景的即时寻径方法。

背景技术

自动寻径问题长期活跃于机器人自动导游、无人驾驶和安全控制等研究领域，等价于移动规划问题。随着虚拟现实技术与计算机游戏的快速发展，路径规划得到了更为广泛的探索和研究。在越来越多的游戏虚拟场景中，自动寻径已成为不可分割的应用部件。不同于机器人寻径规划，对机器人运动形态、行进路径存在安全性要求。虚拟场景中的运动物体并无安全性要求，仅需满足路径搜索的有效性和合理性。

自动寻径的研究一般划分为经典寻径和启发式寻径算法，经典寻径的典型算法有最短路径搜索的Dijkstra算法、势场Potential Field(PF)、元分解Cell Decomposition(CD)、可视图方法Visibility Graph(Vgraph)、Voronoi Diagram(VD)、路线图Road Map、Subgoal Network等。寻径问题属于NP-hard问题，经典寻径算法存在高维度、高复杂度的困难，在地图规模较大情况下不如启发式寻径更为有效。因此，近期的研究中，启发式寻径算法逐渐占据主导地位。

启发式寻径的基本思想是使用启发式线索用以表征地图空间点信息，并使用启发式函数值于指导路径序列点的选取。启发式寻径的代表算法为：A*算法，采用生物或群体智能方法(神经网络方法Neural Network(NN)，遗传算法Generic Algorithm(GA)，粒子群优化ParticleSwarm Optimization(PSO)，蚁群优化Ant Colony Optimization(ACO))。启发式寻径并不保证像经典寻径算法一样寻求最优的解路径，但适用于更短的时间获得相对优化的解路径。在三维虚拟场景中，启发式寻径的特点正好表现出寻径的智能特征，更加适合于这类应用场景。

然而，由于三维虚拟场景中的三维地表模型具有不规则性或不连续性等特征，地表区域范围较大，场景中的障碍物难以使用预先标定方法进行区分，因而启发式方法也会有计算失效的风险。并且，启发式寻径方法存在多变量的特点，启发式线索越多则不确定量越大，计算成本也随指数增长。因此，需要解决三维场景中的即时路径优化问题。

发明内容

本发明的目的在于提供一种三维场景的即时寻径方法，主要解决现有三维场景自动寻径方式存在计算失效风险大、计算成本高的问题。

为了实现上述目的，本发明采用的技术方案如下：

一种三维场景的即时寻径方法，包括以下步骤：

(1)利用三角线框模型表示三维场景的地形数据并对其进行初始化，该三角线框模型中，初始化的起点POS和目标点TARGET的坐标分别为(u₀，v₀，w₀)和(u_e，v_e，w_e)；

(2)利用公式DIR＝TARGET-POS计算出三角线框模型的起始方向DIR的坐标，并将其标记为(r₀，y₀，p₀)；

(3)按照方向DIR以逆时针CCW的顺序得到三角线框；

(4)分别设定探测射线运动质点P和射线方向D的坐标(u_t,v_t,w_t)、(r_t,y_t,p_t)，以及运动质点保持在地表之上的高度OH、单步步进距离δ，并将射线算法和三角线框重心坐标公式进行联立，得到公式①、②、③：

b_{1} = \frac{D \cdot P \times E_{2}}{D \cdot N}

①

b_{2} = \frac{D \cdot E_{2} \times P}{D \cdot N}

②

t = \frac{- P \cdot N}{D \cdot N}

③

利用公式①、②、③分别计算出b₁、b₂和t，其中，b₁、b₂均为三角线框重心坐标的分量，N为三角线框的单位法线，t为射线与三角线框的交点；

(5)判断计算得到的b₁、b₂和t是否满足条件：b₁≥0，b₂≥0，b₁+b₂≤1，t≥0，是，则确定射线与三角线框相交，执行步骤(6)；否，则确定射线与三角线框不相较，执行步骤(7)；

(6)按照下列公式判断三维场景中是否存在障碍物：

④

⑤

其中，l₁为交线U-V水平面的投影距离，l₂为起点-目标U-V水平面投影距离，如果l₁<l₂，则确定存在障碍物，执行步骤(9)，否则确定不存在障碍物，执行步骤(7)；

(7)沿着当前射线的方向D，按照下列公式对运动质点的行进位置点POS(u_pre,v_pre,w_pre)进行预计算，并执行步骤(8)：

POS(u_pre,v_pre,w_pre)＝POS(u_t,v_t,w_t)+DIR(r_t,y_t,p_t)*δ

(8)验证预计算点是否满足高度约束，是，则执行步骤(11)；否，则执行步骤(9)；

(9)以δ为半径，作圆周形锋面扩散计算，确定局部最佳步进点P″(u_o,v_o,w_o)，然后执行步骤(10)；

(10)按照下列公式更新P″(u_o,v_o,w_o)的运动坐标，然后执行步骤(11)：

P(u_t，v_t,，w_t)＝POS(u_t，v_t，w_t)+Vector3(0，0，OH) ⑥

DIR(r_t，y_t，p_t)＝TARGET(u_e，v_e，w_e)－P(u_t，v_t，w_t) ⑦

(11)在目标点位置可到达情况下，设定阈值Disvalve，并确定公式sqrt((P.u_t-TARGET.u_t)²+(P.v_t-TARGET.v_t)²)≤Disvalve是否成立，是，则确定运动质点到达目标点位置，寻径结束；否，则返回步骤(4)。

具体地说，所述步骤(8)包括以下步骤：

(8a)在预计算点处构造探测射线，射线起点为步骤(7)中的位置点POS(u_pre,v_pre,w_pre)，射线方向为竖直向下，且其坐标为(0,0,-1)；

(8b)根据构造的探测射线，确定其与三角线框的交点坐标为(u_pre，v_pre，w_pre′)；

(8c)计算高度差△_w＝(u_pre，v_pre，w_pre)·w_pre－(u_pre，v_pre，w_pre′)·w_pre′，若△_w≤OH，则满足高度约束，执行步骤(11)；否则，执行步骤(9)。

具体地说，所述步骤(9)包括以下步骤：

(9a)以当前运动质点POS(u_t,v_t,w_t)为圆心，δ为半径，以U轴正向处(u_t，δ)为起点，沿圆周划分为8*2^k-1个均匀分布点，k表示最大轮数，并且k＝4，l表示单步最大搜索轮数；

(9b)根据最大轮数，按照公式iLevelSet[i]＝i*5.625循环计算水平集iLevelSet，i为正数，且i<64；

(9c)根据最大轮数，每一轮均按8*2^i-1次方数量从水平集iLevelSet中选取备选数据，并且每选取一轮后，均进行标记，避免对已访问过的备选数据重复计算，若某一轮中存在有效的步进点，则进入步骤(9d)，该轮对应的水平集表示为iLevelSet[j]；否则确定无法寻找到达目标的路径，寻径结束；

(9d)利用公式P′＝POS(u_t,v_t,w_t)+(δ*cos(iLevelSet[j])，δ*sin(iLevelSet[j])，OH)计算P′，并设定射线方向D′的坐标为(0,0,-1)；

(9e)根据P′、D′构造射线，确定其与三角线框的交点P″的坐标为(u_pre，v_pre，w_pre)；

(9f)计算高度差△_w＝(u_pre，v_pre，w_pre)·w_t－(u_pre，v_pre，w_pre)·w_pre，若△_w≤OH，则满足高度约束，加入备选CandiSet(P″)，并标记为已访问，执行步骤(9g)；否则结束寻径；

(9g)从备选的CandiSet(P″)中选取一个元素P′，并根据公式Distance＝|P′－P|得到P′与P的距离，然后判断该Distance是否大于或等于最小距离minDistance，是，则令P″＝P′，执行步骤(10)；否，则结束寻径。

或者，所述步骤(9)包括以下步骤：

(9g)从备选的CandiSet(P″)中选取一个元素P′，并得到与该元素相应的返回值optValue，然后判断该optValue是否大于或等于最大返回值maxoptValue，是，则令P″＝P′，执行步骤(10)；否，则结束寻径。

本发明的设计原理在于，针对三维地表的特点，选择采用三角线框的表达方法，既能较好地对地表进行离散化近似，也有利于对地表进行局部几何特征分析。而在即时寻径过程中，采用碰撞检测用于快速确定是否存在障碍物，并在遇到障碍物情况下，采用锋面扩散方法寻找优化路径，该方式是步进方式，沿圆形锋面方向均匀发展选取候选行进点，并采用了局部优化技术以确定优化路径。在当前计算无法找到有效备选集的情况下，采用多轮计算，并随计算轮次自适应增强锋面扩展强度，增加备选集大小和扩展搜索方向，从而使得寻径过程具有智能性和自适应性，满足寻径的需求。

与现有技术相比，本发明具有以下有益效果：

(1)本发明设计严谨、流程合理、寻径稳定性高，其针对三维场景中障碍物状态复杂性、地表数据的不规则性和区域范围广，以及应用场景对即时或全局寻径的不同需求，充分利用了三角形线框表达模型的特点，并结合了三维碰撞检测技术与锋面传播方法，优化了三维场景即时、快速、有效的寻径方式，从而在有效地解决自动寻径问题的同时，也大幅降低了计算失效风险和计算成本。

(2)在三维场景中，由于存在一些地形约束条件，例如很陡的斜面，或斜面过于光滑，粘滞系数低等，因此，本发明对预计算点进行了是否满足高度约束的验证，从而可以使行进仿真更加自然和真实的同时，保证自动寻径的准确性。

(3)在遇到障碍物情况下，由于智能行为应对障碍物采用攀越或绕行，因此本发明以δ为半径，作圆周形锋面扩散计算，并确定局部最佳步进点，然后对其运动坐标进行更新，并通过阈值Disvalve判断运动质点是否到达目标点位置。本发明通过对局部几何特征进行评估分析，确定了下一步的行进方向，易于获得可行进点集的地形，所需计算轮次小，便于快速朝目标点行进；对于不存在可达路径的情况，可通过局部候选点集搜索失效判断，如此一来，便可为即时路径搜索的快速性和有效性提供很好的保障。

附图说明

图1为本发明的流程示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明，本发明的实施方式包括但不限于下列实施例。

实施例

本发明提供了一种三维场景的即时寻径方法，可以使寻径过程具有智能性和自适应性，优化三维户外场景下的启发式自动寻径。本发明的理论依据主要有以下四点：

1、三角线框模型(tri-Mesh)

在二维平面和部分三维地形的路径搜索讨论中，通常采用规则的矩形线框模型来表示平面地图。规则的矩形线框具有更好的规律性和对称性，在细节分级(LOD)的地形管理中有更多的优势。区别于矩形线框模型的表达方式，实际应用中，地形的制作可通过3D-Max，Maya等三维建模工具生成，并直接导出为地形数据。其中，三角线框模型是典型的表达形式。三角线框表示中，三角形的顶点数据和顶点索引分别存储。一个三角形可通过顺序的三个索引值确定三角线顶点在顶点数据集中的位置。这一存储方式可使顶点数据能够复用。此外，三维对象的三角形线框在渲染时，通常有方向性。按观测点到三角形表面方向，三角形的三个点有顺时针CW(Clock Wind)和逆时针CCW(Counter Clock Wind)两种标记方向，本实施例按CCW方式取为正向。

每一个有效三角形线框的几何特性包括：(1)三角形顶点确定的空间位置；(2)顶点索引顺序确定的三角形表面的正向；(3)由三角形任意两条边可计算得到三角形面的法向，并通过三角形表面的正向确定法线方向。除三角线框直接的几何特性，还可为其增加地形相关的启发式特性。比如表示该三角线框的地形类型、粘滞度、可通过性等。这些启发式属性可用于寻径过程中的启发式优化函数设计，并用于确定当前位置的启发式路径搜索方向。

三维地表使用三角形线框模型表示方法的好处是具有更好的普适性和通用性，可适用于对不规则对象表面的表示，也可用于对无法精确使用函数表达形式f(x，y，z)的地表数据进行表示。三角形线框表达模型本身是对连续表面的离散化表达，可满足数值计算的需求。

2、三维空间障碍物检测和避免

采用三角形线框的表达方式，物体表面由三角线框组合而成,且在普遍的三维场景中，三角形线框组成对象为凸形。机器人运动中，可通过图像采集器模拟视觉。在三维对象的运动中，典型的实现方法是从视点位置发出射线，模拟视线行为。三维场景中射线与对象表面的三角线框相交可作为碰撞检测，并用于指导对障碍物避让。单一射线覆盖范围有限，可采用多射线方法，模拟视觉可视区域的视线探测。

射线法实现碰撞检测：

空间直线表示为：

Line3:P+tD,t∈R。 (1)

其中，P为三维空间上的一点，单位长度向量D是射线的方向。t为实数域R上的连续计量值。若用直线形式表示空间射线，则P为射线的起点，t满足t≥0。

三角线框按前述表示，一个三角线框有三个顶点V_i,0≤i≤2。则三角线框内的点可用重心坐标式表示为：

b₀v₀+b₁v₁+b₂v₂＝v₀+b₁E₁+b₂E₂ (2)

其中，b₀+b₁+b₂＝1；边E₁＝v₁-v₀,E₂＝v₂-v₀；且0≤b₁,0≤b₂,b₁+b₂≤1。

射线与三角线框交点，即联立(1)、(2)方程，得到：

P+tD＝b₁E₁+b₂E₂ (3)

采用向量计算求解即可得到：

b_{1} = \frac{D \cdot P \times E_{2}}{D \cdot N} - - - (4)

b_{2} = \frac{D \cdot E_{2} \times P}{D \cdot N} - - - (5)

t = \frac{- P \cdot N}{D \cdot N} - - - (6)

其中，N为三角线框的单位法线。按射线与三角线框相交的约束要求，对(4)(5)(6)的计算结果应同时满足条件b₁≥0，b₂≥0，b₁+b₂≤1，t≥0，否则射线与三角线框不相交。

通过射线探测方法，不仅可确定射线与特定对象的三角线框有无交点，还可确定交点位置，以及通过t确定射线上多个相交三角线框的相交次序。采用射线探测方法模拟运动物视线可简单、直观判定运动物体与目标点之间是否存在障碍物，并作用于粗粒度的行进方向调整指示。

3、锋面扩散

锋面扩散存在于大量的物理现象中，例如海洋波浪、火焰的传播。水平集方法是分析锋面扩散的典型模型，为移动锋面的分析、计算提供了一致的数值计算方法。以二维平面上一任意曲线或三维空间上的任一曲面为例，将平面或空间划分为两个区域。设定曲线或曲面沿着自身的法线方向以速度V传播，忽略切向运动，即为锋面扩散情况。

以二维平面的曲线为例。设曲线γ(t)，初始位置为γ₀＝γ(t＝0)，沿曲线法向量方向的运动速度V。V是标量函数，由曲线上点的单位法向量与点的位置函数相对于时间的变化速度确定，水平集方法中，难以为曲线上的点设定时间函数，普遍方法是将速度函数V＝V(k)表述成依赖曲线几何特征的函数，k为曲线在该点处的曲率。

对曲线γ(t)上点的位置向量参数化表示：并设定t时刻封闭曲线满足周期性该参数表示时，以曲线沿s正向时，内部位于该方向的左面，外部位于该方向的右面。参数化表示后，对应的法线与曲率分别表示为：投影至二维平面x,y轴，分别得到x,y向的速度分量为：

\begin{matrix} x_{t} = V (k) \cdot \frac{y_{s}}{{(x_{s}^{2} + y_{s}^{2})}^{1 / 2}}, & y_{t} = - V (k) \cdot \frac{x_{s}}{{(x_{s}^{2} + y_{s}^{2})}^{1 / 2}} \end{matrix}

其中，曲率k关于s的参数化表示为

则x(s,t),y(s,t)即为锋面扩散运动的描述。通过对速度函数V(k)的设计，可调整锋面的几何特征。对连续时间变量t作离散化处理，可得到锋面扩散运动的数值计算表达式，因此易于将该方法应用到离散化的计算场景中。

4、运动质点

运动质点设计为不变形刚体，在不考虑体积情况下，以质点形式表达。可用坐标位置POS(u_t,v_t,w_t)表示质点当前位置，其中w_t应保持在地表高度OH处，其实现可用直线与地形表面的碰撞检测计算。质点运动有方向性，用单位向量DIR(r_t，y_t，p_t)表示。对于具有体积的更复杂运动对象物理模型，对象的位置可用类似方法计算，采用对象与地表接触部位设定探测点方式求解。

如图1所示，依据上述四个理论基础，下面对本发明的流程进行介绍。

本发明从三维场景中任意一点出发，朝设定目标点行进，路径的计算实时更新，寻径算法不采用全局优化，而是根据运动质点当前状态即时计算，具体如下所述：

(3)按照方向DIR以逆时针CCW的顺序得到三角线框；该步骤中，顶点数组Vector3Vertex[VERTEXNUM]，每个数组对应三维场景中一个顶点的空间位置；索引数组intVertexIndex[VERTEXNUM]，对每一顶点顺序编号；struct{intVertexIndex[3]；}Triangle[TRINUM]，一个三角线框对应了CCW顺序排列的三个顶点索引；

b_{1} = \frac{D \cdot P \times E_{2}}{D \cdot N}

①

b_{2} = \frac{D \cdot E_{2} \times P}{D \cdot N}

②

t = \frac{- P \cdot N}{D \cdot N}

③

(5)判断计算得到的b₁、b₂和t是否满足条件：b₁≥0，b₂≥0，b₁+b₂≤1，t≥0，是，则确定射线与三角线框相交，执行步骤(6)；否，则确定射线与三角线框不相交，当前朝目标方向无障碍，执行步骤(7)；

(6)按照下列公式判断三维场景中是否存在障碍物：

④

⑤

POS(u_pre,v_pre,w_pre)＝POS(u_t,v_t,w_t)+DIR(r_t,y_t,p_t)*δ

(9)以δ为半径，作圆周形锋面扩散计算，确定局部最佳步进点P″′(u_o,v_o,w_o)，然后执行步骤(10)；

P(u_t，v_t,，w_t)＝POS(u_t，v_t，w_t)+Vector3(0，0，OH) ⑥

DIR(r_t，y_t，p_t)＝TARGET(u_e，v_e，w_e)-P(u_t，v_t，w_t) ⑦

上面步骤(8)验证预计算点是否满足高度约束的具体过程如下：

(a)在预计算点处构造探测射线，射线起点P′为步骤(7)中的位置点POS(u_pre,v_pre,w_pre)，射线方向D′为竖直向下，且其坐标为(0,0,-1)；

(b)根据构造的探测射线，确定其与三角线框的交点P″的坐标为POS(u_pre，v_pre，w_pre′)；

(c)计算高度差△_w＝P′·w_pre-P″·w_pre′，若△_w≤OH，则满足高度约束，执行步骤(11)；否则，执行步骤(9)。

而步骤(9)确定局部最佳步进点P″′(u_o,v_o,w_o)的方式则如下：

(a)以当前运动质点POS(u_t,v_t,w_t)为圆心，δ为半径，以U轴正向处(u_t，δ)为起点，沿圆周划分为8*2^k-1个均匀分布点，k表示最大轮数，并且k＝4，l表示单步最大搜索轮数；

(b)根据最大轮数，按照公式iLevelSet[i]＝i*5.625循环计算水平集iLevelSet，i为正数，且i<64；

(c)根据最大轮数，每一轮均按8*2^i-1次方数量从水平集iLevelSet中选取备选数据，并且每选取一轮后，均进行标记，避免对已访问过的备选数据重复计算，若某一轮中存在有效的步进点，则进入步骤(9d)，该轮对应的水平集表示为iLevelSet[j]；否则确定无法寻找到达目标的路径，寻径结束；

(d)利用公式P′＝POS(u_t,v_t,w_t)+(δ*cos(iLevelSet[j])，δ*sin(iLevelSet[j])，OH)计算P′，并设定射线方向D′的坐标为(0,0,-1)；

(e)根据P′、D′构造射线，确定其与三角线框的交点P″的坐标为(u_pre，v_pre，w_pre)；

(f)计算高度差△_w＝(u_pre，v_pre，w_pre)·w_t－(u_pre，v_pre，w_pre)·w_pre，若△_w≤OH，则满足高度约束，加入备选CandiSet(P″)，并标记为已访问；否则结束寻径。

上述计算过程的伪代码如下：

由于目前采用的候选点集方法具有对称性，即从当前位置出发，总能将上一节点增选入当前节点的候选节点，进而可能形成局部的循环。因此，通过增设对三角形线框的访问标记，在设定阈值达到后可对该候选点过滤，避免产生上述局部循环状态。

而备选集中的点均为从当前点位置可行的行进方向，本实施例提供了两种方案，可用于局部优化选择。

方案一、空间距离优化选择

从备选的CandiSet(P″)中选取一个元素P′，并根据公式Distance＝|P′－P|得到P′与P的距离，然后判断该Distance是否大于或等于最小距离minDistance，是，则令P″＝P′，执行步骤(10)；否，则结束寻径。其伪代码如下：

P′＝CandiSet.GetUniqueElement

Distance＝|P′－P|

如果minDistance≤Distance，则令P″＝P′，执行步骤(10)。

该方案的优点是较为直观，从当前位置总是找最接近目标点的步进点更新，

方案二、备选点的几何特性优化选择

从备选的CandiSet(P″)中选取一个元素P′，并得到与该元素相应的返回值optValue，然后判断该optValue是否大于或等于最大返回值maxoptValue，是，则令P″＝P′，执行步骤(10)；否，则结束寻径。其伪代码如下：

P′＝CandiSet.GetUniqueElement

optValue＝optFunc(curTrimeshNumber)

如果maxOptValue≤optValue，则令P″＝P′，执行步骤(10)。

说明：optValue为函数optFunc[优化函数]在实参curTrimeshNumber[当前三角线框序号]时的返回值。

本发明仅根据当前运动位置，作沿圆周均匀分布的径向锋面扩散路径计算，单步计算无记忆性，为局部优化方法。在后续研究中，为更好使用锋面传播的特点，可用于全局性的路径优化算法。另外，对于对象行进方向、旋转的障碍检测，本发明同样适用。

本发明从三维地形的表示方法、局部几何特性表达、锋面传播方法等多个方面进行分析和设计，对三维场景自动寻径方式进行了有效的优化，实现了即时路径的快速、自动搜索。因此，本发明相比现有技术来说，具有突出的实质性特点和显著的进步。

上述实施例仅为本发明的优选实施例，并非是对本发明保护范围的限制，但凡采用本发明的设计原理，以及在此基础上进行非创造性劳动而作出的变化，均应属于本发明的保护范围之内。

Claims

1.一种三维场景的即时寻径方法，其特征在于，包括以下步骤：

(3)按照方向DIR以逆时针CCW的顺序得到三角线框；

b_{1} = \frac{D \cdot P \times E_{2}}{D \cdot N}

①

b_{2} = \frac{D \cdot E_{2} \times P}{D \cdot N}

②

t = \frac{- P \cdot N}{D \cdot N}

③

(6)按照下列公式判断三维场景中是否存在障碍物：

l_{1} = t * s q r t ({r_{t}}^{2} + y_{t}^{2}) / s q r t ({r_{t}}^{2} + y_{t}^{2} + p_{t}^{2})

④

l₂＝|TARGET(u_e,v_e)-POS(u_t,v_t)| ⑤

POS(u_pre,v_pre,w_pre)＝POS(u_t,v_t,w_t)+DIR(r_t,y_t,p_t)*δ

P(u_t，v_t,，w_t)＝POS(u_t，v_t，w_t)+Vector3(0，0，OH) ⑥

DIR(r_t，y_t，p_t)＝TARGET(u_e，v_e，w_e)－P(u_t，v_t，w_t) ⑦

2.根据权利要求1所述的一种三维场景的即时寻径方法，其特征在于，所述步骤(8)包括以下步骤：

3.根据权利要求2所述的一种三维场景的即时寻径方法，其特征在于，所述步骤(9)包括以下步骤：

4.根据权利要求2所述的一种三维场景的即时寻径方法，其特征在于，所述步骤(9)包括以下步骤：