【技术实现步骤摘要】
本专利技术涉及视觉定位，尤其涉及一种基于epnp算法的单目视觉姿态估计方法及系统。
技术介绍
1、数字医学技术的发展推进了骨科手术机器人导航技术的进步，现有的骨科手术机器人导航系统对于术中病人的移动体位监测大多以ndi光学导航定位追踪技术实现，其先通过多个marker标记物表示手术器械、病人痛点及定位标尺的空间位姿信息，接着ndi将上述的marker标记物统一到同一坐标系下，并借助坐标系提供的空间数据确定手术路径规划的起止点、机器人定位位置及实时跟踪术中病人的移动情况。因此，各marker标记物的空间位姿信息精度直接影响到手术路径规划以及机器人的精准度，marker标记物数量越多，其累计的误差越大，且计算量也就越大。为此，本专利技术提出了一种基于epnp算法的单目视觉姿态估计方法，将epnp非迭代算法与单目相机结合，通过在只有一个marker标记物下，通过单目相机实现机械臂对病人移动过程的跟踪；
2、现有的光学导航定位追踪技术借助ndi实时追踪显示多个marker标记物位的位置，存在marker标记物数量多，累计误差大，计算量大，且成本高等问题，本专利技术提出了一种基于epnp算法的单目视觉姿态估计方法，其将epnp非迭代算法与单目相机结合，且在只有一个marker标记物情况下，解决了ndi相机自身的需要匹配多个marker来进行术中定位所带来的误差累计及计算量大等缺点，该方法降低了配准误差，提高了配准精度，减小了计算量且降低了成本，本专利技术的应用至少能够解决现有技术的部分问题。
技术实
1、本专利技术实施例提供一种基于epnp算法的单目视觉姿态估计方法及系统，至少能够解决现有技术中部分问题。
2、本专利技术实施例的第一方面，
3、提供基于epnp算法的单目视觉姿态估计方法，包括：
4、通过相机标定获得单目相机内参；确定所述单目相机的视野内布置的空间参考标志物上特征点的世界坐标,以及所述特征点在相机图像上的像素坐标；将所述特征点的世界坐标表示为4个非共面虚拟控制点的线性组合,建立相机透视投影模型,形成关于所述虚拟控制点相机坐标的线性方程组,确定所述虚拟控制点在相机坐标系对应的相机坐标；其中，所述单目相机固定于机械臂末端，所述单目相机的光轴与所述机械臂的末端中心点重合；
5、引入平行透视投影模型，通过迭代的平行透视求解过程，获得相机位姿初始估计，基于所述相机位姿初始估计，确定相机坐标系下控制点坐标，计算控制点齐次重心坐标系数的初始值；以缩小控制点在相机坐标系和世界坐标系下的距离差为优化目标,利用所述控制点齐次重心坐标系数的初始值,通过迭代优化算法求解，获得空间参考标志物相对相机的精确位姿；
6、将空间参考标志物相对于所述机械臂末端的初始位姿作为基准位姿；当所述空间参考标志物发生移动时,相机实时捕获所述空间参考标志物的新位姿，计算所述机械臂末端当前位姿,结合所述基准位姿和所述机械臂末端当前位姿,实时计算机械臂末端的目标位姿，根据所述目标位置移动机械臂。
7、在一种可选的实施例中，
8、通过相机标定获得单目相机内参；确定所述单目相机的视野内布置的空间参考标志物上特征点的世界坐标,以及所述特征点在相机图像上的像素坐标；将所述特征点的世界坐标表示为4个非共面虚拟控制点的线性组合,建立相机透视投影模型,形成关于所述虚拟控制点相机坐标的线性方程组,确定所述虚拟控制点在相机坐标系对应的相机坐标包括：
9、采用张正友标定法对所述单目相机进行标定,获得所述单目相机的内参数矩阵，包括相机焦距、光心坐标；
10、将相机视野内的棋盘格作为平面参考标志物,通过特征提取算法检测所述平面参考标志物上角点的像素坐标，选取n个所述角点作为特征点，基于所述棋盘格的尺寸和所述角点的排列，计算所述特征点在所述棋盘格对应的棋盘坐标系下的世界坐标；
11、在所述棋盘坐标系中，选取4个互不共面的点作为虚拟控制点，建立所述棋盘格坐标系到所述虚拟控制点对应的虚拟控制点坐标系的变换关系，将所述特征点的世界坐标通过坐标变换和齐次重心坐标表示，得到所述特征点在所述虚拟控制点坐标系下的坐标表示；
12、基于所述内参数矩阵，结合所述特征点的世界坐标，以及所述特征点在虚拟控制点坐标系下的坐标表示,建立相机成像透视投影模型方程组；
13、在所述相机成像透视投影模型方程组中,引入虚拟控制点在相机坐标系下的未知坐标表示，将所述相机成像透视投影模型方程组转化为控制点在相机坐标系的线性方程组，通过奇异值分解数值优化方法求解所述线性方程组,得到4个虚拟控制点在相机坐标系中的坐标。
14、在一种可选的实施例中，
15、还包括：
16、所述单目相机的内参数矩阵，其公式如下：
17、
18、其中，k表示内参数矩阵，fx表示相机在x轴方向上的焦距，fy表示相机在y轴方向上的焦距，u0表示相机的光心横坐标，v0表示相机光心纵坐标；
19、所述相机成像透视投影模型方程组，其公式如下：
20、
21、其中，j表示虚拟控制点索引，i表示特征点索引，αij表示第i个特征点在虚拟控制点坐标系下的齐次重心坐标，xjc表示第j个虚拟控制点在相机坐标系下的x分量，yjc表示第j个虚拟控制点在相机坐标系下的y分量，zjc表示第j个虚拟控制点在相机坐标系下的z分量，ui表示第i个特征点在图像平面上的横坐标，vi表示第i个特征点在图像平面上的纵坐标。
22、在一种可选的实施例中，
23、引入平行透视投影模型，通过迭代的平行透视求解过程，获得相机位姿初始估计，基于所述相机位姿初始估计，确定相机坐标系下控制点坐标，计算控制点齐次重心坐标系数的初始值；以缩小控制点在相机坐标系和世界坐标系下的距离差为优化目标,利用所述控制点齐次重心坐标系数的初始值,通过迭代优化算法求解，获得空间参考标志物相对相机的精确位姿包括：
24、通过建立平行透视投影模型，将所述相机成像透视投影模型的方程组重构成关于控制点坐标和齐次重心坐标系数的线性方程组；
25、通过迭代求解所述平行透视投影模型的方程组，获得相机位姿的初始估计,并根据所述初始估计，计算控制点在相机坐标系下的初始坐标，基于控制点在相机坐标系下的初始坐标，通过最小二乘法求解齐次重心坐标方程组,得到控制点齐次重心坐标系数的初始值；
26、基于相机位姿，以及所述控制点齐次重心坐标系数的初始值,根据控制点在相机坐标系和世界坐标系下距离差为基础，构建目标函数，使所述目标函数最小化，得到最优的相机位姿估计，获得空间参考标志物相对相机的精确位姿。
27、在一种可选的实施例中，
28、所述目标函数包括：
29、其公式如下：
30、
31、其中，β表示优化目标，j、l表示虚拟控制点索引，cjc表示第j个虚拟控制点在相机坐标系下的坐标，clc表示第l个虚本文档来自技高网...
【技术保护点】
1.一种基于EPnP算法的单目视觉姿态估计方法，其特征在于，包括：
2.根据权利要求1所述的方法，其特征在于，通过相机标定获得单目相机内参；确定所述单目相机的视野内布置的空间参考标志物上特征点的世界坐标,以及所述特征点在相机图像上的像素坐标；将所述特征点的世界坐标表示为4个非共面虚拟控制点的线性组合,建立相机透视投影模型,形成关于所述虚拟控制点相机坐标的线性方程组,确定所述虚拟控制点在相机坐标系对应的相机坐标包括：
3.根据权利要求2所述的方法，其特征在于，还包括：
4.根据权利要求1所述的方法，其特征在于，引入平行透视投影模型，通过迭代的平行透视求解过程，获得相机位姿初始估计，基于所述相机位姿初始估计，确定相机坐标系下控制点坐标，计算控制点齐次重心坐标系数的初始值；以缩小控制点在相机坐标系和世界坐标系下的距离差为优化目标,利用所述控制点齐次重心坐标系数的初始值,通过迭代优化算法求解，获得空间参考标志物相对相机的精确位姿包括：
5.根据权利要求4所述的方法，其特征在于，所述目标函数包括：
6.根据权利要求1所述的方法，其
7.根据权利要求1所述的方法，其特征在于，所述单目相机实时拍摄所述空间参考标志物对应的图像包括：
8.一种基于EPnP算法的单目视觉姿态估计系统，其特征在于，包括：
9.一种电子设备，其特征在于，包括：
10.一种计算机可读存储介质，其上存储有计算机程序指令，其特征在于，所述计算机程序指令被处理器执行时实现权利要求1至7中任意一项所述的方法。
...【技术特征摘要】
1.一种基于epnp算法的单目视觉姿态估计方法，其特征在于，包括：
2.根据权利要求1所述的方法，其特征在于，通过相机标定获得单目相机内参；确定所述单目相机的视野内布置的空间参考标志物上特征点的世界坐标,以及所述特征点在相机图像上的像素坐标；将所述特征点的世界坐标表示为4个非共面虚拟控制点的线性组合,建立相机透视投影模型,形成关于所述虚拟控制点相机坐标的线性方程组,确定所述虚拟控制点在相机坐标系对应的相机坐标包括：
3.根据权利要求2所述的方法，其特征在于，还包括：
4.根据权利要求1所述的方法，其特征在于，引入平行透视投影模型，通过迭代的平行透视求解过程，获得相机位姿初始估计，基于所述相机位姿初始估计，确定相机坐标系下控制点坐标，计算控制点齐次重心坐标系数的初始值；以缩小控制点在相机坐标系和世界坐标系下的距离差为优化目标,利用所述控制点齐次重心坐标系数的初始值,通过迭代优化算法求解，...
【专利技术属性】
技术研发人员：胡宁，朱远远，
申请(专利权)人：苏州铸正机器人有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。