CN115741720A - 基于双目视觉技术及lm算法的机器人的零点标定系统及其方法 - Google Patents

Abstract

本发明公开了一种基于双目视觉技术及LM算法的机器人的零点标定系统，包括机器人、运动控制器、上位机、靶标、固定安装在机器人末端的双目相机；还公开了一种基于双目视觉技术及LM算法的机器人的零点标定方法，包括：S1：根据机器人本体上的标志完成各关节零点的初步标定；S2：记录处机器人末端到达若干点时机器人的关节角度值及目标点相对于相机的空间坐标；S3：建立机器人正运动学模型和误差模型，获取各关节的当前零点位置相对于实际零点位置的偏差；S4：通过LM算法对待标定参数进行迭代优化，获得标定后的参数；S5：将获得标定后的参数输入到工业机器人运动控制器中进行误差补偿，完成机器人零点标定。

Description

基于双目视觉技术及LM算法的机器人的零点标定系统及其 方法

技术领域

本发明涉及机器人领域，特别是涉及一种基于双目视觉技术及LM算法的机器人的零点标定系统及其方法。

背景技术

零点是机器人坐标系的基准,没有零点机器人就没有办法判断自身的位置。通常工业机器人在出厂之前均会对机器人的机械参数进行标定，给出工业机器人各轴的参数及零点位置，但在一些意外的情况下可能出现零点丢失的问题，如突然断电、电池没电、超越机械极限位置、与环境发生碰撞、手动移动机器人关节等情况，目前多数情况下是人工根据机器人本体上的零点标志位置进行回零标记，这种回零的方法简单，但是标零位置精确较低，同时，在一些情况下，机器人的安装位置不易查看零点标记位置，或者机器人本体上的标志位置不够清晰，则进行机器人的标零会变得非常困难。

因此亟需提供一种新型的基于双目视觉技术及LM算法的机器人的零点标定系统及其方法来解决上述问题。

发明内容

本发明所要解决的技术问题是提供一种基于双目视觉技术及LM算法的机器人的零点标定系统及其方法，可以提高其零点标定精度，并且该方法结合对机器人通讯模块的二次开发，可实现机器人自动化标定，提高标定速度，降低标定成本。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种基于双目视觉技术及LM算法的机器人的零点标定系统，包括机器人、运动控制器、上位机、靶标、固定安装在机器人末端的双目相机；

所述机器人用于执行运动规划，带动双目相机运动到多个指定位置；

所述靶标固定安装，其中心点相对于机器人基坐标系的空间坐标为(x_p，y_p，z_p)，作为相机检测的目标点；

所述运动控制器用于搭载服务器端程序，建立与上位机之间的数据交互，控制机器人本体运动；

所述上位机用于运行标定算法程序、运动控制指令发送，对双目相机采集目标点的图像进行处理和与运动控制器进行数据交互。

在本发明一个较佳实施例中，所述机器人运动控制器采用基于TCP/IP协议的套接字通讯协议建立与上位机之间的通讯连接，机器人运动控制器作为套接字通讯的服务器端，上位机作为客户端。

为解决上述技术问题，本发明采用的另一个技术方案是：提供一种基于双目视觉技术及LM算法的机器人的零点标定方法，采用如上所述的基于双目视觉技术及LM算法的机器人的零点标定系统，包括以下步骤：

S1：根据机器人本体上的标志完成各关节零点的初步标定；

S2：在工业机器人工作空间中均匀地选择若干点，使机器人末端到达这些点，并使用双目相机采集靶标的图像信息，根据图像使用双目测距技术计算靶标中心相对于相机的空间坐标，记录每个点处机器人的关节角度值及目标点相对于相机的空间坐标。

S3：建立机器人正运动学模型和误差模型，获取待标定的参数，即各关节的当前零点位置相对于实际零点位置的偏差(Δθ₁，Δθ₂，Δθ₃，Δθ₄，Δθ₅，Δθ₆)；

S4：根据步骤S2中记录与采集的数据与步骤三中的机器人正运动学模型与误差模型，通过LM算法对待标定参数进行迭代优化，获得标定后的参数；

S5：将步骤S4中获得标定后的参数输入到工业机器人运动控制器中进行误差补偿，完成机器人零点标定。

在本发明一个较佳实施例中，在步骤S2中，根据双目相机采集到的靶标图像获取靶标中心空间坐标的具体包括如下步骤：

S201：基于双目测距理论利用棋盘格对双目相机进行相机标定，获取左右相机的内参矩阵M_L、M_R与畸变参数k、p，对左右相机进行双目标定获得左相机相对于右相机的旋转矩阵R与平移向量T；

S202：根据相机标定后获得的单目内参数据和双目相对位置关系，分别对左右视图进行消除畸变和行对准，使得左右视图的成像原点坐标一致、两相机光轴平行、左右成像平面共面、对极线行对齐，获得矫正后的相机；

S203：使用半全局立体匹配算法对立体校正后的故障电弧图像进行立体匹配，获得靶标图像的视差图；

S204：通过图像处理技术获取图像中圆形靶标中心点的像素坐标；

S205：根据S204中的像素坐标在S203中找到对应像素点的视差，根据S201中的相机内参矩阵与平移向量获得相机焦距f与基线B，获得靶标中心点相对于相机的空间坐标(x_m，y_m，z_m)。

在本发明一个较佳实施例中，步骤S3的具体步骤包括：

S301：根据机器人连杆坐标系建立机器人DH运动学模型，连杆长度a_i是z_i-1沿着x_i轴方向移动到z_i的距离；连杆扭角α_i是z_i-1绕着x_i轴旋转到z_i的距离；关节偏距d_i是x_i-1沿着z_i-1轴方向移动到x_i的距离；关节转角θ_i是x_i-1绕着z_i-1轴旋转到x_i的角度；相邻关节的齐次变换矩阵：

其中，cθ_i＝cosθ_i，sθ_i＝sinθ_i；对于六自由度的关节机器人，机器人正运动学模型为：

S302：相机的中心位置为机器人运动学方程右乘一个平移变换矩阵，即：

其中，

表示相机中心坐标系相对机器人基坐标系的变换矩阵，

表示相机中心坐标系相对机器人第6关节坐标系的变换矩阵，p_cx，p_cy，p_cz表示主相机相对于机器人第六关节坐标系的位置坐标；

通过计算可得到相机中心相对于机器人基坐标系的空间坐标：

x_nc＝p_cxn_x+p_cyo_x+p_cza_x+p_x

y_nc＝p_cxn_y+p_cyo_y+p_cza_y+p_y

z_nc＝p_cxn_z+p_cyo_z+p_cza_z+p_z

得到相机中心的名义位置P_n＝(x_nc，y_nc，z_nc)^T；

S303：根据步骤S2中计算获得的目标点相对于相机的空间坐标(x_m，y_m，z_m)与靶标中心点相对于机器人基坐标系的空间坐标(x_p，y_p，z_p)，得到相机相对于机器人极坐标系的实际空间坐标：

x_cc＝x_p-x_m

y_cc＝y_p-y_m

z_cc＝z_p-z_m

得到相机中心的实际位置P_c＝(x_cc，y_cc，z_cc)^T；

S304：获得相机位置误差：

其中ΔP＝(ΔP_x，ΔP_y，ΔP_z)^T；

对运动学方程进行全微分处理，相机位置误差可以近似表示为：

ΔP＝J_δΔδ

其中，J_δ是一个3×6型矩阵，称为误差系数矩阵，即

Δδ是一个由需要辨识的几何参数误差组成的1×6的矢量，即：

Δδ＝(Δθ₁，Δθ₂，Δθ₃，Δθ₄，Δθ₅，Δθ₆)^T；

S305：根据步骤S2中获得的L组数据及上述计算过程，获得相机位置误差的总和，作为目标函数：

其中P_n(θ_i，Δδ)表示在Δθ＝Δδ情况下第i组数据计算得到的相机名义位置。

在本发明一个较佳实施例中，步骤S4的具体步骤包括：

使用LM算法不断对Δδ迭代更新，最小化目标函数S，每次迭代后计算S直到获得满足要求S＜e的标定参数Δδ，其中e判断条件阈值，LM算法迭代公式为：

其中k为迭代次数，λ为阻尼系数，λ＞0；I为单位矩阵；

在迭代过程中，LM算法通过引入参数μ对λ值的大小进行调整，来控制计算精度，使参数误差不断接近准确值，其中μ＞1。

本发明的有益效果是：本发明基于双目视觉技术及LM算法的机器人的零点标定方法，使用双目测距的方法代替常用的激光测距方法，降低机器人零点标定的成本，使用LM算法结合，标定精度较高；同时结合网络通讯技术，设计机器人零点标定软件，可实现自动化的零点标定。

附图说明

图1是本发明基于双目视觉技术及LM算法的机器人的零点标定系统一较佳实施例的系统结构示意图；

图2是所述FANUC机器人连杆坐标系示意图；

图3是基于LM算法的参数辨识流程图；

图4是所述上位机与机器人运动控制器通讯机制示意图；

图5是本发明FANUC机器人的零点标定界面示意图；

图6是误差随迭代次数的变化曲线示意图；

图7是误差标定前后的误差对比示意图。

附图中各部件的标记如下：1、机器人，2、双目相机，3、靶标，4、运动控制器，5、上位机。

具体实施方式

下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征能更易于被本领域技术人员理解，从而对本发明的保护范围做出更为清楚明确的界定。

请参阅图1，本发明实施例包括：

一种基于双目视觉技术及LM算法的机器人的零点标定系统，包括机器人1、运动控制器4、上位机5、靶标3、固定安装在机器人1末端的双目相机2。所述机器人1用于执行运动规划，带动双目相机2运动到多个指定位置，本示例中所述机器人1为6R型FANUC工业机器人。所述靶标3固定安装，其中心点相对与机器人基坐标系的空间坐标为(x_p，y_p，z_p)，作为相机检测的目标点；双目相机2固定安装在机器人1末端上，用于采集目标点的图像信息，相机坐标系相对与机器人末端坐标系的齐次变换矩阵为

所述运动控制器4用于搭载服务器端程序，建立与上位机5之间的数据交互，控制机器人1本体运动；所述上位机5用于运行标定算法程序、运动控制指令发送，对双目相机2采集目标点的图像进行处理和与运动控制器4进行数据交互。

其中，所述机器人运动控制器4采用基于TCP/IP协议的套接字通讯协议建立与上位机5之间的通讯连接，机器人运动控制器4作为套接字通讯的服务器端，上位机5作为客户端。

该标定系统中采用双目视觉系统，双目视觉系统用于测量目标点的三维坐标信息，并且采用眼在手(Eye-in-Hand)的安装方式，其中左相机是主相机，目标点相对于相机的空间坐标，即目标点相对于主相机光心的空间坐标。

采用该零点标定系统，本发明示例中，还提供一种基于双目视觉技术及LM算法的机器人的零点标定方法，包括以下步骤：

S1：根据机器人本体上的标志完成各关节零点的初步标定；

S2：在工业机器人工作空间中均匀地选择若干点，使机器人末端到达这些点，使用双目相机采集靶标的图像信息，并根据图像使用双目测距技术计算靶标中心相对于相机的空间坐标，记录每个点机器人的关节角度值及目标点相对于相机的空间坐标。根据双目相机采集到的靶标图像获取靶标中心空间坐标的具体包括如下步骤：

S201：基于双目测距理论利用棋盘格对双目相机进行相机标定，获取左右相机的内参矩阵M_L、M_R与畸变参数k、p，对左右相机进行双目标定获得左相机相对于右相机的旋转矩阵R与平移向量T；

S202：根据相机标定后获得的单目内参数据和双目相对位置关系，分别对左右视图进行消除畸变和行对准，使得左右视图的成像原点坐标一致、两相机光轴平行、左右成像平面共面、对极线行对齐，获得矫正后的相机靶标图像；

S203：使用半全局立体匹配算法对立体校正后的相机靶标图像进行立体匹配，获得靶标图像的视差图；

S204：通过图像处理技术获取图像中圆形靶标中心点的像素坐标；

S205：根据S204中的像素坐标在S203中找到对应像素点的视差，根据S201中的相机内参矩阵与平移向量获得相机焦距f与基线B，获得靶标中心点相对于相机的空间坐标(x_m，y_m，z_m)。

S3：建立机器人正运动学模型和误差模型，获取待标定的参数，即各关节的当前零点位置相对于实际零点位置的偏差(Δθ₁，Δθ₂，Δθ₃，Δθ₄，Δθ₅，Δθ₆)；

S4：根据步骤S2中记录与采集的数据与步骤三中的机器人正运动学模型与误差模型，通过LM算法对待标定参数进行迭代优化，获得标定后的参数；

S5：将步骤S4中获得标定后的参数输入到工业机器人运动控制器中进行误差补偿，完成机器人零点标定。

在步骤S3中具体包括以下过程：根据如2图所示的机器人连杆坐标系建立机器人DH运动学模型，DH参数表如表1所示。

表1 FANUC M-20iA六自由度工业机器人DH参数表

其中，连杆长度a_i是z_i-1沿着x_i轴方向移动到z_i的距离；连杆扭角α_i是z_i-1绕着x_i轴旋转到z_i的距离；关节偏距d_i是x_i-1沿着z_i-1轴方向移动到x_i的距离；关节转角θ_i是x_i-1绕着z_i-1轴旋转到x_i的角度。

根据空间坐标转换原理，相邻两个坐标系i-1和i的坐标转换方程可以用齐次变换矩阵的乘积形式来描述：

式中，

为坐标转换关系式；Rot为旋转变换矩阵；Trans为平移变换矩阵。

展开即可得到相邻关节的齐次变换矩阵：

其中，cθ_i＝cosθ_i，sθ_i＝sinθ_i，式中其余情况以此类推。

对于六自由度的关节机器人，机器人正运动学模型为：

在测量过程中，主相机的中心位置并不与机器人第六轴坐标系的原点位置重合，可以将相机的中心位置看作工具坐标系中心位置，即相机的中心位置为机器人运动学方程右乘一个平移变换矩阵，即：

其中，

表示相机中心坐标系相对机器人基坐标系的变换矩阵，

表示相机中心坐标系相对机器人第6关节坐标系的变换矩阵，p_cx、p_cy、p_cz表示主相机相对于机器人第六关节坐标系的位置坐标。

通过计算可得到相机中心相对于机器人基坐标系的空间坐标：

x_nc＝p_cxn_x+p_cyo_x+p_cza_x+p_x

y_nc＝p_cxn_y+p_cyo_y+p_cza_y+p_y

z_nc＝p_cxn_z+p_cyo_z+p_cza_z+p_z

得到相机中心的名义位置P_n＝(x_nc，y_nc，z_nc)^T。

根据步骤S2中计算获得的目标点相对于相机的空间坐标(x_m，y_m，z_m)与靶标中心点相对于机器人基坐标系的空间坐标(x_p，y_p，z_p)，可得到相机相对于机器人极坐标系的实际空间坐标：

x_cc＝x_p-x_m

y_cc＝y_p-y_m

z_cc＝z_p-z_m

得到相机中心的实际位置P_c＝(x_cc，y_cc，z_cc)^T。

进一步可以获得相机位置误差：

其中ΔP＝(ΔP_x，ΔP_y，ΔP_z)^T。

经过初步标定的Δθ_i误差较小，可以利用微分运动学模型来近似代替误差方程，即对运动学方程进行全微分处理，相机位置误差可以近似表示为：

ΔP＝J_δΔδ

其中，J_δ是一个3×6型矩阵，称为误差系数矩阵，即

Δδ是一个由需要辨识的几何参数误差组成的1×6的矢量，即：

Δδ＝(Δθ₁,Δθ₂,Δθ₃,Δθ₄,Δθ₅,Δθ₆)^T

根据步骤S2中获得的L组数据及上述计算过程，获得相机位置误差的总和，作为目标函数：

其中P_n(θ_i，Δδ)表示在Δθ＝Δδ情况下第i组数据计算得到的相机名义位置。

在步骤S4中根据LM算法进行参数标定，具体包括以下过程：使用LM算法不断对Δδ迭代更新，最小化目标函数S，每次迭代后计算S直到获得满足要求S＜e的标定参数Δδ，其中e判断条件阈值，LM算法迭代公式为：

其中k为迭代次数，μ为阻尼系数，μ＞0；I为单位矩阵。

在迭代过程中，LM算法通过引入参数μ对λ值的大小进行调整，来控制计算精度，使参数误差不断接近准确值，其中μ＞1，当第k+1次迭代后的S_k+1小于第k次迭代后的S_k时，λ＝λ/μ，减小阻尼系数λ，否则λ＝λμ，增大阻尼系数λ。当λ较大时，算法接近梯度下降法，发挥全局特性，保证全局收敛；当λ较小时，算法接近Gauss-Newton法，发挥局部收敛。具体的参数辨识流程图如图3所示。

如图4所示，展示了上位机与机器人运动控制器间的通讯机制，FANUC机器人在搭载通讯模块后支持基于TCP/IP协议的套接字通讯协议，采用基于TCP/IP协议的套接字通讯协议建立FANUC机器人运动控制器与上位机之间的连接，数据传输速快、安全且不容易丢包。机器人运动控制器作为套接字通讯的服务器端，上位机作为客户端。通讯流程:服务器创建套接字，用这个套接字完成通信的监听；绑定一个端口号和IP地址；服务器调用监听函数，使服务器的这个端口和IP处于监听状态，等待客户机的连接；客户端创建套接字，设定远程IP和端口；客户端调用连接函数连接远程服务器端指定的端口；服务器使用接受函数来接受远程客户端的连接，建立起与客户端之间的通信；建立连接以后，客户端和服务器端可以用读写函数读取套接字中对方发送来的数据或向套接字中写入数据；完成通信以后，用关闭函数关闭套接字连接。

其中客户端程序基于Qt软件平台开发，并使用计算机基础语言C++编写，程序运行速度快，数据传输效率高；服务器端程序须先在上位机上基于ROBOGUIDE软件使用FANUC基础语言KAREL编写并编译，然后再将编译后的文件导入机器人运动控制器中使用，服务器程序从创建到运行步骤如下：

S01：使用RONBOGUIDE软件创建程序的源代码文件，未编译的源代码程序为.kl形式文件。

S02：对源代码文件进行编译，编译后的源代码程序为.pc形式文件。

S03：将编译后的.pc程序存储到USB储存器之后导入机器人控制器。

S04：使用示教器调用并运行.pc程序。

如图5所示，其中示意出了机器人零点标定程序界面示意图，基于该程序界面可设定远程通讯IP和端口号，其中建立连接和断开连接按钮实现对上位机与机器人通讯的连接与断开控制，当连接成功时，文本提示框中会打印出提示信息“已成功连接到服务器…”，当断开连接时，文本提示框中会打印出提示信息“已断开与服务器的连接…”。在成功建立通讯之后，将采集的数据文件导入程序，点击开始标定按钮，在数据处理完成之后会在文本提示框中会打印出提示信息“数据处理已完成”，并依次打印出计算结果，然后根据计算结果自动向机器人运动控制器发送控制指令，机器人完成控制指令后，文本提示框中会打印出提示信息“零点标定已完成…”。基于此软件平台及上述网络通讯的搭建可以提高机器人零点标定的自动化程度。

为验证本发明的可行性，对FANUC六自由度串联工业机器人进行仿真验证，将所有的测量点分成两部分，一部分作为辨识点来辨识几何参数误差，另一部分作为检验点，只用来检验辨识效果。在500个测量点中选取前300个作为辨识点，用来进参数误差的辨识，剩下的200个测量点作为检验点。将300个点对应的机器人关节角度值以及实际位置Pc数据带入到LM算法中，利用算法程序迭代计算出辨识结果如表2所示，根据LM算法得到的零点误差来修正理论模型。标定过程中误差随迭代次数的变化曲线如图6所示。

表2零点标定结果

通过计算修正模型求出的200个检验点的理论位置和测量的实际位置的差值来检验几何参数标定效果，检验点在经过LM算法标定前与标定后的误差如图7所示。经过几何参数误差辨识后，机器人的平均绝对定位精度相对于标定前的85.9mm提高到了0.427mm，有非常显著的提升。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (6)

1.一种基于双目视觉技术及LM算法的机器人的零点标定系统，其特征在于，包括机器人、运动控制器、上位机、靶标、固定安装在机器人末端的双目相机；

所述机器人用于执行运动规划，带动双目相机运动到多个指定位置；

所述靶标固定安装，其中心点相对于机器人基坐标系的空间坐标为(x_p，y_p，z_p)，作为相机检测的目标点；

所述运动控制器用于搭载服务器端程序，建立与上位机之间的数据交互，控制机器人本体运动；

所述上位机用于运行标定算法程序、运动控制指令发送，对双目相机采集目标点的图像进行处理和与运动控制器进行数据交互。

2.根据权利要求1所述的基于双目视觉技术及LM算法的机器人的零点标定系统，其特征在于，所述机器人运动控制器采用基于TCP/IP协议的套接字通讯协议建立与上位机之间的通讯连接，机器人运动控制器作为套接字通讯的服务器端，上位机作为客户端。

3.一种基于双目视觉技术及LM算法的机器人的零点标定方法，采用如权利要求1或2所述的基于双目视觉技术及LM算法的机器人的零点标定系统，其特征在于，包括以下步骤：

S1：根据机器人本体上的标志完成各关节零点的初步标定；

S2：在工业机器人工作空间中均匀地选择若干点，使机器人末端到达这些点，并使用双目相机采集靶标的图像信息，根据图像使用双目测距技术计算靶标中心相对于相机的空间坐标，记录每个点处机器人的关节角度值及目标点相对于相机的空间坐标；

S3：建立机器人正运动学模型和误差模型，获取待标定的参数，即各关节的当前零点位置相对于实际零点位置的偏差(Δθ₁，Δθ₂，Δθ₃，Δθ₄，Δθ₅，Δθ₆)；

S4：根据步骤S2中记录与采集的数据与步骤S3中的机器人正运动学模型与误差模型，通过LM算法对待标定参数进行迭代优化，获得标定后的参数；

S5：将步骤S4中获得标定后的参数输入到工业机器人运动控制器中进行误差补偿，完成机器人零点标定。

4.根据权利要求3所述的基于双目视觉及LM算法的机器人的零点标定方法，其特征在于，在步骤S2中，根据双目相机采集到的靶标图像获取靶标中心空间坐标的具体包括如下步骤：

S201：基于双目测距理论利用棋盘格对双目相机进行相机标定，获取左右相机的内参矩阵M_L、M_R与畸变参数k、p，对左右相机进行双目标定获得左相机相对于右相机的旋转矩阵R与平移向量T；

S202：根据相机标定后获得的单目内参数据和双目相对位置关系，分别对左右视图进行消除畸变和行对准，使得左右视图的成像原点坐标一致、两相机光轴平行、左右成像平面共面、对极线行对齐，获得矫正后的相机靶标图像；

S203：使用半全局立体匹配算法对立体校正后的相机靶标图像进行立体匹配，获得靶标图像的视差图；

S204：通过图像处理技术获取图像中圆形靶标中心点的像素坐标；

S205：根据S204中的像素坐标在S203中找到对应像素点的视差，根据S201中的相机内参矩阵与平移向量获得相机焦距f与基线B，获得靶标中心点相对于相机的空间坐标(x_m，y_m，z_m)。

5.根据权利要求3所述的基于双目视觉及LM算法的机器人的零点标定方法，其特征在于，步骤S3的具体步骤包括：

S301：根据机器人连杆坐标系建立机器人DH运动学模型，连杆长度a_i是z_i-1沿着x_i轴方向移动到z_i的距离；连杆扭角α_i是z_i-1绕着x_i轴旋转到z_i的距离；关节偏距d_i是x_i-1沿着z_i-1轴方向移动到x_i的距离；关节转角θ_i是x_i-1绕着z_i-1轴旋转到x_i的角度；相邻关节的齐次变换矩阵：

其中，cθ_i＝cosθ_i，sθ_i＝sinθ_i；对于六自由度的关节机器人，机器人正运动学模型为：

S302：相机的中心位置为机器人运动学方程右乘一个平移变换矩阵，即：

其中，

表示相机中心坐标系相对机器人基坐标系的变换矩阵，

表示相机中心坐标系相对机器人第6关节坐标系的变换矩阵，p_cx，p_cy，p_cz表示主相机相对于机器人第六关节坐标系的位置坐标；

通过计算可得到相机中心相对于机器人基坐标系的空间坐标：

x_nc＝p_cxn_x+p_cyo_x+p_cza_x+p_x

y_nc＝p_cxn_y+p_cyo_y+p_cza_y+p_y

z_nc＝p_cxn_z+p_cyo_z+p_cza_z+p_z

得到相机中心的名义位置P_n＝(x_nc，y_nc，z_nc)^T；

S303：根据步骤S2中计算获得的目标点相对于相机的空间坐标(x_m，y_m，z_m)与靶标中心点相对于机器人基坐标系的空间坐标(x_p，y_p，z_p)，得到相机相对于机器人极坐标系的实际空间坐标：

x_cc＝x_p-x_m

y_cc＝y_p-y_m

z_cc＝z_p-z_m

得到相机中心的实际位置P_c＝(x_cc，y_cc，z_cc)^T；

S304：获得相机位置误差：

其中ΔP＝(ΔP_x，ΔP_y，ΔP_z)^T；

对运动学方程进行全微分处理，相机位置误差可以近似表示为：

ΔP＝J_δΔδ

其中，J_δ是一个3×6型矩阵，称为误差系数矩阵，即

Δδ是一个由需要辨识的几何参数误差组成的1×6的矢量，即：

Δδ＝(Δθ₁,Δθ₂,Δθ₃，Δθ₄，Δθ₅,Δθ₆)T；

S305：根据步骤S2中获得的L组数据及上述计算过程，获得相机位置误差的总和，作为目标函数：

其中P_n(θ_i，Δδ)表示在Δθ＝Δδ情况下第i组数据计算得到的相机名义位置。

6.根据权利要求3所述的基于双目视觉技术及LM算法的机器人的零点标定方法，其特征在于，步骤S4的具体步骤包括：

使用LM算法不断对Δδ迭代更新，最小化目标函数S，每次迭代后计算S直到获得满足要求S＜e的标定参数Δδ，其中e判断条件阈值，LM算法迭代公式为：

其中k为迭代次数，λ为阻尼系数，λ＞0；I为单位矩阵；

在迭代过程中，LM算法通过引入参数μ对λ值的大小进行调整，来控制计算精度，使参数误差不断接近准确值，其中μ＞1。

Images