CN113298881B - 单目相机-imu-机械臂的空间联合标定方法 - Google Patents

单目相机-imu-机械臂的空间联合标定方法 Download PDF

Info

Publication number
CN113298881B
CN113298881B CN202110584637.XA CN202110584637A CN113298881B CN 113298881 B CN113298881 B CN 113298881B CN 202110584637 A CN202110584637 A CN 202110584637A CN 113298881 B CN113298881 B CN 113298881B
Authority
CN
China
Prior art keywords
imu
monocular camera
mechanical arm
coordinate system
rotation
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
CN202110584637.XA
Other languages
English (en)
Other versions
CN113298881A (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.)
Shenyang Institute of Automation of CAS
Original Assignee
Shenyang Institute of Automation of CAS
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 Shenyang Institute of Automation of CAS filed Critical Shenyang Institute of Automation of CAS
Priority to CN202110584637.XA priority Critical patent/CN113298881B/zh
Publication of CN113298881A publication Critical patent/CN113298881A/zh
Application granted granted Critical
Publication of CN113298881B publication Critical patent/CN113298881B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G06T7/85—Stereo camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Manipulator (AREA)

Abstract

本发明提出了一种单目相机‑IMU‑机械臂的空间联合标定方法。将单目相机和惯性测量单元(IMU)固定在机械臂末端执行器附近，可以有效地协助机械臂进行运动估计和场景感知。单目相机、IMU和机械臂之间可靠、准确的标定是实现随动感知的前提。该方法包括单目相机‑IMU联合标定、单目相机‑机械臂联合标定、IMU‑机械臂联合标定和统一框架下的联合标定四个步骤。这种方法能够可靠且准确地关联单目相机、IMU和机械臂之间的空间关系。该方法采用先估计旋转矩阵，再估计平移向量的方式来解决平移向量误差易耦合到旋转矩阵估计的问题。本发明方法适用于对单目相机、IMU和机械臂进行联合标定，具有稳定、准确的优势。

Description

单目相机-IMU-机械臂的空间联合标定方法
技术领域
本发明涉及多源信息融合技术和图像处理技术，具体地说是一种单目相机-IMU-机械臂的空间联合标定方法。
背景技术
智能化的机械臂通常配有多模态感知单元，比如视觉传感器、惯性传感器，触觉传感器、力觉传感器等，通过多模态信息采集与处理来实现机械臂对自身和周围环境的感知功能。虽然市场上已有的感知模块，比如深度相机、激光雷达和双目相机能够有效做出载体的运动估计和深度估计，但由于其感知空间受限、反射敏感、感知单元尺寸过大等原因而无法适用于机械臂随动感知系统。
而单目相机、IMU在感知精度和响应速度方面具有互补的优势。单目相机能够采集丰富的场景信息，在定位和导航领域应用广泛。IMU是测量物体三轴角速度和加速度的装置，它提供了类似于人类内部前庭系统一样的自身惯性信息，在导航中有着重要的应用价值。所以将单目相机和IMU固定在机械臂末端，可以有效地协助机械臂进行运动估计和场景感知。单目相机、IMU和机械臂之间可靠、准确的标定是实现随动感知的前提，因此，本发明提出一种单目相机-IMU-机械臂的联合标定方法。
发明内容
本发明提出了一种单目相机-IMU-机械臂的空间联合标定方法，该方法能够实现三者的有效融合。
所采用的技术方案是：一种单目相机-IMU-机械臂的空间联合标定方法，包括以下步骤：
步骤1：单目相机和IMU单元固定在机械臂末端，设定机械臂运动轨迹，即机械臂末端执行器坐标系{R}下的位置变化：控制机械臂带动末端的IMU单元和单目相机移动；移动过程中，IMU单元自动获取与机械臂运动轨迹相对应的IMU实际移动的姿态序列，即IMU坐标系{I}下的位置变化：{I1→I2→I3→…}，单目相机拍摄标定板目标图像，并实时定位单目相机坐标系{C}下的位置：{C1→C2→C3→…}；
步骤2：对单目相机、IMU、机械臂两两之间进行分别标定，获取单目相机坐标系{C}、IMU坐标系{I}、机械臂末端执行器坐标系{R}两两之间的空间变换关系；
步骤3：根据上述三个坐标系之间的变换关系获取统一框架下的联合标定模型，判断模型是否收敛，若不收敛则返回步骤1按照设定的机械臂运动轨迹控制机械臂末端执行器再次运动执行建模步骤直到模型收敛结束；从而获取优化的单目相机-IMU-机械臂的空间联合标定模型。
所述分别标定包括三个步骤：
步骤2.1：通过最小化IMU旋转和单目相机旋转之间的差异值来计算偏差，在迭代过程中计算角速度偏差估计用于估计单目相机-IMU旋转矩阵计算加速度偏差估计用于估计单目相机-IMU平移向量，并迭代更新相关参数，获取单目相机-IMU联合标定；
步骤2.2：通过单目相机跟随机械臂运动的位姿，获取单目相机-机械臂联合标定；
步骤2.3：通过IMU跟随机械臂运动的位姿，获取IMU-机械臂联合标定。
包括：步骤2.1.1：角速度偏差估计：
单目相机在时刻t到t+1之间的旋转可以由下式给出：
其中与/>分别表示t和t+1时刻从世界坐标系{W}到IMU坐标系{I}的旋转矩阵；同样，/>和/>分别表示t和t+1时刻从世界坐标系{W}到单目相机坐标系{C}的旋转矩阵；时刻t到t+1之间的单目相机旋转与IMU预积分的关系可以表示为：
其中表示t和t+1时刻IMU预积分后的旋转增量；/>是SO(3)的右雅克比行列式；/>是t时刻从单目相机坐标系{C}到世界坐标系{W}的旋转矩阵；是t+1时刻从世界坐标系{W}到单目相机坐标系{C}的旋转矩阵；/>和/>分别是估算得到的从IMU坐标系{I}到单目相机坐标系{C}的旋转矩阵和从单目相机坐标系{C}到IMU坐标系{I}的旋转矩阵；/>表示t和t+1时刻预积分后的旋转增量；/>是估算得到的角速度偏差；通过使用对数运算，/>可以被最小化为：
该式通过Levenberg-Marquart非线性优化计算得到。
包括：步骤2.1.2：单目相机-IMU旋转矩阵估计：
旋转矩阵通过将时刻t到t+1之间的单目相机旋转与预积分处理后的IMU旋转对齐得到；
假设在时间间隔[t,t+1]之间，单目相机与IMU的姿态变化分别为和/>尽管在不同的参考坐标系中，/>和/>仍然表示相同的旋转；将单目相机坐标系转换为IMU坐标系的四元数/>其满足以下关系：
其中表示四元数/>的共轭，然后，通过将以下函数最大化来获得最佳四元数
然后将公式(1.6)得到的最佳四元数转换为矩阵形式，得到最优估计的单目相机与IMU间的旋转矩阵/>
包括：步骤2.1.3：加速度偏差估计：
在时间间隔[i,j]内，单目相机平移和IMU预积分平移之间的关系为：
其中，是估算得到的单目相机与IMU之间的旋转矩阵；/>表示单目相机在i时刻和j时刻之间的平移；/>表示时刻t相对于i帧的速度；Δt是IMU采样率；/>表示IMU在i时刻和j时刻之间的旋转；
最佳的加速度偏差通过最小化/>得到：
其中表示第i帧和第t帧之间的旋转矩阵，它被表示为/>
包括：步骤2.1.4：单目相机-IMU平移向量估计：
此处给定加速度偏差和角速度偏差单目相机-IMU旋转矩阵/>得到单目相机坐标系{C}到IMU坐标系{I}的平移向量为：
其中是时间间隔[i,j]的惯性平移量，它可以通过IMU预积分得到；/>是时间间隔[i,j]的单目相机平移量，它可以通过标定板特征点跟踪得到。
步骤2.2：单目相机-机械臂联合标定具体实现为：
假设机械臂从位置运动到/>单目相机也将随着机械臂从C1运动到C2，然后获得以下公式：
其中XRC表示从单目相机坐标系{C}到机械臂末端执行器坐标系{R}的变换矩阵，经过转换后得到：
其中表示机械臂末端执行器的相对运动；同样，C2C1 -1表示单目相机的相对运动，/>通过计算编码器测量值得到；C2C1 -1通过计算标定板坐标系(也称为世界坐标系{W})相对于单目相机坐标系{C}来得到；同样地，将机械臂末端执行器旋转到另一个位置，得到：
相对旋转矩阵RRC和平移向量tRC通过以下公式得到：
其中和/>分别对应于/>的旋转和平移；/>和/>分别对应于C2C1 -1的旋转和平移；同样，/>和/>分别对应于/>的旋转和平移；/>和/>分别代表C3C2 -1的旋转和平移；由于旋转矩阵通过角轴的形式表示为RR＝RR(kR,θR)(kR是旋转轴矢量，θR是旋转角度的比例因子)，所以旋转矩阵RRC满足如下等式：
转换得到：
通过将式(2.9)代入到式和/>其中i＝1,2，得到：
并得到平移向量tRC：
tRC＝(ATA)-1ATB (2.11)
其中，A和B表示如下：
步骤2.3：IMU-机械臂联合标定具体为：
IMU-机械臂的标定通过齐次变换矩阵XRI来制定，机械臂以设计好的轨迹运动，即同时，IMU也按照类似的轨迹(IMU姿态序列)进行运动，即{I1→I2→I3→…}；
与公式(2.1)类似，IMU和机械臂的位姿关系满足以下公式：
经转换可得：
以类似的方法，IMU与机械臂之间的相对旋转矩阵RRI和平移向量tRI可以通过求解以下各式得到：
其中，和/>分别对应于机械臂末端执行器坐标系变换/>的旋转和平移；/>和/>分别对应于IMU坐标系变换I2I1 -1的旋转和平移；同样，/>和/>分别对应于机械臂末端执行器坐标系变换/>的旋转和平移；/>和/>分别对应于IMU坐标系变换I3I2 -1的旋转和平移。
所述步骤3获取优化的单目相机-IMU-机械臂的空间联合标定模型具体为：
变换矩阵TRC、TRI、TIC满足以下几何关系：
其中表示从{C}到{R}的空间变换关系；/>表示从{I}到{R}的空间变换关系；/>表示从{C}到{I}的空间变换关系；
该联合标定方法通过评估变换矩阵和单位矩阵I4×4之间的相似性来判断标定是否达到收敛标准；/>表示根据测量结果计算得到的转换矩阵，λn，n＝1,2,3,4表示变换矩阵/>在每个轴上的特征值：
|λnE-A|＝0 (4.2)
其中E是四阶的单位矩阵，通过以下的方式，将λn，n＝1,2,3,4与对应于单位矩阵I4×4的每个轴上的特征值1进行比较，从而判断标定结果是否收敛：
λn≤1，n＝1,2,3,4 (4.3)
一种单目相机-IMU-机械臂的空间联合标定系统，包括：单目相机和IMU单元、控制器和机械臂驱动器、标定板；单目相机和IMU单元固定在机械臂末端，通过单目相机拍摄标定板的目标位置进行标定后获取单目相机的像素坐标位置；机械臂驱动器用于按照设定的轨迹控制机械臂带动末端的IMU单元移动；IMU单元用于自动获取与机械臂运动轨迹相对应的IMU实际移动的姿态序列；控制器用于读取程序执行如上所述的方法步骤，获取优化的单目相机-IMU-机械臂的空间联合标定模型，用于实际的机械臂运动。
本发明具有以下优点：
1.本发明方法采用先估计旋转矩阵，再估计平移向量的方式来解决平移向量误差易耦合到旋转矩阵估计的问题。
2.本发明方法适用于对单目相机、IMU和机械臂进行联合标定，具有稳定、准确的优势。
3.本发明方法能够使单目相机和IMU实现稳定、准确的信息融合。并使机械臂在复杂、非结构化的环境下实现随动感知的功能。
附图说明
图1为总体流程图；
图2为单目相机-IMU-机械臂空间联合标定的系统装置。
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方法做详细的说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明。但本发明能够以很多不同于在此描述的其他方式来实施，本领域技术人员可以在不违背发明内涵的情况下做类似改进，因此本发明不受下面公开的具体实施的限制。
除非另有定义，本文所使用的所有技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。
本发明方法是基于硬件系统实现的，硬件系统包括：单目相机、IMU单元、控制器和Kinova Jaco2机械臂、机械臂操作杆、标定板。设定机械臂运动轨迹，机械臂操作杆用于控制机械臂带动末端的IMU单元移动。IMU单元用于自动获取与机械臂运动轨迹相对应的IMU实际移动的姿态序列。
单目相机和IMU单元固定在机械臂末端，通过单目相机拍摄标定板的目标位置进行标定后获取单目相机的像素坐标位置C1、C2等。
Kinova Jaco2机械臂操作杆用于控制机械臂带动单目相机和IMU单元运动，并通过机械臂的SDK获得末端执行器坐标位置 等。
本发明方法包括四部分：
(1)单目相机-IMU联合标定；
(2)单目相机-机械臂联合标定；
(3)IMU-机械臂联合标定；
(4)统一框架下的联合标定。
具体实现过程如下所述。
步骤1：单目相机-IMU联合标定。
假设角速度偏差和加速度偏差始终保持不变，然后可以通过最小化IMU旋转和单目相机旋转之间的差异值来计算偏差。在迭代过程中，估计的旋转矩阵用于更新角速度偏差。
步骤1.1：角速度偏差估计
单目相机在时刻t到t+1之间的旋转可以由下式给出：
其中与/>分别表示t和t+1时刻从世界坐标系{W}到IMU坐标系{I}的旋转矩阵；同样，/>和/>分别表示t和t+1时刻从世界坐标系{W}到单目相机坐标系{C}的旋转矩阵。在时刻t到t+1之间的单目相机旋转与IMU预积分的关系：
其中表示t和t+1时刻IMU预积分后的旋转增量；/>是SO(3)的右雅克比行列式；/>是t时刻从单目相机坐标系{C}到世界坐标系{W}的旋转矩阵；是t+1时刻从世界坐标系{W}到单目相机坐标系{C}的旋转矩阵；/>和/>分别是估算得到的从IMU坐标系{I}到单目相机坐标系{C}的旋转矩阵和从单目相机坐标系{C}到IMU坐标系{I}的旋转矩阵；/>表示t和t+1时刻预积分后的旋转增量；/>是估算得到的角速度偏差。通过使用对数运算，/>可以被最小化为：
该式可以通过Levenberg-Marquart非线性优化计算得到。
步骤1.2：单目相机-IMU旋转矩阵估计
此处，旋转矩阵可以通过将时刻t到t+1之间的单目相机旋转与预积分处理后的IMU旋转对齐得到。假设在时间间隔[t,t+1]之间，单目相机与IMU的姿态变化分别为和尽管在不同的参考坐标系中，/>和/>仍然表示相同的旋转。将单目相机坐标系转换为IMU坐标系的四元数/>其满足以下关系：
其中表示四元数/>的共轭。然后，通过将以下函数最大化来获得最佳四元数
然后将公式(1.6)得到的最佳四元数转换为矩阵形式，即可得到最优估计的单目相机与IMU间的旋转矩阵/>
步骤1.3：加速度偏差估计
在时间间隔[i,j]内，单目相机平移和IMU预积分平移之间的关系为：
其中，是估算得到的单目相机与IMU之间的旋转矩阵；/>表示单目相机在i时刻和j时刻之间的平移；/>表示时刻t相对于i帧的速度；Δt是IMU采样率；/>表示IMU在i时刻和j时刻之间的旋转。
最佳的加速度偏差可以通过最小化/>得到：
其中表示时刻t相对于i帧的速度，/>表示第i帧和第t帧之间的旋转矩阵，它被表示为/>
步骤1.4：单目相机-IMU平移向量估计
此处给定加速度偏差和角速度偏差单目相机-IMU旋转矩阵/>得到单目相机坐标系{C}到IMU坐标系{I}的平移向量为：
其中是时间间隔[i,j]的惯性平移量，它可以通过IMU预积分得到；/>是时间间隔[i,j]的单目相机平移量，它可以通过标定板特征点跟踪得到。
步骤2：单目相机-机械臂联合标定。
假设机械臂从位置运动到/>单目相机也将随着机械臂从C1运动到C2，(其中，单目相机的位置C1、C2是通过单面相机拍摄标定板的目标位置进行标定后获取的像素坐标)然后可以获得以下公式：
其中XRC表示从单目相机坐标系{C}到机械臂末端执行器坐标系{R}的变换矩阵。经过转换后可以得到：
其中表示机械臂末端执行器的相对运动；同样，C2C1 -1表示单目相机的相对运动。/>可通过计算编码器测量值得到；C2C1 -1可以通过计算标定板坐标系(也称为世界坐标系{W})相对于单目相机坐标系{C}来得到。同样地，将机械臂末端执行器旋转到另一个位置，就可以得到：
相对旋转矩阵RRC和平移向量tRC可以通过以下公式得到：
其中和/>分别对应于/>的旋转和平移；/>和/>分别对应于C2C1 -1的旋转和平移。同样，/>和/>分别对应于/>的旋转和平移；/>和/>分别代表C3C2 -1的旋转和平移。由于旋转矩阵也可以通过角轴的形式来表示，即RR＝RR(kR,θR)(kR是旋转轴矢量，θR是旋转角度的比例因子)，所以旋转矩阵RRC满足如下等式：
转换得到：
通过将式(2.9)代入到2.4式和2.5式/>其中i＝1,2，可以得到：
并可以得到平移向量tRC：
tRC＝(ATA)-1ATB (2.11)
其中，A和B表示如下：
步骤3：IMU-机械臂联合标定。
IMU-机械臂的标定可以通过齐次变换矩阵XRI来制定。机械臂以设计好的螺旋轨迹运动，即同时，IMU也按照类似的轨迹(IMU姿态序列)进行运动，即{I1→I2→I3→…}。
与公式(2.1)类似，IMU和机械臂的位姿关系满足以下公式：
经转换可得：
以类似的方法，IMU与机械臂之间的相对旋转矩阵RRI和平移向量tRI可以通过求解以下各式得到：
其中，和/>分别对应于机械臂末端执行器坐标系变换/>的旋转和平移；/>和/>分别对应于IMU坐标系变换I2I1 -1的旋转和平移。
同样，和/>分别对应于机械臂末端执行器坐标系变换/>的旋转和平移；和/>分别对应于IMU坐标系变换I3I2 -1的旋转和平移。
步骤4：统一框架下的联合标定。
在单目相机-IMU-机械臂联合标定模型中，由于单目相机和IMU牢牢固定在机械臂末端，所以机械臂末端执行器坐标系{R}、IMU坐标系{I}和单目相机坐标系{C}之间存在固有的几何约束。理论上，变换矩阵TRC、TRI、TIC满足以下几何关系：
其中表示从{C}到{R}的空间变换关系；/>表示从{I}到{R}的空间变换关系；/>表示从{C}到{I}的空间变换关系。
该联合标定方法通过评估变换矩阵和单位矩阵I4×4之间的相似性来判断标定是否达到收敛标准。/>表示根据测量结果计算得到的转换矩阵，λn，n＝1,2,3,4表示变换矩阵/>在每个轴上的特征值：
|λnE-A|＝0 (4.2)
其中E是四阶的单位矩阵，通过以下的方式，将λn，n＝1,2,3,4与对应于单位矩阵I4×4的每个轴上的特征值1进行比较，从而判断标定结果是否收敛：/>
λn≤1，n＝1,2,3,4 (4.3)
以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种单目相机-IMU-机械臂的空间联合标定方法，其特征在于，包括以下步骤：
步骤1：单目相机和IMU单元固定在机械臂末端，设定机械臂运动轨迹，即机械臂末端执行器坐标系{R}下的位置变化：控制机械臂带动末端的IMU单元和单目相机移动；移动过程中，IMU单元自动获取与机械臂运动轨迹相对应的IMU实际移动的姿态序列，即IMU坐标系{I}下的位置变化：{I1→I2→I3→…}，单目相机拍摄标定板目标图像，并实时定位单目相机坐标系{C}下的位置：{C1→C2→C3→…}；
步骤2：对单目相机、IMU、机械臂两两之间进行分别标定，获取单目相机坐标系{C}、IMU坐标系{I}、机械臂末端执行器坐标系{R}两两之间的空间变换关系；
步骤3：根据上述三个坐标系之间的变换关系获取统一框架下的联合标定模型，判断模型是否收敛，若不收敛则返回步骤1按照设定的机械臂运动轨迹控制机械臂末端执行器再次运动执行建模步骤直到模型收敛结束；从而获取优化的单目相机-IMU-机械臂的空间联合标定模型；所述获取优化的单目相机-IMU-机械臂的空间联合标定模型具体为：
变换矩阵TRC、TRI、TIC满足以下几何关系：
其中表示从{C}到{R}的空间变换关系；
表示从{I}到{R}的空间变换关系；/>表示从{C}到{I}的空间变换关系；
该联合标定方法通过评估变换矩阵和单位矩阵I4×4之间的相似性来判断标定是否达到收敛标准；/>表示根据测量结果计算得到的转换矩阵，λn，n＝1,2,3,4表示变换矩阵/>在每个轴上的特征值：
|λnE-A|＝0 (4.2)
其中E是四阶的单位矩阵，通过以下的方式，将λn，n＝1,2,3,4与对应于单位矩阵I4×4的每个轴上的特征值1进行比较，从而判断标定结果是否收敛：
λn≤1，n＝1,2,3,4 (4.3)。
2.根据权利要求1所述的一种单目相机-IMU-机械臂空间联合标定方法，其特征在于，所述分别标定包括三个步骤：
步骤2.1：通过最小化IMU旋转和单目相机旋转之间的差异值来计算偏差，在迭代过程中计算角速度偏差估计用于估计单目相机-IMU旋转矩阵计算加速度偏差估计用于估计单目相机-IMU平移向量，并迭代更新相关参数，获取单目相机-IMU联合标定；
步骤2.2：通过单目相机跟随机械臂运动的位姿，获取单目相机-机械臂联合标定；
步骤2.3：通过IMU跟随机械臂运动的位姿，获取IMU-机械臂联合标定。
3.根据权利要求2所述的一种单目相机-IMU-机械臂空间联合标定方法，其特征在于，包括：步骤2.1.1：角速度偏差估计：
单目相机在时刻t到t+1之间的旋转由下式给出：
其中与/>分别表示t和t+1时刻从世界坐标系{W}到IMU坐标系{I}的旋转矩阵；同样，/>和/>分别表示t和t+1时刻从世界坐标系{W}到单目相机坐标系{C}的旋转矩阵；时刻t到t+1之间的单目相机旋转与IMU预积分的关系表示为：
其中表示t和t+1时刻IMU预积分后的旋转增量；/>是SO(3)的右雅克比行列式；/>是t时刻从单目相机坐标系{C}到世界坐标系{W}的旋转矩阵；/>是t+1时刻从世界坐标系{W}到单目相机坐标系{C}的旋转矩阵；/>和/>分别是估算得到的从IMU坐标系{I}到单目相机坐标系{C}的旋转矩阵和从单目相机坐标系{C}到IMU坐标系{I}的旋转矩阵；/>表示t和t+1时刻预积分后的旋转增量；/>是估算得到的角速度偏差；通过使用对数运算，bt g被最小化为：
该式通过Levenberg-Marquart非线性优化计算得到。
4.根据权利要求2所述的一种单目相机-IMU-机械臂空间联合标定方法，其特征在于，包括：步骤2.1.2：单目相机-IMU旋转矩阵估计：
旋转矩阵通过将时刻t到t+1之间的单目相机旋转与预积分处理后的IMU旋转对齐得到；
假设在时间间隔[t,t+1]之间，单目相机与IMU的姿态变化分别为和/>尽管在不同的参考坐标系中，/>和/>仍然表示相同的旋转；将单目相机坐标系转换为IMU坐标系的四元数/>其满足以下关系：
其中表示四元数/>的共轭，然后，通过将以下函数最大化来获得最佳四元数/>
然后将公式(1.6)得到的最佳四元数转换为矩阵形式，得到最优估计的单目相机与IMU间的旋转矩阵/>
5.根据权利要求2所述的一种单目相机-IMU-机械臂空间联合标定方法，其特征在于，包括：步骤2.1.3：加速度偏差估计：
在时间间隔[i,j]内，单目相机平移和IMU预积分平移之间的关系为：
其中，是估算得到的单目相机与IMU之间的旋转矩阵；/>表示单目相机在i时刻和j时刻之间的平移；/>表示时刻t相对于i帧的速度；Δt是IMU采样率；/>表示IMU在i时刻和j时刻之间的旋转；
最佳的加速度偏差通过最小化/>得到：
其中表示第i帧和第t帧之间的旋转矩阵，它被表示为/>
6.根据权利要求2所述的一种单目相机-IMU-机械臂空间联合标定方法，其特征在于，包括：步骤2.1.4：单目相机-IMU平移向量估计：
此处给定加速度偏差和角速度偏差单目相机-IMU旋转矩阵/>得到单目相机坐标系{C}到IMU坐标系{I}的平移向量为：
其中是时间间隔[i,j]的惯性平移量，它通过IMU预积分得到；/>是时间间隔[i,j]的单目相机平移量，它通过标定板特征点跟踪得到。
7.根据权利要求2所述的一种单目相机-IMU-机械臂的空间联合标定方法，其特征在于，步骤2.2：单目相机-机械臂联合标定具体实现为：
假设机械臂从位置运动到/>单目相机也将随着机械臂从C1运动到C2，然后获得以下公式：
其中XRC表示从单目相机坐标系{C}到机械臂末端执行器坐标系{R}的变换矩阵，经过转换后得到：
其中表示机械臂末端执行器的相对运动；同样，C2C1 -1表示单目相机的相对运动，通过计算编码器测量值得到；C2C1 -1通过计算标定板坐标系，也称为世界坐标系{W}，相对于单目相机坐标系{C}来得到；同样地，将机械臂末端执行器旋转到另一个位置，得到：
相对旋转矩阵RRC和平移向量tRC通过以下公式得到：
其中和/>分别对应于/>的旋转和平移；/>和/>分别对应于C2C1 -1的旋转和平移；同样，/>和/>分别对应于/>的旋转和平移；/>和/>分别代表C3C2 -1的旋转和平移；由于旋转矩阵通过角轴的形式表示为RR＝RR(kR,θR)，kR是旋转轴矢量，θR是旋转角度的比例因子，所以旋转矩阵RRC满足如下等式：
转换得到：
通过将式(2.9)代入到式和/>其中i＝1,2，得到：
并得到平移向量tRC：
tRC＝(ATA)-1ATB (2.11)
其中，A和B表示如下：
8.根据权利要求2所述的一种单目相机-IMU-机械臂的空间联合标定方法，其特征在于，步骤2.3：IMU-机械臂联合标定具体为：
IMU-机械臂的标定通过齐次变换矩阵XRI来制定，机械臂以设计好的轨迹运动，即同时，IMU也按照类似的轨迹，IMU姿态序列，进行运动，即{I1→I2→I3→…}；
与公式(2.1)类似，IMU和机械臂的位姿关系满足以下公式：
经转换可得：
以类似的方法，IMU与机械臂之间的相对旋转矩阵RRI和平移向量tRI通过求解以下各式得到：
其中，和/>分别对应于机械臂末端执行器坐标系变换/>的旋转和平移；/>和/>分别对应于IMU坐标系变换I2I1 -1的旋转和平移；同样，/>和/>分别对应于机械臂末端执行器坐标系变换/>的旋转和平移；/>和/>分别对应于IMU坐标系变换I3I2 -1的旋转和平移。
9.根据权利要求1所述的一种单目相机-IMU-机械臂的空间联合标定系统，其特征在于，包括：单目相机和IMU单元、控制器和机械臂驱动器、标定板；单目相机和IMU单元固定在机械臂末端，通过单目相机拍摄标定板的目标位置进行标定后获取单目相机的像素坐标位置；机械臂驱动器用于按照设定的轨迹控制机械臂带动末端的IMU单元移动；IMU单元用于自动获取与机械臂运动轨迹相对应的IMU实际移动的姿态序列；控制器用于读取程序执行如权利要求1-8任意一项所述的方法步骤，获取优化的单目相机-IMU-机械臂的空间联合标定模型，用于实际的机械臂运动。
CN202110584637.XA 2021-05-27 2021-05-27 单目相机-imu-机械臂的空间联合标定方法 Active CN113298881B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110584637.XA CN113298881B (zh) 2021-05-27 2021-05-27 单目相机-imu-机械臂的空间联合标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110584637.XA CN113298881B (zh) 2021-05-27 2021-05-27 单目相机-imu-机械臂的空间联合标定方法

Publications (2)

Publication Number Publication Date
CN113298881A CN113298881A (zh) 2021-08-24
CN113298881B true CN113298881B (zh) 2023-09-12

Family

ID=77325616

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110584637.XA Active CN113298881B (zh) 2021-05-27 2021-05-27 单目相机-imu-机械臂的空间联合标定方法

Country Status (1)

Country Link
CN (1) CN113298881B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110880189A (zh) * 2018-09-06 2020-03-13 舜宇光学（浙江）研究院有限公司 联合标定方法及其联合标定装置和电子设备
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN111735479A (zh) * 2020-08-28 2020-10-02 中国计量大学 一种多传感器联合标定装置及方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110880189A (zh) * 2018-09-06 2020-03-13 舜宇光学（浙江）研究院有限公司 联合标定方法及其联合标定装置和电子设备
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN111735479A (zh) * 2020-08-28 2020-10-02 中国计量大学 一种多传感器联合标定装置及方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
连续体机械臂振动特性研究;高国华;李炼石;夏齐霄;王皓;任晗;;振动.测试与诊断(第01期);全文 *

Also Published As

Publication number Publication date
CN113298881A (zh) 2021-08-24

Similar Documents

Publication Publication Date Title
CN109655024B (zh) 采用空间变换技术的位移传感器外部参数标定方法
CN109308693B (zh) 由一台ptz相机构建的目标检测和位姿测量单双目视觉系统
Wang et al. Adaptive visual servoing using point and line features with an uncalibrated eye-in-hand camera
JP7326911B2 (ja) 制御システムおよび制御方法
CN111801198B (zh) 一种手眼标定方法、系统及计算机存储介质
CN107627303B (zh) 一种基于眼在手上结构的视觉伺服系统的pd-smc控制方法
Cifuentes et al. Probabilistic articulated real-time tracking for robot manipulation
Grosso et al. Robust visual servoing in 3-D reaching tasks
Hutchinson et al. A tutorial on visual servo control
Assa et al. A robust vision-based sensor fusion approach for real-time pose estimation
CN111052183A (zh) 利用事件相机的视觉惯性里程计
CN109807937B (zh) 一种基于自然场景的机器人手眼标定方法
Assa et al. Virtual visual servoing for multicamera pose estimation
CN114519738A (zh) 一种基于icp算法的手眼标定误差修正方法
CN115446847A (zh) 用于提高机器人系统的3d眼手协调准确度的系统和方法
Olsson et al. Force control and visual servoing using planar surface identification
CN114067210A (zh) 一种基于单目视觉导引的移动机器人智能抓取方法
CN113298881B (zh) 单目相机-imu-机械臂的空间联合标定方法
Maeda et al. Frequency response experiments of 3-d pose full-tracking visual servoing with eye-vergence hand-eye robot system
Zhang et al. High-precision calibration of camera and IMU on manipulator for bio-inspired robotic system
Lange et al. Predictive visual tracking of lines by industrial robots
CN114764830A (zh) 一种基于四元数ekf和未标定手眼系统的物体位姿估算方法
CN109542094A (zh) 无期望图像的移动机器人视觉镇定控制
CN110189359B (zh) 一种基于分类学习的快速智能跟踪方法和装置
An et al. Rgbmanip: Monocular image-based robotic manipulation through active object pose estimation

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