专利名称：在城市环境下微型飞行器三维轨迹仿真方法
技术领域：
在城市环境下微型飞行器三维轨迹仿真方法属于微型飞行器三维实时路径导航技术领域。
背景技术：
三维路径导航视景仿真系统针对复杂的地形环境，对运动对象集群(通常为无人飞行器)的三维航迹进行规划和导航，是近年出现的国内外虚拟现实研究的热门课题。目前国内外研究较多的是机器人避障碍物或者无人机躲避威胁点(如雷达等)的路径规划，由于地面机器人只能在地平面上运动，而无人机飞行的高度一般较高，因此这两类路径规划算法都只是二维导航问题，对三维路径规划和协同导航仿真技术的研究还比较缺乏。对单机到单目标点路径规划，目前的研究一般采用迪斯科特(Dijsktra)算法进行二维路径规划，对障碍物采取绕行方式，这样运动对象凡是遇到障碍物都必须绕行，反而浪费资源或增大危险。本发明采用迪斯科特(Dijsktra)算法进行三维路径规划。对障碍物采取两种通过方式绕行和飞跃，提高了路径规划的适用性和安全性。另外，本发明针对复杂地形环境，以城市内微型飞行器低空飞行作战为背景，既包含地面敌方火力/侦察威胁点，也包含建筑物等有高度指标的障碍物。把二者结合在一起应用于飞行群的协同仿真尚属首次。
从系统的接口功能来看，一般三维仿真系统通过鼠标、键盘等输入设备和运动模型控制运动对象的运动姿态和轨迹。本系统除具有一般仿真系统的硬件接口输入功能外，还具有软件接口输入功能，即可从路径规划系统实时输入导航路径到仿真系统进行飞行仿真，在同类研究中尚属首次。此外，在同类三维仿真系统中，运动对象若采用路径控制运动方式，则该路径一般采用固定导航路径，通过预先制定曲线方程或输入坐标矩阵方式生成。本系统的路径导航器突破了这一限制，能够在程序运行时实时生成可随输入参数变化的动态导航路径。因此，本系统是目前国内外已有成果中具有创新意义的仿真系统。
发明内容
本发明的目的在于提供一个基于复杂地形环境的三维环境的实时路径导航仿真系统，为实现微飞行群战斗系统打下理论基础。
本发明特征在于它是一个针对三维路径导航，拥有逼真视景仿真效果，可从路径规划系统实时输入数据到视景仿真系统的飞行仿真系统。它包含五个模块，各个模块的主要功能为 (1)地图数字化模块 这个部分的主要功能是将复杂的三维作战环境简化成由基本几何图形组成的数字化地图。针对此简化三维模型，给出系统地图初始化的方案，利用几何方法将地图分成各个区域，以实现微飞行群的航迹的规划，该方案分成三个步骤 步骤1将敌军的卫兵或者其他防御工事看成威胁点，将建筑物看成直棱柱。
本发明选择城市内作战的三维环境，由于微型飞行器(Micro Air Vehicle，简称MAV)具有一定的飞行高度，因此选择忽略城市内地形对其飞行轨迹的影响，而主要的影响来自于地表较高的建筑物，因此，用直棱柱来近似城市内的建筑物，将城市环境简化成一个只有直棱柱的三维模型。
假设一个城市内作战的模型，要求一MAV到敌方控制区域内执行侦察任务。如图1所示，在2km×2km的城市环境中，避障对象包括建筑物和均布的有一定作用半径的威胁点，其中坐标原点表示MAV的起飞点，五角星表示将要到达并进行侦察的目标位置，图中的四边形表示建筑物(建筑物的范围需要根据飞行器的导航精度作适当扩大，比如MAV采用GPS导航，根据GPS一般有15米的定位精度，因此建筑物需要在实际建筑范围的基础上向外扩大15m)，斜十字表示敌军的威胁点(如卫兵等)。
步骤2利用威胁点生成迪诺雷(Delaunay)图。
迪诺雷图中导航点的候选者都在战场范围内，相比于人工势场法，得到的路径由折线组成，更适合于MAV飞行。迪诺雷图用三角形将战场划分成各个空间，其意义是任意三角形的外接圆内不包含其他威胁点。因此，选取外接圆法由威胁点生成迪诺雷图，如图2所示。
步骤3运用Matlab函数库求出三角形各边的中点，这些中点组成的几何点群就是候选导航点的集合。
由于各迪诺雷边(以下简写为D边)的中点离D边两端点(即威胁点)的距离最远，因而穿越该点的危险在整条线段上最小，因此选择各D边的中点(图2中用黑点标出)作为导航点的侯选者。(按照飞行器运动学要求和避障要求，每个D边中点有两个高度指标，其一是当MAV通过该点时，有一个高度最大值，MAV高度不能超过该值，否则在到达目标点时，不能具有合适的侦察高度，该指标与该点距离目标点的距离、飞行器的最大下滑角以及到达目标点时的高度有关；其二是MAV飞过该点时有一个高度最小值，防止与地表或地表建筑发生碰撞，该指标与地貌和MAV纵向导航精度有关。当某中点的高度最大值指标小于高度最小值指标时，该点不能作为导航点。) 本模块的输入(由图形用户界面输入)为地图参数和地形、建筑物、树木、人等三维模型，地图参数包括建筑物直棱柱底面各顶点的三维坐标及棱柱高度、威胁点数量及威胁点三维坐标、微型飞行器的数量及起飞点的三维坐标、目标点数量及其三维坐标、飞行场地尺寸，此外，还有侯选导航点的数量及三维坐标。本模块将地图参数和生成的候选导航点输出到航迹规划模块，并将地图参数和三维模型输出到系统建模模块，以建立数字化的带有威胁点的城市逼真三维场景。
(2)航迹规划模块 本模块主要功能是单机到单目标点的轨迹规划和指标计算。用于计算各架飞行器到各个目标点的指标，并记录其最佳飞行轨迹。
本发明选择几何的方法进行单机到单目标点的轨迹规划和指标计算，这样保证了飞行轨迹有较多的直线，从而可以减小飞行的误差。函数，包括路径的资源消耗指标和路径威胁指标。在路径规划中，保证最终得到的路径指标最小。
步骤1根据飞行器实际情况，在路径寻优中，制定路径的寻优指标。
考虑MAV飞行的资源损耗和生存概率，定义以MAV的资源损耗和威胁点对MAV的威胁度作为路径的指标。
所述各路径指标说明如下 (A)路径总指标J 在平面路径规划中，资源损耗指标为路径长度，威胁指标为威胁点发现并攻击飞行器的概率。在三维路径规划中，资源损耗函数不仅包含路径长度，还应包含飞行器的高度变化。考虑飞行器上升高度时要增加能源的使用，而下降高度时节省能源，因此高度指标包括飞行器高度上升和下降两个指标。
定义路径总指标 J＝α(JL+c1HC-c2HD)+βJT(1) 其中JL表示路径在水平面内投影的长度指标，HC表示路径中MAV爬升高度指标，HD表示高度下降的指标，JT表示路径的威胁度指标。α，β分别为资源损耗指标与威胁指标之间的加权系数，c1为爬升高度相对于路径长度的比例系数，与MAV上升时的升阻比有关，其关系表达式为 ，其中dZc为爬升高度，dL为对应路径长度， c2为下降高度相对于路径长度的比例系数，与MAV下降时的升阻比有关，其关系表达式为 ，其中dZd为下降高度，dL为对应路径长度。
在计算总指标时，路径的损耗指标和威胁度指标要进行归一化处理。在路径寻优时，要求总指标J→min。
(B)路径长度指标JL 路径长度指标指MAV空中飞行轨迹在地面上投影的长度，对于通过连接各导航点形成的路径，路径长度指标可以用导航点之间的距离之和来表示，假设路径有n个导航点，则 其中x，y表示导航点在水平面内的坐标，为表示方便，坐标系采用的是东-北-地坐标系。
(C)高度指标 定义MAV爬升高度指标HC为 其中，是计算飞行路径爬升高度的函数，z表示路径的高度，s表示MAV飞行的路径。
MAV轨迹下降高度指标HD为 其中，是计算飞行路径下降高度的函数，其余定义同上。
(D)威胁度指标JT 这里仅考虑分布在战场中的威胁点对飞行器的威胁，比如人、雷达等，因为在寻优过程中可以避免建筑物等障碍物的威胁。通常威胁度指标和MAV与威胁点之间的距离有关，对于雷达，指标与距离的四次方成反比，在本系统中假设威胁点为卫兵或狙击手，根据人眼的光学原理，由于MAV尺寸固定，它对人眼所成的视角与距离成反比，定义一个威胁度密度函数， d为MAV与威胁点之间的距离与威胁点最大作用半径之比，该最大作用半径的取值范围为(0，100)，k为比例系数，其取值范围为(0，10)。路径威胁度指标为路径的威胁度密度函数在整个路径上的积分，即 在计算威胁度积分时，为降低计算量，本系统采用近似的方法。如图3所示，其中Wi和Wi+1是两个导航点，曲线即为威胁度密度函数，本系统计算该段路径威胁度时，取路径上威胁度密度最大的点，以及在密度最大点两侧取的插值点，利用这几个点连接而成的折线来近似威胁度密度函数，通过计算面积得到整条路径的威胁度。
步骤2利用网络寻优的迪斯科特(Dijsktra)算法进行寻优，并在此基础上对障碍物进行局部避障设计，从而得到在上述指标下的最优可行路径。
算法的目标是生成一条最优路径，使得路径的总指标最小。本文采用Dijsktra算法得到最优路径。
在寻优过程中，计算各迪诺雷(Delaunay)边(以下简写为D边)中点组成的网络中两中点之间的指标的过程，包含着一个局部避障寻优的过程，即当两个D边中点之间存在建筑物等障碍物时，或者两中点有处在建筑物之上时，要设计局部的最优路径。此局部最优路径在保证与建筑物等障碍物无碰的前提下，使总指标J最小。此时，可以把这两个D边中点看成起始点和目标点，与建筑物等障碍物的顶点一起作为局部网络的节点，同样利用迪斯科特算法可以得到局部的最优路径。计算两个局部网络节点之间的指标时，直接把两个局部网络节点相连，如果二者可以直接相连，则只需计算路径的威胁度指标和路径长度指标，高度指标为零。如果两所述局部网络节点间的连线穿过障碍物，则增加两个局部网络节点的高度，使其高度等于两个节点之间的建筑物的高度与由微型飞行器的导航精度决定的纵向导航误差之和，来控制路径的高度，计算时需考虑高度指标，即爬升高度和下降高度的两个指标不为零。当节点的高度升高时，如果前面的路径有足够的长度保证MAV在节点处达到需要的高度，并且可以在到达目标点时，达到目标点高度，则两个节点之间具有一定的指标，根据前面的指标进行计算，否则两点之间的指标为无穷大。这样，不仅得到了全局网络中两D边中点间的指标，也达到了避开建筑物等障碍物的目的。
由于网络中节点只有准确的x、y坐标(水平面内位置准确)，路径需要再次的高度规划。通常，当导航点下面有建筑物等障碍物的时候，为安全考虑，该点的高度必须严格控制(称这类导航点为高度控制点)。在得到的路径中，起点和目标点的高度是固定的，可以作为高度控制点，这样整个路径中必定含有两个或者两个以上的高度控制点，在高度规划时，根据这些高度控制点的位置和高度，其他导航点进行插值处理。假设两个高度控制点的高度分别为h1和h2，插值的方法是 (1)h1＜h2时，MAV首先按照尽量大的航迹倾角爬升，爬升到需要的高度时，随后MAV进行等高飞行。
(2)h1＝h2时，MAV等高飞行。
(3)h1＞h2时，MAV首先按照尽量大的航迹倾角下降，下降到需要的高度时，随后MAV进行等高飞行。
步骤3生成飞行轨迹。
由于路径规划算法得到的是由各导航点连线组成的路径，其转角处不满足微型飞行器的运动学约束，因此需要在转弯处进行光滑处理，通常采用的方法是用微型飞行器最佳转弯半径的圆弧来代替折角，该方法有两种方式，一种是用不经过导航点的圆弧代替折角，另一种采用的是用经过导航点的圆弧代替折角，如图5所示。比较两种方法相对于原路径的最大误差Le，得 Le2＝R(1-sinθ)(8) 其中R表示飞行器最佳转弯半径。由于Le1≥Le2始终成立，故选择经过导航点的方法，即用经过导航点的圆弧来代替折线，可以满足威胁最小的要求。
本模块的输入为地图参数及上述各指标系数，输出为各导航点、运动对象的实时运动参数(本发明定义运动参数为运动对象的实时三维坐标和绕坐标系轴的3个转角角度，即位置和姿态坐标值)。该模块的实例计算效果如图5所示。
(3)数据采集模块 本模块对航迹规划模块的路径数据输入进行实时采集并进行数据格式转换，然后将转换后的数据传给系统建模模块。通过把接收的路径数据传给自定义的运动模型，指定其位置和姿态坐标值，实现数据格式转换和实体模型的运动仿真。运动模型结构体定义中，用笛卡儿坐标变量X、Y、Z传输微型飞行器的实时三维坐标值，变量h、p、r传输微型飞行器的俯仰、偏航和滚转角度，所述X、Y、Z、h、p、r六个参数组成微型飞行器的实时运动参数。
若选择采用人机交互运动控制模式，则本模块可实时采集标准鼠标、键盘、头部跟踪器、游戏手柄等输入信号，并进行数据格式转换，然后将转换后的数据传给系统建模模块作为输入。
(4)系统建模模块 本模块负责建立系统模型并进行仿真运算。
步骤1建立和加载系统的三维模型，建立运动对象。
本发明为用户提供两种建模方法，一是可在系统的图形用户界面输入城市建筑外形坐标，在仿真运行的时候实时生成菱柱状简化建筑模型；二是采用多边形建模和纹理技术，预先创建包括单个模型(如运动对象MAV的模型、敌方部队的模型等)和地形表面模型的三维数据库。地形数据库的生成主要运用Creator Terrain Studio创建出高效、连续的自适应地形。单个模型数据库则使用MultiGen Creator、3dsmax等软件以交互方式创建。用户只需在运行系统时选择需要加载的模型即可得到真实感强的三维仿真场景。
步骤2根据数据采集模块输入的运动参数控制运动对象，进行碰撞检测运算。
对于本系统，采用两种碰撞检测方式高度相交矢量和距离相交矢量。其中高度相交矢量可实时返回运动对象与地形(包括建筑等障碍物)的投影交点的高度，当运动对象(MAV)与地形相交时，返回一个碰撞信息。距离矢量功能相当于一个激光测距仪，当运动对象与障碍物的距离等于零时返回碰撞信息，并能返回碰撞点的三维坐标。
系统设置一个碰撞变量，当检测到碰撞信息时，置该变量值为1。在实时循环中程序不断检测该变量，当其值为1时，MAV将停止飞行。为实现较为逼真的模拟效果，将爆炸、烟火、碎片粒子系统和声音的特效与所返回的碰撞信息联系起来。这样MAV与地面、建筑物碰撞时将触发以上特效。并将仿真结果传给视景驱动模块。
(5)视景驱动模块 本模块将得到的仿真结果数据进行三维可视化的显示。它的功能包括取景变换和透视投影。将三维几何描述转换为二维透视图；进行隐藏面消除，即确定场景中的所有可见面，将视域之外或被其它物体遮挡的不可见面消去；进行颜色计算和着色。
本模块同时负责实现特效，特效由图形用户界面输入的特效参数控制。特效参数包括各视角显示通道的数量，显示光、雾、纹理、网格、飞行轨迹、声音、飞行器文字标识等效果的选择，录制和回放功能的选择以及仿真仪表功能参数。
上述五个模块的总体结构如图6所示。地图数字化模块根据从图形用户界面输入的地图参数和所选择的三维模型库生成候选导航点的集合，航迹规划模块根据此候选导航点和地图参数生成飞行轨迹并输出到数据采集模块。数据采集模块收集来自航迹规划模块和来自外部硬件(标准键盘、鼠标、手柄等)的运动参数，传给系统建模模块。系统建模模块建立系统三维模型，进行仿真运算，将结果输出到视景驱动模块进行视景仿真，并用显示器进行显示。仿真结果 按照前面描述的环境进行路径规划的数值仿真，在2km×2km的城市内环境中，有9栋高度较高的建筑物对MAV的飞行路径有影响。同时在环境中具有30个分布的静态威胁点，假设MAV最佳侦察高度为40m，纵向导航精度η＝10m，飞行器起飞点在坐标原点，起飞高度为0。
假设损耗指标与威胁指标之间的权重比为1∶1，c1＝10，c2＝5，MAV的最佳转弯半径R＝25m，生成的路径如图9。其中导航点5在建筑物上方，因此加入了导航点4和6来控制高度，避免与建筑物的边缘相撞。从导航点10到导航点13之间，有建筑物阻断，通过局部寻优，MAV从建筑物上方飞过，因此加入了导航点11和12来控制高度；在导航点15到17之间，由于建筑物的阻隔，进行局部规划，从而增加了一个导航点16，它是建筑物的一个顶点。
设计获得的各导航点高度如表1 表一各导航点的高度 其中粗体的导航点由于在建筑物的上方飞行，必须进行较精确的高度控制，避免出现危险(4、5、6导航点处建筑物高度为54.6m，11、12导航点处建筑物高度为52.3m)。
实例的地图制作程序界面图如图8所示。仿真结果如图9所示。
本发明采用特殊方法提高模型加载和三维渲染速度 (1)在三维虚拟仿真场景的构造中，在忽略各个侧面外观不同的条件下，对于树木等从任何角度看外形都类似的轴对称物体，为提高渲染速度，用一个粘贴了该物体(如树)的图像纹理的面片代替三维的实体造型。但如果视线转动到与面的法向垂直时，这个面就变成了一条直线。本发明通过下面方法予以解决令这个面的法向随着视线的转动而转动，在仿真过程中令该面的法向始终与视线方向平行。此建模方法对于外观对称并呈圆柱形或圆锥形，但又细节复杂不易建立三维立体模型的物体的模型表示非常有用。
(2)本发明中采用多边形建模所得的虚拟场景模型的地形较为复杂，其中具有多个几何形状相同但位置不同的物体，例如有很多结构、形状、纹理相同的房屋，房屋之间的差别仅在于所处的位置、大小、方向的不同，如果把所有房屋都放入内存，将造成极大的浪费。所以本系统采用内存实例的方法，相同的房屋只在内存中存放一份实例，将一棵树木进行平移、旋转、缩放之后得到所有相同结构的树木，因而大大地节约了内存空间。
(3)本模型数据库中采用外部引用技术，在一个模型系统中引入其他的模型，而整个模型的数据总量不会增加多少。本发明所提供的模型库通过使用外部引用，将地形分成25个外部引用的地形文件，并将所有建筑单体和树木文件导入，这样每个建筑模型的纹理文件和三维模型文件都能保持独立，从而使最后的整合文件大小符合处理要求。因此，计算机在处理场景时，不会因为一次性调入过多的图形和纹理而使得速度过慢。
(4)本发明在不同的层次、不同的视觉条件下，采用不同精细程度的模型来表示同一个对象，以加快三维渲染速度。当视点远离对象时，对象被显示为低的细节程度，只包含相关的少数的几个多边形，而当视点向对象移动时，实时系统将其替换为逐渐增加的更复杂的细节程度。本发明为虚拟场景中每个建筑物或它的一些部分构造两到三个细节程度层次，通过改变细节程度属性参数可以决定该层次所出现的视力范围，从而实现各层次之间的过渡。在本系统中，粗糙层次造型的三角面片数目是精细层次造型的10％～20％。
本发明具有如下优点 (1)本发明创建了多层次的复杂地形模型数据库，其中包括一系列地形、道路、房屋、路标、路灯、树、人等单个模型库，以及将单个模型有机组合而成的多个不同的具体场景。包括城市建筑密集型、稀疏型以及城乡结合型等。在此基础上利用环境效果模块建立了逼真的天空盒，能显示一天不同时间、云、雾等变化和光线及阴影的环境特效。在上述虚拟场境中成功导入了在其他软件环境下创建的运动对象(飞行器)模型。
(2)本发明建立了多个实时生成的路径导航器，可分别控制多架飞行器飞行轨迹和姿态。该路径导航器使用路径规划算法得到的结果，在程序运行时即时生成路径，控制MAV在移动和转动方向的六个自由度。另外，系统还建立了人机交互运动控制模式，通过鼠标键盘输入设备对飞行器进行实时控制，可控制飞行器的姿态和各种方向的匀速、加速运动。并且在飞行环境中设置了飞行器和地形、房屋等之间的碰撞检测，并在碰撞检测反馈的基础上，加入了一定的特效，如爆炸和烟雾等。
(3)为提供更多方位的视觉效果和信息，本发明设置了多个通道显示不同角度、对象的实时虚拟效果。其中包括俯视视角和摄像头视角等。用户通过系统的人机交互界面，能实时选择光、雾、纹理、网格等显示效果。在基本的仿真功能基础上，添加了录制和回放功能，可即时录制仿真画面并随时回放。通过图形界面，用户可选择所要录制的对象，系统自动保存录像文件。本系统还加入了仿真仪表功能，该仪表可通过刻度尺和文字两种方式显示飞行器的飞行姿态、高度、速度等参数。用户可在系统运行时选择是否显示由路径规划决定的飞行器飞行轨迹。本系统具有一定的声音效果，可模拟飞机引擎声效和城市噪音声效。另外，系统还具有飞机引擎动态模拟、飞行器文字标识等特殊效果。
本仿真系统的地形尺寸为5000平方米，其中密集建筑群占地约2000平方米。对飞行器(飞行高度约为40米)有威胁的高层建筑物总数量超过30。在路径规划有效的限制下，建筑物间最小距离为20米。仿真系统能支持多架飞行器协同执行任务。运行环境Win2000/xp，Matlab 6.5版本以上，Visual C++6.0版本以上。
图1是地图数字化示意图(其中四边形和直棱柱表示建筑物，斜十字表示威胁点，五角星表示将要到达并进行侦察的目标位置，表示建筑物) 图2是路径威胁度的计算示意图(其中四边形表示建筑物，三角形顶点(斜十字)表示威胁点，标记斜十字的四边形表示将要到达并进行侦察的目标位置，黑点表示各Delaunay边中点) 图3是路径威胁度的计算示意图 图4是用圆弧代替折线方法示意图 图5是单机到单目标点的路径规划模块效果图(其中直棱柱表示建筑物，圆点表示威胁点，黑色折线表示得到的轨迹) 图6是本发明的总体结构 图7是本发明的程序设计流程图 图8是地图制作程序界面图 图9是实例仿真结果 图10是实例的路径导航视景仿真效果图
具体实施例方式 本发明针对地理环境的简化三维模型，给出系统地图初始化的方案，利用几何方法将地图分成各个区域，以实现微飞行群的任务的规划，将该规划的计算结果通过仿真建模运算后，实时地在输出端给出逼真的三维视景仿真。程序设计流程图如图7所示。
本发明实现流程分成以下步骤 步骤一用户通过标准鼠标和键盘，在图形用户界面输入地图参数，指定所加载三维模型库，系统进行地图初始化； 步骤二系统利用威胁点生成Delaunay图，得出导航点的集合； 步骤三系统调用航迹规划模块运算，得出飞行轨迹和运动参数； 步骤四系统调用初始化函数初始化视景仿真进程； 步骤五系统调用系统定义函数载入地图参数和三维模型； 步骤六系统调用运动体注册函数建立各个运动对象，注册相应的运动模型； 步骤七系统通过数据采集模块获得运动参数，赋给运动模型结构体(定义如前述)； 步骤八系统设置一个主循环，调用系统帧函数进行帧同步，实现场景驱动； 步骤九用户通过图形用户界面改变特效参数，系统调用对应特效函数演示特效，用户还可以选择不使用路径导航，而使用标准鼠标、键盘、手柄等控制运动模型的运动； 步骤十用户通过图形用户界面命令系统暂停或停止仿真演示。
上述步骤中，如果采用人机交互运动控制模式，可通过键盘、鼠标、手柄交互控制运动对象，如果采用路径规划导航器，则须预先输入地图参数进行导航路径规划。地图参数经过地图数字化模块生成数字地图，在航迹规划模块生成路径，由数据采集模块转换为运动对象的坐标和姿态参数，使运动模型状态随着导航器作出相应改变。改变结果输向视景驱动平台，生成三维视图并通过显示适配器输出到用户。系统运行时，通过设置一个线程执行上述实时循环来实现仿真功能。
权利要求
1.在城市环境下微型飞行器三维轨迹仿真方法，其特征在于，该方法是在计算机中依次按以下步骤实现的
步骤(1)在计算机中设置以下模块地图数字化模块、航迹规划模块以及数据采集模块；
步骤(2)依次按以下步骤，把复杂的三维作战环境简化为由基本几何图形组成的数字化地图，并初始化；
步骤(2.1)把城市环境简化为一个三维模型，坐标原点表示柔性翼微型飞行器的起飞点，把建筑物视为该三维模型中的一个直棱柱，把敌军卫兵或者其他防御工事视为该三维模型中的一个威胁点，该三维模型中还包含一个表示将要到达并进行侦察的目标位置；
步骤(2.2)用三角形把作为战场的城市环境划分成各个空间，使得任意一个三角形的外接圆内不包含其他威胁点，而只有所述三角形的各顶点为威胁点，从而由各威胁点生成一个迪诺雷Delaunay步骤(2.3)用Matlab函数库求出步骤(2.2)中所有三角形个边的中点，得到候选导航点的集合；
步骤(2.4)通过用户界面把步骤(2.2)、步骤(2.3)得到的三维模型及下述地图参数输入到一个地图数字化模型并初始化，所述地图参数包括建筑物直棱柱底面各顶点的三维坐标及棱柱高度、威胁点数量及威胁点三维坐标、微型飞行器的数量及起飞点的三维坐标、目标点数量及其三维坐标、飞行场地尺寸，此外，还有侯选导航点的数量及三维坐标；
步骤(2.5)该地图数字化模块把步骤(2.4)得到的地图参数及候选导航点集合输出到所述航迹规划模块；
步骤(3)航迹规划模块依次按以下步骤计算在路径总指标J最优条件下的最优可行路径
步骤(3.1)按下式计算路径总指标J
J＝α(JL+c1HC-c2HD)+βJT
其中α，β分别为兼顾资源损耗和威胁点发现并攻击微型飞行器的概率时的加权系数，α，β的取值范围为0～10，JL表示路径在水平面内投影的长度指标，按下式计算
其中，x，y表示导航点在水平面内的坐标，坐标系采用的是东-北-地坐标系，n为导航点数，i为导航点序号；
HC为微型飞行器爬升高度，按下式计算
其中，
fc(dz)是计算飞行路径爬升高度的函数，s是微型飞行器飞行的路径，z表示路径上各点飞行位置的高度，dz是爬升高度变化量，
HD为微型飞行器下降高度指标，
fc(dz)是计算飞行路径下降高度的函数，dz是下降时高度的变化量，
JT是路径的威胁度指标，按下式计算
其中d为微型飞行器与威胁点之间的距离与威胁点最大作用半径之比，该最大作用半径的取值范围为0～100，k为比例系数，其取值范围为0～10，路径威胁度指标JT相邻两个导航点之间的威胁度密度函数T(d)在该两个导航点之间的整个路径上的积分；
步骤(3.2)根据步骤(2.2)得到的候选导航点集合，利用网络寻优的迪斯科特Dijsktra算法依次按照以下步骤，在路径总指标J最小的条件下形成一条最优飞行可行路径
步骤(3.2.1)把所述各候选导航点连接成一个候选的飞行路径网络，各飞行路径中的候选导航点组成该候选飞行路径网络的节点；
步骤(3.2.2)若两个节点之间的连线没有穿过障碍物，两边所述两个节点上也没有障碍物，则直接把两个节点相连，则路径总指标J只包括威胁点带来的风险指数和路径长度指标，高度指标为零；
步骤(3.2.3)若两个节点之间的连线穿过障碍物，或者两节点中有处在建筑物之上的节点时，则按照以下规则进行局部寻优把两个节点看成起始点和目标点，与存在于所述两节点之间的包含建筑物在内的障碍物的顶点一起作为局部网络的节点，同样利用迪斯科特Dijsktra算法计算最优路径
步骤(3.2.3.1)若计算两个局部网络节点的路径总指标时两个局部网络节点之间的连线没有穿过障碍物，则只需计算路径的威胁度指标和路径长度指标，高度指标为零；
步骤(3.2.3.2)若计算两个局部网络节点的路径总指标时两局部网络节点间的连线穿过障碍物，则增加两个局部网络节点的高度，使其高度等于两个节点之间的建筑物的高度与由微型飞行器的导航精度决定的纵向导航误差之和，来控制路径的高度，计算时需考虑高度指标，即爬升高度和下降高度的两个指标不为零；
步骤(3.2.4)从微型飞行器的起飞点开始，沿着所述飞行路径局部网络，以此判别到达下一个局部网络节点之间是否有足够的长度保证在飞到所述下一个局部网络节点处达到所需要的高度，并且当微型飞行器沿着所选的某条路径依次逐个地选择下一个要到达的局部网络节点而到达目标点时能否达到目标点的高度，若能满足需求，则所述某条路径即为一条可行的飞行路径；
步骤(3.2.5)根据步骤(3.2.4)所述的方法，依次得到可行的飞行路径的集合；
步骤(3.2.6)计算步骤(3.2.5)所述的集合中，各条可行飞行路径的路径总指标J，其中飞行高度指标HC和HD由步骤(3.2.2)和步骤(3.2.3)求得，得到一条最优飞行路径，其路径总指标J最小；
步骤(3.2.7)按以下规则对步骤(3.2.6)中所述最优飞行路径中各相邻高度控制点之间的飞行路径进行飞行时的高度规划，所述高度控制点是指，微型飞行器的起飞点、目标点以及凡是障碍物所处的导航点飞行路径上障碍物的所在点
若h1＜h2时，则微型飞行器首先按照尽量大的航迹倾角爬升，当到达所需要的高度时，作等高飞行，其中，h1、h2分别是相邻两个高度控制点的高度，下同；
若h1＝h2时，则微型飞行器作等高飞行；
若h1＞h2时，则微型飞行器首先按照尽量大的航迹倾角下降，下降到所需要的高度时，作等高飞行；
步骤(3.2.8)用经过步骤(3.2.7)的各导航点的圆弧来代替路径转弯处的折角，从而得到微型飞行器绕所述东-北-地坐标系的用三个方向的转向角度表示的姿态坐标值；
步骤(3.2.9)所述航迹规划模块向数据采集模块输出微型飞行器的实时三维坐标和姿态的坐标值；
步骤(4)用一个数据采集模块对轨迹规划模块的路径数据进行实时采集，再把接收的路径数据传给自定义的运动模型，该模型用笛卡儿坐标变量X、Y、Z传输微型飞行器的实时三维坐标值，变量h、p、r传输微型飞行器的俯仰、偏航和滚转角度，所述X、Y、Z、h、p、r六个参数组成微型飞行器的实时运动参数。
全文摘要
在城市环境下微型飞行器三维轨迹仿真方法，属于微型飞行器三维实时路径导航技术领域。在计算机中，设置以下模块地图数字化模块，把图形用户界面输入的包括威胁点、建筑物、微型飞行器起飞点和目标点在内的城市环境三维模型、地图参数以及由此构成的候选导航点坐标数字化；航迹规划模块，从地图数字化模块输入上述数据，计算兼顾飞行资源消耗率和生存率的路径总指标，并在考虑障碍物的条件下用网络寻优算法计算最优可行路径，再把转弯处折角平滑化后生成实时飞行轨道；数据采集模块，实时采集航迹规划模块输出的飞行器的位置和姿态坐标值，形成包括直角坐标变量X、Y、Z和俯仰角h、偏航角p和滚转角r在内的姿态坐标值。本发明在节约飞行资源的同时提高了飞行。
文档编号G06T17/00GK101118622SQ20071009959
公开日2008年2月6日 申请日期2007年5月25日 优先权日2007年5月25日
发明者凌 田, 温颖怡, 行开新 申请人:清华大学