[发明专利]基于TOF深度相机与IMU的平仓机器人自主定位方法

摘要

本发明涉及粮仓机器人定位技术领域，提供一种基于TOF深度相机与IMU的平仓机器人自主定位方法。本发明的方法包括：步骤1：通过TOF深度相机构建二维点云粮仓环境，并通过当前帧与参考帧的特征匹配，解算平仓机器人的位姿；步骤2：对IMU进行初始化；步骤3：通过IMU解算平仓机器人的位姿；步骤4：将步骤1中通过TOF深度相机解算出的平仓机器人的位姿作为状态向量，将步骤3中通过IMU解算出的平仓机器人的位姿作为观测向量，对步骤1和步骤3中解算出的位姿进行卡尔曼滤波融合，得到平仓机器人的位姿，完成平仓机器人的自主定位。本发明能够提高平仓机器人在环境复杂的粮仓环境中自主定位的准确性。

主权项

1.一种基于TOF深度相机与IMU的平仓机器人自主定位方法，所述平仓机器人上配置有TOF深度相机与IMU，其特征在于，包括下述步骤：步骤1：通过TOF深度相机解算平仓机器人的位姿：步骤1.1：通过TOF深度相机采集第t时刻的粮仓图像，提取第t时刻的粮仓图像的特征点，构建二维点云粮仓环境；步骤1.2：将第t‑1时刻的粮仓图像作为参考帧，将第t时刻的粮仓图像作为当前帧，将当前帧与参考帧进行特征匹配，若当前帧与参考帧中有比例为α的特征点的位置坐标相同，则当前帧为关键帧；步骤1.3：通过当前帧与参考帧之间多组对应点的帧间匹配，计算本质矩阵E，对本质矩阵E进行分解，求出旋转矩阵R和平移向量(x_t,y_t,1)，由旋转矩阵R求解出平仓机器人的姿态角(γ_t,θ_t,ψ_t)，得到平仓机器人的位姿(x_t,y_t,γ_t,θ_t,ψ_t)；其中，γ_t、θ_t、ψ_t分别为第t时刻平仓机器人相对导航坐标系的俯仰角、翻滚角、偏航角，(x_t,y_t)为第t时刻平仓机器人的位置坐标；步骤2：对IMU进行初始化；步骤3：通过初始化后的IMU解算平仓机器人的位姿；步骤3.1：将导航坐标系作为参考坐标系，计算第t时刻导航坐标系到机器人坐标系的关系为其中，为第t时刻平仓机器人在导航坐标系下的三轴角速度，为第t时刻陀螺仪采集的平仓机器人在机器人坐标系下的三轴角速度；步骤3.2：计算平仓机器人的初始四元数为其中，(γ₀,θ₀,ψ₀)为平仓机器人的初始姿态；步骤3.3：采用二阶龙格库塔法，计算第t时刻平仓机器人的四元数为(λ_t，p_1t，p_2t，p_3t)；步骤3.4：计算第t时刻平仓机器人的姿态矩阵为步骤3.5：计算第t时刻平仓机器人的姿态角为其中，T₁₁(t)＝λ_t²+p_1t²‑p_2t²‑p_3t²，T₁₂(t)＝2(p_1tp_2t+λ_tp_3t)，T₁₃(t)＝2(p_1tp_3t‑λ_tp_2t)，T₂₃(t)＝2(p_2tp_3t+λ_tp_1t)，T₃₃(t)＝λ_t²+p_3t²‑p_1t²‑p_2t²；步骤3.6：根据四元数和姿态矩阵将加速度计采集的平仓机器人在机器人坐标系下的比力分量转换到导航坐标系中，在导航坐标系下通过积分求解比力方程，得到平仓机器人相对地球的速度，并由平仓机器人的速度积分得到第t时刻平仓机器人的位置(x_t,y_t)；步骤4：将步骤1中通过TOF深度相机解算出的平仓机器人的位姿作为状态向量X_t，将步骤3中通过IMU解算出的平仓机器人的位姿作为观测向量Z_t，对步骤1和步骤3中解算出的位姿进行卡尔曼滤波融合，得到平仓机器人的位姿，完成平仓机器人的自主定位。