一种果园特征识别与定位的导航方法

摘要

针对高山丘陵等复杂果园环境中，植保无人机导航作业精度低，果实特征位置估计误差大，且果树特征提取中噪声多等问题，提出了一种基于四元数的平方根容积卡尔曼滤波算法，该算法以姿态四元数误差及陀螺仪漂移误差为状态量，以SINS/SLAM组合导航的姿态四元数为量测量，并进行时间更新与量测更新；同时，采用Kmeans等方法处理田园环境信息，实现了较高精度的果树特征的提取.基于四元数的平方根CKF算法，既解决了传统四元数的规范化问题，降低了传统四元数的平方根UKF算法的状态维数及计算复杂度，与四元数SRUKF、四元数SRCDKF算法比较，仿真实验结果表明新算法估计横滚角、俯仰角、航偏角误差均值分别为0.05°、0.08°、0.03°,误差均为最小，且算法精度较四元数SRUKF-SLAM算法提高了30%左右.在较大初始角误差条件下，进行对SRCKF、CKF、SRUKF 3种滤波算法的估计误差对比，实验表明SRCKF算法均具有最高的滤波精度，且滤波收敛速度最快、稳定性最好.

关键词

四元数 / 状态模型 / 平方根容积卡尔曼滤波 / 姿态估计 / 数值稳定性