1.一种基于渐进容积信息滤波的机器人跟随方法,其特征在于,所述方法包括以下步骤:步骤1,初始化参数,包括k=0时刻跟随系统的状态向量 状态协方差P0|0以及过程噪声协方差Qk,并将量测噪声协方差Rk放大N倍;
步骤2,机器人获取k时刻传感器的量测信息,根据球面径向规则,由k-1时刻估计状态和协方差Pk-1|k-1构造Sigma点σi,k-1|k-1,i=1,2,…,L,从而获得k时刻跟随系统的预测状态 及协方差Pk|k-1;
步骤3,引入信息矩阵Yk|k-1及信息状态向量 对量测更新过程进行迭代,根据k时刻预测状态 及协方差Pk|k-1得迭代初始状态 和 已知迭代过程中状态 和协方差构造Sigma点 从而获得k时刻的量测预测值 及其互协方差步骤4,根据步骤3获得的量测预测值 与互协方差 结合k时刻机器人获取的传感器量测信息zk计算得第n次迭代更新后的信息矩阵 及信息状态向量步骤5,根据步骤4获得的信息矩阵 及信息状态向量 得第n次迭代更新后跟随系统的状态估计 与协方差步骤6,判断n≤N是否成立,若满足,执行步骤3-步骤6;否则,进入下一时刻跟随系统的状态估计,执行步骤2-步骤6。
2.如权利要求1所述的一种基于渐进容积信息滤波的机器人跟随方法,其特征在于:所述步骤1中,k时刻跟随系统状态向量 其中, 为k时刻移动机器人的状态向量,且 和 分别为k时刻移动机器人在x和y轴上的位置, 和为其在x和y轴上的速度; 为k时刻跟随对象的状态向量,且
和 分别为k时刻跟随对象在x和y轴上的位置, 和 为其在x和y轴上的速度,上标T表示矩阵转置。
3.如权利要求1或2所述的一种基于渐进容积信息滤波的机器人跟随方法,其特征在于:在所述步骤1中,根据经验初始化k=0时刻跟随系统的状态向量 状态协方差P0|0;根据跟随系统的状态模型初始化过程噪声协方差Qk;根据传感器的量测精度初始化量测噪声协方差Rk。
4.如权利要求1或2所述的一种基于渐进容积信息滤波的机器人跟随方法,其特征在于:所述步骤2中,传感器量测信息为机器人相对跟随对象的距离及方位,且L=2d,d为跟随系统状态向量xk的维数。