CN102831298A - 一种用于目标发生器的目标实时路径规划方法 - Google Patents

Abstract

本发明公开了一种用于目标发生器的目标实时路径规划方法，通过采用模糊规则实现虚拟目标运行参数的实时计算，同时，目标动态检测环境中的动态障碍物，根据障碍物的威胁度与位置信息，规划一条新的路径实现动态避障，大大提高了目标发生器的智能性；在障碍物对目标威胁较小时，采用几何算法，有效地减小路径规划算法的复杂度，加快虚拟场景渲染速度并提高虚拟目标运动画面的连贯性；在障碍物对目标威胁较大时，采用模糊算法控制目标快速避开障碍物，有效避免由于位姿计算更新跟帧频率不同步而造成的跳帧或运动不连贯的现象，提高目标发生器的画面质量。

Description

一种用于目标发生器的目标实时路径规划方法

技术领域

本发明涉及虚拟仿真领域，具体涉及一种用于目标发生器的目标实时路径规划方法。

背景技术

随着图像检测技术的发展，与之相关的系统优化、系统效能评估及搜索算法的性能评估等技术也得到了很大的发展。图像检测仿真系统的研制特别是成像系统的研制、改进和性能测试，需要在实验室仿真或在实战情况下进行。而目标发生器图像检测仿真系统提供精确可控、可重复的试验环境，提高了试验效率加快了研究进度。

现有的视频检测技术多数是基于面阵CCD摄像机获得的图像，任何一种CCD探测系统，不论其复杂程度如何，最终都必须在现场进行试验，以便进行验证和评价其性能是否达到设计的指标。然而，频繁地采取实地实物试验，有时气候设备等条件是不完备的，实现很困难一次试验又只能得到有限的数据。若要得到不同场景下的性能测试数据，就必须重新实验，这将花费大量的时间和经费。此外，现场试验检测出来的数据往往难以与真实数据比对，难以完成系统评价。

为了满足试验条件的可控、可重复和供比对数据的需求，虚拟现实技术与分布式系统仿真技术被广泛应用到目标生成器的开发中，现有的目标发生器利用虚拟现实技术构建逼真的虚拟环境对现实世界进行“沉浸式”仿真。一般在虚拟环境中构建森林、丘陵、平原场景及每类场景有不同的特征事物，同时，为提高虚拟环境的沉浸感与真实感，采用粒子系统为虚拟环境创建雨、雪、雾等自然现象的特效，采用Vega特殊效果模拟模块实现动态添加爆炸等特效。确定目标的起点、终点以及路径后，目标在虚拟环境中运动，由此来模拟真正的目标（汽车、坦克等）运动。将目标在虚拟环境中的运动过程采用屏幕投影的方式输出，由此为CCD图像检测跟踪系统实验提供了一个动态变化的虚拟环境以及运动目标。

现有的虚拟目标发生器中，运动目标的运动控制是虚拟场景真实性的关键部分。常用的实现方法有两种，一种是根据vega中的path tool设定运动目标的行驶路径，在系统运行过程中，目标按设定路径与运动方式行驶，这样方法容易实现，且渲染速度较快，但是运动目标的系统运行过程中不能更改路径。另外一种方法，是采用vega自带的API函数，用自定义的方式设定目标行驶路径的位置控制点和速度，这样可以实现在系统运行过程中，每次开始行驶路径，可以更改目标的路径和速度大小。但与现实中的目标相比，现有的虚拟目标的机动性效果仍有待改善与提高。运动目标无法感知虚拟环境信息，不能对实时变化的环境信息做出反应；也不能接收分布式仿真系统的指挥交互信息以及其它相邻目标的信息，不具有改变行为和行驶规则的自适应机制，不能最终实现实时动态路径规划。

发明内容

有鉴于此，本发明提供了一种用于目标发生器的目标实时路径规划方法，目标能够感知虚拟环境信息，实现实时路径规划，同时降低路径规划算法的复杂程度，提高运算速度和提高虚拟目标运动画面的连贯性。

本发明的一种用于目标发生器的目标实时路径规划方法，包括如下步骤：

步骤1、将用户在虚拟场景中选择的所有的路径控制点中相邻的两点连接起来，最终将起点与终点之间连成具有多条线段的曲线，将该曲线作为目标的原始路径；

步骤2、根据目标所在位置当前的虚拟环境的能见度、风向、地形地貌以及障碍物位置信息坐标目标的初始环境信息，计算目标的初始速度，具体方法为：

对能见度s进行标准化处理，使能见度s在[0,100]内变化；同时，计算地形综合因子d＝V+Δz，其中Δz＝z_t-z_t-1，z_t表示当前地形的高程值，z_t-1表示上一秒地形的高程值，V表示地形的遮挡量，然后对地形综合因子d进行标准化处理，使其在[0,10]范围内变化；

将目标的运动速度v的变化范围设定在[0,100]，将能见度s和地形综合因子d作为输入变量，将目标的速度v作为输出变量，首先将输入变量模糊化：将能见度s的取值范围分成7段，0-20为负大SNB，20-30为负中SNM，30-40为负小SNS，40-50为零SNP，50-60为正小SPS，60-70为正中SPM，70-100为正大SPB；将地形综合因子的取值范围分成7段，0-2为负大DNB，2-3为负中DNM，3-4为负小DNS，4-5为零DNP，5-6为正小DPS，6-7为正中DPM，7-10为正大DPB；将输出的速度V的取值范围分成7段，0-20为负大VNB，20-30为负中VNM，30-40为负小VNS，40-50为零VNP，50-60为正小VPS，60-70为正中VPM，70-100为正大VPB；采用三角形函数作为能见度s、地形综合因子d和速度v的隶属度函数，建立49条模糊规则：

将当前的能见度s和地形综合因子d输入到步骤2建立的49条模糊规则中，计算得到四个非零的输出速度v的模糊集VF1,VF2,VF3,VF4和对应的隶属度vf1,vf2,vf3,vf4；采用重心法计算得到速度v的精确值：v＝(VF1×vf1+VF2×vf2+VF3×vf3+VF4×vf4)/(vf1+vf2+vf3+vf4)，根据得到速度大小控制目标沿原始路径运动；

步骤3、对目标的路径进行动态规划，具体方法为：

S301、目标运动过程中，实时计算目标与障碍物中心的距离：

$d=\sqrt{{(x_{\mathrm{obs}}-x_t)}^2+{(y_{\mathrm{obs}}-y_t)}^2+{(z_{\mathrm{obs}}-z_t)}^2},$ 则dir＝d_now-d_past；

其中x_obs,y_obs,z_obs为目标在虚拟环境中的当前位置坐标，障碍物中心的位置坐标为p_t＝(x_t,y_t,z_t)；dir表示目标当前与障碍物中心的距离d_now和上一时刻目标与障碍物中心的距离d_past的差；

S302、判断dir的值：当dir＞0时，控制目标按照原始路径以初始速度进行运动；

当dir＜0时，根据步骤S301中计算所得的距离，计算障碍物对目标的威胁值M：

$M=\left\{\begin{array}{cc}0& (d-d_s>0)\\ 0.8\frac{d_s-d}{d_s}-0.2\frac{\mathrm{dir}}{\left|\mathrm{dir}\right|}& (d-d_s\≤0)\end{array}\right.,$ 其中 $d_s=\frac{{v_t}^2}{2\×(0.083v_t-0.618)}+3.6$ 为安全距离，v_t为目标的当前时刻的速度；

判断威胁值M与设定的反应安全阈值Ms的关系，所述反应安全阈值Ms表示运动目标能避开障碍物的最大威胁值；超过Ms运动目标将受到损伤；当计算得到的威胁值

时，采用几何算法实现新路径最优规划，使虚拟目标以最短时间绕过障碍物，最终沿着原始路径行驶，具体方法为：实时计算目标的运动速度v_t+1＝v_t-10×M，控制目标以时刻速度v_t+1运动；记目标当前位置点O与下一个路径标记点A之间的线段为OA，计算目标当前位置O与障碍物左侧边缘点连线与线段OA之间的夹角θ₁，计算目标当前位置点O与障碍物右侧边缘点连线与线段OA之间的夹角θ₂；目标当前位置点O与较小夹角对应的边缘点C连线记为线段OC，障碍物外切圆与线段OC的交点记为D点，则以边缘点C为圆点，线段CD的长为半径的圆记为圆C，经过A点的圆C的切线记为AP，经过O点的圆C的切线记为OQ，与圆C相切且与直线AO平行的直线与AP和OQ的交点记为F和E，则控制目标沿路径OEFA绕过障碍物，最终目标回到原始路径，然后执行步骤3；

当计算得到的威胁值满足

时，目标的实时路径规划方法如下：

将目标当前速度的方向和目标与下一个路径标记点连线之间的夹角定义为D0，将目标下一时刻需要输出的转向角定义为D1；将威胁值M和D0作为输入变量，将D1作为输出变量，建立模糊函数，同时规定M,D0和D1的隶属度函数采用三角形隶属度函数；将威胁值M在范围内均分成三段，分别定义为小、中和大，将D0的范围均分成7段，按照从小到大依次定义为NB、NM、NS、NP、PS、PM和PB，建立21条模糊规则：

将当前夹角D0和威胁值M和输入集输入到所述饿21条模糊规则中，得到输出转角D1的4个模糊集，分别为DF1、DF2、DF3和DF4，对应的隶属度分别为df1、df2、df3和df4；采用重心法将角度输出的模糊量转换成清晰量，得到： $D1=\frac{\mathrm{DF}1\×\mathrm{df}1+\mathrm{DF}2\×\mathrm{df}2+\mathrm{DF}3\×\mathrm{df}3+\mathrm{DF}4\×\mathrm{df}4}{\mathrm{df}1+\mathrm{df}2+\mathrm{df}3+\mathrm{df}4};$

实时计算目标的运动速度：v_t+1＝v_t-10×M，并控制目标以速度v_t+1且沿转向角D1的方向运动；在目标运动中实时判断目标障碍物中心的距离，当两者之间的距离大于安全距离

的两倍时，将此时目标所在的位置作为新的起始点，将障碍物前方的原始路径中的路径控制点中寻找，该路径控制点满足：与障碍物中心距离最近且该距离大于安全距离

该路径控制点定义为新的终点，控制目标沿着新的起始点和新的终点的连线运动，当目标运动到原始路径上后，执行步骤3，直到目标运动到原始路径的终点，完成本次的实时路径规划。

本发明基于人工智能算法的思想提出一种用于目标发生器的目标实时路径规划方法，通过采用模糊规则实现虚拟目标运行参数的实时计算，同时，智能虚拟目标动态检测环境中的动态障碍物，根据障碍物的威胁度与位置信息，规划一条新的路径实现动态避障，大大提高了目标发生器的智能性；在障碍物对目标威胁较小时，采用几何算法，有效地减小路径规划算法的复杂度，加快虚拟场景渲染速度并提高虚拟目标运动画面的连贯性；在障碍物对目标威胁较大时，采用模糊算法控制目标快速避开障碍物，有效避免由于位姿计算更新跟帧频率不同步而造成的跳帧或运动不连贯的现象，提高目标发生器的画面质量；本发明的实时路径规划方法可应用到智能虚拟目标发生器中，是目标检测、目标分析算法评估的重要基础性工作，在国防安全、智能交通等需要为图像检测跟踪算法试验评估等诸多领域都具有广泛的应用前景和市场价值。

附图说明

图1为本发明中计算初始速度步骤中的隶属度函数曲线，其中图a为能见度S的隶属度函数曲线、图b为地形综合因子d的隶属度函数曲线，以及图c为输出的速度V的隶属度函数曲线。

图2为本发明采用几何算法实现的新路径规划。

图3为本发明中目标威胁值较大时计算目标转角步骤中的隶属度函数曲线，其中图a为威胁值M的隶属度函数曲线、图b为夹角D0的隶属度函数曲线，以及图c为输出的转角D1的隶属度函数曲线。

图4为本发明中目标威胁值较大时新路径规划的几何关系图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种用于目标发生器的目标实时路径规划方法，包括如下步骤：

步骤1、在构建目标发生器的前期工作中，首先要构建虚拟环境，在虚拟环境中构建了多种环境因素和特效，形成一个环境元素数据库，可以通过用户需求实时调用环境元素数据库中的环境元素，包括不同的地形、不同的光、天气等特效条件，提高环境的可控性；用户还可根据需要在虚拟环境中选择多个路径控制点，形成原始路径，本发明中，将用户在虚拟场景中选择的所有的路径控制点中相邻的两点连接起来，最终将起点与终点之间连成具有多条线段的曲线，将该曲线作为目标的原始路径；

步骤2、环境的能见度主要受雾效果以及雨雪等恶劣天气影响，雾气越少，能见度越高，表示行驶环境越好，在雨雪天气下的恶劣环境中，则环境能见度比较低，运动目标行驶环境较差。根据目标所在位置当前的虚拟环境的能见度、风向、地形地貌以及障碍物位置信息坐标目标的初始环境信息，计算目标的初始速度，具体方法为：

首先对能见度s进行标准化处理，使能见度s在[0,100]内变化；然后计算地形综合因子d＝V+Δz，其中Δz＝z_t-z_t-1，z_t表示当前地形的高程值，z_t-1表示上一秒地形的高程值，V表示地形的遮挡量，可以从目标当前所在虚拟环境中直接提取；然后对地形综合因子d进行标准化处理，使其在[0,10]范围内变化；

在平原上，地形平坦运动目标行驶状态良好，森林环境下将造成较大的遮挡，而丘陵环境则存在严重的颠簸现象，地形状况较差，因此，目标速度不能过快。模糊综合判断方法是应用模糊关系合成原理，将一些边界不清、不易定量的因素定量化，多个因素对被评价事物隶属度等级状况进行综合性评价的一种方法。主要应用模糊变换原理和最大隶属度原则，考虑与被评判事物有关的各个因素而做出的综合判断。确定影响目标速度的环境因素有环境能见度和地形综合影响因素，将目标获取的环境信息进行模糊化，使输入精确量转化为模糊量。在动态虚拟环境下的目标速度模糊控制的思想是，目标在一个稳定的环境中，会选择相对较快的速度前行，如果遇到恶劣环境，目标会自动减低速度到一个安全速度行驶。具体体现在，虚拟环境能见度越高，地形越平坦，视线遮挡越少，则速度相对较快；若环境能见度差，地形起伏，视线遮挡多，则目标速度自动放慢。

根据以上思想，将目标的速度v作为输出变量，将能见度s和地形综合因子d作为输入变量：将能见度s的取值范围分成7段，0-20为负大SNB，20-30为负中SNM，30-40为负小SNS，40-50为零SNP，50-60为正小SPS，60-70为正中SPM，70-100为正大SPB；将地形综合因子的取值范围分成7段，0-2为负大DNB，2-3为负中DNM，3-4为负小DNS，4-5为零DNP，5-6为正小DPS，6-7为正中DPM，7-10为正大DPB；将目标的运动速度v的变化范围设定在[0,100]，将输出的速度V的取值范围分成7段，0-20为负大VNB，20-30为负中VNM，30-40为负小VNS，40-50为零VNP，50-60为正小VPS，60-70为正中VPM，70-100为正大VPB，如图1所示，采用三角形函数作为能见度s、地形综合因子d和速度v的隶属度函数，建立49条模糊规则，如下表所示：

将当前的能见度s和地形综合因子d输入到步骤2建立的模糊规则中，根据三角形隶属度函数的属性，计算得到四个非零的输出速度v的模糊集VF1,VF2,VF3,VF4和对应的隶属度vf1,vf2,vf3,vf4；采用重心法计算得到速度v的精确值：v＝(VF1×vf1+VF2×vf2+VF3×vf3+VF4×vf4)/(vf1+vf2+vf3+vf4)，根据得到速度大小控制目标沿原始路径运动；

步骤3、虚拟目标通过感知系统获取环境变化信息，以及通过分布式仿真系统的交互得到的信息，对周围环境实时检测障碍物动态，根据障碍物的威胁值大小进行决策，当障碍物进入目标威胁范围时，目标需要进行路径新规划并设定安全速度值。像现实世界中一样，对目标的路径进行动态规划，实现主动的避免动作，具体方法为：

S301、目标运动过程中，实时计算目标与障碍物中心的距离：

$d=\sqrt{{(x_{\mathrm{obs}}-x_t)}^2+{(y_{\mathrm{obs}}-y_t)}^2+{(z_{\mathrm{obs}}-z_t)}^2},$ 则dir＝d_now-d_past；

其中x_obs,y_obs,z_obs为目标在虚拟环境中的当前位置坐标，障碍物中心的位置坐标为p_t＝(x_t,y_t,z_t)；dir表示目标当前与障碍物中心的距离d_now和上一时刻目标与障碍物中心的距离d_past的差；

S302、判断dir的值：当dir＞0时，说明目标正在远离障碍物，控制目标按照原始路径以初始速度进行运动；

当dir＜0时，目标接近障碍物，因此要采取措施使目标绕过障碍物，基于清华大学汽车安全与节能国家重点实验室的侯德藻等在2005年《汽车工程》中的论文“新型汽车主动避障安全距离模型”中提出的虚拟目标在虚拟场景中的安全距离模型，再根据步骤S301中计算所得的距离d和dir，计算障碍物对目标的威胁值M：

$M=\left\{\begin{array}{cc}0& (d-d_s>0)\\ 0.8\frac{d_s-d}{d_s}-0.2\frac{\mathrm{dir}}{\left|\mathrm{dir}\right|}& (d-d_s\≤0)\end{array}\right.,$ 其中v_t为目标的当前时刻的速度，为安全距离，为运动目标与障碍物的距离变化率，与障碍物威胁值成正相关，此处设定的相关系数为0.8，而威胁值与障碍物的方向的正负值

成反相关，此处设定的相关系数为0.2；

判断威胁值M与设定的反应安全阈值Ms的关系，此处的反应安全阈值Ms表示运动目标能避开障碍物的最大威胁值；表示当障碍物威胁值超过一定值时，目标没有足够的反应时间，每一种运动目标类型的Ms是一个常量（本发明中的运动目标之一汽车的安全阈值为50）。

当计算得到的威胁值

时，说明此时障碍物对目标的威胁较小，此时可采用计算较复杂的几何算法实现新路径最优规划，使虚拟目标以最短时间绕过障碍物，最终沿着原始路径行驶，具体方法为：实时计算目标的运动速度v_t+1＝v_t-10×M，控制目标以速度v_t+1运动；如图2所示，记目标当前位置点O与下一个路径标记点A之间的线段为OA，计算目标当前位置O与障碍物左侧边缘点连线与线段OA之间的夹角θ₁，计算目标当前位置点O与障碍物右侧边缘点连线与线段OA之间的夹角θ₂；目标当前位置点O与较小夹角对应的边缘点C连线记为线段OC，障碍物外切圆与线段OC的交点记为D点，则以边缘点C为圆点，线段CD的长为半径的圆记为圆C，经过A点的圆C的切线记为AP，经过O点的圆C的切线记为OQ，与圆C相切且与直线AO平行的直线与AP和OQ的交点记为F和E，则控制目标沿路径OEFA绕过障碍物，最终目标回到原始路径，然后执行步骤3；

当计算得到的威胁值满足

时，说明此时障碍物对目标威胁较大，几何算法由于计算相对复杂，因此考虑采用模糊推理快速判断目标转角，较大程度偏离原始路径，但保证安全行驶。模糊控制算法基于模糊集合理论，本发明构造运动目标角度控制模糊算法，首先制定规则库和相应的数据库，根据威胁值及目标与障碍物的夹角的模糊输入和模糊控制规则，推理后输出目标路径偏转角度，具体的目标实时路径规划方法如下：

如图4所示，设目标当前位置点为O点，将目标当前速度的方向和目标与下一个路径控制点A连线OA之间的夹角定义为D0，将目标下一时刻需要输出的运动方向与线段OA的夹角定义为转向角D1；将威胁值M和D0作为输入变量，将D1作为输出变量，建立模糊函数，同时规定M,D0和D1的隶属度函数采用三角形隶属度函数；将威胁值M在

范围内均分成三段，分别定义为小、中和大，将D0的范围均分成7段，按照从小到大依次定义为NB、NM、NS、NP、PS、PM和PB，建立21条模糊规则：

将当前夹角D0和威胁值M和输入集输入到所述的21条模糊规则中，得到输出转角D1的4个模糊集，分别为DF1、DF2、DF3和DF4，对应的隶属度分别为df1、df2、df3和df4；采用重心法将角度输出的模糊量转换成清晰量，得到： $D1=\frac{\mathrm{DF}1\×\mathrm{df}1+\mathrm{DF}2\×\mathrm{df}2+\mathrm{DF}3\×\mathrm{df}3+\mathrm{DF}4\×\mathrm{df}4}{\mathrm{df}1+\mathrm{df}2+\mathrm{df}3+\mathrm{df}4};$

实时计算目标的运动速度：v_t＝v_t-1-10×M，并控制目标以当前速度v_t且沿转向角D1的方向运动；在目标运动中实时判断目标障碍物中心的距离，当两者之间的距离大于安全距离

的两倍时，将此时目标所在的位置点P作为新的起始点，在障碍物前方的原始路径中的路径控制点中寻找，该路径控制点满足：与障碍物中心距离最近且该距离大于安全距离

该路径控制点A定义为新的终点，控制目标沿着新的起始点和新的终点的连线PA运动，当目标运动到原始路径上后，重复执行步骤3，直到目标运动到原始路径的终点，完成本次的实时路径规划。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种用于目标发生器的目标实时路径规划方法，其特征在于，包括如下步骤：

步骤1、将用户在虚拟场景中选择的所有的路径控制点中相邻的两点连接起来，最终将起点与终点之间连成具有多条线段的曲线，将该曲线作为目标的原始路径；

步骤2、根据目标所在位置当前的虚拟环境的能见度、风向、地形地貌以及障碍物位置信息坐标目标的初始环境信息，计算目标的初始速度，具体方法为：

对能见度s进行标准化处理，使能见度s在[0,100]内变化；同时，计算地形综合因子d＝V+Δz，其中Δz＝z_t-z_t-1，z_t表示当前地形的高程值，z_t-1表示上一秒地形的高程值，V表示地形的遮挡量，然后对地形综合因子d进行标准化处理，使其在[0,10]范围内变化；

将目标的运动速度v的变化范围设定在[0,100]，将能见度s和地形综合因子d作为输入变量，将目标的速度v作为输出变量，首先将输入变量模糊化：将能见度s的取值范围分成7段，0-20为负大SNB，20-30为负中SNM，30-40为负小SNS，40-50为零SNP，50-60为正小SPS，60-70为正中SPM，70-100为正大SPB；将地形综合因子的取值范围分成7段，0-2为负大DNB，2-3为负中DNM，3-4为负小DNS，4-5为零DNP，5-6为正小DPS，6-7为正中DPM，7-10为正大DPB；将输出的速度V的取值范围分成7段，0-20为负大VNB，20-30为负中VNM，30-40为负小VNS，40-50为零VNP，50-60为正小VPS，60-70为正中VPM，70-100为正大VPB；采用三角形函数作为能见度s、地形综合因子d和速度v的隶属度函数，建立49条模糊规则：

将当前的能见度s和地形综合因子d输入到步骤2建立的49条模糊规则中，计算得到四个非零的输出速度v的模糊集VF1,VF2,VF3,VF4和对应的隶属度vf1,vf2,vf3,vf4；采用重心法计算得到速度v的精确值：v＝(VF1×vf1+VF2×vf2+VF3×vf3+VF4×vf4)/(vf1+vf2+vf3+vf4)，根据得到速度大小控制目标沿原始路径运动；

步骤3、对目标的路径进行动态规划，具体方法为：

S301、目标运动过程中，实时计算目标与障碍物中心的距离：

$d=\sqrt{{(x_{\mathrm{obs}}-x_t)}^2+{(y_{\mathrm{obs}}-y_t)}^2+{(z_{\mathrm{obs}}-z_t)}^2},$ 则dir＝d_now-d_past；

其中x_obs,y_obs,z_obs为目标在虚拟环境中的当前位置坐标，障碍物中心的位置坐标为p_t＝(x_t,y_t,z_t)；dir表示目标当前与障碍物中心的距离d_now和上一时刻目标与障碍物中心的距离d_past的差；

S302、判断dir的值：当dir＞0时，控制目标按照原始路径以初始速度进行运动；

当dir＜0时，根据步骤S301中计算所得的距离，计算障碍物对目标的威胁值M：

$M=\left\{\begin{array}{cc}0& (d-d_s>0)\\ 0.8\frac{d_s-d}{d_s}-0.2\frac{\mathrm{dir}}{\left|\mathrm{dir}\right|}& (d-d_s\≤0)\end{array}\right.,$ 其中 $d_s=\frac{{v_t}^2}{2\×(0.083v_t-0.618)}+3.6$ 为安全距离，v_t为目标的当前时刻的速度；

判断威胁值M与设定的反应安全阈值Ms的关系，所述反应安全阈值Ms表示运动目标能避开障碍物的最大威胁值；超过Ms运动目标将受到损伤；

当计算得到的威胁值

时，采用几何算法实现新路径最优规划，使虚拟目标以最短时间绕过障碍物，最终沿着原始路径行驶，具体方法为：实时计算目标的运动速度v_t+1＝v_t-10×M，控制目标以时刻速度v_t+1运动；记目标当前位置点O与下一个路径标记点A之间的线段为OA，计算目标当前位置O与障碍物左侧边缘点连线与线段OA之间的夹角θ₁，计算目标当前位置点O与障碍物右侧边缘点连线与线段OA之间的夹角θ₂；目标当前位置点O与较小夹角对应的边缘点C连线记为线段OC，障碍物外切圆与线段OC的交点记为D点，则以边缘点C为圆点，线段CD的长为半径的圆记为圆C，经过A点的圆C的切线记为AP，经过O点的圆C的切线记为OQ，与圆C相切且与直线AO平行的直线与AP和OQ的交点记为F和E，则控制目标沿路径OEFA绕过障碍物，最终目标回到原始路径，然后执行步骤3；

当计算得到的威胁值满足

时，目标的实时路径规划方法如下：

将目标当前速度的方向和目标与下一个路径标记点连线之间的夹角定义为D0，将目标下一时刻需要输出的转向角定义为D1；将威胁值M和D0作为输入变量，将D1作为输出变量，建立模糊函数，同时规定M,D0和D1的隶属度函数采用三角形隶属度函数；将威胁值M在

范围内均分成三段，分别定义为小、中和大，将D0的范围均分成7段，按照从小到大依次定义为NB、NM、NS、NP、PS、PM和PB，建立21条模糊规则：

将当前夹角D0和威胁值M和输入集输入到所述饿21条模糊规则中，得到输出转角D1的4个模糊集，分别为DF1、DF2、DF3和DF4，对应的隶属度分别为df1、df2、df3和df4；采用重心法将角度输出的模糊量转换成清晰量，得到： $D1=\frac{\mathrm{DF}1\×\mathrm{df}1+\mathrm{DF}2\×\mathrm{df}2+\mathrm{DF}3\×\mathrm{df}3+\mathrm{DF}4\×\mathrm{df}4}{\mathrm{df}1+\mathrm{df}2+\mathrm{df}3+\mathrm{df}4};$

实时计算目标的运动速度：v_t+1＝v_t-10×M，并控制目标以速度v_t+1且沿转向角D1的方向运动；在目标运动中实时判断目标障碍物中心的距离，当两者之间的距离大于安全距离

的两倍时，将此时目标所在的位置作为新的起始点，将障碍物前方的原始路径中的路径控制点中寻找，该路径控制点满足：与障碍物中心距离最近且该距离大于安全距离该路径控制点定义为新的终点，控制目标沿着新的起始点和新的终点的连线运动，当目标运动到原始路径上后，执行步骤3，直到目标运动到原始路径的终点，完成本次的实时路径规划。

Images