一种基于离群样本筛选的双层非线性优化单目视觉手眼标定方法

本发明属于计算机视觉和机器人，具体涉及一种基于离群样本筛选的双层非线性优化单目视觉手眼标定方法。

背景技术：

1、随着工业智能化的发展，视觉技术与工业机器人相结合的案例层出不穷。将视觉系统安装在机器人末端的方法称为眼在手上，这种方法得益于机器人的灵活性。它比固定在一个位置的视觉传感器具有更宽的操作范围。目前广泛应用于各类机器人制造系统，典型应用于机器人焊接、磨削、装配、航空航天、汽车等领域。但是对视觉机器人而言，手眼标定的精度会直接影响到机器人末端的最终定位精度，因此手眼标定的稳定性与精度则显得尤为重要。

2、目前在手眼标定方法上，主要有三种方法——tsai的标定法、dual-quaternion标定法和张正友标定法。tsai的标定法通过对多组已知的机器人末端执行器和相机之间的关系进行非线性最小二乘求解，得到标定矩阵。dual-quaternion标定法通过将机器人末端执行器和相机之间的运动关系表示为双四元数，然后通过非线性优化求解双四元数的参数，从而得到标定矩阵。张正友标定法是一种基于相机内参和外参的手眼标定方法，该方法通过对相机的内参和外参进行标定，并通过相机测量和机器人运动轨迹之间的优化匹配，从而得到标定矩阵。

3、目前关于手眼标定的求解，例如公告号为cn115781698a的中国发明专利公开的“分层式手眼标定机器人运动位姿自动生成方法、系统、设备和介质”，该发明公开了手眼标定的具体流程，控制机器人移动到目标点位，拍摄标定板照片；检测棋盘格角点且重投影误差小于预设值，若在预设范围内，则执行手眼标定任务；否则调整预设的参数，并重新计算机器人点位数据，然后重新执行运动-拍照-校验任务；根据记录的机器人位姿和相机-标定板位姿执行手眼标定解算。然而，这种方法求解出来的手眼矩阵没有进行优化，会有误差的干扰导致手眼标定的不准确，并且机器人系统的精度和鲁棒性受到影响。

技术实现思路

1、针对上述存在的问题，本发明设计一种基于非线性优化的原理设计出基于离群样本筛选的双层非线性优化法，对手眼矩阵初始值使用传统lm方法进行初次优化后，再使用加入离群样本筛选方法的lm优化法进行二次优化，该方法能够在不需要特制靶标的情况下完成标定，减少了机器人视觉系统中存在的随机误差，所提出的标定方法精度满足视觉机器人在工业领域的工作需求。

2、为实现上述目标，本发明的主要技术方案如下：

3、首先，选取棋盘格标定板，固定棋盘格标定板位置，改变机器人末端位置和姿态，使得棋盘格标定板完整的呈现在单目相机视野内；然后，求解视野内棋盘格标定板上特征点p在机器人基坐标1-1下的表达式并获得手眼矩阵的初始值最后，利用lm非线性优化对手眼矩阵初始值进行初次优化，得到第一次优化后的手眼矩阵再结合样本筛选进行二次非线性优化，得到第二次优化后的手眼矩阵在迭代过程中将随机误差较大的点移出样本点集，获得第二次优化后的手眼矩阵即可作为手眼标定的最终结果，具体包括以下步骤：

4、步骤1、在机器人系统中创建三个坐标系分别为：相机坐标系o-xcyczc(1-3)，机器人末端tcp坐标系o-xeyeze(1-2)和机器人基坐标系o-xbybzb(1-1)；

5、步骤2、求解特征点p在机器人基坐标系1-1下坐标表达式，其至少包括以下两个步骤：

6、step2-1：将棋盘格标定板上的第一个角点即特征点p在相机坐标系1-3下的坐标记作pc，在机器人基坐标系1-1下的坐标记作pb，使用pnp算法求解特征点p在相机坐标系1-3中的坐标pc；

7、step2-2：根据图1所示的坐标转换关系可通过式(1)

8、

9、获得特征点在机器人基坐标系1-1下的坐标pb；

10、步骤3、手眼矩阵初始值的确定，其至少包括以下三个步骤：

11、step3-1：首先变更机器人末端的位姿，使得单目相机在不同位姿下对特征点p进行拍照，采集n(n≥4)张图片，由式(2)可以求解在不同姿态下特征点p在世界坐标系下的坐标pb：

12、

13、n张图片中特征点分别为：p1,p2,…,pn，其在机器人基坐标系1-1中的位置始终未变，所以式(2)便可改写为：

14、

15、step3-2：在式(3)中，和分别表示第i个和第j个样本中从机器人末端tcp坐标系1-2到机器人基坐标系1-1的变换矩阵(i＝1,2,...,n-1,j＝i+1,i+2,...,n)；

16、将和改写为式(4)：

17、

18、由式(3)和式(4)得到矩阵方程，如式(5)所示：

19、

20、step3-3：令a为左侧系数矩阵，x为手眼矩阵未知量，b为右侧系数矩阵；在a和b已知的情况下，求解x的问题转变为求矩阵方程的解的问题；由于所采集的图片数量对于求解以上矩阵方程而言，存在冗余数据，为使最终误差最小，使用最小二乘法求解矩阵方程，矩阵方程的最小二乘解如式(6)所示：

21、x＝(ata)-1atb                                  (6)

22、在解出初始手眼矩阵x后，也就是手眼矩阵初始值手眼矩阵的旋转分量rh2e和平移th2e分量如式(7)所示：

23、

24、步骤4、初次lm非线性优化，其至少包括以下五个步骤：

25、step4-1：将步骤3中求解出的手眼矩阵初始值表示为式(8)所示：

26、

27、step4-2：对手眼矩阵中的旋转分量rh2e进行欧拉角变换，求解出旋转过程中相机坐标系1-3的x，y，z轴绕机器人末端tcp坐标系1-2的x，y，z轴的变换角度θx，θy，θz，如式(9)所示：

28、

29、step4-3：利用欧拉角逆变换求旋转矩阵r，如式(10)所示：

30、

31、故手眼矩阵的初始值如式(11)所示：

32、

33、step4-4：构建误差函数w1和损失函数l1：由于特征点p在世界坐标系下坐标值恒定，建立初始优化模型h1，初始优化模型h1的误差函数w1如式(12)所示：

34、

35、初始优化模型h1的损失函数l1如式(13)所示：

36、

37、step4-5：由式(12)和式(13)的误差函数w1和损失函数l1，使用lm算法对手眼矩阵初始值进行第一次优化，得到第一次优化后手眼矩阵的欧拉角的表示形式，如式(14)所示：

38、[θx θy θz tx ty tz]t                     (14)

39、步骤5、结合样本筛选的二次非线性优化，其至少包括以下六个步骤：

40、step5-1：将第一次优化后的手眼矩阵的欧拉角表示形式转换为矩阵表示的形式，如式(15)所示：

41、

42、step5-2：采用求均值的方法，得到标准点ps的在机器人基坐标系1-1下的坐标值，如式(16)所示：

43、

44、step5-3：修正初始优化模型h1，代入标准点ps的坐标；修正后的误差函数w2，如式(17)所示：

45、

46、修正后的损失函数l2，如式(18)所示：

47、

48、step5-4：初始化第二次优化中的迭代相关参数，并代入第一次优化后的手眼矩阵为第二次优化的迭代初始值；

49、step5-5：判断迭代次数m和误差e是否满足停止要求，若误差e小于设定的误差阈值e′，则停止迭代，否则得到本轮迭代的步长δh；若步长δh小于设定的步长阈值δh′，则停止迭代，否则更新手眼矩阵和lm算法迭代相关参数；

50、step5-6：对每次迭代中的样本点进行离群点标记；获取未进行样本点筛选的迭代次数m，如果次数达到s，对样本点进行筛选；所述筛选步骤是指遍历所有样本以判断上述s次迭代中样本的标记次数，将标记次数超过0.5*s的样本点视为误差较大的样本点，将所有误差较大的样本点从样本点集合中移除。

51、本发明的特点和有益效果是：

52、1、本发明所述的一种基于离群样本筛选的双层非线性优化单目视觉手眼标定方法，相较于传统方法标定的手眼矩阵，基于离群样本筛选的双层非线性优化单目视觉手眼标定方法能够提升机器人视觉系统的稳定性，并且精度上满足工业要求。

53、2、该算法引入了带离群点检测的双层优化方法，优化过程中采用了欧拉角变换来保证手眼矩阵的正交性，能够准确标定视觉机器人的手眼矩阵，减小了系统随机误差的影响，提高了手眼标定的鲁棒性。