CN110640789B - 基于有界优化的机器人位姿递归估计方法 - Google Patents

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

Info

Publication number
CN110640789B
CN110640789B CN201910896428.1A CN201910896428A CN110640789B CN 110640789 B CN110640789 B CN 110640789B CN 201910896428 A CN201910896428 A CN 201910896428A CN 110640789 B CN110640789 B CN 110640789B
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.)
Active
Application number
CN201910896428.1A
Other languages
English (en)
Other versions
CN110640789A (zh
Inventor
陈博
章宇晨
俞立
张文安
洪榛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN201910896428.1A priority Critical patent/CN110640789B/zh
Publication of CN110640789A publication Critical patent/CN110640789A/zh
Application granted granted Critical
Publication of CN110640789B publication Critical patent/CN110640789B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
Figure BDA0002210399810000021
其中
Figure BDA0002210399810000022
是机器人在相机坐标系中的位置变量，
Figure BDA0002210399810000023
是机器人在相机坐标系中的方向变量；
1.2当每个采样周期内机器人的速度保持常量时，根据机器人运动规律得到状态转移方程
Figure BDA0002210399810000024
其中t是采样周期，wk是系统噪声；
1.3定义位姿估计系统的量测变量zk
Figure BDA0002210399810000025
其中p是特征点数目，
Figure BDA0002210399810000026
和
Figure BDA0002210399810000027
分别是第i个标定点在图像坐标系中的横纵坐标；
1.4机器人坐标系和相机坐标系的坐标变换
Figure BDA0002210399810000028
Figure BDA0002210399810000029
其中
Figure BDA00022103998100000210
是第i个特征点在机器人坐标系中的坐标，通过提前测量得到，
Figure BDA00022103998100000211
是第i个特征点在相机坐标系中的坐标；R(Θ)是旋转矩阵；
1.5相机针孔模型描述特征点在相机坐标系中和图像坐标系中的坐标变换
Figure BDA00022103998100000212
其中PX和PY分别是图像平面上沿X轴和Y轴的像素间距，F是相机焦距，这些参数都是通过相机标定测试获得的；
1.6根据坐标变换和相机针孔模型得到量测变量zk和状态变量x的非线性关系，并表示为量测方程
Figure BDA0002210399810000031
其中vk是量测噪声；
1.7过程噪声wk和量测噪声vk是上界未知的有界噪声
Figure BDA0002210399810000032
其中δw和
Figure BDA0002210399810000033
分别是过程噪声和量测噪声的上界，且是未知的；
步骤2：设计递归估计器，泰勒展开非线性量测方程并将高阶展开项作为有界噪声，给出估计误差平方的一个上界，最小化估计误差平方上界得到最优估计器增益，过程如下：
2.1设计递归估计器
Figure BDA0002210399810000034
2.2定义估计误差
Figure BDA0002210399810000035
Figure BDA0002210399810000036
2.3得到非线性估计误差系统
Figure BDA0002210399810000037
2.4泰勒展开非线性项G(xk)
Figure BDA0002210399810000038
Figure BDA0002210399810000039
其中Hk是非线性项的雅克比矩阵，
Figure BDA00022103998100000310
是泰勒展开后的高阶项，这里看成有界的噪声扰动，同时引入有界噪声项
Figure BDA00022103998100000311
表示
Figure BDA00022103998100000312
和vk的噪声影响；
2.5定义增广噪声向量ξk
Figure BDA00022103998100000313
2.6定义矩阵GL，k
Figure BDA00022103998100000314
2.7定义矩阵BL，k
Figure BDA00022103998100000315
2.8非线性估计误差系统等价于
Figure BDA0002210399810000041
2.9设计估计误差平方的一个上界
Figure BDA0002210399810000042
其中Pk和Φk都是正定的矩阵；
2.10通过求解如下最优化问题得到最优估计增益Kk，使得最小化估计误差平方上界且确保上界成立
Figure BDA0002210399810000043
其中矩阵Φk、Pk和常变量θk都是优化变量，η是大于0的任意常数，此优化问题能够通过MATLAB的线性矩阵不等式工具箱求解。
本发明的技术构思：针对噪声统计特性位姿下的机器人位姿估计问题，考虑相机针孔模型和坐标变换过程建立位姿估计系统模型，设计有界递归估计器对机器人姿态进行估计。通过泰勒展开非线性量测方程并将高阶展开项作为有界噪声，给出估计误差平方的一个上界，构造最优化问题求解最优估计器增益，使得估计误差平方上界最小化且确保估计误差上界成立。本发明所设计的有界递归估计器能有效克服系统噪声服从高斯分布的限制性条件，实现噪声统计特性未知情况下机器人位姿的实时估计。
本发明的优点为：避免基于扩展卡尔曼滤波的位姿估计方法对高斯白噪声的假设，实现噪声统计特性未知情况下的机器人位姿实时估计。
附图说明
图1为本发明估计方法流程图；
图2为本发明机器人在X轴上的真实轨迹和有界递归优化的估计轨迹对比图；
图3为本发明机器人在Y轴上的真实轨迹和有界递归优化的估计轨迹对比图；
图4为本发明机器人在Z轴上的真实轨迹和有界递归优化的估计轨迹对比图；
图5为本发明机器人在翻滚角上的真实值和有界递归优化的估计值对比图；
图6为本发明机器人在俯仰角上的真实值和有界递归优化的估计值对比图；
图7为本发明机器人在偏航角上的真实值和有界递归优化的估计值对比图；
具体实施方式
下面结合附图对本发明做进一步说明。
参照图1-图7，一种基于有界优化的机器人位姿递归估计方法，所述估计方法包括如下步骤：
步骤1：定义位姿估计系统的状态变量和量测变量，根据坐标变换和相机针孔模型建立位姿估计系统模型，过程如下：
1.1定义位姿估计系统的状态变量x
Figure BDA0002210399810000051
其中
Figure BDA0002210399810000052
是机器人在相机坐标系中的位置变量，
Figure BDA0002210399810000053
是机器人在相机坐标系中的方向变量；
1.2当每个采样周期内机器人的速度保持常量时，根据机器人运动规律得到状态转移方程
Figure BDA0002210399810000054
其中t是采样周期，wk是系统噪声；
1.3定义位姿估计系统的量测变量zk
Figure BDA0002210399810000055
其中p是特征点数目，
Figure BDA0002210399810000056
和
Figure BDA0002210399810000057
分别是第i个标定点在图像坐标系中的横纵坐标；
1.4机器人坐标系和相机坐标系的坐标变换
Figure BDA0002210399810000058
Figure BDA0002210399810000059
其中
Figure BDA00022103998100000510
是第i个特征点在机器人坐标系中的坐标，通过提前测量得到，
Figure BDA00022103998100000511
是第i个特征点在相机坐标系中的坐标；R(Θ)是旋转矩阵；
1.5相机针孔模型描述特征点在相机坐标系中和图像坐标系中的坐标变换
Figure BDA0002210399810000061
其中PX和PY分别是图像平面上沿X轴和Y轴的像素间距，F是相机焦距，这些参数都是通过相机标定测试获得的；
1.6根据坐标变换和相机针孔模型得到量测变量zk和状态变量x的非线性关系，并表示为量测方程
Figure BDA0002210399810000062
其中vk是量测噪声；
1.7过程噪声wk和量测噪声vk是上界未知的有界噪声
Figure BDA0002210399810000063
其中δw和
Figure BDA0002210399810000064
分别是过程噪声和量测噪声的上界，且是未知的；
步骤2：设计递归估计器，泰勒展开非线性量测方程并将高阶展开项作为有界噪声，给出估计误差平方的一个上界，最小化估计误差平方上界得到最优估计器增益，过程如下：
2.1设计递归估计器
Figure BDA0002210399810000065
2.2定义估计误差
Figure BDA0002210399810000066
Figure BDA0002210399810000067
2.3得到非线性估计误差系统
Figure BDA0002210399810000068
2.4泰勒展开非线性项G(xk)
Figure BDA0002210399810000069
Figure BDA00022103998100000610
其中Hk是非线性项的雅克比矩阵，
Figure BDA00022103998100000611
是泰勒展开后的高阶项，这里看成有界的噪声扰动，同时引入有界噪声项
Figure BDA00022103998100000612
表示
Figure BDA00022103998100000613
和vk的噪声影响；
2.5定义增广噪声向量ξk
Figure BDA00022103998100000614
2.6定义矩阵GL，k
Figure BDA0002210399810000071
2.7定义矩阵BL，k
Figure BDA0002210399810000072
2.8非线性估计误差系统等价于
Figure BDA0002210399810000073
2.9设计估计误差平方的一个上界
Figure BDA0002210399810000074
其中Pk和Φk都是正定的矩阵；
2.10通过求解如下最优化问题得到最优估计增益Kk，使得最小化估计误差平方上界且确保上界成立
Figure BDA0002210399810000075
其中矩阵Φk、Pk和常变量θk都是优化变量，η是大于0的任意常数，此优化问题能够通过MATLAB的线性矩阵不等式工具箱求解。
本发明针对机器人位姿估计问题，基于坐标变换和相机针孔模型建立位姿估计系统模型，结合有界优化的思想，设计一种基于有界优化的机器人位姿递归估计方法，克服传统机器人位姿估计方法对噪声的高斯假设，实现噪声统计特性未知情况下的机器人位姿实时估计。为验证所提方法的有效性，本发明给出所设计有界递归优化方法对机器人位姿的估计效果图。
为了实现机器人位姿变化和相机观测的效果，将系统采用时间设置为t＝0.05s，并且设置相机参数为
Figure BDA0002210399810000076
和
Figure BDA0002210399810000077
在机器人上标定四个特征点，其对应于机器人坐标系中的坐标分别为(-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
Figure FDA0002635250700000011
其中
Figure FDA0002635250700000012
是机器人在相机坐标系中的位置变量，
Figure FDA0002635250700000013
是机器人在相机坐标系中的方向变量；
1.2当每个采样周期内机器人的速度保持常量时，根据机器人运动规律得到状态转移方程
Figure FDA0002635250700000014
其中t是采样周期，wk是系统噪声；
1.3定义位姿估计系统的量测变量zk
Figure FDA0002635250700000015
其中p是特征点数目,
Figure FDA0002635250700000016
和
Figure FDA0002635250700000017
分别是第i个标定点在图像坐标系中的横纵坐标；
1.4机器人坐标系和相机坐标系的坐标变换
Figure FDA0002635250700000018
Figure FDA0002635250700000019
其中
Figure FDA00026352507000000110
是第i个特征点在机器人坐标系中的坐标，通过提前测量得到，
Figure FDA00026352507000000111
是第i个特征点在相机坐标系中的坐标；R(Θ)是旋转矩阵；
1.5相机针孔模型描述特征点在相机坐标系中和图像坐标系中的坐标变换
Figure FDA00026352507000000112
其中PX和PY分别是图像平面上沿X轴和Y轴的像素间距，F是相机焦距，这些参数都是通过相机标定测试获得的；
1.6根据坐标变换和相机针孔模型得到量测变量zk和状态变量x的非线性关系，并表示为量测方程
Figure FDA0002635250700000021
其中vk是量测噪声；
1.7系统噪声wk和量测噪声vk是上界未知的有界噪声
Figure FDA0002635250700000022
其中δw和
Figure FDA00026352507000000215
分别是系统噪声和量测噪声的上界，且是未知的；
步骤2：设计递归估计器，泰勒展开非线性量测方程并将高阶展开项作为有界噪声，给出估计误差平方的一个上界，最小化估计误差平方上界得到最优估计器增益，过程如下：
2.1设计递归估计器
Figure FDA0002635250700000023
2.2定义估计误差
Figure FDA0002635250700000024
Figure FDA0002635250700000025
2.3得到非线性估计误差系统
Figure FDA0002635250700000026
2.4泰勒展开非线性项G(xk)
Figure FDA0002635250700000027
Figure FDA0002635250700000028
其中Hk是非线性项的雅克比矩阵，
Figure FDA0002635250700000029
是泰勒展开后的高阶项，这里看成有界的噪声扰动，同时引入有界噪声项
Figure FDA00026352507000000210
表示
Figure FDA00026352507000000211
和vk的噪声影响；
2.5定义增广噪声向量ξk
Figure FDA00026352507000000212
2.6定义矩阵GL,k
Figure FDA00026352507000000213
2.7定义矩阵BL,k
Figure FDA00026352507000000214
2.8非线性估计误差系统等价于
Figure FDA0002635250700000031
2.9设计估计误差平方的一个上界
Figure FDA0002635250700000032
其中Pk和Φk都是正定的矩阵；
2.10通过求解如下最优化问题得到最优估计增益Kk，使得最小化估计误差平方上界且确保上界成立
Figure FDA0002635250700000033
Figure FDA0002635250700000034
其中矩阵Φk、Pk和常变量
Figure FDA0002635250700000035
都是优化变量，η是大于0的任意常数，此优化问题能够通过MATLAB的线性矩阵不等式工具箱求解。
CN201910896428.1A 2019-09-23 2019-09-23 基于有界优化的机器人位姿递归估计方法 Active CN110640789B (zh)

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 CN110640789A (zh) 2020-01-03
CN110640789B true 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)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111258309A (zh) * 2020-01-15 2020-06-09 上海锵玫人工智能科技有限公司 一种用于城市消防机器人的灭火方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109129482B (zh) * 2018-08-29 2021-05-25 武汉理工大学 一种动态补偿机器人直线导轨运动误差的方法
CN109240343B (zh) * 2018-09-10 2021-11-16 西北工业大学 一种绳系机器人逼近目标位姿一体化控制方法
CN109910013A (zh) * 2019-04-04 2019-06-21 江南大学 一种scara机器人加加速度连续有界的ptp轨迹规划方法
CN110160522A (zh) * 2019-04-16 2019-08-23 浙江大学 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法

Also Published As

Publication number Publication date
CN110640789A (zh) 2020-01-03

Similar Documents

Publication Publication Date Title
CN109885883B (zh) 一种基于gk聚类算法模型预测的无人车横向运动的控制方法
CN108235725B (zh) 基于云端的轨迹地图生成方法、装置、设备及应用程序
CN111598946B (zh) 一种物体位姿测量方法、装置及存储介质
CN105261060A (zh) 基于点云压缩和惯性导航的移动场景实时三维重构方法
CN112837352B (zh) 基于图像的数据处理方法、装置及设备、汽车、存储介质
CN112669385B (zh) 基于三维点云特征的工业机器人工件识别与位姿估计方法
CN113253717B (zh) 一种基于动态障碍物的室内移动机器人局部路径规划方法
CN112907735B (zh) 一种基于点云的柔性电缆识别与三维重建方法
CN110345936B (zh) 运动装置的轨迹数据处理方法及其处理系统
CN110097599B (zh) 一种基于部件模型表达的工件位姿估计方法
Wang et al. Vision-based tracking control of underactuated water surface robots without direct position measurement
CN114721275B (zh) 一种基于预设性能的视觉伺服机器人自适应跟踪控制方法
CN110640789B (zh) 基于有界优化的机器人位姿递归估计方法
CN114549549B (zh) 一种动态环境下基于实例分割的动态目标建模跟踪方法
CN110340738B (zh) 一种基于pca的机器人拉丝高铁白车身工件精确标定方法
CN112000135A (zh) 基于人脸最高温点特征反馈的三轴云台视觉伺服控制方法
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
CN116579989B (zh) 一种基于深度相机的隧道打孔倾角修正方法
CN112734823A (zh) 一种基于图像的视觉伺服的雅可比矩阵深度估计方法
CN107292445A (zh) 一种空间非合作目标位姿预测方法
CN109542094B (zh) 无期望图像的移动机器人视觉镇定控制
CN108469729B (zh) 一种基于rgb-d信息的人体目标识别与跟随方法

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