基于有界优化的机器人位姿递归估计方法技术

一种基于有界优化的机器人位姿递归估计方法，根据相机针孔模型和坐标变换过程建立位姿估计系统模型，设计递归滤波器形式，通过泰勒展开非线性量测方程并将高阶展开项作为有界噪声，给出估计误差平方的一个上界，构造最优化问题求解最优估计器增益，使得估计误差平方上界最小化且确保估计误差上界成立。本发明专利技术提供一种在噪声统计特性未知情况下的机器人位姿有界递归优化估计方法，实现机器人位姿的实时估计。

Recursive estimation of robot pose based on bounded optimization

【技术实现步骤摘要】
基于有界优化的机器人位姿递归估计方法
本专利技术涉及一种基于有界优化的机器人位姿递归估计方法，特别是机器人系统噪声统计特性未知情况下的位姿估计方法。
技术介绍
机器人视觉伺服是一种使用视觉传感器所反馈的信息来控制机器人运动的技术，随着视觉传感器精读的提高和图像处理硬件的快速发展，机器人视觉伺服逐渐发展为跨机器人、自动控制和图像处理的多学科交叉方向。如何从图像几何特征和非几何特征中提取有用的信息来确定移动目标的位置和方向是相关研究的热点，也是实现机器人实时运动控制的第一步，机器人位姿估计问题因此受到了越来越多的关注。目前大多数位姿估计算法是基于扩展卡尔曼滤波器设计的，需要假设噪声服从已知协方差的高斯分布。然而，在实际的机器人控制系统中很难直接获取噪声的统计特性并且噪声也不完全满足高斯分布假设。针对噪声统计特性未知情况下的状态估计问题，包括能量有界噪声、协方差未知的高斯噪声和有界噪声，在位姿估计上有很大的应用空间。有界递归优化估计是一种针对有界噪声提出来的最优状态估计方法，能够克服传统卡尔曼滤波的高斯噪声假设带来的限制，更加符合实际系统的噪声统计特性未知的情况。另一方面，有界递归优化估计保留了卡尔曼滤波的递归计算形式，在每个时刻通过最小化估计误差上界递归计算最优估计增益，满足位姿估计等问题的实时性要求。
技术实现思路
为了克服现有机器人位姿估计方法需要噪声统计特性已知的不足，本专利技术提出一种基于有界优化的机器人位姿递归估计方法，并结合线性矩阵不等式技术，利用多个相机标定点中的位姿变换信息，在此基础上设计有界递归优化位姿估计方法，实现实时的机器人位姿估计。为了解决上述技术问题提出的技术方案如下：一种基于有界优化的机器人位姿递归估计方法，包括以下步骤：步骤1：定义位姿估计系统的状态变量和量测变量，根据坐标变换和相机针孔模型建立位姿估计系统模型，过程如下：1.1定义位姿估计系统的状态变量x其中是机器人在相机坐标系中的位置变量，是机器人在相机坐标系中的方向变量；1.2当每个采样周期内机器人的速度保持常量时，根据机器人运动规律得到状态转移方程其中t是采样周期，wk是系统噪声；1.3定义位姿估计系统的量测变量zk其中p是特征点数目，和分别是第i个标定点在图像坐标系中的横纵坐标；1.4机器人坐标系和相机坐标系的坐标变换其中是第i个特征点在机器人坐标系中的坐标，通过提前测量得到，是第i个特征点在相机坐标系中的坐标；R(Θ)是旋转矩阵；1.5相机针孔模型描述特征点在相机坐标系中和图像坐标系中的坐标变换其中PX和PY分别是图像平面上沿X轴和Y轴的像素间距，F是相机焦距，这些参数都是通过相机标定测试获得的；1.6根据坐标变换和相机针孔模型得到量测变量zk和状态变量x的非线性关系，并表示为量测方程其中vk是量测噪声；1.7过程噪声wk和量测噪声vk是上界未知的有界噪声其中δw和分别是过程噪声和量测噪声的上界，且是未知的；步骤2：设计递归估计器，泰勒展开非线性量测方程并将高阶展开项作为有界噪声，给出估计误差平方的一个上界，最小化估计误差平方上界得到最优估计器增益，过程如下：2.1设计递归估计器2.2定义估计误差2.3得到非线性估计误差系统2.4泰勒展开非线性项G(xk)其中Hk是非线性项的雅克比矩阵，是泰勒展开后的高阶项，这里看成有界的噪声扰动，同时引入有界噪声项表示和vk的噪声影响；2.5定义增广噪声向量ξk2.6定义矩阵GL，k2.7定义矩阵BL，k2.8非线性估计误差系统等价于2.9设计估计误差平方的一个上界其中Pk和Φk都是正定的矩阵；2.10通过求解如下最优化问题得到最优估计增益Kk，使得最小化估计误差平方上界且确保上界成立其中矩阵Φk、Pk和常变量θk都是优化变量，η是大于0的任意常数，此优化问题能够通过MATLAB的线性矩阵不等式工具箱求解。本专利技术的技术构思：针对噪声统计特性位姿下的机器人位姿估计问题，考虑相机针孔模型和坐标变换过程建立位姿估计系统模型，设计有界递归估计器对机器人姿态进行估计。通过泰勒展开非线性量测方程并将高阶展开项作为有界噪声，给出估计误差平方的一个上界，构造最优化问题求解最优估计器增益，使得估计误差平方上界最小化且确保估计误差上界成立。本专利技术所设计的有界递归估计器能有效克服系统噪声服从高斯分布的限制性条件，实现噪声统计特性未知情况下机器人位姿的实时估计。本专利技术的优点为：避免基于扩展卡尔曼滤波的位姿估计方法对高斯白噪声的假设，实现噪声统计特性未知情况下的机器人位姿实时估计。附图说明图1为本专利技术估计方法流程图；图2为本专利技术机器人在X轴上的真实轨迹和有界递归优化的估计轨迹对比图；图3为本专利技术机器人在Y轴上的真实轨迹和有界递归优化的估计轨迹对比图；图4为本专利技术机器人在Z轴上的真实轨迹和有界递归优化的估计轨迹对比图；图5为本专利技术机器人在翻滚角上的真实值和有界递归优化的估计值对比图；图6为本专利技术机器人在俯仰角上的真实值和有界递归优化的估计值对比图；图7为本专利技术机器人在偏航角上的真实值和有界递归优化的估计值对比图；具体实施方式下面结合附图对本专利技术做进一步说明。参照图1-图7，一种基于有界优化的机器人位姿递归估计方法，所述估计方法包括如下步骤：步骤1：定义位姿估计系统的状态变量和量测变量，根据坐标变换和相机针孔模型建立位姿估计系统模型，过程如下：1.1定义位姿估计系统的状态变量x其中是机器人在相机坐标系中的位置变量，是机器人在相机坐标系中的方向变量；1.2当每个采样周期内机器人的速度保持常量时，根据机器人运动规律得到状态转移方程其中t是采样周期，wk是系统噪声；1.3定义位姿估计系统的量测变量zk其中p是特征点数目，和分别是第i个标定点在图像坐标系中的横纵坐标；1.4机器人坐标系和相机坐标系的坐标变换其中是第i个特征点在机器人坐标系中的坐标，通过提前测量得到，是第i个特征点在相机坐标系中的坐标；R(Θ)是旋转矩阵；1.5相机针孔模型描述特征点在相机坐标系中和图像坐标系中的坐标变换其中PX和PY分别是图像平面上沿X轴和Y轴的像素间距，F是相机焦距，这些参数都是通过相机标定测试获得的；1.6根据坐标变换和相机针孔模型得到量测变量zk和状态变量x的非线性关系，并表示为量测方程其中vk是量测噪声；1.7过程噪声wk和量测噪声vk是上界未知的有界噪声本文档来自技高网...

【技术保护点】
1.一种基于有界优化的机器人位姿递归估计方法，其特征在于，所述方法包括以下步骤：/n步骤1：定义位姿估计系统的状态变量和量测变量，根据坐标变换和相机针孔模型建立位姿估计系统模型，过程如下：/n1.1定义位姿估计系统的状态变量x/n

【技术特征摘要】
1.一种基于有界优化的机器人位姿递归估计方法，其特征在于，所述方法包括以下步骤：
步骤1：定义位姿估计系统的状态变量和量测变量，根据坐标变换和相机针孔模型建立位姿估计系统模型，过程如下：
1.1定义位姿估计系统的状态变量x



其中是机器人在相机坐标系中的位置变量，是机器人在相机坐标系中的方向变量；
1.2当每个采样周期内机器人的速度保持常量时，根据机器人运动规律得到状态转移方程



其中t是采样周期，wk是系统噪声；
1.3定义位姿估计系统的量测变量zk



其中p是特征点数目，和分别是第i个标定点在图像坐标系中的横纵坐标；
1.4机器人坐标系和相机坐标系的坐标变换






其中是第i个特征点在机器人坐标系中的坐标，通过提前测量得到，是第i个特征点在相机坐标系中的坐标；R(Θ)是旋转矩阵；
1.5相机针孔模型描述特征点在相机坐标系中和图像坐标系中的坐标变换



其中PX和PY分别是图像平面上沿X轴和Y轴的像素间距，F是相机焦距，这些参数都是通过相机标定测试获得的；
1.6根据坐标变换和相机针孔模型得到量测变量zk和状态变量x的非线性关系，并表示为量测方程



其中vk是量测噪声；
1.7过程噪声wk和量测噪声vk...

【专利技术属性】
技术研发人员：陈博，章宇晨，俞立，张文安，洪榛，
申请(专利权)人：浙江工业大学，
类型：发明
国别省市：浙江;33