CN117464692A - 一种基于结构光视觉系统的衬板抓取机械臂控制方法 - Google Patents

Abstract

本发明公开了一种基于结构光视觉系统的衬板抓取机械臂控制方法，该方法实现基于待抓取的新型衬板和由两个线结构光发生器以及成像系统组成的结构光视觉系统，机械臂抓取衬板前，两个线结构光发生器分别将线激光投射到衬板的左右衬耳合作目标上，成像系统对线激光进行成像，并测量出相机与衬耳合作目标坐标系的相对位姿关系，从而构造视觉伺服控制器，控制液压重载机械臂完成新型衬板的视觉伺服抓取任务。本发明方法实现了复杂结构衬板的精确位姿测量与高效伺服控制，为衬板自主抓取创造了条件。

Description

一种基于结构光视觉系统的衬板抓取机械臂控制方法

技术领域

本发明属于机械加工技术领域，涉及基于液压重载机械臂结构光视觉系统的应用，具体涉及一种基于结构光视觉系统的新型衬板抓取控制方法。

背景技术

矿物磨机衬板是易损件，它的换装方式直接决定磨机运转率与安全性。我国传统衬板换装多采用人工模式，不仅用工多、作业时间长，且工人工作于受限作业空间，环境恶劣且危险性高；

为解决该问题，现有技术中提出了一种液压重载机械臂，该机械臂具有主从式人机交互操作模式和智能控制模式，可以实现机械臂的可达集/力反馈主从式人机交互控制和基于简易合作目标的智能视觉伺服衬板抓取控制。但这种液压重载机械臂在使用过程中存在几个问题：

衬板衬耳同轴导致机械臂抓持衬板时，需要额外的扶支缸对衬板轴向转动进行限制；

衬板缺乏纹理信息，单纯依赖视觉传感器无法完成衬板位姿的测量，为解决该问题，二代臂在对衬板操作时利用了简易合作目标，不过针对合作目标的特征提取算法受到光照条件的极大限制；

传统视觉伺服控制器的任务函数由唯一的位姿信息构造，而新型衬板左右衬耳各有一组位姿信息，目前缺乏一种有效的位姿信息融合方法。

因此，需要一种可实现复杂结构衬板的精确位姿测量与高效伺服控制，能够实现衬板自主抓取的抓取控制方法。

发明内容

有鉴于此，本发明提供了一种基于结构光视觉系统的衬板抓取机械臂控制方法，基于改进的光视觉系统构建视觉伺服控制器从而对控制方法进行改进，实现复杂结构衬板的精确位姿测量与高效伺服控制。

为实现上述目的，本发明所采用的技术方案是：一种基于结构光视觉系统的衬板抓取机械臂控制方法，基于结构光视觉系统，结合视觉伺服控制器对被测复杂结构衬板位姿进行测量和控制，实现衬板的自主抓取，该方法包括以下步骤：

S1、构建光视觉系统，包括成像系统和线结构光发生器，所述光视觉系统设置在液压重载机械臂抓具上并对衬板进行测量，所述衬板上设有非同轴衬耳和设置在衬耳上方的衬耳合作目标；

S2、结构光视觉系统的标定，获取标定图像，求出结构光形成的平面在成像系统坐标系中的参数；

S3、构建视觉伺服控制系统，控制机械臂进行衬板自主抓取；具体包括以下子步骤：

S31、结构光视觉系统测量，根据结构光投射到衬耳合作目标上的图像，计算衬耳合作目标位姿；

S32、设计中间位姿融合方法获取衬耳合作目标位姿的中间位姿，并利用中间位姿设计视觉伺服控制器，根据测量位姿和期望位姿，输出机械臂关节位移指令；

S33、重载机械臂按位移指令运动，直到成像系统采集的当前图像成功逼近期望图像，完成衬板抓取动作。

进一步地，所述步骤S1中成像系统通过支架设置在液压重载机械臂抓具的上侧且倾斜俯视衬板衬耳合作目标，线结构光发生器设置在液压重载机械臂抓具的底部两端，对应衬板上左、右两侧的衬耳合作目标。

更进一步地，所述线结构光发生器为十字线结构光发生器，一个线结构光发生器投射的两条线结构光形成的两个空间平面之间夹角为。

进一步地，衬耳合作目标上表面平面法向与衬耳圆孔轴向正交。

具体地，所述线结构光发生器为两个，左、右两端的线结构光发生器分别向对应一侧的衬耳合作目标投射十字线结构激光，成像系统对线结构激光进行成像，并测量出它与衬耳合作目标坐标系的相对位姿。

进一步地，所述步骤S31中结构光视觉系统测量，根据结构光投射到衬耳合作目标上的图像，分别计算出左、右两侧衬耳合作目标坐标系与成像系统坐标系/>间的相对位姿关系/>。

更进一步地，构造视觉伺服控制器，具体包括：

由当前视图测量出的左、右两侧衬耳合作目标分别相对当前成像系统的位姿，以及期望视图测量出的左、右两侧衬耳合作目标分别相对于期望成像系统的位姿/>，构造中间位姿及视觉伺服特征集、视觉伺服任务函数和视觉伺服控制律，根据液压重载机械臂逆运动学模型，求出下一采样时刻各关节角绝对位移，生成并输出机械臂关节位移指令，控制液压重载机械臂完成对衬板进行抓取。

更进一步地，所述中间位姿融合方法具体包括：

a、获取当前视图测量出的左、右两侧合作目标相对当前相机的位姿，以及期望视图测量出的左、右两侧合作目标相对于期望相机的位姿/>；

b、记四维齐次位姿矩阵为：

，其中/>为左侧合作目标的当前姿态矩阵和位置向量，/>为右侧合作目标的当前姿态矩阵和位置向量，/>为左侧合作目标的期望姿态矩阵和位置向量，/>为右侧合作目标的期望姿态矩阵和位置向量；

c、由左侧合作目标求出当前相机与期望相机的相对姿态，由右侧合作目标求出当前相机与期望相机的相对姿态/>，则利用四元数插值可求得/>与/>的中间姿态：

。

本发明的有益效果是：

1、本发明方法通过建立新型衬板结构光视觉系统，线结构光发生器将线激光投射到衬板的衬耳合作目标上，工业相机对线激光进行成像，并测量出它与衬耳合作目标坐标系的相对位姿，进而构造视觉伺服控制器，控制液压重载机械臂完成视觉伺服任务，实现了复杂结构衬板位姿的精确测量，为衬板自主抓取创造了条件；

2、本发明衬板上创新性地设有非同轴衬耳，解决了现有技术中机械臂抓持衬板时需额外的扶支缸对衬板轴向转动进行限制的问题，使得液压重载机械臂省去了一组扶支缸及其阀控系统，改良并简化了衬板和机械臂手爪的机械结构；

3、为解决可见光视觉合作目标特征提取和位姿测量严重受限于光照条件的缺点，采用了结构光视觉系统，该系统将十字线结构激光投射到衬耳上方合作目标上形成明显的视觉特征，使视觉特征的提取和衬板位姿测量不再受到光照条件的制约；进一步地，衬耳合作目标上表面平面法向与衬耳圆孔轴向正交，该设置方式保证了机械臂抓取时的准确性，同时采用的结构光视觉系统标定方法简单可靠，精度较高；

4、提出了左右衬耳合作目标两组位姿的中间位姿融合方法，实现位姿信息融合，并利用中间位姿实现了视觉伺服控制器的设计；得益于结构光视觉系统的加持，液压重载机械臂可以在恶劣工作条件和恶劣光照条件下，准确、高效、稳定地完成衬板换装过程中的智能视觉伺服衬板抓取作业。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明重载机械臂的结构示意图的立体图，

图2是本发明重载机械臂的结构示意图的正视图；

图3是本发明重载机械臂的结构示意图的左视图；

图4是本发明衬板的结构示意图的正视图；

图5是本发明衬板的结构示意图的立体图；

图6是本发明衬板的结构示意图的左视图；

图7是本发明的结构光视觉系统的工作原理图； {L}为左侧衬耳合作目标坐标系，{R}为右衬耳合作目标坐标系， {C}为成像系统坐标系；

图8是本发明实施例中的结构光视觉系统标定原理图，其中为成像系统坐标系，/>为结构光形成的平面，/>为像点的归一化欧氏坐标，/>为相机像平面上线结构光上的点，I是相机的图像平面；

图9是本发明实施例中结构光视觉系统标定过程示意图；其中R为三维正交姿态矩阵，t为三维位置向量，、/>为标定板坐标系中角点的齐次坐标，/>为标定板坐标系；

图10是本发明实施例中视觉伺服控制系统结构图；

图11是本发明实施例中结构光视觉系统位姿测量原理图；其中l ₁、l ₂、l ₃、l ₄为空间直线与空间平面的四个交点，a为线段的长度，b为线段/>的长，c为线段/>的长，d为线段/>的长；

附图标记如下：

1、成像系统，2、液压重载机械臂抓具，3、线结构光发生器，4、衬耳合作目标，5、衬耳。

具体实施方式

下面给出具体实施例，对本发明的技术方案作进一步清楚、完整、详细地说明。本发明所述实施例仅为本发明的部分较佳实施例而已，并非用于限定本发明的保护范围。

实施例1

本发明提供一种基于结构光视觉系统的衬板抓取机械臂控制方法，该实现方法基于待抓取的新型衬板结构、对应的重载机械臂手爪结构和由两个线结构光发生器3以及成像系统1组成的结构光视觉系统，基于上述结构光视觉系统，结合视觉伺服控制器对被测复杂结构衬板位姿进行测量和控制，实现衬板的自主抓取，该方法包括以下步骤：

步骤1、成像系统1与线结构光发生器3构成结构光视觉系统；

图1-6所示为结构光视觉系统的组成及工作原理图。该视觉系统主要由图1-3所示的两个线结构光发生器3以及成像系统1组成；衬板上设有非同轴衬耳5和对应设置在衬耳5上方的衬耳合作目标4，衬耳合作目标4和衬耳5为一体浇筑成型结构，其上表面为长方形。

在本实施例中，成像系统1选择了工业相机，工业相机通过支架安装在液压重载机械臂抓具2的侧方，倾斜俯视衬板上的衬耳合作目标4，线结构光发生器3共2个，为十字线结构光发生器，安装在液压重载机械臂抓具2的底部两端，分别向左、右衬耳上的衬耳合作目标4投射线结构光。

进一步地，为防止位姿测量结果的多义性，一个发生器投射的两条线结构光形成的两个空间平面之间夹角为。通过一根线激光和激光出射原点（线外的一点）可以唯一确定一个平面，在本实施例中，两条线激光所在平面的法向夹角是45度，投射到衬耳合作目标4平面上后两条线夹角随抓具位移变化而发生变化。

如图7所示，所述线结构光发生器3为十字线结构光发生器，左、右两个线结构光发生器3分别将线激光投射到对应一侧衬耳上的衬耳合作目标4上，形成明显的视觉特征，工业相机对线激光进行成像，并测量出该成像与衬耳合作目标坐标系的相对位姿，进而构造视觉伺服控制器，控制液压重载机械臂完成视觉伺服任务。

如图4-6所示，新型衬板的结构采用非同轴衬耳5设置，两个衬耳5圆孔的轴向平行而不重合，衬耳合作目标4上表面平面法向与衬耳5圆孔轴向正交，改良并简化了衬板和机械臂手爪的机械结构，使得液压重载机械臂省去了一组扶支缸及其阀控系统，不需要额外的扶支缸对衬板轴向转动进行限制。

步骤2、结构光视觉系统的标定；

图8所示为结构光视觉系统标定原理图，目的是获取标定图像，求出结构光形成的平面在成像系统坐标系中的参数；

以成像系统坐标系为参考坐标系，结构光形成的平面/>为/>，其中n为平面单位法向量，d>0为相机光心到平面的距离；相机像平面上线结构光上的点/>所成的像为/>，其中/>、/>分别为像点的横、纵坐标；记相机内参矩阵为K，则像点的归一化欧氏坐标为：

；

过相机光心与归一化像点/>的空间直线的普朗克坐标为：

；

则空间直线与空间平面的交点满足：

；

其中，为尺度因子，负责齐次坐标的归一化。将上式整理为平面参数的线性方程：

；

可以看出，仅需三个线性无关(即不共线)的像点，即可求出成像系统坐标系中结构光所在的空间平面。

可参考图9，具体结构光视觉系统的标定步骤如下：

1、利用标定板和张正友标定法对相机的内参矩阵进行标定；

2、随机摆放标定板，并使结构光与标定板任一边缘重合，测量与标定板角点重合的激光点在标定板坐标系中的3D坐标/>，/>是随机摆放标定板的次数。采集标定板图像，利用张正友标定法求出标定板坐标系与成像系统1坐标系间的外参数/>，其中/>、/>分别为第i次摆放标定板时第j个标定板角点在标定板坐标系{o}中的x、y分量；

3、计算出激光点在成像系统坐标系中的空间3D点，以及相机的归一化欧氏坐标/>，得到线结构光所在的空间平面的线性方程组；

4、重新摆放标定板，重复步骤2、3，得到线性方程组；

5、利用奇异值分解求出满足约束的空间平面参数，并通过尺度缩放，求得满足/>的空间平面参数。

采用了结构光视觉系统，该系统将线激光结构光投射到衬板左右衬耳合作目标4上形成明显的视觉特征，使视觉特征的提取和衬板位姿测量不再受到光照条件的制约，同时采用的结构光视觉系统标定方法简单可靠，精度较高，克服了可见光视觉合作目标特征提取和位姿测量严重受限于光照条件的缺点。

步骤3、构建视觉伺服控制系统；

图10所示为视觉伺服控制系统的结构图。在本实施例中，该系统为双伺服环结构，其中外环为视觉伺服环，控制周期为100ms，内环为关节位移伺服环，控制周期为5ms。在视觉伺服外环，主要实现位姿测量和视觉伺服控制器设计，具体包括以下内容：

步骤31、结构光视觉系统测量，根据结构光的图像，实现位姿测量；

图11所示为结构光视觉系统位姿测量原理图；结构光视觉系统测量的目的是根据结构光的图像，计算出左右衬耳合作目标坐标系与成像系统坐标系/>间的相对位姿关系/>，其中/>为三维正交姿态矩阵，/>为三维位置向量。

衬耳合作目标坐标系原点位于左上角，水平向右方向为/>轴，竖直向下方向为/>轴，位姿测量具体步骤如下：

1、结参考图11，线结构光照射到衬耳合作目标4上，由于深度不连续在衬耳5边缘处形成四个边缘断点，在图像平面内提取出对应空间断点的像素点的归一化欧氏坐标/>；

2、利用空间直线与空间平面的交点计算公式计算出步骤1中各点以成像系统坐标系为参考坐标系的空间3D点坐标：

；

分别为点/>的x、y、z轴向坐标，/>；/>为分别为点/>的x、y、z轴向坐标；

3、根据各点的3D坐标计算交点M到各边缘断点的线段的长度：

；

4、计算出线段的长度：

；

式中，W，H分别为衬耳合作目标4的宽、高，它们为已知常量，为线激光/> 与线段AD的夹角，/>为线激光/> 与线段AB的夹角，/>为线段A/>的长度，/>为线段D/>的长度，为线段A/>的长度，/>为线段B/>的长度；

5、以合作目标坐标系为参考坐标系，点/>的坐标为：

；

式中，W，H分别为衬耳合作目标4的宽、高，它们为已知常量；

6、记成像系统坐标系{C}与衬耳合作目标坐标系{L}间的相对姿态和相对位置分别为R，t，根据坐标变换关系，记两向量的差分别为/>，，/>表示向量叉乘，可得旋转矩阵约束方程：

；

由于的二义性，计算出的旋转矩阵有两个，选择三维正交姿态矩阵为：

；

作为唯一正确解，则平移外参数：

；

7、同理，利用步骤1~6可同样求出衬耳合作目标坐标系{R}与成像系统坐标系{C}的相对位姿中的相对姿态和相对位置/>。

步骤32、设计视觉伺服控制器，视觉伺服控制器的任务是由当前视图测量出的左右衬耳合作目标4相对当前相机的位姿，以及期望视图测量出的合作目标相对于期望相机的位姿/>，输出机械臂关节位移指令。

视觉伺服控制器设计具体步骤如下：

1、视觉伺服特征集构造；

记四维齐次位姿矩阵

分别表示左右侧衬耳合作目标坐标系{L}、{R}在当前成像系统坐标系{C}和期望成像系统坐标系{C*}中的位姿，其中/>分别为三维正交姿态矩阵，/>分别为3维位置向量。

则当前平动特征集与期望平动特征集/>分别为：

；

由左、右侧衬耳合作目标4分别求出当前成像系统与期望成像系统的相对姿态，理想情况下两个位姿应相等，不过由于测量噪声和误差的存在，两个位姿存在较小的差异。利用四元数插值可求得/>与/>的中间位姿：

；

求出的等效转轴/>和等效转角/>，构造当前转动特征集/>与期望转动特征集：

；

2、视觉伺服任务函数构造；

根据当前与期望的视觉伺服特征集，可构造出当前与期望的视觉伺服任务函数：

；

3、雅可比矩阵构造；

根据刚体运动学，构造出当前与期望的视觉伺服任务函数的雅可比矩阵：

；

其中，I₃为3维单位矩阵，[ ]_×为3维列向量的反对称矩阵，、/>为相机的运动线速度与角速度微分方程，/>、/>为平动雅可比矩阵与转动雅可比矩阵，/>为成像系统坐标系{C}的运动速度；

4、视觉伺服控制律设计；

根据上述微分方程，设计指数收敛的控制律：

；

式中，为比例控制系数；

5、末端执行器运动速度求解；

根据标定出的手眼关系，将相机运动速度/>映射到重载机械臂的末端执行器坐标系/>的运动速度V _E ：

；

式中分别为标定出的末端执行器坐标系{E}与成像系统坐标系{C}的相对姿态与位置；

6、视觉伺服环关节控制指令生成；

按照机器人末端执行器运动速度V _E 和视觉伺服控制器控制周期，计算出微分运动：

；

为末端执行器的平移微分运动，/>为旋转微分运动，/>为末端执行器坐标系{E}的角速度；

进一步计算出下一采样时刻末端执行器坐标系的位姿：

；

式中，为时刻表示，⁰ T _E 为根据机械臂正运动学模型计算出的末端执行器坐标系{E}在基座标系{0}的齐次位姿，/>为三维单位矩阵，可以看出，微分运动以末端执行器坐标系{E}为参考坐标系，因此通过右乘叠加到当前末端位姿；

根据液压重载机械臂逆运动学模型，求出下一采样时刻各关节角绝对位移：

；

其中，q为机械臂各关节的绝对角位移，代表机械臂的逆运动学模型；

7、关节伺服环控制指令生成；

关节伺服环控制周期，因此需对视觉伺服一个采样间隔的关节位移进行插值，以适应关节控制器的节拍。

常用插值方法有线性插值、多项式插值、样条插值等。

重载机械臂按上述步骤获取的关节角指令运动，直到相机采集的当前图像成功逼近期望图像，完成新型衬板视觉伺服抓取任务。

在视觉伺服控制器设计过程中，提出了左右衬耳合作目标4两组位姿的中间位姿融合方法，并利用中间位姿实现了视觉伺服控制器的设计，克服传统视觉伺服控制器的任务函数由唯一的位姿信息构造的缺点，同时得益于结构光视觉系统的加持，液压重载机械臂可以在恶劣工作条件和恶劣光照条件下，准确、高效、稳定地完成衬板换装过程中的智能视觉伺服衬板抓取作业。

综上所述，本发明提供了一种基于液压重载机械臂结构光视觉系统的新型衬板抓取方法，包括新型衬板和由两个线结构光发生器3以及成像系统组成的结构光视觉系统，机械臂抓取衬板前，两个线结构光发生器3分别将线激光投射到衬板的左右衬耳合作目标4上，工业相机对线激光进行成像，并测量出成像系统坐标系与衬耳合作目标坐标系的相对位姿关系，从而构造视觉伺服控制器，控制液压重载机械臂完成新型衬板的视觉伺服抓取任务。该系统实现了复杂结构衬板的精确位姿测量与高效伺服控制，为衬板自主抓取创造了条件；采用的结构光视觉系统标定方法简单可靠，精度较高，基于该结构光视觉系统，液压重载机械臂可以在恶劣工作条件和恶劣光照条件下，准确、高效、稳定地完成衬板换装过程中的智能视觉伺服衬板抓取作业。

以上显示和描述了本发明的主要特征、基本原理以及本发明的优点。本行业技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会根据实际情况有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (8)

1.一种基于结构光视觉系统的衬板抓取机械臂控制方法，其特征在于，基于结构光视觉系统，结合视觉伺服控制器对被测复杂结构衬板位姿进行测量和控制，实现衬板的自主抓取，该方法包括以下步骤：

S1、构建光视觉系统，包括成像系统（1）和线结构光发生器（3），所述光视觉系统设置在液压重载机械臂抓具（2）上并对衬板进行测量，所述衬板上设有非同轴衬耳（5）和设置在衬耳上方的衬耳合作目标（4）；

S2、结构光视觉系统的标定，获取标定图像，求出结构光形成的平面在成像系统（1）坐标系中的参数；

S3、构建视觉伺服控制系统，控制机械臂进行衬板自主抓取；具体包括以下子步骤：

S31、结构光视觉系统测量，根据结构光投射到衬耳合作目标（4）上的图像，计算衬耳合作目标（4）位姿；

S32、设计中间位姿融合方法获取衬耳合作目标（4）位姿的中间位姿，并利用中间位姿设计视觉伺服控制器，根据测量位姿和期望位姿，输出机械臂关节位移指令；

S33、重载机械臂按位移指令运动，直到成像系统（1）采集的当前图像成功逼近期望图像，完成衬板抓取动作。

2.根据权利要求1所述的一种基于结构光视觉系统的衬板抓取机械臂控制方法，其特征在于，所述成像系统（1）通过支架设置在液压重载机械臂抓具（2）的上侧且倾斜俯视衬板衬耳合作目标（4），线结构光发生器（3）设置在液压重载机械臂抓具（2）的底部两端，对应衬板上左右两侧的衬耳合作目标（4）。

3.根据权利要求2所述的一种基于结构光视觉系统的衬板抓取机械臂控制方法，其特征在于，所述线结构光发生器（3）为十字线结构光发生器，线结构光发生器（3）投射的两条线结构光形成的两个空间平面之间夹角为。

4.根据权利要求1所述的一种基于结构光视觉系统的衬板抓取机械臂控制方法，其特征在于，衬耳合作目标（4）上表面平面法向与衬耳（5）圆孔轴向正交。

5.根据权利要求1-4任一项所述的一种基于结构光视觉系统的衬板抓取机械臂控制方法，其特征在于，所述线结构光发生器为两个，左、右两端的线结构光发生器分别向对应一侧的衬耳合作目标（4）投射十字线结构激光，成像系统对线结构激光进行成像，并测量出它与衬耳合作目标坐标系的相对位姿。

6.根据权利要求1所述的一种基于结构光视觉系统的衬板抓取机械臂控制方法，其特征在于，所述步骤S31中结构光视觉系统测量，根据结构光投射到衬耳合作目标上的图像，分别计算出左、右两侧的衬耳合作目标（4）坐标系与成像系统（1）坐标系间的相对位姿关系。

7.根据权利要求6所述的一种基于结构光视觉系统的衬板抓取机械臂控制方法，其特征在于，所述步骤S32中构造视觉伺服控制器具体包括：

由当前视图测量出的左、右两侧衬耳合作目标（4）分别相对当前成像系统（1）的位姿，以及期望视图测量出的左、右两侧衬耳合作目标（4）分别相对于期望成像系统（1）的位姿/>，构造中间位姿及视觉伺服特征集、视觉伺服任务函数和视觉伺服控制律，根据液压重载机械臂逆运动学模型，求出下一采样时刻各关节角绝对位移，生成并输出机械臂关节位移指令，控制液压重载机械臂完成对衬板进行抓取。

8.根据权利要求1所述的一种基于结构光视觉系统的衬板抓取机械臂控制方法，其特征在于，所述中间位姿融合方法具体包括：

a、获取当前视图测量出的左、右两侧合作目标相对当前相机的位姿，以及期望视图测量出的左、右两侧合作目标相对于期望相机的位姿/>；

b、记四维齐次位姿矩阵，其中

分别为左、右侧合作目标的当前姿态矩阵和位置向量，/>分别为左、右侧合作目标的期望姿态矩阵和位置向量；

c、由左侧合作目标求出当前相机与期望相机的相对姿态，由右侧合作目标求出当前相机与期望相机的相对姿态/>，则利用四元数插值可求得/>与/>的中间姿态：

。