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

摘要

一种基于滚动时域的机器人视觉伺服位姿估计方法，包括以下步骤：1)利用相机投影模型进行特征点变换；2)建立离散时间模型；3)根据离散时间模型以及滚动时域策略，定义代价函数；4)确保代价函数达到最小，从而设计最优滚动时域估计器。本发明提供了一种基于滚动时域的机器人视觉伺服位姿估计方法，通过引入滚动时域目标函数来最小化代价函数，并确定最优预测方程的设计方案。

主权项

1.一种基于滚动时域的机器人视觉伺服位姿估计方法，所述方法包括以下步骤：/n1)特征点变换；/n定义物体相对于相机的相对位姿为W＝[X,Y,Z,φ,α,ψ]^T，相机坐标系中第j个特征点的坐标向量为物体坐标系中第j个特征点的坐标向量为图像平面上第j个特征点的投影坐标为其中，j∈{1,2，…，5}，X，Y，Z表示物体坐标系相对于相机坐标系的相对位置，φ，α，ψ表示滚动，俯仰和偏航参数的相对姿态，则第j个特征点在物体坐标系与相机坐标系之间关系为/n /n其中，/n根据投影定律，特征点在图像平面上的投影坐标与的变换关系为/n /n其中，P_X和P_Y分别为图像平面Xⁱ和Yⁱ轴上的像素间隔，F为焦距；/n2)建立离散时间模型；/n对于位姿估计，定义k时刻的状态向量为如下包含位姿和速度参数的形式/n /n定义y_k是k时刻的量测向量，初始状态x₀为未知常数，u_k为k时刻的控制向量，ξ_k为k时刻的系统噪声向量，η_k为k时刻的量测噪声向量，由此得离散时间状态方程：/nx_k+1＝Ax_k+Bu_k+ξ_k (4)/ny_k＝Cx_k+η_k (5)/n其中，为状态矩阵，B为控制输入矩阵，为与特征点相关的量测矩阵，/n3)定义代价函数；/n根据滚动时域估计将式(4)转化为如下方程：/n /n其中，为状态向量x_k-M-1基于k-1时刻的估计值，为的预测值，M为滚动时域窗口长度；定义方程(6)的代价函数如下/n /n其中，和是欧几里得范数，μ是一个非负常数；/n4)设计滚动时域估计器；/n定义如下向量/n /n /n对于给定的找到最优估计确保代价函数(7)达到最小/nminΛ_k (8)/n并满足约束/n /n根据一阶KKT条件，对式(7)进行求导得/n /n进一步由式(10)得最优估计器为/n /n结合给定先验预测以及最优估计器(11)，得到最终的最优预测更新方程为：/n /n