態(tài)四元數(shù)；其中，所獲取的姿態(tài)四元數(shù)是由實(shí)數(shù)加上三個(gè)虛數(shù)單位 i、j、k組成，i、j、k為相互正交的單位矢量；姿態(tài)四元數(shù)可以表征被控端在三維空間中的旋 轉(zhuǎn)。由此，所獲取的姿態(tài)四元數(shù)包含了被控端的姿態(tài)信息。
[0207] 上述被控端的角速度和姿態(tài)四元數(shù)是使用被控端的機(jī)體坐標(biāo)系b作為基準(zhǔn)得到 的。
[0208] 其中，機(jī)體坐標(biāo)系b為空間直角坐標(biāo)系，包括三個(gè)坐標(biāo)軸，令X軸為俯仰軸，對(duì)應(yīng)云 臺(tái)的俯仰電機(jī)軸，云臺(tái)的俯仰電機(jī)軸轉(zhuǎn)動(dòng)，控制被控端繞俯仰軸進(jìn)行俯仰轉(zhuǎn)動(dòng)；y軸為橫滾 軸，對(duì)應(yīng)云臺(tái)的橫滾電機(jī)軸，云臺(tái)的橫滾電機(jī)軸轉(zhuǎn)動(dòng)，控制被控端繞橫滾軸進(jìn)行橫滾轉(zhuǎn)動(dòng)；z 軸為航向軸，對(duì)應(yīng)云臺(tái)的航向電機(jī)軸，云臺(tái)的航向電機(jī)軸轉(zhuǎn)動(dòng)，控制被控端繞航向軸進(jìn)行航 向轉(zhuǎn)動(dòng)。
[0209] 作為一種實(shí)施例，傳感單元110獲取被控端的角速度為Ω :
其中，ω ' x為被控端在俯仰軸上的角速度，ω ' 被控端在橫滾 軸上的角速度，ω ' ζ為被控端在航向軸上的角速度。
[0211] 為提高被控端當(dāng)前姿態(tài)的精度，傳感單元110采用四元數(shù)乘法的近似算法計(jì)算得 到被控端當(dāng)前的姿態(tài)四元數(shù)，被控端的姿態(tài)四元數(shù)的計(jì)算過(guò)程如下：
[0212] 任意選取姿態(tài)四元數(shù)滿(mǎn)足模為1，記錄為Q。作為初始值，在前一時(shí)刻的姿態(tài)四元數(shù) 的基礎(chǔ)上進(jìn)行下述計(jì)算：首先，傳感單元110獲取的被控端上一時(shí)刻姿態(tài)四元數(shù)為Qn 1;其 中，Qn iX i+q< 2χ j+V 3Xk，i、j、k為相互正交的單位矢量；
[0213] 獲取被控端的重力加速度：G。= 2 W 3-qr0 · q' 2)τ；
[0214] 計(jì)算當(dāng)前傳感器的校準(zhǔn)值e為：
[0215] 然后，計(jì)算得到被控端當(dāng)前的姿態(tài)四元數(shù)Qn，計(jì)算公式為：
[0216] Qn= Qn !+Qn !X (Ω+e);其中，Qn= qo+q! i+q2X j+q3Xk，i、j、k 為相互正交的單 位矢量。
[0217] 由于傳感器的校準(zhǔn)值e的不斷修正，在有限步后，姿態(tài)四元素會(huì)收斂到準(zhǔn)確值，即 傳感單元110獲得被控端姿態(tài)四元數(shù)Qn，也即該收斂后的準(zhǔn)確值為姿態(tài)四元數(shù)Qn。
[0218] 矩陣單元120,用于根據(jù)所述被控端的姿態(tài)四元數(shù)，獲取被控端的旋轉(zhuǎn)矩陣。
[0219] 在傳感單元110獲取被控端的姿態(tài)四元數(shù)％后，根據(jù)Q n，矩陣單元120計(jì)算被控 CN 105116926 A 說(shuō)明書(shū) 14/20 頁(yè) 端的姿態(tài)角由機(jī)體坐標(biāo)系b轉(zhuǎn)為大地坐標(biāo)系
R的轉(zhuǎn)換矩陣Cf，計(jì)算公式為：
[0221 ] 將Qn中的q。、qp q2、q3，代入上述公式計(jì)算，得到被控端的旋轉(zhuǎn)矩陣記：
[0223] 姿態(tài)單元130,用于根據(jù)所述被控端的旋轉(zhuǎn)矩陣，獲取被控端的歐拉角，得到被控 端當(dāng)前姿態(tài)。
[0224] 在矩陣單元120獲取被控端的旋轉(zhuǎn)矩陣Cf后，姿態(tài)單元130根據(jù)Cf計(jì)算被控端 的歐拉角；被控端的歐拉角包括：俯仰角Θ、橫滾角γ和航向角計(jì)算公式為：
[0226] 將€^的值，代入上述公式計(jì)算，獲得俯仰角Θ、橫滾角γ和航向角警；
[0227] 由此，獲取單元140根據(jù)獲取的被控端的角速度和歐拉角，得到被控端當(dāng)前狀態(tài)。
[0228] 被控端當(dāng)前狀態(tài)表征了被控端當(dāng)前所處于的狀態(tài)，包括：被控端的角速度Ω和被 控端的歐拉角；其中，被控端的歐拉角包括：俯仰角Θ、橫滾角γ和航向角φ。
[0229] 獲取單元140,用于獲取被控端的目標(biāo)姿態(tài)角和目標(biāo)角速度，作為被控端目標(biāo)狀 態(tài)；獲取固定端與被控端的轉(zhuǎn)動(dòng)慣量比。
[0230] 獲取單元140獲取被控端的目標(biāo)姿態(tài)角。被控端的目標(biāo)姿態(tài)角可以是預(yù)設(shè)的目標(biāo) 姿態(tài)角，也可以是用戶(hù)調(diào)節(jié)的目標(biāo)姿態(tài)角，可根據(jù)實(shí)際需要靈活設(shè)置。被控端的目標(biāo)姿態(tài)角 包括：目標(biāo)俯仰角θ'、目標(biāo)橫滾角γ'和目標(biāo)航向角
[0231 ] 獲取單元140獲取被控端的目標(biāo)角速度。由于在被控端達(dá)到目標(biāo)姿態(tài)后，若要保 持在穩(wěn)定狀態(tài)，此時(shí)角速度應(yīng)該為〇,因此，在本實(shí)施例中，令被控端的目標(biāo)角速度為預(yù) 設(shè)的固定值：〇。
[0232] 由此，獲取預(yù)先設(shè)置的被控端目標(biāo)狀態(tài)。所獲取的被控端目標(biāo)狀態(tài)包括：被控端 的目標(biāo)角速度和被控端的目標(biāo)姿態(tài)角；其中，被控端的目標(biāo)姿態(tài)角包括：目標(biāo)俯仰角 Θ '、目標(biāo)橫滾角γ'和目標(biāo)航向角φ'以及目標(biāo)角速度c〇t。
[0233] 獲取單元140獲取固定端和被控端在云臺(tái)電機(jī)軸上的轉(zhuǎn)動(dòng)慣量比k，固定端與被 控端的轉(zhuǎn)動(dòng)慣量比為預(yù)設(shè)系數(shù)。得到的轉(zhuǎn)動(dòng)慣量比包括：固定端和被控端在俯仰電機(jī)軸上 的轉(zhuǎn)動(dòng)慣量比kp，固定端和被控端在橫滾電機(jī)軸上的轉(zhuǎn)動(dòng)慣量比k，固定端和被控端在航 向電機(jī)軸上的轉(zhuǎn)動(dòng)慣量比ky。
[0234] 需要說(shuō)明的是，得到的轉(zhuǎn)動(dòng)慣量比為大量理論計(jì)算或?qū)嶒?yàn)數(shù)據(jù)所得，可根據(jù)實(shí)際 需要進(jìn)行數(shù)值調(diào)整。
[0235] 傳感單元110,還用于獲取固定端的角速度和姿態(tài)角，作為固定端當(dāng)前狀態(tài)。
[0236] 傳感單元110獲取固定端的角速度wb，
[0237] 傳感單元110獲取固定端的旋轉(zhuǎn)角度，得到的固定端的姿態(tài)角為：橫滾角Θ p俯 仰角θρ，航向角0y。固定端的姿態(tài)角也即飛行器的姿態(tài)角。
[0238] 由此，傳感單元110得到固定端當(dāng)前狀態(tài)。所得到的固定端當(dāng)前狀態(tài)包括：固定端 的角速度和固定端的姿態(tài)角；其中，固定端的姿態(tài)角包括：橫滾角Θ p俯仰角θρ，航向 角Qy〇
[0239] 在本實(shí)施例中，根據(jù)傳感單元110獲取的被控端角速度和姿態(tài)四元數(shù)，矩陣單元 120獲取被控端的旋轉(zhuǎn)矩陣，姿態(tài)單元130從而獲取所述被控端的歐拉角，所得到被控端當(dāng) 前姿態(tài)精度高；獲取單元140獲取被控端目標(biāo)狀態(tài)、固定端當(dāng)前狀態(tài)、固定端與被控端的轉(zhuǎn) 動(dòng)慣量比。本實(shí)施例中所獲取的各項(xiàng)數(shù)據(jù)精度高，為后續(xù)控制被控端有效機(jī)械能的計(jì)算提 供了精確的數(shù)據(jù)基礎(chǔ)。
[0240] 進(jìn)一步的，參照?qǐng)D8,本發(fā)明第三實(shí)施例提供一種云臺(tái)控制裝置，基于上述圖6所 示的實(shí)施例，所述計(jì)算模塊200包括：
[0241 ] 速度計(jì)算單元210，用于根據(jù)所述被控端當(dāng)前狀態(tài)和目標(biāo)狀態(tài)，以及固定端當(dāng)前狀 態(tài)，獲取控制被控端的補(bǔ)償角速度。
[0242] 在機(jī)體擾動(dòng)時(shí)，固定端會(huì)在云臺(tái)電機(jī)軸上產(chǎn)生響應(yīng)的干擾角速度，為隔離機(jī)體擾 動(dòng)，達(dá)到目標(biāo)姿態(tài)，需控制云臺(tái)電機(jī)軸轉(zhuǎn)動(dòng)對(duì)被控端補(bǔ)償角速度，以使被控端達(dá)到目標(biāo)姿態(tài) 并保持狀態(tài)穩(wěn)定。
[0243] 因此，在獲取模塊100獲取被控端當(dāng)前狀態(tài)和目標(biāo)狀態(tài)，固定端當(dāng)前狀態(tài)，以及固 定端與被控端的轉(zhuǎn)動(dòng)慣量比之后，首先，速度計(jì)算單元210計(jì)算獲取控制被控端的補(bǔ)償角 速度，該補(bǔ)償角速度為：云臺(tái)電機(jī)軸轉(zhuǎn)動(dòng)時(shí)所給予被控端的補(bǔ)償角速度，使被控端隔離機(jī)體 擾動(dòng)，能夠達(dá)到目標(biāo)姿態(tài)并保持穩(wěn)定狀態(tài)。
[0244] 具體的，作為一種實(shí)施方式，首先，速度計(jì)算單元210根據(jù)被控端當(dāng)前的角速度： 在俯仰電機(jī)軸上的角速度《piteh，在橫滾電機(jī)軸上的角速度ωΜ?1，和在航向電機(jī)軸上的角 速度c^yaw。
[0245] 速度計(jì)算單元210根據(jù)被控端當(dāng)前姿態(tài)角和目標(biāo)姿態(tài)角，計(jì)算獲取姿態(tài)角差值： 俯仰角差值A(chǔ) Θ，橫滾角差值Δ γ和航向角差值&φ。
[0246] 然后，速度計(jì)算單元210根據(jù)被控端當(dāng)前姿態(tài)角、固定端在云臺(tái)三個(gè)電機(jī)軸上的 角速度，以及姿態(tài)角差值，獲取控制被控端的補(bǔ)償角速度；控制被控端的補(bǔ)償角速度包括： 在俯仰電機(jī)軸上的補(bǔ)償角速度ωρ，在橫滾電機(jī)軸上的補(bǔ)償角速度ω。，和在航向電機(jī)軸上 的補(bǔ)償角速度《y。
[0247] 能量計(jì)算單元220,用于根據(jù)所述控制被控端的補(bǔ)償角速度、固定端與被控端的轉(zhuǎn) 動(dòng)慣量比以及固定端當(dāng)前狀態(tài)，計(jì)算獲取控制被控端的有效機(jī)械能。
[0248] 在速度計(jì)算單元210獲取控制被控端的補(bǔ)償角速度后，能量計(jì)算單元220根據(jù)控 制被控端的補(bǔ)償角速度、固定端與被控端的轉(zhuǎn)動(dòng)慣量比以及固定端當(dāng)前狀態(tài)，計(jì)算獲取控 制被控端的有效機(jī)械能，該有效機(jī)械能為：控制云臺(tái)電機(jī)軸轉(zhuǎn)動(dòng)，使被控端達(dá)到目標(biāo)姿態(tài)并 保持在穩(wěn)定狀態(tài)時(shí)，電機(jī)傳出的有效機(jī)械能。
[0249] 具體的，作為一種實(shí)施方式，能量計(jì)算單元220可以根據(jù)轉(zhuǎn)動(dòng)慣量守恒和功能關(guān) 系，列出方程組，計(jì)算控制被控端的有效機(jī)械能。由于是通過(guò)控制云臺(tái)的各電機(jī)軸轉(zhuǎn)動(dòng)來(lái)控 制被控端，因此，所得到的控制被控端的有效機(jī)械能也即控制云臺(tái)各電機(jī)軸的有效機(jī)械能。
[0250] 首先，能量計(jì)算單元220得到以下數(shù)據(jù)：
[0251] 固定端與被控端的轉(zhuǎn)動(dòng)慣量比：k，即三軸云臺(tái)各軸上的k可表示為：k = kp，I， ky；
[0252] -次控制后的云臺(tái)各電機(jī)定子的角速度為cos，在后續(xù)的公式中可將其消掉；
[0253] 云臺(tái)各電機(jī)軸上的補(bǔ)償角速度：ω。，即三軸云臺(tái)各軸上的ω。可表示為：ω ωρ，ωΓ，《y;
[0254] 根據(jù)固定端當(dāng)前姿態(tài)獲取的固定端在云臺(tái)各電機(jī)軸上的角速度：ws，即三軸云臺(tái) 各軸上的表示為 ^ s ^ pitch? ^ roll ? ^ yaw?
[0255] 然后，能量計(jì)算單元220令控制云臺(tái)各電機(jī)軸的有效機(jī)械能為E，即三軸云臺(tái)各軸 上的E可表示為：E = Ep，Er，Ey;
[0256] 根據(jù)轉(zhuǎn)動(dòng)慣量守恒和功能關(guān)系，則有方程組：
[0258] 其中，cot為被控端目標(biāo)角速度，由于被控端到達(dá)目標(biāo)姿態(tài)并保持穩(wěn)定時(shí)，被控端 目標(biāo)速度為零，因此，被控端在云臺(tái)各電機(jī)軸上的目標(biāo)角速度ω ^勺值均為〇
[0259] 然后，能量計(jì)算單元220求解上述方程組，得到：
[0261] 更為具體的：當(dāng)計(jì)算控制俯仰電機(jī)軸的有效機(jī)械能時(shí)，令 ^ s ^ pitch? ^ c ^ p? k = kp;此時(shí)，根據(jù)上述方程組計(jì)算得到控制俯仰電機(jī)軸的有效機(jī)械能E p，能夠使被控端在 俯仰軸方向達(dá)到目標(biāo)姿態(tài)并保持穩(wěn)定狀態(tài)；
[0262] 當(dāng)計(jì)算控制橫滾電機(jī)軸的有效機(jī)械能時(shí)，令Ws= ω roll，ω。= ω = I;此時(shí)， 根據(jù)上述方程組計(jì)算得到控制橫滾電機(jī)軸的有效機(jī)械能Ep能夠使被控端在橫滾軸方向達(dá) 到目標(biāo)姿態(tài)并保持穩(wěn)定狀態(tài)；
[0263] 當(dāng)計(jì)算控制航向電機(jī)軸的有效機(jī)械能時(shí)，令Ws= ω yaw，ω。= ω y，k = ky;此時(shí)， 根據(jù)上述方程組計(jì)算得到控制航向電機(jī)軸的有效機(jī)械能Ey，能夠使被控端在航向軸方向達(dá) 到目標(biāo)姿態(tài)并保持穩(wěn)定狀態(tài)。
[0264] 由此，得到控制云臺(tái)各電機(jī)軸的有效機(jī)械能：控制俯仰電機(jī)軸的有效機(jī)械能Ep、控 制橫滾電機(jī)軸的有效機(jī)械能Ep和控制航向電機(jī)軸的有效機(jī)械能Ey。也即，得到控制被控端 的有效機(jī)械能Ep、民和E y。
[0265] 其中，需要說(shuō)明的是，E的符號(hào)代表電機(jī)軸轉(zhuǎn)動(dòng)方向，可根據(jù)實(shí)際情況具體確定。例 如：+(正）為順時(shí)針轉(zhuǎn)動(dòng)，一（負(fù)）為逆時(shí)針轉(zhuǎn)動(dòng)。
[0266] 在本實(shí)施例中，根據(jù)獲取的被控端當(dāng)前狀態(tài)和目標(biāo)狀態(tài)，固定端當(dāng)前狀態(tài)，以及固 定端與被控端的轉(zhuǎn)動(dòng)慣量比，首先速度計(jì)算單元210計(jì)算控制被控端的補(bǔ)償角速度，然后 能量計(jì)算單元220計(jì)算控制被控端的有效機(jī)械能，能夠控制云臺(tái)電機(jī)軸轉(zhuǎn)動(dòng)使被控端達(dá)到 目標(biāo)姿態(tài)并保持在穩(wěn)定狀態(tài)。本實(shí)施例實(shí)現(xiàn)了對(duì)云臺(tái)的精確控制，使云臺(tái)根據(jù)得到的有效 機(jī)械能，能夠控制云臺(tái)各電機(jī)軸轉(zhuǎn)動(dòng)，使被控端達(dá)到目標(biāo)姿態(tài)并保持在穩(wěn)定狀態(tài)，達(dá)到了云 臺(tái)自穩(wěn)控制效果。
[0267] 進(jìn)一步的，參照?qǐng)D9,本發(fā)明第四實(shí)施例提供一種云臺(tái)控制裝置，基于上述圖6所 示的實(shí)施例，所述速度計(jì)算單元210包括：
[0268] 干擾子單元211，用于根