CN110640789A - 基于有界优化的机器人位姿递归估计方法 - Google Patents
基于有界优化的机器人位姿递归估计方法 Download PDFInfo
- Publication number
- CN110640789A CN110640789A CN201910896428.1A CN201910896428A CN110640789A CN 110640789 A CN110640789 A CN 110640789A CN 201910896428 A CN201910896428 A CN 201910896428A CN 110640789 A CN110640789 A CN 110640789A
- Authority
- CN
- China
- Prior art keywords
- noise
- robot
- estimation
- bounded
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/0095—Means or methods for testing manipulators
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
一种基于有界优化的机器人位姿递归估计方法,根据相机针孔模型和坐标变换过程建立位姿估计系统模型,设计递归滤波器形式,通过泰勒展开非线性量测方程并将高阶展开项作为有界噪声,给出估计误差平方的一个上界,构造最优化问题求解最优估计器增益,使得估计误差平方上界最小化且确保估计误差上界成立。本发明提供一种在噪声统计特性未知情况下的机器人位姿有界递归优化估计方法,实现机器人位姿的实时估计。
Description
技术领域
本发明涉及一种基于有界优化的机器人位姿递归估计方法,特别是机器人系统噪声统计特性未知情况下的位姿估计方法。
背景技术
机器人视觉伺服是一种使用视觉传感器所反馈的信息来控制机器人运动的技术,随着视觉传感器精读的提高和图像处理硬件的快速发展,机器人视觉伺服逐渐发展为跨机器人、自动控制和图像处理的多学科交叉方向。如何从图像几何特征和非几何特征中提取有用的信息来确定移动目标的位置和方向是相关研究的热点,也是实现机器人实时运动控制的第一步,机器人位姿估计问题因此受到了越来越多的关注。目前大多数位姿估计算法是基于扩展卡尔曼滤波器设计的,需要假设噪声服从已知协方差的高斯分布。然而,在实际的机器人控制系统中很难直接获取噪声的统计特性并且噪声也不完全满足高斯分布假设。针对噪声统计特性未知情况下的状态估计问题,包括能量有界噪声、协方差未知的高斯噪声和有界噪声,在位姿估计上有很大的应用空间。
有界递归优化估计是一种针对有界噪声提出来的最优状态估计方法,能够克服传统卡尔曼滤波的高斯噪声假设带来的限制,更加符合实际系统的噪声统计特性未知的情况。另一方面,有界递归优化估计保留了卡尔曼滤波的递归计算形式,在每个时刻通过最小化估计误差上界递归计算最优估计增益,满足位姿估计等问题的实时性要求。
发明内容
为了克服现有机器人位姿估计方法需要噪声统计特性已知的不足,本发明提出一种基于有界优化的机器人位姿递归估计方法,并结合线性矩阵不等式技术,利用多个相机标定点中的位姿变换信息,在此基础上设计有界递归优化位姿估计方法,实现实时的机器人位姿估计。
为了解决上述技术问题提出的技术方案如下:
一种基于有界优化的机器人位姿递归估计方法,包括以下步骤:
步骤1:定义位姿估计系统的状态变量和量测变量,根据坐标变换和相机针孔模型建立位姿估计系统模型,过程如下:
1.1定义位姿估计系统的状态变量x
1.2当每个采样周期内机器人的速度保持常量时,根据机器人运动规律得到状态转移方程
其中t是采样周期,wk是系统噪声;
1.3定义位姿估计系统的量测变量zk
1.4机器人坐标系和相机坐标系的坐标变换
1.5相机针孔模型描述特征点在相机坐标系中和图像坐标系中的坐标变换
其中PX和PY分别是图像平面上沿X轴和Y轴的像素间距,F是相机焦距,这些参数都是通过相机标定测试获得的;
1.6根据坐标变换和相机针孔模型得到量测变量zk和状态变量x的非线性关系,并表示为量测方程
其中vk是量测噪声;
1.7过程噪声wk和量测噪声vk是上界未知的有界噪声
步骤2:设计递归估计器,泰勒展开非线性量测方程并将高阶展开项作为有界噪声,给出估计误差平方的一个上界,最小化估计误差平方上界得到最优估计器增益,过程如下:
2.1设计递归估计器
2.3得到非线性估计误差系统
2.4泰勒展开非线性项G(xk)
2.5定义增广噪声向量ξk
2.6定义矩阵GL,k
2.7定义矩阵BL,k
2.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
1.4机器人坐标系和相机坐标系的坐标变换
1.5相机针孔模型描述特征点在相机坐标系中和图像坐标系中的坐标变换
其中PX和PY分别是图像平面上沿X轴和Y轴的像素间距,F是相机焦距,这些参数都是通过相机标定测试获得的;
1.6根据坐标变换和相机针孔模型得到量测变量zk和状态变量x的非线性关系,并表示为量测方程
其中vk是量测噪声;
1.7过程噪声wk和量测噪声vk是上界未知的有界噪声
步骤2:设计递归估计器,泰勒展开非线性量测方程并将高阶展开项作为有界噪声,给出估计误差平方的一个上界,最小化估计误差平方上界得到最优估计器增益,过程如下:
2.1设计递归估计器
2.3得到非线性估计误差系统
2.4泰勒展开非线性项G(xk)
2.5定义增广噪声向量ξk
2.6定义矩阵GL,k
2.7定义矩阵BL,k
2.8非线性估计误差系统等价于
2.9设计估计误差平方的一个上界
其中Pk和Φk都是正定的矩阵;
2.10通过求解如下最优化问题得到最优估计增益Kk,使得最小化估计误差平方上界且确保上界成立
其中矩阵Φk、Pk和常变量θk都是优化变量,η是大于0的任意常数,此优化问题能够通过MATLAB的线性矩阵不等式工具箱求解。
本发明针对机器人位姿估计问题,基于坐标变换和相机针孔模型建立位姿估计系统模型,结合有界优化的思想,设计一种基于有界优化的机器人位姿递归估计方法,克服传统机器人位姿估计方法对噪声的高斯假设,实现噪声统计特性未知情况下的机器人位姿实时估计。为验证所提方法的有效性,本发明给出所设计有界递归优化方法对机器人位姿的估计效果图。
为了实现机器人位姿变化和相机观测的效果,将系统采用时间设置为t=0.05s,并且设置相机参数为和在机器人上标定四个特征点,其对应于机器人坐标系中的坐标分别为(-12.5;-46;0),(12.5;-46;0),(-12.5;-21;0)和(12.5;-21;0)。机器人沿着给定的轨迹匀速移动,只有在特定的时间点上改变速度大小和方向。
系统的过程噪声和量测噪声设置为有界噪声,即w(t)=0.2φw(t)-0.1和v(t)=0.2φv(t)-0.1,其中φw(t)和φv(t)是由MATLAB的“rand”函数生成的随机变量。通过执行有界递归优化位姿估计算法,得到的估计结果呈现在图2-7中。图2-4比较了机器人在X、Y和Z坐标轴方向上的真实运动轨迹和有界递归优化估计的轨迹,图5-7比较了机器人在俯仰角、偏航角和翻滚角上的真实值和有界递归优化估计的值,结果表明机器人有界递归优化位姿估计方法在机器人位姿估计上具有很好的估计效果,能实现实时的位姿估计。
以上阐述的是本发明给出的一个实施例表现出的优良优化效果,显然本发明不只是限于上述实施例,在不偏离本发明基本精神及不超出本发明实质内容所涉及范围的前提下对其可作种种变形加以实施。
Claims (1)
1.一种基于有界优化的机器人位姿递归估计方法,其特征在于,所述方法包括以下步骤:
步骤1:定义位姿估计系统的状态变量和量测变量,根据坐标变换和相机针孔模型建立位姿估计系统模型,过程如下:
1.1定义位姿估计系统的状态变量x
1.2当每个采样周期内机器人的速度保持常量时,根据机器人运动规律得到状态转移方程
其中t是采样周期,wk是系统噪声;
1.3定义位姿估计系统的量测变量zk
1.4机器人坐标系和相机坐标系的坐标变换
1.5相机针孔模型描述特征点在相机坐标系中和图像坐标系中的坐标变换
其中PX和PY分别是图像平面上沿X轴和Y轴的像素间距,F是相机焦距,这些参数都是通过相机标定测试获得的;
1.6根据坐标变换和相机针孔模型得到量测变量zk和状态变量x的非线性关系,并表示为量测方程
其中vk是量测噪声;
1.7过程噪声wk和量测噪声vk是上界未知的有界噪声
步骤2:设计递归估计器,泰勒展开非线性量测方程并将高阶展开项作为有界噪声,给出估计误差平方的一个上界,最小化估计误差平方上界得到最优估计器增益,过程如下:
2.1设计递归估计器
2.3得到非线性估计误差系统
2.4泰勒展开非线性项G(xk)
2.5定义增广噪声向量ξk
2.6定义矩阵GL,k
2.7定义矩阵BL,k
2.8非线性估计误差系统等价于
2.9设计估计误差平方的一个上界
其中Pk和Φk都是正定的矩阵;
2.10通过求解如下最优化问题得到最优估计增益Kk,使得最小化估计误差平方上界且确保上界成立
其中矩阵Φk、Pk和常变量θk都是优化变量,η是大于0的任意常数,此优化问题能够通过MATLAB的线性矩阵不等式工具箱求解。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910896428.1A CN110640789B (zh) | 2019-09-23 | 2019-09-23 | 基于有界优化的机器人位姿递归估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910896428.1A CN110640789B (zh) | 2019-09-23 | 2019-09-23 | 基于有界优化的机器人位姿递归估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110640789A true CN110640789A (zh) | 2020-01-03 |
CN110640789B CN110640789B (zh) | 2021-01-01 |
Family
ID=68992457
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910896428.1A Active CN110640789B (zh) | 2019-09-23 | 2019-09-23 | 基于有界优化的机器人位姿递归估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110640789B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111258309A (zh) * | 2020-01-15 | 2020-06-09 | 上海锵玫人工智能科技有限公司 | 一种用于城市消防机器人的灭火方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109129482A (zh) * | 2018-08-29 | 2019-01-04 | 武汉理工大学 | 一种动态补偿机器人直线导轨运动误差的方法 |
CN109240343A (zh) * | 2018-09-10 | 2019-01-18 | 西北工业大学 | 一种绳系机器人逼近目标位姿一体化控制方法 |
CN109910013A (zh) * | 2019-04-04 | 2019-06-21 | 江南大学 | 一种scara机器人加加速度连续有界的ptp轨迹规划方法 |
CN110160522A (zh) * | 2019-04-16 | 2019-08-23 | 浙江大学 | 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 |
-
2019
- 2019-09-23 CN CN201910896428.1A patent/CN110640789B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109129482A (zh) * | 2018-08-29 | 2019-01-04 | 武汉理工大学 | 一种动态补偿机器人直线导轨运动误差的方法 |
CN109240343A (zh) * | 2018-09-10 | 2019-01-18 | 西北工业大学 | 一种绳系机器人逼近目标位姿一体化控制方法 |
CN109910013A (zh) * | 2019-04-04 | 2019-06-21 | 江南大学 | 一种scara机器人加加速度连续有界的ptp轨迹规划方法 |
CN110160522A (zh) * | 2019-04-16 | 2019-08-23 | 浙江大学 | 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111258309A (zh) * | 2020-01-15 | 2020-06-09 | 上海锵玫人工智能科技有限公司 | 一种用于城市消防机器人的灭火方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110640789B (zh) | 2021-01-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111598946B (zh) | 一种物体位姿测量方法、装置及存储介质 | |
CN112837352B (zh) | 基于图像的数据处理方法、装置及设备、汽车、存储介质 | |
CN112907735B (zh) | 一种基于点云的柔性电缆识别与三维重建方法 | |
CN113253717B (zh) | 一种基于动态障碍物的室内移动机器人局部路径规划方法 | |
CN110345936B (zh) | 运动装置的轨迹数据处理方法及其处理系统 | |
CN110097599B (zh) | 一种基于部件模型表达的工件位姿估计方法 | |
Wang et al. | Vision-based tracking control of underactuated water surface robots without direct position measurement | |
CN107633105B (zh) | 一种基于改进混合蛙跳算法的四旋翼无人机参数辨识方法 | |
CN114721275B (zh) | 一种基于预设性能的视觉伺服机器人自适应跟踪控制方法 | |
CN110640789B (zh) | 基于有界优化的机器人位姿递归估计方法 | |
CN114549549B (zh) | 一种动态环境下基于实例分割的动态目标建模跟踪方法 | |
CN110340738B (zh) | 一种基于pca的机器人拉丝高铁白车身工件精确标定方法 | |
Chen et al. | A novel pan-tilt camera control approach for visual tracking | |
CN112986977B (zh) | 一种克服雷达扩展卡尔曼航迹滤波发散的方法 | |
CN110930444A (zh) | 一种基于双边优化的点云匹配方法、介质、终端和装置 | |
Tsai et al. | Robust visual tracking control system of a mobile robot based on a dual-Jacobian visual interaction model | |
CN107292445A (zh) | 一种空间非合作目标位姿预测方法 | |
CN112734823A (zh) | 一种基于图像的视觉伺服的雅可比矩阵深度估计方法 | |
Lin et al. | Pose estimation in industrial machine vision systems under sensing dynamics: A statistical learning approach | |
CN109542094B (zh) | 无期望图像的移动机器人视觉镇定控制 | |
Perrin et al. | Unknown object grasping using statistical pressure models | |
CN111504276B (zh) | 一种基于视觉投影尺度因子集的联合目标函数多推进器姿态角获取方法 | |
CN113763434A (zh) | 一种基于卡尔曼滤波多运动模型切换的目标轨迹预测方法 | |
CN114764830A (zh) | 一种基于四元数ekf和未标定手眼系统的物体位姿估算方法 | |
CN109934853B (zh) | 基于响应图置信区域自适应特征融合的相关滤波跟踪方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |