本发明涉及机器人定位，具体涉及一种基于误差扩展卡尔曼滤波器的多源融合定位方法及系统。
背景技术：
1、移动机器人定位问题最早诞生于上世纪60年代，定位算法的基本原理是通过安装在机器人上的内部传感器或者外部传感器感知机器人自身状态以及外部环境，根据一定方法对传感器数据进行处理从而得到机器人在地图中的位姿估计。自主定位技术是移动机器人的核心技术，准确、可靠的定位是机器人实现避障、路径规划以及运动控制等复杂任务的基础和前提。
2、卡尔曼滤波(kalman filter，kf)是一套由数字计算机实现的实时递推算法，它以随机系统的量测作为滤波器的输入，滤波器的输出是对系统状态的最优估计。在卡尔曼滤波中，所有利用的信息都是时域内的参量，它不但可以应用于一维平稳的随机过程，还可应用于多维非平稳过程，这就避免了维纳滤波器设计的困境。
3、一些技术中，仅使用激光雷达感知周围环境，并和已输入的地图进行匹配来获取自身位姿，这种技术无法保证机器人定位的鲁棒性，在一些相似场景(如长走廊等)以及环境变化场景(有行人等)会无法定位；另一些技术中，仅使用轮速计和惯性测量单元(inertial measurement unit，imu)进行组合导航，这种技术由于imu的随时间累计误差的特性而无法保证长时间的准确定位，即使有轮速计提供速度和位置的约束，但由于存在打滑等因素使其并不能完全消除累积误差，从而无法保证长时间情况下的定位精度。
4、经典卡尔曼滤波是基于现行系统的估计方法，一般只能适用于线性或者非常接近于线性的非线性问题，对于非线性比较明显的问题，卡尔曼滤波往往不能给出满意的结果，需要采用非线性估计方法。最为广泛使用的非线性估计方法就是扩展卡尔曼滤波(extended kalman filter，ekf)，它通过泰勒级数展开，对非线性函数进行线性化近似。
技术实现思路
1、为了解决上述问题，本发明提供一种基于误差扩展卡尔曼滤波器的多源融合定位方法及系统，能够有效解决现有技术中的不足。
2、本发明是通过以下技术方案来实现的：一种基于误差状态扩展卡尔曼滤波器的多源融合定位方法，该方法包括以下步骤：
3、s1、标定激光雷达、惯性测量单元imu和轮式里程计与机器人定位原点的相对位置；
4、s2、设定各传感器数据在融合过程中的权重占比；
5、s3、使用激光雷达数据进行全局搜索以获取初始位姿估计，或采用其他方式赋予初始值；
6、s4、读取imu数据并应用误差状态扩展卡尔曼滤波器进行位置预测；
7、s5、利用轮式里程计数据，通过误差状态扩展卡尔曼滤波器进行位置更新；
8、s6、利用激光雷达数据结合先前的位置估计结果，通过迭代最近点匹配或其他匹配方法进行位置更新，此过程结合视觉定位、特征定位、超宽带定位技术；
9、其中，步骤s3、s4、s5的执行顺序不固定，因应不同传感器数据的频率而调整。
10、作为优选的技术方案，所述激光雷达为多线激光雷达或单线激光雷达，用于提供机器人当前环境的局部区域的点云数据。
11、作为优选的技术方案，其中，所述imu包括三个单轴加速度计和三个单轴陀螺仪，用于测量机器人的三轴加速度和三轴角速度，并用于确定机器人的姿态。
12、作为优选的技术方案，所述轮式里程计为基于光电编码器的传感器，用于测量轮子转速，并通过脉冲信号转换为电压信号，用于计算行驶速度和行驶距离。
13、作为优选的技术方案，所述误差状态扩展卡尔曼滤波器用于优化机器人的全局位置、速度和姿态的估计，该滤波器通过最小化参数表达，使用三维变量表达旋转的增量，降低计算量并提升计算效率。
14、本发明的一种多源融合定位系统，包括：
15、激光雷达：配置于机器人上，用于向环境发射激光波束，并接收反射波束以生成机器人当前环境的点云数据，该激光雷达可为多线激光雷达或单线激光雷达；
16、惯性测量单元：集成于机器人系统中，包含用于测量机器人三轴加速度和三轴角速度的三个单轴加速度计和三个单轴陀螺仪，用于实时提供机器人的姿态数据；
17、轮式里程计：安装在机器人的轮子上，包括用于测量轮子转速的光电编码器传感器，以及将脉冲信号转换为电压信号的电路，用于计算机器人的行驶速度和行驶距离，从而辅助定位；
18、处理单元：设有用于执行误差状态扩展卡尔曼滤波器的算法和软件，负责综合处理来自激光雷达、imu和轮式里程计的数据，用于优化机器人的全局位置、速度和姿态估计；
19、数据融合接口：用于将来自上述激光雷达、imu和轮式里程计的数据汇集到处理单元，并支持额外的定位模块的数据整合，以提高定位的精度和系统的鲁棒性。
20、作为优选的技术方案，还包括一个或多个用于提供额外定位信息的模块，选自视觉定位模块、特征定位模块、超宽带定位模块，或其组合。
21、作为优选的技术方案，激光雷达、imu和轮式里程计均与处理单元相连，处理单元配置有用于执行误差状态扩展卡尔曼滤波器的软件，用于处理来自各传感器的数据，以提供精确的机器人定位和姿态估计。
22、作为优选的技术方案，所述处理单元进一步配置有用于接收和处理来自视觉定位、特征定位或超宽带定位模块的数据的接口和算法，以实现多源数据融合和提高定位精度。
23、作为优选的技术方案，所述处理单元包含一个用于动态调整各传感器数据在融合过程中权重占比的算法，以根据实时环境和传感器性能优化定位结果；
24、所述激光雷达、imu和轮式里程计的数据通过有线或无线方式传输至处理单元。
25、本发明的有益效果是：本发明通过融合激光雷达、imu和轮式里程计的数据，本系统能够在多种环境下提供更为精确和稳定的定位结果，特别是在激光雷达视线受限或imu数据不稳定的情况下，多源数据融合能够互补各自的不足，确保定位的连续性和准确性；
26、本系统不完全依赖于外部环境特征，如激光雷达点云匹配依赖的特征丰富的环境，即使在特征匮乏或动态变化的环境中，系统仍能通过内部传感器数据实现有效定位；
27、本发明采用误差状态扩展卡尔曼滤波器(esekf)，本系统在进行状态估计时，能够有效减少计算量，esekf使用三维变量表达旋转增量，较传统四元数或旋转矩阵方法更为高效，尤其适合实时或计算资源受限的应用场景；
28、在面对各种传感器噪声和误差时，本系统的多源数据融合策略能有效抑制单一传感器的误差影响，提高系统的容错能力，确保定位的鲁棒性；
29、本系统适用于各种类型的机器人和移动装置，如自动引导车(agv)、无人机(uav)、服务机器人等，在室内外、地面和空中等多种环境中均表现出良好的适应性和性能。
1.一种基于误差状态扩展卡尔曼滤波器的多源融合定位方法，该方法包括以下步骤：
2.根据权利要求1所述的基于误差状态扩展卡尔曼滤波器的多源融合定位方法，其特征在于：所述激光雷达为多线激光雷达或单线激光雷达，用于提供机器人当前环境的局部区域的点云数据。
3.根据权利要求1所述的基于误差状态扩展卡尔曼滤波器的多源融合定位方法，其特征在于：其中，所述imu包括三个单轴加速度计和三个单轴陀螺仪，用于测量机器人的三轴加速度和三轴角速度，并用于确定机器人的姿态。
4.根据权利要求1所述的基于误差状态扩展卡尔曼滤波器的多源融合定位方法，其特征在于：所述轮式里程计为基于光电编码器的传感器，用于测量轮子转速，并通过脉冲信号转换为电压信号，用于计算行驶速度和行驶距离。
5.根据权利要求1所述的基于误差状态扩展卡尔曼滤波器的多源融合定位方法，其特征在于：所述误差状态扩展卡尔曼滤波器用于优化机器人的全局位置、速度和姿态的估计，该滤波器通过最小化参数表达，使用三维变量表达旋转的增量，降低计算量并提升计算效率。
6.一种基于权利要求1至5任一权利要求所述方法的多源融合定位系统，其特征在于，包括：
7.根据权利要求6所述的多源融合定位系统，其特征在于：还包括一个或多个用于提供额外定位信息的模块，选自视觉定位模块、特征定位模块、超宽带定位模块，或其组合。
8.根据权利要求6所述的多源融合定位系统，其特征在于：激光雷达、imu和轮式里程计均与处理单元相连，处理单元配置有用于执行误差状态扩展卡尔曼滤波器的软件，用于处理来自各传感器的数据，以提供精确的机器人定位和姿态估计。
9.根据权利要求6所述的多源融合定位系统，其特征在于：所述处理单元进一步配置有用于接收和处理来自视觉定位、特征定位或超宽带定位模块的数据的接口和算法，以实现多源数据融合和提高定位精度。
10.根据权利要求6所述的多源融合定位系统，其特征在于：所述处理单元包含一个用于动态调整各传感器数据在融合过程中权重占比的算法，以根据实时环境和传感器性能优化定位结果；
