[發明專利]一種近距離三維姿態測量方法

有效

• 申請號: 201810107136.0
• 申請日: 2018-02-02
• 公開（公告）號: CN108458692B
• 公開（公告）日: 2020-04-03
• 發明（設計）人: 郝沖;王致強;吳易明;朱帆
• 申請（專利權）人: 中國科學院西安光學精密機械研究所;中國科學院大學;西安中科光電精密工程有限公司
• 主分類號: G01C11/02
• 分類號: G01C11/02
• 代理公司: 西安智邦專利商標代理有限公司 61211
• 代理人: 楊引雪

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