據(jù)所述固定端當(dāng)前狀態(tài)獲取固定端在云臺電機(jī)軸上的角 速度。
[0269] 干擾子單元211可以根據(jù)固定端當(dāng)前狀態(tài)獲取固定端在云臺電機(jī)軸上的角速度， 固定端在云臺電機(jī)軸上的角速度包括：在俯仰電機(jī)軸上的角速度《piteh，在橫滾電機(jī)軸上的 角速度COroll，和在航向電機(jī)軸上的角速度COyaw。干擾子單元211計算固定端在云臺各電機(jī) 軸上的角速度，即為計算 〇 pitch、〇 roll萍卩 〇 yaw0
[0270] 具體的，作為一種實(shí)施方式，首先，固定端當(dāng)前狀態(tài)包括：固定端的角速度cob和固 定端的姿態(tài)角；其中
固定端的姿態(tài)角包括：橫滾角Θ p俯仰角θ p，航向角 9 y。
[0271] 然后，干擾子單元211將固定端的角速度耦合到云臺電機(jī)軸，根據(jù)坐標(biāo)變換關(guān)系， 得到固定端在航向電機(jī)軸上的角速度《yaw、Coroll和ω yaw:
[0273] 其中，θρ，θρ 0y分別為電機(jī)相對零位值的旋轉(zhuǎn)角度，由磁編碼器采集得到。由 此，干擾子單元211得到固定端在云臺各電機(jī)軸上的角速度：c〇piteh、ω yaw。
[0274] 差值子單元212,用于根據(jù)所述被控端當(dāng)前姿態(tài)和目標(biāo)姿態(tài)，獲取姿態(tài)角差值。
[0275] 姿態(tài)角差值為被控端由當(dāng)前姿態(tài)達(dá)到目標(biāo)姿態(tài)的角度差值，差值子單元212令姿 態(tài)角差值為：俯仰角差值△ θ，橫滾角差值△ γ和航向角差值Λφ。
[0276] 根據(jù)獲取的被控端目標(biāo)姿態(tài)：目標(biāo)俯仰角Θ '、目標(biāo)橫滾角γ'和目標(biāo)航向角 爭_f;和獲取的被控端當(dāng)前的歐拉角：俯仰角Θ、橫滾角γ和航向角屮，差值子單元212計 算姿態(tài)角差值：
[0278] 由此，差值子單元212得到姿態(tài)角差值，包括：俯仰角差值△ Θ，橫滾角差值Δ γ 和航向角差值么中。
[0279] 補(bǔ)償子單元213,用于根據(jù)所述被控端的當(dāng)前狀態(tài)、固定端在云臺電機(jī)軸上的角速 度和所述姿態(tài)角差值，獲取控制被控端的補(bǔ)償角速度。
[0280] 控制被控端的補(bǔ)償角速度由隔離固定端干擾的反向補(bǔ)償角速度和使被控端達(dá)到 目標(biāo)姿態(tài)的補(bǔ)償角速度組成。
[0281] 首先，補(bǔ)償子單元213根據(jù)被控端當(dāng)前狀態(tài)中的角速度
固定端在云 臺電機(jī)軸上的角速度《piteh、Coroll和ω yaw，計算隔離固定端干擾的反向補(bǔ)償角速度：
[0282] 設(shè)反向補(bǔ)償角速度為：橫滾角速度δ ^俯仰角速度δ p、航向角速度δ y;
[0283] 則根據(jù)坐標(biāo)變換關(guān)系，得到方程式：
[0286] 補(bǔ)償子單元213解算上述方程，得到反向補(bǔ)償角速度：橫滾角速度δ ^俯仰角速 度Sp和航向角速度S y。
[0287] 然后，在本實(shí)施例中，補(bǔ)償子單元213取使被控端達(dá)到目標(biāo)姿態(tài)的補(bǔ)償角速度為： 俯仰角速度A ·△ θ，橫滾角速度A ·△ γ和航向角速度A'?φ;其中，A為預(yù)設(shè)系數(shù)，用于 精確校準(zhǔn)補(bǔ)償角速度，可根據(jù)實(shí)際情況靈活設(shè)定。
[0288] 然后，補(bǔ)償子單元213計算控制被控端的補(bǔ)償角速度，令：在俯仰電機(jī)軸上的補(bǔ)償 角速度ωρ，在橫滾電機(jī)軸上的補(bǔ)償角速度％,和在航向電機(jī)軸上的補(bǔ)償角速度coy;
[0289] 則有如下計算公式：
[0291] 由此，補(bǔ)償子單元213計算得到控制被控端的補(bǔ)償角速度，包括：在俯仰電機(jī)軸 上的補(bǔ)償角速度ωρ，在橫滾電機(jī)軸上的補(bǔ)償角速度ωρ和在航向電機(jī)軸上的補(bǔ)償角速度 ω y°
[0292] 在本實(shí)施例中，根據(jù)獲取的被控端當(dāng)前狀態(tài)和被控端目標(biāo)狀態(tài)、固定端當(dāng)前狀態(tài)， 獲取隔離固定端干擾的反向補(bǔ)償角速度和使被控端達(dá)到目標(biāo)姿態(tài)的補(bǔ)償角速度，補(bǔ)償子單 元213得到控制被控端的補(bǔ)償角速度。本實(shí)施例得到的補(bǔ)償角速度，能夠使控制云臺電機(jī) 軸轉(zhuǎn)動時，實(shí)現(xiàn)被控端隔離機(jī)體擾動，達(dá)到目標(biāo)姿態(tài)并保持穩(wěn)定狀態(tài)。
[0293] 進(jìn)一步的，參照圖10,本發(fā)明第五實(shí)施例提供一種云臺控制裝置，基于上述圖1所 示的實(shí)施例，所述控制模塊300包括：
[0294] 電流單元310,用于根據(jù)所述有效機(jī)械能，獲取控制所述被控端的目標(biāo)電流。
[0295] 計算模塊200獲取控制被控端的有效機(jī)械能后，電流單元310根據(jù)控制被控端的 有效機(jī)械能，計算控制被控端的目標(biāo)電流，也即：根據(jù)控制云臺各電機(jī)軸的有效機(jī)械能，分 別計算控制云臺各電機(jī)軸的目標(biāo)電流。
[0296] 具體的，作為一種實(shí)施例，獲取的控制被控端的有效機(jī)械能為：控制俯仰電機(jī)軸的 有效機(jī)械能Ερ、控制橫滾電機(jī)軸的有效機(jī)械能Ep和控制航向電機(jī)軸的有效機(jī)械能Ey。
[0297] 電流單元310設(shè)云臺各電機(jī)軸的目標(biāo)電流為：控制俯仰電機(jī)軸的目標(biāo)電流Ip，控 制橫滾電機(jī)軸的目標(biāo)電流L，控制航向電機(jī)軸的目標(biāo)電流Iy;則有：
[0299] 根據(jù)上述公式，電流單元310計算得到控制云臺各電機(jī)軸的目標(biāo)電流：IP、I^ I y。 也即，得到控制被控端的目標(biāo)電流：IP、IdP I y。
[0300] 其中，需要說明的是，E的符號代表電機(jī)軸轉(zhuǎn)動方向，可根據(jù)實(shí)際情況具體確定。例 如：+(正）為順時針轉(zhuǎn)動，一（負(fù)）為逆時針轉(zhuǎn)動。Ip、Ir和I ^勺符號與E p、艮和E y分別 相同。
[0301 ] 控制單元320,用于根據(jù)所述目標(biāo)電流，通過云臺電機(jī)軸控制所述被控端保持在穩(wěn) 定狀態(tài)。
[0302] 在電流單元310獲得控制被控端的目標(biāo)電流后，控制單元320以相應(yīng)的目標(biāo)電流 控制云臺各電機(jī)軸，使被控端達(dá)到目標(biāo)姿態(tài)并保持在穩(wěn)定狀態(tài)。
[0303] 具體的，控制單元320以控制俯仰電機(jī)軸的目標(biāo)電流Ip控制俯仰電機(jī)軸，從而使 被控端在俯仰方向達(dá)到目標(biāo)俯仰角，并保持俯仰方向的穩(wěn)定狀態(tài)；
[0304] 控制單元320以控制橫滾電機(jī)軸的目標(biāo)電流Id空制橫滾電機(jī)軸，從而使被控端在 橫滾方向達(dá)到目標(biāo)橫滾角，并保持橫滾方向的穩(wěn)定狀態(tài)；
[0305] 控制單元320以控制航向電機(jī)軸的目標(biāo)電流1￥控制航向電機(jī)軸，從而使被控端在 航向方向達(dá)到目標(biāo)航向角，并保持航向方向的穩(wěn)定狀態(tài)。
[0306] 在本實(shí)施例中，根據(jù)獲取的有效機(jī)械能，電流單元310獲取控制被控端的目標(biāo)電 流，控制單元320以控制被控端的目標(biāo)電流，控制云臺各電機(jī)軸，使被控端達(dá)到目標(biāo)姿態(tài)并 保持穩(wěn)定，實(shí)現(xiàn)了云臺的自穩(wěn)控制效果。
[0307] 以上僅為本發(fā)明的優(yōu)選實(shí)施例，并非因此限制本發(fā)明的專利范圍，凡是利用本發(fā) 明說明書及附圖內(nèi)容所作的等效結(jié)構(gòu)或等效流程變換，或直接或間接運(yùn)用在其他相關(guān)的技 術(shù)領(lǐng)域，均同理包括在本發(fā)明的專利保護(hù)范圍內(nèi)。
【主權(quán)項(xiàng)】
1. 一種云臺控制方法，其特征在于，所述云臺控制方法包括以下步驟： 獲取云臺上被控端當(dāng)前狀態(tài)和目標(biāo)狀態(tài)、固定端當(dāng)前狀態(tài)，以及固定端與被控端的轉(zhuǎn) 動慣量比； 根據(jù)所述被控端當(dāng)前狀態(tài)和目標(biāo)狀態(tài)、固定端當(dāng)前狀態(tài)，以及固定端與被控端的轉(zhuǎn)動 慣量比，計算獲取控制被控端的有效機(jī)械能； 根據(jù)所述有效機(jī)械能，控制所述被控端保持在穩(wěn)定狀態(tài)。2. 如權(quán)利要求1所述的云臺控制方法，其特征在于，所述獲取云臺上被控端當(dāng)前狀態(tài) 和目標(biāo)狀態(tài)、固定端當(dāng)前狀態(tài)，以及固定端與被控端的轉(zhuǎn)動慣量比的步驟包括： 獲取被控端的角速度和姿態(tài)四元數(shù)； 根據(jù)所述被控端的姿態(tài)四元數(shù)，獲取被控端的旋轉(zhuǎn)矩陣； 根據(jù)所述被控端的旋轉(zhuǎn)矩陣，獲取被控端的歐拉角，得到被控端當(dāng)前姿態(tài)； 獲取被控端的目標(biāo)姿態(tài)角和目標(biāo)角速度，作為被控端目標(biāo)狀態(tài)； 獲取固定端的角速度和姿態(tài)角，作為固定端當(dāng)前狀態(tài)； 獲取固定端與被控端的轉(zhuǎn)動慣量比。3. 如權(quán)利要求1或2所述的云臺控制方法，其特征在于，所述根據(jù)所述被控端當(dāng)前狀態(tài) 和目標(biāo)狀態(tài)、固定端當(dāng)前狀態(tài)，以及固定端與被控端的轉(zhuǎn)動慣量比，計算獲取控制被控端的 有效機(jī)械能的步驟包括： 根據(jù)所述被控端當(dāng)前狀態(tài)和目標(biāo)狀態(tài)，以及固定端當(dāng)前狀態(tài)，獲取控制被控端的補(bǔ)償 角速度； 根據(jù)所述控制被控端的補(bǔ)償角速度、固定端與被控端的轉(zhuǎn)動慣量比以及固定端當(dāng)前狀 態(tài)，計算獲取控制被控端的有效機(jī)械能。4. 如權(quán)利要求3所述的云臺控制方法，其特征在于，所述根據(jù)所述被控端當(dāng)前狀態(tài)和 目標(biāo)狀態(tài)，以及固定端當(dāng)前狀態(tài)，獲取控制被控端的補(bǔ)償角速度的步驟包括： 根據(jù)所述固定端當(dāng)前狀態(tài)獲取固定端在云臺電機(jī)軸上的角速度； 根據(jù)所述被控端當(dāng)前姿態(tài)和目標(biāo)姿態(tài)，獲取姿態(tài)角差值； 根據(jù)所述被控端的當(dāng)前狀態(tài)、固定端在云臺電機(jī)軸上的角速度和所述姿態(tài)角差值，獲 取控制被控端的補(bǔ)償角速度。5. 如權(quán)利要求1所述的云臺控制方法，其特征在于，所述根據(jù)所述有效機(jī)械能，控制所 述被控端保持在穩(wěn)定狀態(tài)的步驟包括： 根據(jù)所述有效機(jī)械能，獲取控制所述被控端的目標(biāo)電流； 根據(jù)所述目標(biāo)電流，通過云臺電機(jī)軸控制所述被控端保持在穩(wěn)定狀態(tài)。6. -種云臺控制裝置，其特征在于，所述云臺控制裝置包括： 獲取模塊，用于獲取云臺上被控端當(dāng)前狀態(tài)和目標(biāo)狀態(tài)、固定端當(dāng)前狀態(tài)，以及固定端 與被控端的轉(zhuǎn)動慣量比； 計算模塊，用于根據(jù)所述被控端當(dāng)前狀態(tài)和目標(biāo)狀態(tài)、固定端當(dāng)前狀態(tài)，以及固定端與 被控端的轉(zhuǎn)動慣量比，計算獲取控制被控端的有效機(jī)械能； 控制模塊，用于根據(jù)所述有效機(jī)械能，控制所述被控端保持在穩(wěn)定狀態(tài)。7. 如權(quán)利要求6所述的云臺控制裝置，其特征在于，所述獲取模塊包括： 傳感單元，用于獲取被控端的角速度和姿態(tài)四元數(shù)； 矩陣單元，用于根據(jù)所述被控端的姿態(tài)四元數(shù)，獲取被控端的旋轉(zhuǎn)矩陣； 姿態(tài)單元，用于根據(jù)所述被控端的旋轉(zhuǎn)矩陣，獲取被控端的歐拉角，得到被控端當(dāng)前姿 態(tài)； 獲取單元，用于獲取被控端的目標(biāo)姿態(tài)角和目標(biāo)角速度，作為被控端目標(biāo)狀態(tài)；獲取固 定端與被控端的轉(zhuǎn)動慣量比； 所述傳感單元還用于，獲取固定端的角速度和姿態(tài)角，作為固定端當(dāng)前狀態(tài)。8. 如權(quán)利要求6所述的云臺控制裝置，其特征在于，所述計算模塊包括： 速度計算單元，用于根據(jù)所述被控端當(dāng)前狀態(tài)和目標(biāo)狀態(tài)，以及固定端當(dāng)前狀態(tài)，獲取 控制被控端的補(bǔ)償角速度； 能量計算單元，用于根據(jù)所述控制被控端的補(bǔ)償角速度、固定端與被控端的轉(zhuǎn)動慣量 比以及固定端當(dāng)前狀態(tài)，計算獲取控制被控端的有效機(jī)械能。9. 如權(quán)利要求8所述的云臺控制裝置，其特征在于，所述速度計算單元包括： 干擾子單元，用于根據(jù)所述固定端當(dāng)前狀態(tài)獲取固定端在云臺電機(jī)軸上的角速度； 差值子單元，用于根據(jù)所述被控端當(dāng)前姿態(tài)和目標(biāo)姿態(tài)，獲取姿態(tài)角差值； 補(bǔ)償子單元，用于根據(jù)所述被控端的當(dāng)前狀態(tài)、固定端在云臺電機(jī)軸上的角速度和所 述姿態(tài)角差值，獲取控制被控端的補(bǔ)償角速度。10. 如權(quán)利要求6所述的云臺控制裝置，其特征在于，所述控制模塊包括： 電流單元，用于根據(jù)所述有效機(jī)械能，獲取控制所述被控端的目標(biāo)電流； 控制單元，用于根據(jù)所述目標(biāo)電流，通過云臺電機(jī)軸控制所述被控端保持在穩(wěn)定狀態(tài)。
【專利摘要】本發(fā)明公開了一種云臺控制方法，獲取云臺上被控端當(dāng)前姿態(tài)和目標(biāo)姿態(tài)、固定端當(dāng)前姿態(tài)，以及固定端與被控端的轉(zhuǎn)動慣量比；根據(jù)所述被控端當(dāng)前姿態(tài)和目標(biāo)姿態(tài)、固定端當(dāng)前姿態(tài)，以及固定端與被控端的轉(zhuǎn)動慣量比，計算獲取控制被控端的有效機(jī)械能；根據(jù)所述有效機(jī)械能，控制所述被控端保持在穩(wěn)定狀態(tài)。本發(fā)明還公開了一種云臺控制裝置。本發(fā)明使被控端達(dá)到目標(biāo)姿態(tài)并保持在穩(wěn)定狀態(tài)，實(shí)現(xiàn)了對云臺的精確控制，有效隔離機(jī)體擾動，達(dá)到了云臺自穩(wěn)控制效果，保障了云臺負(fù)載相機(jī)的成像質(zhì)量。
【IPC分類】G05D3/12
【公開號】CN105116926
【申請?zhí)枴緾N201510515606
【發(fā)明人】張顯志
【申請人】深圳一電科技有限公司, 深圳一電航空技術(shù)有限公司
【公開日】2015年12月2日
【申請日】2015年8月20日