基于三靶球坐標求解空間物體位姿的研究

賴夢恬，蹇開林

(重慶大學 航空航天學院， 重慶 400044)

在航空航天領域中，為解決飛行器外掛物在與母體分離后的初始階段不至于撞擊到母體的安全問題，需要確定外掛物分離初始階段的運動軌跡，從而為其在母機上的合適布局和投放參數控制提供可靠依據。

采用捕獲軌跡試驗(captive trajectory system，CTS)可以預測外掛物離開母體的初始階段的運動軌跡。捕獲軌跡試驗又稱可控軌跡試驗，它是一種先進的測量外掛物分離后軌跡的試驗方法。它利用電子計算機、六自由度機構和風洞的相互配合，通過對每一分離時刻外掛物氣動載荷的測量、外掛物空間六自由度運動方程的求解以及對六自由度機構支撐的外掛物模型在母機流場中運動的控制，來實現對外掛物投放軌跡的模擬[1]，現代飛行器及其外掛物的研制和改型都要求進行CTS試驗[2]。捕獲軌跡試驗的外掛物模型只需外形幾何相似，容易設計制作，且模型可反復使用，無需像投放試驗要求模型動力相似，因而該方法已被廣泛用于測量外掛物投放/發射軌跡的測量[3]。

六自由度機構的運動精度對試驗成功與否有著重要影響。本次試驗所用的六自由機構是一個串聯機構，其尾支桿末端能實現沿著X、Y、Z方向的平移和俯仰、偏航、滾轉[4]。六自由度機構的運動學方程對應的是理想設計狀態的模型。而對于實際加工的六自由度機構，由于機械加工中的加工誤差和裝配誤差導致機構的幾何參數和理論設計存在著誤差，從而導致利用運動學逆解通過控制器在給定驅動下，控制機構運動所得到的外掛物的實際位姿與要求的目標位姿存在誤差。因此，必須考慮實際制作完成的六自由度機構由于加工而引起的幾何參數誤差，將幾何誤差引入到六自由度機構運動學方程中，建立機構運動學誤差模型，實際應用時通過計算機軟件程序進行補償[5]。補償過程首先需要對機構進行標定，因此需要測量出機構運行時外掛物模型的實際位姿，并與理想的外掛物模型位姿比較才能獲得機構的誤差，從而實現六自由度機構的標定。

本文采用法如(FARO)公司的激光跟蹤儀，通過測量2個角度和1個距離來確定目標靶球在固定坐標系下的坐標。2個角度是由安裝在定點角軸和方位角軸上的編碼器來測量的，1個距離是由距離測量儀來測量。依次測到固聯在外掛物模型上的3個靶球的坐標之后，通過本文給出的算法可以求解出外掛物的實際位姿，從而確定機構的定位精度，并對機構運動學進行修正，為試驗順利進行提供保障。

1 外掛物模型位姿求解原理

外掛物模型的位姿是外掛物模型的位置和姿態的簡稱。外掛物模型的位置采用質心在固定坐標系的3個坐標來描述，外掛物的姿態采用3個姿態角來描述，3個姿態角的具體定義將在下面給出。事實上，外掛物的質心在物體的內部，不便直接測量其坐標，而3個姿態角需要專門的陀螺儀進行測量。本文使用的激光跟蹤儀只能測量出固定在外掛物模型表面的靶球(可視為一點)在固定坐標系下的坐標，即外表面上點的坐標。因此，本文通過建立外掛物表面上點的坐標與外掛物模型位姿的方程，利用測量出的外掛物表面上3個點的坐標，采用非線性最小二乘法求解出了外掛物模型的位置和姿態。

外掛物模型的位姿可以由如下方式描述：首先定義一個六自由度機構的初始位置以及一個固連在大地的坐標系，稱為固定坐標系。原點定義在初始零位的外掛物模型轉動中心，也是外掛物的理論質心。再定義一個固定在外掛物模型上的坐標系，稱為體軸坐標系，初始時刻與機構坐標系重合。試驗用六自由度機構是一個串聯機構，先執行偏航機構，再帶動俯仰機構，最后運動滾轉機構。因此，建立的2個坐標系的關系可以用以下順序的3次旋轉說明：首先使固定坐標系繞y轉過偏航角ψ，再繞z轉過俯仰角θ，最后繞x轉過滾轉角φ，于是固定坐標系就轉換到了體軸坐標系。根據以上的關系，可以定義外掛物模型在空間中的姿態并得到姿態矩陣[6-7]。固聯在外掛物模型表面上的靶球在固定坐標系下旋轉前坐標{rm}與旋轉后坐標{rn}可以由方向余弦矩陣表示：

{A}3×3{rm}={rn}

(1)

其中{A}3×3的形式為

在零位時外掛物模型理論質心指向運動后外掛物模型理論質心的向量在固定坐標系上的投影{Δr}定義為外掛物模型的位置。

固連在外掛物模型上的靶球在固定坐標系下前后兩次的坐標可以由如下關系導出：設在零位時激光跟蹤儀的讀數，即靶球在固定坐標系下的坐標為

{rr0}={xc0yc0zc0}T

(2)

由于零位時固定坐標系和體軸坐標系重合，{rr0}的值也是靶球在體軸坐標系的值。經過平移和旋轉后運動到某一位姿(該位姿即本文求解的位姿)后讀數為(在同一固定坐標系)

{rr1}={xc1yc1zc1}T

(3)

物體由零位運動到某一位姿的過程可以分解成先平移后轉動的一個過程。

如圖1所示，設進行第1步平移后靶球在固定坐標系的坐標為{rp}，則有

{rp}={Δr}+{r′}

(4)

式中{Δr}是兩個坐標系的平移量，其物理意義為外掛物模型的位置。{r′}為靶球在體軸坐標系o1x′y′z′上的坐標，值與{rr0}相等，即

{r′}={rr0}

(5)

如圖2所示，經過繞體軸3次轉動后，靶球在坐標系o1x′y′z′下的投影為{r1}，根據式(1)有

{r1}={A}3×3{r′}

(6)

圖2 轉動關系

根據圖1，激光跟蹤儀所測的固定坐標系ooxoyoz0與坐標系o1x′y′z′存在著一個向量{Δr}={ΔxΔyΔz}T平移的關系(表示外掛物模型的位置)。因此，實測空間物體運動后靶球的坐標{rr1}應為

{rr1}={Δr}+{r1}

(7)

聯立式(5)-(7) 得

{A}3×3{rr0}+{Δr}={rr1}

(8)

式(8)為激光跟蹤儀兩次讀數{rr0}、{rr1}之間的關系。

2 非線性最小二乘法求解位姿

根據式(8)，將3個靶球的運動前后共6組坐標分別代入向量方程可以得到：

(9)

將其展開并移項后寫成：

(10)

其中aij為方向余弦矩陣{A}3×3中的元素，是姿態角ψθφ三個未知量的函數。加上未知的平移量ΔxΔyΔz，方程組為包含有6個未知量的9個方程，為超定非線性方程組，因此將求解轉化為非線性最小二乘問題。根據本文第1部分的理論推導，方程組存在著一組解為外掛物的真實位姿,為避免姿態角多解情況的出現，選取分離體模型上的3個靶球點位必須不共線。該方程組的求解可選用具有較高求解精度的Gauss-Newton法格式[8-9]。

設：

β=(ΔxΔyΔzψθγ)T

(11)

超定方程組可以寫成

fi(β)=0,i=1,2,…,9

(12)

Gauss-Newton法求解上述方程組的最小二乘解，首先確定初值β0，迭代格式為:

β(k))

(13)

計算雅克比矩陣

(14)

非線性方程組(10)的雅克比矩陣展開形式如下：

判斷誤差值是否滿足預設精度要求

β(k+1)-β(k)<ε

(15)

若式(15)不滿足則返回式(13)循環計算直至誤差值小于預設值輸出模型實際位姿。

在點位數據計算量大的情況下為避免求導的運算可以用差商近似替代微商[10]：

(16)

通過Gauss-Newton法求解坐標變換方程組，最終得出分離體質心的坐標。

3 實際標定測量

六自由度機構出廠實際標定時，使用法如(FARO)公司的激光跟蹤儀進行測量。通過激光跟蹤儀建立原點在模型零位時的轉動中心以及與地面坐標系平行的固定坐標系后開始測量。首先讓控制電機回到零位，測量此時3個靶球位的坐標。然后再控制電機運動使模型運動到不同的位姿，記錄當前位姿下3個靶球位的坐標值，將每一組位姿下的值與零位值代入算法求解。求解算法使用Gauss-Newton法，初值選取為零向量，為避免初值與真實值相差太大而影響迭代求解，姿態角較大的情況下初值可以選為機構的驅動位姿。事實上，根據該項目試驗機構的設計指標以及風洞試驗時的實際情況，外掛物的姿態角都不會很大，一次完整的捕獲軌跡試驗結束后外掛物的3個姿態角最大值均在10°以內[1]。

表1 靶球坐標數據(固定坐標系)

表2中的驅動位姿為在機構控制系統的輸入值依據CTS機構的理論尺寸，驅動裝置控制模型運動到的理想位置和姿態。測量位姿為在機構控制系統輸入理論位姿并驅動模型運動后，用激光跟蹤儀測量得到的靶球坐標(表1)，再通過本文提供的非線性最小二乘法計算得到實際的位置和姿態。試驗結果表明理論位姿與實際位姿相差不大，可以進行后續誤差修正補償工作。

表2 模型驅動位姿與測量位姿

4 結束語

本文基于風洞捕獲軌跡裝置的標定，通過激光跟蹤儀測量出點的坐標，按照剛體平動與轉動的坐標變換關系建立超定方程組，轉化為了一個非線性最小二乘問題的求解，從而為捕獲軌跡試驗中的下一步的標定與誤差修正提供數據支持。后續試驗根據該方法測出的數據進行了機構誤差補償，從已經得出的修正情況來看，該測量方法是可行的。至于誤差如何修正以及修正補償的具體結果非本文考慮的問題，具體的修正方法另文再述。