买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：东南大学

摘要：本发明公开了一种GNSS拒止环境下UWB‑激光雷达LiDAR‑惯导INS融合定位方法，包括：首先对UWB的测距信息进行非视距误差的剔除，LiDAR点云进行原始观测运动畸变的补偿；UWB利用扩展卡尔曼滤波算法实现位置解算，计算初始位置和姿态，完成INS系统的初始化；UWB定位的同时需经过精度因子DOP、基站数量，观测残差进行质量判断，剔除误差较大的定位点；INS系统采用机械编排算法，实现高频位姿解算的同时，将结果作为先验用于LiDAR的动态匹配过程；后端构造维护一个轻量化的因子图，将激光雷达里程计的观测值、IMU预积分、UWB绝对观测进行融合以减少累积误差。本发明能为GNSS拒止环境下的实时定位问题提供一种可靠的解决方案，并能保持分米级的定位精度。

主权项：1.一种GNSS拒止环境下UWB-激光雷达-惯导融合定位方法，其特征在于，所述方法包括以下步骤：步骤1、首先对UWB的测距信息进行非视距误差的剔除，LiDAR点云进行原始观测运动畸变的补偿；步骤2、UWB利用扩展卡尔曼滤波算法实现位置解算，计算初始位置和姿态，完成INS系统的初始化；步骤3、UWB定位同时需经过精度因子DOP、基站数量，观测残差进行质量判断，剔除误差较大的定位点；步骤4、INS系统采用的是机械编排算法，实现高频位姿解算的同时，将结果作为先验用于LiDAR的动态匹配过程；步骤5、后端构造维护一个轻量化的因子图，将激光雷达里程计的观测值、IMU预积分、UWB绝对观测进行融合以减少累积误差，同时动态维护特征地图，其中，步骤1中UWB定位和非视距误差剔除，LiDAR点云运动补偿按下式获得： 式1中，Ak为状态转移矩阵，为其对应转置矩阵，为上一时刻的观测量估计，为状态方程的预测状态量，代表状态方程的预测协方差，∑Q为过程噪声，Kk、R、Hk分别是卡尔曼增益、观测方程的观测噪声矩阵和状态观测矩阵，为状态观测矩阵的转置矩阵，Zk为观测向量，residual为观测和预测的残差，当残差大于设定的阈值，threshold为0.35，将此观测视为非视距误差，令卡尔曼增益为0实现剔除，LiDAR点云运动补偿公式按下式： 式中为INS测量的相对帧间变换，为LiDAR和INS的外参矩阵，为当前帧下的需要补偿激光点，为把当前点补偿到最后一点的坐标表示形式；其中，步骤3中UWB定位同时需经过精度因子DOP、基站数量，观测残差进行质量判断，剔除误差较大的定位点，按下式： 式中，COV为由观测矩阵计算得到的观测协方差矩阵，PDOP为测量的误差精度因子，tr表示求迹运算；当PDOP值最小时，对应的基站几何布局最优；以此当UWB基站质量差时，可根据PDOP的值进行剔除，阈值设为3.0；引入基站数量和平均残差作为质量控制的判断，当基站数量少于4个，此时UWB定位结果不可用，在去除非视觉误差后，基站的平均值小于阈值，设置为0.15，定位有效；其中，步骤5中后端构造维护一个轻量化的因子图，将激光雷达里程计的观测值、IMU预积分、UWB绝对观测进行融合以减少累积误差，同时动态维护特征地图，优化求解方程如下： 式中，min表示优化中的最小化问题，为雷达相邻两帧变换的雷达里程计因子，为INS系统中的IMU预积分约束，RUx为UWB全局约束。

全文数据：

权利要求：

百度查询： 东南大学 一种GNSS拒止环境下的UWB-激光雷达-惯导融合定位方法