机械臂 手眼标定 手眼矩阵 eye-in-hand 原理、实践及代码

1.手眼标定

所谓手眼系统，就是人眼睛看到一个东西的时候要让手去抓取，就需要大脑知道眼睛和手的坐标关系。而相机知道的是像素坐标，机械手是空间坐标系，所以手眼标定就是得到像素坐标系和空间机械手坐标系的坐标转化关系。

目前工业上通常使用两种方法进行机械臂的手眼协作。第一种是：手在眼外(eye-to-hand)，即摄像机与机械臂分离，应用场景为：机械臂的工作位置固定，工作平面固定，摄像机置于合理的位置以识别机械臂要工作的平面，例如：货物码垛、货物搬运等。第二种是：手在眼上(eye-in-hand)，即摄像机布置在机械臂末端，应用场景为：机械臂移动式的工作，例如：果园采摘、货物运转等。总的来说，采用手在眼上可以适应更多的应用场景，所以本项目的摄像机搭载方案为眼在手上。

(1)原理

在推导过程中，我们会用到四个坐标系，分别是：

• 基础坐标系(用base表示)
• 机械臂末端坐标系(用gripper表示)
• 相机坐标系(用cam表示)
• 标定物坐标系(用target表示)

下图为坐标系分布的示意图：

坐标系之间的转换关系说明：

• t a r g e t b a s e T _{target}^{base}T targetbase​T ：目标坐标系到基础坐标系的变换矩阵
• t a r g e t c a m T _{target}^{cam}T targetcam​T ：目标坐标系到相机坐标系的变换矩阵
• c a m g r i p p e r T _{cam}^{gripper}T camgripper​T ：相机坐标系到机械臂末端坐标系的变换矩阵
• g r i p p e r b a s e T _{gripper}^{base}T gripperbase​T ：机械臂末端坐标系到基础坐标系的变换矩阵

根据线性代数中矩阵连乘的性质可知：

t a r g e t b a s e T _{target}^{base}T targetbase​T = t a r g e t c a m T _{target}^{cam}T targetcam​T * c a m g r i p p e r T _{cam}^{gripper}T camgripper​T * g r i p p e r b a s e T _{gripper}^{base}T gripperbase​T ①

在此项目中我们采用张正友博士的张氏标定法，第一步：我们将标定板置于固定位置不动。第二步：移动机械臂使相机可以清晰拍到整个标定板，记录此时机械臂位姿，并拍摄照片，第三步：重复第二步十次以上，保存数据备用。

类似这样的多张图片

根据上述标定步骤可知，在标定过程中，标定板与基础坐标系的位姿关系不变，即 t a r g e t b a s e T _{target}^{base}T targetbase​T 保持不变。同理，在标定过程中相机与机械臂末端的位姿关系保持不变，即 c a m g r i p p e r T _{cam}^{gripper}T camgripper​T 保持不变。

故可对 ① 进行变换：

t a r g e t b a s e T 1 _{target}^{base}T_1 targetbase​T1​ = t a r g e t c a m T 1 _{target}^{cam}T_1 targetcam​T1​ * c a m g r i p p e r T _{cam}^{gripper}T camgripper​T * g r i p p e r b a s e T 1 _{gripper}^{base}T_1 gripperbase​T1​

t a r g e t b a s e T 2 _{target}^{base}T_2 targetbase​T2​ = t a r g e t c a m T 2 _{target}^{cam}T_2 targetcam​T2​ * c a m g r i p p e r T _{cam}^{gripper}T camgripper​T * g r i p p e r b a s e T 2 _{gripper}^{base}T_2 gripperbase​T2​

由于 t a r g e t b a s e T 1 = t a r g e t b a s e T 2 _{target}^{base}T_1 = _{target}^{base}T_2 targetbase​T1​=targetbase​T2​ 故可得到下式：

t a r g e t c a m T 1 _{target}^{cam}T_1 targetcam​T1​ * c a m g r i p p e r T _{cam}^{gripper}T camgripper​T * g r i p p e r b a s e T 1 _{gripper}^{base}T_1 gripperbase​T1​ = t a r g e t c a m T 2 _{target}^{cam}T_2 targetcam​T2​ * c a m g r i p p e r T _{cam}^{gripper}T camgripper​T * g r i p p e r b a s e T 2 _{gripper}^{base}T_2 gripperbase​T2​

可得： t a r g e t c a m T 2 − 1 _{target}^{cam}T_2^{-1} targetcam​T2−1​ * t a r g e t c a m T 1 _{target}^{cam}T_1 targetcam​T1​ * c a m g r i p p e r T _{cam}^{gripper}T camgripper​T = c a m g r i p p e r T _{cam}^{gripper}T camgripper​T * g r i p p e r b a s e T 2 _{gripper}^{base}T_2 gripperbase​T2​ * g r i p p e r b a s e T 1 − 1 _{gripper}^{base}T_1^{-1} gripperbase​T1−1​

即：AX = XB

A：不同位姿下目标物坐标系到相机坐标系的转换

B：不同位姿下机械臂末端坐标系到基础坐标系的转换

接下来利用先前拍照获得的多组数据即可求出 c a m g r i p p e r T _{cam}^{gripper}T camgripper​T，再将 c a m g r i p p e r T _{cam}^{gripper}T camgripper​T 带回 ① 式即可获得每张照片对应的 t a r g e t b a s e T _{target}^{base}T targetbase​T 。至此，手眼标定的工作完成。

(2)机械臂末端坐标系到基础坐标系的变换矩阵的获取

使用D-H Model对机械臂进行建模，对各个旋转轴建立坐标系：

Z轴：穿过旋转轴中心

X轴：前一个关节的Z轴和本关节Z轴的公垂线

Y轴：根据右手定则确定Y轴

根据D-H model可得机械臂的每个连杆都可以用四个运动学参数来描述，其中两个参数用于描述连杆本身，另外两个参数用于描述连杆之间的连接关系。这四个参数分别为：

a i − 1 a_{i-1} ai−1​：连杆长度，用来描述关节轴 i-1 和关节轴 i 之间公垂线的长度。

α i − 1 \alpha_{i-1} αi−1​：连杆转角，用来描述两关节轴相对位置的第二个参数。

d i d_i di​：连杆偏距，用来描述沿两个相邻连杆公共轴线方向的距离。

θ i \theta_i θi​：关节角，用来描述两相邻连杆绕公共轴线旋转的夹角。

连杆参数的获取：

a i = a_i= ai​= 沿 X i X_i Xi​轴，从 Z i Z_i Zi​移动到 Z i + 1 Z_{i+1} Zi+1​的距离；

α i = \alpha_i= αi​= 绕 X i X_i Xi​轴，从 Z i Z_i Zi​旋转到 Z i + 1 Z_{i+1} Zi+1​的角度；

d i = d_i= di​= 沿 Z i Z_i Zi​轴，从 X i − 1 X_{i-1} Xi−1​移动到 X i X_i Xi​的距离；

θ i = \theta_i= θi​= 绕 Z i Z_i Zi​轴，从 X i − 1 X_{i-1} Xi−1​旋转到 X i X_i Xi​的角度；

由以上条件结合实际机械臂情况可得该机械臂的D-H model表：

	ai-1	αi-1	di	θi
1	0	0	0	θ \theta θ1-160°
2	0	90°	37 mm	180°- θ 2 \theta_2 θ2​
3	96 mm	0	0	θ 3 \theta_3 θ3​-180°
4	96 mm	0	0	θ 4 \theta_4 θ4​-180°
5	0	90°	55 mm	0

已知D-H model参数表，再根据连杆转换矩阵的一般表达式：

i i − 1 T = [ c o s θ i − s i n θ i 0 a i − 1 s i n θ i ∗ c o s α i − 1 c o s θ i ∗ c o s α i − 1 − s i n α i − 1 − s i n α i − 1 ∗ d i s i n θ i ∗ s i n α i − 1 c o s θ i ∗ s i n α i − 1 c o s α i − 1 c o s α i − 1 ∗ d i 0 0 0 1 ] _{i}^{i-1}T = \begin{bmatrix} cos\theta_i & -sin\theta_i &0 &a_{i-1} \\ sin\theta_i*cos\alpha_{i-1} & cos\theta_i*cos\alpha_{i-1} & -sin\alpha_{i-1} & -sin\alpha_{i-1}*d_i \\ sin\theta_i*sin\alpha_{i-1} & cos\theta_i*sin\alpha_{i-1} & cos\alpha_{i-1} & cos\alpha_{i-1}*d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} ii−1​T=⎣⎢⎢⎡​cosθi​sinθi​∗cosαi−1​sinθi​∗sinαi−1​0​−sinθi​cosθi​∗cosαi−1​cosθi​∗sinαi−1​0​0−sinαi−1​cosαi−1​0​ai−1​−sinαi−1​∗di​cosαi−1​∗di​1​⎦⎥⎥⎤​

已知DH参数表，利用DH参数表，以及每次拍摄时舵机旋转的角度得到每次拍摄时的机械臂末端坐标系到基础坐标系的变换矩阵：

1 0 T = [ c o s ( θ 1 − 160 ° ) − s i n ( θ 1 − 160 ° ) 0 0 s i n ( θ 1 − 160 ° ) c o s ( θ 1 − 160 ° ) 0 0 0 0 1 0 0 0 0 1 ] _1^0T = \begin{bmatrix} cos(\theta_1-160°) & -sin(\theta_1-160°) &0 &0 \\ sin(\theta_1-160°) & cos(\theta_1-160°) & 0 & 0 \\ 0 & 0& 1& 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} 10​T=⎣⎢⎢⎡​cos(θ1​−160°)sin(θ1​−160°)00​−sin(θ1​−160°)cos(θ1​−160°)00​0010​0001​⎦⎥⎥⎤​

2 1 T = [ c o s ( 180 ° − θ 2 ) − s i n ( 180 ° − θ 2 ) 0 0 0 0 − 1 − 37 s i n ( 180 ° − θ 2 ) c o s ( 180 ° − θ 2 ) 0 0 0 0 0 1 ] _2^1T = \begin{bmatrix} cos(180°-\theta_2) & -sin(180°-\theta_2) &0 &0 \\ 0 & 0& -1& -37 \\ sin(180°-\theta_2) & cos(180°-\theta_2) & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} 21​T=⎣⎢⎢⎡​cos(180°−θ2​)0sin(180°−θ2​)0​−sin(180°−θ2​)0cos(180°−θ2​)0​0−100​0−3701​⎦⎥⎥⎤​

3 2 T = [ c o s ( θ 3 − 180 ° ) − s i n ( θ 3 − 180 ° ) 0 96 s i n ( θ 3 − 180 ° ) c o s ( θ 3 − 180 ° ) 0 0 0 0 1 0 0 0 0 1 ] _3^2T = \begin{bmatrix} cos(\theta_3-180°) & -sin(\theta_3-180°) &0 &96 \\ sin(\theta_3-180°) & cos(\theta_3-180°) & 0 & 0 \\ 0 & 0& 1& 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} 32​T=⎣⎢⎢⎡​cos(θ3​−180°)sin(θ3​−180°)00​−sin(θ3​−180°)cos(θ3​−180°)00​0010​96001​⎦⎥⎥⎤​

4 3 T = [ c o s ( θ 4 − 180 ° ) − s i n ( θ 4 − 180 ° ) 0 96 s i n ( θ 4 − 180 ° ) c o s ( θ 4 − 180 ° ) 0 0 0 0 1 0 0 0 0 1 ] _4^3T = \begin{bmatrix} cos(\theta_4-180°) & -sin(\theta_4-180°) &0 &96 \\ sin(\theta_4-180°) & cos(\theta_4-180°) & 0 & 0 \\ 0 & 0& 1& 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} 43​T=⎣⎢⎢⎡​cos(θ4​−180°)sin(θ4​−180°)00​−sin(θ4​−180°)cos(θ4​−180°)00​0010​96001​⎦⎥⎥⎤​

5 4 T = [ 1 0 0 0 0 0 − 1 − 55 0 1 0 0 0 0 0 1 ] _5^4T = \begin{bmatrix} 1 & 0 &0 &0 \\ 0 & 0& -1& -55 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} 54​T=⎣⎢⎢⎡​1000​0010​0−100​0−5501​⎦⎥⎥⎤​

(3)目标坐标系到相机坐标系的变换矩阵的获取

我们将使用opencv-python的方法获得目标坐标系到相机坐标系得变换矩阵，但同时我们需要解决一些相机带来的问题。

在实际应用上，镜头并非理想的透视成像，带有不同程度的畸变。理论上镜头的畸变包括径向畸变和切向畸变，切向畸变影响较小，通常只考虑径向畸变。

径向畸变：径向畸变主要由镜头径向曲率产生（光线在远离透镜中心的地方比靠近中心的地方更加弯曲）。导致真实成像点向内或向外偏离理想成像点。其中畸变像点相对于理想像点沿径向向外偏移，远离中心的，称为枕形畸变；径向畸点相对于理想点沿径向向中心靠拢，称为桶状畸变。

用数学公式表示：

x ~ K[R|t]X

其中，x为相机中的像素坐标；X为相机坐标系下的三维坐标；K为内参矩阵，包含了焦距和光心坐标；[R|t]为外参矩阵，R和t分别为世界坐标系到相机坐标系的旋和平移。然后我们利用opencv-python中的cameraCalibration方法，将棋盘格上的真实世界坐标与我们假设出来的坐标对应，就可以得到该相机在该焦距下的内参矩阵、外参矩阵，以及相机的畸变系数。

(4)实现

在熟悉原理后，实现上面两个步骤即可得到多组机械臂末端到基础坐标系的变换矩阵以及多组目标坐标系到相机坐标系得变换矩阵。接下来使用python-opencv中的calibrateHandeye()
参数描述如下：

R_gripper2base，t_gripper2base是机械臂抓手相对于机器人基坐标系的旋转矩阵与平移向量，需要通过机器人运动控制器获得相关参数转换计算得到；

R_target2cam, t_target2cam 是标定板相对于双目相机的齐次矩阵，在进行相机标定时可以求取得到；

R_cam2gripper ，t_cam2gripper是求解的手眼矩阵分解得到的旋转矩阵与平移矩阵；

将上述得到的矩阵带入该方法即可得到该机械臂的手眼矩阵。

2.抓取

相机参数和手眼参数标定完成后，就可以进行基于视觉的抓取了。其步骤为：

• 首先将机械臂末端移动到拍照点（拍照高度需要已知，也就是物体到相机光心的距离），并保证相机垂直向下。
• 相机拍摄桌面上的目标，识别其中彩色方块的位置。
• 根据相机内参及拍照高度，将彩色方块中心点的像素坐标转换为相机坐标系下的坐标。
• 根据手眼矩阵，将相机坐标系下的坐标转换为机械臂坐标系下的坐标。
• 控制机械臂末端移动到机械臂坐标系下的坐标，抓取方块。

根据上述手眼标定原理中的矩阵关系我们可以得到：

[ X c a m e r a Y c a m e r a Z c a m e r a ] = \begin{bmatrix}X_{camera} & Y_{camera} & Z_{camera} \end{bmatrix}= [Xcamera​​Ycamera​​Zcamera​​]= t a r g e t c a m e r a T ∗ [ X t a r g e t Y t a r g e t Z t a r g e t ] _{target}^{camera}T * \begin{bmatrix}X_{target} & Y_{target} & Z_{target}\end{bmatrix} targetcamera​T∗[Xtarget​​Ytarget​​Ztarget​​]

[ X g r i p p e r Y g r i p p e r Z g r i p p e r ] = \begin{bmatrix}X_{gripper} & Y_{gripper} & Z_{gripper} \end{bmatrix}= [Xgripper​​Ygripper​​Zgripper​​]= c a m e r a g r i p p e r T ∗ [ X c a m e r a Y c a m e r a Z c a m e r a ] _{camera}^{gripper}T * \begin{bmatrix}X_{camera} & Y_{camera} & Z_{camera}\end{bmatrix} cameragripper​T∗[Xcamera​​Ycamera​​Zcamera​​]

[ u v 1 ] = C a m M a t r i x ∗ t a r g e t c a m e r a T ∗ [ X t a r g e t Y t a r g e t Z t a r g e t ] \begin{bmatrix} u&v&1 \end{bmatrix}=CamMatrix*_{target}^{camera}T*\begin{bmatrix}X_{target} & Y_{target} & Z_{target}\end{bmatrix} [u​v​1​]=CamMatrix∗targetcamera​T∗[Xtarget​​Ytarget​​Ztarget​​]

用以上的转换关系，可以计算出相机坐标系中的点、物体坐标系中的点、图像像素坐标系中的点在基础坐标系下的点坐标：

[ X b a s e Y b a s e Z b a s e ] = g r i p p e r b a s e T ∗ c a m e r a g r i p p e r T ∗ [ X c a m e r a Y c a m e r a Z c a m e r a ] \begin{bmatrix}X_{base} & Y_{base} & Z_{base} \end{bmatrix}=_{gripper}^{base}T*_{camera}^{gripper}T * \begin{bmatrix}X_{camera} & Y_{camera} & Z_{camera}\end{bmatrix} [Xbase​​Ybase​​Zbase​​]=gripperbase​T∗cameragripper​T∗[Xcamera​​Ycamera​​Zcamera​​]

[ X b a s e Y b a s e Z b a s e ] = g r i p p e r b a s e T ∗ c a m e r a g r i p p e r T ∗ t a r g e t c a m e r a T ∗ [ X t a r g e t Y t a r g e t Z t a r g e t ] \begin{bmatrix}X_{base} & Y_{base} & Z_{base} \end{bmatrix}=_{gripper}^{base}T*_{camera}^{gripper}T*_{target}^{camera}T * \begin{bmatrix}X_{target} & Y_{target} & Z_{target}\end{bmatrix} [Xbase​​Ybase​​Zbase​​]=gripperbase​T∗cameragripper​T∗targetcamera​T∗[Xtarget​​Ytarget​​Ztarget​​]

[ X b a s e Y b a s e Z b a s e ] = g r i p p e r b a s e T ∗ c a m e r a g r i p p e r T ∗ C a m M a t r i x − 1 ∗ [ u v 1 ] \begin{bmatrix}X_{base} & Y_{base} & Z_{base} \end{bmatrix}=_{gripper}^{base}T*_{camera}^{gripper}T*CamMatrix^{-1}*\begin{bmatrix} u&v&1 \end{bmatrix} [Xbase​​Ybase​​Zbase​​]=gripperbase​T∗cameragripper​T∗CamMatrix−1∗[u​v​1​]

经上述变换即可得到物体在基础坐标系下的坐标。接下来凭借该坐标对机械臂进行运动学逆解，若能达到该点则判断多组解中，哪个解最省时省力，即采用该解，最后再将该物块放置至预定位置即可。

import cv2
import numpy as np
import glob
import math
import serial
import pigpio
import time

serialHandle = serial.Serial("/dev/ttyAMA0", 115200)  #初始化串口， 波特率为115200

command = {"MOVE_WRITE":1, "POS_READ":28, "LOAD_UNLOAD_WRITE": 31}

gripperPose = np.array([[ -47.46614798,-30.93409332,-106.33248484,-2.66670287,0.17899222,1.52106046],
                        [ -50.64555014,-25.69099839,-106.03448307,-2.60620767,0.31446276,1.49742558],
                        [-4.11761269e+01,-3.96028022e+01,-1.05198809e+02,2.69146962e+00,8.27020253e-02,-1.55768099e+00],
                        [ -35.24396468,-44.69683285,-105.73538777,2.62497237,0.25738079,-1.51919588],
                        [-1.35740504e+02,-5.24577701e+01,-1.42120520e+02,2.49769933e+00,1.39643072e-01,-1.79081589e+00],
                        [-1.40319905e+02,-3.85697620e+01,-1.42120520e+02,2.54783819e+00,1.42299609e-02,-1.82676476e+00],
                        [-1.40607129e+02,-3.85729704e+01,-1.41216360e+02,2.54206370e+00,1.41977097e-02,-1.83474341e+00],
                        [-144.89888667,-16.20343465,-141.21636039,-2.4742213,0.1799589,1.78577792],
                        [-2.87355186e+01,-3.54512169e+01,-1.17795839e+02,-2.94734829e+00,8.23268923e-02,.02983728e+00],
                        [ -23.97006612,-40.2412104,-119.7291674,2.85844706,0.17583165,-1.17350036],
                        [ -34.66609758,-31.49890098,-119.7291674,-2.83632764,0.24615135,1.16441951],
                        [ -33.0391174,-33.69010423,-118.9732689,-2.86150817,0.15196856,1.18527549],
                        [-5.87034928e+01,-3.25139467e+01,-1.52581512e+02,-2.84878652e+00,1.11431156e-01,1.24342019e+00],
                        [-1.20653741e+01,-3.61680661e+01,-1.65803499e+02,-3.04942726e+00,1.19279280e-01,6.71576060e-01],
                        [-7.21552695e+01,-4.10918877e+01,-2.20040184e+02,3.12903017e+00,8.74017266e-02,-2.16608054e-01],
                        [-2.71427089e+01,-3.14464549e+01,-2.10189887e+02,-3.10436556e+00,3.74570060e-01,1.46398163e-01],
                        [-1.81869149e+01,-3.72054666e+01,-2.10918478e+02,3.13801503e+00,1.75261644e-02,-1.38106174e-01],
                        [-1.17375606e+02,-2.47069077e+01,-1.87936493e+02,-2.92199001e+00,1.55180620e-01,1.04162328e+00]])

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

objp = np.zeros((4 * 6, 3), np.float32)
objp[:, :2] = np.mgrid[0:6, 0:4].T.reshape(-1, 2)

objpoints = []  # 3d point in real world space
imgpoints = []  # 2d points in image plane

images = glob.glob('image/*.jpg')

# 命令发送
def servoWriteCmd(id, cmd, par1=None, par2=None):
    buf = bytearray(b'\x55\x55')
    try:
        len = 3  # 若命令是没有参数的话数据长度就是3
        buf1 = bytearray(b'')

        ## 对参数进行处理
        if par1 is not None:
            len += 2  # 数据长度加2
            par1 = 0xffff & par1
            buf1.extend([(0xff & par1), (0xff & (par1 >> 8))])  # 分低8位 高8位 放入缓存
        if par2 is not None:
            len += 2
            par2 = 0xffff & par2
            buf1.extend([(0xff & par2), (0xff & (par2 >> 8))])  # 分低8位 高8位 放入缓存

        buf.extend([(0xff & id), (0xff & len), (0xff & cmd)])  # 追加 id， 数据长度， 命令
        buf.extend(buf1)  # 追加参数

        ##计算校验和
        sum = 0x00
        for b in buf:  # 求和
            sum += b
        sum = sum - 0x55 - 0x55  # 去掉命令开头的两个 0x55
        sum = ~sum  # 取反
        buf.append(0xff & sum)  # 取低8位追加进缓存

        serialHandle.write(buf)  # 发送

    except Exception as e:
        print(e)


# 读取位置
def readPosition(id):
    serialHandle.flushInput()  # 清空接收缓存
    servoWriteCmd(id, command["POS_READ"])  # 发送读取位置命令
    time.sleep(0.00034)  # 小延时，等命令发送完毕。不知道是否能进行这么精确的延时的，但是修改这个值的确实会产生影响。
    # 实验测试调到这个值的时候效果最好
    time.sleep(0.005)  # 稍作延时，等待接收完毕
    count = serialHandle.inWaiting()  # 获取接收缓存中的字节数
    pos = None
    if count != 0:  # 如果接收到的数据不空
        recv_data = serialHandle.read(count)  # 读取接收到的数据
        if count == 8:  # 如果接收到的数据是8个字节（符合位置读取命令的返回数据的长度）
            if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[4] == 0x1C:
                # 第一第二个字节等于0x55, 第5个字节是0x1C 就是 28 就是 位置读取命令的命令号
                pos = 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8)))  # 将接收到的字节数据拼接成完整的位置数据
                # 上面这小段代码我们简化了操作没有进行和校验，只要帧头和命令对了就认为数据无误

    return pos  # 返回读取到的位置

def init():
    servoWriteCmd(1, 1, 500, 1000)
    servoWriteCmd(2, 1, 750, 1000)
    servoWriteCmd(3, 1, 700, 1000)
    servoWriteCmd(4, 1, 700, 1000)
    servoWriteCmd(5, 1, 500, 1000)
    servoWriteCmd(6, 1, 500, 1000)

def getPosition(frame):
    img = cv2.resize(frame, (720, 480))
    copy = img.copy()

    lower = np.array([0, 96, 92])
    upper = np.array([24, 255, 255])
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv, lower, upper)
    result = cv2.bitwise_and(img, img, mask=mask)
    canny = cv2.Canny(result, 80, 180)
    contours, hierarchy = cv2.findContours(canny, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    size = copy.shape

    maxPixels = 0
    maxContour = 0
    print(size)
    for i in range(len(contours)):
        single_rect = np.zeros((size[0], size[1]))
        fill_image = cv2.fillConvexPoly(single_rect, contours[i], 255)
        pixels = cv2.countNonZero(fill_image)
        peri = cv2.arcLength(contours[i], True)
        vertices = cv2.approxPolyDP(contours[i], peri*0.02, True)
        corners = len(vertices)
        if pixels > maxPixels and pixels > 500:
            maxPixels = pixels
            maxContour = i

    peri = cv2.arcLength(contours[maxContour], True)
    vertices = cv2.approxPolyDP(contours[maxContour], peri*0.02, True)
    corners = len(vertices)
    x, y, w, h = cv2.boundingRect(vertices)
    cv2.rectangle(copy, (x, y), (x + w, y + h), (0, 255, 0), 4)
    cv2.putText(copy, 'rectangle', (x, y-5), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1)
    cv2.imshow('result', result)
    cv2.imshow('canny', canny)
    cv2.imshow('img', copy)
    # cv2.waitKey(0)
    position = [x+w/2, y+h/2]
    if maxPixels == 0:
        return None
    return position

def gripper2base(a1, a2, a3, a4):
    t1 = np.array([[math.cos(math.radians(a1 * 0.32 - 160)), -math.sin(math.radians(a1 * 0.32 - 160)), 0, 0],
                  [math.sin(math.radians(a1 * 0.32 - 160)), math.cos(math.radians(a1 * 0.32 - 160)), 0, 0],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])

    t2 = np.array([[math.cos(math.radians(180 - a2 * 0.36)), -math.sin(math.radians(180 - a2 * 0.36)), 0, 0],
                  [0, 0, -1, -37],
                  [math.sin(math.radians(180 - a2 * 0.36)), math.cos(math.radians(180 - a2 * 0.36)), 0, 0],
                  [0, 0, 0, 1]])

    t3 = np.array([[math.cos(math.radians(a3 * 0.36 - 180)), -math.sin(math.radians(a3 * 0.36 - 180)), 0, 96],
                  [math.sin(math.radians(a3 * 0.36 - 180)), math.cos(math.radians(a3 * 0.36 - 180)), 0, 0],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])
    t4 = np.array([[math.cos(math.radians(a4 * 0.36 - 180)), -math.sin(math.radians(a4 * 0.36 - 180)), 0, 96],
                  [math.sin(math.radians(a4 * 0.36 - 180)), math.cos(math.radians(a4 * 0.36 - 180)), 0, 0],
                  [0, 0, 1, 0],
                  [0, 0, 0, 1]])

    t5 = np.array([[1, 0, 0, 0],
                  [0, 0, -1, -55],
                  [0, 1, 0, 0],
                  [0, 0, 0, 1]])

    t = t1 @ t2 @ t3 @ t4 @ t5
    return t

def getBasePosition(position):
    pos3 = readPosition(3)  # 获取3号舵机的位置
    pos4 = readPosition(4)  # 获取4号舵机的位置
    pos5 = readPosition(5)  # 获取5号舵机的位置
    pos6 = readPosition(6)  # 获取6号舵机的位置
    H = np.zeros(shape=(4, 4), dtype=float)
    H = gripper2base(pos6, pos5, pos4, pos3)
    mtx, R = getEyeHand()
    mtx = mtx.T
    base = np.array([0, 0, 0])
    base = H * R * mtx * [position[0], position[1], 1]
    return base

def grasp(basePosition):
    # 进行运动学逆解，得到多个解，从多个解中找到最适合的解
    # 机械臂各个舵机旋转到指定角度
    # 机械臂末端到达指定位置，并略微调整位置
    # 夹取，抬升物体
    # 移动到稳定点

def place():
    # 第一步：云台转到朝向目标方向
    # 第二步：移到接近点
    # 第三步：移到目标点
    # 第四步：抬升
    # 第五步：放置
    # 第六步：移到稳定点

def getEyeHand():
    R_gripper2base = []
    t_gripper2base = []
    R_target2camera = []
    t_target2camera = []

    for frame in images:
        img = cv2.imread(frame)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        ret, corners = cv2.findChessboardCorners(gray, (6, 4), None)
        if ret == True:
            objpoints.append(objp)

            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)  # 在初步提取的角点信息上进一步提取亚像素信息，降低相机标定偏差
            imgpoints.append(corners)

            img = cv2.drawChessboardCorners(img, (6, 4), corners2, ret)

            cv2.imshow("img", img)
            cv2.waitKey(0)

    # mtx:相机内参矩阵
    # dist:畸变系数
    # rvecs:旋转向量
    # tvecs:平移向量
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
    # 求外参，对于每张图片外参矩阵都是不一样的，外参矩阵随着刚体位置的变换而变换

    # retval, rvec, tvec  = cv2.solvePnP(np.float32(objpoints), np.float32(imgpoints), mtx, dist)
    a = np.array([0, 0, 0, 1])
    for i in range(len(rvecs)):
        # 先利用solvePnP得到各个R_target2cam和T_target2cam
        # 利用gripper2base得到RT_gripper2base，然后将其分为R_gripper2base和T_gripper2base
        # 最后得到R,T = cv2.calibrateHandEye(R_gripper2base,T_gripper2base,R_target2cam,T_target2cam)
        # RT = np.column_stack((R,T))
        # RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
        # 即可得到手眼标定最后的结果
        R_matrix, _ = cv2.Rodrigues(rvecs[i])
        R_target2camera.append(R_matrix)
        t_target2camera.append(tvecs[i])
        # Rt_matrix = np.concatenate((R_matrix, tvecs[i]), axis=1)
        # Rt_matrix = np.row_stack((Rt_matrix, a))
        # print(Rt_matrix)


    for i in range(len(gripperPose)):
        R_vector = gripperPose[i, 3:6]
        R_matrix, _ = cv2.Rodrigues(R_vector)
        R_gripper2base.append(R_matrix)
        t_gripper2base.append(gripperPose[i, 0:3].T)


    print("内参=", mtx)
    print("畸变系数=", dist)
    print("旋转向量=", rvecs)
    print("平移向量=", tvecs)

    newImage = cv2.imread('image/0.jpg')
    h, w = newImage.shape[:2]
    newCameraMtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
    dst = cv2.undistort(newImage, mtx, dist, None, newCameraMtx)
    x, y, w, h = roi
    dst = dst[y:y + h, x:x + w]

    # 求解重投影误差，越趋近于0越好
    mean_error = 0
    for i in range(len(objpoints)):
        imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
        error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
        mean_error += error
    print("total error: ", mean_error / len(objpoints))

    cv2.imshow('dst', dst)
    cv2.waitKey(0)

    # calibrateHandEye的结果是gripper2camera
    print("基坐标系的旋转矩阵与平移向量求解完毕————————————————")
    print("进入手眼标定函数————————————————————————————————")

    R, T = cv2.calibrateHandEye(R_gripper2base, t_gripper2base, R_target2camera, t_target2camera)#手眼标定

    print("手眼矩阵分解得到的旋转矩阵")
    print(R)
    print("\n")


    print("手眼矩阵分解得到的平移矩阵")
    print(T)

    RT=np.column_stack((R,T))
    RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
    print("\n")
    print('相机相对于末端的变换矩阵为：')
    print(RT)
    return mtx, RT

if __name__=='__main__':
    init()
    cap = cv2.VideoCapture(0)
    while True:
        ret, frame = cap.read()
        if ret:
            position = getPosition(frame)
            if position is not None:
                basePosition = getBasePosition(position)
                print("basePosition", basePosition)
                grasp(basePosition)
                time.sleep(2)
                place()
                time.sleep(2)
            else:
                continue