TW202246804A - 基於四元數卡爾曼濾波器的載體姿態控制系統及其控制方法 - Google Patents
基於四元數卡爾曼濾波器的載體姿態控制系統及其控制方法 Download PDFInfo
- Publication number
- TW202246804A TW202246804A TW110119095A TW110119095A TW202246804A TW 202246804 A TW202246804 A TW 202246804A TW 110119095 A TW110119095 A TW 110119095A TW 110119095 A TW110119095 A TW 110119095A TW 202246804 A TW202246804 A TW 202246804A
- Authority
- TW
- Taiwan
- Prior art keywords
- attitude
- carrier
- torque
- processor
- prediction result
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 110
- 230000007613 environmental effect Effects 0.000 claims abstract description 80
- 238000004364 calculation method Methods 0.000 claims abstract description 20
- 238000005259 measurement Methods 0.000 claims description 102
- 230000008859 change Effects 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 8
- 238000004422 calculation algorithm Methods 0.000 abstract description 14
- 239000011159 matrix material Substances 0.000 description 17
- 230000001133 acceleration Effects 0.000 description 9
- 230000008569 process Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 7
- 238000006243 chemical reaction Methods 0.000 description 3
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 238000000691 measurement method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 108091092878 Microsatellite Proteins 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013277 forecasting method Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/244—Spacecraft control systems
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/244—Spacecraft control systems
- B64G1/245—Attitude control algorithms for spacecraft attitude control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/36—Guiding or controlling apparatus, e.g. for attitude control using sensors, e.g. sun-sensors, horizon sensors
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/26—Guiding or controlling apparatus, e.g. for attitude control using jets
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/26—Guiding or controlling apparatus, e.g. for attitude control using jets
- B64G1/262—Guiding or controlling apparatus, e.g. for attitude control using jets having adjustable angles, e.g. gimbaled thrusters
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/28—Guiding or controlling apparatus, e.g. for attitude control using inertia or gyro effect
- B64G1/283—Guiding or controlling apparatus, e.g. for attitude control using inertia or gyro effect using reaction wheels
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/28—Guiding or controlling apparatus, e.g. for attitude control using inertia or gyro effect
- B64G1/286—Guiding or controlling apparatus, e.g. for attitude control using inertia or gyro effect using control momentum gyroscopes (CMGs)
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/32—Guiding or controlling apparatus, e.g. for attitude control using earth's magnetic field
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/36—Guiding or controlling apparatus, e.g. for attitude control using sensors, e.g. sun-sensors, horizon sensors
- B64G1/361—Guiding or controlling apparatus, e.g. for attitude control using sensors, e.g. sun-sensors, horizon sensors using star sensors
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/36—Guiding or controlling apparatus, e.g. for attitude control using sensors, e.g. sun-sensors, horizon sensors
- B64G1/366—Guiding or controlling apparatus, e.g. for attitude control using sensors, e.g. sun-sensors, horizon sensors using magnetometers
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B64—AIRCRAFT; AVIATION; COSMONAUTICS
- B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
- B64G1/00—Cosmonautic vehicles
- B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
- B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
- B64G1/38—Guiding or controlling apparatus, e.g. for attitude control damping of oscillations, e.g. nutation dampers
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Health & Medical Sciences (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
Abstract
本案揭露一種基於四元數卡爾曼濾波器的載體姿態判定與控制系統及其方法,其主要包含:具有四元數卡爾曼濾波器的處理器、至少一姿態致動器、及至少二個不同的環境感測器。首先,藉由系統動力學計算得到第一姿態預測結果。接著,系統至少測量兩個單獨的目標並更新第一姿態預測結果。再接著,根據由使用者提供的姿態命令執行為了補償衛星姿態的計算以獲得轉矩控制命令並輸入姿態致動器。藉由調整系統的動力學方程式及系列演算法,使得姿態致動器及環境感測器的數量可以被自由的調整,而無需為了執行在姿態致動器或環境感測器上有新配置的新任務而重新編寫程式。此外,如果一些組件失效,該演算法能夠不參考這些新配置而自行調節,進而避免整體的失效。
Description
本案是關於一種基於四元數卡爾曼濾波器的載體姿態判定與控制系統及其方法,特別是關於一種可擴充環境感測器數量與姿態制動器數量、相容於球型馬達致動器、基於四元數卡爾曼濾波器的衛星姿態控制系統及所述衛星姿態控制系統所運用的判定方法與控制方法。
衛星通常會搭載具指向性需求的酬載,例如:天線、太陽能板、或攝像頭等。通常這些酬載是「固定」於衛星上、無法自行移動或轉動的,衛星與這些酬載會一同移動或一同轉動。舉例而言,為了將這些天線對準地面接收站、或將攝像頭對準東京鐵塔,衛星需要進行姿態校準,姿態校準包含以下步驟:(1)判定衛星於t時間的姿態,及(2)於t+1時間控制及調整衛星姿態至特定姿態,以將所述天線或攝像頭調整轉至特定指向。而所謂的姿態,是一個慣性座標系(Inertial reference frame,或稱「慣性參考系」,例如:地球上加附的一套座標系)與一個衛星上的座標之間的旋轉關係,通常以四元數(Quaternion)表示。
慣性座標系可以是以直角座標系、圓柱座標系、或球型座標系等所表述的地球慣性座標系(Earth-Centered, Inertial,簡稱「ECI」)、或地心地固坐標系(Earth-Centered, Earth-Fixed,簡稱「ECEF」)等。以直角座標系為例,定義上通常會 選定一個原點、X軸的方向與Y軸的方向,並透過右手定則指出Z軸的方向。
為了得出姿態,可以使用以下兩種方法:(1)使用姿態量測裝置或(2)使用計算機根據空間中環境參考點進行計算。
使用姿態量測裝置可以直接地得出姿態,姿態量測裝置,例如:星象追蹤儀(Star Tracker,或稱「星體追蹤儀」、「星象儀」等)。
使用計算機根據「環境感測器對空間中環境參考點測量的數據」進行計算。環境感測器,例如:太陽感測器(Sun Sensor)、磁力計(Magnetometer)。空間中環境參考標的,例如:太陽光或地球磁場等。
為了確實地判定、控制調整衛星的姿態,還需要確認衛星內部的角動量交換元件(如動量輪、控制力矩陀螺等,即間接式姿態致動元件)於t時間所儲存的角動量。舉例而言,即使同一衛星與同一慣性座標系之間有相同的姿態關係,要進行姿態校準所需提供的力矩會因為衛星內部的角動量交換元件於t時間所儲存的角動量而有差異,所以需要以角動量交換元件上的轉速感測器,搭配已知的轉子轉動慣量,來確認衛星內部間接式姿態制動器於t時間的所儲存的角動量。
習用衛星搭載的判定及控制系統僅支援「一種」寫定的姿態量測方法,即要使用的環境感測器之種類與數目、姿態量測元件之使用與否、與慣性量測元件之使用與否,都無法隨使用者需求任意調整,而需要在使用之姿態感測器有更動之後,重新寫過姿態判定演算法。
除此之外,習用衛星搭載的判定及控制系統必須依照每次任務所配置的不同姿態制動器,來重新編寫演算法,無法隨使用者需求任意調整。
因此,習用姿態控制系統無法擴充環境感測器的數量、自由加入/移除慣性量測元件(如陀螺儀)或姿態量測元件(如追星儀),也無法自由加入/移除不同類型的直接式姿態致動器、間接式姿態致動器或前述二者的組合,導致每次依客戶需求與預算去配置姿態感測元件、直接式姿態致動器、間接式姿態致動器之後,都需要重新撰寫姿態判定之演算法,而造成交期延長與客戶之不便;或者衛星在地球軌道上運行時,若有其中幾個姿態感測元件陸續失效,無法自動更新演算法,而會造成誤判。
本案提出一種基於四元數卡爾曼濾波器的載體姿態控制系統及其控制方法,所述載體姿態控制系統主要是「利用至少二個環境感測器對空間中至少二個『不同的』環境參考點測量的數據」進行計算,以獲得對衛星更準確的姿態判定、控制調整。同時本案提出所述載體姿態控制系統所使用的演算法,包含:載體姿態判定方法及載體姿態控制方法。所述「利用至少二個環境感測器對空間中至少二個環境參考點測量的數據進行計算」的計算過程包含:至少一次的「預測」及至少一次以修正該預測所產生的結果為目的之「更新」,預測的過程以下簡稱為「預測階段」,另一方面,更新的過程以下簡稱為「更新階段」。所述載體姿態判定方法及所述載體姿態控制方法包含所述載體姿態判定方法,相較於所述載體姿態判定方法,所述載體姿態判定方法至少更進一步增加了「控制」,而控制的過程以下簡稱為「控制階段」。
所述載體姿態控制系統所包含的元件是用以一次的「預測」及至少一次「更新」,其中預測的方法(即得到第一姿態預測結果的方法)並不只有一種,本案提出六種預測的方法,但不限於只有這六種預測的方法。舉凡利用本案精神所修飾而完成的預測的方法均應被包含於本案申請專利範圍。根據預測方法的不同,載體姿態控制系統可能使用不同的元件或元件組合,例如:(1)當預測方法需要使用到慣性數據時,載體姿態控制系統會更進一步包含慣性量測元件,可以使用慣性量測元件與處理器的組合,或是(2)當預測方法直接地使用到姿態資料時,可以使用姿態量測元件直接地得到姿態資料。慣性量測元件可例如但不限於:陀螺儀。姿態量測元件可例如但不限於:星象追蹤儀(Star Tracker,或稱「星體追蹤儀」、「星象儀」等)。
本案之一實施例提出一種基於四元數卡爾曼濾波的載體姿態控制系統,係用以控制所述載體之姿態,包含:一控制器、一姿態致動器、一第1乃至第N環境狀態感測元件、一處理器。所述控制器,係用以接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令。所述姿態致動器,係用以根據一轉矩控制命令輸出轉矩以改變所述載體的姿態,可以藉由以一轉矩感測器對姿態致動器進行測量,以得到的一輸出轉矩測量值。第1乃至第N環境狀態感測元件,係用以量測一第1乃至第m被測目標,以得到t時間的一第1乃至第N量測值,其中N及m皆為大於或等於2的整數,且m是小於或等於N的整數。所述處理器,具有一四元數卡爾曼濾波器(Quaternion Kalman Filter,簡稱QKF),所述處理器連接於所述控制器、所述姿態致動器及所述第1乃至第N環境狀態感測元件,所述處理器係用以:根據所述轉矩輸入命令及所述輸出轉矩測量值至少一者計算,以得到一第一姿態預測結果及根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果。
在一些實施例中,所述之載體姿態控制系統,更包含:一慣性量測元件及一姿態量測元件至少一者。所述慣性量測元件係用以測量所述載體的慣性,以獲得一慣性數據。所述姿態量測元件係用以測量所述載體的姿態,以獲得一姿態資料。
其中,當所述載體姿態控制系統係包含所述「慣性量測元件」時,所述處理器更連接於所述慣性量測元件;所述處理器係用以:根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「慣性數據」計算,以得到所述第一姿態預測結果。
又其中,當所述載體姿態控制系統係包含所述「姿態量測元件」時,所述處理器更連接於所述姿態量測元件;所述處理器係用以:根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「姿態資料」計算,以得到所述第一姿態預測結果。
在上述實施例中,無論所述載體姿態控制系統係包含所述「姿態量測元件」或是包含所述「姿態量測元件」,所述處理器在得到第一姿態預測結果之後,更係用以:根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果。
本案之另一實施例提出一種基於四元數卡爾曼濾波的載體姿態判定方法,係用以預測該載體之姿態,包含:一預測階段及一更新階段。所述預測階段,包含:利用一控制器接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令;利用具有一四元數卡爾曼濾波器(QKF)的一處理器根據「來自一控制器的一轉矩輸入命令」及「以一轉矩感測器對姿態致動器進行測量,以得到的一輸出轉矩測量值」至少一者計算,以得到一第一姿態預測結果。所述更新階段包含:利用一第1乃至第N環境狀態感測元件量測一第1乃至第m被測目標,以得到一第1乃至第N量測值,其中N及m皆為大於或等於2的整數,且m是小於或等於N的整數;及利用所述處理器根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果。
在一些實施例中,所述預測階段更包含:「利用一慣性量測元件測量所述載體的慣性,以獲得一慣性數據」及「利用一姿態量測元件測量所述載體的姿態,以獲得一姿態資料」至少一者。當所述預測階段更包含「利用一慣性量測元件測量所述載體的慣性,以獲得一慣性數據」時,所述處理器係根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「慣性數據」計算,以得到一第一姿態預測結果。當所述預測階段更包含「利用一姿態量測元件測量所述載體的姿態,以獲得一姿態資料」時,所述處理器係根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「姿態資料」,以得到一第一姿態預測結果。
本案之另一實施例提出一種基於四元數卡爾曼濾波的載體姿態控制方法,係用以控制所述載體之姿態,包含:一預測階段、一更新階段及一控制階段。所述預測階段,包含:利用一控制器接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令;利用具有一四元數卡爾曼濾波器(QKF)的一處理器根據「來自一控制器的一轉矩輸入命令」及「以一轉矩感測器對姿態致動器進行測量,以得到的一輸出轉矩測量值」至少一者計算,以得到一第一姿態預測結果;更新階段,包含:利用一第1乃至第N環境狀態感測元件量測一第1乃至第m被測目標,以得到一第1乃至第N量測值,其中N及m皆為大於或等於2的整數,且m是小於或等於N的整數;及利用所述處理器根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果。所述控制階段,包含:利用所述處理器根據所述「姿態需求」與「第二姿態預測結果」進行以修正為目的計算,以得到一轉矩控制命令;利用所述致動器根據所述轉矩控制命令對所述載體輸出轉矩,以改變所述載體的姿態。
在一些實施例中,所述預測階段更包含:「利用一慣性量測元件測量所述載體的慣性,以獲得一慣性數據」及「利用一姿態量測元件測量所述載體的姿態,以獲得一姿態資料」至少一者。當所述預測階段更包含「利用一慣性量測元件測量所述載體的慣性,以獲得一慣性數據」時,所述處理器係根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「慣性數據」計算,以得到一第一姿態預測結果。當所預測階段更包含「利用一姿態量測元件測量所述載體的姿態,以獲得一姿態資料」時,所述處理器係根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「姿態資料」,以得到一第一姿態預測結果。
在一些實施例中,所述姿態致動器是一直接式姿態致動器。
在一些實施例中,所述姿態致動器更包含:至少一直接式姿態致動器及至少一間接式姿態致動器。
在一些實施例中,所述姿態控制器包含一間接式姿態致動器,所述間接式姿態致動器是球型動量輪馬達,且所述姿態控制器具有至少三種控制模式:衛星減滾(De-tumbling)、衛星消除角動量飽和(De-Saturation)及衛星指向控制。
以下是藉由各具體實施例來說明本案所揭露有關「載體姿態控制系統」的實施方式,其中相同的元件將以相同的參照符號加以說明。
需要說明的是,本案各具體實施例中所有方向性用語(例如:上、下、左、右、前、後等)僅是被用於解釋某一特定狀態(如所附的各圖式所示)中各部件之間的相對位置關係、運動情況或姿態等,如果所述特定狀態發生改變時,則方向性用語也會隨之改變。
各具體實施例的說明伴隨併入且組成說明書之一部分的圖式,說明本揭露之實施例,然而本揭露並不受限於該實施例。此外,以下的實施例可以被適當地修飾或整合而完成另一實施例。
以下針對每一個「一實施例」、「一具體實施例」、「較佳實施例」、「其他實施例」、「另一實施例」等或任何與「實施例」或「實施方式」相關的說明,皆僅係用於描述所述實施例可包含之技術特徵、結構或是特性,然而並非其它實施例皆必須包含該特定特徵、結構或是特性。此外,重複使用「在本實施例中」或「在一些實施例中」等近似用語並非必須指相同實施例,也可以是指不同的實施例。
首先需要說明的是,在現有的技術中並未將四元數卡爾曼濾波器應用於衛星姿態控制系統,且在傳統的四元數卡爾曼濾波器架構下無法為衛星姿態控制系統擴充多個環境感測元件,且現有的技術中也不支援致動器。
本案載體姿態控制系統的技術特徵主要在於將四元數卡爾曼濾波器應用於載體姿態控制系統。載體是指衛星、微型衛星、無人機等。本案在載體姿態控制系統中內建能夠完成數次預測及更新的疊代(iteration,或稱「迭代」)的演算法。本案技術特徵主要在於將四元數卡爾曼濾波器應用於載體姿態控制系統,四元數卡爾曼濾波器的詳細運作過程可以參考 D. Choukroun, L. Y. Bar-Itzhack, and Y. Oshman. (2005) Novel Quaternion Kalman Filter. Published on IEEE Transactions on Aerospace and Electronic Systems Vol.42, No.1(簡稱為「文獻1」),本案通過對載體姿態控制系統內建的演算法進行改良,進而完成可擴充多個環境感測元件(或稱環境傳感器)的架構。卡爾曼濾波器(Kalman filter)是一種高效率的遞歸濾波器(自回歸濾波器),它能夠從一系列的不完全及包含雜訊的測量中,估計動態系統的狀態。此外,特別地本案載體姿態控制系統之一目的在於能夠。本案載體姿態控制系統內建的演算法包含了三個部分:初始化(Initialize,以下簡稱「Part A」)、預測(Propagate,以下簡稱「Part B」)及更新(Update,以下簡稱「Part C」)。
為了方便說明初始狀態以及形式上的對比,在此將方程式改寫為以k=0為初始狀態,亦即假設k+1時刻的真實狀態是從k時刻的狀態演化而來,因此方程式被改寫為
。為了方便說明本案的一些實施例,將卡爾曼濾波器應用於衛星姿態時,需要先定義幾個參數如下:
(1)「
」是誤差共變矩陣(Initial estimation error covariance matrix),而「
」是初始估計的誤差共變矩陣(Initial estimation error covariance matrix);
(2)「
」是「姿態的四元數」(Attitude quaternion),而「
」是「初始姿態的四元數」(Initial attitude quaternion);
(3)「
」是「漂移估計」(Drift estimations),而「
」是「初始漂移估計」(Initial drift estimations);
(4)「
」是「空間狀態」(state space),而「
」是「初始的空間狀態」(Initial state space)。
以下依序說明Part A、Part B及Part C的計算過程。
[關於Part A-初始化(Initialize)]
首先,在一些實施例中,關於衛星的姿態的四元數(即
)與時刻k之間的關係以及衛星的空間狀態(
)、姿態的四元數(
)、漂移估計(
)與時刻k三者之間的關係可以表示為下方所示的方程式Eq. A-1及Eq. A-2:
(Eq. A-1) | |
(Eq. A-2) |
本案利用至少兩個環境感測元件及三位組方法(Triad Method)訂定載體最初的姿態,並將該姿態輸入四元數卡爾曼濾波器中做為k=0時的空間狀態及k=0時的誤差共變矩陣,k=0時的空間狀態及k=0時的誤差共變矩陣二者的方程式如下所示:
(Eq. A-1’) | |
(Eq. A-2’) |
其中,根據改寫後的算式,在本案的演算法中,只有在第一個迭代時(即符合「k=0」時)需要操作到Part A。完成Part A之後,按照輸入進類比數位轉換器(Analog-to-Digital Converters,簡稱ADCS)的命令速率(command rate,簡稱CMD Rate)進行數次Part B 與Part C的循環(以下簡稱為「預測與更新循環」)。本案提供的演算法,理論上可以支援無限支環境姿態感測器,意即無限次「更新」、即Part C的循環。
不過,需要特別說明的是,按照載體姿態控制系統中被安裝且實際參與工作的環境感測元件之數量,「1次」的預測與更新循環,可能包含:1次預測及1次更新、1次預測及2次更新、或1次預測及3次更新等。具體而言,當載體姿態控制系統中有N個被安裝且實際參與工作的環境感測元件(N為任意正整數),則「1次」的預測與更新循環中會包含1次預測及N次更新。
[關於Part B-預測(Propagate)]
在說明本案實施例的Part B預測之前,在此先說明本案實施例的衛星系統動態方程式、及預測的前置作業,也就是求取方程式Eq. B-1至Eq. B-2的衛星角加速度(
),而衛星角加速度也等同可以用衛星角速度的一階導數(
)作表示,在求取
後接著進行卡爾曼濾波器的預測。
在衛星控制系統中利用統御方程式(governing equation,也稱控制方程式)來計算、表現角動量與轉矩之間的關係。如下方方程式Eq. B0-1所示,在本案實施例中,統御方程式考量直接式致動器、間接式致動器與環境擾動(disturbance torque)。順帶一提,方程式Eq. B0-1’及Eq. B0-1’’與方程式Eq. B0-1完全相同,項次的挪移僅是為了方便說明
及表現角動量守恆原理於本案實施例中的應用。
其中,方程式Eq. B0-1的各參數說明如下:
(1)在方程式Eq. B0-1中,H表示角動量,τ則表示轉矩,其中
表示角動量對時間微分得到的一階導函數;
(2)方程式Eq. B0-1右式第一項(即
)表示「直接式」致動器所輸出的轉矩;
(3)方程式Eq. B0-1左式第二項(即
)表示藉由操縱轉子角度、速度、加減速以達到轉矩輸出之目的「間接式」致動器的輸出轉矩;
(4)
是角動量對時間的一階導數,如方程式Eq. B0-1’所示,
同時也表示直接式致動器、間接式致動器及的環境擾動三者對衛星產生的轉矩總和;
(5)
表示太空中所存在的各種、會對衛星造成轉矩的環境擾動,具體包含了由太陽輻射壓所產生的力矩(solar radiation pressure torque)、重力梯度扭矩(gravity-gradient torque)、磁偶極矩(magnetic dipole moment torque)、氣動扭矩(aerodynamic torque)等。不過,在大多數的情況下,這些環境擾動對致動器輸出的影響很小且難以預期,所以在本案實施例中,在卡爾曼濾波器的Part B0將環境擾動預設為0(即令
)以利於後續方程式Eq. B0-6的計算。然而,這些環境擾動對衛星系統動態的影響,仍會在四元數卡爾曼濾波器(QKF)中以程序雜訊(Process noise)的方式被考慮。環境擾動對衛星系統動態的影響便可在姿態控制系統的參數設定階段被考慮進來。
(Eq. B0-1) | |
(Eq. B0-1’) | |
(Eq. B0-1’’) |
請再參考下方方程式Eq. B0-4至Eq. B0-5。方程式Eq. B0-4至Eq. B0-5是Eq. B0-1中重要的幾項展開後的結果,展開的方法可以參考 GEORGE W. HOUSNER, DONALD E. HUDSON. (1991) Applied Mechanics Dynamics(簡稱為文獻2)第202至203頁7.8 The General Equations of Motion for a Rigid Body的部分。在本案實施例中,模型全面地闡述任何一種間接式致動器的系統動態,包含球型馬達(Spherical motor,例如:本發明人的中華民國證書號第I719585號「馬達及其驅動方法」發明專利所揭露的馬達)、單框架(single-gimbal)控制力矩陀螺、雙框架(dual-gimbal)控制力矩陀螺、反作用輪(reaction wheel)等。甚至,轉子與衛星本身的慣性積(product of inertia,簡稱POI)也被完整地考量進系統模型中了。
(Eq. B0-2) | |
(Eq. B0-3) | |
(Eq. B0-4) | |
(Eq. B0-5) |
本案以下各實施例的載體姿態控制系統,可以直接地或間接地經由測量、推導及/或計算得到轉子的轉速
、轉子的角加速度
、轉子的慣性張量(inertia tensor
)、衛星的角速度(
)、及衛星的慣性張量(inertia tensor
)。因此,能夠藉由上述方程式解得
如下方程式Eq. B0-6。
(Eq. B0-6) |
根據方程式Eq. B0-1至Eq. B0-5可知,本案載體姿態控制系統可以任意擴充直接式姿態致動器與間接式致動器的數量。甚至,即便間接式致動器有多種樣態,但因為都是建立在角動量交換的原理(角動量守恆原理,如方程式Eq. B0-1’’)上,因此不同樣態的間接式致動器皆可適用於本案載體姿態控制系統。
在取得衛星角速度對時間的一階導數
之後,開始進行卡爾曼濾波器的預測。角度變化量估計值(delta angle estimation,
)、三軸角速度(
)、角加速度(
)、時間變化量(
)之間的關係可以通過下方所示的方程式Eq. B-1作表示:
(Eq. B-1) |
接著,將這組校正後角度變化量估計值(
),帶入下方所示的方程式Eq. B-4至Eq. B-6做計算,得到一個預測下、k+1時刻的空間狀態(
),同時空間狀態
也是後續任一實施例中所指的「第一姿態預測結果」。其中「
」是指
函數(
function),詳細內容可以參考文獻1的第176頁。
(Eq. B-4) | |
(Eq. B-5) | |
(Eq. B-6) |
再接著,為了校正誤差共變矩陣(covariance matrix)。再次地將前述校正後角度變化量估計值(
),帶入下方所示的方程式Eq. B-7至Eq. B-13做計算,以得到校正後的誤差共變矩陣(covariance matrix,
)。
(Eq. B-7) | |
(Eq. B-8) | |
(Eq. B-9) | |
(Eq. B-10) | |
(Eq. B-11) | |
(Eq. B-12) | |
(Eq. B-13) |
其中,
是取自空間狀態
的前四項、
則是取自共變異數矩陣
左上方4*4的數字。其中,
是電子雜訊的標準差(Standard deviation of Electronic noise);
是轉矩雜訊的標準差(Standard deviation of float torque noise);
是輸出漂移率的標準差(Standard deviation of the output drift rate);「E」是指E函數(E function),詳細內容同樣可以參考文獻1。得到經漂移量校正的角度變化量估計值及經漂移量校正後的誤差共變矩陣,即完成Part B預測的部分,接著可以進入Part C更新的部分。
[關於Part C-更新(Update)]
經過上述Part A、Part B後,可以得出k+1時刻的空間狀態
及環境感測向量對
。其中,如前所述,載體姿態控制系統中有N個被安裝且實際參與工作的環境感測元件(N為任意正整數),i代表環境感測器的編號,可從0至n(n為任意正整數)。「n」或「N」除了代表衛星上環境感測器的數量,另一方面,「n」或「N」也代表著演算法中「更新」的次數。
在Part C中,利用四元數(quaternion)的特性求取H函數(H function)當此H函數乘上一個能正確地把
映射(mapping)到
的姿態值時 (quaternion),二者的乘積會等於0。
(Eq. C-1) | |
(Eq. C-2) | |
(Eq. C-3) | |
(Eq. C-4) |
接著,利用此H函數與在預測階段(Part B)時所得到的k+1時刻的數值,包含:空間狀態
、共變異數矩陣
、環境感測向量對
為環境感測元件量測值,)等,及利用下方所示的方程式Eq. C-5至Eq. C-9計算卡爾曼增益(Kalman Gain)。其中,
則是取自共變異數矩陣
左上方4*4的數字,而共變異數矩陣
是定義自文獻1中第179頁的更新用環境感測量測值的共變異數矩陣模型:
。
(Eq. C-5) | |
(Eq. C-6) | |
(Eq. C-7) | |
(Eq. C-8) | |
(Eq. C-9) |
為了方便說明,在此將上述方程式中的參數符號與後續實施例中的用語整理為以下表格:
參數符號 | 後續實施例中相關用語 |
k | 時間(控制器接收使用者對所述載體的一姿態需求) |
k+1 | 時間(姿態致動器輸出轉矩的時間) |
「輸出轉矩量測值」或「轉矩輸入命令的扭矩值」 | |
第一姿態預測結果 | |
環境感測元件量測值 | |
N | 載體姿態控制系統中有N個被安裝且實際參與工作的環境感測元件 |
i | 環境感測器的編號(可從0至n,n為任意正整數) |
n | 更新的次數 |
第二姿態預測結果 |
接著,以下開始說明本案之具體實施例。
[第一具體實施例]
請參考圖1。圖1及圖2是本案之第一實施例載體姿態控制系統E1的示意圖。載體姿態控制系統是基於四元數卡爾曼濾波器41運作,基於四元數卡爾曼濾波器41的載體姿態控制系統,係用以控制所述載體之姿態,包含一控制器1、一姿態致動器2、一第1乃至第N環境狀態感測元件、一處理器4。所述控制器1,係用以接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令。所述姿態致動器2,係用以根據一轉矩控制命令輸出轉矩以改變所述載體的姿態,可以藉由以一轉矩感測器對姿態致動器2進行測量,以得到的一輸出轉矩測量值。所述第1乃至第N環境狀態感測元件,係用以量測一第1乃至第m被測目標,以得到t時間的一第1乃至第N量測值,其中N及m皆為大於或等於2的整數,且m是小於或等於N的整數。所述處理器4,具有一四元數卡爾曼濾波器41(Quaternion Kalman Filter,簡稱QKF),所述處理器4連接於所述控制器1、所述姿態致動器2及所述第1乃至第N環境狀態感測元件,所述處理器4係用以:根據所述轉矩輸入命令及所述輸出轉矩測量值至少一者計算,以得到一第一姿態預測結果及根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果。並且,所述處理器4還係用以:根據所述「姿態需求」與「第二姿態預測結果」進行以修正為目的計算,以得到一轉矩控制命令,以及利用所述姿態致動器2根據所述轉矩控制命令對所述載體輸出轉矩,以改變所述載體的姿態。
在本實施例中,載體可例如但不限於是衛星或水上載具,以下是以所述載體姿態控制系統應用於衛星作說明。所述載體姿態控制系統係用以執行:(1)判定衛星於t時間的姿態;及(2)於t+1時間控制及調整衛星姿態至特定姿態。所述姿態需求係一特定姿態,可以透過四元數等進行表示。
如圖1及圖2所示,所述第1乃至第N環境狀態感測元件,係指第1環境狀態感測元件31乃至第N環境狀態感測元件3N。在圖1的狀況中,N等於2,因此載體姿態控制系統E11包含第1環境狀態感測元件31及第2環境狀態感測元件32。另外,在圖2的狀況中,N大於2,因此載體姿態控制系統E11包含第1環境狀態感測元件31、第2環境狀態感測元件32以及第N環境狀態感測元件3N。圖1至圖4表示本案所提供的載體姿態控制系統E11、E12、E13可以擴充N個環境狀態感測元件。
在本實施例中,為了得到載體於t時間的姿態,所述載體姿態控制系統是使用處理器4根據空間中環境參考點(以下簡稱為「被測目標」)的位置進行計算,所述空間中環境參考點,例如:太陽或地球。而判別被測目標位置的方法,可利用太陽光或地球磁場等。
與先前技術相較,特別值得一提的是,載體姿態控制系統的處理器4內建能夠至少依序地或同時地對「藉由二個環境狀態感測元件對二個『不同的』被測目標量測所得到的二個量測值(例如:太陽的位置與地球的位置)」進行計算或進行資料處理的演算法。因此,當載體姿態控制系統的處理器4連接於二個環境狀態感測元件的功能時,處理器4必須是連接於二個不同的環境狀態感測元件(例如:太陽感測器(Sun Sensor)與磁力計(Magnetometer))以對二個「不同的」被測目標(例如但不限於:太陽及地球磁場)進行量測。需要說明的是,在此對於被測目標數量上的計算,是以前述環境參考點的數量作計算,舉例而言,如果一個環境感測元件僅重複地對一個環境參考點(例如:地球等)的位置進行數次的量測,則仍然視為僅對一個被測目標進行量測,並非本案載體姿態控制系統的技術手段。
在此對於環境狀態感測元件數量上的計算,並不以外觀上的數量作計算,也不是以環境狀態感測元件能夠無損傷地拆解、分割得到最大數量作計算,也不是以環境狀態感測元件的種類或品牌進行計算,而是以能夠單獨有效地對被測目標進行量測的最小單元作計算。舉例而言,本案所提供的載體姿態控制系統也可能同時使用五個太陽感測器對太陽(第一被測目標)的位置進行量測,並使用一個地磁感測器對地球(第二被測目標)的位置進行量測。
在此環境狀態感測元件的數量是依據在預測階段S1與更新階段S2中,環境狀態感測元件至多能夠對多少個不同的被測目標進行有效測量為計算,舉例而言,當一環境狀態感測元件能夠依序地或同時地針對太陽的磁場及地球的磁場進行,則該技術中是應用了二個環境狀態感測元件,因為這個環境狀態感測元件在載體姿態判定方法中,依序地或同時地針對二個(太陽的磁場及地球的磁場)不同的被測目標進行有效測量為計算。
除此之外,如圖3所示,載體姿態控制系統可擴充二個以上的環境狀態感測元件,載體姿態控制系統具備能夠同時連接N個環境狀態感測元件的功能,在此分別命名為第1乃至第N環境狀態感測元件,能夠依序地或同時地量測一m個「不同的」被測目標,在此分別命名為第1乃至第m被測目標。舉例而言,載體姿態控制系統使用了六個環境狀態感測元件,其中五個環境狀態感測元件是太陽感測器(Sun Sensor)對應的被測目標應為太陽,其中一個環境狀態感測元件是磁力計(Magnetometer)對應的被測目標應為地球,則N等於6、m等於2。
在本實施例中,載體姿態控制系統具有至少二種得到一第二姿態預測結果的功能,但本案並不以此為限。載體姿態控制系統能夠有以下選擇:(1)根據所述轉矩輸入命令計算,以得到一第一姿態預測結果及根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果;或者是,(2)根據所述輸出轉矩測量值計算,以得到一第一姿態預測結果及根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果。
在一些實施例中,所述姿態致動器2是一直接式姿態致動器2,例如:磁力棒(Magnetorquers)或推進器(Thrusters)等。
在一些較佳實施例中,所述姿態致動器2更包含:至少一直接式姿態致動器2及至少一間接式姿態致動器2。間接式姿態致動器2可例如但不限於球型馬達(Spherical motor,例如:本發明人的中華民國第I719585號「馬達及其驅動方法」發明專利所揭露的馬達)、單框架(single-gimbal)控制力矩陀螺、雙框架(dual-gimbal)控制力矩陀螺、反作用輪(reaction wheel)等。當間接式姿態致動器2的轉速越高時,裡頭所具有的「角動量 (Angular Momentum,單位為kg‧m
2/s,定義為「L」)」越高,可以從角動量公式「L=Iω」中理解,其中I為「轉動慣量(Moment of inertia,單位為kg‧m
2)」;ω為「角速度(Angular velocity,rad/s)」。
其中,欲藉由球型馬達對產生載體產生「轉矩(Torque,單位N‧m,定義為「τ」)」,則球型馬達需要具有「角加速度(Angular acceleration,rad/s
2,定義為「α」)」,可以從轉矩公式「τ =I‧α= I
」中理解。然而,球型馬達有轉速的極限,不大可能無限制地加速。因此當馬達來到轉速極限,也就是角動量儲存的極限時,就自動進到消除動量飽和的模式,利用直接式的姿態致動元件產生一個反向轉矩,所述反向轉矩於方向上與間接式姿態致動元件產生的轉矩方向相反,來與球型馬達減速卸載時衍生的反向轉矩抵銷,進而完成衛星消除角動量飽和的目的。
基於上述原因,載體姿態控制系統中的姿態致動器2可以單獨採用直接式姿態致動器2,但不能夠單獨採用間接式姿態致動器2。
[第二具體實施例]
請參閱圖3。圖3是本案另一實施例載體姿態控制系統E2的示意圖。在本實施例中,所述之載體姿態控制系統E2,更包含一慣性量測元件5。所述慣性量測元件5係用以測量所述載體的慣性,以獲得一慣性數據。當所述載體姿態控制系統係包含所述「慣性量測元件5」時,所述處理器4更連接於所述慣性量測元件5;所述處理器4係用以:根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「慣性數據」計算,以得到所述第一姿態預測結果。也就是說,載體姿態控制系統E2的所述處理器4係用以根據所述「轉矩輸入命令」及所述「慣性數據」計算,以得到所述第一姿態預測結果。或者是,載體姿態控制系統E2的所述處理器4係用以根據所述「轉矩輸出測量值」及所述「慣性數據」計算,以得到所述第一姿態預測結果。
[第三具體實施例]
請參考圖4。圖4是本案另一實施例載體姿態控制系統E3的示意圖。在另一實施例中,所述載體姿態控制系統E3,更包含一姿態量測元件6至少一者。所述姿態量測元件6係用以測量所述載體的姿態,以獲得一姿態資料。當所述載體姿態控制系統係包含所述「姿態量測元件6」時,所述處理器4更連接於所述姿態量測元件6;所述處理器4係用以:根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「姿態資料」計算,以得到所述第一姿態預測結果。也就是說,載體姿態控制系統E3的所述處理器4係用以根據「轉矩輸入命令」及所述「姿態資料」計算,以得到所述第一姿態預測結果。或者是,載體姿態控制系統E3的所述處理器4係用以根據所述「轉矩輸出測量值」及所述「姿態資料」計算,以得到所述第一姿態預測結果。
[載體姿態判定及控制方法]
以下藉由第四至十具體實施例(姿態判定方法E21至E26)及第十一具體實施例載體姿態控制方法E31說明本案前揭載體姿態控制系統對於衛星姿態的判定及控制方法。在說明第四至的具體實施例(姿態判定方法E21至E26)及載體姿態控制方法E31的具體實施例之前,先說明各具體實施例之間的主要差異。
首先定義以下名詞(請一併參考前述公式或方程式Eq. A0-1至Eq. C-11及相關說明)。
(1)慣性數據,例如但不限於:轉子的轉速
、轉子的角加速度
、轉子的慣性張量(inertia tensor
)、衛星的角速度(
)、衛星的角加速度
及/或衛星的慣性張量(inertia tensor
)。
(3)轉矩輸入命令或輸出轉矩測量值,例如但不限於:
。簡言之,轉矩輸入命令是未經測量而由使用者或電腦計算提供的數位訊號,輸出轉矩測量值是經由實際量測而得的物理量對類比物理量(類比訊號)。
(4)姿態資料,例如但不限於:衛星的姿態。
(5)第一姿態預測結果,例如但不限於:
。
(6)第二姿態預測結果,例如但不限於:
。
請參閱圖5。圖5是本案一實施例載體姿態判定方法E21至E26與本案一實施例載體姿態控制方法E31的流程圖示意圖。需說明的是,相較於本案提供的載體姿態判定方法E21至E26,本案提供的載體姿態控制方法E31僅是增加了「控制階段S3」。
請參閱圖6及圖9。本案提供的載體姿態判定方法大致上可分為兩個類型(共六種):圖6顯示本案載體姿態判定方法的第一個類型,為載體姿態判定方法E21及E22,載體姿態判定方法E21及E22分別以第四具體實施例與第五具體實施例描述;另一方面,圖9顯示本案載體姿態判定方法的第二個類型,為載體姿態判定方法E23至E26,載體姿態判定方法E23至E26分別以第六具體實施例至第九具體實施例作為描述。
第二個類型相較於第一個類型差異在於:第二個類型的載體姿態判定方法E23至E26較第一個類型的載體姿態判定方法E21至E22更進一步包含步驟S12a「利用一慣性量測元件5測量所述載體的慣性,以獲得一慣性數據」及步驟S12b「利用一姿態量測元件6測量所述載體的姿態,以獲得一姿態資料」至少一者。
又第二個類型更可以被分類為載體姿態判定方法E23至E26,載體姿態判定方法E23至E26差異在於:「預測階段」中是利用具有一四元數卡爾曼濾波器41(QKF)的一處理器4根據「何者」進行計算而得到第一姿態預測結果,可以是根據:
(1)「來自一控制器1的一轉矩輸入命令」及所述「慣性數據」(例如步驟S13c);或
(2)「以一轉矩感測器對姿態致動器2進行測量,以得到的一輸出轉矩測量值」及所述「慣性數據」(例如步驟S13d);或
(3)「來自一控制器1的一轉矩輸入命令」及所述「姿態資料」(例如步驟S13e);或
(4)「以一轉矩感測器對姿態致動器2進行測量,以得到的一輸出轉矩測量值」及所述「姿態資料」(例如步驟S13f)
以得到第一姿態預測結果(步驟S15)。
以下藉由第四具體實施例至第九具體實施例依序揭示載體姿態判定方法E21至E26於「預測階段S1」的具體實施方式。
[第四具體實施例]
請參考圖7。圖7是本案另一實施例載體姿態判定方法E21中預測階段S1的流程圖。本案之另一實施例提出一種基於四元數卡爾曼濾波的載體姿態判定方法,係用以預測該載體之姿態,包含:一預測階段S1及一更新階段S2。所述預測階段S1,包含:
步驟S11:利用一控制器1接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令;
步驟S13a:利用具有一四元數卡爾曼濾波器41(QKF)的一處理器4根據「來自一控制器1的一轉矩輸入命令」計算;
步驟S15:以得到一第一姿態預測結果。
[第五具體實施例]
請參考圖8。圖8是本案另一實施例載體姿態判定方法E22中預測階段S1的流程圖。本案之另一實施例提出一種基於四元數卡爾曼濾波的載體姿態判定方法,係用以預測該載體之姿態,包含:一預測階段S1及一更新階段S2。所述預測階段S1,包含:
步驟S11:利用一控制器1接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令;
步驟S13b:利用具有一四元數卡爾曼濾波器41(QKF)的一處理器4根據「以一轉矩感測器對姿態致動器2進行測量,以得到的一輸出轉矩測量值」計算;
步驟S15:得到一第一姿態預測結果。
圖9是本案一實施例載體姿態判定方法E23、E24、E25及E26中預測階段S1的流程圖。如前所述,載體姿態判定方法E23至E26差異在於:「預測階段」中是利用具有一四元數卡爾曼濾波器41(QKF)的一處理器4根據「何者」進行計算而得到第一姿態預測結果,可以是根據:
(1)「來自一控制器1的一轉矩輸入命令」及所述「慣性數據」(例如步驟S13c);或
(2)「以一轉矩感測器對姿態致動器2進行測量,以得到的一輸出轉矩測量值」及所述「慣性數據」(例如步驟S13d);或
(3)「來自一控制器1的一轉矩輸入命令」及所述「姿態資料」(例如步驟S13e);或
(4)「以一轉矩感測器對姿態致動器2進行測量,以得到的一輸出轉矩測量值」及所述「姿態資料」(例如步驟S13f)
以得到第一姿態預測結果(步驟S15)。
[第六具體實施例]
請參考圖10。圖10是本案一實施例載體姿態判定方法E23中預測階段S1的流程圖。在姿態判定方法E23的預測階段S1中,更包含:
步驟S11:利用一控制器1接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令;
步驟S12a:「利用一慣性量測元件5測量所述載體的慣性,以獲得一慣性數據」;
步驟S13c:所述處理器4根據所述「轉矩輸入命令」及所述「慣性數據」計算;
步驟S15:得到一第一姿態預測結果。
[第七具體實施例]
請參考圖11。圖11是本案一實施例載體姿態判定方法E24中預測階段S1的流程圖。在載體姿態判定方法E24的預測階段S1中,更包含:
步驟S11:利用一控制器1接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令;
步驟S12a:「利用一慣性量測元件5測量所述載體的慣性,以獲得一慣性數據」;
步驟S13d:所述處理器4係根據所述「轉矩輸出測量值」及所述「慣性數據」計算;
步驟S15:得到一第一姿態預測結果。
[第八具體實施例]
請參考圖12。圖12是本案一實施例載體姿態判定方法E25中預測階段S1的流程圖。在載體姿態判定方法E25的預測階段S1中,更包含:
步驟S11:利用一控制器1接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令;
步驟S12b:「利用一姿態量測元件6測量所述載體的姿態,以獲得一姿態資料」;
步驟S13c:所述處理器4係根據所述「轉矩輸入命令」及所述「姿態資料」計算;
步驟S15:得到一第一姿態預測結果。
[第九具體實施例]
請參考圖13。圖13是本案一實施例載體姿態判定方法E26中預測階段S1的流程圖。在載體姿態判定方法E26的預測階段S1中,更包含:
步驟S11:利用一控制器1接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令;
步驟S12b:「利用一姿態量測元件6測量所述載體的姿態,以獲得一姿態資料」;
步驟S13f:所述處理器4係根據所述「轉矩輸出測量值」及「姿態資料」計算;
步驟S15:得到一第一姿態預測結果。
[第十具體實施例]
請參考圖14。圖14是本案一實施例載體姿態判定方法E21至E26中更新階段S2的流程圖。需要說明的是,雖然載體姿態判定方法E21至E26於預測階段S1有步驟的差異,然而,載體姿態判定方法E21至E26於得到第一姿態預測結果後,會進入相同的更新階段S2。更新階段S2,包含:
步驟S21:利用一第1乃至第N環境狀態感測元件量測一第1乃至第m被測目標,以得到一第1乃至第N量測值,其中N及m皆為大於或等於2的整數,且m是小於或等於N的整數;及
步驟S22:利用所述處理器4根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新;
步驟S23:得到一第二姿態預測結果。
[第十一具體實施例]
請一併參考圖5及圖15。圖15是本案一實施例載體姿態控制方法E31中控制階段S3的流程圖。如前所述,本案提供的載體姿態控制方法E31相較於載體姿態判定方法E21至E26僅是增加了「控制階段S3」。也就是說,載體姿態控制方法E31包含載體姿態判定方法E21至E26任一者。控制階段S3包含:
步驟S31:利用所述處理器4根據所述「姿態需求」與「第二姿態預測結果」進行以修正為目的的計算,以得到一轉矩控制命令;接著,
步驟S32:利用所述致動器根據所述轉矩控制命令對所述載體輸出轉矩,以改變所述載體的姿態。
簡言之,於上述段落中所描述的載體姿態控制方法E31可泛稱為「衛星指向控制模式」,載體姿態控制系統藉由反饋補償式的控制邏輯(即載體姿態控制方法E31)根據使用者對所述載體「姿態需求」,與第二姿態預測結果相比較,藉此得出該下達給姿態致動器的控制命令,以更加精準地完成使用者對所述載體「姿態需求」。
除此之外,在一些實施例中,載體姿態控制方法E31包含:至少三種控制模式:除了前述的衛星指向控制模式之外,更包含衛星減滾(De-tumbling)模式、及衛星消除角動量飽和(De-Saturation)模式。與衛星指向控制模式不同的是:衛星減滾(De-tumbling)模式、衛星消除角動量飽和(De-Saturation)模式目的並不是根據使用者對所述載體「姿態需求」執行,而是根據載體姿態控制系統內建的規則執行。
如前所述,利用間接式姿態致動器2對載體進行姿態改變的載體姿態控制系統,有動量飽和的問題。因此,在一些載體姿態控制系統中,載體姿態控制方法E31更包含:衛星消除角動量飽和(De-Saturation)模式。
另外,所謂減滾(De-tumbling)模式,係用於衛星剛從火箭被發射出來時、或衛星失控時會觸發的緊急模式。當衛星藉由陀螺儀量測到衛星本身的旋轉速度過快,會觸發減滾模式,相較於載體姿態控制方法E31,減滾模式係利用所述「直接式」姿態致動器2根據所述轉矩控制命令對所述載體輸出轉矩,以降低所述載體的滾動速度。
綜上所述,本案提供一種四元數卡爾曼濾波器的載體姿態控制系統及其控制方法,藉由將四元數卡爾曼濾波器應用於載體姿態控制系統,使得載體姿態控制系統達到「可擴充環境感測器」、「利用至少二個環境感測器對空間中至少二個『不同的』環境參考點測量的數據」、及「對第一預測結果進行至少一次『以修正該預測所產生的結果為目的』之『更新』」的功效,進而完成「姿態判定系統可擴充」之目的。
除此之外,本案一併提供同時使用間接式姿態致動器(例如球型馬達等)與直接式姿態致動器的載體姿態控制方法,以解決利用間接式姿態致動器進行姿態調整之姿態控制系統所存在的角動量飽和問題。
雖然上述所有的之實施例是以較詳細的方式揭示,然而,以上述所有的實施例皆是舉例性而非限制性者,所屬技術領域中具有通常知識者可以了解本案之各種修飾手段,並得以在不背離所附之申請專利範圍中進行。因此,在任何未脫離本案所提供的發明與創作精神與本案所明示與隱含的範疇中,對其進行的修改或變更者,皆應被包含於後附的申請專利範圍中。
1:控制器
2:姿態致動器
31:第1環境狀態感測元件
32:第2環境狀態感測元件
3N:第N環境狀態感測元件
4:處理器
41:四元數卡爾曼濾波器
5:慣性量測元件
6:姿態量測元件
E11、E12、E13:載體姿態控制系統
E21、E22、E23、E24、E25、E26:載體姿態判定方法
E31:載體姿態控制方法
S1:預測階段
S11、S12a、S12b、S13a、S13b、S13c、S13d、S13e、S13f、S15:步驟(預測階段的步驟)
S2:更新階段
S21、S22、S23:步驟(更新階段的步驟)
S3:控制階段
S31、S32:步驟(控制階段的步驟)
圖1是本案一實施例載體姿態控制系統E11的示意圖
圖2是本案另一實施例載體姿態控制系統E11的示意圖
圖3是本案另一實施例載體姿態控制系統E12的示意圖
圖4是本案另一實施例載體姿態控制系統E13的示意圖
圖5是本案一實施例載體姿態判定方法E21至E26與載體姿態控制方法E31的流程圖
圖6是本案另一實施例載體姿態判定方法E21及E22中預測階段的流程圖
圖7是本案另一實施例載體姿態判定方法E21中預測階段的流程圖
圖8是本案另一實施例載體姿態判定方法E22中預測階段的流程圖
圖9是本案一實施例載體姿態判定方法E23、E24、E25及E26中預測階段的流程圖
圖10是本案一實施例載體姿態判定方法E23中預測階段的流程圖
圖11是本案一實施例載體姿態判定方法E24中預測階段的流程圖
圖12是本案一實施例載體姿態判定方法E25中預測階段的流程圖
圖13是本案一實施例載體姿態判定方法E26中預測階段的流程圖
圖14是本案一實施例載體姿態判定方法E21至E26中更新階段的流程圖
圖15是本案一實施例載體姿態控制方法E31中控制階段的流程圖
1:控制器
2:姿態致動器
31:第1環境狀態感測元件
32:第2環境狀態感測元件
3N:第N環境狀態感測元件
4:處理器
41:四元數卡爾曼濾波器
E11:載體姿態控制系統
Claims (9)
- 一種基於四元數卡爾曼濾波的載體姿態控制系統,係用以控制所述載體之姿態,包含: 一控制器,係用以接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令; 一姿態致動器,係用以根據一轉矩控制命令輸出轉矩以改變所述載體的姿態,可以藉由以一轉矩感測器對姿態致動器進行測量,以得到一輸出轉矩測量值;及 一第1乃至第N環境狀態感測元件,係用以量測一第1乃至第m被測目標,以得到t時間的一第1乃至第N量測值,其中N及m皆為大於或等於2的整數,且m是小於或等於N的整數;及 一處理器,具有一四元數卡爾曼濾波器(QKF),所述處理器連接於所述控制器、所述姿態致動器及所述第1乃至第N環境狀態感測元件,所述處理器係用以: 根據所述轉矩輸入命令及所述輸出轉矩測量值至少一者計算,以得到一第一姿態預測結果;及 根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果。
- 如請求項1所述之載體姿態控制系統,更包含:一慣性量測元件及一姿態量測元件至少一者;所述慣性量測元件係用以測量所述載體的慣性,以獲得一慣性數據;所述姿態量測元件係用以測量所述載體的姿態,以獲得一姿態資料;其中, 當所述載體姿態控制系統係包含所述「慣性量測元件」時,所述處理器更連接於所述慣性量測元件;所述處理器係用以:根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「慣性數據」計算,以得到所述第一姿態預測結果;又 當所述載體姿態控制系統係包含所述「姿態量測元件」時,所述處理器更連接於所述姿態量測元件;所述處理器係用以:根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「姿態資料」計算,以得到所述第一姿態預測結果; 並且,所述處理器係用以: 根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果。
- 一種基於四元數卡爾曼濾波器的載體姿態判定方法,係用以判定該載體之姿態,包含: 預測階段,包含: 利用一控制器接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令; 利用具有一四元數卡爾曼濾波器(QKF)的一處理器根據「來自一控制器的一轉矩輸入命令」及「以一轉矩感測器對姿態致動器進行測量,以得到的一輸出轉矩測量值」至少一者計算,以得到一第一姿態預測結果;及 更新階段,包含: 利用一第1乃至第N環境狀態感測元件量測一第1乃至第m被測目標,以得到一第1乃至第N量測值,其中N及m皆為大於或等於2的整數,且m是小於或等於N的整數;及 利用所述處理器根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果。
- 如請求項3所述之載體姿態判定方法,其中所述預測階段更包含:「利用一慣性量測元件測量所述載體的慣性,以獲得一慣性數據」及「利用一姿態量測元件測量所述載體的姿態,以獲得一姿態資料」至少一者; 當所述預測階段更包含「利用一慣性量測元件測量所述載體的慣性,以獲得一慣性數據」時,所述處理器係根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「慣性數據」計算,以得到一第一姿態預測結果; 當所預測階段更包含「利用一姿態量測元件測量所述載體的姿態,以獲得一姿態資料」時,所述處理器係用以:根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「姿態資料」,以得到一第一姿態預測結果。
- 一種基於四元數卡爾曼濾波的載體姿態控制方法,係用以控制所述載體之姿態,包含: 預測階段,包含: 利用一控制器接收使用者對所述載體的一姿態需求並產生一轉矩輸入命令; 利用具有一四元數卡爾曼濾波器(QKF)的一處理器根據「來自一控制器的一轉矩輸入命令」及「以一轉矩感測器對姿態致動器進行測量,以得到的一輸出轉矩測量值」至少一者計算,以得到一第一姿態預測結果;及 更新階段,包含: 利用一第1乃至第N環境狀態感測元件量測一第1乃至第m被測目標,以得到一第1乃至第N量測值,其中N及m皆為大於或等於2的整數,且m是小於或等於N的整數;及 利用所述處理器根據所述第1乃至第N量測值計算,並對所述第一姿態預測結果進行更新,以得到一第二姿態預測結果;及 控制階段,包含: 利用所述處理器根據所述「姿態需求」與「第二姿態預測結果」進行以修正為目的計算,以得到一轉矩控制命令; 利用所述姿態致動器根據所述轉矩控制命令對所述載體輸出轉矩,以改變所述載體的姿態。
- 如請求項5所述之載體姿態控制方法,其中所述預測階段更包含:「利用一慣性量測元件測量所述載體的慣性,以獲得一慣性數據」及「利用一姿態量測元件測量所述載體的姿態,以獲得一姿態資料」至少一者; 當所述預測階段更包含「利用一慣性量測元件測量所述載體的慣性,以獲得一慣性數據」時,所述處理器係根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「慣性數據」計算,以得到一第一姿態預測結果; 當所預測階段更包含「利用一姿態量測元件測量所述載體的姿態,以獲得一姿態資料」時,所述處理器係根據所述「轉矩輸入命令及所述轉矩輸出測量值至少一者」及所述「姿態資料」,以得到一第一姿態預測結果。
- 如請求項1至2任一項所述之載體姿態控制系統、如請求項3至4任一項所述之載體姿態判定方法或如請求項5至6任一項所述之載體姿態控制方法,其中所述姿態致動器是一直接式姿態致動器。
- 如請求項1至2任一項所述之載體姿態控制系統、如請求項3至4任一項所述之載體姿態判定方法或如請求項5至6任一項所述之載體姿態控制方法,其中所述姿態致動器更包含:至少一直接式姿態致動器及至少一間接式姿態致動器。
- 如請求項1至2任一項所述之載體姿態控制系統或如請求項5至6任一項所述之載體姿態控制方法,其中所述姿態控制器包含一間接式姿態致動器,所述間接式姿態致動器是球型動量輪馬達,且所述姿態控制器具有至少三種控制模式:衛星減滾(De-tumbling)、衛星消除角動量飽和(De-Saturation)及衛星指向控制。
Priority Applications (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
TW110119095A TWI764735B (zh) | 2021-05-26 | 2021-05-26 | 基於四元數卡爾曼濾波器的載體姿態控制系統及其控制方法 |
US17/464,043 US20220388692A1 (en) | 2021-05-26 | 2021-09-01 | Attitude determination and control system based on a quaternion kalman filter and method thereof |
US17/545,766 US11554881B2 (en) | 2021-05-26 | 2021-12-08 | Attitude determination and control system and method thereof |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
TW110119095A TWI764735B (zh) | 2021-05-26 | 2021-05-26 | 基於四元數卡爾曼濾波器的載體姿態控制系統及其控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
TWI764735B TWI764735B (zh) | 2022-05-11 |
TW202246804A true TW202246804A (zh) | 2022-12-01 |
Family
ID=82594317
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
TW110119095A TWI764735B (zh) | 2021-05-26 | 2021-05-26 | 基於四元數卡爾曼濾波器的載體姿態控制系統及其控制方法 |
Country Status (2)
Country | Link |
---|---|
US (2) | US20220388692A1 (zh) |
TW (1) | TWI764735B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114919774B (zh) * | 2022-05-20 | 2024-06-14 | 南京航空航天大学 | 非接触载荷无扰卫星平台洛伦兹力执行器在轨标定方法 |
Family Cites Families (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6285927B1 (en) * | 1999-05-26 | 2001-09-04 | Hughes Electronics Corporation | Spacecraft attitude determination system and method |
US6477465B1 (en) * | 1999-11-29 | 2002-11-05 | American Gnc Corporation | Vehicle self-carried positioning method and system thereof |
US7193559B2 (en) * | 2003-01-21 | 2007-03-20 | Novatel, Inc. | Inertial GPS navigation system with modified kalman filter |
US7246775B1 (en) * | 2004-08-02 | 2007-07-24 | Lockheed Martin Corporation | System and method of substantially autonomous geosynchronous time-optimal orbit transfer |
US20110238308A1 (en) * | 2010-03-26 | 2011-09-29 | Isaac Thomas Miller | Pedal navigation using leo signals and body-mounted sensors |
US9194756B2 (en) * | 2010-07-14 | 2015-11-24 | University Of Florida Research Foundation, Inc. | System and method for assessing the performance of an attitude control system for small satellites |
TW201311214A (zh) * | 2011-09-14 | 2013-03-16 | Ming-Da Lin | 視覺化imu步態偵測裝置及其分析方法 |
US9038958B1 (en) * | 2012-05-29 | 2015-05-26 | United States Of America As Represented By The Secretary Of The Navy | Method and apparatus for contingency guidance of a CMG-actuated spacecraft |
US9475592B2 (en) * | 2013-01-31 | 2016-10-25 | Northrop Grumman Systems Corporation | Reaction sphere for stabilization and control in three axes |
JP2017511885A (ja) * | 2014-02-26 | 2017-04-27 | コーエン, クラーク, エマーソンCOHEN, Clark, Emerson | 性能及びコストが改善されたグローバルナビゲーション衛星システムアーキテクチャ |
TW201709025A (zh) * | 2015-08-26 | 2017-03-01 | 巨擘科技股份有限公司 | 整合位置、姿態與無線傳輸之裝置 |
CN113804191B (zh) * | 2016-11-17 | 2024-03-19 | 格兰菲智能科技有限公司 | 移动设备及求取移动设备姿态的方法 |
US10371530B2 (en) * | 2017-01-04 | 2019-08-06 | Qualcomm Incorporated | Systems and methods for using a global positioning system velocity in visual-inertial odometry |
SG11201811001WA (en) * | 2018-07-02 | 2020-02-27 | Beijing Didi Infinity Technology & Development Co Ltd | Vehicle navigation system using pose estimation based on point cloud |
TWI680382B (zh) * | 2018-10-19 | 2019-12-21 | 宏達國際電子股份有限公司 | 電子裝置及其姿態校正方法 |
US11124320B2 (en) * | 2019-02-12 | 2021-09-21 | Canadian Space Agency | Spacecraft control using residual dipole |
-
2021
- 2021-05-26 TW TW110119095A patent/TWI764735B/zh active
- 2021-09-01 US US17/464,043 patent/US20220388692A1/en not_active Abandoned
- 2021-12-08 US US17/545,766 patent/US11554881B2/en active Active
Also Published As
Publication number | Publication date |
---|---|
US20220388693A1 (en) | 2022-12-08 |
TWI764735B (zh) | 2022-05-11 |
US20220388692A1 (en) | 2022-12-08 |
US11554881B2 (en) | 2023-01-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110174899B (zh) | 一种基于敏捷卫星的高精度成像姿态指向控制方法 | |
CN104932510B (zh) | 一种针对飞轮低速摩擦的挠性航天器姿态控制系统与方法 | |
US9043177B2 (en) | Posture information calculation device, posture information calculation system, posture information calculation method, and information storage medium | |
JP4249902B2 (ja) | 衛星姿勢制御における特異点回避 | |
US9643740B2 (en) | Gyroless three-axis sun acquisition using sun sensor and unscented kalman filter | |
CN109030867B (zh) | 使用加速度传感器和地磁传感器计算角速度的方法和设备 | |
CN108548542B (zh) | 一种基于大气阻力加速度测量的近地轨道确定方法 | |
CN110567461B (zh) | 一种考虑无陀螺仪的非合作航天器姿态和参数估计方法 | |
JP2011042358A (ja) | パドル電流測定値フィードバックのみを使用したジャイロなし遷移軌道太陽捕捉 | |
JP2008189235A (ja) | 姿勢変更制御方法、姿勢変更制御システム、姿勢変更制御プログラムおよびプログラム記録媒体 | |
CN109632185B (zh) | 一种大惯量转动部件动不平衡特性在轨估计方法 | |
CN113361013B (zh) | 一种基于时间同步稳定的航天器姿态鲁棒控制方法 | |
CN110316402A (zh) | 一种编队控制模式下的卫星姿态控制方法 | |
JP3726884B2 (ja) | 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム | |
CN110803304B (zh) | 一种卫星姿态控制系统 | |
TWI764735B (zh) | 基於四元數卡爾曼濾波器的載體姿態控制系統及其控制方法 | |
CN110645976B (zh) | 一种移动机器人的姿态估计方法及终端设备 | |
CN108871323A (zh) | 一种低成本惯性传感器在机动环境下的高精度导航方法 | |
CN110597274B (zh) | 一种适应姿态重定向的sgcmg动态框架角速度确定方法 | |
CN110375773B (zh) | Mems惯导系统姿态初始化方法 | |
TW442425B (en) | State control device of moving body and its state control method | |
CN109781103B (zh) | 基于双超平台的姿态角速度测量方法及系统 | |
CN107870063B (zh) | 基于动量守恒的航天器转动惯量在轨测量方法 | |
KR101846019B1 (ko) | 비행체 위치 산출 장치 및 방법 | |
JPH11281300A (ja) | 慣性航法装置 |