一種基于標志點位置信息的相對位姿計算方法

摘 要 針對實際應用中求解物體間相對位姿的問題，提出了一種基于標志點位置信息的相對位姿計算方法。仿真結果表明，所提算法能夠獲得物體間精確的相對位姿。

【關鍵詞】標志點 相對位置 相對姿態

在實際應用或工程實踐中，經常會遇到求解兩個或多個運動體間的相對位置、相對姿態（以下簡稱相對位姿）的問題，尤其是要求獲得位于物體內部、無法直接進行測量的幾何中心的相對位姿等應用場合。基于上述需求，本文建立了物體幾何測量中心隨運動體運動的測量模型，并提出了一種適用于物體幾何測量中心間相對位姿測量的計算方法。

1 問題描述

如圖1所示，在參考單元上建立參考坐標系R（O-XYZ），在運動單元上建立運動坐標系S（o-B1B2B3），并且可以實時獲得運動單元表面各點在R系的精確位置坐標，需要計算O-o間的相對位姿。

由于O和o均位于物體內部，傳統的方法無法進行直接測量；同時由于位置和姿態的耦合關系，利用運動單元上標志點的空間位置坐標，也無法直接提供所需的姿態信息。在現有條件下，可以準確測得運動單元上的標志點在R系中的實時空間三維位置坐標，同時還有標志點與運動單元內部測量點之間距離的先驗信息。如何根據上述數據，快速、準確地獲得物體間的相對位姿，是本文所需解決的問題。

2 相對位姿求解

相對位姿的求解包含兩方面：一是如何獲得待測點在R系中的實時空間三維位置坐標，二是如何獲得運動單元相對參考單元的位姿。這里認為運動單元和參考單元均是剛體，在運動過程中本身無形變。針對問題一，由于現有技術手段僅能測得運動單元表面標志點的實時空間三維位置坐標，因此考慮在運動單元表面布設不共面的3個標志點，通過多球面相交測量原理實現對待測點的三維定位，如圖2所示，具體計算步驟如下：

2.1 計算標志點與待測點的距離，這里距離是已知的先驗知識

在得到待測點位置增量的基礎上，即可得到待測點的三維位置坐標，進一步可獲得所需的相對位置信息。在上述解算過程中，只要位置增量是在線性化點附近，這種線性化方法就是可行的。若有多余3個標志點的信息，還可利用冗余測量值，通過最小二乘估計技術進一步提高解算精度。

2.4 求解相對姿態

在得到待測點的三維位置坐標基礎上，結合在S系中標志點與待測點所構成的位置向量保持不變的特性，利用運動單元待測點與標志點構成的位置向量在R系與S系不同投影之間的轉換關系，計算得到運動單元與參考單元間的姿態轉換矩陣：

3 算法驗證

為了驗證所提算法的可行性，構造了如下的實驗場景：參考坐標系R與參考單元的體軸坐標系重合，運動單元為一尺寸為300mm×200mm×200mm（長×寬×高）的長方體，待測點位于該長方體的幾何中心。初始時刻該待測點在R系中的坐標為[0；1000；0]mm，運動單元上的3個標志點在S系的坐標分別為[0；0；100]mm、[100；0；0]mm、[0；150；100]mm，與待測點的距離分別為100 mm、100 mm、180.28 mm。以上幾何位置關系可作為先驗知識，為后續計算提供必要的空間約束關系。

設運動單元相對參考單元三個軸向均以1°/s轉動，運動單元的待測點相對參考單元三個軸向均以10mm/s平動，待測點初始位置的近似三維坐標相對理想值分別相差30mm、25mm和15mm。在上述假設下，通過轉動和平動信息可實時給出運動單元上的3個標志點在R系的位置坐標。利用本文所提的相對位姿計算方法，得到如圖3、圖4的仿真結果。

4 結論

本文提出一種基于標志點位置信息的相對位姿計算方法，該算法基于運動單元上標志點的動態位置信息、標志點與運動單元待測點的相對距離以及位置向量在不同坐標系中的轉換關系，可以實現物體間高精度的相對位姿計算。該算法實現簡便，可滿足相對位姿測量的要求。

參考文獻

[1]王惠南.GPS導航原理與應用[M].科學出版社，2003.

[2]秦永元.慣性導航[M].科學出版社，2006.

作者簡介

張仟雨（2000-），女，陜西省西安市人。研究方向為數據處理。

張亞崇（1972-），男，陜西省西安市人。博士。研究方向為導航信息融合。

作者單位

1.西北大學附屬中學 陜西省西安市 710075

2.西安飛行自動控制研究所 陜西省西安市 710065endprint