基于微型雷达阵列的未知环境并行导航方法和系统

1.本发明属于制导、导航与控制领域，具体涉及基于微型雷达阵列的未知环境 并行导航方法和系统。
技术背景
2.在地形复杂多变的室内未知环境下，卫星导航无法使用，而惯性导航系统由 于其误差累积导致导航精度不够，无法长时间工作。在采用雷达阵列的情况下， 常采用序惯采样方式，本专利导航方法针对了序惯采样设计。本专利方法适用于 超声波雷达、激光雷达、微波雷达等雷达阵列。本专利进一步提出通过雷达阵列 估计坡度、坡向的方法。
3.数字高程模型(digital elevation model)，简称dem，是通过有限的地形高 程数据实现对地面地形的数字化模拟(即地形表面形态的数字化表达)，它是用 一组有序数值阵列形式表示地面高程的一种实体地面模型。如果利用数字高程模 型解决未知环境导航是需要解决的问题。


技术实现要素：

4.为解决现有技术中无法进行未知环境导航的问题，本发明提供了基于微型雷 达阵列的未知环境并行导航方法和系统，旨在不依赖于卫星导航的情况下，实现 无人系统在地形复杂多变的未知环境中进行自主导航的能力，并实现对典型的坡 面特征识别。
5.为解决上述问题，本发明采用的技术方案如下：
6.基于微型雷达阵列的未知环境并行导航方法，包括以下步骤：
7.获取微型雷达阵列序贯采样的实时数据；依据姿态角对实时测量数据进行补 偿；
8.根据采集数据计算坡度和坡向以进行坡面的识别；基于坡面的识别根据地形 匹配算法对突变地形进行匹配；
9.根据匹配结果进行导航计算，由速度累加得到从对地面探测时间开始到经过 任意地形时刻导航所需的相对位置信息，进而计算得到导航结果。
10.作为本发明的进一步改进，所述微型雷达阵列按照矩阵式排布在无人系统上， 阵列共m行，每行安装n个雷达，雷达阵列安装于无人系统本体坐标系，前后排 雷达一一对应；雷达数据采集端指向朝下且相互平行。
11.作为本发明的进一步改进，阵列雷达以序贯采样方式测量地形、地物，阵列 中相邻雷达采样存在时间差。
12.作为本发明的进一步改进，所述依据姿态角对雷达数据进行补偿，具体包括：
13.雷达阵列前后排距无人系统质心距离为lg/2，当俯仰角为θ时，雷达阵列数 据修正为
[0014][0015]
其中，dm为雷达采集相对地形的距离，d为修正后的无人系统飞行相对地 形的距
离；
[0016]
本专利提出将前排和后排每两个对应的雷达看作一组导航系统，共构成n组 导航系统，设k时刻前排第j个雷达经过某个地形，此时得到的雷达数据为经过i个周期后排第j个雷达经过同一地形，雷达测量到的数据为根据上 述条件：
[0017][0018]
其中，表示前排第j个雷达在k时刻的测量数据，表示后排第j个雷 达在k i时刻的测量数据。
[0019]
作为本发明的进一步改进，所述置信度参数ε的计算为：
[0020][0021]
其中，φ(x)为对应的概率分布函数；
[0022]
给出的取值，结合已知σr，σh通过概率分布得到置信度 参数ε；
[0023]
进而得到补偿后的雷达数据。
[0024]
作为本发明的进一步改进，所述根据采集数据计算坡度和坡向以进行坡面的 识别，具体包括：
[0025]
无人系统沿y方向运动，机上雷达阵列扫过计算区域，机载共2n个雷达，每 排阵列由n个雷达构成，雷达采用序惯采样，2n个雷达序惯采样周期为t；相邻 雷达安装距离记为x
cellsize
，同一雷达在上一时刻k-1至当前时刻k飞过的距离由无人 系统速度v和序惯采样周期t所决定：
[0026]ycellsize
＝v
×
t
[0027]
其中，v为无人系统在经过上一次突变点时的速度。
[0028]
分别计算k-1时刻各点的坡向和坡度。设曲面函数为z＝f(x,y),则坡度为
[0029][0030]
其中，和分别为x方向和y方向的坡度；以k时刻各点为例，其x和y 方向的坡度分别为
[0031][0032][0033]
坡向计算方法为
[0034][0035]
其中，
[0036][0037]
当雷达阵列采用序贯采样方式时，相邻雷达的采样时间间隔则相邻 雷达在无人系统前进方向上产生的相对位移为
[0038]
lr′
＝vt
′
[0039]
其中，v为无人系统飞行速度；进而得到：
[0040][0041]
以k-1时刻地形高度数据和k时刻地形高度数据作直线拟合，得到
[0042][0043]
为在x方向上与k时刻采样点在同一直线的采样点地形高度数据估计值；
[0044]
由此，x方向坡度修正为：
[0045][0046]
进而完成坡面的识别。
[0047]
作为本发明的进一步改进，所述基于坡面的识别根据地形匹配算法对突变地 形进行匹配，具体包括：
[0048]
通过地形坡度来判断突变地形，满足：
[0049][0050]
其中的e表示坡度变化的阈值；当无人系统经过的地形满足上述条件时，判 断为突变地形；
[0051]
其中，坡度变化的阈值e的取值满足下式：
[0052][0053]
根据地形特征复杂度，给出的取值，进而求出e值。
[0054]
作为本发明的进一步改进，所述根据匹配结果进行导航计算，具体包括：
[0055]
雷达阵列的探测周期为t，当无人系统沿垂直于阵列方向直线运动时，各雷 达采样点在周期t内沿飞行方向产生相对位移，每组导航系统对应雷达产生的相 对位移为
[0056][0057]
其中，v为无人系统飞行速度，t为雷达阵列的序惯测量周期；
[0058]
前排和后排雷达阵列的安装距离为lm，在一个探测周期内，前排和后排对应 雷达采样点距离修正为
[0059]
l＝l
m-lr[0060]
前排雷达和后排雷达经过同一地形的时间差t＝it，则联立以上各式得到无人 系统经过该地形的速度：
[0061][0062]
其中，t为每次探测的雷达阵列采样周期；
[0063]
无人系统速度为
[0064][0065]
当i＝1时两种速度计算结果相差最大，最大相差
[0066]
基于微型雷达阵列的未知环境并行导航系统，包括：
[0067]
数据获取模块，用于获取微型雷达阵列序贯采样的实时数据；
[0068]
识别匹配模块，用于依据姿态角对实时数据进行补偿；根据采集数据计算坡 度和坡向以进行坡面的识别；基于坡面的识别根据地形匹配算法对突变地形进行 匹配；
[0069]
导航计算模块，用于根据匹配结果进行导航计算，由速度累加得到从对地面 探测时间开始到经过任意地形时刻导航所需的相对位置信息，进而计算得到导航 结果。
[0070]
一种无人系统，包括所述的基于微型雷达阵列的未知环境并行导航系统。
[0071]
与现有技术相比，本发明有以下优势：
[0072]
本发明基于微型雷达阵列的无人机未知环境自主导航方法，具体设计了复杂 环境地形匹配导航算法，在此算法基础上，首先修正了微型雷达阵列序贯采样引 起的系统误差，其次建立了复杂地形的数字高程模型，获取地形的坡度和坡向等 信息，同时对算法进行优化，提高了导航计算的精确度。实现无人系统在地形复 杂多变的未知环境中进行自主导航，消除雷达阵列序贯采样方式造成的系统性误 差，提高导航精度，同时获取地形包括坡度和坡向的信息。
附图说明
[0073]
图1为本发明基于微型雷达阵列的未知环境并行导航方法流程示意图；
[0074]
图2为本发明并行导航系统示意图；
[0075]
图3为本发明微型雷达阵列序贯采样示意图；
[0076]
图4为本发明仿真时探测到的三维地形图；
[0077]
图5为本发明仿真地形坡度图；
[0078]
图6为本发明仿真地形坡向图；
[0079]
图7为本发明基于微型雷达阵列的未知环境并行导航系统结构示意图；
[0080]
图8为本发明一种电子设备结构示意图。
[0081]
具体实施例方式
[0082]
本发明旨在不依赖于卫星导航的情况下，实现无人系统在地形复杂多变的未 知环境中进行自主导航的能力。基于地形匹配的自主导航算法，在此基础上开发 了一种实时性好、成本低的基于微型雷达阵列的自主导航系统，并消除雷达阵列 序贯采样方式造成的系统性误差，提高了导航精度，同时能够获取未知地形的坡 面包括坡度和坡向的信息。
[0083]
本发明提供了一种实时性好、成本低的未知环境自主导航系统，该导航系统 基于微型雷达阵列实现无人系统在地形复杂多变的未知环境中进行自主导航。如 图1所示，基于微型雷达阵列的未知环境并行导航方法，包括以下步骤：
[0084]
获取微型雷达阵列序贯采样的实时数据；
[0085]
依据姿态角对实时数据进行补偿；根据采集数据计算坡度和坡向以进行坡面 的识别；基于坡面的识别根据地形匹配算法对突变地形进行匹配；
[0086]
根据匹配结果进行导航计算，由速度累加得到从对地面探测时间开始到经过 任意地形时刻导航所需的相对位置信息，进而计算得到导航结果。
[0087]
本发明将根据所述基于微型雷达阵列的导航系统建立未知环境地形的dem， 获取地形的坡度和坡向等信息。基于地形匹配的自主导航算法，在此基础上开发 了一种实时性好、成本低的基于微型雷达阵列的自主导航系统，并消除雷达阵列 序贯采样方式造成的系统性误差，提高了导航精度，同时能够获取未知地形的坡 面包括坡度和坡向的信息。
[0088]
基于超声波雷达阵列实现三维地图重建，以及使得无人系统具备一定的导航 能力。本发明旨在实现无人系统在地形复杂多变的未知环境中进行自主导航，消 除雷达阵列序贯采样方式造成的系统性误差，提高导航精度，同时获取地形包括 坡度和坡向的信息。该发明的精度高、成本低、实时性好。
[0089]
具体包括以下步骤：
[0090]
s100，在无人系统上安装矩阵式排布的微型测距雷达阵列，雷达信号类别可 以取为超声波、激光、无线电波等；
[0091]
步骤s100的具体步骤如下：
[0092]
在无人系统平台上按照矩阵排布安装雷达阵列。阵列共m行，每行安装n 个雷达，为便于说明，下文选取m＝2。前后排雷达一一对应。雷达阵列安装于无 人系统本体坐标系，雷达数据采集端指向朝下且相互平行，保证数据采集无任何 遮挡。
[0093]
s200，阵列中各雷达采样方式为序贯采样，提出基于雷达阵列序贯采样的并 行导航算法；
[0094]
步骤s200的具体步骤如下：
[0095]
首先，雷达阵列采样方式为序贯采样，即由前排第一个雷达至后排最后一个 雷达依次采样，相邻雷达、前后排雷达采样存在时间差。由于无人系统飞行速度 较快，由该时间差产生的数据误差不可忽略。
[0096]
其次，由于前后两排雷达阵列安装于无人系统正下方，且与无人系统质心距 离较大，无人系统保持高度h做水平直线运动时，姿态角的变化对雷达产生影响， 使其采集的数据无法反映真实飞行相对高度，因此需要依据姿态角对雷达数据进 行补偿。
[0097]
前后排雷达阵列距无人系统质心距离为lg/2，姿态角由机载imu获得，当 俯仰角为θ时，雷达阵列数据修正为
[0098][0099]
其中，dm为雷达采集相对地形高度数据，d为修正后的无人系统飞行相对 地形高度。同时，阵列中每个雷达的测量误差满足正态分布：
[0100]
ν～n(0，σ
r2
)
[0101]
对于做直线运动的无人系统，前排的测距雷达阵列会率先飞越过某个地形， 随后经过一定时间后，后排的测距雷达阵列飞过同一地形，前排和后排雷达对同 一地形的采样数据存在相似性。由于雷达阵列为序贯采样，为保证实时性，需要 在每次采样结束时处理数据，为此本发明将前排和后排每两个对应的雷达看作一 组导航系统，共构成n组导航系统，并且各组导航系统之间相互并行，互不干扰， 可最大程度地减少误差累积。
[0102]
设k时刻前排第j个雷达经过某个地形，此时得到的雷达数据为经过i次 测量后排第j个雷达经过同一地形，此时得到的雷达数据为根据上述条 件：
[0103]
对于j，使得
[0104][0105]
其中，ε为置信度参数；
[0106]
当置信度参数ε为足够小的量时，保证和相似，即：
[0107][0108]
其中，表示前排第j个雷达在k时刻的测量数据，表示后排第j个雷 达在k i时刻的测量数据。
[0109]
对于置信度参数ε的计算，首先，根据lindeberg-fellercentral中心极限定理，地 面起伏程度g满足正态分布：
[0110]
h～n(0,σ
h2
)
[0111]
则雷达测量数据d
i,j
＝h h
i,j
ν
i,j
满足正态分布：
[0112]di,j
～n(h,σ
r2
σ
h2
)
[0113]
可知前排雷达采集数据与后排雷达采集数据相互独立，则
[0114]
[0115]
将其转化为标准正态分布，即
[0116][0117]
标准正态分布的密度函数为
[0118][0119]
则，令由式(3)可知
[0120][0121]
其中，φ(x)为标准正态分布的分布函数。
[0122]
为满足尽量少的i满足条件，保证前后排雷达数据匹配的准确性，取结合已知σr，σh通过查标准正态分布表，采用线性插值 法即可求得ε。
[0123]
下面根据匹配结果进行导航计算：
[0124]
雷达阵列的探测周期为t，当无人系统沿垂直于阵列方向做直线运动时，各 雷达采样点在周期t内沿飞行方向产生相对位移。则前后排对应雷达产生的相对 位移为
[0125][0126]
其中，v为无人系统飞行速度，t为雷达阵列的探测周期。
[0127]
前排和后排雷达阵列的安装距离为lm，由上式可得，在一个探测周期内，前 排和后排对应雷达采样点距离修正为
[0128]
l＝l
m-lrꢀꢀꢀ
(7)
[0129]
前排雷达和后排雷达经过同一地形的时间差t＝it，则联立以上各式得到无人 系统经过该地形的速度
[0130][0131]
其中，t为每次探测的雷达阵列采样周期。
[0132]
在不考虑雷达序贯采样的情况下，无人系统速度为
[0133][0134]
当i＝1时两种速度计算结果相差最大，最大相差因此在雷达阵列频率较 低的情况下，不可忽略序贯采样对导航计算的影响。
[0135]
进而可知，由速度累加即可得到从对地面探测时间开始到经过任意地形时刻 无
人系统导航所需的相对位置信息。
[0136]
s300，根据雷达阵列测量数据提出坡面地形特征识别的方法；
[0137]
步骤s300的具体步骤如下：
[0138]
无人系统沿y方向运动，机上单行n个雷达构成的雷达阵列扫过计算区域。 可根据采集数据计算坡度和坡向以进行坡面的识别。
[0139]
当n个雷达同时触发时，可建立如表1所示的栅格图，第一至三行分别表示 当前k时刻、k-1时刻和k-2时刻下的地形起伏高度。该栅格图中的数值随着无人 系统运动不断更新。
[0140]
表1
[0141]
时刻雷达1雷达2雷达3雷达4雷达5kh
k,1hk,2hk,3hk,4hk,5
k-1h
k-1,1hk-1,2hk-1,3hk-1,4hk-1,5
k-2h
k-2,1hk-2,2hk-2,3hk-2,4hk-2,5
[0142]
其中，相邻雷达安装距离记为x
cellsize
，同一雷达在上一时刻k-1至当前时刻k飞 过的距离由无人系统速度v和计算周期t所决定：
[0143]ycellsize
＝v
×
t
ꢀꢀꢀ
(10)
[0144]
其中，v为无人系统在经过上一次突变点时的速度，由此可分别计算k-1时刻 各点的坡向和坡度。
[0145]
设曲面函数为z＝f(x,y),则坡度为
[0146][0147]
其中，和分别为x方向和y方向的坡度。以k时刻各点为例，其x和y 方向的坡度分别为
[0148][0149][0150]
在测量数据精准的前提下，x
cellsize
和y
cellsize
的大小决定了坡度和坡向的计算精度。
[0151]
坡向计算公式为
[0152][0153]
其中，
[0154][0155]
当雷达阵列采用序贯采样方式时，相邻雷达的采样时间间隔则相邻 雷达在无人系统前进方向上产生的相对位移为
[0156]
lr′
＝vt
′ꢀꢀꢀ
(16)
[0157]
其中，v为无人系统飞行速度。联合式(10)得
[0158][0159]
以k-1时刻地形高度数据和k时刻地形高度数据作直线拟合，得到
[0160][0161]
为在x方向上与k时刻采样点在同一直线的采样点地形高度数据估计值。
[0162]
由此，x方向坡度修正为
[0163][0164]
s400，提出基于坡面地形特征匹配的导航算法。
[0165]
步骤s400的具体步骤如下：
[0166]
根据步骤s200所述算法，对于平坦地形，该算法无法提供有效的导航信息。 基于雷达阵列测量的相对高度数据，导航计算要求地形存在突变，因此需识别出 突变地形从而根据地形匹配算法得出有效的导航信息。根据权利要求步骤s300 所述，对于特殊地形的识别，可通过地形坡度来判断，满足：
[0167][0168]
即有
[0169][0170]
其中的e表示坡度变化的阈值；当无人系统经过的地形满足上述条件时，即 可得
到高精度的位置和速度信息。
[0171]
下面确定e的取值，可知
[0172]
(h
k 1,j-h
k-1,j
)～n(0,2σ
h2
)
[0173]
将其转化为标准正态分布，
[0174][0175]
令则式(20)等价于
[0176][0177]
类似的，
[0178][0179]
根据地形特征复杂度，为减小误差，令进而求出e值。
实施例
[0180]
基于本发明基于微型雷达阵列的无人机并行导航系统及坡面识别方法，实施 例如下所述：
[0181]
针对步骤s100，我们应用已有的配置好微型雷达阵列的无人机，按图2所示 保持一定高度飞行，其飞行地面环境如图3所示。
[0182]
针对步骤s200，在飞行的同时地面站运行所设计的地形匹配导航算法，由于 微型雷达阵列采样方式为序贯采样，在无人机飞行过程中各雷达采样点存在相对 位移，如图4所示，因此有必要修正算法，消除系统误差。此时得到的地形数字 高程模型如图5所示。
[0183]
针对步骤s300，我们在步骤s200获取的数字高程模型的基础上，通过坡面 识别算法得到地形的坡度和坡向的信息，结果如图6、图7。
[0184]
针对步骤s400，我们在步骤s300得到的地形坡度和坡向云图的基础上，对 地形匹配导航算法进一步修正，使得算法在地形突变位置的导航计算精度进一步 提高。根据导航的计算结果，得到如图6、图7所示速度和位置估计曲线。
[0185]
如图7所示，本发明还提供基于微型雷达阵列的未知环境并行导航确定系统， 包括：
[0186]
数据获取模块，用于获取微型雷达阵列序贯采样的实时数据；
[0187]
识别匹配模块，用于依据姿态角对实时数据进行补偿；根据采集数据计算坡 度和坡向以进行坡面的识别；基于坡面的识别根据地形匹配算法对突变地形进行 匹配；
[0188]
导航计算模块，用于根据匹配结果进行导航计算，由速度累加得到从对地面 探测时间开始到经过任意地形时刻导航所需的相对位置信息，进而计算得到导航 结果。
[0189]
如图8所示，本发明的另一目的在于提出基于微型雷达阵列的未知环境并行 导航的设备，包括：
[0190]
存储器，
[0191]
处理器，
[0192]
所述处理器被配置为：执行所述的基于微型雷达阵列的未知环境并行导航方 法。
[0193]
本发明还提供一种计算机可读存储介质，当所述存储介质中的指令由处理器 执行时，使得处理器能够执行基于微型雷达阵列的未知环境并行导航方法。
[0194]
本发明还提供一种无人系统，包括所述的基于微型雷达阵列的未知环境并行 导航系统。
[0195]
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算 机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软 件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计 算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、 光学存储器等)上实施的计算机程序产品的形式。
[0196]
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品 的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或 方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框 的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机 或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可 编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个 流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0197]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备 以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指 令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和 /或方框图一个方框或多个方框中指定的功能。
[0198]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得 在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从 而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或 多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0199]
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制， 尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当 理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发 明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范 围之内。