[发明专利]一种近距离三维姿态测量方法

权利要求书

1.一种近距离三维姿态测量方法，其特征在于，包括以下步骤：

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，得到由世界坐标系到相机坐标系的方位角：