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

权利要求书

1.一种基于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；

步骤6：经步骤4得到的机器人初始高度X(k)和步骤5获得楼层目标高度H_n；通过改进的扩展卡尔曼滤波和目标楼层高度补偿多传感器融合算法对轮式机器人运动过程中的高度进行校正，得到更新后的EKF测量值实现轮式机器人乘坐电梯移动过程中楼层高度变化的厘米级定位；

步骤7：当楼层高度不变时，垂直加速度a_z更新为零，利用航位推算算法获取轮式机器人的位移路径上的位置和航向角信息；

步骤8：使用IMU传感器计算欧拉角校正编码器获得的航向角ψ，降低重心偏移导致的角速度积分误差；

步骤9：通过步骤8获得的航向角ψ校正轮式机器人前进方向，通过IMU校正姿态后的航位推算定位算法得到轮式机器人在n时刻的位置坐标，结合步骤6获得的EKF-TC测量值得到轮式机器人的楼层间的三维定位。

2.如权利要求1所述的基于IMU和楼层高度目标补偿的3维定位方法，其特征在于：所述集成的MEMS传感器包括IMU传感器及差压传感器；

所述IMU传感器采集加速度数据及角速度数据；

所述加速度数据，包括x轴水平加速度a_x、y轴水平加速度a_y、垂直高度加速度a_z；

所述角速度数据，包括x轴水平角速度g_x、y轴水平角速度g_y、垂直高度角速度g_z；

所述差压传感器采集气压数据p；

所述MEMS传感器，采集的数据包括加速度数据a_x，a_y，a_z、角速度数据g_x，g_y，g_z及气压数据p。