基于EPnP算法的单目视觉姿态估计方法及系统

本发明提供一种基于EPnP算法的单目视觉姿态估计方法及系统，涉及视觉定位技术领域，包括获得单目相机内参；布置空间参考标志物,确定特征点的世界坐标和像素坐标；将特征点的世界坐标表示为4个非共面虚拟控制点的线性组合,建立相机透视投影模型,形成线性方程组,确定虚拟控制点在相机坐标系下的坐标；引入平行透视投影模型,获得初始估计,并确定相机坐标系下控制点坐标，计算初始值；以缩小坐标系距离差为优化目标，利用初始值，通过迭代优化算法求解，获得精确位姿；将单目相机固定于机械臂末端,记录下基准位姿；当发生移动时,相机获取当前位姿,结合基准位姿和机械臂末端当前位姿,实时计算出机械臂末端应移动到的目标位姿。