研究了基于二维(QR)码识别的P4P算法,提出利用QR码中存储的四个共面顶点的坐标和QR码图像中的像素坐标进行定位。通过相机标定和图像校正得到校正的QR码四个共面顶点的像素坐标,通过译码程序解析得到四个共面顶点的实际位置坐标,将P4P算法解算输出的旋转矩阵和平移向量作为下一步算法的迭代初始值。在透视N点投影算法中,借助李群和李代数之间的局部同构关系,将流形上的寻优转化为切空间中的寻优,利用线性空间中的最优化手段对目标值进行优化。在切空间的任何梯度方向,参数值都在该空间内,即所求矩阵都在流形上。实验结果表明,在导航运动过程中,相机坐标位置误差小于±2 mm,旋转角度误差小于±0.5°,算法具有较高的定位精度及稳定性。