CN109709801B

CN109709801B - 一种基于激光雷达的室内无人机定位系统及方法

Info

Publication number: CN109709801B
Application number: CN201811511818.4A
Authority: CN
Inventors: 马鸣; 丁久辉
Original assignee: Intelligent & Agile Aerocraft Beijing Technology Co ltd
Current assignee: Intelligent & Agile Aerocraft Beijing Technology Co ltd
Priority date: 2018-12-11
Filing date: 2018-12-11
Publication date: 2024-02-02
Anticipated expiration: 2038-12-11
Also published as: CN109709801A

Abstract

本发明属于雷达系统与非雷达系统技术领域，公开了一种基于激光雷达的室内无人机定位系统及方法，利用INS消除激光雷达在采集点云过程中的快速运动畸变；完成激光雷达采集到的相邻点云之间的位姿估计、检测无人机是否经过已经走过的地方、对之前的各种估计进行全局优化；通过传感器数据获得精确的无人机位姿和环境的位置信息，根据控制需求完成无人机控制命令的生成。本发明的激光雷达作为一种测距传感器，具有分辨率高、抗干扰能力强等优点，能够适用于多种复杂场景，完成姿态测量、运动估计以及形状构造等任务。本发明可以通过传感器数据获得精确的无人机位姿和环境的位置信息，根据控制需求完成无人机控制命令的生成。

Description

一种基于激光雷达的室内无人机定位系统及方法

技术领域

本发明属于雷达系统与非雷达系统技术领域，尤其涉及一种基于激光雷达的室内无人机定位系统及方法。

背景技术

目前，业内常用的现有技术是这样的：当前，大多数无人机都是室外飞行，不便于课堂讲授和演示，一款飞行性能优良、便于课堂教学的室内无人机成为无人机教学领域的刚需；无人机的室内飞行也一直是无人机领域的研究热点之一，这是由于在GPS信号较弱的室内环境中，基于GPS的常规定位方法失效。因此，室内环境下精确的位置估计是无人机面临的又一大难题。同步定位与地图构建(Simultaneous Localization and Mapping，SLAM)是机器人智能化进程中的基础技术之一。机器人利用该项技术可以在未知的环境当中，通过一定数量的传感器，实现自身位置和姿态的确定，同时建立周围环境的图像信息，为机器人的高级功能提供技术支持。无人机作为机器人中特殊的一类，SLAM方法在该领域的应用也成为了研究的重点和难点，各国学者都在寻求稳定而可靠的SLAM方案来解决无人机的自主定位问题。

综上所述，现有技术存在的问题是：

(1)一般多旋翼无人机的定位手段单一，只适合室外定位，依赖GPS信号。在室内环境下，没有GPS信号，则无法完成位置定位。

(2)SLAM方法是针对无人机自主飞行的一种可进行位置定位和地图重建的方法。其计算量较大，需要携带机载计算机，更新频率慢，实时性不够，无法直接应用于无人机定位和控制。

解决上述技术问题的难度和意义：

通过融合IMU数据和SLAM数据，可以得到具有实时性，更新频率快的位置数据；并可同时得到局部地图。无人机可根据这些数据，在未知封闭空间进行自主飞行，探索周围环境。

发明内容

针对现有技术存在的问题，本发明提供了一种基于激光雷达的室内无人机定位系统及方法。

本发明是这样实现的，一种基于激光雷达的室内无人机定位方法，所述基于激光雷达的室内无人机定位方法利用INS消除激光雷达在采集点云过程中的快速运动畸变；完成激光雷达采集到的相邻点云之间的位姿估计、检测无人机是否经过已经走过的地方、对之前的各种估计进行全局优化；通过传感器数据获得精确的无人机位姿和环境的位置信息，根据控制需求完成无人机控制命令的生成。

进一步，所述基于激光雷达的室内无人机定位方法包括以下步骤：

步骤一，从INS系统中读取无人机的加速度、角速度数据，通过航迹推算计算无人机实时位姿、速度以及位移，以备点云数据的校准；

步骤二，激光雷达每一采集时刻以较高分辨率完成平面扫描，记为一次Scan扫描；然后同步INS数据完成数据融合，消除每一Scan扫描的运动畸变，利用激光里程计算法进行特征提取；

步骤三，激光雷达在电机的控制下绕垂直于扫描平面进行360°旋转，其分辨率低于平面扫描，旋转一周完成一次三维空间的扫描，记为一次Sweep扫描；当完成一次Sweep扫描后开始利用激光里程计算法进行和下一次的Sweep扫描的特征匹配，建立位姿估计方程并进行求解，获得无人机位姿的粗略估计，构建位姿图模型，完成前端算法；

步骤四，根据构建的位姿图模型进行非线性优化，利用优化完成后端算法求解，并且输出地图模型；如果检测到回环，利用回环算法进行位姿约束方程的构建，将约束以边的形式添加到位姿图模型中，根据优化结果进行导航避障等高级应用。

进一步，所述步骤三中特征匹配需要连续两帧的点云数据，t_k表示第k次Sweep扫描的开始时间，在第k次Sweep扫描结束后，将所获得的点云集合Ω_k重映射到下一次扫描的开始时间t_k+1，标记为并且利用三维空间的KD-tree存储，然后利用/>去和第k+1次扫描采集到的点做匹配；

在第k+1次扫描中，利用上节介绍的方法计算点云的曲率，获得特征角点集合和特征平面点集合，分别记为E_k+1和H_k+1；采用中的边线作为特征角点的匹配，采用/>中的局部平面作为特征平面点的匹配，其中，边线需要在/>寻找两个最近点来构建，而局部平面则需要三个最近点表示；

在第k+1次Sweep扫描的开始时间t_k+1，采集到的点云数据集Ω_k+1是空的，每次匹配时，都需要将E_k+1和H_k+1重映射到初始时刻，标记为和/>假设/>中的第i个点为通过KD-tree搜索，在/>中寻找其最近点，记为/>然后在/>相邻的前后两次Scan扫描中寻找/>的最近点，记为/>注意该点和/>不属于同一次Scan扫描；

同样，中的第i个点为/>在/>中搜寻3个点形成局部平面，作为特征平面点的匹配，在KD-tree中寻找最近点，记为/>然后继续搜索另外两个最近点，其中一个和来自同一次的Scan扫描，另外一个则是在/>相邻的前后两次Scan扫描中寻找/>的最近点，分别标记为/>和/>找到三个点组成的局部平面/>作为特征角点/>的匹配；

计算特征点和它的匹配之间的距离，通过最小化全部距离来估计激光雷达的运动，特征角点和对应边线匹配之间的距离为：

特征平面点和对应局部平面匹配之间的距离为：

t是激光雷达当前的时刻，t_k+1是第k+1次3D扫描的开始时间，是激光雷达在时间[t_k+1,t]内的位姿变换矩阵，并且：

式中，t_x,t_y,t_z表示激光雷达在雷达坐标系中的平移量，θ_x,θ_y,θ_z表示激光雷达在雷达坐标系中的旋转角度。对于Ω_k+1中的任意一个点云点i，i∈Ω_k+1，t_i是其时间戳，在时间位姿变换矩阵[t_k+1,t_i]，根据/>计算：

建立E_k+1和以及H_k+1和/>之间的几何关系，/>是E_k+1或者H_k+1中的点云点，是/>或者/>中的点云点，利用上面的变换关系有：

和/>中的点与其上一帧中的匹配之间的距离d_E和d_H，建立E_k+1和其匹配边线之间的几何关系：

以及H_k+1和其匹配平面之间的几何关系：

对每一个特征点进行处理之后，获得表示两帧之间几何关系的非线性方程：

式中，每一行的f对应一个特征点，d是和相应匹配之间的距离，计算关于/>的雅可比矩阵，标记为J：

然后进行非线性迭代，使得d趋近于零，即：

式中，λ是一个系数。

进一步包括：算法的输入包括前一帧的重映射点云数据最近一次的位姿变换矩阵/>以及点云数量逐渐增加的当前帧点云数据Ω_k+1；如果新的Sweep扫描开始，/>会被重新置零；将从Ω_k+1中提取特征点，形成特征点集合E_k+1和H_k+1；对每个特征点，在/>中寻找它的匹配点；对每次迭代求解出的位姿变换矩阵进行了更新；随着Ω_k+1中的数据不断增加继续迭代，一直到该帧结束，通过最终的变换矩阵Ω_k+1被重映射到下一帧的初始时刻，继续新的运动估计：

进一步，所述步骤四中SLAM的优化问题最终都转变成为非线性最小二乘的求解：

最小二乘问题：

首先对F(x)进行一阶泰勒展开：

F(x+Δx)≈F(x)+J(x)Δx；

式中，J(x)表示雅可比矩阵，通过一阶近似线性化，目标函数变为：

通过寻找Δx使得目标函数完成最小化的任务。展开目标函数：

对上式求Δx的导数，并且使其等于零，得到方程：

J(x)^TJ(x)Δx＝-J(x)^TF(x)；

关于Δx的线性方程，左侧的系数定义为H，右边定义为g，则：

HΔx＝g；

求解该方程获得增量Δx，利用x^*＝x+Δx进行更新优化变量，迭代求解最小二乘问题，直到满足终止条件便可得到位姿估计的最优状态。

本发明的另一目的在于提供一种实施所述基于激光雷达的室内无人机定位方法的基于激光雷达的室内无人机定位系统，所述基于激光雷达的室内无人机定位系统包括：

无人机平台，用于搭载系统构成的各个有效部件和模块，具有可扩展性机架；

激光雷达，用于采集飞行过程的相关数据；

机载计算机模块，用于完成定位算法的配置和运行；

飞行控制模块，根据机载计算机生成的控制命令控制动力系统以调整无人机的姿态；同时接收遥控器模块的控制命令，或者根据地面站指令选择飞行模式；

动力系统，由电机和电子调速器组成，用于根据飞行控制模块的指令完成无人机位姿的调整和更改。

进一步，所述机载计算机模块包括激光里程计、回环检测以及后端优化；生成无人机位姿；通过和飞行控制模块的接口读取INS信息，用于校正激光雷达的数据采集过程中的运动畸变，根据控制需求生成相应的指令发送给飞行控制模块，实现最终的无人机控制任务。

进一步，所述基于激光雷达的室内无人机定位系统的其他辅助系统包括地面站系统、遥控器系统。

本发明的另一目的在于提供一种应用所述基于激光雷达的室内无人机定位方法的无人机。

综上所述，本发明的优点及积极效果为：本发明的室内无人机定位系统主要由无人机平台、机载计算机、飞行控制器、激光雷达、INS系统、电机等构成。无人机平台使用四旋翼无人机，具有可扩展性机架，方便安装各种扩展模块。机载计算机完成主要定位算法的运行，计算无人机的位姿，同时和飞行控制器通信，读取无人机的实时状态，并且生成控制命令，然后向飞行控制器发送控制命令。激光雷达采用低成本的二维激光雷达，在电机的驱动下激光雷达的扫描平面能够完成360°的旋转，实现扫描三维空间的目的，同时，通过编码器可以调节电机的旋转速度以适合相应的工作频率。飞行控制器中包含处理器、加速度计、陀螺仪、磁强计等部分，负责测量无人机的姿态，同时接收遥控器或者机载计算机的命令信号，控制无人机的电机以及其他底层设备，实现无人机的控制。

本发明基于激光雷达的室内无人机定位方法采用图SLAM框架，传感器数据使用激光雷达的点云和INS的数据，利用INS消除激光雷达在采集点云过程中的快速运动畸变，可以提升整个定位方法的精度。由基于曲率特征的激光里程计、基于NDT外形特征的回环检测以及基于位姿图的后端优化组成，其中激光里程计部分完成激光雷达采集到的相邻点云之间的位姿估计，回环检测部分可以检测无人机是否经过已经走过的地方，后端优化部分对之前的各种估计进行全局优化，降低误差。经过该方法的处理，可以通过传感器数据获得精确的无人机位姿和环境的位置信息，根据控制需求完成无人机控制命令的生成。激光雷达作为一种测距传感器，具有分辨率高、抗干扰能力强等优点，能够适用于多种复杂场景，完成姿态测量、运动估计以及形状构造等任务。近年来，激光雷达在无人驾驶领域应用广泛，有关激光雷达的研究，比如固态激光的解决方案、激光雷达成本控制等，引起了诸多学者和研究单位的关注。

附图说明

图1是本发明实施例提供的基于激光雷达的室内无人机定位系统结构示意图；

图中：1、无人机平台；2、激光雷达；3、机载计算机；4、飞行控制模块；5、动力系统。

图2是本发明实施例提供的基于激光雷达的室内无人机定位方法流程图。

图3是本发明实施例提供的基于激光雷达的室内无人机定位方法实现流程图。

图4是本发明实施例提供的特征角点匹配过程示意图。

图5是本发明实施例提供的特征平面点的匹配过程示意图。

图6是本发明实施例提供的CMU数据集实验无人机位置曲线图。

图7是本发明实施例提供的CMU数据集实验三维建图。

图8是本发明实施例提供的KITTI数据集定位算法估计误差图

图9是本发明实施例提供的KITTI数据集定位算法估计误差图。

图10是本发明实施例提供的KITTI数据集定位算法估计误差图。

图11是本发明实施例提供的KITTI数据集定位算法估计误差图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

下面结合附图对本发明的应用原理作详细的描述。

如图1所示，本发明实施例提供的基于激光雷达的室内无人机定位系统包括：无人机平台1、激光雷达2、机载计算机3、飞行控制模块4、动力系统5。

无人机平台1，用于搭载系统构成的各个有效部件和模块，具有可扩展性机架，方便随时增加和更换系统模块；

激光雷达2，用于采集飞行过程的相关数据；

机载计算机模块3，采用Linux操作系统，完成定位算法的配置和运行，包括激光里程计、回环检测以及后端优化，生成精确的无人机位姿，保存精确的环境地图，通过和飞行控制模块4的接口读取INS信息，用于校正激光雷达2的数据采集过程中的运动畸变，根据控制需求生成相应的指令发送给飞行控制模块，实现最终的无人机控制任务。

飞行控制模块4，根据机载计算机3生成的控制命令控制动力系统以调整无人机的姿态，为了保证无人机飞行的安全性和可靠性，飞行控制模块4同时可以接收遥控器模块的控制命令，或者根据地面站指令选择飞行模式。

动力系统5，由电机和电子调速器组成，用于根据飞行控制模块4的指令完成无人机位姿的调整和更改。

本发明实施例提供的基于激光雷达的室内无人机定位系统的其他辅助系统可以包括地面站系统、遥控器系统等。

如图2所示，本发明实施例提供的基于激光雷达的室内无人机定位方法包括以下步骤：

S201：从INS系统中读取无人机的加速度、角速度等数据，通过航迹推算计算无人机实时位姿、速度以及位移，以备点云数据的校准；

S202：激光雷达每一采集时刻以较高分辨率完成平面扫描，记为一次Scan扫描；然后同步INS数据完成数据融合，消除每一Scan扫描的运动畸变，利用激光里程计算法进行特征提取；

S203：激光雷达在电机的控制下绕垂直于扫描平面进行360°旋转，其分辨率低于平面扫描，旋转一周完成一次三维空间的扫描，记为一次Sweep扫描；当完成一次Sweep扫描后开始利用激光里程计算法进行和下一次的Sweep扫描的特征匹配，建立位姿估计方程并进行求解，获得无人机位姿的粗略估计，构建位姿图模型，完成前端算法；

S204：根据构建的位姿图模型进行非线性优化，利用优化完成后端算法求解，并且输出地图模型；如果检测到回环，利用回环算法进行位姿约束方程的构建，将约束以边的形式添加到位姿图模型中，根据优化结果进行导航避障等高级应用。

在步骤S203中特征匹配需要连续两帧的点云数据，假设t_k表示第k次Sweep扫描的开始时间，在第k次Sweep扫描结束后，需要将所获得的点云集合Ω_k重映射到下一次扫描的开始时间t_k+1，标记为并且利用三维空间的KD-tree存储，然后利用/>去和第k+1次扫描采集到的点做匹配。

在第k+1次扫描中，利用上节介绍的方法计算点云的曲率，获得特征角点集合和特征平面点集合，分别记为E_k+1和H_k+1。由于激光雷达的运动，很难在中找到直接对应的点，所以，本发明采用/>中的边线作为特征角点的匹配，采用/>中的局部平面作为特征平面点的匹配，其中，边线需要在/>寻找两个最近点来构建，而局部平面则需要三个最近点来表示。

事实上，在第k+1次Sweep扫描的开始时间t_k+1，采集到的点云数据集Ω_k+1是空的，随着时间的增加，采集到的数据量不断扩大。点云的特征提取和匹配是在此过程中迭代进行的，所以每次匹配时，都需要将E_k+1和H_k+1重映射到初始时刻，标记为和/>假设中的第i个点为/>通过KD-tree搜索，在/>中寻找其最近点，记为/>然后在相邻的前后两次Scan扫描中寻找/>的最近点，记为/>注意该点和/>不属于同一次Scan扫描，这是因为某点的匹配点在同一激光束下只能有一个。至此，已经找到由两个最近点组成的边线/>作为特征角点/>的匹配，如图4所示。

同样，假设中的第i个点为/>需要在/>中搜寻3个点形成局部平面，作为特征平面点的匹配。和之前的方法类似，在KD-tree中寻找最近点，记为/>然后继续搜索另外两个最近点，其中一个和/>来自同一次的Scan扫描，另外一个则是在/>相邻的前后两次Scan扫描中寻找/>的最近点，分别标记为/>和/>至此，已经找到三个点组成的局部平面/>作为特征角点/>的匹配，如图5所示。

经过上述步骤，已经完成特征点的步骤，接下来需要计算特征点和它的匹配之间的距离，在下一节将通过最小化全部距离来估计激光雷达的运动。特征角点和对应边线匹配之间的距离为：

特征平面点和对应局部平面匹配之间的距离为：

假设t是激光雷达当前的时刻，t_k+1是第k+1次3D扫描的开始时间，是激光雷达在时间[t_k+1,t]内的位姿变换矩阵，并且：

式中，t_x,t_y,t_z表示激光雷达在雷达坐标系中的平移量，θ_x,θ_y,θ_z表示激光雷达在雷达坐标系中的旋转角度。对于Ω_k+1中的任意一个点云点i，i∈Ω_k+1，假设t_i是其时间戳，是其在时间位姿变换矩阵[t_k+1,t_i]，可以根据/>计算：

为了计算激光雷达的运动，需要建立E_k+1和以及H_k+1和/>之间的几何关系，假设/>是E_k+1或者H_k+1中的点云点，/>是/>或者/>中的点云点，利用上面的变换关系有：

上一节计算了和/>中的点与其上一帧中的匹配之间的距离d_E和d_H，结合上式，可以建立E_k+1和其匹配边线之间的几何关系：

以及H_k+1和其匹配平面之间的几何关系：

对每一个特征点进行上述处理之后，可以获得表示两帧之间几何关系的非线性方程：

式中，每一行的f对应一个特征点，d是和相应匹配之间的距离。本发明使用LM方法求解上述方程，为此，计算关于/>的雅可比矩阵，标记为J，即：

然后进行非线性迭代，使得d趋近于零，即：

式中，λ是一个系数。

求解上述方程，可以获得这两帧之间的位置姿态变换，从而确定激光雷达或者无人机以及环境路标点的位置信息，但是该结果是粗略估计，仍然需要进一步优化。

该算法的输入包括前一帧的重映射点云数据最近一次的位姿变换矩阵/>以及点云数量逐渐增加的当前帧点云数据Ω_k+1。如果新的Sweep扫描开始，/>会被重新置零；接下来该算法将从Ω_k+1中提取特征点，形成特征点集合E_k+1和H_k+1；对每个特征点，在/>中寻找它的匹配点。另外，算法对每个特征点进行了权重的设置，与匹配距离较远的特征点的权重小，并且如果距离值超出阈值，则该点的权重为零；接着对每次迭代求解出的位姿变换矩阵进行了更新。算法随着Ω_k+1中的数据不断增加继续迭代，一直到该帧结束，通过最终的变换矩阵Ω_k+1被重映射到下一帧的初始时刻，继续新的运动估计。

在步骤S204中SLAM的优化问题最终都转变成为非线性最小二乘的求解，本节将研究这类问题的求解步骤。

假设一个最小二乘问题：

首先对F(x)进行一阶泰勒展开：

F(x+Δx)≈F(x)+J(x)Δx (12)

式中，J(x)表示雅可比矩阵。通过一阶近似线性化，目标函数变为：

对上式求Δx的导数，并且使其等于零，可以得到方程：

J(x)^TJ(x)Δx＝-J(x)^TF(x) (15)

这是一个关于Δx的线性方程，左侧的系数定义为H，右边定义为g，则：

HΔx＝g (16)

求解该方程可以获得增量Δx，然后利用x^*＝x+Δx进行更新优化变量，迭代求解最小二乘问题，直到满足终止条件便可得到位姿估计的最优状态。

上述步骤可以完成最优化算法求解，并且在展开点附近具有良好的近似效果，为了提升局部近似的精度，可以通过给Δx增加可信赖区域扩大近似范围。假设存在非负数对角阵D，使用拉格朗日乘子构建优化问题：

对上式进行展开求导运算，可以得到增量的线性方程：

HΔx+λD^TDΔx＝g (18)

式中，λ表示拉格朗日乘子。

利用该方法求解问题，可以减少线性方程的系数矩阵的非奇异问题，搜索更加稳定、精确的增量Δx。完整的计算流程下表：

1、给定初始值x₀：以及初始优化半径；

2、对于第k次迭代，求解：

3、计算：

4、若则μ＝2μ；否则μ＝0.5μ；

5、如果ρ大于某阈值，令x_k+1＝x_k+Δx_k；

6、判断是否收敛。如果不收敛返回第2步，否则结束。

如图3所示，本发明实施例提供的基于激光雷达的室内无人机定位方法具体包括以下步骤：从INS系统中读取无人机的加速度、角速度等数据，通过航迹推算计算无人机实时位姿、速度以及位移，以备点云数据的校准；激光雷达属于二维扫描，每一采集时刻以较高分辨率完成平面扫描，记为一次Scan扫描；然后同步INS数据完成数据融合，消除每一Scan扫描的运动畸变，之后利用激光里程计算法进行特征提取。同时，激光雷达在电机的控制下绕垂直于扫描平面进行360°旋转，其分辨率低于平面扫描，旋转一周完成一次三维空间的扫描，记为一次Sweep扫描；当完成一次Sweep扫描后开始利用激光里程计算法进行和下一次的Sweep扫描的特征匹配，建立位姿估计方程并进行求解，获得无人机位姿的粗略估计，构建位姿图模型，完成前端算法。根据构建的位姿图模型进行非线性优化，利用优化完成后端算法求解，并且输出地图模型。如果检测到回环，利用回环算法进行位姿约束方程的构建，将约束以边的形式添加到位姿图模型中，根据优化结果进行导航避障等高级应用。通过上述步骤，可以根据传感器数据完成无人机的位姿确定，具有较高的定位精度，同时满足无人机定位的实时性和可靠性需求。

本发明使用更加完整的两个数据集进行基于图优化的SLAM算法的验证。第一个来自CMU的数据集，该数据集是手持无人机在一个楼梯中采集到的，移动速度是5m/s，过程中存在一定的抖动，通过该实验可以检验INS数据对基于激光雷达的SLAM算法的影响，如图6CMU数据集实验无人机位置曲线。

图中黑色点是起点，黑色曲线是只有INS工作时的无人机位置曲线，可以看到随着时间的增加，和无人机的实际曲线越来越大；浅灰色曲线是没有融合INS数据的SLAM算法得到的无人机位置，深灰色曲线是经过INS校准之后的SLAM算法的输出结果，可以看到相对于只有INS时的情况，这两种方案都可以获得更精确的无人机位置信息，而且保持在长时间内有一定的精度。经过统计，只用INS系统的情况下，运动估计的误差是11.4％，只有激光雷达的SLAM算法的误差是3.7％，而经过INS和激光雷达点云数据融合的SLAM算法的误差是2.1％。图7是图6中的深灰色曲线对应的三维场景的重建结果。

第二次实验是利用KITTI数据集进行验证，KITTI是数据集包含比较大的场景信息，同时具有算法评估系统，通过不同场景序列的运行，获得SLAM算法在无人机运动估计上的误差平均值，本发明研究的算法的运行结果如图8-图11分别表示不同飞行距离和不同飞行速度下的无人机的平移运动估计和旋转运动估计的误差值。

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

Claims

1.一种基于激光雷达的室内无人机定位方法，其特征在于，所述基于激光雷达的室内无人机定位方法利用INS消除激光雷达在采集点云过程中的快速运动畸变；完成激光雷达采集到的相邻点云之间的位姿估计、检测无人机是否经过已经走过的地方、对之前的各种估计进行全局优化；通过传感器数据获得精确的无人机位姿和环境的位置信息，根据控制需求完成无人机控制命令的生成；

所述基于激光雷达的室内无人机定位方法包括以下步骤：

步骤四，根据构建的位姿图模型进行非线性优化，利用优化完成后端算法求解，并且输出地图模型；如果检测到回环，利用回环算法进行位姿约束方程的构建，将约束以边的形式添加到位姿图模型中，根据优化结果进行导航避障等高级应用；

所述步骤三中特征匹配需要连续两帧的点云数据，t_k表示第k次Sweep扫描的开始时间，在第k次Sweep扫描结束后，将所获得的点云集合Ω_k重映射到下一次扫描的开始时间t_k+1，标记为并且利用三维空间的KD-tree存储，然后利用/>去和第k+1次扫描采集到的点做匹配；

在第k+1次Sweep扫描的开始时间t_k+1，采集到的点云数据集Ω_k+1是空的，每次匹配时，都需要将E_k+1和H_k+1重映射到初始时刻，标记为和/> 中的第i个点为/>通过KD-tree搜索，在/>中寻找其最近点，记为/>然后在/>相邻的前后两次Scan扫描中寻找/>的最近点，记为/>注意该点和/>不属于同一次Scan扫描；

同样，中的第i个点为/>在/>中搜寻3个点形成局部平面，作为特征平面点的匹配，在KD-tree中寻找最近点，记为/>然后继续搜索另外两个最近点，其中一个和/>来自同一次的Scan扫描，另外一个则是在/>相邻的前后两次Scan扫描中寻找/>的最近点，分别标记为/>和/>找到三个点组成的局部平面/>作为特征角点的匹配；

特征平面点和对应局部平面匹配之间的距离为：

t是激光雷达当前的时刻，t_k+1是第k+1次3D扫描的开始时间，是激光雷达在时间[t_k+1，t]内的位姿变换矩阵，并且：

式中，t_x，t_y，t_z表示激光雷达在雷达坐标系中的平移量，θ_x，θ_y，θ_z表示激光雷达在雷达坐标系中的旋转角度；对于Ω_k+1中的任意一个点云点i，i∈Ω_k+1，t_i是其时间戳，在时间位姿变换矩阵[t_k+1，t_i]，根据/>计算：

建立E_k+1和以及H_k+1和/>之间的几何关系，/>是E_k+1或者H_k+1中的点云点，/>是/>或者/>中的点云点，利用上面的变换关系有：

以及E_k+1和其匹配平面之间的几何关系：

式中，每一行的f对应一个特征点，d是和相应匹配之间的距离，计算关于的雅可比矩阵，标记为J：

然后进行非线性迭代，使得d趋近于零，即：

式中，λ是一个系数；

进一步包括：算法的输入包括前一帧的重映射点云数据最近一次的位姿变换矩阵以及点云数量逐渐增加的当前帧点云数据Ω_k+1；如果新的Sweep扫描开始，/>会被重新置零；将从Ω_k+1中提取特征点，形成特征点集合E_k+1和H_k+1；对每个特征点，在/>中寻找它的匹配点；对每次迭代求解出的位姿变换矩阵进行了更新；随着Ω_k+1中的数据不断增加继续迭代，一直到该帧结束，通过最终的变换矩阵Ω_k+1被重映射到下一帧的初始时刻，继续新的运动估计：

所述步骤四中SLAM的优化问题最终都转变成为非线性最小二乘的求解：

最小二乘问题：

首先对F(x)进行一阶泰勒展开：

F(x+△x)≈F(x)+J(x)△x；

通过寻找Δx使得目标函数完成最小化的任务，展开目标函数：

对上式求Δx的导数，并且使其等于零，得到方程：

J(x)^TJ(x)Δx＝-J(x)^TF(x)；

H△x＝g；

2.一种实施权利要求1所述基于激光雷达的室内无人机定位方法的基于激光雷达的室内无人机定位系统，其特征在于，所述基于激光雷达的室内无人机定位系统包括：

激光雷达，用于采集飞行过程的相关数据；

机载计算机模块，用于完成定位算法的配置和运行；

3.如权利要求2所述的基于激光雷达的室内无人机定位系统，其特征在于，所述机载计算机模块包括激光里程计、回环检测以及后端优化；生成无人机位姿；通过和飞行控制模块的接口读取INS信息，用于校正激光雷达的数据采集过程中的运动畸变，根据控制需求生成相应的指令发送给飞行控制模块，实现最终的无人机控制任务。

4.如权利要求2所述的基于激光雷达的室内无人机定位系统，其特征在于，所述基于激光雷达的室内无人机定位系统的其他辅助系统包括地面站系统、遥控器系统。

5.一种应用权利要求1所述基于激光雷达的室内无人机定位方法的无人机。