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

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

Info

Publication number
CN110470298A
CN110470298A CN201910597156.5A CN201910597156A CN110470298A CN 110470298 A CN110470298 A CN 110470298A CN 201910597156 A CN201910597156 A CN 201910597156A CN 110470298 A CN110470298 A CN 110470298A
Authority
CN
China
Prior art keywords
vector
moment
cost function
characteristic point
rolling time
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
Application number
CN201910597156.5A
Other languages
English (en)
Other versions
CN110470298B (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

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个特征点的坐标向量为物体坐标系中第j个特征点的坐标向量为图像平面上第j个特征点的投影坐标为其中，j∈{1,2，…，5}，X，Y，Z表示物体坐标系相对于相机坐标系的相对位置，φ，α，ψ表示滚动，俯仰和偏航参数的相对姿态，则第j个特征点在物体坐标系与相机坐标系之间关系为
其中，
根据投影定律，特征点在图像平面上的投影坐标与的变换关系为
其中，PX和PY分别为图像平面Xi和Yi轴上的像素间隔，F为焦距；
2)建立离散时间模型；
对于位姿估计，定义k时刻的状态向量为如下包含位姿和速度参数的形式
定义yk是k时刻的量测向量，初始状态x0为未知常数，uk为k时刻的控制向量，ξk为k时刻的系统噪声向量，ηk为k时刻的量测噪声向量，由此得离散时间状态方程：
xk+1＝Axk+Buk+ξk (4)
yk＝Cxk+ηk (5)
其中，为状态矩阵，B为控制输入矩阵，为与特征点相关的量测矩阵，
3)定义代价函数；
根据滚动时域估计将式(4)转化为如下方程：
其中，为状态向量xk-M-1基于k-1时刻的估计值，为的预测值，M为滚动时域窗口长度；定义方程(6)的代价函数如下
其中，和是欧几里得范数，μ是一个非负常数；
4)设计滚动时域估计器；
定义如下向量
对于给定的找到最优估计确保代价函数(7)达到最小
min Λk (8)
并满足约束
根据一阶KKT条件，对式(7)进行求导得
进一步由式(10)得最优估计器为
结合给定先验预测以及最优估计器(11)，得到最终的最优预测更新方程为：
本发明的技术构思为：首先，利用相机投影模型进行特征点变换，并考虑系统过程噪声及量测噪声，从而建立离散时间模型；然后，引入并最小化代价函数，得到最优预测；最后，结合给定的先验预测，得到最终的最优预测更新方程。
本发明的有益效果主要表现在：引入并最小化代价函数，得到最优预测，有利于更好地估计离散时间模型的状态；通过选择合适的自由参数u可以保证滚动时域估计器即使在高噪声影响下也能执行估计。
附图说明
图1是物体特征点在图像平面上的投影示意图。
具体实施方式
下面结合附图对本发明作进一步描述。
参照图1，一种基于滚动时域的机器人视觉伺服位姿估计方法，包括以下步骤：
1)特征点变换；
定义物体相对于相机的相对位姿为W＝[X,Y,Z,φ,α,ψ]T，相机坐标系中第j个特征点的坐标向量为物体坐标系中第j个特征点的坐标向量为图像平面上第j个特征点的投影坐标为其中，j∈{1,2，…，5}，X，Y，Z表示物体坐标系相对于相机坐标系的相对位置，φ，α，ψ表示滚动，俯仰和偏航参数的相对姿态，则第j个特征点在物体坐标系与相机坐标系之间关系为
其中，
根据投影定律，特征点在图像平面上的投影坐标与的变换关系为
其中，PX和PY分别为图像平面Xi和Yi轴上的像素间隔，F为焦距；
2)建立离散时间模型；
对于位姿估计，定义k时刻的状态向量为如下包含位姿和速度参数的形式
定义yk是k时刻的量测向量，初始状态x0为未知常数，uk为k时刻的控制向量，ξk为k时刻的系统噪声向量，ηk为k时刻的量测噪声向量，由此得离散时间状态方程：
xk+1＝Axk+Buk+ξk (4)
yk＝Cxk+ηk (5)
其中，为状态矩阵，B为控制输入矩阵，为与特征点相关的量测矩阵，
3)定义代价函数；
根据滚动时域估计将式(4)转化为如下方程：
其中，为状态向量xk-M-1基于k-1时刻的估计值，为的预测值，M为滚动时域窗口长度；定义方程(6)的代价函数如下
其中，和是欧几里得范数，μ是一个非负常数；
4)设计滚动时域估计器；
定义如下向量
对于给定的找到最优估计确保代价函数(7)达到最小
min Λk (8)
并满足约束
根据一阶KKT条件，对式(7)进行求导得
进一步由式(10)得最优估计器为
结合给定先验预测以及最优估计器(11)，得到最终的最优预测更新方程为：

Claims (1)

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

Cited By (2)

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

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106525049A (zh) * 2016-11-08 2017-03-22 山东大学 一种基于计算机视觉的四足机器人本体位姿跟踪方法
US20170284799A1 (en) * 2013-01-07 2017-10-05 Wexenergy Innovations Llc System and method of measuring distances related to an object utilizing ancillary objects
CN108711166A (zh) * 2018-04-12 2018-10-26 浙江工业大学 一种基于四旋翼无人机的单目相机尺度估计方法
CN109102525A (zh) * 2018-07-19 2018-12-28 浙江工业大学 一种基于自适应位姿估计的移动机器人跟随控制方法
CN109213175A (zh) * 2018-10-31 2019-01-15 浙江工业大学 一种基于原对偶神经网络的移动机器人视觉伺服轨迹跟踪预测控制方法
CN109495654A (zh) * 2018-12-29 2019-03-19 武汉大学 一种基于智能手机感知行人安全方法
CN109509230A (zh) * 2018-11-13 2019-03-22 武汉大学 一种应用于多镜头组合式全景相机的slam方法
CN109712172A (zh) * 2018-12-28 2019-05-03 哈尔滨工业大学 一种初始位姿测量结合目标追踪的位姿测量方法

Patent Citations (8)

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

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
杨东方 等: "一种新的惯性_视觉组合系统初始对准的时域约束方法", 《仪 器 仪 表 学 报》 *

Cited By (4)

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

Also Published As

Publication number Publication date
CN110470298B (zh) 2021-02-26

Similar Documents

Publication Publication Date Title
Li et al. Adaptive trajectory tracking of nonholonomic mobile robots using vision-based position and velocity estimation
CN109102525B (zh) 一种基于自适应位姿估计的移动机器人跟随控制方法
CN102722697B (zh) 一种无人飞行器视觉自主导引着陆的目标跟踪方法
CN110470298A (zh) 一种基于滚动时域的机器人视觉伺服位姿估计方法
CN110744541A (zh) 一种视觉引导的水下机械臂控制方法
Wahrmann et al. Fast object approximation for real-time 3D obstacle avoidance with biped robots
Kragic et al. Model based techniques for robotic servoing and grasping
Kuleshov et al. Machine learning in appearance-based robot self-localization
Hafez Visual servo control by optimizing hybrid objective function with visibility and path constraints
Thotakuri et al. Survey on robot vision: techniques, tools and methodologies
Suh et al. A visual servoing algorithm using fuzzy logics and fuzzy-neural networks
Kawasaki et al. Motion estimation for non-overlapping cameras by improvement of feature points matching based on urban 3D structure
Adams et al. An empirical comparison of methods for image-based motion estimation
Wu et al. Monocular vision SLAM based on key feature points selection
Sinisterra et al. Stereo vision-based target tracking system for an USV
Fan et al. Optical flow based speed estimation in AUV target tracking
Lin et al. Vision aided motion estimation for unmanned helicopters in GPS denied environments
Funakubo et al. Verification of illumination tolerance for clothes recognition
Sun et al. Precise grabbing of overlapping objects system based on end-to-end deep neural network
Deng et al. Decoupled EKF for simultaneous target model and relative pose estimation using feature points
Zou et al. Vision-guided mobile robot navigation
Zhang et al. Triangle codes and tracer lights based absolute positioning method for terminal visual docking of autonomous underwater vehicles
Hsu et al. General dual wiimote 3D localization scheme: sensitivity analysis and its application on robot arm attitude detection
Xie et al. Reduced-order Observer Design for Structure and Motion Estimation
Ge A real-time stereo visual servoing for moving object grasping based parallel algorithms

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