CN115202383A - 一种无人机多维航迹表达及生成方法 - Google Patents

Abstract

本发明公开了一种无人机多维航迹表达及生成方法，属于无人机导航领域；具体为首先，在电子地图上规划无人机的航迹规划，输出各航迹点的大地坐标系下的绝对坐标，作为第一层维度；然后，建立导航坐标系，利用大地坐标系与地心地固坐标系的互换，计算各航迹点相对于基准点的方位角、俯仰角和距离作为第二层维度；通过先验知识补充航迹规划的空间中地形地貌中的视觉特征点坐标作为控制点，计算各航迹点与周围范围内各个控制点之间的方位角、俯仰角和距离作为第三层维度；最后，将各航迹点在导航坐标系中的三维度航迹表达式进行装订，输出多维航迹描述文件，上传到无人机进行飞行引导。本发明增强了无人运动平台的航迹跟踪保持和智能化导航避障能力。

Description

一种无人机多维航迹表达及生成方法

技术领域

本发明属于无人机导航领域，具体是一种无人机多维航迹表达及生成方法，用于提升无人机的智能化水平，使其更加适用于道路应急勘察任务。

背景技术

我国整体地形复杂，气候环境多变，自然灾害频繁发生。自然灾害发生后，为了应急抢险，需要及时掌握目标区域道路交通的情况和损毁程度等信息。无人机由于具有灵活机动、高度自主、全天候执行任务的特点，能较好的适应道路应急勘察的应用场景。

但是，自然灾害及其引发的塌方与山体滑坡等次生灾害、建筑物遮挡与电子信号干扰等人为因素，可能会造成任务区域的通信基站供电不足甚至中断、通信线路故障，再结合极端气象和地形遮挡带来的卫星定位信号较差的情况，对无人机执行应急道路勘察任务造成了巨大障碍。

目前无人机的航迹描述与表达方式主要为：基于大地坐标系的空间点三维坐标集合(经度、纬度和高度)。无人机在飞行过程中的定位和航迹跟踪飞行控制，则主要依靠全球导航卫星系统(GNSS)。GNSS系统是一种利用卫星的伪距、星历、卫星发射时间等观测量和用户钟差，在地球表面或近地空间的任何地点为载体，提供全天候的三维坐标和速度以及时间信息的空基无线电导航定位系统。

在道路应急勘查任务中，会存在GNSS卫星信号失效的情况，一旦失效则会导致无人机失灵甚至坠落，造成重大经济损失和任务失败。因此，开展GNSS失效情况下的无人机自主导航研究，提高无人机导航可靠性，具有重要且迫切的应用需求。

惯性导航技术利用陀螺仪、加速度计等惯性测量单元输出载体沿轴向的旋转角速度和相对惯性空间所受力，在数学平台进行导航解算得到载体的速度、姿态和位置信息。虽然惯性导航短时间内精度高，但误差会随时间积累，不能满足无人机的导航需求。

纯视觉导航技术有两个分支：一是视觉里程计(VO)技术，侧重于利用连续图像帧之间的位姿增量进行路径积分，二是视觉同步定位与构图(VSLAM)技术，侧重于全局路径和地图的优化估计。纯视觉导航算法存在无法避免的固有缺点：依赖于场景的纹理特征、易受光照条件影响、难以处理快速旋转运动、更新速率低及延迟大等。

为了提高视觉导航系统的稳定性，引入惯性信息，产生了视觉/惯性组合导航技术；但在复杂条件下组合导航技术的可靠性还有待加强：一方面，惯性器件与视觉传感器的信息源质量较低，信息融合算法大多基于静态场景假设且仍存在滤波状态发散或状态收敛到错误值的情况；另一方面，目前的组合导航技术更侧重于定位与导航参数的估计，对于引导、控制、航迹描述等方面关注较少。

发明内容

针对无人机执行应急勘测任务时，GNSS信号不稳定无法精确定位以完成导航的问题，本发明提出了一种无人机多维航迹表达及生成方法，着眼于航迹规划后与无人机起飞前之间的阶段，通过融合大地坐标系、以测控点为原点的站点坐标系和航迹途径中的视觉特征控制点，形成多维航迹的混合表达，辅助判断GNSS是否失效或是否受到欺骗，并作为补充导航方式与位置数据融合的输入参数，进一步增强无人运动平台的航迹跟踪保持和智能化导航避障能力。

所述无人机多维航迹表达及生成方法，具体步骤如下：

步骤一、在电子地图上对无人机需要执行的应急勘查道路进行航迹规划，输出各航迹点的大地坐标系坐标，组成坐标集合(B,L,H)，作为航迹表达式的第一维度；

步骤二、设置无人机航迹规划的理想起飞点(b₀,l₀,h₀)为基准点，以基准点为坐标系原点，建立导航坐标系；

导航坐标系采用正北向X、正东向Y，水平面垂直向上Z；

步骤三、将当前大地坐标系下的各航迹点，分别利用大地坐标系与地心地固坐标系之间的转换，生成各航迹点在导航坐标系下的坐标；

计算过程为：

首先，针对大地坐标系坐标为(b,l,h)的航迹点A，转换到地心地固坐标系中的坐标为(x,y,z)，计算公式为：

R为地球的椭圆曲率半径，R_e为地球长半径，e²为第一偏心率；

然后，结合地心地固坐标系下的无人机理想起飞点坐标(x₀,y₀,z₀)，计算航迹点A相对于基准点的坐标(X,Y,Z)，

计算公式为：

利用导航坐标系中无人机实际起飞点与基准点之间的误差(X′,Y′,Z′)，修正航迹点A在导航坐标系下的坐标(X,Y,Z)；

修正公式如下：

同理，对航迹规划中每个航迹点重复上述过程，利用大地坐标系与地心地固坐标系的转换，生成导航坐标系下各自的对应点。

步骤四、针对航迹规划中无人机的各航迹点，分别计算修正后的各航迹点相对于基准点的方位角、俯仰角和距离，组成各航迹点的位姿集合，作为航迹表达式的第二维度；

针对航迹规划中无人机处于航迹点A，计算该航迹点相对于基准点的方位角θ、俯仰角

和距离d：

步骤五、通过先验知识补充航迹规划的空间中地形地貌中的视觉特征点坐标作为控制点，并将每个控制点坐标转换到导航坐标系，计算各航迹点与周围范围内每个控制点之间的方位角、俯仰角和距离，组成的位姿集合，作为航迹表达式的第三维度；

针对航迹规划中无人机处于航迹点A所在的区域内，根据应急勘测任务的实际情况进行语义分割，将该区域分为N个类别，每个类别均包含M个控制点；

针对航迹点A与类别为n序列为m的控制点之间的方位角、俯仰角和距离

计算公式为：

为当前航迹点周围范围内类别为n序列为m的控制点的导航坐标系坐标；点(X_nm,Y_nm,Z_nm)为根据类别为n序列为m的控制点计算得到的当前航迹点的三维相对坐标；

由于控制点能进行多维扩展，从而每个航迹点与周围多维扩展的各个控制点之间的方位角、俯仰角和距离也进行了多维扩展，导航坐标系下控制点的多维扩展表示为：

由此得到多维扩展的方位角、仰角和斜距表示如下：

步骤六、装订航迹规划中每个航迹点的多维航迹表达式，输出多维航迹描述文件，上传到无人机进行飞行引导。

航迹点A在导航坐标系中的多维航迹表达式为

本发明的优点在于：

1、一种无人机多维航迹表达及生成方法，给出了多维航迹表达方式的标准，将传统无人机航迹表达方法由航迹点三维大地坐标的简单集合(经度、纬度和高度)升级为三层次相互融合的多维描述表达的集合，可以实现无人运动平台(包括但不局限于无人机、无人车和无人船等)不依赖GNSS系统和大地坐标的航迹表达方式。

2、一种无人机多维航迹表达及生成方法，多维航迹的第二层次和第三层次可以提高导航的稳定性和健壮性，一方面作为判断GNSS是否失效或是否受到欺骗的依据与校正数据来源，另一方面作为补充导航方式与位置数据融合的输入参数。

附图说明

图1是本发明一种无人机多维航迹表达及生成方法流程图；

图2是本发明大地坐标系与站心坐标系/导航坐标系的示意图；

图3是本发明航迹点序列的空间描述矢量；

图4是本发明航迹点与视觉特征点的空间描述矢量；

具体实施方式

下面结合附图，对本发明的具体实施方法作进一步的详细说明。

本发明提出了一种不完全依赖全球导航卫星系统的无人机多维航迹表达及生成方法，用于提升无人机的智能化水平，使其更加适用于道路应急勘察任务；利用航迹点大地坐标与航迹空间地形地貌的先验知识，将组合导航技术的部分任务在航迹描述阶段定好表达式的标准并提前进行计算。

具体为：将传统无人机航迹表达方法由航迹点三维大地坐标的简单集合(经度、纬度和高度)升级为三层次相互融合的多维描述表达的集合。将无人运动平台的航迹表达划分为三个层次：第一层次，航迹点大地三维坐标序列集合，与传统航迹表达兼容；第二层次，选定航迹空间中的基准点，并且计算航迹点序列相对于基准点(可以有多个基准点)的空间描述矢量(方位角、俯仰角和距离)；第三层，通过先验知识补充航迹空间中地形地貌中的视觉特征点坐标作为控制点，计算并输出与视觉导航相关的空间描述矢量。第二层和第三层作为提高导航稳定性和健壮性的手段。

所述无人机多维航迹表达及生成方法，如图1所示，具体步骤如下：

步骤一、在电子地图上对无人机需要执行的应急勘查道路进行航迹规划，输出各航迹点的大地坐标系坐标，组成坐标集合(B,L,H)，作为航迹表达式的第一维度；

步骤二、设置无人机航迹规划的理想起飞点(b₀,l₀,h₀)为基准点，以基准点为坐标系原点，建立站心坐标系(北-东-天坐标系)作为导航坐标系；

假设无人机实际起飞点(b₀′,l₀′,h₀′)；

导航坐标系采用：正北向X、正东向Y，水平面垂直向上Z；

步骤三、将当前大地坐标系下的各航迹点，分别利用大地坐标系与地心地固坐标系之间的转换，生成各航迹点在导航坐标系下的坐标；

计算过程为：

首先，针对大地坐标系坐标为(b,l,h)的航迹点A，转换到地心地固坐标系中的坐标为(x,y,z)，计算公式为：

R为地球的椭圆曲率半径，R_e为地球长半径，e²为第一偏心率；

然后，结合地心地固坐标系下的无人机理想起飞点坐标(x₀,y₀,z₀)，计算航迹点A相对于基准点的坐标(X,Y,Z)，

计算公式为：

利用导航坐标系中无人机实际起飞点与基准点之间的误差(X′,Y′,Z′)，修正航迹点A在导航坐标系下的坐标(X,Y,Z)；

同理，对航迹规划中每个航迹点重复上述过程，利用大地坐标系与地心地固坐标系的转换，生成导航坐标系下各自的对应点。

步骤四、针对航迹规划中无人机的各航迹点，分别计算修正后的各航迹点相对于基准点的方位角、俯仰角和距离，组成各航迹点的位姿集合，作为航迹表达式的第二维度；

针对航迹规划中无人机处于航迹点A，计算该航迹点相对于基准点的方位角θ、俯仰角

和距离d：

步骤五、通过先验知识补充航迹规划的空间中地形地貌中的视觉特征点坐标作为控制点，并将每个控制点坐标转换到导航坐标系，计算各航迹点与周围范围内每个控制点之间的方位角、俯仰角和距离，组成的位姿集合，作为航迹表达式的第三维度；

针对航迹规划中无人机处于航迹点A所在的区域内，根据应急勘测任务的实际情况进行语义分割，将该区域分为N个类别，每个类别均包含M个控制点；

针对航迹点A与类别为n序列为m的控制点之间的方位角、俯仰角和距离

计算公式为：

为当前航迹点周围范围内类别为n序列为m的控制点的导航坐标系坐标；点(X_nm,Y_nm,Z_nm)为根据类别为n序列为m的控制点计算得到的当前航迹点的三维相对坐标。

由于控制点能进行多维扩展，从而每个航迹点与周围多维扩展的各个控制点之间的方位角、俯仰角和距离也进行了多维扩展，导航坐标系下控制点的多维扩展表示为：

由此得到多维扩展的方位角、仰角和斜距表示如下：

步骤六、装订航迹规划中每个航迹点的多维航迹表达式，输出多维航迹描述文件，上传到无人机进行飞行引导。

航迹点A在导航坐标系中的多维航迹表达式为

实施例：

本实施例中，电子地图采用的大地坐标系为WGS84，无人机预计使用的GNSS设备为GPS；如图2所示，导航坐标系为站心坐标系即北-东-天坐标系，X轴正北向、Y轴正东向，Z轴水平面垂直向上；

将各个航迹点的WGS84绝对坐标(b,l,h)换算为地心地固坐标系的坐标(x,y,z)，以无人机起飞点为基准点建立站心坐标系作为导航坐标系，任意点航迹点在导航坐标系的坐标为(X,Y,Z)，R_e为地球长半径取6378140，e²为第一偏心率取0.006694379995，X、Y、Z单位为米，精度0.01米。

在无人机飞行过程中，无人机机载GNSS、RTK设备与地面基站测量实时相对坐标，计算同下个航迹点坐标的差值以完成无人机导航；

如图3所示，将各个航迹点的导航坐标系坐标A(X₁,Y₁,Z₁)及其后一个航迹点坐标B(X₂,Y₂,Z₂)换算为导航坐标系下的位姿参数

与

θ、

的单位为度，精度0.1度，d的单位为米，精度0.1米，无人机处于任意航迹点时前往下一个航迹点的方位角改变量Δθ、俯仰角改变量

距离改变量Δd：

在无人机飞行过程中，无人机机载惯性导航设备，包括加速度计、陀螺仪、磁力计、气压计等惯性测量单元(IMU)，IMU测量的三轴加速度、三轴角速度、正北方向、高度参数，使用欧拉角算法进行姿态解算，将惯性数据由无人机刚体坐标系换算至导航坐标系，计算相邻航迹点矢量参数

以完成无人机导航；

如图4所示，将各个航迹点的导航坐标系坐标(X,Y,Z)换算为导航坐标系下的位姿参数

换算过程通过先验知识补充航迹空间中地形地貌中的视觉特征点坐标作为控制点，先验知识以索引文件的形式存在，任意航迹点划定的区域内共有N个类别、每个类别共有M个控制点，三维相对坐标为

方位角

俯仰角

距离

的单位为度，精度0.1度，

单位为米，精度0.1米，点(X_nm,Y_nm,Z_nm)为根据第n类第m个控制点计算得到的航迹点A的大地三维相对坐标，点(X₀,Y₀,Z₀)为通过平均消除误差后的航迹点A的大地三维相对坐标：

上述维度以可扩展数组

的形式存在。

在无人机飞行过程中，无人机机载激光测距仪与单目摄像头，对静态参照物进行语义分割以识别视觉特征点，并测得无人机与控制点的相对位置关系。第三层维度的参数根据实际情况确定，可能为空也可能为多组参数。上述方式得到实时坐标(X₀,Y₀,Z₀)，计算同下个航迹点坐标(X,Y,Z)的差值以辅助无人机导航，如检测第二、三层维度的准确性、校正第二、三层维度的误差；

最后，得到单个航迹点的多维表达式为

通过对所有航迹点的多维表达式进行航迹装订，参考航迹描述文件的KML格式；

额外定义参数<altitudeMode>relativeToGround</altitudeMode>为采用相对海拔，

定义<coordinateMode>EUN_CS</coordinateMode>为采用站心坐标系作为导航坐标系，

定义<Point>B₀,L₀,H₀</Point>为基准点坐标，

定义<dimensionMode>dimension_1_2_3</dimensionMode>为采用多维航迹表达方式，

定义<dimensionMode>dimension_1</dimensionMode>为GNSS信号稳定时的无人机导航方式，以做到航迹描述文件格式向下兼容；

定义

为多维航迹表达字段，输出多维航迹描述文件，然后上传至无人机进行飞行引导。

进一步，本发明的导航坐标系不局限于站心坐标系，后续步骤的坐标运算也可在其他坐标系中进行，如WGS84、CGCS2000、地心地固坐标系、地方独立坐标系等；

第二维度航迹点描述不局限于位姿

由上述参数计算得到的三轴加速度、三轴角速度、径向速度、时间间隔同样可作为航迹点的描述方式；

视觉特征点选取可根据应急勘测任务的实际情况进行语义分割，如公路的分岔路口、标识牌、树木等，如铁路的铁轨、枕木、接触网等；

信息来源不局限于GPS、惯性导航系统、激光测距仪与单目摄像头，其他能测量多维航迹表达式参数的装置均可，如GLONASS、BDS、激光惯导系统、毫米波雷达、激光雷达等；多维航迹描述文件不局限于KML格式，其他能够描述多维航迹表达式的文件格式均可。

多位航迹表达式根据视觉特征点的先验知识与实际情况可扩展，先验知识包括但不限于视觉特征的语义种类与数量，实际情况包括但不限于信息来源于不同的GNSS导航设备、惯导系统、激光雷达、毫米波雷达、光学摄像头等各种物理模块。

Claims (4)

1.一种无人机多维航迹表达及生成方法，其特征在于，所述无人机的航迹规划中，每个航迹点的多维航迹表达式为

(b,l,h)为航迹规划中当前航迹点的大地坐标系的坐标；

为当前航迹点相对于导航坐标系基准点的方位角、俯仰角和距离；

为当前航迹点与周围范围内导航坐标系下类别为n序列为m的控制点之间的方位角、俯仰角和距离；

为当前航迹点周围范围内类别为n序列为m的控制点的导航坐标系坐标；点(X_nm,Y_nm,Z_nm)为根据类别为n序列为m的控制点计算得到的当前航迹点的三维相对坐标，计算公式如下：

由于每个航迹点划定的区域内，根据应急勘测任务的实际情况进行语义分割，均能得到N个类别，每个类别均有M个控制点，控制点能进行多维扩展；从而航迹点与周围多维扩展的各个控制点之间的方位角、俯仰角和距离也进行了多维扩展，形成了每个航迹点的可扩展的多维航迹表达式。

2.根据权利要求1所述的一种无人机多维航迹表达及生成方法，其特征在于，所述每个航迹点的多维航迹表达式的生成方法，具体为：

首先，在电子地图上对无人机需要执行的勘查道路进行航迹规划，输出各航迹点的大地坐标系坐标，组成坐标集合(B,L,H)，作为航迹表达式的第一维度；

然后，设置无人机航迹规划的理想起飞点(b₀,l₀,h₀)为基准点，以基准点为坐标系原点，建立导航坐标系；

进一步将各航迹点的大地坐标系坐标，利用大地坐标系与地心地固坐标系之间的互换，转换成各航迹点对应的导航坐标系，从而计算各航迹点相对于基准点的方位角θ、俯仰角

和距离d，作为航迹表达式的第二维度；

接着，通过先验知识将航迹规划的空间中地形地貌中的视觉特征点坐标，转换到导航坐标系下的控制点坐标，计算各航迹点与周围范围内每个控制点之间的方位角

俯仰角

和距离

作为航迹表达式的第三维度；

最后，将每个航迹点在导航坐标系中的多维航迹表达式进行航迹装订，输出多维航迹描述文件，上传到无人机进行飞行引导。

3.根据权利要求2所述的一种无人机多维航迹表达及生成方法，其特征在于，所述导航坐标系采用：正北向为X轴、正东向为Y轴，水平面垂直向上为Z轴；

针对当前大地坐标系坐标为(b,l,h)的航迹点A，其利用大地坐标系与地心地固坐标系的转换，生成导航坐标系下的对应点，计算过程为：

首先，将航迹点A的坐标(b,l,h)转换到地心地固坐标系中的坐标(x,y,z)，计算公式为：

R为地球的椭圆曲率半径，R_e为地球长半径，e²为第一偏心率；

然后，结合地心地固坐标系的无人机起飞点坐标(x₀,y₀,z₀)，计算航迹点A相对于基准点的坐标(X,Y,Z)；

计算公式为：

利用无人机实际起飞点与基准点之间的误差(X′,Y′,Z′)，修正航迹点A在导航坐标系下的坐标(X,Y,Z)；

修正公式如下：

最后，计算修正后的航迹点A相对于导航坐标系下基准点的方位角θ、俯仰角

和距离d；

计算公式如下

同理，对航迹规划中每个航迹点重复上述过程，利用大地坐标系与地心地固坐标系的转换，生成导航坐标系下各自的对应点。

4.根据权利要求1所述的一种无人机多维航迹表达及生成方法，其特征在于，所述导航坐标系下控制点的多维扩展表示为：

由此得到多维扩展的方位角、俯仰角和距离表示如下：

每个控制点同理利用大地坐标系的坐标转换到地心地固坐标系中的坐标，结合无人机起飞坐标，计算各控制点相对于基准点的坐标，得到各个控制点在导航坐标系下的对应坐标，并修改误差；

利用各个航迹点与修改误差后的控制点在导航坐标系下的坐标，计算两者在导航坐标系中的多维扩展方位角、俯仰角和距离。

Images