【技术实现步骤摘要】
本专利技术涉及定位跟踪，具体为基于时变因子重定位的uwb-lidar-imu组合定位方法。
技术介绍
1、近年来，随着智能驾驶技术的迅速发展，无人驾驶汽车的导航模块对于高精度车辆定位的需求日益增大，高精度的车辆定位决定了导航模块的可靠性，是未来发展无人驾驶技术的重中之重。目前，车辆定位一般由全球卫星导航系统(global navigationsatellite system，gnss)提供，gnss无论在山区还是城市等环境下，都能够提供厘米级别精度的定位，但是，在密闭空间和信号遮挡严重的环境下无法进行高精度定位。
2、当gnss无法提供定位时，仅依靠车辆搭载的单一传感器无法提供较为精准的车辆定位。目前，在gnss拒止环境下的车库、作业车间等一般使用超宽带(ultra wide band，uwb)对车辆进行定位。uwb可实现分米级精度的定位，特别是在无遮挡的视距环境下，uwb定位精度可达厘米级，现已广泛应用于医院患者跟踪、无人机定位、超市购物等场景。然而，在非视距环境下，uwb在信号传输过程中容易产生多径效应、信号强度衰减以及信号损失等情况，使得车辆定位精度大幅下降。
技术实现思路
1、(一)解决的技术问题
2、针对现有技术的不足，本专利技术提供了基于时变因子重定位的uwb-lidar-imu组合定位方法，具备有效提高了车辆定位精度等优点，解决了上述技术问题。
3、(二)技术方案
4、为实现上述目的，本专利技术提供如下技术方案：基于时变
5、s1、在视距非视距环境下的矩形定位区域布置4个uwb基站，获取基站和标签之间的测距值，并解算出车辆定位；
6、s2、在uwb定位环境内利用ndt算法对lidar点云数据进行解算，并将lidar定位点坐标转换为uwb坐标系下坐标；
7、s3、利用uwb和lidar定位坐标在限定定位区域内对车辆进行一次融合定位；
8、s4、计算得到车辆最优二次定位点；
9、s5、将得到的二次定位点作为时变因子的求解条件，求解得到车辆重定位后的定位坐标；
10、s6、将求解得到的车辆重定位坐标作为交互式多模型-无迹卡尔曼滤波的量测信息，imu测量值作为状态输入，对车辆进行融合定位。
11、作为本专利技术的优选技术方案，所述步骤s3包括以下步骤：
12、s3.1、对一次定位点的定位区域进行限定；
13、s3.2、对一次定位点进行计算。
14、作为本专利技术的优选技术方案，所述步骤s3.1中定位区域的划定方法如下：
15、在k时刻时，以uwb和lidar初始定位点为圆心，并以uwb定位区域半径ru和lidar的定位区域半径rl的圆形区域作为两传感器的定位区域，并在定位区域中分别生成n个随机点作为一次定位点的解算条件。
16、作为本专利技术的优选技术方案，所述步骤s3.2将每个随机点坐标与lidar传感器的当前定位点之间的欧式距离由大到小的顺序重新排列并存储，求一次定位点坐标的公式如下：
17、
18、其中，表示第n个随机粒子在k时刻的坐标，表示一次定位点坐标，表示随机点与lidar定位点的欧式距离残差归一化函数，表示对内部2n个数据进行求和计算，的计算公式如下：
19、
20、其中，表示第2n-n+1个随机粒子在k时刻相对于lidar定位点的残差大小，表示第n个随机粒子在k时刻相对于lidar定位点的残差大小，具体表达式如下：
21、
22、其中，表示lidar定位点坐标，||*||表示二范数符号。
23、作为本专利技术的优选技术方案，所述步骤s4包括以下步骤：
24、s4.1、对二次定位点定位区域进行划定；
25、s4.2、在二次定位区域内利用粒子群优化算法根据定义函数搜索得到最优二次定位点。
26、作为本专利技术的优选技术方案，所述步骤s4.1的划定方法如下：
27、k时刻，以初始lidar定位点和uwb定位点为圆心，rl和ru为半径的圆形区域分别作为lidar和uwb的定位区域，若两传感器定位区域相交或包含，则以两传感器定位区域的重叠部分作为二次定位点的定位区域，反之，则以lidar定位区域作为二次定位点的定位区域。
28、作为本专利技术的优选技术方案，所述步骤s4.2中的定义函数表达式如下：
29、
30、其中，表示定位区域内k时刻的第t个二次定位点，代表k时刻一次定位点与定位区域内第t个二次定位点的欧式距离，计算得出的k时刻最佳二次定位点记为
31、作为本专利技术的优选技术方案，所述步骤s5定位步骤如下：
32、s5.1、将最优二次定位点作为当前时刻的实际定位点求得与lidar和uwb初步定位点之间的欧式距离残差和计算公式如下：
33、
34、
35、其中，表示最佳二次定位点，表示初始lidar定位点，表示uwb定位点，之后根据和的值对时变因子和进行计算，计算公式如下：
36、
37、
38、其中，和分别为lidar和uwb在k时刻的时变因子；
39、s5.2、根据求解得到的传感器时变因子对传感器进行重定位，计算公式如下：
40、
41、其中，表示k时刻时重定位后的车辆定位坐标。
42、作为本专利技术的优选技术方案，所述步骤s6中根据步骤s5的重定位后的车辆定位坐标作为imm的量测信息，imu测量数据作为状态输入，引入匀速、匀加速和恒定转向速度三种运动模型,作为各滤波器滤波时对应的运动模型，利用ukf作为交互式多模型的滤波器。
43、作为本专利技术的优选技术方案，所述粒子群优化算法的最佳适应度函数为
44、与现有技术相比，本专利技术提供了基于时变因子重定位的uwb-lidar-imu组合定位方法，具备以下有益效果：
45、本专利技术通过基于时变因子的uwb和lidar重定位模块和基于imm_ukf的uwb/lidar/imu融合定位两个模块，其中，基于时变因子的uwb和lidar重定位模块根据lidar在复杂非视距环境下定位精度较高，uwb在点云特征较少的视距环境下定位精度较高的互补特性，在对uwb和lidar进行一次定位和利用粒子群算法进行寻优得到二次定位后引入时变因子对车辆进行重定位，为后续使用交互式多模型方法进行定位解算提供了较为可靠的量测信息，提高了车辆定位对环境的适应性，基于imm_ukf的uwb/lidar/imu融合定位模块使用车辆重定位作为交互式多模型的量测信息，imu测量值作为状态输入，引入匀速、匀加速、恒定速度转向模型作为运动模型以描述车辆的运动状态，利用无迹卡尔曼滤波作为交互式多模型的滤波器，有利于对非线性运动模型的滤波，本专利技术本文档来自技高网...
【技术保护点】
1.基于时变因子重定位的UWB-LiDAR-IMU组合定位方法，其特征在于：包括以下步骤：
2.根据权利要求1所述的基于时变因子重定位的UWB-LiDAR-IMU组合定位方法，其特征在于：所述步骤S3包括以下步骤：
3.根据权利要求2所述的基于时变因子重定位的UWB-LiDAR-IMU组合定位方法，其特征在于：所述步骤S3.1中定位区域的划定方法如下：
4.根据权利要求3所述的基于时变因子重定位的UWB-LiDAR-IMU组合定位方法，其特征在于：所述步骤S3.2中将每个随机点坐标与LiDAR传感器的当前定位点之间的欧式距离由大到小的顺序重新排列并存储，求一次定位点坐标的公式如下：
5.根据权利要求1所述的基于时变因子重定位的UWB-LiDAR-IMU组合定位方法，其特征在于：所述步骤S4包括以下步骤：
6.根据权利要求5所述的基于时变因子重定位的UWB-LiDAR-IMU组合定位方法，其特征在于：所述步骤S4.1的划定方法如下：
7.根据权利要求6所述的基于时变因子重定位的UWB-LiDAR-IMU组合定位
8.根据权利要求1所述的基于时变因子重定位的UWB-LiDAR-IMU组合定位方法，其特征在于：所述步骤S5定位步骤如下：
9.根据权利要求8所述的基于时变因子重定位的UWB-LiDAR-IMU组合定位方法，其特征在于：所述步骤S6中根据步骤S5的重定位后的车辆定位坐标作为IMM的量测信息，IMU测量数据作为状态输入，引入匀速、匀加速和恒定转向速度三种运动模型,作为各滤波器滤波时对应的运动模型，利用UKF作为交互式多模型的滤波器。
10.根据权利要求5所述的基于时变因子重定位的UWB-LiDAR-IMU组合定位方法，其特征在于：所述粒子群优化算法的最佳适应度函数为
...【技术特征摘要】
1.基于时变因子重定位的uwb-lidar-imu组合定位方法，其特征在于：包括以下步骤：
2.根据权利要求1所述的基于时变因子重定位的uwb-lidar-imu组合定位方法，其特征在于：所述步骤s3包括以下步骤：
3.根据权利要求2所述的基于时变因子重定位的uwb-lidar-imu组合定位方法，其特征在于：所述步骤s3.1中定位区域的划定方法如下：
4.根据权利要求3所述的基于时变因子重定位的uwb-lidar-imu组合定位方法，其特征在于：所述步骤s3.2中将每个随机点坐标与lidar传感器的当前定位点之间的欧式距离由大到小的顺序重新排列并存储，求一次定位点坐标的公式如下：
5.根据权利要求1所述的基于时变因子重定位的uwb-lidar-imu组合定位方法，其特征在于：所述步骤s4包括以下步骤：
6.根据权利要求5所述的基于时变因子重定位的uwb-li...
【专利技术属性】
技术研发人员：杨秀建，敖鹏，肖海承，肖仁鑫，雷基林，郭兴瑞，冯楚琦，
申请(专利权)人：昆明理工大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。