CN110170995A - 一种基于立体视觉的机器人快速示教方法 - Google Patents

Abstract

本发明公开了一种基于立体视觉的机器人快速示教方法，涉及机器人示教和立体视觉领域。本发明使用独立的具有特征识别单元的手持示教装置，通过安装在机器人末端或末端夹持工具上的立体视觉模块采集手持示教装置的图像，视觉信号处理模块识别、处理特征识别单元的信息，获取立体视觉坐标系到特征识别单元坐标系的变换关系，构建手持示教装置末端坐标系到机器人基坐标系的变换关系，获得手持示教装置末端点在机器人基坐标系下的位姿信息。根据该信息，机器人控制模块可控制机器人复现示教点的位姿。示教过程简便，充分利用人手部的灵活性，通过位姿测量杆件迅速到达所需示教点的位姿状态，极大提升示教效率，提高机器人易用性、灵活性、适应性。

Description

一种基于立体视觉的机器人快速示教方法

技术领域

本发明属于机器人视觉技术领域，特别涉及一种基于立体视觉的机器人快速示教方法。

背景技术

随着机器人技术的不断发展，工业机器人已经在人类社会的工业生产领域当中扮演着举 足轻重的地位，基于工业机器人的自动化生产线已经在机械加工、喷涂、装配、焊接、搬运 等领域广泛应用。

根据中国制造2025的发展规划，未来制造业将形成新一代信息技术和制造业相融合的新 模式，力求实现中国由制造大国向制造强国转变。而智能制造是中国制造2025的核心，工业 机器人的应用更是其关键，以机器人为核心的智能制造已经成为推动我们制造业转型升级的 重要手段。

目前，大部分机器人在运动前需要经过示教，常采用的方式是示教器示教，这种传统的 示教方法需要先操作机器人末端到指定位置，再操作机器人进行姿态变化，过程非常耗时耗 力。如遇到工件表面结构较复杂，还需要不断反复地变换机器人末端点位置和机器人末端姿 态才能到达机器人末端工作时所需位姿，极大地降低了机器人的工作效率和使用的易用性。

近年来，机器人视觉技术为解决机器人示教问题带来新的可能。目前比较成熟的视觉技 术有基于飞行时间(TOF)、结构化光、双目视觉和光场的技术。这些技术都能通过一定的算 法得到被测对象的深度信息，将深度信息反馈给机器人***，便可解决机器人示教中的深度 问题。

中国专利号：CN201610595202.4，名称：一种基于立体视觉技术对机器人进行示教的方 法，该发明公开了一种基于立体视觉技术对机器人进行示教的方法，该方法具体为：在工具 (如焊枪、喷头)上放置标识物，使用立体视觉摄像头识别工具，并连续记录每一帧的深度 图，再将工具放置于机器人末端，标定机器人末端工具坐标系到焊枪标识物坐标系的位姿变 换矩阵，然后进行示教复现。该方法示教时无需拖拽机器手臂，轻便而且节省空间，***搭 建较为简便。然而，该方法具有一定的局限性：首先，该方法使用的是真实焊枪或者其他工 具，标识物需要考虑如何固接于不同的工具，不具有通用性；其次，该方法在示教过程中摄 像头无法移动，有效视场范围有所限制，示教姿态存在死区；并且该方法采用连续记录运动 中的每一帧的方法，这对操作人员具有较高要求，示教速度过慢，则数据过于冗余，降低效 率，示教速度过快或不稳定，则有可能出现模糊图像，在这种情况下无法识别工具标识物， 只能重新示教；此外，该方法每次示教完都需要将工具安装到机器人末端，重新标定摄像头 坐标系下工具位姿矩阵到机器人坐标系下手臂末端位姿矩阵的变换关系，这对于需要多次示 教不同规划路径的工况环境是十分耗时耗力的，机器人使用易用性有所限制。

发明内容

本发明提出一种基于立体视觉的机器人快速示教方法，将立体视觉模块安装在机器人末 端或机器人末端夹持工具上，通过立体视觉模块获得特征信息，根据该特征信息构建手持示 教装置末端坐标系与机器人基坐标系之间的位姿变换关系，从而获取手持示教装置末端在机 器人基坐标系下的位姿信息，即所示教点在机器人基坐标系下的位姿信息，实现快速示教功 能，提高机器人的工作效率和使用易用性。

具体过程如下步骤：

第一步，将立体视觉模块安装在机器人末端或末端夹持工具上，获取立体视觉坐标系在 机器人末端夹持工具坐标系下的位姿矩阵T₂。

第二步，建立手持示教装置上的位姿测量杆件末端坐标系与手持示教装置上的特征识别 单元坐标系的位姿变换矩阵T₄。

第三步，调整机器人末端的位置和姿态，保证规划路径和手持示教装置的特征识别单元 在立体视觉模块的有效视场范围内；所述规划路径指的是根据当前机器人的位置及机器人所 能到达的有效工作空间范围所规划的设定点形成的路径。

第四步，视觉信号处理模块从机器人控制模块获取机器人末端夹持工具坐标系在机器人 基坐标系下的位姿矩阵T₁并保存。

第五步，利用位姿测量杆件直观地达到所示教的设定点的位置和姿态，具体为，操作手 持示教装置，使手持示教装置的位姿测量杆件的末端点移动到规划路径上的设定点的位置， 在保持位姿测量杆件末端点的位置不变的情况下，调整位姿测量杆件至设定的姿态。

第六步，手持示教装置将信号传输给立体视觉模块，控制所述立体视觉模块采集图像， 并通过视觉处理***识别、处理特征信息，首先，所述立体视觉模块控制立体视觉成像单元 完整清晰地采集当前所述手持示教装置上特征识别单元的图像信息；然后，所述立体视觉模 块将采集到的手持示教装置上特征识别单元的图像信息传输给所述视觉信号处理模块；最后， 通过所述视觉信号处理模块进行特征识别和数据处理，构建出所述手持示教装置上特征识别 单元坐标系在立体视觉坐标系下的位姿变换矩阵T₃。

第七步，基于第一步、第二步、第四步和第六步所求得的位姿变换矩阵T₁、T₂、T₃、T₄的结果，通过所述视觉信号处理模块进行数据处理，获取机器人基坐标系与手持示教装置的 位姿测量杆件末端坐标系之间的位姿变换矩阵T₅，进而获取位姿测量杆件末端点在机器人基 坐标系下的位姿信息，也就是所示教的一个设定点在机器人基坐标系下的位姿信息，将该设 定点在机器人基坐标系下的位姿信息保存在立体视觉信号处理模块中。

第八步，重复第三步到第七步，直到完成所有的规划路径上的设定点的示教工作。

第九步，视觉信号处理模块将所有保存的规划路径上的设定点在机器人基坐标系下的位 姿信息传输给机器人控制模块，机器人控制模块控制机器人复现所有规划路径上的设定点的 位姿，实现机器人工具末端点的规划轨迹，完成快速示教。

第十步，如果在当前机器人的位置及机器人所能到达的有效工作空间范围外，存在未完 成的示教路径的示教工作，则移动机器人到达新的位置，重复第三步到第十步，直到完成示 教路径的示教工作；所述示教路径指的是不限于机器人在当前位置下的工作空间范围的所有 规划路径。

手持示教装置上的特征识别单元至少包括三个不共线的特征信息，其相互位置关系固定 且已知，依靠三个特征信息，可确定唯一特征平面，构建立体视觉坐标系与特征识别单元坐 标系之间的位姿变换矩阵T₃。具体方法为：

首先，视觉信号处理模块通过对特征识别单元的图像信息进行数据处理，分别获得代表 三个特征信息的点1、点2和点3在立体视觉坐标系下的位置信息，依次为(x₁、y₁、z₁)、(x₂、 y₂、z₃)、(x₃、y₃、z₃)；其次，根据三点的空间坐标构建在立体视觉坐标系下的空间向量，假 设以点1为公共点，点2和点3分别与点1构成向量，且两向量互相垂直，形成特征识别单 元坐标系的X、Y轴，根据右手定则确定Z轴，如式(1)所示：

取向量的单位向量，如式(2)所示：

同时，立体视觉坐标系上单位向量，如式(3)所示:

再由旋转矩阵的定义，建立特征识别单元坐标系相对于立体视觉坐标系的旋转矩阵R₃， 如式(4)所示：

最后，选择点1的坐标值(x₁、y₁、z₁)作为特征识别单元坐标系的原点，即该坐标系的平移向量，最终构建出立体视觉坐标系与特征识别单元坐标系的位姿变换矩阵T₃，如式(5) 所示：

除上述构造空间向量时所指定的垂直关系外，三个点还可以是除了共线之外的任意位置 关系，在求取位姿变换矩阵时加入几何变换即可。

上述第一步中机器人手眼关系的标定使用经典“两步法”手眼标定方法，将至少带有三 个特征点的标定物放在立体视觉模块视场范围内，机器人控制模块控制安装有立体视觉模块 的机器人末端到达若干位置点和姿态，在保证标定物能被立体视觉模块完整清晰拍摄的前提 下，机器人末端每到达一个位置点和姿态进行一次标定物图像采集，至少采集六次标定物图 像后，通过视觉信号处理模块进行数据处理，基于经典“两步法”手眼标定算法获取立体视 觉坐标系与机器人末端夹持工具坐标系之间的位姿变化矩阵T₂。

上述第二步建立手持示教装置上的位姿测量杆件末端坐标系与手持示教装置上的特征识 别单元坐标系的位姿变换矩阵T₄的方法具体为：

首先，将至少包含三个特征点的标定物放在立体视觉模块的视场范围内，获取这些点在 立体视觉坐标系下的空间位置信息；其次，操作手持示教装置，使手持示教装置的位姿测量 杆件的末端点对准这些特征点，调整位姿测量杆件的姿态，使手持示教装置上的特征识别单 元能被立体视觉模块完整清晰拍摄；然后通过立体视觉模块采集手持示教装置上特征识别单 元图像并将手持示教装置上特征识别单元图像传输给视觉信号处理模块，通过视觉信号处理 模块进行数据处理，获取在手持示教装置的位姿测量杆件的当前位姿下，手持示教装置上特 征识别单元坐标系与立体视觉坐标系的位姿变换矩阵T₃；最后，基于手持示教装置上特征识 别单元坐标系在立体视觉坐标系下的位姿变换矩阵T₃和对应的特征点的位置信息，建立手持 示教装置上的位姿测量杆件末端坐标系与手持示教装置上的特征识别单元坐标系的位姿变换 矩阵T₄。

在第二步中，在保证手持示教装置上的特征识别单元可被立体视觉模块识别到的前提下， 手持示教装置末端的位姿测量杆可以以任意姿态对准标定物上的特征点。

在第五步进行示教过程中，在保证手持示教装置上的特征识别单元能被立体视觉模块完 整清晰拍摄的前提下，可以调节机器人末端呈现出任意姿态，以适应各种姿态的示教。

本发明的有益效果是：

1、本发明所述的机器人快速示教方法使用带有特征识别单元的手持示教装置，充分利用 人手部的灵活性，能够迅速、直观将手持示教装置移动到所需示教的规划路径上的设定点的 位置和姿态，相比传统的示教器示教，工作效率大幅度提升。

2、本发明所述的机器人快速示教方法操作简单，过程简便，在进行新的规划路径示教前， 可移动机器人至新的工作位置；在进行每一个设定点的示教前，可调整机器人末端姿态以适 应在复杂示教姿态下采集手持示教装置的特征识别单元图像；对需要进行多次不同规划路径 的示教工况，无需进行多次位姿变换关系标定，极大提高机器人工作的使用效率和使用易用 性。

附图说明

图1为一种基于立体视觉的机器人快速示教方法示意图。

图2为一种基于立体视觉的机器人快速示教方法的流程图。

图3为一种基于立体视觉的机器人快速示教方法的坐标系转换关系示意图。

附图中：1-手持示教装置；2-立体视觉模块；3-机器人末端夹持工具；4-机器人本体； 5-视觉信号处理模块；6-机器人控制模块；7-位姿测量杆件；8-特征识别单元；9-待加工物 件。立体视觉模块2和视觉信号处理模块5之间、视觉信号处理模块5和机器人控制模块6 之间、机器人控制模块6和机器人本体4之间的连线均表示通信。2-1表示立体视觉坐标系 CCS；3-1表示机器人末端夹持工具坐标系TCS；4-1表示机器人基坐标系BCS；7-1表示位姿 测量杆件末端坐标系PCS；8-1表示特征识别单元坐标系SCS。

具体实施方式

以下结合附图进一步说明本发明。

实施例：如图2所示的示教方法的流程图，结合图3所示的坐标系转换关系示意图。首 先，将立体视觉模块2安装在机器人末端夹持工具3上，机器人末端夹持工具3安装在机器 人本体4末端，进行机器人手眼标定，获取机器人末端夹持工具坐标系3-1到立体视觉坐标 系2-1的位姿变换矩阵T₂。

在进行示教前，需要做一个一次性工作，构建手持示教装置1上特征识别单元坐标系8-1 到位姿测量杆件末端坐标系7-1的位姿变换矩阵T₄。将带有特征点的标定物(图中未标出) 放在立体视觉模块2的视场范围内，计算特征点在立体视觉坐标系2-1下的位置信息，记作 (x_n1,y_n1,z_n1),(x_n2,y_n2,z_n2),...,(x_ni,y_ni,z_ni)，i为特征点个数，i≥3。然后操作手持示教装置1分 别示教这些特征点，立体视觉模块2采集每次示教时特征识别单元8的图像，视觉信号处理 模块5获取图像信息并进行数据处理，计算每次示教时立体视觉坐标系2-1到特征识别单元 坐标系8-1的位姿变换矩阵T₃。

为方便说明，设T_g为立体视觉坐标系2-1下特征识别单元坐标系8-1到位姿测量杆件末 端坐标系7-1的平移向量，如式(6)所示：

其中，[x y z]^T为特征识别单元坐标系8-1到位姿测量杆件末端坐标系7-1的平移向 量，为待求量，记作T。进一步进行矩阵运算，可以得到式(7)：

其中，i代表用于标定的特征点数量，且i≥3，[x_ni y_ni z_ni]^T为立体视觉坐标系2-1下 的对应特征点的位置信息。

将i个特征点在立体视觉坐标系2-1下的位置信息(x_n1,y_n1,z_n1),(x_n2,y_n2,z_n2),...,(x_ni,y_ni,z_ni) 带入式(7)，则存在如式(8)所示的数学关系：

对形如A·X＝B的矩阵格式且矩阵A不为方阵，可通过最小二乘法求得矩阵如式(9)所示：

从而求得特征识别单元坐标系8-1原点到位姿测量杆件末端坐标系7-1原点的平移向量 T_t，再由手持示教装置1上特征识别单元8与位姿测量杆件7的形位关系确定旋转矩阵R_t， 最终由构建出特征识别单元坐标系8-1到位姿测量杆件末端坐标系7-1的位姿变换 矩阵T₄。

为保证特征识别单元8在立体视觉模块2的视场范围内，可通过调整机器人末端的位置 和姿态以适应不同角度的示教。

手持示教装置1上的特征识别单元8至少具有三个可被识别的特征信息，且代表三个特 征信息的点1、点2和点3在立体视觉坐标系2-1下的位置信息依次为(x₁、y₁、z₁)、(x₂、y₂、 z₃)、(x₃、y₃、z₃)。根据三点的空间坐标构建在立体视觉坐标系2-1下的空间向量，以点1为公共点，点2和点3分别与点1构成向量，且两向量互相垂直，形成特征识别单元坐标系 8-1的X、Y轴，根据右手定则确定Z轴，根据三个特征点的位置信息构建特征识别单元坐 标系8-1，进而获取立体视觉坐标系2-1与特征识别单元坐标系8-1之间的位姿变化矩阵T₃。 调整机器人到合适姿态后，从机器人控制模块6获取机器人基坐标系4-1到机器人末端夹持工具坐标系3-1的位姿变化矩阵T₁。

将待加工物件9放置在立体视觉模块2视场范围内，操作手持示教装置1示教规划路径 上的设定点的位姿，立体视觉***2采集图像，视觉信号处理模块5获取图像，计算获取立 体视觉坐标系2-1到特征识别单元坐标系8-1的位姿变换矩阵T₃。此时，根据位姿变换矩阵 的封闭运动链，机器人基坐标系4-1到位姿测量杆件末端坐标系7-1的位姿变换关系T₅与机 器人基坐标系4-1到机器人末端夹持工具坐标系3-1的位姿变化矩阵T₁、机器人末端夹持工 具坐标系3-1到立体视觉坐标系2-1的位姿变换矩阵T₂、立体视觉坐标系2-1到特征识别单 元坐标系8-1的位姿变换矩阵T₃、手持示教装置1上特征识别单元坐标系8-1到位姿测量杆 件末端坐标系7-1的位姿变换矩阵T₄之间存在式(10)所示的关系：

T₅＝T₁·T₂·T₃·T₄ (10)

根据机器人基坐标系4-1到位姿测量杆件末端坐标系7-1的位姿变换关系T₅，可求得所 示教的设定点在机器人基坐标系4-1下的位姿信息，视觉信号处理模块将其保存，等待所有 设定点示教完成后传输给机器人控制模块6，控制机器人复现所有设定点，形成规划轨迹。

以上仅是本发明的具体应用范例，对本发明的保护范围不构成任何限制。除上述实施例 外，本发明还可以有其它实施方式。凡采用等同替换或等效变换形成的技术方案，均落在本 发明所要求保护的范围之内。

Claims (9)

1.一种基于立体视觉的机器人快速示教方法，其特征在于包括如下步骤：

第一步，将立体视觉模块安装在机器人机器人末端或末端夹持工具上，进行机器人手 眼标定，获取立体视觉坐标系在机器人末端夹持工具坐标系下的位姿变换矩阵T₂；

第二步，建立手持示教装置上的位姿测量杆件末端坐标系与手持示教装置上的特征识别单元坐标系之间的位姿变换矩阵T₄；

第三步，调整机器人末端的位置和姿态，保证规划路径和手持示教装置的特征识别单元在立体视觉模块的有效视场范围内；所述规划路径指的是根据当前机器人的位置及机器人所能到达的有效工作空间范围所规划的设定点形成的路径；

第四步，视觉信号处理模块从机器人控制模块获取机器人末端夹持工具坐标系在机器人基坐标系下的位姿变换矩阵T₁并保存；

第五步，利用位姿测量杆件直观地达到所示教的设定点的位置和姿态，具体为，操作手持示教装置，使手持示教装置的位姿测量杆件的末端点移动到规划路径上的设定点的位置，在保持位姿测量杆件末端点的位置不变的情况下，调整位姿测量杆件至设定的姿态；

第六步，手持示教装置将信号传输给立体视觉模块，控制所述立体视觉模块采集图像，并通过视觉处理***识别、处理特征信息，首先，所述立体视觉模块控制立体视觉成像单元完整清晰地采集当前所述手持示教装置上特征识别单元的图像信息；然后，所述立体视觉模块将采集到的手持示教装置上特征识别单元的图像信息传输给所述视觉信号处理模块；最后，通过所述视觉信号处理模块进行特征识别和数据处理，构建出所述手持示教装置上特征识别单元坐标系在立体视觉坐标系下的位姿变换矩阵T₃；

第七步，基于第一步、第二步、第四步和第六步所求得的位姿变换矩阵T₁、T₂、T₃、T₄的结果，通过所述视觉信号处理模块进行数据处理，获取机器人基坐标系与手持示教装置的位姿测量杆件末端坐标系之间的位姿变换矩阵T₅，进而获取位姿测量杆件末端点在机器人基坐标系下的位姿信息，即所示教的一个设定点在机器人基坐标系下的位姿信息，将该设定点在机器人基坐标系下的位姿信息保存在立体视觉信号处理模块中；

第八步，重复第三步到第七步，直到完成规划路径上所有设定点的示教工作；

第九步，所述视觉信号处理模块将保存的规划路径上的在机器人基坐标系下的所有设定点的位姿信息传输给机器人控制模块，所述机器人控制模块控制机器人复现规划路径上的所有设定点的位姿状态，实现机器人末端夹持工具的末端点的规划轨迹，完成快速示教；

第十步，如果在当前机器人的位置及机器人所能到达的有效工作空间范围外，存在未完成的示教路径的示教工作，则移动机器人到达新的位置，重复第三步到第十步，直到完成示教路径的示教工作；所述示教路径指的是不限于机器人在当前位置下的工作空间范围的所有规划路径。

2.如权利要求1所述的一种基于立体视觉的机器人快速示教方法，其特征在于，所述手持示教装置采用有线传输或无线传输或有线传输和无线传输方式与所述视觉信号处理模块进行信息交互。

3.如权利要求1所述的一种基于立体视觉的机器人示教方法，其特征在于，所述手持示教装置上的特征识别单元至少包括三个不共线的特征信息，其相互位置关系固定且已知，用于确定特征识别单元坐标系的位姿。

4.如权利要求1所述的一种基于立体视觉的机器人快速示教方法，其特征在于，所述第一步中建立位姿变换矩阵T₂的手眼标定具体方法为，首先，将至少带有三个特征点的标定物放在立体视觉模块视场范围内；然后，所述机器人控制模块控制安装有立体视觉模块的机器人末端到达若干位置点和姿态，在保证标定物能被立体视觉模块完整清晰拍摄的前提下，机器人末端每到达一个位置点和姿态进行一次标定物图像采集；最后，在进行至少采集六次标定物图像后，通过视觉信号处理模块进行数据处理，基于经典“两步法”手眼标定算法获取立体视觉坐标系与机器人末端夹持工具坐标系之间的位姿变化矩阵T₂。

5.如权利要求1所述的一种基于立体视觉的机器人快速示教方法，其特征在于，第二步中的位姿变换矩阵T₄的建立方法具体为，首先，将至少包含三个特征点的标定物放在立体视觉模块的视场范围内，获取这些点在立体视觉坐标系下的空间位置信息；其次，操作手持示教装置，使手持示教装置的位姿测量杆件的末端点对准这些特征点，调整位姿测量杆件的姿态，使手持示教装置上的特征识别单元能被立体视觉模块完整清晰拍摄；然后，通过立体视觉模块采集手持示教装置上特征识别单元图像并将手持示教装置上特征识别单元图像传输给视觉信号处理模块，通过视觉信号处理模块进行数据处理，获取在手持示教装置的位姿测量杆件的当前位姿下，手持示教装置上特征识别单元坐标系与立体视觉坐标系的位姿变换矩阵T₃；最后，基于手持示教装置上特征识别单元坐标系在立体视觉坐标系下的位姿变换矩阵T₃和对应的特征点的位置信息，可以建立手持示教装置上的位姿测量杆件末端坐标系与手持示教装置上的特征识别单元坐标系的位姿变换矩阵T₄。

6.如权利要求1所述的一种基于立体视觉的机器人快速示教方法，在第二步和第六步中获取手持示教装置上特征识别单元坐标系在立体视觉坐标系下的位姿变换矩阵T₃的具体方法为：

首先，视觉信号处理模块通过对特征识别单元的图像信息进行数据处理，分别获得代表三个特征信息的点1、点2和点3在立体视觉坐标系下的位置信息，依次为(x₁、y₁、z₁)、(x₂、y₂、z₃)、(x₃、y₃、z₃)；其次，根据三点的空间坐标构建在立体视觉坐标系下的空间向量，假设以点1为公共点，点2和点3分别与点1构成向量，且两向量互相垂直，形成特征识别单元坐标系的X、Y轴，根据右手定则确定Z轴，如式(1)所示：

取向量的单位向量，分别为式(2)所示：

同时，立体视觉坐标系上单位向量分别为式(3)所示:

再由旋转矩阵的定义，建立特征识别单元坐标系相对于立体视觉坐标系的旋转矩阵R₃，如式(4)所示：

最后，选择点1的坐标值(x₁、y₁、z₁)作为特征识别单元坐标系的原点，即该坐标系的平移向量，最终构建出立体视觉坐标系与特征识别单元坐标系的位姿变换矩阵T₃，如式(5)所示：

除上述构造空间向量时所指定的垂直关系外，三个点还可以是除了共线之外的任意位置关系，在求取位姿变换矩阵时加入几何变换即可。

7.如权利要求1所述的一种基于立体视觉的机器人快速示教方法，其特征在于，在第三步调整机器人末端位姿时，所述机器人在保证手持示教装置上的特征识别单元能被立体视觉模块完整清晰拍摄的前提下，可以呈现任意姿态。

8.如权利要求5所述的一种基于立体视觉的机器人快速示教方法，其特征在于，建立位姿变换矩阵T₄时，所述手持示教装置的位姿测量杆件在保证手持示教装置上的特征识别单元可被立体视觉模块识别到的前提下，可以以任意姿态对准标定物上的特征点。

9.如权利要求5所述的一种基于立体视觉的机器人快速示教方法，其特征在于，所述手持示教装置上的位姿测量杆件末端坐标系与手持示教装置上的特征识别单元坐标系的位姿变换矩阵T₄可用最优解估计方法进行求算。