转移中相机、标定板、机器人末端与机器人基座坐标系之间的转 换关系，建立W下状态转移方程：
[0050]灯。山=iv ?化.心 阳0川灯。四=1V ?化r)四.心
[0052] .
[0053] .
[0054] . 阳〇对灯。(…)=1V.灯/)(仙.心
[0056] 运动转移状态示意图如图3所示，图3为本发明技术方案标定过程中运动状态转 移示意图，其中T/是机器人基座坐标系到相机坐标系的齐次转换矩阵，IV是标定板坐标系 到机器人末端坐标系的齐次转换矩阵，T/与IV都是固定不变的；
[0057] 做将妨中的方程两两顺序组合，消去Tb%令巧)(,>=货)(,+,,?促诘， (不、,=巧'％+,>?(了/记，Tb表示标定板的状态转移矩阵，T。表示机器人末端的转移矩阵，可 得n个齐次变换方程：(Tb)W?T/=T/ .灯。)W，其中i表示状态索引；
[0058] (7)将状态转移矩阵分块表示，齐次变换方程可表示为：
[0059]
[0060] 其中R表示旋转矩阵，t表示平移向量。 阳〇6U做旋转矩阵1?等价的旋转向量表示为：巧=451。0(巧，》，_，";)'，其中0为旋转角， (ni，n2，n3)t为转轴方向的单位向量，
表示Pf的反对称矩阵，由齐次 变换方程可得（Rb)wR/=RfE(R。)W，将其中的旋转矩阵都变换为旋转向量可得n个线性方 程Skew( (Pb) (1)+(PJW)Pr" = (Pe) (1)- (Pb) (1);
[0062] (9)由线性最小二乘算法求解得到Pr"，根据公式
求解出P/， 将P/转换为欧拉角表示的旋转矩阵R/，再将R/带回齐次变换方程，求得t/，将此结果
作为迭代优化的初始值；
[006引 （10)在像素空间迭代优化，得到精确的T巧日V，根据相机的成像模型W及方程 灯。(。=1\。.灯/)(。.心，可得：
[0064]
[0065] 此方程为优化求解的目标方程，其中相机的内参数矩构
是已知 参数，迭代优化求解的步骤如下：
[0066] (10-1)迭代过程包括两个阶段：标定板到机器人末端位姿关系求解阶段和机器 人基座到相机位姿关系求解阶段；
[0067] (10-2)将巧）中得到的结果作为初始值灯刀;。尸Tr"，将代入方程 W側灯。。)=T/ ?灯/)?v得到T,6的初始值灯,6)扣)；
[0069] (10-3)标定板到机器人末端位姿关系求解阶段：n+1组状态数据
[0070] {T/，心，{(叫，Vi),(咕V2), . . .，（Um,Vm)}} (1,2,...,W1)作为输入数据，将灯 /)(k1)作 为已知量带入优化目标方程，灯。AU作为优化的初始值，优化的目标为最小化重投影误差
其中（U，，V,)为观测角点像坐标，（U,'，V,'）为重投影后 的角点像坐标，采用Levenberg-Marquar化算法求解（IVO
[0071] (10-4)机器人基座到相机位姿关系求解阶段：n+1组状态数据 阳07引 |1/，心，{(111，乂1)，（112，乂2)，...，（山，乂。)}}(1,2,...,。4)作为输入数据，将0\6)化)作为 已知量带入优化目标方程，灯/)AU作为优化的初始值，优化的目标为最小化重投影误差
巧中（U，，V，）为观测角点像坐标，（U,'，V,'）为重投影后 的角点像坐标，采用Levenberg-Marquar化算法求解（Tf。) 阳〇7：3] (10-f5)根据灯。W，（巧冲灯b%U，化％U判断标定结果是否收敛：计算
[0074]err=II(巧的-(巧(k1)IIf+II化6)似-化6)(k1)IIf 阳〇7引II?II康示矩阵的化obenius范数，如果err小于设定的阔值，则停止迭化得到 最终的标定结果T/= 0V)W，否则置k=k+1。
[0076] W上例举的仅是本发明的优选实施方式，本发明并不限于W上实施例，本领域技 术人员在不脱离本发明的精神和构思的前提下直接导出或联想到的其他改进和变化，均应 认为包含在本发明的保护范围内。
【主权项】
1. 一种基于像素空间优化的相机与机器人相对位姿标定方法，其特征在于，机器人在 末端固定标定板，通过运动控制使标定板在固定安装的相机视野下摆放不同位姿，结合各 个姿态下已知的机器人末端运动信息和标定板在图像中的识别结果，迭代优化相机与机器 人基座和标定板与机器人末端两个参数，最终获得精确的相机与机器人基座相对位姿关 系。2. 根据权利要求1所述的基于像素空间优化的相机与机器人相对位姿标定方法，其特 征在于，具体标定步骤如下： 1) 、固定标定板在机器人末端，控制机器人运动，使得运动停止时，标定板在相机视野 内； 2) 、当运动停止后，记录机器人末端坐标系到基座坐标系的转换关系T/，采集图像得到 标定板上棋盘格角点像坐标{ (Ui,Vi)，（u2,v2)，. . .，（um,vm)}，计算标定板坐标系到相机坐 标系的转换关系TJ; 3) 、再控制机器人运动n次，得到n+1运动停止时的状态数据{T/，Tb' {(Ul，Vl)，（u2,v2 )，???，（Um，vm)}} (1,2, ...,n+l)； 4) 、根据运动状态转移中相机、标定板、机器人与机器人基座坐标系之间的转换关系， 建立以下状态转移方程： (V)⑴=Trc. (Ter)⑴.Tbe (Tbc)(2)=Trc. (Ter)⑵.Tbe /rr\C\ _rpC/rpr\ rpe 、丄b / (n+1) 1 r、丄e / (n+1) lb T/是机器人基座坐标系到相机坐标系的齐次转换矩阵，Tb6是标定板坐标系到机器人 末端坐标系的齐次转换矩阵； 5) 、消去V，令W)(n = (r6%+1>= ?(;〇&， Tb表示标定板的状态 转移矩阵，表示机器人末端的转移矩阵，得到n个齐次转换方程：（Tb)w ?T/=T/ ?(TJ⑴，即AX=XB; 6) 、利用旋转矩阵的线性不变性求解齐次转换方程AX=XB，将求得的T/'作为优化求 解的初始值； 7) 、在像素空间迭代优化，得到精确的T/和Tb%根据相机的成像模型以及方程CC)W =Tr、（Ter)⑴.Tbe，可得：此方程为优化求解的目标方程，其中相机的内参数矩卩I已知参数。3. 根据权利要求2所述的基于像素空间优化的相机与机器人相对位姿标定方法，其特 征在于，所述的步骤7)中的迭代优化过程包括两个阶段：标定板到机器人末端位姿关系求 解阶段和机器人基座到相机位姿关系求解阶段。4. 根据权利要求2或3所述的基于像素空间优化的相机与机器人相对位姿标定方法， 其特征在于，所述的迭代优化求解的具体步骤如下： 1) 、将权利要求2中步骤6)中得到的结果作为初始值（T/)w =T/'，将代入方程CC)⑴=T/ ? (T/)⑴?V得到V的初始值（V) w; 2) 、标定板到机器人末端位姿关系求解阶段：n+1组状态数据作为输入数据，将（T/) &u作为已知量带入优化目标方程，（Tb6) & 作为优化的初始值，优化的目标为最小化重投 影误其中（u,，V])为观测角点像坐标，（u/，v/ )为重 J.投影后的角点像坐标，采用Levenberg-Marquardt算法求解（Tbe) 3) 、机器人基座到相机位姿关系求解阶段：n+1组状态数据{T/，Tbe，{(Ul，Vl)，（u2,v2)， ...，作为输入数据，将〇V)w作为已知量带入优化目标方程，0V)(kl) 作为优化的初始值，优化的目标为最小化重投影误I其中（u,，V])为观测角点像坐标，（u/，v/ )为重投影后的角点像坐标，采用Levenberg-Marquardt算法求解（Tr。）(k); 4) 、根据CC)w，（T/)w和（Tb*〇 1}，（T/) & 1}判断标定结果是否收敛：计算 err= | | (Trc) (k)- (Trc)(k1} | | f+1 | (Tbe) (k)- (Tbe)(k1} | |f II?IIf表示矩阵的Frobenius范数，如果err小于设定的阈值，则停止迭代，得到最终 的标定结果T/=(T/) ，否则置k=k+1。
【专利摘要】本发明公开了一种基于像素空间优化的相机与机器人相对位姿标定方法。本发明采用机器人末端携带标定板在固定相机视野内运动，利用标定板的空间运动约束信息，标定得到最优的相机与机器人基座的相对位姿关系。首先通过利用旋转矩阵的线性不变性求解齐次变换矩阵，得到的初步的标定结果；然后将初步标定结果作为优化初值，在像素空间进行优化，使得重投影误差最小。本发明采用了迭代优化算法，无需精密的外部测量设备，利用图像像素空间的模型约束，结合有效的优化初值获取方法，得到了相对高精度的标定结果，可满足工业应用中视觉伺服机器人完成定位和抓取工作的需求。
【IPC分类】B25J9/16
【公开号】CN105014667
【申请号】CN201510478644
【发明人】章逸丰, 程玉立, 熊蓉
【申请人】浙江大学
【公开日】2015年11月4日
【申请日】2015年8月6日