[发明专利]一种基于滚动时域的机器人视觉伺服位姿估计方法

权利要求书

1.一种基于滚动时域的机器人视觉伺服位姿估计方法，所述方法包括以下步骤：

1)特征点变换；

定义物体相对于相机的相对位姿为W＝[X,Y,Z,φ,α,ψ]^T，相机坐标系中第j个特征点的坐标向量为物体坐标系中第j个特征点的坐标向量为图像平面上第j个特征点的投影坐标为其中，j∈{1,2，…，5}，X，Y，Z表示物体坐标系相对于相机坐标系的相对位置，φ，α，ψ表示滚动，俯仰和偏航参数的相对姿态，则第j个特征点在物体坐标系与相机坐标系之间关系为

其中，

根据投影定律，特征点在图像平面上的投影坐标与的变换关系为

其中，P_X和P_Y分别为图像平面Xⁱ和Yⁱ轴上的像素间隔，F为焦距；

2)建立离散时间模型；

对于位姿估计，定义k时刻的状态向量为如下包含位姿和速度参数的形式

定义y_k是k时刻的量测向量，初始状态x₀为未知常数，u_k为k时刻的控制向量，ξ_k为k时刻的系统噪声向量，η_k为k时刻的量测噪声向量，由此得离散时间状态方程：

x_k+1＝Ax_k+Bu_k+ξ_k (4)

y_k＝Cx_k+η_k (5)

其中，为状态矩阵，B为控制输入矩阵，为与特征点相关的量测矩阵，

3)定义代价函数；

根据滚动时域估计将式(4)转化为如下方程：

其中，为状态向量x_k-M-1基于k-1时刻的估计值，为的预测值，M为滚动时域窗口长度；定义方程(6)的代价函数如下

其中，和是欧几里得范数，μ是一个非负常数；

4)设计滚动时域估计器；

定义如下向量

对于给定的找到最优估计确保代价函数(7)达到最小

minΛ_k (8)

并满足约束

根据一阶KKT条件，对式(7)进行求导得

进一步由式(10)得最优估计器为

结合给定先验预测以及最优估计器(11)，得到最终的最优预测更新方程为：