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

专利主权项内容

1.一种基于激光雷达的室内无人机定位方法，其特征在于，所述基于激光雷达的室内无人机定位方法利用INS消除激光雷达在采集点云过程中的快速运动畸变；完成激光雷达采集到的相邻点云之间的位姿估计、检测无人机是否经过已经走过的地方、对之前的各种估计进行全局优化；通过传感器数据获得精确的无人机位姿和环境的位置信息，根据控制需求完成无人机控制命令的生成；所述基于激光雷达的室内无人机定位方法包括以下步骤：步骤一，从INS系统中读取无人机的加速度、角速度数据，通过航迹推算计算无人机实时位姿、速度以及位移，以备点云数据的校准；步骤二，激光雷达每一采集时刻以较高分辨率完成平面扫描，记为一次Scan扫描；然后同步INS数据完成数据融合，消除每一Scan扫描的运动畸变，利用激光里程计算法进行特征提取；步骤三，激光雷达在电机的控制下绕垂直于扫描平面进行360°旋转，其分辨率低于平面扫描，旋转一周完成一次三维空间的扫描，记为一次Sweep扫描；当完成一次Sweep扫描后开始利用激光里程计算法进行和下一次的Sweep扫描的特征匹配，建立位姿估计方程并进行求解，获得无人机位姿的粗略估计，构建位姿图模型，完成前端算法；步骤四，根据构建的位姿图模型进行非线性优化，利用优化完成后端算法求解，并且输出地图模型；如果检测到回环，利用回环算法进行位姿约束方程的构建，将约束以边的形式添加到位姿图模型中，根据优化结果进行导航避障等高级应用；所述步骤三中特征匹配需要连续两帧的点云数据，t表示第k次Sweep扫描的开始时间，在第k次Sweep扫描结束后，将所获得的点云集合Ω重映射到下一次扫描的开始时间t，标记为并且利用三维空间的KD-tree存储，然后利用/>去和第k+1次扫描采集到的点做匹配；kkk+1在第k+1次扫描中，利用上节介绍的方法计算点云的曲率，获得特征角点集合和特征平面点集合，分别记为E和H；采用中的边线作为特征角点的匹配，采用/>中的局部平面作为特征平面点的匹配，其中，边线需要在/>寻找两个最近点来构建，而局部平面则需要三个最近点表示；k+1k+1在第k+1次Sweep扫描的开始时间t，采集到的点云数据集Ω是空的，每次匹配时，都需要将E和H重映射到初始时刻，标记为和/>中的第i个点为/>通过KD-tree搜索，在/>中寻找其最近点，记为/>然后在/>相邻的前后两次Scan扫描中寻找/>的最近点，记为/>注意该点和/>不属于同一次Scan扫描；k+1k+1k+1k+1同样，中的第i个点为/>在/>中搜寻3个点形成局部平面，作为特征平面点的匹配，在KD-tree中寻找最近点，记为/>然后继续搜索另外两个最近点，其中一个和/>来自同一次的Scan扫描，另外一个则是在/>相邻的前后两次Scan扫描中寻找/>的最近点，分别标记为/>和/>找到三个点组成的局部平面/>作为特征角点的匹配；计算特征点和它的匹配之间的距离，通过最小化全部距离来估计激光雷达的运动，特征角点和对应边线匹配之间的距离为：特征平面点和对应局部平面匹配之间的距离为：t是激光雷达当前的时刻，t是第k+1次3D扫描的开始时间，是激光雷达在时间[t，t]内的位姿变换矩阵，并且：k+1k+1式中，t，t，t表示激光雷达在雷达坐标系中的平移量，θ，θ，θ表示激光雷达在雷达坐标系中的旋转角度；对于Ω中的任意一个点云点i，i∈Ω，t是其时间戳，在时间位姿变换矩阵[t，t]，根据/>计算：xyzxyzk+1k+1ik+1i建立E和以及H和/>之间的几何关系，/>是E或者H中的点云点，/>是/>或者/>中的点云点，利用上面的变换关系有：k+1k+1k+1k+1
和/>中的点与其上一帧中的匹配之间的距离d和d，建立E和其匹配边线之间的几何关系：EHk+1以及E和其匹配平面之间的几何关系：k+1对每一个特征点进行处理之后，获得表示两帧之间几何关系的非线性方程：式中，每一行的f对应一个特征点，d是和相应匹配之间的距离，计算关于的雅可比矩阵，标记为J：然后进行非线性迭代，使得d趋近于零，即：式中，λ是一个系数；进一步包括：算法的输入包括前一帧的重映射点云数据最近一次的位姿变换矩阵以及点云数量逐渐增加的当前帧点云数据Ω；如果新的Sweep扫描开始，/>会被重新置零；将从Ω中提取特征点，形成特征点集合E和H；对每个特征点，在/>中寻找它的匹配点；对每次迭代求解出的位姿变换矩阵进行了更新；随着Ω中的数据不断增加继续迭代，一直到该帧结束，通过最终的变换矩阵Ω被重映射到下一帧的初始时刻，继续新的运动估计：k+1k+1k+1k+1k+1k+1所述步骤四中SLAM的优化问题最终都转变成为非线性最小二乘的求解：最小二乘问题：首先对F(x)进行一阶泰勒展开：F(x+△x)≈F(x)+J(x)△x；式中，J(x)表示雅可比矩阵，通过一阶近似线性化，目标函数变为：通过寻找Δx使得目标函数完成最小化的任务，展开目标函数：对上式求Δx的导数，并且使其等于零，得到方程：J(x)J(x)Δx＝-J(x)F(x)；TT关于Δx的线性方程，左侧的系数定义为H，右边定义为g，则：H△x＝g；求解该方程获得增量Δx，利用x＝x+Δx进行更新优化变量，迭代求解最小二乘问题，直到满足终止条件便可得到位姿估计的最优状态。*