到目標姿態(tài)并保持穩(wěn)定狀態(tài)。
[0144] 由此，得到控制云臺各電機軸的有效機械能：控制俯仰電機軸的有效機械能Ep、控 制橫滾電機軸的有效機械能Ep和控制航向電機軸的有效機械能Ey。也即，得到控制被控端 的有效機械能Ep、民和E y。
[0145] 其中，需要說明的是，E的符號代表電機軸轉動方向，可根據實際情況具體確定。例 如：+(正）為順時針轉動，一（負）為逆時針轉動。
[0146] 在本實施例中，根據獲取的被控端當前狀態(tài)和目標狀態(tài)，固定端當前狀態(tài)，以及固 定端與被控端的轉動慣量比，首先計算控制被控端的補償角速度，然后計算控制被控端的 有效機械能，能夠控制云臺電機軸轉動使被控端達到目標姿態(tài)并保持在穩(wěn)定狀態(tài)。本實施 例實現(xiàn)了對云臺的精確控制，使云臺根據得到的有效機械能，能夠控制云臺各電機軸轉動， 使被控端達到目標姿態(tài)并保持在穩(wěn)定狀態(tài)，達到了云臺自穩(wěn)控制效果。
[0147] 進一步的，參照圖4,本發(fā)明第四實施例提供一種云臺控制方法，基于上述圖3所 示的實施例，所述步驟S21包括：
[0148] 步驟S211、根據所述固定端當前狀態(tài)獲取固定端在云臺電機軸上的角速度。
[0149] 根據固定端當前狀態(tài)可以獲取固定端在云臺電機軸上的角速度，固定端在云臺電 機軸上的角速度包括：在俯仰電機軸上的角速度Wpiteh，在橫滾電機軸上的角速度ω"η，和 在航向電機軸上的角速度《yaw。計算固定端在云臺各電機軸上的角速度，即為計算《piteh、 ? rollon W yaw 〇
[0150] 具體的，作為一種實施方式，首先，固定端當前狀態(tài)包括：固定端的角速度COb和固 定端姿態(tài)角；其中，
固定端姿態(tài)角包括：橫滾角θ ρ俯仰角θρ，航向角9y。
[0151] 然后，將固定端的角速度耦合到云臺電機軸，根據坐標變換關系，得到固定端在航 向電機軸上的角速度Wyaw、COroll和ω yaw:
[0153] 其中，θ ρ，Θ y Θ y分別為電機相對零位值的旋轉角度，由磁編碼器采集得到。根據 公式可以得到《piteh、Coroll和ω yaw，由此，得到固定端在云臺各電機軸上的角速度：copiteh、 ? rollon W yaw 〇
[0154] 步驟S212、根據所述被控端當前姿態(tài)和目標姿態(tài)，獲取姿態(tài)角差值。
[0155] 姿態(tài)角差值為被控端由當前姿態(tài)達到目標姿態(tài)的角度差值，令姿態(tài)角差值為：俯 仰角差值△ Θ，橫滾角差值△ γ和航向角差值·4φ'。
[0156] 根據被控端目標姿態(tài)：目標俯仰角θ'、目標橫滾角γ'和目標航向角和被 控端當前的歐拉角：俯仰角Θ、橫滾角γ和航向角f，計算姿態(tài)角差值：
[0157] CN 105116926 A 說明書 10/20 頁
[0158] 由此，得到姿態(tài)角差值，包括：俯仰角差值A θ，橫滾角差值△ γ和航向角差值
[0159] 步驟S213、根據所述被控端的當前狀態(tài)、固定端在云臺電機軸上的角速度和所述 姿態(tài)角差值，獲取控制被控端的補償角速度。
[0160] 控制被控端的補償角速度由隔離固定端干擾的反向補償角速度和使被控端達到 目標姿態(tài)的補償角速度組成。
[0161] 首先，根據被控端當前狀態(tài)中的角速度.
固定端在云臺電機軸上的角 速度copiteh、ω"η和ω yaw，計算隔離固定端干擾的反向補償角速度：
[0162] 設反向補償角速度為：橫滾角速度δ ^俯仰角速度δ p、航向角速度δ y;
[0163] 則根據坐標變換關系，得到方程式：
[0165] 解算上述方程，得到反向補償角速度：橫滾角速度δ ^俯仰角速度δ p和航向角速 度δ y。
[0166] 然后，在本實施例中，取使被控端達到目標姿態(tài)的補償角速度為：俯仰角速度 A · Δ Θ，橫滾角速度A · △ γ和航向角速度其中，A為預設系數(shù)，用于精確校準補 償角速度，可根據實際情況靈活設定。
[0167] 然后，計算控制被控端的補償角速度，取：在俯仰電機軸上的補償角速度ωρ，在橫 滾電機軸上的補償角速度&^，和在航向電機軸上的補償角速度《y。
[0168] 則有如下計算公式：
[0170] 由此，計算得到控制被控端的補償角速度，包括：在俯仰電機軸上的補償角速度 ωρ，在橫滾電機軸上的補償角速度ω p和在航向電機軸上的補償角速度coy。
[0171] 在本實施例中，根據獲取的被控端當前狀態(tài)和目標狀態(tài)、固定端當前狀態(tài)，獲取隔 離固定端干擾的反向補償角速度和使被控端達到目標角姿態(tài)的補償角速度，得到控制被控 端的補償角速度。本實施例得到的補償角速度，能夠使控制云臺電機軸轉動時，實現(xiàn)被控端 隔離機體擾動，達到目標姿態(tài)并保持穩(wěn)定狀態(tài)。
[0172] 進一步的，參照圖5，本發(fā)明云臺控制方法第五實施例提供一種云臺控制方法，基 于上述圖1所示的實施例，所述步驟S30包括：
[0173] 步驟S31、根據所述有效機械能，獲取控制所述被控端的目標電流。
[0174] 在獲取控制被控端的有效機械能后，根據控制被控端的有效機械能，計算控制被 控端的目標電流，也即：根據控制云臺各電機軸的有效機械能，分別計算控制云臺各電機軸 的目標電流。
[0175] 具體的，作為一種實施例，獲取的控制被控端的有效機械能為：控制俯仰電機軸的 有效機械能EP、控制橫滾電機軸的有效機械能Ep和控制航向電機軸的有效機械能Ey。
[0176] 設控制云臺各電機軸的目標電流為：控制俯仰電機軸的目標電流Ip，控制橫滾電 機軸的目標電流Ip控制航向電機軸的目標電流Iy;則有：
[0178] 根據上述公式，計算得到控制云臺各電機軸的目標電流：Ip、ldP Iy。也即，得到控 制被控端的目標電流：IP、L和I y。
[0179] 其中，需要說明的是，E的符號代表電機軸轉動方向，可根據實際情況具體確定。例 如：+(正）為順時針轉動，一（負）為逆時針轉動。Ip、Ir和I ^勺符號與E p、艮和E y分別 相同。
[0180] 步驟S32、根據所述目標電流，通過云臺電機軸控制所述被控端保持在穩(wěn)定狀態(tài)。
[0181] 在獲得控制被控端的目標電流后，以相應的目標電流控制云臺各電機軸，使被控 端達到目標姿態(tài)并保持在穩(wěn)定狀態(tài)。
[0182] 具體的，以控制俯仰電機軸的目標電流Ip控制俯仰電機軸，從而使被控端在俯仰 方向達到目標俯仰角，并保持俯仰方向的穩(wěn)定狀態(tài)；
[0183] 以控制橫滾電機軸的目標電流Id空制橫滾電機軸，從而使被控端在橫滾方向達到 目標橫滾角，并保持橫滾方向的穩(wěn)定狀態(tài)；
[0184] 以控制航向電機軸的目標電流1』空制航向電機軸，從而使被控端在航向方向達到 目標航向角，并保持航向方向的穩(wěn)定狀態(tài)。
[0185] 在本實施例中，根據獲取的有效機械能，獲取控制被控端的目標電流，并以控制被 控端的目標電流，控制云臺各電機軸，使被控端達到目標姿態(tài)并保持穩(wěn)定，實現(xiàn)了云臺的自 穩(wěn)控制效果。
[0186] 參照圖6,本發(fā)明云臺控制裝置第一實施例提供一種云臺控制裝置，所述云臺控制 裝置包括：
[0187] 獲取模塊100,用于獲取云臺上被控端當前狀態(tài)和目標狀態(tài)、固定端當前狀態(tài)，以 及固定端與被控端的轉動慣量比。
[0188] 本實施例方案主要應用于三軸云臺中，當然也不限于其他具有精確控制處理需求 的單軸或二軸云臺。云臺可應用于飛行器、汽車、輪船、機器人、人體等，例如：當云臺應用于 飛行器時，為機載云臺，可負載拍攝設備或攝影設備，進行動態(tài)拍攝或動態(tài)攝影。
[0189] 本實施例以無人機三軸云臺進行舉例，三軸云臺固定在無人機體上，且三軸云臺 搭載有相機。其中，三軸云臺通過三個電機軸分別控制相機進行俯仰、橫滾和航向方向的轉 動，具體的：俯仰電機軸控制相機進行俯仰轉動，橫滾電機軸控制相機進行橫滾轉動，航向 電機軸控制相機進行航向轉動。
[0190] 本實施例在75Γ臺啟動后，獲取模塊100首先進行?目息米集：獲取被控端當前狀態(tài)、 固定端當前狀態(tài)；獲取被控端目標狀態(tài)，獲取固定端與被控端的轉動慣量比。
[0191] 其中，被控端為云臺負載的相機部分，與被控端相對，固定端可以為與無人機相連 的云臺部分，也可能包括與其剛性連接的人機；以固定端為云臺進行舉例，云臺通過云臺上 的三個電機軸與被控端連接，分別控制被控端繞俯仰電機軸、橫滾電機軸和航向電機軸轉 動。
[0192] 具體的，作為一種實施方式，在云臺啟動后，獲取模塊100獲取被控端當前姿態(tài)。 被控端的當前狀態(tài)包括：被控端的角速度和被控端的狀態(tài)（即被控端的歐拉角）。
[0193] 獲取模塊100獲取被控端目標狀態(tài)。被控端目標狀態(tài)包括：被控端的目標姿態(tài)角 和目標角速度。
[0194] 獲取模塊100獲取固定端當前狀態(tài)。固定端當前狀態(tài)包括：固定端的角速度和當 前姿態(tài)角。
[0195] 獲取模塊100獲取固定端與被控端的轉動慣量比。固定端與被控端的轉動慣量比 為預設系數(shù)，可以通過測量得到。
[0196] 計算模塊200,用于根據所述被控端當前狀態(tài)和目標狀態(tài)、固定端當前狀態(tài)，以及 固定端與被控端的轉動慣量比，計算獲取控制被控端的有效機械能。
[0197] 在獲取模塊100獲取被控端當前狀態(tài)和目標狀態(tài)，固定端當前狀態(tài)，以及固定端 與被控端的轉動慣量比之后，計算模塊200計算獲取控制被控端的有效機械能。
[0198] 具體的，作為一種實施方式，首先，計算模塊200根據被控端當前狀態(tài)和目標狀 態(tài)，以及固定端當前狀態(tài)，獲取控制被控端的補償角速度，該補償角速度為：云臺電機軸轉 動時所給予被控端的補償角速度，能夠使被控端隔離機體擾動，達到目標姿態(tài)并保持在穩(wěn) 定狀態(tài)。
[0199] 然后，計算模塊200根據控制被控端的補償角速度、固定端與被控端的轉動慣量 比以及固定端當前狀態(tài)，計算獲取控制被控端的有效機械能，該有效機械能為：控制云臺電 機軸轉動使被控端達到目標姿態(tài)并保持在穩(wěn)定狀態(tài)時，電機傳出的有效機械能。
[0200] 控制模塊300,用于根據所述有效機械能，控制所述被控端保持在穩(wěn)定狀態(tài)。
[0201] 在計算模塊200獲取有效機械能后，控制模塊300根據獲取的有效機械能，計算獲 得控制被控端的目標電流。由于是通過控制云臺各電機軸轉動，來控制被控端在俯仰、橫滾 和航向三個方向的轉動，因此，控制被控端的目標電流也即控制云臺電機軸的目標電流；
[0202] 然后根據獲取的目標電流，控制云臺各電機軸轉動，從而控制被控端達到目標姿 態(tài)并保持在穩(wěn)定狀態(tài)。
[0203] 在本實施例中，通過獲取模塊100獲取的云臺上被控端當前狀態(tài)和目標狀態(tài)、固 定端當前狀態(tài)，以及固定端與被控端的轉動慣量比，計算模塊200計算得到控制被控端的 有效機械能，控制模塊300根據該有效機械能控制所述被控端，使被控端達到目標姿態(tài)并 保持在穩(wěn)定狀態(tài)，有效隔離機體擾動，實現(xiàn)了對云臺的精確控制，達到了云臺自穩(wěn)控制效 果，保障了云臺負載相機的成像質量。
[0204] 進一步的，參照圖7,本發(fā)明云臺控制方法第二實施例提供一種云臺控制裝置，基 于圖6所示的實施例，所述獲取模塊100包括：
[0205] 傳感單元110,用于獲取被控端的角速度和姿態(tài)四元數(shù)。
[0206] 在云臺啟動后，傳感單元110獲取被控端的角速度和姿態(tài)四元數(shù)，也即云臺負載 相機的當前角速度和姿