一种基于ekf的松耦合多传感器融合定位方法及系统
阅读说明：本技术 一种基于ekf的松耦合多传感器融合定位方法及系统 (EKF-based loosely-coupled multi-sensor fusion positioning method and system ) 是由 梁婉琳 关伟鹏 伍文飞 于 2021-08-23 设计创作，主要内容包括：本发明公开了一种基于EKF的松耦合多传感器融合定位方法及系统,包括：基于SLO-VLP获得移动终端的SLO-VLP位姿,将SLO-VLP位姿作为自适应蒙特卡罗定位算法粒子的初始化位姿,基于初始化位姿获得AMCL位姿；将SLO-VLP位姿和AMCL位姿作为EKF算法的输入值,获得移动终端的位姿值；系统包括RSE照相机、激光雷达和里程计。本发明通过里程计的状态预测和激光雷达扫描仪和VLP的测量更新,可以同时将可观测的LED数量的要求放宽到零,实现高精度定位。本发明使移动终端能够最大限度地提高其对环境的感知意识,并获得足够的测量值,为室内定位及导航提供了技术参考。(The invention discloses an EKF-based loosely-coupled multi-sensor fusion positioning method and system, which comprises the following steps: the SLO-VLP pose of the mobile terminal is obtained based on the SLO-VLP, the SLO-VLP pose is used as an initialization pose of a self-adaptive Monte Carlo positioning algorithm particle, and an AMCL pose is obtained based on the initialization pose; the SLO-VLP pose and the AMCL pose are used as input values of an EKF algorithm to obtain a pose value of the mobile terminal; the system includes an RSE camera, a lidar, and a odometer. The invention can simultaneously relax the requirement of the number of observable LEDs to zero by predicting the state of the odometer and updating the measurement of the laser radar scanner and the VLP, thereby realizing high-precision positioning. The invention enables the mobile terminal to improve the perception consciousness of the mobile terminal to the environment to the maximum extent, obtains enough measured values and provides technical reference for indoor positioning and navigation.)
技术领域
本发明属于智能设备定位领域，特别是涉及一种基于EKF的松耦合多传感器融合定位方法及系统。
背景技术
可见光通信技术(简称VLC)以其精度高、成本低、易于实现的优点极大填补了商业化室内定位的刚需。同时，由于可见光通信技术不会产生任何的射频干扰，可见光定位算法在射频辐射被严格限制的环境中(如医院、核电站等)同样有良好的表现。可见光定位技术(简称VLP)是通过对LED进行调制，使各个LED以不同频率明暗闪烁并通过空气传播其位置信息。移动终端的接收装置对VLP信息进行捕获，通过图像处理技术对捕获的图片进行处理，解调出LED的位置信息，再利用几何学等原理，计算出移动终端的位置信息。
基于相机的VLP系统采用安装在已知位置(例如天花板上)的调制LED灯作为人工地标，将每个LED通过基于RSE的OCC测量与ID坐标相关联。若要减少VLP中所需的指示灯的数量，则不再将捕获的LED图像作为点，而是作为利用几何特征确定接收器方向和位置的图像。此方法需要在LED上放置一个附加的标记。
SLAM定位技术对于在未知环境中构建和更新地图很有用，而移动终端则维护有关其位置的信息。它包括同时构建的环境模型(地图)和在其内部移动的终端的姿态的估计。SLAM可根据传感器分为两类：基于视觉和基于激光雷达。与其他距离测量装置相比，激光雷达传感器以较大的扫描角和高角分辨率进行其周围的测量。此外，它对照明是不变的。高可靠性和精度使激光雷达传感器成为姿态估计的流行选择。
发明内容
针对现有的基于激光雷达传感器的SLAM定位技术在没有相对准确的手动初始位姿的情况下不能精确引导定位以及从激光雷达获取的特征十分有限的问题，本发明提供了如下方案：一种基于EKF的松耦合多传感器融合定位方法,包括：
基于SLO-VLP获得移动终端的SLO-VLP位姿，将所述SLO-VLP位姿作为自适应蒙特卡罗定位算法粒子的初始化位姿，基于所述初始化位姿，获得AMCL位姿；
将所述SLO-VLP位姿和所述AMCL位姿作为EKF算法的输入值，获得移动终端的位姿值。
优选地，基于所述SLO-VLP获得所述移动终端的SLO-VLP位姿包括：
获得LED灯体和所述移动终端的相机图像中心在世界坐标系中的位置；
根据所述移动终端的相机坐标系与所述移动终端的base_link坐标系的第一变换关系获得所述移动终端的姿态；
通过所述移动终端的里程计传感器获取偏航角，通过激光雷达匹配器获得所述姿态的第二变换关系，根据所述第二变换关系对所述偏航角进行修正，获得所述移动终端的SLO-VLP位姿。
优选地，获得所述LED灯体和所述移动终端的相机图像中心在世界坐标系中的位置包括：
获取所述LED灯体的感光区域，通过所述感光区域进行身份识别，构建LED地标图，所述LED灯体在所述LED地标图中的位置为所述LED灯体在世界坐标系中的位置；
基于所述LED地标图中的位置获得所述LED灯体在所述相机坐标系的位置即为所述移动终端的相机图像中心在世界坐标系中的位置。
优选地，基于所述LED地标图中的位置获得所述LED灯体在所述相机坐标系的位置具体通过计算所述LED灯体在z轴上的高度差获得；
所述高度差通过LED像平面的直径和LED灯体直径的比例关系、三角形性质及成像原理获得。
优选地，获得所述AMCL位姿包括：
将所述SLO-VLP位姿作为AMCL滤波器粒子的初始化位姿，将里程计传感器获取的位姿信息和激光雷达数据输入到所述AMCL滤波器，输出获得所述AMCL位姿。
优选地，获得所述移动终端的位姿值包括：
通过移动终端的里程计传感器获得位姿估计值；
基于所述SLO-VLP位姿和所述AMCL位姿，获得位姿观测值，通过卡尔曼滤波器对所述位姿估计值进行校正，获得所述移动终端的位姿值。
优选地，通过所述卡尔曼滤波器对所述位姿估计值进行校正，获得所述移动终端的位姿值还包括：
计算卡尔曼增益，根据所述卡尔曼增益更新位姿向量和协方差矩阵。
一种基于EKF的松耦合多传感器融合定位系统，包括，
RSE照相机、激光雷达、里程计；
所述RSE照相机用于基于SLO-VLP获得移动终端的SLO-VLP位姿；
所述激光雷达用于将所述SLO-VLP位姿作为自适应蒙特卡罗定位算法粒子的初始化位姿，获得AMCL位姿；
所述里程计用于将所述SLO-VLP位姿和所述AMCL位姿作为EKF算法的输入值，获得移动终端的位姿值。
优选地，所述里程计还用于获取偏航角；
所述激光雷达还用于获得移动终端姿态的第二变换关系，根据所述第二变换关系对所述偏航角进行修正。
本发明公开了以下技术效果：
本发明通过里程计的状态预测和激光雷达扫描仪和VLP的测量更新，可以同时将可观测的LED数量的要求放宽到零，实现高精度定位。本发明使移动终端能够最大限度地提高其对环境的感知意识，并获得足够的测量值，为室内定位及导航提供了技术参考。
本发明通过为激光SLAM技术提供比较可靠的姿态观测值，可以校正里程计、激光雷达等传感器的累积误差，提供更为精准的位姿，增强算法的鲁棒性。
本发明通过多传感器的融合，可以弥补独立传感器的不足，并提供更可靠的姿态估计。在LED短缺/故障时为终端提供准确而可靠的定位，算法的鲁棒性使移动终端不仅适用于照明设施完善的场合，在信源短缺的情况下也能提供高精度的定位服务，为路径规划和自主导航打下坚实基础。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例的方法流程图；
图2为本发明实施例的世界、相机和图像坐标系之间的变换示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
如图1所示，本发明提供了一种基于EKF的松耦合多传感器融合定位方法及系统,包括：
步骤S1：获取从SLO-VLP计算得到的移动终端观测位姿(Xt,Yt,Zt，θt)，而其方向角θt通过激光雷达匹配器(AMCL)校正。
步骤S11：获取某一LED灯体在世界坐标系中的位置(Xi，Yi,Zi)，并计算终端相机图像中心在世界坐标系中的位置P(Xs，Ys,Zs)。其方法包括：
步骤S111：获取该LED灯体的感光区域ROI并识别其身份(LED-ID)，并从注册的灯光数据库(预制的LED地标地图)检索LED的3D位置PLED(Xi，Yi,Zi)。
步骤S112：由步骤S11获取的PLED(Xi，Yi,Zi)计算该LED灯体在相机坐标系中获得的位置为(xi,yi)，其方法包括：
λ为所述移动终端与所述LED灯体之间在z轴上的高度差，根据计算的所述LED像平面的直径和所述LED灯体实际直径的比例关系、三角形性质及成像原理，求得所述移动终端与所述LED灯体之间在z轴上的高度差，即
其中，D为LED的直径，dpixel为相机的像素距离，Pd是像素距离到物理距离的转换，f为相机的焦距。
f(焦距),dx/dy(像素距离),u0/v0(图像中心像素)组成了特征矩阵K；
T是平移矩阵，相当于移动终端在三维世界坐标系中的位置；
R是从世界坐标系到相机坐标系的旋转矩阵。
当只考虑二维平面时，α，β视为常数，而γ由里程计传感器获取。
步骤S113：由步骤S111得到的PLED(Xi，Yi,Zi)，可以计算：
Zs＝Zi-λ (2)
步骤S114：相机中心在世界坐标系的位姿计算方法包括：
得到Ps[Xs，Ys，Zs，γ]。
步骤S115：移动终端在世界坐标系的位姿St可以由以下方法获取：
其中rx，ry，rz和tx，ty，tz是从相机坐标系到移动终端的base_link坐标系的坐标变换的旋转和平移的系数。
步骤S12：由SLO-VLP和里程计输入的偏航角θ计算得到γ，通过激光雷达匹配器(AMCL)进行TF坐标关系校正，其方法包括：
激光雷达通过将扫描数据与以前的扫描相匹配，获得两个之间的相对位姿变换关系，以方向γ的坐标变换关系来修正里程计传感器得到的移动终端的方向，具体方法如下：
θt＝γ＝γodom+γLIDAR→Map
步骤S2：将步骤S1得到的SLO-VLP位姿作为自适应蒙特卡罗定位算法(AMCL)粒子的初始化位姿，得到由AMCL估计的位置(Xi，Yi)。其方法包括：
步骤S21：将步骤S1获得的SLO-VLP姿态St作为AMCL滤波器粒子的初始化位置；
步骤S22：把里程计传感器获取的位姿信息和激光雷达数据输入到AMCL定位器，输出终端在地图上的位姿；
步骤S3：将步骤S1、S2得到的观测位姿作为EKF算法的输入值以纠正里程计传感器计算的移动终端位姿估计值。其方法包括：
步骤S31：计算基于里程计传感器的姿态估计值，该信息由机器人内部的里程计测量。更具体的，在时间间隔(t-1，t]内，机器人从位姿xt-1运动到位姿xt，里程计反馈了从xt-1＝(x，y，θ)T到xt＝(x’，y’，θ’)T的相对前进。
其中F为变换矩阵，为根据xt-1对现状态真实值xt的估计，ut为运动控制信息。
估计位姿的误差协方差矩阵Pt|t-1＝FPt-1FT+Qt(7)，其中Qt是输入噪声协方差矩阵，且假设输入噪声服从正态分布。
步骤S22：将步骤S1得到的观测位姿作为EKF算法的输入值以纠正步骤S21计算的估计值。其方法包括：
步骤S221：计算卡尔曼增益K＝Pt|t-1Ht T(HtPt|t-1Ht T+Rt)-1(8)，其中Ht是观测矩阵，Rt是观测噪声的误差协方差矩阵，且假设观测噪声服从正态分布。
步骤S222：以步骤S221获得的卡尔曼增益K去更新位姿向量和协方差矩阵，具体做法如下：
Pt＝Pt|t-1-KHtPt|t-1 (10)
以上中，为对现状态真实值xt的估计；为基于t-1次对t次的预估计；假设输入噪声和观测噪声均服从正态分布，Q为输入噪声的协方差，R为观测噪声的协方差。
至此，通过状态预测方程式(6)和多传感器融合的估计姿态可以利用卡尔曼滤波算法更新预测姿态和纠正里程计的累积误差。
以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种基于LIFI的视障人士室内导航方法及系统