CN118191831A - 无人机载毫米波雷达与视觉设备的非视距定位系统与方法 - Google Patents

Abstract

本发明公开一种无人机载毫米波雷达与视觉设备的非视距定位系统与方法，应用于非视距范围目标检测技术领域，针对现有技术在城市区域非视距目标定位上存在无人机杂波难处理、需要建筑布局先验信息、环境复杂难规划以及灵活性等问题；本方法首先通过视觉传感器获取周边的环境信息，并基于视觉里程计以及稠密建图方法来估算出无人机当前位置以及周边建筑物布局，再开启毫米波雷达，得到目标回波，但毫米波雷达回波仍旧存在无人机杂波以及地面杂波，需要根据视觉传感器所建立的建筑物布局对无人机所反射的杂波进行剔除，并同时矫正目标位置。最后，根据初步的探测结果，无人机载电脑对结果进行滤波处理，从而实现对城市非视距目标的自主定位功能。

Description

无人机载毫米波雷达与视觉设备的非视距定位系统与方法

技术领域

本发明属于非视距范围目标探测技术领域，特别涉及一种非视距目标定位技术。

背景技术

在城市环境中，由于城市环境建筑布局复杂、电磁环境复杂以及气象受限等问题，导致危险人员目标可躲藏于遮蔽物后，达到避免被发现等目的。采用基于光学反射原理的摄像头传感器由于难以透过遮蔽物，并且也受制于气象、光照等问题，无法定位非视距目标，而对于主动发射近红外光的IR(近红外)传感器。由于需要直接照射到目标，对于需要一定隐蔽性的应用场景则受到了相当的限制。

因此，基于发射电磁波的雷达系统来探测这类人员目标在近年来得到了相当的发展。这些方法通常是基于电磁波的穿透特性以及多径传播特性。其中，基于电磁波穿透特性的方法常用于穿墙目标定位以及建筑布局成像，而基于电磁波多径传播特性的方法常用于室内非视距目标定位。然而，这些非视距定位方法几乎是基于地基平台的，这些平台不仅受限于灵活性问题，并且后者基于多径传播的方法需要建筑先验信息来对定位目标进行矫正。因此，为了应对地基平台的灵活性问题以及对于建筑物先验信息的需求问题，许多学者提出将毫米波雷达部署在诸如飞行器、无人车的动平台上进行多径目标探测定位，以此来解决平台灵活性问题，并同时利用直接照射、雷达成像方法或者视觉传感器等方法来获取建筑信息。

毫米波雷达是一种发射FMCW(调频连续波形)，并通过接受目标的回波来定位目标的传感器。相比于其他雷达，毫米波雷达尺寸较小，可封装在PCB(印刷电路板)上，并且其载波频率更高，具备更大的带宽，因而其速度分辨率与距离分辨率更高。同时，相比于光学传感器，毫米波雷达同其他雷达一样不受雾霾、光照、气象的影响。因而十分适合用于动平台上的多径探测。在传统的无人机动平台上，毫米波雷达被成功地用于前前车探测、“鬼探头”目标预警以及多径目标识别。但是考虑到城市环境中的人员目标也会处于不同高度，因而需要更加灵活的机载平台。现有的机载雷达平台也实现了多径探测，但依旧缺乏完全利用多径传播路径并实现完全的多径探测的方案。同时，现有用于探测城市环境的机载平台大多是基于大型的直升机、卫星平台，这些平台尽管实现了不同高度的多径目标定位，但不便于在城市不同建筑物之间灵活地变换位置。

发明内容

为了解决上述的技术问题，本发明提出了一种无人机载毫米波雷达与视觉设备的非视距定位系统与方法，利用无人机载毫米波雷达和视觉传感器来采集数据，并将视觉数据用于建立周边建筑布局信息、城市区域自主路径规划，将毫米波雷达用于探测非视距目标、剔除杂波目标。最终，基于无人机载平台，实现了三维非视距目标的精确定位功能。

本发明采用的技术方案之一为：一种无人机载毫米波雷达与视觉设备的非视距定位系统，包括：无人机、控制系统、机载电脑、视觉传感器、云台以及毫米波雷达；其中，毫米波雷达放置在位于无人机底座的云台上，视觉传感器设置于无人机机架之上；

视觉传感器在控制系统的控制下工作，用于获取周边环境的信息，并将获取的周边环境信息传送至机载电脑；

机载电脑用于根据无人机的周边环境信息，得到当前环境的建筑布局信息；

控制系统根据当前环境的建筑布局信息控制无人机进行自主规划路线；

毫米波雷达在控制系统的控制下工作，用于获取当前环境的建筑布局中墙面、地面以及潜在的目标回波；并将目标回波传送至机载电脑；

机载电脑根据当前环境的建筑布局信息与毫米波雷达获取的目标回波，完成三维非视距目标的定位。

本发明采用的技术方案之二为：一种无人机载毫米波雷达与视觉设备的非视距定位方法，包括：

S1、获取视觉传感器数据，并对所获取的视觉传感器数据进行处理，得到当前建筑物布局信息；

S2、选取感兴趣的目标地点，无人机进行自主规划路线；同时无人机在移动过程中实时更新当前建筑物布局信息；

S3、无人机到达感兴趣的目标地点之后，开启毫米波雷达，利用当前建筑物布局信息，接收毫米波雷达数据；

S4、基于多个脉冲的毫米波雷达数据进行速度向FFT，获得距离-多普勒谱图；

S5、步骤S3接收到的毫米波雷达数据、步骤S4获得的距离-多普勒谱图，获取目标的初步三维定位结果；

S6、根据当前建筑布局信息，对初步三维定位结果进行矫正，得到真实的目标定位结果；

S7、对真实的目标定位结果进行优化，得到精确地目标定位结果。

本发明的有益效果：本发明提供了一种无人机载毫米波雷达与视觉设备的非视距定位系统与方法，可实现在城市环境中无人机自主路线规划，建立建筑布局，并利用无人机载毫米波雷达实现对非视距目标的定位功能。同时，配合已建立的地图信息和无人机位置信息，移除受无人机运动与无人机桨叶旋转干扰的单元。因此，本方法相比于地基平台方法与现有的机载平台方法，不仅具备优秀的灵活性，完全利用了多径传播路径实现了隐蔽性更佳的探测方法，并且，本方法还解决了传统多径探测非视距目标对于建筑先验信息的需求问题，可以直接运用在配备了毫米波雷达和视觉传感器的无人机平台上。同时，本发明所提滤除杂波方法以及路径规划方法也可用于其他毫米波雷达平台与无人机平台上，亦能直接用于非视距目标跟踪以及无人机路径规划。

附图说明

图1是本发明中仪器示意图；

其中，图1(a)是一种配备了激光发射头的双目摄像头；图1(b)是一种配备了在偏航轴云台的毫米波雷达；图1(c)是搭载了双目摄像头以及偏航轴云台毫米波雷达的无人机平台。

图2是本发明中场景示意图；

其中，图2(a)是本发明的虚拟场景图，图2(b)则是本发明为了测试本发明设计算法的有效性所布置的一个场景。

图3是本发明中视觉传感器的处理流程；

图4是本发明中无人机自主规划的处理流程；

图5是本发明中毫米波雷达的处理流程；

图6是本发明中无人机自主规划路线示意图；

其中，图6(a)展示了无人机在完全初始化的情况下，从零开始规划路线的情形；图6(b)展示了无人机在飞行的时候、建图的时候，并遭遇障碍物的情况下进行的路径修改，同时，在图6(b)中，还展示了RRT路线以及优化后的B样条曲线路径；图6(c)则展示了原始的RRT算法所规划的路线；

图7是本发明中数据处理结果图；

其中，图7(a)展示了初步定位结果；图7(b)展示了在同一场景下采用了本发明后的定位结果。

具体实施方式

为便于本领域技术人员理解本发明的技术内容，下面结合附图对本发明内容进一步阐释。

如图1(a)所示，为配备了双目视觉的传感器，运用其自带的激光头可以直接获得深度信息，并且也能利用双目视觉直接为视觉里程计(VIO,Visual-Inertial Odometry)提供里程信息。如图1(b)所示，为配备了单自由度旋转云台的毫米波雷达传感器，运用云台可调整毫米波雷达的角度，实现对同一障碍物不同角度的测量，以得到不同范围内的非视距目标。最后，图1(c)展示了本发明中的传感器安装示意图，其中，配备有毫米波雷达传感器的单自由度云台被放置在了无人机下方，以保证不受无人机自身桨叶的直接影响；而配备了双目视觉的传感器被放置在了无人机上方，以保证VIO所得姿态是以无人机机身为原点。

本发明首先通过视觉传感器获得周边环境的信息，将建筑布局信息加入到无人机载电脑中，由此无人机不仅可同步记录地图信息，还能根据地图信息自行更改规划路线，并根据已有的建筑信息来矫正目标定位。当无人机接收到航点指令的时候，先利用基于采样的路径规划算法(RRT,Rapidly-Exploring Random Tree)进行随机采样，再利用B样条曲线和Ego-Planner优化方法对路线进行动力学可行性评估、光滑化以及碰撞评估。当无人机到达指定位置的时候，无人机开启毫米波雷达，通过对已知布局上的墙面进行扫射，能得到墙面、地面以及潜在的目标回波。以此，先通过二维快速傅里叶变换(2D-FFT,Two-Dimensional Discrete Fourier Transform)得到距离-多普勒谱图(RD-Map,Range-Doppler-Map)，然后，利用RD-Map和建筑布局信息，生成合适的掩膜来分别滤除墙面、地面杂波，并同时滤除零频成分。再通过高虚警率的单元平均恒虚警率检测方法(CA-CFAR,cellaveraging-constant false alarm rate)，去除RD-Map中的噪声单元，并根据当前RD-Map的幅值大小设定阈值，去除幅值过低的单元，以得到初步检测单元。最后，利用三维快速傅里叶方法(3D-FFT,Three-dimensional fast Fourier transform)，对每个初步检测单元进行角度测量，得到初步定位结果。接着，利用每个初步定位结果和下一帧的几何、时间关系，去除噪声定位结果，以此得到最终定位结果，并利用三维扩展卡尔曼滤波(EKF3)，实现对无人机本身的震动的滤波效果。最后，根据双目视觉传感器建立的建筑布局信息，对最终定位结果进行矫正，以此实现在城市环境中定位三维的非视距目标的效果。

关于指定位置的说明，例如：无人机在没有任务的情况下将悬停等待地面人员给予指示，当地面人员指示无人机到达任务区域的时候，无人机将自主规划路线，并到达指定位置。

如图2(a)所示，为本发明的虚拟场景示意图，通过无人机载毫米波雷达和视觉传感器系统，无人机将分析环境特征和回波特征，并去除潜在的杂波单元，进行初步地除杂，同时，利用所提方法来进一步滤波与矫正，即可得到遮蔽的非视距目标。

在图2(a)标注出了无人机自身杂波的电磁波传播路径，并且无人机相对于墙面的速度为V_D1，该路径包含了无人机自身到墙面任意一点R_p1到毫米波雷达，以及无人机自身到墙面任意一点R_p1到地面R_p2再到毫米波雷达的路径，这些路径主要是由无人机自身到建筑物表面导致的雷达杂波传播路径。本发明将非视距目标模拟定位场景设置为如图2(b)所示的场景中，在该场景中，人体目标始终处于在无人机系统的非视距范围之内。其中，以无人机为原点中心，测试人员被要求从坐标(2.1m,1.7m)直行到(4.05m,2.21m)，无人机飞行高度为1米，并且周边障碍物的高度为1.5米。并且，无人机上的搭载毫米波雷达的云台被调整为保持和无人机相同的角度，以此来进行了实验。

本发明将以图2(b)中的实际测试场景为例，介绍具体的实现步骤：

步骤1：视觉信息处理，如图3所示，具体处理过程为：

步骤1-1：视觉信息处理

首先考虑到视觉传感器可提供双目视觉信息，视觉传感器基线信息b，相机焦距信息f。以此，假设已知左相机的某一像素点(x_l,y_l)和右相机某一像素点(x_r,y_r)具备匹配关系，并假设真实目标的位置位于点P(x,y,z)，那么，根据相似三角形关系，有：

同时，在Y轴上，也有：

以此，每个能得到匹配点的真实坐标有：

此时，利用RtabMAP根据双目视觉传感器的图像，即可获得当前环境的三维点云坐标P(x,y,z)。同时，利用VINS-Fusion算法所提供的本地姿态R_cw与位移T_cw，结合已知的无人机相对摄像头的姿态R_bc与位移T_bc，即可得到矫正后的点云P′：

P′＝R_bc(R_cwP+T_cw)+T_bc＝T_bwP

T_cw,T_bc∈R³

R_cw,R_bc∈R^3×3

并且其中，I是单位矩阵，此处/>是矩阵R_cw的转置。T_bw表示目标从摄像头到世界到坐标系转换。考虑到双目摄像头所提供的深度信息的可靠度和稠密度远低于市面上常用的彩色-深度摄像头(RGB-D)传感器，本发明在这里利用连续观测至少N帧的准则来滤除可靠度较低的点云，并同时根据距离阈值r滤除过远的点云数据，以此来剔除稠密度较低的点云数据。

VINS-Fusion算法的输入为双目视觉传感器的图像。

距离阈值r的依据应根据摄像头自身的基线所决定，在本实施例中采用了Realsense摄像头，尽管其最大距离能探测到65米，但为了精确在本实施例中将限制为7米，即距离阈值取值为7米。

步骤1-2：无人机自主路径规划与建筑布局记录，具体过程如图4所示：

假设无人机正在飞行悬停中，首先无人机将会自动开启视觉传感器来记录周边障碍物情况。利用图3所示的算法流程，无人机将开启视觉传感器，并同时自主建立周边地图。

当无人机接收到航点指令P_dest的时候，无人机立刻执行RRT算法，如算法1所示，RRT算法首先初始化了一个随机树结构Τ，基于Τ随机在采用图3所示方法建立的地图Map中进行采样，并得到位置P_rand。

根据位置P_rand，在树结构Τ中选择最近的节点P_near，并向节点P_near步进距离StepSize。当边E_i在地图Map是无碰撞可行的一条路线的情况下，在树结构Τ中插入新节点P_D与新路径边E_i。当且仅当P_D到达P_dest的情况下，算法结束运行，并可得到初步的可行路线航点P_D。

通过RRT算法，尽管可以在大型的空间中得到一条可行的路线，然而，该路线并不光滑，并且对于无人机来说，直接由RRT算法规划得到的路线在动力学上不可行，易发生碰撞。由此，可利用由RRT算法生成的航点P_D进而生成光滑的B样条曲线，并优化航点P_D的位置来避免上述问题。

为了生成和航点P_D所对应的B样条曲线，可以直接将航点P_D当作为B样条曲线的控制点来使用。即，新生成的B样条曲线P_B有以下关系：

其中，是航点P_D中的第i个航点，B_i,deg(s)是基本函数表的参数，其计算过程为：

当deg＝0的情况下，规定：

其中，deg代表B样条曲线的当前阶数，knot_i是第i个节点矢量，s是比例值。进一步的，由于此时得到了光滑的B样条曲线，但并未结合当前环境来设计无人机路径，由此，本发明结合了Ego-Planner中所提的针对碰撞、动力可行性以及光滑项目进行优化。针对每个路径点Ego-Planner首先定义了速度V_i，加速度A_i，以及加加速度J_i：

A_i＝V_i-V_i-1

J_i＝A_i-A_i-1

基于每个路径点，Ego-Planner首先提出利用平滑高阶导数，即最小化加速度以及加加速度来平滑轨迹，该项目为光滑项惩罚J_s：

其中，||·||₂表示取向量二范数，N_c是路径点的数目。其次，根据已观测到的障碍物，Ego-Planner提出可设置安全阈值s_f来优化每个路径点使得每条路径点远离所观测到的障碍物Q_i，达到重新规划出安全的路径点的目的，该项目为碰撞项惩罚J_c：

其中，c_ij＝s_f-d_ij，d_ij是第i个路径点到所观测到第j个障碍物的距离。利用该式子，有：

此外，Ego-Planner提出了动力学可行项J_d来实现限制三维轨迹，具体为：

其中，ω_v，ω_a，ω_j是每项目的权重，c_r是速度V_i、加速度A_i、加加速度J_i，a₁，a₂，b₁，b₂，c₁，c₂是多项式系数，c_m是导数限制项，c_j是二次、三次函数的交界，λ是弹性系数。

最后，结合以上项目，Ego-Planner将这些项目进行结合，并最小化惩罚项：

argminJ＝λ_sJ_s+λ_cJ_c+λ_dJ_d

其中，λ_s，λ_c，λ_d是各项目的权重。至此，无人机接收到航点信息的时候，无人机可安全地在城市环境中飞行，并且也能随时记录城市环境地图。特别地，当无人机载电脑检测到雷达开启的时候，无人机载电脑将同时记录该时刻下的无人机姿态、建筑布局信息，并用于后续定位目标矫正、杂波剔除等。

如图6所示，本发明在这里展示了采用所提方法的初步规划结果以及优化后规划结果。其中，图6(a)展示了初始状态下无人机执行的建图功能。在地图上选择任意一路径点，明显的，图6(b)所展示的Ego-Planner后所优化的路线较为光滑，并且避开了障碍物，在无人机上满足动力学可行性。反之，图6(c)所展示的RRT的规划路径较为曲折，对于无人机而言不太安全，易发生碰撞。

步骤2：雷达数据处理：

步骤2-1：初步检测结果获取：

首先建立雷达的回波模型，考虑到现有的毫米波雷达具备多发多收(MIMO，Multiple-Input Multiple-Output)天线，并且交替地发射连续线性调频信号(FMCW，Frequency Modulated Continuous Wave)，假设雷达的载波频率为f₀，调频周期为T，调频带宽为B，则调频斜率为幅度为A，得到发射信号S_T的表达式为：

假设目标为雷达的初始距离为R_t，目标相对于雷达的径向速度为v_t，那么，此时雷达回波的多普勒频率为其中，在这里c是光速，λ_r是波长。目标相对于雷达的距离为：R(t)＝R_t-v_tt，则接收信号的延迟为：

其中，以此，雷达的接收信号S_R则为：

其中，σ是目标的反射系数，将雷达本振信号和接收信号混频，首先可以得到差频信号S_D(t)：

以cos项为例，S_D(t)可被写为：

进一步地，经过低通滤波后得到差频信号的第一项，有：

显然地，该项目是一个频率为μτ(t)的中频信号，通过对其进行快速傅里叶变换处理(Fast Fourier Transform，FFT)，即可获得目标的距离信息，同时，该信号也包含了目标相对于雷达的径向速度信息。

考虑到雷达信号中依旧包含了环境中的静态杂波。因此，为了剔除这一部分的静态杂波，可以基于中频信号进行了一次动目标检测(MTI，Moving Targets Indication)，如图5所示，假设位于通道n索引i的毫米波雷达数据为x_n[i]，那么MTI后的毫米波雷达数据y_n[i]可以被写作为：

y_n[i]＝x_n[i]-x_n[i-1]

基于y_n[i]，直接进行距离向FFT即可获得距离像R，本发明在这里将第n个通道索引为k的距离像数据记作为Y_n[k]。

Y_n[k]＝FFT(y_n[i])

由于毫米波雷达通常会发射多个脉冲，因此可以基于多个脉冲的毫米波雷达数据进行速度向FFT，由此可获得距离-多普勒谱图(RD-Map)，本发明在这里将第n个通道索引为l，m的RD-Map记作为Z_n[l，m]，且有：

Z_n[l，m]＝FFT(Y_n[k])

为了剔除RD-Map中多余的噪声单元，采用CA-CFAR方法。其中，针对单元k的阈值TH_k可以被计算作：

其中，P_fa是虚警率，N_r是参考单元数目。针对RD-Map每个单元，有：

其中abs(*)是绝对值函数。最后，根据建筑物布局情况，设计掩膜来滤除无人机自身杂波的影响，假设，当前建筑物布局为Map，且无人机到达当前建筑布局最远距离为max(Map)，且当前无人机在RD-Map所对应的速度单元为V，那么进一步地对RD-Map对处理为：

其中，Map具体根据图3所示方法建立，M是RD-Map对速度维大小。以此，得到初步检测结果。根据每个通道的初步检测结果，可以进一步地进行角度维FFT，以此得到目标方位向角度A和俯仰向角度E，同时，根据距离向FFT的结果R，可以得到目标的初步三维定位结果P_T(x，y，z)：

步骤3：定位结果处理

步骤3-1：初步定位结果获取：

如图5下半部分所示，在获得初步三维定位结果后，利用各个建立的航迹的时间关系与几何关系来关联，假设已有第i条确定航迹为T_i(x，y，z)，第i条临时航迹为C_i(x，y，z)，那么针对初步三维定位结果P_T(x，y，z)，随机在选取P_T(x，y，z)中任意一个点P_C(x，y，z)，首先对其进行内部融合，以防止已有航迹T_i(x，y，z)出现重复更新的情况：

其中，P_A是指定的关联距离，norm表示取得两个定位点之间的距离，P_C(*)，P_T(*)表示取得定位点中的某个元素。R_A的取值是0.5米，根据人体目标的行走的速度约为1.6-3.2米/秒，本实施例中，雷达每帧持续的时间为0.04秒，假设目标每数帧(设为5帧)观测到一次，那么，经过计算雷达所能观测到的目标出现的距离约为0.32米至0.64米，本实施例取中间值即为0.5米。

进一步地，将融合后的初步三维定位结果对每条确定航迹T_i(x，y，z)与临时航迹C_i(x，y，z)进行更新：

即，最新的确定航迹T_i(x，y，z)和临时航迹C_i(x，y，z)分别对任意点P_C(x，y，z)进行关联，如果最新的航迹点与任意点P_C(x，y，z)的距离小于R_A，航迹点进行更新，丢失帧数置零，同时，增加临时航迹C_i(x，y，z)的观测帧数F_B；否则，不进行更新，并同时增加丢失帧数。

然后，每条航迹将检测自身的丢失帧数F_c，一旦丢失帧数大于指定值F_L，那么删除航迹：

F_L在本实施例中取值为20。

针对临时航迹C_i(x，y，z)，当其观测帧数F_B大于阈值F_O的情况下，将其归纳为新的确定航迹T_i(x，y，z)，并最后将无法与任何航迹关联上的初步三维定位结果P_T(x，y，z)，将其初始化为新的临时航迹C_i(x，y，z)。F_O在本实施例中取值为5。

最后，假设一条航迹从临时航迹C_i(x，y，z)被确认为确定航迹T_i(x，y，z)，由于此时的航迹坐标并不是真实的目标位置，而是由一阶多径反射而来的镜像坐标点，因此，需要利用建筑物布局距离r_w以及照射墙体相对无人机的角度θ_w来矫正目标定位点，将其转换为真实目标点，即定位结果矫正为：

其中，P′_T是矫正后的定位结果。矫正后的定位结果P′_T用于提供为后续扩展卡尔曼滤波的初始值，以供后续获得更加精确的定位结果P′_T。扩展卡尔曼滤波的初始值设置为(x，0，y，0，z，0)。

步骤3-2：初步定位结果优化：

虽然此时已将大部分杂波噪声剔除，但是无人机自身还有震动、位移，导致矫正后的三维定位结果存在偏差。因此，可利用扩展卡尔曼滤波方法(EKF3)实现对这部分噪声的滤除。

考虑到毫米波雷达所得到的目标定位，通常是目标距离、俯仰角度和方位角度，其包含了非线性项目，因此不适合利用卡尔曼滤波。

本发明为了利用EKF3方法，首先定义状态转移矩阵F：

即本发明将每条航迹的状态当作为(x，v_x，y，v_y，z，v_z)。同时，假设雷达观测目标的距离为R，且相对的xY平面的距离为R_xy，那么观测矩阵H为：

/>

同时，记作每条航迹的协方差为P，那么协方差的预测步可以被写作为：

其中，Q是运动方程噪声。而卡尔曼增益K可以被记作为：

其中，R是观测噪声。那么，滤波后的定位结果可以被计算为：

其中，Z是雷达的真实观测结果，Z_f是预测的观测结果，最后，协方差P可以被更新为：

以此，通过获取并更新即可求得滤波后的数据。

步骤4：城市环境下非视距目标三维定位实验：

依据图2(b)场景，得到的结果如图7所示，其中每个滤波后的确定航迹C_i(x,y,z)被用于作为定位结果。同时，基于已有的建筑布局信息进行矫正，并同时将多余的杂波去除。

本发明的效果通过以下实验验证进一步说明：

实验结果：

实验场景如图2(b)所示，开启无人机，并让无人机悬停在1米高度，防止无人机载雷达直接通过墙体探测到非视距目标。同时，开启毫米波雷达与摄像头，并探测非视距目标与建筑布局。

依据上述方法，得到的结果为图7所示，其中，图8(a)在非实现目标矫正之前的三维定位效果，该结果未使用本发明的处理方法，因而噪点严重，此时，目标定位仍然较为粗糙。图7(b)展示了非视距目标通过步骤3矫正后的结果，此时目标的定位点和真实的行走轨迹几乎一致，并用粗线标示了建筑布局的位置。同时，由于目标定位点仍旧有高阶的多径影响，导致了目标定位点有些许错误定位。

通过图7(a)、图7(b)的结果，以无人机视角直接展示了目标的定位结果；可以发现对应的雷达数据探测结果基本上能对应的上真实的人体非视距目标行走轨迹，可以发现，本发明设计的无人机载毫米波雷达三维定位系统具备定位非视距人员的能力，并且其准确性较高，表现较为稳定。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

Claims (8)

1.一种无人机载毫米波雷达与视觉设备的非视距定位系统，其特征在于，包括：无人机、控制系统、机载电脑、视觉传感器、云台以及毫米波雷达；其中，毫米波雷达放置在位于无人机底座的云台上，视觉传感器设置于无人机机架之上；

视觉传感器在控制系统的控制下工作，用于获取周边环境的信息，并将获取的周边环境信息传送至机载电脑；

机载电脑用于根据无人机获取的周边环境信息，得到当前环境的建筑布局信息；

控制系统根据当前环境的建筑布局信息控制无人机进行自主规划路线；

毫米波雷达在控制系统的控制下工作，用于获取当前环境的建筑布局中墙面、地面以及潜在的目标回波；并将目标回波传送至机载电脑；

机载电脑根据当前环境的建筑布局信息与毫米波雷达获取的目标回波，完成三维非视距目标的定位。

2.根据权利要求1所述的一种无人机载毫米波雷达与视觉设备的非视距定位系统，其特征在于，所述云台为单自由度旋转云台，用于调整毫米波雷达的角度。

3.一种基于权利要求2所述的一种无人机载毫米波雷达与视觉设备的非视距定位系统的无人机载毫米波雷达与视觉设备的非视距定位方法，其特征在于，包括：

S1、获取视觉传感器数据，并对所获取的视觉传感器数据进行处理，得到当前建筑物布局信息；

S2、选取感兴趣的目标地点，无人机进行自主规划路线；同时无人机在移动过程中实时更新当前建筑物布局信息；

S3、无人机到达感兴趣的目标地点之后，开启毫米波雷达，利用当前建筑物布局信息，接收毫米波雷达数据；

S4、基于多个脉冲的毫米波雷达数据进行速度向FFT，获得距离-多普勒谱图；

S5、步骤S3接收到的毫米波雷达数据、步骤S4获得的距离-多普勒谱图，获取目标的初步三维定位结果；

S6、根据当前建筑布局信息，对初步三维定位结果进行矫正，得到真实的目标定位结果；

S7、对真实的目标定位结果进行优化，得到精确地目标定位结果。

4.根据权利要求3所述的无人机载毫米波雷达与视觉设备的非视距定位方法，其特征在于，步骤S1具体为：利用RtabMAP根据双目视觉传感器的图像，获得当前环境的三维点云坐标；同时，利用VINS-Fusion方法获取本地姿态R_cw与位移T_cw，结合已知的无人机相对摄像头的姿态R_bc与位移T_bc，得到矫正后的三维点云坐标，根据矫正后的三维点云坐标获得当前建筑物布局信息。

5.根据权利要求4所述的无人机载毫米波雷达与视觉设备的非视距定位方法，其特征在于，步骤S2具体为：

利用基于采样的路径规划算法对当前建筑物布局信息所建立的地图进行随机采样，得到初步的可行路线航点P_D；

利用B样条曲线和Ego-Planner优化方法对初步的可行路线航点P_D进行优化；得到优化后的路线；

无人机根据优化后的路线到达感兴趣的目标地点。

6.根据权利要求5所述的无人机载毫米波雷达与视觉设备的非视距定位方法，其特征在于，步骤S5具体包括以下实现过程：

S51、采用CA-CFAR方法，剔除距离-多普勒谱图中多余的噪声单元；

S52、对经步骤S51处理后的距离-多普勒谱图，根据当前建筑物布局信息，设计掩膜来滤除无人机自身杂波的影响；从而得到初步检测结果；

S53、根据每个通道的初步检测结果，进一步地进行角度维FFT，得到目标方位向角度A和俯仰向角度E，同时，记对步骤S3接收到毫米波雷达数据进行距离向FFT的结果为R，得到目标的初步三维定位结果P_T(x,y,z)表示为：

7.根据权利要求6所述的无人机载毫米波雷达与视觉设备的非视距定位方法，其特征在于，步骤S6具体为：

对初步三维定位结果P_T(x,y,z)进行去噪处理，具体过程包括：

针对初步三维定位结果P_T(x,y,z)，随机选取P_T(x,y,z)中任意一个点P_C(x,y,z)；

最新的确定航迹和临时航迹分别对任意点P_C(x,y,Z)进行关联，如果最新的航迹点与任意点P_C(x,y,z)的距离小于关联距离R_A，航迹点进行更新，丢失帧数置零，同时，增加临时航迹的观测帧数F_B；否则，不进行更新，并同时增加丢失帧数F_c；

然后，每条航迹检测自身的丢失帧数F_c，一旦丢失帧数大于指定值F_L，那么删除航迹；

针对临时航迹，当其观测帧数F_B大于阈值时，将其归纳为新的确定航迹，最后将无法与任何航迹关联上的初步三维定位结果P_T(x,y,z)，将其初始化为新的临时航迹；

对去噪后的初步三维定位结果P_T(x,y,z)，利用建筑物布局距离r_w以及照射墙体相对无人机的角度θ_w，将其转换为真实目标点：

其中，P′_T是真实的目标定位结果，x′、y′、z′分别为P′_T的x轴、y轴、z轴坐标。

8.根据权利要求7所述的无人机载毫米波雷达与视觉设备的非视距定位方法，其特征在于，步骤S7具体采用扩展卡尔曼滤波方法对真实的目标定位结果进行处理。