CN108458692A - 一种近距离三维姿态测量装置及测量方法 - Google Patents

Abstract

本发明涉及一种近距离三维姿态测量装置及测量方法，解决现有测量方法测量精度低、测量系统装置复杂的问题。装置包括合作目标、物镜前组、物镜中组、半透半反棱镜、物镜后组、成像接收单元、数据处理模块和准直发射单元；准直发射单元包括点光源、准直扩束镜组、分划板和反射镜，点光源通过准直扩束镜组发出的光束被反射至半透半反棱镜，反射后的光通过物镜中组、物镜前组射向平面反射镜，被平面反射镜反射后光束再沿着物镜前组、物镜中组、半透半反棱镜和物镜后组被成像接收单元接收；成像接收单元通过物镜后组、半透半反棱镜、物镜中组和物镜前组对合作目标上的合作标识进行成像，本发明还提供一种基于上述装置的三维姿态测量方法。

Description

一种近距离三维姿态测量装置及测量方法

技术领域

本发明涉及通用测角应用技术领域，具体涉及一种近距离三维姿态测量装置及测量方法，主要应用于大型工程如桥梁大坝等形变监测、观测雷达天线基座的三维姿态测量、立体结构件的空间姿态测量等三维姿态测量。

背景技术

在精密测量、大型工程监测、姿态角度等测量中，经常涉及到对待测物体空间姿态的测量，如建筑工程中钢梁的安装方向误差测量、精密仪器装配中结构件装配误差检测、立方体基座姿态测量等。

目前的三维姿态测量方式，有基于针孔成像和双矢量定姿原理的方法，其俯仰角、偏航角、滚动角的精度为9.9″、9.3″、80.2″(江洁等，大量程高精度三维姿态角测量系统设计[J].仪器仪表学报，2013.6:1247-1252.第34卷第6期)；有基于双目视觉测量原理进行三维姿态测量的方法，其静态测量精度为0.1°，动态测量精度为0.5°(阮利锋等，基于标志点识别的三维位姿测量方法[J]，计算机应用，2008.11：2856-2862.第28卷第11期)；有弱透视成像模型单目相机测量的方法，在离相机1～3km成像时姿态测量误差低于1.5°(赵汝进等，基于弱透视成像模型的目标三维姿态测量[J]，光子学报，2014.5：0512002(1-6).第43卷第5期)，但是以上方法具有测量精度低、测量系统装置复杂等问题。

发明内容

本发明的目的是解决现有测量方法测量精度低、测量系统装置复杂的问题，提供一种近距离三维姿态测量装置及测量方法。

本发明的技术方案是：

一种近距离三维姿态测量装置，包括合作目标、物镜前组、物镜中组、半透半反棱镜、物镜后组、成像接收单元、数据处理模块和准直发射单元；所述合作目标安装于待测对象上，包括用于准直测量的平面反射镜和用于成像测量的合作标识；所述合作目标、物镜前组、物镜中组、半透半反棱镜、物镜后组、成像接收单元依次设置；所述准直发射单元包括依次设置的点光源、准直扩束镜组、分划板和反射镜，点光源通过准直扩束镜组发出的准直测量光束被反射镜反射至半透半反棱镜，被半透半反棱镜反射后的光通过物镜中组、物镜前组射向平面反射镜，被平面反射镜反射后光束再沿着物镜前组、物镜中组、半透半反棱镜和物镜后组被成像接收单元接收；所述成像接收单元通过物镜后组、半透半反棱镜、物镜中组和物镜前组对合作目标上的合作标识进行成像；所述数据处理模块与成像接收单元连接，对成像接收单元控制。

进一步地，所述合作目标的一侧设置有主动照明模。

进一步地，所述合作目标为矩形平面反射镜或圆形平面镜加四点合作标识。

进一步地，所述分划板为小孔分划板。

进一步地，所述点光源为激光光源或LED光源。

进一步地，所述成像接收单元为成像传感器，成像传感器阵列的分辨率不小于400万像素。

同时，本发明还提供一种基于上述测量装置的近距离三维姿态测量方法，包括以下步骤：

1)在测量装置正前方的成像准直瞄准区域内，将合作目标固装在待测对象上；

2)求取横滚角、俯仰角；

2.1)装置上电，点光源发出的光被成像接收单元接收，对接收到的光亮点进行捕获，并提取目标像素位置；

2.2)目标像素位置代入二维准直测角算法进行角度解算；

平面反射镜垂直于准直望远系统主光轴，返回光线在成像接收单元上所成像的中心位置O为系统电零位，平面反射镜的法线与准直望远系统主光轴有夹角α时，反射光线与准直望远系统主光轴的夹角为2α，且返回光线在成像接收单元上所成像的中心位置O’，x表示O’相对于系统电零位O的水平方向位移量，y表示O’相对于系统电零位O的垂直方向位移量，α表示反射部件转过的横滚角，β表示反射镜的俯仰角，则得到

3)求取方位角；

3.1)对合作目标上的四点合作标识成像，合作标识的物理尺寸为已知参数，数据处理模块对四点合作标识的图像处理，提取出四点合作标识的像素点坐标，

3.2)标定相机的内参数：等效焦距、图像主点坐标以及像差系数；

3.3)合作标识的物理参数与像素点坐标代入P4P摄影测量算法，获得合作标识相对于相机的方位角Az；

通过摄影测量中的P4P方法，方位角解算方法为：

其中，Zc为物点到光心的距离在光轴方向的投影；(x_i,y_i)为图像像素坐标，等式右边第一项代表相机的内部参数；(F_x,F_y)为相机的等效焦距，f为镜头焦距，a、b为像元在x、y方向的尺寸；(C_x,C_y)为图像主点；(x_Wi y_Wi z_Wi)^T为四个圆形标识的圆心，在合作目标上的物理尺寸坐标，即世界坐标系下的坐标；由于四点共面，取z_Wi＝0，等式右边第二项表示世界坐标系到相机坐标系的转换矩阵，其中：

R、T分别为相机坐标系相对于目标世界坐标系的旋转矩阵和平移向量，设相机坐标系到世界坐标系的旋转过称为先绕Z_w轴转角度A_z，再绕当前的X_w轴转A_x，最后绕当前的Y_w轴转A_y，其中角度A_z为方位角，得r₀＝cos(A_y)cos(A_z)+sin(A_x)sin(A_y)sin(A_z)，r₁＝cos(A_y)sin(A_z)-cos(A_z)sin(A_x)sin(A_y)，r₂＝cos(A_x)sin(A_y)，r₃＝-cos(A_x)sin(A_z)，r₄＝cos(A_x)cos(A_z)，r₅＝sin(A_x)，r₆＝cos(A_y)sin(A_x)sin(A_z)-cos(A_z)sin(A_y)，r₇＝-sin(A_y)sin(A_z)-cos(A_y)cos(A_z)sin(A_x)，r₈＝cos(A_x)cos(A_y)；

令：

因为z_C≠0，将式(3)代入式(1)，得到如下共线方程：

四个圆形标识共面，即z_wi＝0，因此上式简化为：

由于m₁₁＝t_Z≠0，等式两边分别除以m₁₁，并整理得出如下方程：

令：

简化为如下方程组：

通过已知坐标的四个共面点联立线性方程组，组成线性方程组，解出s₀～s₇，进而求解出m₀～m₁₁，再通过式(3)解算，得到R和T，进而得到r3和r4，最后得到由世界坐标系到相机坐标系的方位角：

本发明的优点为：

1.本发明装置和方法可以高精度测出待测对象的三维姿态角度信息，二维准直测量与成像测量进行共轴光路设计，测量精度高；准直测量得到的俯仰角和横滚角，测量精度可以达到2″以内，成像测量得到的方位角，测量精度可以达到10″以内，该装置可以作为一种通用的三维姿态测量仪器，应用于近距三维姿态测角需求的各个领域。

2.本发明装置能够应用于不同的光照环境下，可以在室内外正常日光光照、弱光、无光条件下进行工作。装置具有两个工作模式，即准直测量模式和成像测量模式，两个模式针对不同的测量目标，分别测得俯仰角与横滚角、方位角，提高了装置的环境适应性。

3.本发明合作目标上无需加电装置，使得装置更易维护使用。

4.本发明合作目标设置简单，应用范围广、算法鲁棒性好、可同时实现三维姿态高精度测量，具有重要的实用价值。

5.本发明装置外形紧凑，便于使用。

6.本发明装置具有主动照明模块，当装置处于成像测量模式下时，点亮照明模块，照明成像合作目标，提高成像合作目标与背景的对比度，使装置可以在暗光或者无光环境下正常工作，进一步提高装置的环境适应性。

附图说明

图1为本发明三维姿态测量装置结构图；

图2为本发明准直测角原理图；

图3为本发明成像方位测量原理图；

图4为本发明实施例一种合作目标组成图；

图5为本发明实施例另一种合作目标组成图。

附图标记：1-成像接收单元，2-准直发射单元，3-物镜后组，4-半透半反棱镜，5-物镜中组，6-数据处理模块，7-物镜前组，8-合作目标，9-主动照明模块，11-反射部件，12-准直望远系统，21-点光源，22-准直扩束镜组，23-分划板，24-反射镜，81-平面反射镜，82-合作标识，83-平面反射镜，84-合作标识。

具体实施方式

下面结合说明书附图对本发明的技术方案进行清楚、完整地描述。

本发明三维姿态角度高精度测量装置包括二维准直测量和一维成像测量两部分，准直测量部分与成像测量部分进行光路同轴复用，两部分同时对合作目标进行准直成像，解算出合作目标相对于装置的俯仰角、横滚角和方位角。

如图1所示的近距离三维姿态测量装置，包括合作目标8、物镜前组7、物镜中组5、半透半反棱镜4、物镜后组3、成像接收单元1、数据处理模块6和准直发射单元2；合作目标8安装于待测对象上，包括用于准直测量的平面反射镜81和用于成像测量的合作标识82；合作目标8、物镜前组7、物镜中组5、半透半反棱镜4、物镜后组3、成像接收单元1依次设置；数据处理模块6与成像接收单元1连接，对成像接收单元1控制；准直发射单元2包括依次设置的点光源21、准直扩束镜组22、分划板23和反射镜24，点光源21通过准直扩束镜组22发射出准直测量光束被反射镜24反射至半透半反棱镜4，被半透半反棱镜4反射后的光通过物镜中组5、物镜前组7射向平面反射镜81，被平面反射镜81反射后光束再沿着物镜前组7、物镜中组5、半透半反棱镜4和物镜后组3被成像接收单元1接收；成像接收单元1通过物镜后组3、半透半反棱镜4、物镜中组5和物镜前组7对合作目标8上的合作标识82进行成像。

准直发射单元2的光源可选择激光光源，也可选择LED光源，本实施例中选用LED光源，分划板23选用小孔分划板；成像相机图像传感器的选择应与装置的镜头焦距、成像合作标识82的尺寸大小相匹配；物镜前组7的设计主要考虑测量距离，成像合作目标8允许的尺寸大小等因素；合作目标8的一侧设置有主动照明模块9，主动照明模块9可采用任何可将相机成像区域内合作目标8照亮的器材或装置，照明装置的谱线范围应与相机成像芯片的响应谱线范围一致。在本实施例中，装置光学成像系统的焦距为250mm，成像传感器为CMV4000型传感器阵列(分辨率：2048×2048，像元尺寸5.5μm×5.5μm)；测量工作距离为4m，图4中成像合作标识82的间距为150mm，每个标识的圆直径为6mm。成像接收单元1为成像传感器，成像传感器阵列的分辨率不小于400万像素。

测量的合作目标8分为两部分，一部分为平面反射镜81，作为准直测量的合作目标；另一部分为四点合作标识82，也可将平面镜加工为方形，以其边框作为成像测量的合作目标。待测对象上安装合作目标8，该合作目标8用于表征待测目标自身的姿态基准，根据不同的工作环境，合作目标8有不同的设计形式，其形式可为矩形平面反射镜，也可为圆形平面镜加四点合作标识82(四个圆点的圆心为一矩形)等其他可以作为二维准直和成像测量合作目标8的形式。

如图4所示，合作目标8的功能部分包括准直测量合作目标和成像测量合作目标，准直测量的合作目标用于反射二维自准直仪发射出来的光束，在本案例中选用平面反射镜81，成像合作目标为四点圆形标识，其组成的正方形边长为已知参数；也可以将准直测量合作目标设计为方形，如图5所示，反射部件为矩形平面反射镜，其外部加装方框作为成像测量的合作目标，其中，矩形边四点为合作标识84，矩形平面反射镜为平面反射镜83，方框边长为已知参数。

本发明主要应用光学准直测量与成像摄影测量等方法，具体有光路设计、光机设计、图像处理和视觉测量等，通过二维准直姿态测量和成像方位测量，俯仰角和横滚角测角精度可以达到2″以内，方位角测角精度可以达到10″以内，二维姿态角(即俯仰角和横滚角)的测量范围≥±1°，方位测角范围为±90°。

本发明由于针对的是较近距离的三维姿态角度求解，可取准直测量与成像测量的光学镜头一致，即装置采用共用光路设计方案，实现对待测对象的三维姿态测量，二维准直测量实现俯仰角和横滚角的高精度测量，成像测量实现方位角的高精度测量。该装置的工作测量距离范围主要受光学成像镜头的焦距参数所约束，当测量距离较远时，成像光学镜头与准直接收光学系统不能同轴工作时，即成像光学系统的焦距与准直接收光学系统的光学参数不一致时，该装置不能再进行三维姿态测量工作。在本发明装置中，光学系统参数的设计应该综合考虑测量工作距离、合作目标8所允许的外形尺寸、成像传感器的分辨率与物理尺寸等因素。合作目标8在成像传感器上成像后，所占的画幅尺寸应不小于75％。成像传感器阵列的分辨率应不小于400万像素，分辨率越高，测量精度也越高。

同时，本发明还提供了一种基于上述测量装置的近距离三维姿态测量方法，包括以下步骤：

1)在测量装置正前方的成像准直瞄准区域内，将合作目标固装在待测对象上；

2)求取横滚角、俯仰角；

2.1)装置上电，点光源发出的光被成像接收单元接收，对接收到的光亮点进行捕获，并提取目标像素位置；

2.2)目标像素位置代入二维准直测角算法进行角度解算；

反射部件垂直于准直望远系统主光轴时，返回光线在图像传感器上所成像的中心位置O为系统的电零位，反射部件的法线与准直望远系统主光轴有夹角α时，反射光线与准直望远系统主光轴的夹角为2α，且返回光线在图像传感器上所成像的中心位置O’，用x表示O’相对于系统电零位O的水平方向位移量，用y表示O’相对于系统电零位O的垂直方向位移量，用α表示反射部件转过的横滚角，用β表示反射镜的俯仰角，则可得

3)求取方位角；

对合作目标上的四点合作标识进行清晰成像，合作标识的物理尺寸为已知参数，数据处理模块对图像处理，提取出四点标识的像素点坐标，经合作标识的物理参数与像素坐标代入P4P摄影测量算法，得到成像合作标识相对于相机的方位角Az：

本发明近距离三维姿态测量方法具体为：

1)在本发明装置正前方的成像准直瞄准区域内，将合作目标8固装在待测对象上，本发明装置的标准工作测量距离为0.8m～8m，改变光学成像系统的光学参数，以及合作目标8的大小可进一步增大其工作测量范围。合作目标8安装到位后，应保证合作目标8与待测对象之间不发生位置姿态变化，合作目标8包括用于准直测量的平面反射镜81和用于成像测量的合作标识82，平面反射镜81用于将由本发明装置发射出的准直测量光束反射回发明装置中，协助实现二维准直测量，合作标识82提供四个圆点或者角点的空间坐标信息，协助实现成像测量；

2)求取横滚角、俯仰角；

给本发明装置上电，装置中的二维准直测量模块和成像测量模块开始工作；本装置具有两个工作模式，即二维准直测量模式和成像测量模式，数据处理模块6进行工作模式控制，两种模式交替工作，以每完成一次交替测量为一个工作周期；

二维准直测量的点光源21通过准直扩束发射出准直测量光束，通过半透半反棱镜4、物镜中组5、物镜前组7，射向平面反射镜81，反射后光束，再沿着物镜前组7、物镜中组5、半透半反棱镜4，被装置的成像接收单元1(成像传感器)接收，通过对接收到的光亮点，进行捕获并提取目标像素位置，代入二维准直测角算法进行角度解算，就可以得到目标的俯仰角和横滚角；

二维准直测量的原理如图2所示，点光源21发出的光经准直望远系统12形成平行光，平行光束经反射部件11返回后在图像传感器上成像。若反射部件11垂直于准直望远系统12主光轴，则光线原路返回，且返回光线在图像传感器上所成像的中心位置O为系统电零位；若反射部件11的法线与准直望远系统12主光轴有夹角α时，则反射光线与准直望远系统12主光轴的夹角为2α，且返回光线在图像传感器上所成像的中心位置O’与系统的电零位距离为s。根据几何光学基本原理可得：s＝f′·tan2α，其中，f′为准直望远系统12的物镜焦距；

若α很小，则tan2α≈2α，则可得：在二维情况下，若用x表示O’相对于系统电零位O的水平方向位移量，用y表示O’相对于系统电零位O的垂直方向位移量，用α表示反射部件11转过的横滚角，用β表示反射镜24的俯仰角，则可得

3)求取方位角；

在二维准直测量的同时，成像接收单元1通过物镜后组3、半透半反棱镜4、物镜中组5和物镜前组7对合作目标8上的合作标识82进行清晰成像，合作标识82的物理尺寸为已知参数，通过数据处理模块6，对图像进行去噪、二值化、形态学处理、质心提取等一系列处理，提取出四点标识的像素点坐标，合作标识的物理参数与像素坐标代入P4P摄影测量算法，即可得到方位角。

成像方位测量的原理如图3所示，四点圆形标识作为成像测量的合作标识，四点组成的正方形边长已知，在相机图像传感器上成像，得到四个像点，数据处理模块对拍得的图像进行处理解算，通过去噪、二值化、形态学算法处理、质心提取等一系列图像处理操作，对像点进行亚像素定位，对其像素坐标进行提取，提取得到四个像点的质心像素坐标。在进行方位解算前，应先标定出相机的内参数，主要是等效焦距、图像主点坐标以及像差系数。

在目前的技术条件下，相机光学镜头的设计与加工水平较高，特别是视场中心附近，相机成像的像差可忽略不计，即像差系数可不予标定，默认为零。相机的内参数标定方法，在本实施案例中用棋盘格标定相机线性参数或者其他可以得到相机内部参数的标定方法；

利用摄影测量中的P4P方法，方位角解算方法为：

其中，Zc为物点到光心的距离在光轴方向的投影，(x_i,y_i)为图像像素坐标，等式右边第一项代表了相机的内部参数,(F_x,F_y)为相机的等效焦距，f为镜头焦距，a、b为像元在x、y方向的尺寸，(C_x,C_y)为图像主点，即光轴与像面交点O的图像坐标；(x_Wiy_Wi z_Wi)^T为四个圆形标识的圆心，在合作目标台上的物理尺寸坐标，即世界坐标系下的坐标，由于四点共面，可取z_Wi＝0，第二项表示世界坐标系到相机坐标系的转换矩阵，其中：

分别为相机坐标系相对于目标世界坐标系的旋转矩阵和平移向量。设相机坐标系到世界坐标系的旋转过称谓先绕Z_w轴转角度A_z，再绕当前的X_w轴转A_x，最后绕当前的Y_w轴转A_y，其中角度A_z为方位角，可得r₀＝cos(A_y)cos(A_z)+sin(A_x)sin(A_y)sin(A_z)，r₁＝cos(A_y)sin(A_z)-cos(A_z)sin(A_x)sin(A_y)，r₂＝cos(A_x)sin(A_y)，r₃＝-cos(A_x)sin(A_z)，r₄＝cos(A_x)cos(A_z)，r₅＝sin(A_x)，r₆＝cos(A_y)sin(A_x)sin(A_z)-cos(A_z)sin(A_y)，r₇＝-sin(A_y)sin(A_z)-cos(A_y)cos(A_z)sin(A_x)，r₈＝cos(A_x)cos(A_y)；

令：

因为z_C≠0，将式(3)代入式(1)，因此可以得到如下共线方程：

四个圆形标识共面，因此可以认为即z_wi＝0。因此上式可以简化为：

由于m₁₁＝t_Z≠0，因此等式两边分别除以m₁₁，并整理得出如下方程：

令： 因此可以简化为如下方程组：

通过已知坐标的四个共面点联立线性方程组，八个方程八个未知数，组成线性方程组，即可求解出s₀～s₇，进而求解出m₀～m₁₁。

进而求出相机坐标系相对于世界坐标系的作用矩阵，得到作用矩阵后通过进一步的解算即可得到世界坐标系与相机坐标系之间的旋转矩阵。

旋转矩阵表征了目标体坐标系相对于相机坐标系的姿态，平移向量表征了目标体坐标系原点相对于相机坐标系的位置，由于已知m₀～m₁₁，再通过式(3)解算，最终可得到由世界坐标系到相机坐标系的方位角：

即得到成像合作标识相对于相机的方位角。

成像相机与二维自准直仪的相互姿态关系事先标定到位，即可得到待测对象的三维姿态信息。

Claims (7)

1.一种近距离三维姿态测量装置，其特征在于：包括合作目标(8)、物镜前组(7)、物镜中组(5)、半透半反棱镜(4)、物镜后组(3)、成像接收单元(1)、数据处理模块(6)和准直发射单元(2)；所述合作目标(8)安装于待测对象上，包括用于准直测量的平面反射镜(81)和用于成像测量的合作标识(82)；

所述合作目标(8)、物镜前组(7)、物镜中组(5)、半透半反棱镜(4)、物镜后组(3)、成像接收单元(1)依次设置；

所述准直发射单元(2)包括依次设置的点光源(21)、准直扩束镜组(22)、分划板(23)和反射镜(24)，点光源(21)通过准直扩束镜组(22)发出的准直测量光束被反射镜(24)反射至半透半反棱镜(4)，被半透半反棱镜(4)反射后的光通过物镜中组(5)、物镜前组(7)射向平面反射镜(81)，被平面反射镜(81)反射后光束再沿着物镜前组(7)、物镜中组(5)、半透半反棱镜(4)和物镜后组(3)被成像接收单元(1)接收；

所述成像接收单元(1)通过物镜后组(3)、半透半反棱镜(4)、物镜中组(5)和物镜前组(7)对合作目标(8)上的合作标识(82)进行成像；

所述数据处理模块(6)与成像接收单元(1)连接，对成像接收单元(1)控制。

2.根据权利要求1所述的近距离三维姿态测量装置，其特征在于：所述合作目标(8)的一侧设置有主动照明模块(9)。

3.根据权利要求2所述的近距离三维姿态测量装置，其特征在于：所述合作目标(8)为矩形平面反射镜或圆形平面镜加四点合作标识。

4.根据权利要求1或2或3所述的近距离三维姿态测量装置，其特征在于：所述分划板(23)为小孔分划板。

5.根据权利要求4所述的近距离三维姿态测量装置，其特征在于：所述点光源(21)为激光光源或LED光源。

6.根据权利要求5所述的近距离三维姿态测量装置，其特征在于：所述成像接收单元(1)为成像传感器，成像传感器阵列的分辨率不小于400万像素。

7.一种基于权利要求1至6任一所述测量装置的近距离三维姿态测量方法，其特征在于，包括以下步骤：

1)在测量装置正前方的成像准直瞄准区域内，将合作目标固装在待测对象上；

2)求取横滚角、俯仰角；

2.1)装置上电，点光源发出的光被成像接收单元接收，对接收到的光亮点进行捕获，并提取目标像素位置；

2.2)目标像素位置代入二维准直测角算法进行角度解算；

平面反射镜垂直于准直望远系统主光轴，返回光线在成像接收单元上所成像的中心位置O为系统电零位，平面反射镜的法线与准直望远系统主光轴有夹角α时，反射光线与准直望远系统主光轴的夹角为2α，且返回光线在成像接收单元上所成像的中心位置O’，x表示O’相对于系统电零位O的水平方向位移量，y表示O’相对于系统电零位O的垂直方向位移量，α表示反射部件转过的横滚角，β表示反射镜的俯仰角，则得到

3)求取方位角；

3.1)对合作目标上的四点合作标识成像，合作标识的物理尺寸为已知参数，数据处理模块对四点合作标识的图像处理，提取出四点合作标识的像素点坐标，

3.2)标定相机的内参数：等效焦距、图像主点坐标以及像差系数；

3.3)合作标识的物理参数与像素点坐标代入P4P摄影测量算法，得到合作标识相对于相机的方位角Az；

通过摄影测量中的P4P方法，方位角解算方法为：

其中，Zc为物点到光心的距离在光轴方向的投影；(x_i,y_i)为图像像素坐标，等式右边第一项代表相机的内部参数；(F_x,F_y)为相机的等效焦距，f为镜头焦距，a、b为像元在x、y方向的尺寸；(C_x,C_y)为图像主点；(x_Wi y_Wi z_Wi)^T为四个圆形标识的圆心，在合作目标上的物理尺寸坐标，即世界坐标系下的坐标；由于四点共面，取z_Wi＝0，等式右边第二项表示世界坐标系到相机坐标系的转换矩阵，其中：

R、T分别为相机坐标系相对于目标世界坐标系的旋转矩阵和平移向量，设相机坐标系到世界坐标系的旋转过程为先绕Z_w轴转角度A_z，再绕当前的X_w轴转A_x，最后绕当前的Y_w轴转A_y，其中角度A_z为方位角，得

r₀＝cos(A_y)cos(A_z)+sin(A_x)sin(A_y)sin(A_z)，r₁＝cos(A_y)sin(A_z)-cos(A_z)sin(A_x)sin(A_y)，r₂＝cos(A_x)sin(A_y)，r₃＝-cos(A_x)sin(A_z)，r₄＝cos(A_x)cos(A_z)，r₅＝sin(A_x)，r₆＝cos(A_y)sin(A_x)sin(A_z)-cos(A_z)sin(A_y)，r₇＝-sin(A_y)sin(A_z)-cos(A_y)cos(A_z)sin(A_x)，r₈＝cos(A_x)cos(A_y)；

令：

因为z_C≠0，将式(3)代入式(1)，得到如下共线方程：

四个圆形标识共面，即z_wi＝0，因此上式简化为：

由于m₁₁＝t_Z≠0，等式两边分别除以m₁₁，并整理得出如下方程：

令：

简化为如下方程组：

通过已知坐标的四个共面点联立线性方程组，组成线性方程组，解出s₀～s₇，进而求解出m₀～m₁₁，再通过式(3)解算，得到R和T，进而得到r3和r4，得到由世界坐标系到相机坐标系的方位角：