手术机器人红外相机标定方法
技术领域
本发明涉及手术机器人技术领域,尤其涉及了手术机器人红外相机标定方法。
背景技术
红外相机标定是光学定位***中至关重要的一个步骤,其精度在一定意义上决定着光学定位***的精度。在基于多目红外相机的光学***中,红外相机高精度标定的必要条件是***中的每一个红外相机都能同时观察到整个标定物。然而,传统的基于三维或二维标定物的标定方法因自遮挡而无法满足这一条件。
发明内容
本发明提供了手术机器人红外相机标定方法,能够克服自遮挡问题,使得每个红外相机都能观察到整个标定物品,提高标定精度。
为达到上述目的,本发明采用如下技术方案:
手术机器人红外相机标定方法,包括:
利用十字定位法对标定物进行标定,将标定物上的标志点和其在红外相机平面上投影的图像点对应,将标定物多次旋转,利用旋转后标志点和图像点的关系,求解红外相机的参数混合矩阵;
利用参数混合矩阵和正交矩阵的乘积性质,求得红外相机的***矩阵;
利用红外相机***矩阵解算红外相机的内参数矩阵;
利用参数混合矩阵和红外相机内参矩阵求解红外相机的外参数矩阵;
利用极大似然估计原理,以参数混合矩阵、***矩阵、内参数矩阵、外参数矩阵和标志点为优化变量,以最小化重投影误差为优化目标进行非线性优化,得到红外相机投影矩阵的优化解。
进一步的,计算外参数矩阵时,利用旋转矩阵最优估计的方式对外参数矩阵进行解算。
进一步的,旋转矩阵最优估计具体包括:
构造初始矩阵,对初始矩阵的奇异值进行分解;
利用奇异值计算初始矩阵的最佳估计值,并将最佳估计值中的元素相乘,相乘得到的值和初始矩阵的最佳估计值共同组成旋转矩阵的最佳估计值。
本发明的有益效果是:
本发明分阶段确定了参数混合矩阵、***矩阵、内参数矩阵、外参数矩阵,上述均为线性解,再利用极大似然估计原理对上述线性解进行优化,再利用极大似然估计原理以红外相机矩阵和空间点为优化变量,以最小化重投影误差为优化目标进行非线性优化,最终求解得到极大似然估计意义下的红外相机参数,从而提高相机标定精度。
附图说明
为了更清楚地说明本发明实施例中的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其它的附图。
图1是十字定位法所用标定物模型;
图2是标定参考物的投影过程;
图3是十字定位法求得的红外相机参数线性解;
图4是十字定位法求得的红外相机参数非线性优化解(α、β);
图5是十字定位法求得的红外相机参数非线性优化解(u0、v0)。
具体实施方式
为使本领域技术人员更好地理解本发明的技术方案,下面结合具体实施方式对本发明作进一步详细描述。
手术机器人红外相机标定方法,具体步骤如下:
1、首先利用十字定位法对标定物进行标定。十字定位法如图1所示,标定物由两个相互垂直的一维线状物组成,标定物上固定有五个标志点M1,M2,M3,M4,M5,其中M1点位于两个一维线状物的中点处,d为预先设定的距离值,即有以下等式成立:
||M2-M1||=||M3-M1||=||M4-M1||=||M5-M1||=d (1)
标定物上的五个标志点共面,所以图1所示标定物是介于一维标定物与二维标定物之间的标定参考物。
以两个一维线状物的交点为实际坐标系的原点,标定参考物所在的平面为XY平面,依照笛卡尔坐标系的规则即可得到实际坐标系Z轴的方向。这样可以得到十字定位法下的五个标志点在实际坐标系下的坐标,即:
M1=[0,0,0],M2=[-d,0,0],M3=[d,0,0],M4=[0,d,0],M5=[0,-d,0] (2)
令采集图像的左上顶点为原点在图像上建立直角坐标系u、v,这样每一像素的坐标(u、v)分别是该像素在数组中的列数和行数,所以(u、v)是以像素为单位的位置坐标。因图像像素坐标系无法用物理单位表示出该像素在图像中的位置,无法与实际坐标系及红外相机坐标系联系起来,因而需要另外建立一个物理单位表示的图像坐标系,以红外相机光轴与图像平面的交点即主点为原点,x,y轴分别为xu轴与yu轴,xu轴与yu轴分别与u、v轴平行,建立二维直角坐标系。若原点O1在u、v坐标系中的坐标为(uo、vo),则每一个像素在xu轴与yu轴方向的物理距离为dx、dy。
令以O点为坐标原点建立直角坐标系Xc、Yc、Zc,其中Xc轴和Yc轴与图像坐标系的xu轴与yu轴平行,Zc轴为红外相机的光轴,Zc轴与图像平面垂直。光轴与图像平面的交点即为图像坐标系的原点
红外相机坐标系与实际坐标系均为三维欧氏坐标系,由空间几何的知识可得,两者的关系可以用旋转矩阵R与平移向量t来描述,(Xw、Yw、Zw)为实际坐标系,(tx、ty、tz)表示红外相机相对于实际坐标系的平移向量,(θ、ψ、
)表示红外相机相对于实际坐标系围绕x、y、z轴的旋转角度。R为正交单位矩阵(旋转矩阵),t为三维平移向量。
因所有标志点的Z坐标均为0,故得到下式:
设t=[t1 t2 t3]T,R=[r1 r2 r3],r1=[r11 r21 r31]T,r2=[r12 r22 r32]T,r3=[r13r23 r33]T,经推导式(3)可转化为:
其中α、β分别为x、y的导数。
将C标记为红外相机的参数混合矩阵、B为***矩阵、A为红外相机内参矩阵。
分阶段确定红外相机参数的方法包括五个阶段:
1、利用空间点与图像点的对应求得参数混合矩阵C。
2、利用参数混合矩阵C及正交矩阵的性质求得B矩阵。
3、利用B矩阵分解得到红外相机内参矩阵A。
4、由参数混合矩阵C及红外相机内参矩阵A进一步求得红外相机外部参数。
5、融合调整。
(1)参数混合矩阵C的求解
图2描述了标定参考物上的5个标志点在红外相机下的投影过程。记第i个标志点在第j次旋转时在红外相机平面上的图像点为{mij|i=1,2,3,4,5,j=1,2,……n}。
将C用下式表达:
引入一个新的用于计算的向量:
其中:
Mix代表标志点Mi的X坐标,Miy代表Mi的Y坐标,i=1,2,3,4,5。N10*9是为了求解方便的计算变量矩阵。
将图1所示标定参考物旋转N次,可得N个式(6)所示的方程,将所得的N个方程罗列起来,其最小二乘解即为参数混合矩阵C的线性解。
(2)B矩阵的求解
由参数混合矩阵C的定义C=A·[r1 r2 t]可得:
[C1 C2 C3]=A·[r1 r2 t] (8)
因r1,r2分别为正交旋转矩阵R的第一列和第二列,所以有以下关系:
由B矩阵的定义B=A-TA-1可知,B为实对称矩阵,故可以引入一个6维计算向量b表示B矩阵:
b=[B11,B12,B22,B13,B23,B33]T (11)
则式(9)和式(10)可转化为:
Vij为了计算方便所引入的中间量,i,j的取值范围均为1,2,3…10。
若将图1所示的标定参考物旋转N次,则可以得到N个类似式(12)所示的方程,将此N个方程罗列起来,可以得到:
Vb=0 (13)
其中V是为了计算所引入的中间量,是一个2N×6的矩阵。当N>3时,可以通过以下步骤解得唯一的b向量:
1、计算VTV
2、计算VTV的特征值和对应的特征向量
3、VTV最小特征值对应的特征向量即为b的解
在解得向量b之后可以构造出矩阵B。
(3)红外相机内参数矩阵A的求解
由B矩阵的定义B=A-TA-1可得:
由式(14)可以得到红外相机内参数与B矩阵元素之间的对应,其中s为Sin函数缩写:
(4)红外相机外参矩阵的求解
红外相机外参矩阵计算
只要不改变相机的结构,而只是变动相机与实际坐标系的相对位置,那么相机的内参是不改变的。也就是说矩阵A是不变的,改变的只是旋转矩阵R和平移向量t。在求得红外相机内参矩阵以后,可以利用式(8)得到如下等式:
r1=λA-1C1 (21)
r2=λA-1C2 (22)
r3=r1×r2 (23)
t=λA-1C3 (24)
因为r
1和r
2都是单位向量,所以
λ是为了计算方便的系数。
而实际中由于噪声及误差的影响,实际求出的[r1 r2 r3]并不能构成旋转矩阵所要求的正交矩阵。即需要以下的旋转矩阵寻优过程。
旋转矩阵寻优
利用r1,r2构造一个旋转矩阵。理论上r1,r2是相互正交的单位向量,由于噪声的影响,它们可能不完全满足单位正交的关系。旋转矩阵寻优的目标是寻找两个和r1,r2最接近的单位正交向量。设g1,g2是两个相互正交的单位向量。旋转矩阵寻优的基本思路就是使得r1,r2和g1,g2的差异最小。
其中,k为计数起始值,F为范数。上式实际上就是求矩阵g,使得r-g的F范数最小,因为
所以寻优函数为式(26)所示。
根据F范数性质有:
其中tr为范数函数,因为矩阵r已知,所以tr(rTr)是一个确定值,即
由此寻优函数转变为:
旋转矩阵最优估计推导
r是一个3×2的矩阵,有奇异值分解为
其中U是3×3正交矩阵,V是2×2正交矩阵,σ
1,σ
2是矩阵r的奇异值。设
因为
所以向量[z
11 z
12 z
13]
T,[z
21 z
22 z
33]
T均是单位向量;因此有:
-1≤z11,z22≤1 (30)
根据矩阵迹的性质有:
tr(rTg)=tr(VTΣTUTg)=tr(ΣTUTgVT)=tr(ΣTZ) (31)
Σ为求和函数,根据式(30)所示的关系,并且任何矩阵的奇异值均大于0,所以σ1·z11+σ2·z22≤σ1+σ2。当z11=z22=1时候,tr(rTg)取得最大值。
因为向量[z
11 z
12 z
13]
T,[z
21 z
22 z
33]
T均是单位向量,且z
11=z
22=1;所以
因为
这样即可得出对r的最优估计g。
综上,旋转矩阵最优估计步骤可总结为
4、计算另一个单位向量g3=g1×g2
5、矩阵[g1 g2 g3]就是对旋转矩阵的最佳估计
(5)融合调整
分阶段确定参数的以上4个阶段是通过最小化代数距离得出的线性解,而利用极大似然估计原理可以对线性算法的结果进行优化。融合调整是利用极大似然估计原理以红外相机矩阵和空间点为优化变量以最小化重投影误差为优化目标的非线性优化过程。当图像点噪声服从各向同性零均值的高斯分布且独立同分布时,融合调整通过求解如下非线性最小化问题得到极大似然估计意义下的红外相机参数:
其中R
j为正交单位矩阵(旋转矩阵),t
j为三维平移向量,
为空间点M
i在第j次旋转下红外相机平面上的投影点。以前4个阶段求得的线性解为初值,利用LM(Levenberg-Marquardt)算法求解式(33)所示的非线性最小化问题,最终得到红外相机投影矩阵的优化解。另外,此非线性优化方法也可以应用于分阶段确定参数的第一阶段求出参数混合矩阵C后优化参数混合矩阵C。
效果验证
为验证上述分阶段确定红外相机参数的十字定位法,进行了以下实验过程。红外相机内参数分别为:α=980,β=890,γ=0.1,u0=338,and v0=288。选择如图2所示的标定参考物的尺寸为:d=100。在实验中,旋转标定参考物6次,标定参考物所在平面的方向用[0,90]之间的三个随机数表示,而标定参考物的位置用数组[500,500,1000]各乘以一个[0,1]之间的随机数表示。将标定参考物上的5个空间标志点按上述红外相机内外参数投影,得到图像点。考虑到实际情况中的误差,将均值为0标准差为σ高斯噪声加于图像点上,调整标准差的值从0.1像素至1.0像素,每一误差水平进行100次独立实验。将得到的图像点与标定参考物上的空间标志点对应,使用本文提出的分阶段确定参数的五点法计算红外相机内外参数,进而与实际值进行比较。红外相机各个内参数相对于α的相对误差如图3及图4、图5所示。以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应该以权利要求的保护范围为准。