[发明专利]基于有界优化的机器人位姿递归估计方法

权利要求书

1.一种基于有界优化的机器人位姿递归估计方法，其特征在于，所述方法包括以下步骤：

步骤1：定义位姿估计系统的状态变量和量测变量，根据坐标变换和相机针孔模型建立位姿估计系统模型，过程如下：

1.1定义位姿估计系统的状态变量x

其中是机器人在相机坐标系中的位置变量，是机器人在相机坐标系中的方向变量；

1.2当每个采样周期内机器人的速度保持常量时，根据机器人运动规律得到状态转移方程

其中t是采样周期，w_k是系统噪声；

1.3定义位姿估计系统的量测变量z_k

其中p是特征点数目,和分别是第i个标定点在图像坐标系中的横纵坐标；

1.4机器人坐标系和相机坐标系的坐标变换

其中是第i个特征点在机器人坐标系中的坐标，通过提前测量得到，是第i个特征点在相机坐标系中的坐标；R(Θ)是旋转矩阵；

1.5相机针孔模型描述特征点在相机坐标系中和图像坐标系中的坐标变换

其中P_X和P_Y分别是图像平面上沿X轴和Y轴的像素间距，F是相机焦距，这些参数都是通过相机标定测试获得的；

1.6根据坐标变换和相机针孔模型得到量测变量z_k和状态变量x的非线性关系，并表示为量测方程

其中v_k是量测噪声；

1.7系统噪声w_k和量测噪声v_k是上界未知的有界噪声

其中δ_w和分别是系统噪声和量测噪声的上界，且是未知的；

步骤2：设计递归估计器，泰勒展开非线性量测方程并将高阶展开项作为有界噪声，给出估计误差平方的一个上界，最小化估计误差平方上界得到最优估计器增益，过程如下：

2.1设计递归估计器

2.2定义估计误差

2.3得到非线性估计误差系统

2.4泰勒展开非线性项G(x_k)

其中H_k是非线性项的雅克比矩阵，是泰勒展开后的高阶项，这里看成有界的噪声扰动，同时引入有界噪声项表示和v_k的噪声影响；

2.5定义增广噪声向量ξ_k

2.6定义矩阵G_L,k

2.7定义矩阵B_L,k

2.8非线性估计误差系统等价于

2.9设计估计误差平方的一个上界

其中P_k和Φ_k都是正定的矩阵；

2.10通过求解如下最优化问题得到最优估计增益K_k，使得最小化估计误差平方上界且确保上界成立

其中矩阵Φ_k、P_k和常变量都是优化变量，η是大于0的任意常数，此优化问题能够通过MATLAB的线性矩阵不等式工具箱求解。