[發明專利]一種基于六自由度運動平臺的電子穩定系統的設計方法

有效

• 申請號: 202110643923.9
• 申請日: 2021-06-09
• 公開（公告）號: CN113377048B
• 公開（公告）日: 2022-08-16
• 發明（設計）人: 李雨凡;司一凡;周函玉;張勐飛;尤延鋮;朱劍鋒
• 申請（專利權）人: 廈門大學
• 主分類號: G05B19/042
• 分類號: G05B19/042;G05D1/08
• 代理公司: 廈門南強之路專利事務所(普通合伙) 35200
• 代理人: 馬應森

1.一種基于六自由度運動平臺的電子穩定系統的設計方法，其特征在于包括以下步驟：

1)通過陀螺儀和加速度傳感器獲得載體的姿態信息；所述陀螺儀采用三軸MEMS陀螺儀，用于采集運載體軸向角速率傳輸數學平臺進行姿態運算得出姿態角，分別是航向角Ψ、橫滾角γ、俯仰角θ；所述加速度傳感器采用三軸加速度計，用于分別采集載體3個軸向的加速度；通過陀螺儀和加速度傳感器獲得載體的姿態信息的具體步驟為：

陀螺儀所測得3個軸向在導航坐標系相對與地理坐標系下角速度矢量ω_tb＝[ψ，γ，θ]^T，角速度在導航坐標系下的投影為：

其中，分別表示x、y、z軸方向角速度；

代入四元素法中進行坐標變換，經過一次旋轉直接將導航坐標系轉變到地理坐標系下；四元素Q＝λ+p₁i+p₂j+p₃k描述的是載體的b系相對于t系的角位置，利用α轉角的三角函數得出規范化的四元素為：

得到姿態角：

基于加速度計進行姿態解算，對于靜止狀態下物體，其加速度計測量得到的三軸加速度數據a_x，a_y，a_z，其矢量和為當地的重力加速度，根據歐拉角的關系，利用即求得物體的姿態角

2)將姿態信息轉化為電信號通過集成電路傳輸，具體步驟為：

加速度計采集到信號，加以陀螺儀獲取的姿態信息，經由RS232接口傳送到導航計算機；運用MATLAB進行編程，利用四元素法將載體坐標的信息轉換到地理坐標后，將信號進行低通濾波處理，獲取到實際所需的信號：

加速計測量出的豎直方向的原始信號減去低通濾波法計算出趨勢項曲線，所得到去除趨勢項后的曲線再經過平均值法去除直流，既去除由溫度引起的趨勢項以及直流量，又避免在后期二次積分后發生數值失真和畸變現象；

采集的加速度信號轉變為速度或者位移時需要進行一次或二次積分，將積分得出的結果用滑動平均算法擬合出一條趨勢線，然后用結果減去所擬合的趨勢項從而得到平滑穩定的數據：t＝1，2…，其中，f(x)為實際測得的數據；T0是滑動長度；t代表的是采集信號個數；

假設外界角速率為常值，考慮系統機械正交耦合并忽略量級較小的離心力，三軸MEMS陀螺儀的運動方程為：

式中：U_x，U_γ，U_z表示3個軸向的控制力，C_xx，C_yy，C_zz為三軸等效阻尼項，K_xx，K_yy，K_zz為三軸等效剛度項；假設機械耦合導致對稱的阻尼項C_xy，C_xz，C_yz和剛度項K_xx，K_yy，K_zz；Ω_x，Ω_y，Ω_z是3個軸向的外界角速率，得到三軸MEMS陀螺儀的無量綱運動方程為：

向量形式為：其中，

控制目標變為：設計合適的控制律讓陀螺3個軸向跟蹤不同頻率的正弦信號，同時實時預估并補償閉環系統的不匹配干擾；

3)基于計算機PCI總線以DSP+FPGA架構建立六自由度運動控制系統；所述六自由度運動控制系統的硬件部分包括：核心處理器DSP模塊、協處理器FPGA模塊、與PC機接口的PCI模塊、外圍接口模塊；所述核心處理器DSP模塊包括DSP電源、基于鎖相環的時鐘、JTAG仿真接口和外部接口；所述協處理器FPGA模塊包括FPGA電源和配置電路；所述與PC機接口的PCI模塊包括PCI9052接口芯片、PCI9052數據傳輸電路以及PCI模塊；所述外圍接口模塊包括軸脈沖輸出電路、編碼器差分輸入電路和通用數字IO信號電路；

PCI9052接口芯片作為橋芯片用于對復雜的PCI總線協議進行轉換，實現PCI局部總線與PCI用戶設備之間的連接；協處理器FPGA模塊用于接收EP1K50接口芯片局部總線端傳遞過來的指令和數據，經過內部邏輯處理后發送給DSP芯片TMS320F28335進行運動控制算法處理，同時將有用信息反饋給PCI局部總線，并且控制外圍接口模塊；DSP芯片TMS320F28335負責處理各種運動控制算法；運動控制系統用于控制步進電機和全數字伺服電機，根據上位機的命令產生脈沖序列，利用所產生的脈沖個數、頻率以及頻率變化率分別控制電機的位置、速度及加速度指令；

4)建立線性誤差模型，進行誤差矯正；所述誤差矯正先建立線性誤差模型，根據載體坐標系下的輸出和IMU傳感器的輸出擬合計算各項誤差系數，利用誤差系數將傳感器坐標系下的原始數據標定為較精確的載體坐標系下的數據，具體步驟為：

(1)三軸陀螺儀的誤差通常包括零偏誤差、靈敏度誤差、非正交誤差、隨機漂移；將三軸MEMS陀螺儀的誤差模型簡化為式：

ω＝(ω₁ ω₂ ω₃)^T為載體角速度的準確值，為陀螺儀測得的角速度；三軸陀螺儀的各種誤差均包含于矩陣K＝(k_ij)_3×3與矢量b＝(b₁ b₂ b₃)^T中；利用叉積法標定陀螺儀，通過叉積標定法的積分形式：

其中，μ為參考系中某個固定不變的矢量；

將陀螺儀的各誤差系數完全分離出來，由于傳感器誤差模型是線性的，故利用最小二乘法擬合該模型求取各項誤差系數，結合上式，即求出標定后的傳感器數據；

(2)在ACEX 1K系列FPGA內部設計能夠使運動控制誤差減小的模塊，包括精插補模塊、編碼器反饋模塊、計數器模塊、通用數字I0接口模塊和雙口RAM模塊；并在運動平臺上安裝光柵尺，用于檢測平臺的實際位置，并反饋給運動控制系統；運動控制系統根據反饋信號隨時調整發給電動機驅動器的信號，其傳動機構、運動平臺和負載產生的各種誤差都即時反饋到運動控制卡中，得到補償使運動平臺的誤差始終控制在精度范圍以內；

5)六自由度座椅的核心機械結構的設計，所述六自由度座椅的核心機械結構采用“六連桿作動”的機械結構，由座椅平臺、固定平臺、鉸鏈組件以及電動缸、伺服電機組成，座椅平臺隨電動缸運動，固定平臺通過螺栓固連在地面，起支撐固定作用，座椅平臺和固定平臺與6組電動缸的上下兩端鉸鏈組件相連接，鉸接點均勻分布在大小不同的圓上，構成六邊形；座椅平臺與電動缸連桿通過兩兩相連的鉸鏈組件構成6個萬向節鉸接點，固定平臺與電動缸通過兩兩相連的鉸鏈組件構成6個萬向節鉸接點，連桿與電動缸構成6個移動副。