控端隔離機體擾動，達到目標姿態(tài)并保持在穩(wěn)定狀態(tài)。
[0078] 然后，根據(jù)控制被控端的補償角速度、固定端與被控端的轉(zhuǎn)動慣量比以及固定端 當前狀態(tài)，計算獲取控制被控端的有效機械能，該有效機械能為：控制云臺電機軸轉(zhuǎn)動使被 控端達到目標姿態(tài)并保持在穩(wěn)定狀態(tài)時，電機傳出的有效機械能。
[0079] 步驟S30、根據(jù)所述有效機械能，控制所述被控端保持在穩(wěn)定狀態(tài)。
[0080] 在獲取有效機械能后，根據(jù)獲取的有效機械能，計算獲得控制被控端的目標電流。 由于是通過控制云臺各電機軸轉(zhuǎn)動，來控制被控端在俯仰、橫滾和航向三個方向的轉(zhuǎn)動，因 此，控制被控端的目標電流也即控制云臺電機軸的目標電流；
[0081] 然后根據(jù)獲取的目標電流，控制云臺各電機軸轉(zhuǎn)動，從而控制被控端達到目標姿 態(tài)并保持在穩(wěn)定狀態(tài)。
[0082] 在本實施例中，通過獲取的云臺上被控端當前狀態(tài)和目標狀態(tài)、固定端當前狀態(tài)， 以及固定端與被控端的轉(zhuǎn)動慣量比，計算得到控制被控端的有效機械能，根據(jù)該有效機械 能控制所述被控端，使被控端達到目標姿態(tài)并保持在穩(wěn)定狀態(tài)，有效隔離機體擾動，實現(xiàn)了 對云臺的精確控制，達到了云臺自穩(wěn)控制效果，保障了云臺負載相機的成像質(zhì)量。
[0083] 進一步的，參照圖2,本發(fā)明第二實施例提供一種云臺控制方法，基于圖1所示的 實施例，所述步驟SlO包括：
[0084] 步驟S11、獲取被控端的角速度和姿態(tài)四元數(shù)。
[0085] 在云臺啟動后，可以通過傳感器獲取被控端的角速度和姿態(tài)四元數(shù)，也即云臺負 載相機的當前角速度和姿態(tài)四元數(shù)；其中，所獲取的姿態(tài)四元數(shù)是由實數(shù)加上三個虛數(shù)單 位i、j、k組成，i、j、k為相互正交的單位矢量；姿態(tài)四元數(shù)可以表征被控端在三維空間中 的旋轉(zhuǎn)。由此，所獲取的姿態(tài)四元數(shù)包含了被控端的姿態(tài)信息。
[0086] 上述被控端的角速度和姿態(tài)四元數(shù)是使用被控端的機體坐標系b作為基準得到 的。
[0087] 其中，機體坐標系b為空間直角坐標系，包括三個坐標軸，令X軸為俯仰軸，對應(yīng)云 臺的俯仰電機軸，云臺的俯仰電機軸轉(zhuǎn)動，控制被控端繞俯仰軸進行俯仰轉(zhuǎn)動；y軸為橫滾 軸，對應(yīng)云臺的橫滾電機軸，云臺的橫滾電機軸轉(zhuǎn)動，控制被控端繞橫滾軸進行橫滾轉(zhuǎn)動；z 軸為航向軸，對應(yīng)云臺的航向電機軸，云臺的航向電機軸轉(zhuǎn)動，控制被控端繞航向軸進行航 向轉(zhuǎn)動。
[0088] 以下詳細闡述使用被控端的機體坐標系b作為基準得到被控端的角速度和姿態(tài) 四元數(shù)的具體過程：
[0089] 作為一種實施例，通過傳感器獲取被控端的角速度為Ω :
[0090]
其中，ω ^ x為被控端在俯仰軸上的角速度，,為被控端在橫滾 軸上的角速度，ω ' z為被控端在航向軸上的角速度。
[0091] 為提高被控端當前姿態(tài)的精度，采用四元數(shù)乘法的近似算法計算得到被控端當前 的姿態(tài)四元數(shù)，被控端的姿態(tài)四元數(shù)的計算過程如下：
[0092] 任意選取姿態(tài)四元數(shù)滿足模為1，記錄為Q。作為初始值，在前一時刻的姿態(tài)四元 數(shù)的基礎(chǔ)上進行下述計算：首先，通過傳感器獲取的被控端上一時刻姿態(tài)四元數(shù)為Qn 1;其 中，Qn iX i+q< 2χ j+V 3Xk，i、j、k為相互正交的單位矢量；
[0093] 獲取被控端的重力加速度：G。= 2 (q< 3-qr0 · q' 2)τ；
[0094] 計算當前傳感器的校準值e為：
[0095] 然后，計算得到被控端當前的姿態(tài)四元數(shù)Qn，計算公式為：
[0096] Qn= Qn !+Qn !X (Ω+e);其中，Qn= qo+q! i+q2X j+q3Xk，i、j、k 為相互正交的單 位矢量。
[0097] 由于傳感器的校準值e的不斷修正，在有限步后，姿態(tài)四元素會收斂到準確值，即 可獲得被控端的姿態(tài)四元數(shù)Qn，也即該收斂后的準確值為姿態(tài)四元數(shù)Qn。
[0098] 步驟S12、根據(jù)所述被控端的姿態(tài)四元數(shù)，獲取被控端的旋轉(zhuǎn)矩陣。
[0099] 在獲取被控端的姿態(tài)四元數(shù)％后，根據(jù)Qn，計算被控端的姿態(tài)角由機體坐標系b轉(zhuǎn) 為大地坐標系R的轉(zhuǎn)換矩陣Cf,汁算公式為：
[0101] 將Qn中的q。、Ql、q2、q3，代入上述公式計算，得到被控端的旋轉(zhuǎn)矩陣記：
[0103] 步驟S13、根據(jù)所述被控端的旋轉(zhuǎn)矩陣，獲取被控端的歐拉角，得到被控端當前姿 〇
[0104] 在獲取被控端的旋轉(zhuǎn)矩陣Cf后，根據(jù)Cf計算被控端的歐拉角；被控端的歐拉角 包括：俯仰角Θ、橫滾角γ和航向角Φ;計算公式為：
[0106] 將Cf的值，代入上述公式計算，獲得俯仰角Θ、橫滾角γ和航向角
[0107] 由此，根據(jù)獲取的被控端的角速度和歐拉角，得到被控端當前狀態(tài)。
[0108] 被控端當前狀態(tài)包括：被控端的角速度Ω和被控端的歐拉角；其中，被控端的歐 拉角包括：俯仰角θ、橫滾角γ和航向角
[0109] 步驟S14、獲取被控端的目標姿態(tài)角和目標角速度，作為被控端目標狀態(tài)。
[0110] 被控端的目標姿態(tài)角可直接獲取。被控端的目標姿態(tài)角可以是預(yù)設(shè)的目標姿態(tài) 角，也可以是用戶調(diào)節(jié)的目標姿態(tài)角，可根據(jù)實際需要靈活設(shè)置。被控端的目標姿態(tài)角包 括：目標俯仰角Θ'、目標橫滾角γ'和目標航向角φ、
[0111] 被控端的目標角速度可直接獲取。由于在被控端達到目標姿態(tài)后，若要保持在穩(wěn) 定狀態(tài)，此時角速度應(yīng)該為0,因此，在本實施例中，令被控端的目標角速度ω t為預(yù)設(shè)的固 定值：〇。
[0112] 由此，獲取預(yù)先設(shè)置的被控端目標狀態(tài)。所獲取的被控端目標狀態(tài)包括：被控端 的目標角速度和被控端的目標姿態(tài)角；其中，被控端的目標姿態(tài)角包括：目標俯仰角 θ '、目標橫滾角γ'和目標航向角以及目標角速度《t。
[0113] 步驟S15、獲取固定端的角速度和姿態(tài)角，作為固定端當前狀態(tài)。
[0114] 固定端的角速度cob可以通過傳感器獲取，得到
[0115] 固定端的旋轉(zhuǎn)角度可以通過傳感器獲取，得到的固定端的姿態(tài)角為：橫滾角Θ y 俯仰角ΘΡ，航向角Gy。固定端的姿態(tài)角也即飛行器的姿態(tài)角。
[0116] 由此，得到固定端當前狀態(tài)。所得到的固定端當前狀態(tài)包括：固定端的角速度cob和固定端的姿態(tài)角；其中，固定端的姿態(tài)角包括：橫滾角Θ P俯仰角θρ，航向角9y。
[0117] 步驟S16、獲取固定端與被控端的轉(zhuǎn)動慣量比。
[0118] 固定端與被控端的轉(zhuǎn)動慣量比為預(yù)設(shè)系數(shù)，可直接獲取，得到固定端和被控端在 云臺電機軸上的轉(zhuǎn)動慣量比k。得到的轉(zhuǎn)動慣量比包括：固定端和被控端在俯仰電機軸上 的轉(zhuǎn)動慣量比kp，固定端和被控端在橫滾電機軸上的轉(zhuǎn)動慣量比k，固定端和被控端在航 向電機軸上的轉(zhuǎn)動慣量比ky。
[0119] 需要說明的是，得到的轉(zhuǎn)動慣量比為大量理論計算或?qū)嶒灁?shù)據(jù)所得，可根據(jù)實際 需要進行數(shù)值調(diào)整。
[0120] 在本實施例中，根據(jù)獲取的被控端角速度和姿態(tài)四元數(shù)，獲取被控端的旋轉(zhuǎn)矩陣， 從而獲取所述被控端的歐拉角，所得到被控端當前姿態(tài)精度高；獲取被控端目標狀態(tài)、固定 端當前狀態(tài)、固定端與被控端的轉(zhuǎn)動慣量比。本實施例中所獲取的各項數(shù)據(jù)精度高，為后續(xù) 控制被控端有效機械能的計算提供了精確的數(shù)據(jù)基礎(chǔ)。
[0121] 進一步的，參照圖3,本發(fā)明第三實施例提供一種云臺控制方法，基于上述圖1或 圖2所示的實施例（本實施例以圖1為例），所述步驟S20包括：
[0122] 步驟S21、根據(jù)所述被控端當前狀態(tài)和目標狀態(tài)，以及固定端當前狀態(tài)，獲取控制 被控端的補償角速度。
[0123] 在機體擾動時，固定端會在云臺電機軸上產(chǎn)生響應(yīng)的干擾角速度，為隔離機體擾 動，達到目標姿態(tài)，需控制云臺電機軸轉(zhuǎn)動對被控端產(chǎn)生補償角速度，以使被控端達到目標 姿態(tài)并保持狀態(tài)穩(wěn)定。
[0124] 因此，在獲取被控端當前狀態(tài)和目標狀態(tài)，固定端當前狀態(tài)，以及固定端與被控端 的轉(zhuǎn)動慣量比之后，首先，計算獲取控制被控端的補償角速度，該補償角速度為：云臺電機 軸轉(zhuǎn)動時所給予被控端的補償角速度，使被控端隔離機體擾動，能夠達到目標姿態(tài)并保持 穩(wěn)定狀態(tài)。
[0125] 具體的，作為一種實施方式，首先，根據(jù)固定端當前的角速度：在俯仰電機軸上的 角速度《piteh，在橫滾電機軸上的角速度ωΜ?1，和在航向電機軸上的角速度coyaw。
[0126] 根據(jù)被控端當前姿態(tài)角和目標姿態(tài)角，計算獲取姿態(tài)角差值：俯仰角差值Δ Θ， 橫滾角差值A(chǔ) γ和航向角差值麵&
[0127] 然后，根據(jù)被控端當前姿態(tài)角、固定端在云臺三個電機軸上的角速度，以及姿態(tài)角 差值，獲取控制被控端的補償角速度；控制被控端的補償角速度包括：在俯仰電機軸上的 補償角速度ωρ，在橫滾電機軸上的補償角速度ωρ和在航向電機軸上的補償角速度coy。
[0128] 步驟S22、根據(jù)所述控制被控端的補償角速度、固定端與被控端的轉(zhuǎn)動慣量比以及 固定端當前狀態(tài)，計算獲取控制被控端的有效機械能。
[0129] 在獲取控制被控端的補償角速度后，根據(jù)控制被控端的補償角速度、固定端與被 控端的轉(zhuǎn)動慣量比以及固定端當前狀態(tài)，計算獲取控制被控端的有效機械能，該有效機械 能為：控制云臺電機軸轉(zhuǎn)動，使被控端達到目標姿態(tài)并保持在穩(wěn)定狀態(tài)時，電機傳出的有效 機械能。
[0130] 具體的，作為一種實施方式，可以根據(jù)轉(zhuǎn)動慣量守恒和功能關(guān)系，列出方程組，計 算控制被控端的有效機械能。由于是通過控制云臺的各電機軸轉(zhuǎn)動來控制被控端，因此，所 得到的控制被控端的有效機械能也即控制云臺各電機軸的有效機械能。
[0131] 首先，獲取的固定端與被控端的轉(zhuǎn)動慣量比為：k，即三軸云臺各軸上的k可表示 為：k = kp，kr，ky;
[0132] -次控制后的云臺各電機定子的角速度為cos，在后續(xù)的公式中可將其消掉；
[0133] 獲取的云臺各電機軸上的補償角速度為：，即三軸云臺各軸上的ω?？杀硎?為：ω。= ω ρ，ωΓ，coy;
[0134] 根據(jù)固定端當前姿態(tài)獲取的固定端在云臺各電機軸上的角速度為：即三軸 云臺各軸上的表示為ω s ^ pitch? ^ roll ? ^ yaw?
[0135] 然后，令控制云臺各電機軸的有效機械能為E，即三軸云臺各軸上的E可表示為：E =Ep, Er, Ey；
[0136] 根據(jù)轉(zhuǎn)動慣量守恒和功能關(guān)系，則有方程組：
[0138] 其中，cot為獲取的被控端目標角速度，由于被控端到達目標姿態(tài)并保持穩(wěn)定時， 被控端目標速度為零，因此，被控端在云臺各電機軸上的目標角速度ω ^勺值均為〇。
[0139] 然后，求解上述方程組，得到：
[0141] 更為具體的：當計算控制俯仰電機軸的有效機械能時，令 ^ s ^ pitch? ^ c ^ p? k = kp;此時，根據(jù)上述方程組計算得到控制俯仰電機軸的有效機械能E p，能夠使被控端在 俯仰軸方向達到目標姿態(tài)并保持穩(wěn)定狀態(tài)；
[0142] 當計算控制橫滾電機軸的有效機械能時，令Ws= ω roll，ω。= ω = I;此時， 根據(jù)上述方程組計算得到控制橫滾電機軸的有效機械能Ep能夠使被控端在橫滾軸方向達 到目標姿態(tài)并保持穩(wěn)定狀態(tài)；
[0143] 當計算控制航向電機軸的有效機械能時，令Ws= ω yaw，ω。= ω y，k = ky;此時， 根據(jù)上述方程組計算得到控制航向電機軸的有效機械能Ey，能夠使被控端在航向軸方向達