CN112419428A - 手术机器人红外相机标定方法 - Google Patents

Abstract

本发明公开了手术机器人红外相机标定方法，涉及手术机器人技术领域，能够克服自遮挡问题，使得每个红外相机都能观察到整个标定物品，提高标定精度本发明包括：利用十字定位法对标定物进行标定，将标定物上的标志点和其在红外相机平面上投影的图像点对应，将标定物多次旋转，利用旋转后标志点和图像点的关系，求解红外相机的参数混合矩阵；利用参数混合矩阵和正交矩阵的乘积性质，求得红外相机***矩阵；利用红外相机***矩阵解算红外相机的内参数矩阵；利用参数混合矩阵和红外相机内参矩阵求解红外相机的外参数矩阵；利用极大似然估计原理，以参数混合矩阵、***矩阵、内参数矩阵、外参数矩阵和标志点为优化变量，以最小化重投影误差为优化目标进行非线性优化，得到红外相机投影矩阵的优化解。

Description

手术机器人红外相机标定方法

技术领域

本发明涉及手术机器人技术领域，尤其涉及了手术机器人红外相机标定方法。

背景技术

红外相机标定是光学定位***中至关重要的一个步骤，其精度在一定意义上决定着光学定位***的精度。在基于多目红外相机的光学***中，红外相机高精度标定的必要条件是***中的每一个红外相机都能同时观察到整个标定物。然而，传统的基于三维或二维标定物的标定方法因自遮挡而无法满足这一条件。

发明内容

本发明提供了手术机器人红外相机标定方法，能够克服自遮挡问题，使得每个红外相机都能观察到整个标定物品，提高标定精度。

为达到上述目的，本发明采用如下技术方案：

手术机器人红外相机标定方法，包括：

利用十字定位法对标定物进行标定，将标定物上的标志点和其在红外相机平面上投影的图像点对应，将标定物多次旋转，利用旋转后标志点和图像点的关系，求解红外相机的参数混合矩阵；

利用参数混合矩阵和正交矩阵的乘积性质，求得红外相机的***矩阵；

利用红外相机***矩阵解算红外相机的内参数矩阵；

利用参数混合矩阵和红外相机内参矩阵求解红外相机的外参数矩阵；

利用极大似然估计原理，以参数混合矩阵、***矩阵、内参数矩阵、外参数矩阵和标志点为优化变量，以最小化重投影误差为优化目标进行非线性优化，得到红外相机投影矩阵的优化解。

进一步的，计算外参数矩阵时，利用旋转矩阵最优估计的方式对外参数矩阵进行解算。

进一步的，旋转矩阵最优估计具体包括：

构造初始矩阵，对初始矩阵的奇异值进行分解；

利用奇异值计算初始矩阵的最佳估计值，并将最佳估计值中的元素相乘，相乘得到的值和初始矩阵的最佳估计值共同组成旋转矩阵的最佳估计值。

本发明的有益效果是：

本发明分阶段确定了参数混合矩阵、***矩阵、内参数矩阵、外参数矩阵，上述均为线性解，再利用极大似然估计原理对上述线性解进行优化，再利用极大似然估计原理以红外相机矩阵和空间点为优化变量，以最小化重投影误差为优化目标进行非线性优化，最终求解得到极大似然估计意义下的红外相机参数，从而提高相机标定精度。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是十字定位法所用标定物模型；

图2是标定参考物的投影过程；

图3是十字定位法求得的红外相机参数线性解；

图4是十字定位法求得的红外相机参数非线性优化解(α、β)；

图5是十字定位法求得的红外相机参数非线性优化解(u₀、v₀)。

具体实施方式

为使本领域技术人员更好地理解本发明的技术方案，下面结合具体实施方式对本发明作进一步详细描述。

手术机器人红外相机标定方法，具体步骤如下：

1、首先利用十字定位法对标定物进行标定。十字定位法如图1所示，标定物由两个相互垂直的一维线状物组成，标定物上固定有五个标志点M₁，M₂，M₃，M₄，M₅，其中M₁点位于两个一维线状物的中点处，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轴分别为x_u轴与y_u轴，x_u轴与y_u轴分别与u、v轴平行,建立二维直角坐标系。若原点O₁在u、v坐标系中的坐标为(u_o、v_o)，则每一个像素在x_u轴与y_u轴方向的物理距离为dx、dy。

令以O点为坐标原点建立直角坐标系Xc、Yc、Zc，其中Xc轴和Yc轴与图像坐标系的x_u轴与y_u轴平行,Zc轴为红外相机的光轴,Zc轴与图像平面垂直。光轴与图像平面的交点即为图像坐标系的原点

红外相机坐标系与实际坐标系均为三维欧氏坐标系,由空间几何的知识可得,两者的关系可以用旋转矩阵R与平移向量t来描述，(Xw、Yw、Zw)为实际坐标系，(tx、ty、tz)表示红外相机相对于实际坐标系的平移向量，(θ、ψ、

)表示红外相机相对于实际坐标系围绕x、y、z轴的旋转角度。R为正交单位矩阵(旋转矩阵),t为三维平移向量。

因所有标志点的Z坐标均为0，故得到下式：

设t＝[t₁ t₂ t₃]^T，R＝[r₁ r₂ r₃]，r₁＝[r₁₁ r₂₁ r₃₁]^T，r₂＝[r₁₂ r₂₂ r₃₂]^T，r₃＝[r₁₃r₂₃ r₃₃]^T，经推导式(3)可转化为：

其中α、β分别为x、y的导数。

令式(4)中的

令B＝A^-TA^-1。

将C标记为红外相机的参数混合矩阵、B为***矩阵、A为红外相机内参矩阵。

分阶段确定红外相机参数的方法包括五个阶段：

1、利用空间点与图像点的对应求得参数混合矩阵C。

2、利用参数混合矩阵C及正交矩阵的性质求得B矩阵。

3、利用B矩阵分解得到红外相机内参矩阵A。

4、由参数混合矩阵C及红外相机内参矩阵A进一步求得红外相机外部参数。

5、融合调整。

(1)参数混合矩阵C的求解

图2描述了标定参考物上的5个标志点在红外相机下的投影过程。记第i个标志点在第j次旋转时在红外相机平面上的图像点为{m_ij|i＝1，2，3，4，5，j＝1，2，……n}。

将C用下式表达：

引入一个新的用于计算的向量：

则式(4)可用下式表示：

其中：

M_ix代表标志点Mi的X坐标，Miy代表Mi的Y坐标，i＝1,2,3,4,5。N10*9是为了求解方便的计算变量矩阵。

将图1所示标定参考物旋转N次，可得N个式(6)所示的方程，将所得的N个方程罗列起来，其最小二乘解即为参数混合矩阵C的线性解。

(2)B矩阵的求解

由参数混合矩阵C的定义C＝A·[r₁ r₂ t]可得：

[C₁ C₂ C₃]＝A·[r₁ r₂ t] (8)

因r₁，r₂分别为正交旋转矩阵R的第一列和第二列，所以有以下关系：

由B矩阵的定义B＝A^-TA^-1可知，B为实对称矩阵，故可以引入一个6维计算向量b表示B矩阵：

b＝[B₁₁,B₁₂,B₂₂,B₁₃,B₂₃,B₃₃]^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、计算V^TV

2、计算V^TV的特征值和对应的特征向量

3、V^TV最小特征值对应的特征向量即为b的解

在解得向量b之后可以构造出矩阵B。

(3)红外相机内参数矩阵A的求解

由B矩阵的定义B＝A^-TA^-1可得：

由式(14)可以得到红外相机内参数与B矩阵元素之间的对应，其中s为Sin函数缩写：

(4)红外相机外参矩阵的求解

红外相机外参矩阵计算

只要不改变相机的结构，而只是变动相机与实际坐标系的相对位置，那么相机的内参是不改变的。也就是说矩阵A是不变的，改变的只是旋转矩阵R和平移向量t。在求得红外相机内参矩阵以后，可以利用式(8)得到如下等式：

r₁＝λA^-1C₁ (21)

r₂＝λA^-1C₂ (22)

r₃＝r₁×r₂ (23)

t＝λA^-1C₃ (24)

因为r₁和r₂都是单位向量，所以

λ是为了计算方便的系数。

而实际中由于噪声及误差的影响，实际求出的[r₁ r₂ r₃]并不能构成旋转矩阵所要求的正交矩阵。即需要以下的旋转矩阵寻优过程。

旋转矩阵寻优

利用r₁，r₂构造一个旋转矩阵。理论上r₁，r₂是相互正交的单位向量，由于噪声的影响，它们可能不完全满足单位正交的关系。旋转矩阵寻优的目标是寻找两个和r₁，r₂最接近的单位正交向量。设g₁，g₂是两个相互正交的单位向量。旋转矩阵寻优的基本思路就是使得r₁，r₂和g₁，g₂的差异最小。

设

根据最小二乘法的思想，相应的判断函数为：

其中，k为计数起始值，F为范数。上式实际上就是求矩阵g，使得r-g的F范数最小，因为

所以寻优函数为式(26)所示。

根据F范数性质有：

其中tr为范数函数，因为矩阵r已知，所以tr(r^Tr)是一个确定值，即

由此寻优函数转变为：

旋转矩阵最优估计推导

r是一个3×2的矩阵，有奇异值分解为

其中U是3×3正交矩阵，V是2×2正交矩阵，σ₁，σ₂是矩阵r的奇异值。设

因为

所以向量[z₁₁ z₁₂ z₁₃]^T，[z₂₁ z₂₂ z₃₃]^T均是单位向量；因此有：

-1≤z₁₁，z₂₂≤1 (30)

根据矩阵迹的性质有：

tr(r^Tg)＝tr(V^TΣ^TU^Tg)＝tr(Σ^TU^TgV^T)＝tr(Σ^TZ) (31)

Σ为求和函数，根据式(30)所示的关系，并且任何矩阵的奇异值均大于0，所以σ₁·z₁₁+σ₂·z₂₂≤σ₁+σ₂。当z₁₁＝z₂₂＝1时候，tr(r^Tg)取得最大值。

因为向量[z₁₁ z₁₂ z₁₃]^T，[z₂₁ z₂₂ z₃₃]^T均是单位向量，且z₁₁＝z₂₂＝1；所以

因为

这样即可得出对r的最优估计g。

综上,旋转矩阵最优估计步骤可总结为

1、构造初始数据

2、计算矩阵r的奇异值分解

3、计算矩阵r的最佳估计

4、计算另一个单位向量g₃＝g₁×g₂

5、矩阵[g₁ g₂ g₃]就是对旋转矩阵的最佳估计

(5)融合调整

分阶段确定参数的以上4个阶段是通过最小化代数距离得出的线性解，而利用极大似然估计原理可以对线性算法的结果进行优化。融合调整是利用极大似然估计原理以红外相机矩阵和空间点为优化变量以最小化重投影误差为优化目标的非线性优化过程。当图像点噪声服从各向同性零均值的高斯分布且独立同分布时，融合调整通过求解如下非线性最小化问题得到极大似然估计意义下的红外相机参数：

其中R_j为正交单位矩阵(旋转矩阵)，t_j为三维平移向量，

为空间点M_i在第j次旋转下红外相机平面上的投影点。以前4个阶段求得的线性解为初值，利用LM(Levenberg-Marquardt)算法求解式(33)所示的非线性最小化问题，最终得到红外相机投影矩阵的优化解。另外，此非线性优化方法也可以应用于分阶段确定参数的第一阶段求出参数混合矩阵C后优化参数混合矩阵C。

效果验证

为验证上述分阶段确定红外相机参数的十字定位法，进行了以下实验过程。红外相机内参数分别为：α＝980，β＝890，γ＝0.1，u₀＝338，and v₀＝288。选择如图2所示的标定参考物的尺寸为：d＝100。在实验中，旋转标定参考物6次，标定参考物所在平面的方向用[0,90]之间的三个随机数表示，而标定参考物的位置用数组[500，500，1000]各乘以一个[0,1]之间的随机数表示。将标定参考物上的5个空间标志点按上述红外相机内外参数投影，得到图像点。考虑到实际情况中的误差，将均值为0标准差为σ高斯噪声加于图像点上，调整标准差的值从0.1像素至1.0像素，每一误差水平进行100次独立实验。将得到的图像点与标定参考物上的空间标志点对应，使用本文提出的分阶段确定参数的五点法计算红外相机内外参数，进而与实际值进行比较。红外相机各个内参数相对于α的相对误差如图3及图4、图5所示。以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (3)

1.手术机器人红外相机标定方法，其特征在于，包括：

利用十字定位法对标定物进行标定，将标定物上的标志点和其在红外相机平面上投影的图像点对应，将标定物多次旋转，利用旋转后标志点和图像点的关系，求解红外相机的参数混合矩阵；

利用参数混合矩阵和正交矩阵的乘积性质，求得红外相机的***矩阵；

利用红外相机***矩阵解算红外相机的内参数矩阵；

利用参数混合矩阵和红外相机内参矩阵求解红外相机的外参数矩阵；

利用极大似然估计原理，以参数混合矩阵、***矩阵、内参数矩阵、外参数矩阵和标志点为优化变量，以最小化重投影误差为优化目标进行非线性优化，得到红外相机投影矩阵的优化解。

2.根据权利要求1所述的手术机器人红外相机标定方法，其特征在于，计算所述外参数矩阵时，利用旋转矩阵最优估计的方式对所述外参数矩阵进行解算。

3.根据权利要求1所述的手术机器人红外相机标定方法，其特征在于，所述旋转矩阵最优估计具体包括：

构造初始矩阵，对初始矩阵的奇异值进行分解；

利用奇异值计算初始矩阵的最佳估计值，并将最佳估计值中的元素相乘，相乘得到的值和初始矩阵的最佳估计值共同组成所述旋转矩阵的最佳估计值。

Images