手眼标定_全面细致的推导过程

本文解决的问题：  机械手搭载双目相机，手眼标定。 本文有细致的推导过程，非常全面。  什么是手眼标定  确定像素坐标系和空间机械手坐标系的坐标转化关系；  为什么会存在这个？  举一个生活中常见的例子——用手移动物体：   第一步：眼睛观察到三维世界，并将其转换到视网膜平面（三维空间转换到二维平面）传送信息给大脑；   第二步：大脑想要移动某个物体，假设想要将物体从A点移动B点（二维坐标），但是物体是三维空间中的物体，是三维坐标，需要将二维坐标换算成三维坐标；   第三步：大脑已经获得A点和B点的三维坐标，大脑给手（执行机构）发出指令去完成这个任务；   其中第二步就是手眼标定，得到二维坐标（像素坐标）到三维坐标的转换矩阵 在实际控制中，相机检测到目标在图像中的像素位置后，通过标定好的坐标转换矩阵将相机的像素坐标变换到机械手的空间坐标系中，然后根据机械手坐标系计算出各个电机该如何运动，从而控制机械手到达指定位置。这个过程中涉及到了图像标定，图像处理，运动学正逆解，手眼标定等。  相机的装载位置  不在手上(eye-to-hand)   相机固定在一个地方，机械手的运动不会带着相机一起移动。  在手上(eye-in-hand)  相机安装在机械手上，随着机械手一起移动。较为常用。这个实际上和eye-to-hand类似。   可以快速有效地标定被测物体的坐标。   这种情况的标定过程实际上和相机和机械手分离的标定方法是一样的，因为相机拍照时，机械手会运动到相机标定的时候的位置，然后相机拍照，得到目标的坐标，再控制机械手，所以简单的相机固定在末端的手眼系统很多都是采用这种方法，标定的过程和手眼分离系统的标定是可以相同对待的。   ●基于图像的视觉控制 ●基于位置的视觉控制 ●结合两者的混合视觉控制   在正式开始讲解之前，可以看一下：深入浅出地理解机器人手眼标定 对手眼标定有一个直观的认识。  正式开始  本文的相机搭载方案是，hand-in-eye。移动相机，标定求解过程 在推导过程中，我们会用到四个坐标系，分别是：   ●基础坐标系（用base表示） ●机械手坐标系（用tool表示） ●相机坐标系（用cam表示） ●标定物坐标系（用cal表示）   下面先给出示意图：

坐标系之间的转换关系说明：   ●baseHtool：表示机械手坐标系到基础坐标系的转换关系，可以由机器人系统中得出。（已知） ●toolHcam：表示相机坐标系到机械手坐标系的转换关系；这个转化关系在机械手移动过程中是不变的；（未知，待求） ●calHcam：表示相机坐标系到标定板坐标系的转换关系（相机外參），可以由相机标定求出；（相当于已知） ●baseHcal：表示标定板坐标系到基础坐标系的变换，这个是最终想要得到的结果；只要机械手和标定板的相对位置不变，这个变换矩阵不发生变化。  

  所以：其中的A已知，X待求，B需要通过相机标定得知（张正友标定法可以求得）。  验证结果 

 1. 基础坐标系(求解baseHtool)  符合右手定则的XYZ三个坐标轴   ●原点：机器人底座的中心点 ●X轴正向：指向机器人的正前方 ●Z轴正向：指向机器人的正上方 ●Y轴正向：由右手定则确定

六个自由度   ●三个位置：x、y、z（第六轴法兰盘圆心相对于原点的偏移量）   ●三个角：Rx、Ry、Rz（第六轴法兰盘的轴线角度，由初始姿态即竖直向上绕x轴旋转Rx度，再绕Y轴旋转Ry度，再绕Z轴旋转Rz度得到）   ●旋转方式（机器人RPY角和Euler角 – 基本公式）（机器人学-熊有伦36-40页）      ●绕定轴X-Y-Z旋转(判断机械臂输出四元数与代码得到的四元数是否相等得到)  

  一定要注意欧拉角和李代数不一样，非常容易搞混，因为他们都是3个量   欧拉角：分别绕x、y、z轴旋转的角度，不一样的旋转次序，得到的R不一样；   李代数：维度是3，是绕一个轴转动一定的角度。欧拉角可以理解成李代数在x、y、z轴上的分解旋转。（不一定正确，不过比较形象）   注：不同机械臂示教器显示的法兰盘的数据格式不一样，有的是用欧拉角显示的，有的是用角轴显示的。  2. camHcal相机到标定板  ●注意：标定板坐标系下的坐标转换到相机坐标系下

思路大致如下：   ●已知双目相机的内参、畸变系数、外参（Pr=R∗Pl+t P_r=R*P_l+tP  r=R∗P l +t）, ●对左右相机的两张图片调用OpenCV中的findChessboardCorners函数，找到内角点（如果结果不好，继续提取亚像素点）； ●将左右相机的像素点对应起来，得到匹配的2d点； ●使用空间异面直线的方法，用对应的2d点计算出以右相机为世界坐标系的3维坐标Pcam P_{cam}P cam ；(立体视觉匹配) ●计算出每个角点以棋盘格为世界坐标的3维坐标Pcal P_{cal}P cal； ●通过解方程Pcam=camHcal∗Pcal P_{cam}=camHcal*P_{cal}P cam=camHcal∗P cal 求解出外参(3d-3d：ICP，SVD奇异值分解（十四讲173页）)   张正友相机标定Opencv实现  

  参数解释：   ●第一个参数Image，传入拍摄的棋盘图Mat图像，必须是8位的灰度或者彩色图像； ●第二个参数patternSize，每个棋盘图上内角点的行列数，一般情况下，行列数不要相同，便于后续标定程序识别标定板的方向； ●第三个参数corners，用于存储检测到的内角点图像坐标位置，一般用元素是Point2f的向量来表示：vector image_points_buf; ●第四个参数flage：用于定义棋盘图上内角点查找的不同处理方式，有默认值。  3. 求解AX=XB  以下四篇论文对应着四种求解方法   Tsai, Roger Y., and Reimar K. Lenz. “A new technique for fully autonomous and efficient 3D robotics hand/eye calibration.” IEEE Transactions on robotics and automation 5.3 (1989): 345-358.(博客：Tsai-Lenz的OpenCV实现)   Horaud, Radu, and Fadi Dornaika. “Hand-eye calibration.” The international journal of robotics research 14.3 (1995): 195-210.   Park, Frank C., and Bryan J. Martin. “Robot sensor calibration: solving AX= XB on the Euclidean group.” IEEE Transactions on Robotics and Automation10.5 (1994): 717-721.(博客：Navy的OpenCV实现)   Daniilidis, Konstantinos. “Hand-eye calibration using dual quaternions.” The International Journal of Robotics Research 18.3 (1999): 286-298.   网上有源代码可以下载：经典手眼标定算法C++代码   文献3采用的是李群的理论，将AX=XB转化成最小二乘问题；   文献4采用的时对偶四元数的知识，用对偶四元数表达旋转和平移，从而进行统一计算；   着四种算法精度差不多，不过文献4的效果要更好点。   具体实现文献3的算法，下面具体介绍   对数：乘法变加法  李群李代数 

利用李群知识求解AX=XB

采用“两步法”求解上述方程，先解算旋转矩阵，再求得平移向量。  

求解旋转矩阵

代码：用两组数据求解方程AX=XB