[发明专利]一种基于卡尔曼滤波的微机器人高精度姿态测量相关滤波方法

权利要求书

1.一种基于卡尔曼滤波的微机器人高精度姿态测量相关滤波方法，其特征在于，包括以下步骤：

1)微机器人位置预测器预测，包括：

1.1)微机器人的状态由矢量s＝{x,y,v_x,v_y}表示，其中x和y是微机器人中心的坐标，v_x，v_y分别是沿x轴和y轴的速度分量，遵循卡尔曼滤波的基本原理，递归得到微机器人离散动态系统的状态方程为

其中，s_k|k-1是微机器人在时间k上的预测状态，s_k-1|k-1是微机器人在时间k-1上的预测状态，A和B是描述微机器人状态转移和状态控制的常数矩阵，u_k-1是控制向量，p_k|k-1是时间k处的估计协方差矩阵，p_k-1|k-1是时间k-1处的预测协方差，Q_k-1是状态转移过程中的误差矩阵；

1.2)通过目标微机器人的目标观测值对目标微机器人的状态进行优化，从而修正估计误差，在离散动态系统中，目标微机器人的状态被修正为

其中，K_k是状态增益矩阵，H是观测传递矩阵，H^T是观测传递矩阵的转置矩阵，p_k|k是校正目标状态后的协方差矩阵,p_k|k-1是时间k处的估计协方差矩阵，s_k|k表示检测区域在当前帧中的预测位置,s_k|k-1是微机器人在时间k上的预测状态，s_k-1|k-1是微机器人在时间k-1上的预测状态,γ_k-1是k-1时刻微机器人的目标观测值，I是恒等矩阵，R为常数矩阵；

2)微机器人位置核相关滤波检测，得到微机器人的位置；

3)通过位置预测器对微机器人的位置自适应校准，得到目标微机器人的准确位置。

2.根据权利要求1所述的基于卡尔曼滤波的微机器人高精度姿态测量相关滤波方法，其特征在于，步骤2)中，微机器人位置核相关滤波检测，具体包括：

2.1)通过式(12)微机器人离散动态系统的状态方程来预测微机器人所在区域的位置，然后在预测状态s_k|k-1提取候选测试补丁z，利用循环特性，得到一组检测样本；

{P^uz|u＝0,1,...,n-1}     (13)

其中，u表示移位元素，P^uz表示元素向量z第u次循环移位结果，每个样本z在移位n后周期性地获得；

2.2)在预测器优化输出微机器人的位置上，从检测样本中提取检测基样本，利用训练好的核相关分类器f(z)＝(K^Z)^Tα对微机器人位置进行检测，响应最大位置即为微机器人的位置，其中f(z)表示元素向量z的所有循环移位向量输出响应，K是核相关矩阵，z是检测样本，K^Z是循环矩阵，(K^Z)^T是循环矩阵的转置矩阵，α是回归分类器中的参数是循环矩阵。