一种通过双相机互视的手眼标定方法

本发明属于机器人视觉领域，具体涉及一种通过双相机互视的手眼标定方法。

背景技术：

1、手眼标定是机器人视觉领域的一项关键技术，用于计算机器人与相机的相对位姿关系，并将相机感知到的信息转换到机器人坐标系下以供机器人理解感知到的信息。按照相机相对于机械臂的安装位置，可分为眼在手上标定(eye in hand calibration)与眼在手外标定(eye to hand calibration)两种。手眼标定的准确性，直接影响了机器人运动的精度和作业的质量。

2、现有的手眼标定方法有很多，但往往存在以下问题：(1)需要较为精密的、已知尺寸的标定物进行标定，如专利cn201710054683公开了一种基于可变标定目标的主动视觉相机标定方法，采用显示在液晶面板上的标定板，标定物维护成本较高，而且不适用于户外、水下等机器人需要移动并连续作业、环境相对恶劣、设备容易被损坏的作业场景。(2)标定方法往往依次标定单个相机，如专利cn202310986672公开了一种双目相机标定方法、装置及电子设备，是对单个相机进行依次标定，标定的效率较低，不适用于同时存在眼在手外与眼在手上具有多个相机的视觉系统，且不能在线标定。因此，迫切需要研发一种无需额外标定物，可以同时标定双相机和在线标定，且标定效率高的手眼标定方法。

技术实现思路

1、本发明的目的在于克服现有技术中存在的缺点，提供一种通过双相机互视的手眼标定方法，无需额外标定物，可以同时标定双相机和在线标定，而且通过卡尔曼滤波迭代对标定结果进行修正，标定效率高。

2、本发明的目的通过下述技术方案实现：

3、一种通过双相机互视的手眼标定方法，包括下述步骤：

4、(1)双相机包括设置在机械臂上的眼在手外相机即外置相机、眼在手上相机即末端相机，且双相机处于互视的姿态；

5、(2)机械臂末端沿着外置相机的坐标轴，进行等距离的平移运动或等旋转角的旋转运动，以机械臂基座坐标系为参考系，记录每次运动后末端位姿的变换；

6、(3)使用外置相机观测末端相机，构建末端相机位姿的状态转移方程和状态观测方程，使用卡尔曼滤波修正末端相机位姿；

7、(4)使用末端相机观测外置相机，利用坐标变换的性质，构建外置相机位姿的状态转移方程和状态观测方程，使用卡尔曼滤波修正外置相机位姿；

8、(5)构建标定的损失函数，根据损失函数的值决定是否重复步骤(2)、(3)、(4)，实现双相机的手眼标定。

9、在步骤(1)之前，可以先使用自标定技术，粗略给外置相机和末端相机的位姿设定一个初始值，包括下述步骤：

10、(0-1)使用两个相机从多个不同角度拍摄图像，并对拍摄到的图像进行处理，包括去噪、色彩校正、归一化等；

11、(0-2)进行立体标定计算，通过视觉算法，将两个相机的图像信息转为空间中的三维坐标，确定两个相机的外部参数，粗略得到外置相机与机械臂基座的位姿变换矩阵末端相机与机械臂末端的位姿变换矩阵作为两个相机位姿的初始值。

12、步骤(1)中，所述双相机处于互视的姿态，是指末端相机和外置相机能够相互观测到彼此，双相机的坐标系x轴和z轴平行且方向相反、y轴平行且方向相同，如图1所示。

13、步骤(2)中，所述变换的操作包括下述步骤：

14、(2-1)假设机械臂末端点在机械臂基座坐标系下的描述为e(x,y,z)，将其转为齐次坐标e(x,y,z,1)，使用外置相机与机械臂末端的变换矩阵将其转到外置相机坐标系下，得到点e′(x′,y′,z′,1)；(所述变换矩阵可以通过步骤(0-2)自标定获得的，也可以人为设置一个初值)

15、(2-2)将e′(x′,y′,z′,1)沿着外置相机的x轴(相机横向方向上)移动dx的距离，得到e″(x″,y″,z″,1)＝e(x′+dx,y′,z′,1)；

16、(2-3)使用外置相机与机械臂末端的变换矩阵，将e″(x″,y″,z″,1)转回到机械臂基座坐标系下，得

17、(2-4)使用矩阵ttranslate描述点e(x,y,z)到点e_new(x_new,y_new,z_new,1)的变换过程，e_new(x_new,y_new,z_new,1)＝ttranslate×e(x,y,z,1)；

18、(2-5)同理，使用矩阵trotate描述机械臂末端绕外置相机坐标轴进行旋转的过程，e_new(x_new,y_new,z_new,1)＝trotate×e(x,y,z,1)。

19、步骤(3)中，所述使用卡尔曼滤波修正末端相机位姿，如图3所示，包括下述步骤：

20、(3-1)以机械臂基座坐标系为参考系，建立末端相机位姿的状态转移方程，末端相机与机械臂末端刚性连接，末端相机与机械臂末端位姿同步变化；t时刻末端相机位姿可由t-1时刻的末端相机位姿framet-1和t到t-1时刻机械臂末端的位姿变换矩阵计算得到，位姿变换矩阵即步骤(2)所求的矩阵ttranslate或trotate；

21、

22、

23、wtranslatet()为机械臂末端在t时刻平移运动误差，wrotate(t)为机械臂末端在t时刻旋转运动误差；

24、(3-2)计算状态协方差矩阵：当机械臂进行仅平移运动时，协方差pt＝ttranslate×pt-1×ttranslate-1+qtranslate(t)；当机械臂进行仅旋转运动时，协方差pt＝trotate×pt-1×trotate-1+qrotate(t)；qtransltate(t)是机械臂进行位移重定位误差的协方差矩阵；qrotate(t)是机械臂进行旋转重定位误差的协方差矩阵；

25、(3-3)建立末端相机位姿的状态观测方程并计算卡尔曼增益；外置相机观测t时刻末端相机，获取末端相机的原始点云图像，并通过观测系统参数h转换到机械臂基座坐标系下，最后使用icp(iterative closest point)点云匹配算法，得到末端相机的位姿；

26、状态观测方程的定义表示为：pcdt＝h×pcd+vt，其中pcd为外置相机观察到的原始点云，vt为相机与icp算法的误差；

27、将末端相机的cad模型转为pcd点云数据格式后，通过icp算法对观测到的点云pcdt和标准模型点云pcdstandard model进行配准，获取末端相机位姿

28、计算卡尔曼增益kt＝pt×ht(h×pt×ht+rt)-1；rt为相机与icp匹配算法的误差方差；

29、(3-4)计算末端相机当前位姿的最优估计值：结合末端相机的预测值与观测值，计算t时刻末端相机位姿的最优估计值：

30、

31、(3-5)更新状态估计的协方差矩阵，pt＝(i-kth)pt。

32、步骤(4)中，所述构建外置相机位姿的状态转移方程和状态观测方程，包括下述步骤：

33、(4-1)以末端相机坐标系为参考系，建立外置相机位姿的状态转移方程，外置相机固定在机械臂外的位置，以世界坐标系为参考系时，机械臂运动过程中位姿没有发生变化；但以末端相机坐标系为参考系时，外置相机的坐标变换与末端相机的位姿变换相反；t时刻末端相机位姿framet可由t-1时刻位姿framet-1和t到t-1时刻机械臂末端的位姿变换矩阵的反矩阵计算得到，利用坐标变换的性质，对步骤(2)所求的矩阵ttranslate或trotate求反，可得位姿变换矩阵的反矩阵ttranslate-1或trotate-1；

34、或

35、(4-2)计算状态协方差矩阵：

36、当机械臂进行仅平移运动时，pt＝ttranslate-1×pt-1×ttranslate+qtranslate；

37、当机械臂进行仅旋转运动时，pt＝trotate-1×pt-1×trotate+qrotate(t)；

38、(4-3)建立外置相机位姿的状态观测方程并计算卡尔曼增益；末端相机观测t时刻外置相机，获取外置相机的点云图像，以末端相机所在坐标系为参考系，观测方程的定义表示：pcdt＝i×pcdt+vt，i为单位矩阵，vt为末端相机的误差；

39、将外置相机的cad模型转为pcd点云数据格式后，通过icp点云匹配算法，得到末端相机的位姿，

40、计算卡尔曼增益kt＝pt×ht(h×pt×ht+rt)-1，其中h为单位矩阵，简化后得kt＝pt(pt+rt)-1，rt为相机与icp匹配算法的误差方差。

41、(4-4)结合末端相机的预测值与观测值，计算外置相机当前位姿的最优估计值，

42、步骤(5)中，构建标定的损失函数包括下述步骤：

43、(5-1)机械臂使用修正后的手眼标定值重新运动到z轴相对、x轴平行且方向相反、y轴平行且方向相同的互视姿态；

44、(5-2)在两个相机能相互观测到彼此的范围内，控制机械臂进行多次移动，每次移动后相机之间相互观测，并使用icp算法将相机的观测点云pcdobservation与相机的标准模型点云pcdstandard model进行点云匹配，估计相互之间的位姿，得到对向被观测相机相对于观测相机自身坐标系下的位姿，计算相互之间的旋转矩阵ri和平移矩阵ti；

45、外置相机观测末端相机时：argeyeonhand(ri|ti)＝icp(pcdobservation，pcdstandardmodel)；

46、末端相机观测外置相机时：argeyetohand(r′i|t′i)＝icp(pcdobservation，pcdstandardmodel)

47、(5-2)计算位移损失函数：

48、对于x轴和z轴位移损失函数：

49、对于y轴位移损失函数：

50、定义旋转损失函数：

51、n为机械臂运动的总数，x为矩阵的元素；当相机标定位移分量准确时，位移损失函数趋向于0；当相机标定旋转分量准确时，旋转损失函数趋向于0。

52、本发明的原理是：

53、(1)本发明采用的自动标定技术原理是利用机器人系统相机以及运动规划进行标定，而不依赖于外部参考对象或设备，可以减少标定过程中的复杂度和成本，并且能够在线标定，适用于动态环境下的标定任务；

54、(2)本发明采用的卡尔曼滤波(kalman filtering)是一种高效的自回归滤波器，能在已知测量方差的情况下从一系列存在测量噪声的数据中估计动态系统的状态。本发明将手眼标定值计算得到的相机位姿和相机被观测到的真实位姿输入卡尔曼滤波，构建了相机位姿的状态方程和观测方程，在机械臂以特定的方式运动时利用滤波对相机位姿进行实时的递归计算，以获得可收敛的、精确的相机位姿值，修正标定误差。

55、(3)本发明采用的icp(iterative closest point)算法是一种经典的点云匹配算法，用于求解两个点云之间的位姿变换。在已知相机标定点云模型的情况下，通过点云采集和预处理、确定初始对应点对、去除不合理点对和坐标变换求解的过程，获取被观测相机相对于观测相机的位姿。

56、(4)本发明的标定损失函数围绕坐标参考系的性质进行构建，当相机标定准确并进行互视状态时，使末端相机沿外置相机的某一轴进行平移或旋转时，相机以自身为参考系观测到的对向相机的运动具有规律，且可以求解；反之当相机使用不准确的标定值进入互视状态时，使用沿某一轴的平移或旋转运动必定包含了其他轴的运动，由此可以通过相机互视，构建描述平移或旋转运动纯粹性的损失函数作为迭代的条件，用于评价修正是否有效，标定值是否准确。

57、本发明与现有技术相比具有如下优点和效果：

58、(1)本发明采用在线标定方式，与传统离线标定不同，本发明的标定过程无需人为参与分布进行。

59、(2)本发明使用相机本体作为标定件，无需额外标定物就可以完成标定。

60、(3)本发明同时对末端相机和外置相机进行标定，并直接修正标定结果，而不是当误差较大时进行重新标定，提高标定准确性。

61、(4)本发明采用适用于多相机系统的标定损失函数，与传统计算已知尺寸物体重投影间接测量误差的方式不同，损失函数直接衡量标定后物体的识别精度，相比于重投影误差测量难度更低。