一种基于滚动时域的机器人视觉伺服位姿估计方法与流程

本发明涉及机器人视觉伺服系统，尤其涉及的是基于滚动时域的位姿估计方法。

背景技术：

随着科学技术和控制技术的发展，计算机视觉在各个领域已有广泛的运用，其中，机器人视觉伺服(rvs)系统的位姿估计问题一直备受广泛关注。位姿估计是指使用图像信息来确定相机相对于物体坐标系的位置和姿态，机器人系统能利用位置和姿态进行机器人运动的实时控制。针对机器人视觉伺服系统位姿估计的研究，不仅可以丰富机器人位姿估计的理论成果，还可以满足多领域对位姿估计技术越来越高的要求，具有实际的理论和工程意义。

然而在实际环境中，rvs系统的位姿估计有两个主要困难，分别是位姿估计的效率以及其鲁棒性。同时机器人在运动过程中总是存在噪声干扰，机器人的位姿估计问题实际上就成为一个带有噪声干扰的状态估计问题。目前主要是应用卡尔曼滤波方法来解决这些困难。对于线性系统的状态估计卡尔曼滤波已经能够很好地解决,但是在非线性系统中就不那么容易.目前已有的解决非线性问题的方法也都是线性系统卡尔曼滤波方法的扩展,如最常用的扩展卡尔曼滤波(extendedkalmanfilter,ekf),无迹卡尔曼滤波(unscentedkalmanfilter,ukf)等。wang等在论文(3drelativepositionandorientationestimationusingkalmanfilteringforrobotcontrol)中，针对机器人位姿估计问题，提出了一种扩展卡尔曼滤波(ekf)方法。shademan等在论文(sensitivityanalysisofekfanditeratedekfforposition-basedvisualservoing)中，主要是采用了迭代卡尔曼滤波(i-ekf)算法并与扩展卡尔曼滤波(ekf)算法进行对比。ficocelli等在论文(adaptivefilteringforposeestimationinvisualservoing)中，利用自适应卡尔曼滤波(a-ekf)算法来实现机器人位姿估计，但是，上述这些方法都没有完全解决rvs位姿估计的效率以及鲁棒性问题。因此，针对基于滚动时域的机器人视觉伺服系统位姿估计方法的研究很有必要。

技术实现要素：

为了克服现有技术无法解决机器人视觉伺服位姿估计问题的不足，本发明提供一种基于滚动时域的机器人视觉伺服位姿估计方法，通过引入滚动时域目标函数来最小化代价函数，并确定最优预测方程的设计方案。

本发明解决其技术问题所采用的技术方案是：

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

1)特征点变换；

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

其中，

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

其中，px和py分别为图像平面xⁱ和yⁱ轴上的像素间隔，f为焦距；

2)建立离散时间模型；

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

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

xk+1＝axk+buk+ξk(4)

yk＝cxk+ηk(5)

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

3)定义代价函数；

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

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

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

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

定义如下向量

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

minλk(8)

并满足约束

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

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

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

本发明的技术构思为：首先，利用相机投影模型进行特征点变换，并考虑系统过程噪声及量测噪声，从而建立离散时间模型；然后，引入并最小化代价函数，得到最优预测；最后，结合给定的先验预测，得到最终的最优预测更新方程。

本发明的有益效果主要表现在：引入并最小化代价函数，得到最优预测，有利于更好地估计离散时间模型的状态；通过选择合适的自由参数u可以保证滚动时域估计器即使在高噪声影响下也能执行估计。

附图说明

图1是物体特征点在图像平面上的投影示意图。

具体实施方式

下面结合附图对本发明作进一步描述。

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

1)特征点变换；

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

其中，

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

其中，px和py分别为图像平面xⁱ和yⁱ轴上的像素间隔，f为焦距；

2)建立离散时间模型；

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

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

xk+1＝axk+buk+ξk(4)

yk＝cxk+ηk(5)

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

3)定义代价函数；

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

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

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

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

定义如下向量

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

minλk(8)

并满足约束

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

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

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