一種三軸穩定伺服平臺的姿態解算及補償方法
【技術領域】
[0001 ]本發明屬于伺服穩定平臺控制領域，涉及姿態解算和補償方法，具體地說，是一種 三軸穩定伺服平臺的姿態解算及補償方法。
【背景技術】
[0002] 機載光電平臺是無人機上用于跟蹤和捕獲目標的平臺，具有機動靈活、適應性強、 實時準確等特點。作為無人機的重要任務設備，平臺上通常搭載有可見光、紅外和激光等適 用于不同環境的任務載荷，是跟蹤過程中實現視軸指向變化和視軸陀螺穩定的執行機構， 使得視軸在飛機姿態發生變化時保持穩定，同時可以進行圖象數據的實時壓縮，通過無線 傳輸將信號返回地面。因此對于三軸穩定平臺的設計與控制就尤為重要了。
【發明內容】
[0003] 為了克服現有技術的缺點，本發明提供一種三軸穩定伺服平臺的姿態解算及補償 方法。它結構簡單，機動靈活，適應性強，實時準確。
[0004] 本發明解決其技術問題所采取的技術方案是:包括如下步驟：
[0005] 步驟一、定義無人機姿態角、負載框架角和負載姿態角；
[0006] 步驟二、建立坐標變換矩陣；
[0007] 步驟三、計算負載姿態角；
[0008] 步驟四、穩定伺服平臺的補償方法；
[0009] 步驟五、通過控制系統，調整穩定伺服平臺，保持天線穩定。
[0010] 本發明結構簡單，機動靈活，適應性強，實時準確。
【附圖說明】
[0011]下面結合附圖和實施例對本發明進一步說明。
[0012] 圖1為本發明無人機坐標系示意圖；
[0013] 圖2為本發明三軸穩定平臺示意圖；
[0014]圖3為本發明負載坐標系不意圖；
[0015]圖4為本發明設備連接方塊圖；
[0016]圖5為本發明慣導輸出角曲線圖；
[0017] 圖6為本發明負載框架角曲線圖；
[0018] 圖7為本發明負載姿態角曲線圖。
【具體實施方式】
[0019] 本發明包括下列步驟：
[0020] 步驟一、定義無人機姿態角、負載框架角和負載姿態角：
[0021] 無人機姿態角，即無人機與大地坐標系的夾角。包括無人機航向角、無人機縱搖角 和無人機橫搖角，無人機坐標系如圖1所示，具體定義如下：
[0022] 無人機航向角：從真北方向開始順時針到無人機機首方向的夾角，在水平面內測 量，順時針為正，通過慣導系統測量，用Φ表示，范圍〇°~360°。
[0023] 無人機縱搖角:從無人機首尾線(縱軸0Y)與水平面的夾角，在鉛垂面內測量，機首 在水平面上方為正，通過慣導系統測量，用Θ表示，范圍-90°~90°。
[0024] 無人機橫搖角:為無人機繞首尾線(縱軸0Y)旋轉的角度，即無人機橫剖面(Χ0Ζ)和 水平面的交線與無人機橫軸(0Y)之間的夾角，測量平面垂直于無人機機翼平面且垂直于首 尾線，右翼下傾為正，通過慣導系統測量，用r表示，范圍-180°~180°。
[0025] 負載框架角包括方位框架角、滾動框架角和俯仰框架角，三軸穩定平臺示意圖如 圖2所示，具體定義如下：
[0026]方位框架角:在三軸穩定平臺中，外框為方位框，繞方位軸旋轉，順時針為正，由旋 變測量得到，用A表示，范圍-170°~170°。
[0027]滾動框架角:在三軸穩定平臺中，中框為滾動框，繞滾動軸旋轉，右弦向下為正，由 旋變測量得到，用R表示，范圍-8°~8°。
[0028]俯仰框架角:在三軸穩定平臺中，內框為俯仰框，繞俯仰軸旋轉，抬頭為正，由旋變 測量得到，用E表示，范圍-8°~-75°。
[0029] 負載姿態角，即視軸與大地坐標系的夾角。包括負載方位姿態角、負載滾動姿態角 和負載俯仰姿態角，負載坐標系如圖3所示，具體定義如下：
[0030] 負載方位角:正北與視軸指向在水平面投影線的夾角，在水平面內測量，順時針為 正，用α表示，范圍0°~360°。
[0031] 負載俯仰角:為視軸指向與其在水平面投影線的夾角，在鉛垂面內測量，波束指向 在水平面之上為正，用β表示，范圍-90°~90°。
[0032] 負載滾動角：為繞視軸指向旋轉的角度，即視軸橫剖面和水平面的交線與視軸橫 軸(0Χ)之間的夾角，測量平面垂直于橫軸平面(Χ0Ζ)且垂直于首尾線，右翼下傾為正，用γ 表示，范圍-180°~180°。
[0033] 步驟二、建立坐標變換矩陣：
[0034] 負載框架角為負載坐標系與無人機坐標系的夾角，因此負載坐標系到無人機坐標 系的方位軸、橫滾軸和俯仰軸轉換矩陣分別為：
[0035]
[0036]由從內到外順序，可得轉換矩陣r1:
[0037]
[0038] 同理，按滾動、俯仰、航向的順序，可得到無人機坐標系到大地坐標系的轉換矩陣 r1：
[0039]
[0040] 步驟三、計算負載姿態角：
[0041] 視軸與負載坐標系的Y軸重合，可以取單位向量[Ο,Ι,ΟΥ，經坐標變換，可以得到 在大地坐標系下的坐標：
[0042]
[0043] 由姿態角定義，負載的方位姿態角和俯仰姿態角分別為：
[0044] a = arctan(xid/yid)
[0045] 0 = arcsin(zid)
[0046] 視軸不能表征滾動姿態的變化，因此取垂直于波束的單位向量[0，0，1 ]7和[1，0， 0 ]'，經坐標變換，得到在大地坐標系下的坐標：
[0047]
[0048] 可以得到負載的滾動角：
[0049] γ =-arctan(z2d/z3d)
[0050] 步驟四、穩定伺服平臺的補償方法：
[0051 ]大地坐標系到無人機坐標系變換，負載坐標系至無人機坐標系變換。兩邊求等式， 進而求解補償的方位框架角、俯仰框架角、滾動框架角。
[0052] (1)滾動框架角：
[0053] -sinR = T3i*cos( γ )cos(-c〇+T32*cos( γ )sin(-c〇_T33sin( γ )
[0054] 其中α、γ為步驟二所求得的負載方位角與滾動角，T31、T32和T33為步驟二中T(與Γ 1 互逆)的元素。
[0055]可以求解出三軸穩定平臺的滾動框架角為R，則中框的補償角度為R