CN110470298B - 一种基于滚动时域的机器人视觉伺服位姿估计方法 - Google Patents

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

Info

Publication number
CN110470298B
CN110470298B CN201910597156.5A CN201910597156A CN110470298B CN 110470298 B CN110470298 B CN 110470298B CN 201910597156 A CN201910597156 A CN 201910597156A CN 110470298 B CN110470298 B CN 110470298B
Authority
CN
China
Prior art keywords
vector
time domain
time
equation
coordinate system
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
CN201910597156.5A
Other languages
English (en)
Other versions
CN110470298A (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 CN201910597156.5A priority Critical patent/CN110470298B/zh
Publication of CN110470298A publication Critical patent/CN110470298A/zh
Application granted granted Critical
Publication of CN110470298B publication Critical patent/CN110470298B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)

Abstract

一种基于滚动时域的机器人视觉伺服位姿估计方法，包括以下步骤：1)利用相机投影模型进行特征点变换；2)建立离散时间模型；3)根据离散时间模型以及滚动时域策略，定义代价函数；4)确保代价函数达到最小，从而设计最优滚动时域估计器。本发明提供了一种基于滚动时域的机器人视觉伺服位姿估计方法，通过引入滚动时域目标函数来最小化代价函数，并确定最优预测方程的设计方案。

Description

一种基于滚动时域的机器人视觉伺服位姿估计方法
技术领域
本发明涉及机器人视觉伺服系统，尤其涉及的是基于滚动时域的位姿估计方法。
背景技术
随着科学技术和控制技术的发展，计算机视觉在各个领域已有广泛的运用，其中，机器人视觉伺服(RVS)系统的位姿估计问题一直备受广泛关注。位姿估计是指使用图像信息来确定相机相对于物体坐标系的位置和姿态，机器人系统能利用位置和姿态进行机器人运动的实时控制。针对机器人视觉伺服系统位姿估计的研究，不仅可以丰富机器人位姿估计的理论成果，还可以满足多领域对位姿估计技术越来越高的要求，具有实际的理论和工程意义。
然而在实际环境中，RVS系统的位姿估计有两个主要困难，分别是位姿估计的效率以及其鲁棒性。同时机器人在运动过程中总是存在噪声干扰，机器人的位姿估计问题实际上就成为一个带有噪声干扰的状态估计问题。目前主要是应用卡尔曼滤波方法来解决这些困难。对于线性系统的状态估计卡尔曼滤波已经能够很好地解决,但是在非线性系统中就不那么容易.目前已有的解决非线性问题的方法也都是线性系统卡尔曼滤波方法的扩展,如最常用的扩展卡尔曼滤波(extended Kalman filter,EKF),无迹卡尔曼滤波(unscentedKalman filter,UKF)等。Wang等在论文(3D relative position and orientationestimation using Kalman filtering for robot control)中，针对机器人位姿估计问题，提出了一种扩展卡尔曼滤波(EKF)方法。Shademan等在论文(Sensitivity analysis ofEKF and Iterated EKF for position-based visual servoing)中，主要是采用了迭代卡尔曼滤波(I-EKF)算法并与扩展卡尔曼滤波(EKF)算法进行对比。Ficocelli等在论文(Adaptive filtering for pose estimation in visual servoing)中，利用自适应卡尔曼滤波(A-EKF)算法来实现机器人位姿估计，但是，上述这些方法都没有完全解决RVS位姿估计的效率以及鲁棒性问题。因此，针对基于滚动时域的机器人视觉伺服系统位姿估计方法的研究很有必要。
发明内容
为了克服现有技术无法解决机器人视觉伺服位姿估计问题的不足，本发明提供一种基于滚动时域的机器人视觉伺服位姿估计方法，通过引入滚动时域目标函数来最小化代价函数，并确定最优预测方程的设计方案。
本发明解决其技术问题所采用的技术方案是：
一种基于滚动时域的机器人视觉伺服位姿估计方法，所述方法包括以下步骤：
1)特征点变换；
定义物体相对于相机的相对位姿为W＝[X,Y,Z,φ,α,ψ]T，相机坐标系中第j个特征点的坐标向量为
Figure BDA0002117954700000021
物体坐标系中第j个特征点的坐标向量为
Figure BDA0002117954700000022
图像平面上第j个特征点的投影坐标为
Figure BDA0002117954700000023
其中，j∈{1,2，…，5}，X，Y，Z表示物体坐标系相对于相机坐标系的相对位置，φ，α，ψ表示滚动，俯仰和偏航参数的相对姿态，则第j个特征点在物体坐标系与相机坐标系之间关系为
Figure BDA0002117954700000024
其中，
Figure BDA0002117954700000025
根据投影定律，特征点在图像平面上的投影坐标与
Figure BDA0002117954700000026
的变换关系为
Figure BDA0002117954700000028
其中，PX和PY分别为图像平面Xi和Yi轴上的像素间隔，F为焦距；
2)建立离散时间模型；
对于位姿估计，定义k时刻的状态向量为如下包含位姿和速度参数的形式
Figure BDA0002117954700000027
定义yk是k时刻的量测向量，初始状态x0为未知常数，uk为k时刻的控制向量，ξk为k时刻的系统噪声向量，ηk为k时刻的量测噪声向量，由此得离散时间状态方程：
xk+1＝Axk+Buk+ξk (4)
yk＝Cxk+ηk (5)
其中，
Figure BDA0002117954700000031
为状态矩阵，
Figure BDA0002117954700000032
B为控制输入矩阵，
Figure BDA0002117954700000033
为与特征点相关的量测矩阵，
Figure BDA0002117954700000034
3)定义代价函数；
根据滚动时域估计将式(4)转化为如下方程：
Figure BDA0002117954700000035
其中，
Figure BDA0002117954700000036
为状态向量xk-M-1基于k-1时刻的估计值，
Figure BDA0002117954700000037
为
Figure BDA0002117954700000038
的预测值，M为滚动时域窗口长度；定义方程(6)的代价函数如下
Figure BDA0002117954700000039
其中，
Figure BDA00021179547000000310
和
Figure BDA00021179547000000311
是欧几里得范数，μ是一个非负常数；
4)设计滚动时域估计器；
定义如下向量
Figure BDA00021179547000000312
Figure BDA00021179547000000313
对于给定的
Figure BDA0002117954700000041
找到最优估计
Figure BDA0002117954700000042
确保代价函数(7)达到最小
min Λk (8)
并满足约束
Figure BDA0002117954700000043
根据一阶KKT条件，对式(7)进行求导得
Figure BDA0002117954700000044
进一步由式(10)得最优估计器为
Figure BDA0002117954700000045
结合给定先验预测
Figure BDA0002117954700000046
以及最优估计器(11)，得到最终的最优预测更新方程为：
Figure BDA0002117954700000047
本发明的技术构思为：首先，利用相机投影模型进行特征点变换，并考虑系统过程噪声及量测噪声，从而建立离散时间模型；然后，引入并最小化代价函数，得到最优预测；最后，结合给定的先验预测，得到最终的最优预测更新方程。
本发明的有益效果主要表现在：引入并最小化代价函数，得到最优预测，有利于更好地估计离散时间模型的状态；通过选择合适的自由参数u可以保证滚动时域估计器即使在高噪声影响下也能执行估计。
附图说明
图1是物体特征点在图像平面上的投影示意图。
具体实施方式
下面结合附图对本发明作进一步描述。
参照图1，一种基于滚动时域的机器人视觉伺服位姿估计方法，包括以下步骤：
1)特征点变换；
定义物体相对于相机的相对位姿为W＝[X,Y,Z,φ,α,ψ]T，相机坐标系中第j个特征点的坐标向量为
Figure BDA0002117954700000048
物体坐标系中第j个特征点的坐标向量为
Figure BDA0002117954700000051
图像平面上第j个特征点的投影坐标为
Figure BDA0002117954700000052
其中，j∈{1,2，…，5}，X，Y，Z表示物体坐标系相对于相机坐标系的相对位置，φ，α，ψ表示滚动，俯仰和偏航参数的相对姿态，则第j个特征点在物体坐标系与相机坐标系之间关系为
Figure BDA0002117954700000053
其中，
Figure BDA0002117954700000054
根据投影定律，特征点在图像平面上的投影坐标与
Figure BDA0002117954700000055
的变换关系为
Figure BDA0002117954700000056
其中，PX和PY分别为图像平面Xi和Yi轴上的像素间隔，F为焦距；
2)建立离散时间模型；
对于位姿估计，定义k时刻的状态向量为如下包含位姿和速度参数的形式
Figure BDA0002117954700000057
定义yk是k时刻的量测向量，初始状态x0为未知常数，uk为k时刻的控制向量，ξk为k时刻的系统噪声向量，ηk为k时刻的量测噪声向量，由此得离散时间状态方程：
xk+1＝Axk+Buk+ξk (4)
yk＝Cxk+ηk (5)
其中，
Figure BDA0002117954700000058
为状态矩阵，
Figure BDA0002117954700000059
B为控制输入矩阵，
Figure BDA00021179547000000510
为与特征点相关的量测矩阵，
Figure BDA00021179547000000511
3)定义代价函数；
根据滚动时域估计将式(4)转化为如下方程：
Figure BDA0002117954700000061
其中，
Figure BDA0002117954700000062
为状态向量xk-M-1基于k-1时刻的估计值，
Figure BDA0002117954700000063
为
Figure BDA0002117954700000064
的预测值，M为滚动时域窗口长度；定义方程(6)的代价函数如下
Figure BDA0002117954700000065
其中，
Figure BDA0002117954700000066
和
Figure BDA0002117954700000067
是欧几里得范数，μ是一个非负常数；
4)设计滚动时域估计器；
定义如下向量
Figure BDA0002117954700000068
Figure BDA0002117954700000069
对于给定的
Figure BDA00021179547000000610
找到最优估计
Figure BDA00021179547000000611
确保代价函数(7)达到最小
min Λk (8)
并满足约束
Figure BDA00021179547000000612
根据一阶KKT条件，对式(7)进行求导得
Figure BDA00021179547000000613
进一步由式(10)得最优估计器为
Figure BDA00021179547000000614
结合给定先验预测
Figure BDA00021179547000000615
以及最优估计器(11)，得到最终的最优预测更新方程为：
Figure BDA0002117954700000071

Claims (1)

1.一种基于滚动时域的机器人视觉伺服位姿估计方法，所述方法包括以下步骤：
1)特征点变换；
定义物体相对于相机的相对位姿为W＝[X,Y,Z,φ,α,ψ]T，相机坐标系中第j个特征点的坐标向量为
Figure FDA0002117954690000011
物体坐标系中第j个特征点的坐标向量为
Figure FDA0002117954690000012
图像平面上第j个特征点的投影坐标为
Figure FDA0002117954690000013
其中，j∈{1,2，…，5}，X，Y，Z表示物体坐标系相对于相机坐标系的相对位置，φ，α，ψ表示滚动，俯仰和偏航参数的相对姿态，则第j个特征点在物体坐标系与相机坐标系之间关系为
Figure FDA0002117954690000014
其中，
Figure FDA0002117954690000015
根据投影定律，特征点在图像平面上的投影坐标与
Figure FDA0002117954690000016
的变换关系为
Figure FDA0002117954690000017
其中，PX和PY分别为图像平面Xi和Yi轴上的像素间隔，F为焦距；
2)建立离散时间模型；
对于位姿估计，定义k时刻的状态向量为如下包含位姿和速度参数的形式
Figure FDA0002117954690000018
定义yk是k时刻的量测向量，初始状态x0为未知常数，uk为k时刻的控制向量，ξk为k时刻的系统噪声向量，ηk为k时刻的量测噪声向量，由此得离散时间状态方程：
xk+1＝Axk+Buk+ξk (4)
yk＝Cxk+ηk (5)
其中，
Figure FDA0002117954690000021
为状态矩阵，
Figure FDA0002117954690000022
B为控制输入矩阵，
Figure FDA0002117954690000023
为与特征点相关的量测矩阵，
Figure FDA0002117954690000024
3)定义代价函数；
根据滚动时域估计将式(4)转化为如下方程：
Figure FDA0002117954690000025
其中，
Figure FDA0002117954690000026
为状态向量xk-M-1基于k-1时刻的估计值，
Figure FDA0002117954690000027
为
Figure FDA0002117954690000028
的预测值，M为滚动时域窗口长度；定义方程(6)的代价函数如下
Figure FDA0002117954690000029
其中，
Figure FDA00021179546900000210
和
Figure FDA00021179546900000211
是欧几里得范数，μ是一个非负常数；
4)设计滚动时域估计器；
定义如下向量
Figure FDA00021179546900000212
Figure FDA00021179546900000213
对于给定的
Figure FDA00021179546900000214
找到最优估计
Figure FDA00021179546900000215
确保代价函数(7)达到最小
minΛk (8)
并满足约束
Figure FDA00021179546900000216
根据一阶KKT条件，对式(7)进行求导得
Figure FDA00021179546900000217
进一步由式(10)得最优估计器为
Figure FDA0002117954690000031
结合给定先验预测
Figure FDA0002117954690000032
以及最优估计器(11)，得到最终的最优预测更新方程为：
Figure FDA0002117954690000033
CN201910597156.5A 2019-07-04 2019-07-04 一种基于滚动时域的机器人视觉伺服位姿估计方法 Active CN110470298B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910597156.5A CN110470298B (zh) 2019-07-04 2019-07-04 一种基于滚动时域的机器人视觉伺服位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910597156.5A CN110470298B (zh) 2019-07-04 2019-07-04 一种基于滚动时域的机器人视觉伺服位姿估计方法

Publications (2)

Publication Number Publication Date
CN110470298A CN110470298A (zh) 2019-11-19
CN110470298B true CN110470298B (zh) 2021-02-26

Family

ID=68506780

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910597156.5A Active CN110470298B (zh) 2019-07-04 2019-07-04 一种基于滚动时域的机器人视觉伺服位姿估计方法

Country Status (1)

Country Link
CN (1) CN110470298B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113822996B (zh) * 2021-11-22 2022-02-22 之江实验室 机器人的位姿估计方法及装置、电子设备、存储介质
CN117506937B (zh) * 2024-01-04 2024-03-12 中铁十四局集团大盾构工程有限公司 一种基于多阶段视觉伺服控制的焊件自主摆放方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106525049A (zh) * 2016-11-08 2017-03-22 山东大学 一种基于计算机视觉的四足机器人本体位姿跟踪方法
CN108711166A (zh) * 2018-04-12 2018-10-26 浙江工业大学 一种基于四旋翼无人机的单目相机尺度估计方法
CN109102525A (zh) * 2018-07-19 2018-12-28 浙江工业大学 一种基于自适应位姿估计的移动机器人跟随控制方法
CN109213175A (zh) * 2018-10-31 2019-01-15 浙江工业大学 一种基于原对偶神经网络的移动机器人视觉伺服轨迹跟踪预测控制方法
CN109509230A (zh) * 2018-11-13 2019-03-22 武汉大学 一种应用于多镜头组合式全景相机的slam方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9691163B2 (en) * 2013-01-07 2017-06-27 Wexenergy Innovations Llc System and method of measuring distances related to an object utilizing ancillary objects
CN109712172A (zh) * 2018-12-28 2019-05-03 哈尔滨工业大学 一种初始位姿测量结合目标追踪的位姿测量方法
CN109495654B (zh) * 2018-12-29 2021-08-03 武汉大学 一种基于智能手机感知行人安全方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106525049A (zh) * 2016-11-08 2017-03-22 山东大学 一种基于计算机视觉的四足机器人本体位姿跟踪方法
CN108711166A (zh) * 2018-04-12 2018-10-26 浙江工业大学 一种基于四旋翼无人机的单目相机尺度估计方法
CN109102525A (zh) * 2018-07-19 2018-12-28 浙江工业大学 一种基于自适应位姿估计的移动机器人跟随控制方法
CN109213175A (zh) * 2018-10-31 2019-01-15 浙江工业大学 一种基于原对偶神经网络的移动机器人视觉伺服轨迹跟踪预测控制方法
CN109509230A (zh) * 2018-11-13 2019-03-22 武汉大学 一种应用于多镜头组合式全景相机的slam方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种新的惯性_视觉组合系统初始对准的时域约束方法;杨东方 等;《仪 器 仪 表 学 报》;20140430;第35卷(第4期);第788-793页 *

Also Published As

Publication number Publication date
CN110470298A (zh) 2019-11-19

Similar Documents

Publication Publication Date Title
Li et al. Adaptive trajectory tracking of nonholonomic mobile robots using vision-based position and velocity estimation
CN108416840B (zh) 一种基于单目相机的三维场景稠密重建方法
CN109102525B (zh) 一种基于自适应位姿估计的移动机器人跟随控制方法
Lamraoui et al. Path following control of fully-actuated autonomous underwater vehicle in presence of fast-varying disturbances
Kosmopoulos Robust Jacobian matrix estimation for image-based visual servoing
Qiu et al. Visual servo tracking of wheeled mobile robots with unknown extrinsic parameters
CN110470298B (zh) 一种基于滚动时域的机器人视觉伺服位姿估计方法
Qiu et al. Concurrent-learning-based visual servo tracking and scene identification of mobile robots
CN112734823B (zh) 一种基于图像的视觉伺服的雅可比矩阵深度估计方法
Kuleshov et al. Machine learning in appearance-based robot self-localization
CN111709095A (zh) 一种面向复杂曲面6d虚拟夹具构造方法
Demim et al. Visual SVSF-SLAM algorithm based on adaptive boundary layer width
CN108469729B (zh) 一种基于rgb-d信息的人体目标识别与跟随方法
CN115219492B (zh) 一种三维物体的外观图像采集方法及装置
CN109542094B (zh) 无期望图像的移动机器人视觉镇定控制
Briales et al. Initialization of 3d pose graph optimization using lagrangian duality
Ji et al. Adaptive correction of landmark for visual homing in mobile vehicles
CN112330698B (zh) 一种改进的几何活动轮廓的图像分割方法
Wu et al. Monocular vision SLAM based on key feature points selection
CN114434441A (zh) 基于自适应动态规划的移动机器人视觉伺服跟踪控制方法
CN114519813A (zh) 一种机械臂目标抓取方法及系统
Kiddee et al. A real-time and robust feature detection method using hierarchical strategy and modified Kalman filter for thick plate seam tracking
CN113359626A (zh) 一种多机器人系统的有限时间分层控制的方法
LIU et al. Robot intelligence for real world applications
Zhang et al. Research on Moving Arm Grasping Based on Computer Vision

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