摘 要 針對(duì)實(shí)際應(yīng)用中求解物體間相對(duì)位姿的問題，提出了一種基于標(biāo)志點(diǎn)位置信息的相對(duì)位姿計(jì)算方法。仿真結(jié)果表明，所提算法能夠獲得物體間精確的相對(duì)位姿。

【關(guān)鍵詞】標(biāo)志點(diǎn) 相對(duì)位置 相對(duì)姿態(tài)

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

1 問題描述

如圖1所示，在參考單元上建立參考坐標(biāo)系R（O-XYZ），在運(yùn)動(dòng)單元上建立運(yùn)動(dòng)坐標(biāo)系S（o-B1B2B3），并且可以實(shí)時(shí)獲得運(yùn)動(dòng)單元表面各點(diǎn)在R系的精確位置坐標(biāo)，需要計(jì)算O-o間的相對(duì)位姿。

由于O和o均位于物體內(nèi)部，傳統(tǒng)的方法無法進(jìn)行直接測量；同時(shí)由于位置和姿態(tài)的耦合關(guān)系，利用運(yùn)動(dòng)單元上標(biāo)志點(diǎn)的空間位置坐標(biāo)，也無法直接提供所需的姿態(tài)信息。在現(xiàn)有條件下，可以準(zhǔn)確測得運(yùn)動(dòng)單元上的標(biāo)志點(diǎn)在R系中的實(shí)時(shí)空間三維位置坐標(biāo)，同時(shí)還有標(biāo)志點(diǎn)與運(yùn)動(dòng)單元內(nèi)部測量點(diǎn)之間距離的先驗(yàn)信息。如何根據(jù)上述數(shù)據(jù)，快速、準(zhǔn)確地獲得物體間的相對(duì)位姿，是本文所需解決的問題。

2 相對(duì)位姿求解

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

2.1 計(jì)算標(biāo)志點(diǎn)與待測點(diǎn)的距離，這里距離是已知的先驗(yàn)知識(shí)

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

2.4 求解相對(duì)姿態(tài)

在得到待測點(diǎn)的三維位置坐標(biāo)基礎(chǔ)上，結(jié)合在S系中標(biāo)志點(diǎn)與待測點(diǎn)所構(gòu)成的位置向量保持不變的特性，利用運(yùn)動(dòng)單元待測點(diǎn)與標(biāo)志點(diǎn)構(gòu)成的位置向量在R系與S系不同投影之間的轉(zhuǎn)換關(guān)系，計(jì)算得到運(yùn)動(dòng)單元與參考單元間的姿態(tài)轉(zhuǎn)換矩陣：

3 算法驗(yàn)證

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

設(shè)運(yùn)動(dòng)單元相對(duì)參考單元三個(gè)軸向均以1°/s轉(zhuǎn)動(dòng)，運(yùn)動(dòng)單元的待測點(diǎn)相對(duì)參考單元三個(gè)軸向均以10mm/s平動(dòng)，待測點(diǎn)初始位置的近似三維坐標(biāo)相對(duì)理想值分別相差30mm、25mm和15mm。在上述假設(shè)下，通過轉(zhuǎn)動(dòng)和平動(dòng)信息可實(shí)時(shí)給出運(yùn)動(dòng)單元上的3個(gè)標(biāo)志點(diǎn)在R系的位置坐標(biāo)。利用本文所提的相對(duì)位姿計(jì)算方法，得到如圖3、圖4的仿真結(jié)果。

4 結(jié)論

本文提出一種基于標(biāo)志點(diǎn)位置信息的相對(duì)位姿計(jì)算方法，該算法基于運(yùn)動(dòng)單元上標(biāo)志點(diǎn)的動(dòng)態(tài)位置信息、標(biāo)志點(diǎn)與運(yùn)動(dòng)單元待測點(diǎn)的相對(duì)距離以及位置向量在不同坐標(biāo)系中的轉(zhuǎn)換關(guān)系，可以實(shí)現(xiàn)物體間高精度的相對(duì)位姿計(jì)算。該算法實(shí)現(xiàn)簡便，可滿足相對(duì)位姿測量的要求。

參考文獻(xiàn)

[1]王惠南.GPS導(dǎo)航原理與應(yīng)用[M].科學(xué)出版社，2003.

[2]秦永元.慣性導(dǎo)航[M].科學(xué)出版社，2006.

作者簡介

張仟雨（2000-），女，陜西省西安市人。研究方向?yàn)閿?shù)據(jù)處理。

張亞崇（1972-），男，陜西省西安市人。博士。研究方向?yàn)閷?dǎo)航信息融合。

作者單位

1.西北大學(xué)附屬中學(xué) 陜西省西安市 710075

2.西安飛行自動(dòng)控制研究所 陜西省西安市 710065endprint