[发明专利]一种基于IMU和楼层高度目标补偿的3维定位方法

说明书

本发明公开了一种基于IMU和楼层高度目标补偿的3维定位方法，涉及传感器检测领域；利用改进的扩展卡尔曼滤波(EKF)和目标楼层高度补偿多传感器融合算法对轮式机器人运动过程中的高度进行校正,结合航位推算定位算法，实时，精准的提供轮式机器人乘坐电梯过程中楼层变更情况，得到了轮式机器人的三维定位；无须在工作环境布置额外传感器，降低了气压变化对轮式机器人高度定位的干扰，提高了轮式机器人3维定位过程中垂直定位的准确性，为轮式机器人在多楼层间运动的推广提供了坚实的基础。有效减少轮式机器人在乘坐电梯时气压突然变化引起的高度变化，解决了轮式机器人在资源有限的情况下进行三维定位的问题。

技术领域

本发明涉及传感器检测领域，提出了一种基于IMU和楼层高度目标补偿的3维定位方法。

背景技术

精准的三维定位是轮式机器人在多楼层间安全移动的基础，尤其是在大型酒店、商场等复杂场景中需要频繁进行物品运送、人员导航等多楼层移动时。然而，使用低成本传感器实现多楼层场景下移动系统的精确三维定位仍然是一项重大技术挑战。例如，目前主流的定位技术，如利用利用雷达的实时建图与定位技术(SLAM)、超宽带(UWB)和全球定位系统(GNSS)等，往往受工作环境所限，不得不牺牲传感器的成本和简单性，才能获得与真实位置所匹配的3维位置。

其中视觉SLAM能够通过激光雷达建立轮式机器人运动过程中的工作环境地图，来完成轮式机器人的3维定位。但是由于高维和参数空间中的大量离散变量，轮式机器人在实际应用中需要大量时间来完成计算获取自身位置和环境信息。并且在轮式机器人乘坐箱式电梯由于其狭窄内部空间和电梯内行人的障碍会导致点云中出现死区，降低了激光雷达的分辨率，从而导致定位出现偏差。基于UWB技术需要在轮式机器人周围楼层布置多个UWB基站和信号发生器来提供距离信息给轮式机器人，为机器人在实际应用当中造成来不必要的成本与工序负担。基于GNSS定位性能受信号环境的影响，如高层建筑物群，地下工程等信号较弱的场景下，存在提供位置信息不准确甚至位置信息跳跃的问题，导致轮式机器人定位导航过程中无法按预定计划完成任务。目前常有的轮式机器人方案3维定位方案存在需要在外界布置额外传感器，成本高，受环境因素干扰等问题。

发明内容

为解决现有技术的不足，本发明提供了一种基于IMU和楼层高度目标补偿的3维定位方法，有效减少轮式机器人在乘坐电梯时气压突然变化引起的高度变化，解决了轮式机器人在资源有限的情况下进行三维定位的问题。

采用的技术方案为：

一种基于IMU和楼层高度目标补偿的3维定位方法，包括以下步骤：

步骤1：将集成的MEMS传感器放置在机器人的中心位置，IMU的z轴方向垂直于地面方向，编码器安装于机器人的左右后轮上；

步骤2：对采集到的MEMS传感器数据进行滑动窗口平滑滤波，如式(1)所示：

其中，m_t为传感器采集到的第t个数据；m_t+1为传感器采集到的第t+1个数据；d为滑动窗口长度；d_t+d为第t+d个数据滑动窗口长度；m'经平滑滤波处理后的MEMS传感器数据；

步骤3：对步骤2中获得的m'进行测定阈值条件判断；若满足测定阈值条件，则确定轮式机器人触发高度变化事件，进入步骤4；若不能满足测定阈值条件，则进入步骤7；

步骤4：当触发了高度变化事件时，将水平速度a_x,a_y更新为零速度，采用卡尔曼滤波器融合加速度az'_i和气压数据p'_i，生成机器人初始高度X(k)；

步骤5:通过机器人高度变化过程中，气压和匀速运动时间变化确定楼层变化F，结合气压数据和楼层变化F，获取轮式机器人乘坐电梯过程中的楼层目标高度H_n；