一種艇載飛行器穩(wěn)定起降裝置及控制方法
【專利摘要】本發(fā)明公開了一種艇載飛行器穩(wěn)定起降裝置及其控制方法，其中，所述艇載飛行器穩(wěn)定起降裝置包括運(yùn)動(dòng)姿態(tài)檢測(cè)單元、微處理器控制單元和起降平臺(tái)，所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元和所述起降平臺(tái)與所述微處理器控制單元電連接，所述起降平臺(tái)包括起安裝支架、起降甲板、直流減速電機(jī)、安裝板和電機(jī)支架。本發(fā)明專門為艇載飛行器的穩(wěn)定起降而設(shè)計(jì)，具有結(jié)構(gòu)簡(jiǎn)單、體積小巧、便于實(shí)現(xiàn)等諸多優(yōu)點(diǎn)，能夠極大地提高飛行器起飛、降落的可靠性，并且與現(xiàn)有的穩(wěn)定平臺(tái)相比，能夠節(jié)約很多成本，具有很好的實(shí)際應(yīng)用價(jià)值。
【專利說(shuō)明】
一種艇載飛行器穩(wěn)定起降裝置及控制方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及對(duì)飛行器升降裝置的自動(dòng)控制技術(shù)領(lǐng)域，特別涉及一種可自動(dòng)保持水 平穩(wěn)定的艇載飛行器穩(wěn)定起降裝置及控制方法。
【背景技術(shù)】
[0002] 艇載飛行器起降平臺(tái)是一種配備在小型水面船艇上的提供給艇載小型無(wú)人垂直 起降飛行器起飛、著降和停靠使用的平臺(tái)式裝置。傳統(tǒng)的起降平臺(tái)直接固定在艇上，會(huì)隨著 艇的運(yùn)動(dòng)而傾斜搖擺，由于艇的排水量小速度快，運(yùn)動(dòng)受風(fēng)浪影響大，其在航行中縱搖運(yùn)動(dòng) 幅度大導(dǎo)致起降平臺(tái)搖擺劇烈，不利于小型無(wú)人飛行器在艇上的停靠、起飛和著降。
[0003] 鑒于上述原因，迫切需要一種能夠自動(dòng)保持水平穩(wěn)定的艇載飛行器穩(wěn)定起降裝 置。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明所要解決的技術(shù)問(wèn)題是：如何提供一種能夠自動(dòng)保持水平穩(wěn)定的艇載飛行 器穩(wěn)定起降裝置及控制方法。為此，本發(fā)明提出一種艇載飛行器穩(wěn)定起降裝置及控制方法， 可充分地消除由于現(xiàn)有技術(shù)的限制和缺陷導(dǎo)致的一個(gè)或多個(gè)問(wèn)題。
[0005] 本發(fā)明另外的優(yōu)點(diǎn)、目的和特性，一部分將在下面的說(shuō)明書中得到闡明，而另一部 分對(duì)于本領(lǐng)域的普通技術(shù)人員通過(guò)對(duì)下面的說(shuō)明的考察將是明顯的或從本發(fā)明的實(shí)施中 學(xué)到。通過(guò)在文字的說(shuō)明書和權(quán)利要求書及附圖中特別地指出的結(jié)構(gòu)可實(shí)現(xiàn)和獲得本發(fā)明 目的和優(yōu)點(diǎn)。
[0006] 本發(fā)明提供了一種艇載飛行器穩(wěn)定起降裝置，其特征在于，所述艇載飛行器穩(wěn)定 起降裝置包括運(yùn)動(dòng)姿態(tài)檢測(cè)單元、微處理器控制單元和起降平臺(tái)，所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元 和所述起降平臺(tái)與所述微處理器控制單元電連接，所述起降平臺(tái)包括起安裝支架、起降甲 板、直流減速電機(jī)、安裝板和電機(jī)支架，其中，所述安裝板置于所述安裝支架上，所述安裝板 上安裝有兩個(gè)軸承座和所述電機(jī)支架，所述電機(jī)支架上安裝有所述直流減速電機(jī)，所述兩 個(gè)軸承座上連接著旋轉(zhuǎn)軸，所述旋轉(zhuǎn)軸和所述直流減速電機(jī)的電機(jī)軸通過(guò)兩個(gè)法蘭盤對(duì)接 連接，所述起降甲板通過(guò)兩個(gè)連接件固定在所述旋轉(zhuǎn)軸上；另外，
[0007] 所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元被設(shè)計(jì)為整合性6軸運(yùn)動(dòng)處理組件MPU6050和3軸電子羅 盤HMC5883L，其中6軸運(yùn)動(dòng)處理組件MPU6050集成了 3軸MEMS陀螺儀、3軸MEMS加速度計(jì) 和一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP(Digital Motion Processor)，并且可通過(guò)I2C接口連 接一個(gè)第三方的數(shù)字傳感器；
[0008] 所述起降甲板由金屬框架和沖孔鋁板組成，并且所述沖孔鋁板上的孔型可以為弧 形角長(zhǎng)方形。
[0009] 優(yōu)選的，所述安裝支架由支撐臂、橫梁和懸臂組成，所述支撐臂下端開有用于將安 裝支架連接到船艇尾部的安裝孔，所述支撐臂通過(guò)所述懸臂連接到船艇。
[0010] 優(yōu)選的，所述安裝支架的上端通過(guò)法蘭盤安裝到所述安裝板的底部。
[0011] 優(yōu)選的，所述兩個(gè)軸承座和所述電機(jī)支架水平分布在同一軸線上。
[0012] 優(yōu)選的，所述第三方的數(shù)字傳感器比如為磁力計(jì)。
[0013] 本發(fā)明還提供了一種用于控制如上所述的艇載飛行器穩(wěn)定起降裝置的方法，所述 方法包括以下步驟：
[0014] (1)系統(tǒng)進(jìn)行初始化，校準(zhǔn)運(yùn)動(dòng)姿態(tài)檢測(cè)單元。
[0015] (2)在預(yù)定的時(shí)間間隔，微處理器控制單元讀取運(yùn)動(dòng)姿態(tài)檢測(cè)單元檢測(cè)到的起降 甲板在X、Y、Z三個(gè)方向的加速度和縱搖角速度數(shù)據(jù)。
[0016] 其中，起降甲板在X、Y、Z三個(gè)方向的加速度記為gnQ(nO = X，y，z)，縱搖角速度記 為：橫搖角速度，縱搖角速度，艏搖角速度ιΓ。
[0017] (3)微處理器控制單元根據(jù)所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元檢測(cè)到的起降甲板在Χ、Υ、Ζ三 個(gè)方向的加速度和縱搖角速度數(shù)據(jù)，計(jì)算起降甲板當(dāng)前的縱搖姿態(tài)角。
[0018] (4)微處理器控制單元根據(jù)步驟（3)計(jì)算得到的起降甲板當(dāng)前的縱搖姿態(tài)角和事 先設(shè)定的水平角作PID控制運(yùn)算得到輸出控制量。
[0019] (5)微處理器控制單元將計(jì)算得到的PID控制量調(diào)制成PffM信號(hào)輸出到起降平臺(tái)。
[0020] (6)起降平臺(tái)根據(jù)接收到的PffM信號(hào)相應(yīng)的使直流減速電機(jī)調(diào)整轉(zhuǎn)向和轉(zhuǎn)速帶動(dòng) 起降甲板旋轉(zhuǎn)，最終使起降甲板達(dá)到水平角度。
[0021] 優(yōu)選的，步驟（3)的具體計(jì)算過(guò)程如下：
[0022] (a)微處理器控制單元根據(jù)公：
.，計(jì)算 得到估計(jì)的橫搖角度Φ:和縱搖角度Θ i，其中，Gx，Gy，Gz分別為所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元檢 測(cè)到的起降甲板在X、Y、Z三個(gè)方向的加速度。
[0023] (b)微處理器控制單元將接收到的陀螺儀的測(cè)量即橫搖角速度Φ '，縱搖角速度 Θ ^，艏搖角速度ιΓ進(jìn)行積分得到角度增量V Θ、V Φ、V Φ。
[0024] (c)微處理器控制單元將通過(guò)步驟（a)的計(jì)算所得到的估計(jì)的橫搖角度t和縱 搖角度S 1與通過(guò)步驟（b)的計(jì)算所得到的角度增量V θ、νφ、V Φ進(jìn)行融合，得到起降平 臺(tái)的縱搖姿態(tài)角，其計(jì)算公式為：gn= gnl*p+gn2*(l-p) {p e (〇，1)}。
[0025] 其中，gn(n = x，y，z) = [ θ φ φ ]為最終計(jì)算得到的起降平臺(tái)姿態(tài)角，gnl(n = X，y，ζ) = [ Θ 1 φι 0]為加速度計(jì)估計(jì)得到的姿態(tài)角，gn2(n = X，y，z) = [V θ νφ νφ ] 為陀螺儀積分得到的姿態(tài)角度增量，其中，P為互補(bǔ)濾波權(quán)重因子。
[0026] 優(yōu)選的，ρ-般取0.95。
[0027] 優(yōu)選的，按照以下公式計(jì)算輸出控制量：
[0028] PffM_0UT = Kp*error+Ki* (error-Last_error)+Kd*(error-2*Last_error+Prev_ error)〇
[0029] 其中，PWM_0UT為輸出控制量，Kp、Ki和Kd分別為比例、積分和微分三項(xiàng)的系數(shù)， error為控制偏差，由當(dāng)前傳感器角度與期望角度做差得到，Last_erro;r和Prev_error分 別為上次誤差和上上次誤差，error、Last_erro;r和Prev_erro;r是在每個(gè)控制周期末尾更 迭得到的，即 Last_erro;r 賦值給 Prev_erro;r，error 賦值給 Last_erro;r。
[0030] 本發(fā)明所提出的艇載飛行器穩(wěn)定起降裝置及其控制裝置專門為艇載飛行器的穩(wěn) 定起降而設(shè)計(jì)，具有結(jié)構(gòu)簡(jiǎn)單、體積小巧、便于實(shí)現(xiàn)等諸多優(yōu)點(diǎn)，能夠極大地提高飛行器起 飛、降落的可靠性，并且與現(xiàn)有的穩(wěn)定平臺(tái)相比，能夠節(jié)約很多成本，具有很好的實(shí)際應(yīng)用 價(jià)值。
【附圖說(shuō)明】
[0031] 圖1為根據(jù)本發(fā)明實(shí)施例的、艇載飛行器穩(wěn)定起降裝置的系統(tǒng)結(jié)構(gòu)圖。
[0032] 圖2為根據(jù)本發(fā)明實(shí)施例的、起降平臺(tái)的正視圖。
[0033] 圖3為根據(jù)本發(fā)明實(shí)施例的、起降平臺(tái)的側(cè)視圖。
[0034] 圖4為根據(jù)本發(fā)明實(shí)施例的、起降甲板的俯視圖。
[0035] 圖5為根據(jù)本發(fā)明實(shí)施例的、控制艇載飛行器穩(wěn)定起降裝置的方法的流程圖。
[0036] 圖6為根據(jù)本發(fā)明實(shí)施例的、計(jì)算輸出控制量的示意圖。
【具體實(shí)施方式】
[0037] 下面參照附圖對(duì)本發(fā)明進(jìn)行更全面的描述，其中說(shuō)明本發(fā)明的示例性實(shí)施例。
[0038] 如圖1所述，本發(fā)明提出的艇載飛行器穩(wěn)定起降裝置包括運(yùn)動(dòng)姿態(tài)檢測(cè)單元11、 微處理器控制單元12和起降平臺(tái)13,所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元11和所述起降平臺(tái)13與所述 微處理器控制單元12電連接。
[0039] 如圖2和圖3所示，所述起降平臺(tái)13包括安裝支架、起降甲板1、直流減速電機(jī)2、 安裝板3和電機(jī)支架5,其中，所述安裝板3置于所述安裝支架上，所述安裝板3上安裝有兩 個(gè)軸承座和所述電機(jī)支架5,所述電機(jī)支架5上安裝有所述直流減速電機(jī)2,所述兩個(gè)軸承 座上連接著旋轉(zhuǎn)軸，所述旋轉(zhuǎn)軸和所述直流減速電機(jī)2的電機(jī)軸通過(guò)兩個(gè)法蘭盤4、6對(duì)接 連接，所述起降甲板1通過(guò)兩個(gè)連接件7、8固定在旋轉(zhuǎn)軸上，其可隨旋轉(zhuǎn)軸的旋轉(zhuǎn)而轉(zhuǎn)動(dòng)。
[0040] 如圖2所示，所述安裝支架由支撐臂10、橫梁9和懸臂14組成，支撐臂10下端開 有用于將安裝支架連接到船艇尾部的安裝孔，支撐臂10通過(guò)懸臂14連接到船艇以加固安 裝支架，安裝支架的上端通過(guò)法蘭盤安裝到安裝板3的底部支撐整個(gè)安裝板3。
[0041] 如圖2所示，所述兩個(gè)軸承座和電機(jī)支架5水平分布在同一軸線上。
[0042] 另外，根據(jù)本發(fā)明的實(shí)施例，所述起降甲板1由金屬框架和沖孔鋁板組成。如圖4 所示，起降甲板1的沖孔鋁板上的孔型可以為弧形角長(zhǎng)方形，以減少風(fēng)阻。
[0043] 通過(guò)以上的描述可知，本發(fā)明所提出的起降平臺(tái)13是專門為艇載飛行器的穩(wěn)定 起降而創(chuàng)造性的設(shè)計(jì)的，該起降平臺(tái)比之當(dāng)下主流的起降平臺(tái)，具有以下優(yōu)點(diǎn)：
[0044] 1、結(jié)構(gòu)合理簡(jiǎn)單，可以實(shí)現(xiàn)在船艇的整體性安裝與拆卸，可以根據(jù)船艇自身是否 需要搭載無(wú)人機(jī)?？靠焖偻瓿纱到y(tǒng)設(shè)計(jì)；
[0045] 2、平衡船艇縱搖，使平臺(tái)始終保持水平，便于無(wú)人機(jī)降落；
[0046] 3、體積小巧，相對(duì)于其他采用液壓、復(fù)雜機(jī)械組成的起降平臺(tái)，重量輕，適用于小 型船；
[0047] 4、適用于垂直起降的各類飛行器，例如多旋翼、直升機(jī)；
[0048] 5、起降甲板采用沖孔鋁板，并且將沖孔鋁板上的孔型創(chuàng)造性地設(shè)計(jì)為弧形角長(zhǎng)方 形，最大限度減小風(fēng)阻影響。
[0049] 另外，根據(jù)本發(fā)明的實(shí)施例，所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元11設(shè)置在所述起降平臺(tái)13的 中心，用于測(cè)量所述起降甲板在X、Y、Z三個(gè)方向的加速度和縱搖角速度，所述運(yùn)動(dòng)姿態(tài)采 集單元13電連接微處理器控制單元12,將采集到的所述起降甲板在X、Y、Z三個(gè)方向的加 速度和縱搖角速度發(fā)送給所述微處理器控制單元12。
[0050] 在船艇行進(jìn)過(guò)程中，當(dāng)自身姿態(tài)在不斷變化的時(shí)候，也會(huì)帶動(dòng)穩(wěn)定平臺(tái)的起降甲 板產(chǎn)生相應(yīng)的姿態(tài)變化，運(yùn)動(dòng)姿態(tài)檢測(cè)單元11采集所述起降甲板在Χ、Υ、Ζ三個(gè)方向的加速 度和縱搖角速度，并將獲得的數(shù)據(jù)傳送給所述微處理器控制單元，經(jīng)過(guò)計(jì)算可以得到起降 甲板的角度。由此可知，運(yùn)動(dòng)姿態(tài)檢測(cè)單元11是本發(fā)明的重要部件，為此，本發(fā)明創(chuàng)造性地 將運(yùn)動(dòng)姿態(tài)檢測(cè)單元11設(shè)計(jì)為整合性6軸運(yùn)動(dòng)處理組件，其集成了 3軸MEMS陀螺儀、3軸 MEMS加速度計(jì)和一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP (Digital Motion Processor)，并且可 通過(guò)I2C接口連接一個(gè)第三方的數(shù)字傳感器，比如磁力計(jì)。拓展之后就可以通過(guò)其I2C或 SPI接口輸出一個(gè)9軸的信號(hào)。相較于現(xiàn)有的多組件方案，本發(fā)明提出的運(yùn)動(dòng)姿態(tài)檢測(cè)單元 11免除了組合陀螺儀與加速器時(shí)之軸間差的問(wèn)題，減少了大量的封裝空間。這也是本發(fā)明 的主要改進(jìn)點(diǎn)之一。
[0051] 作為一款物理傳感器，所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元11的工作原理是利用物理效應(yīng)，諸 如壓電效應(yīng)，磁致伸縮現(xiàn)象，離化、極化、熱電、光電、磁電等效應(yīng)，將被測(cè)信號(hào)量的微小變化 轉(zhuǎn)換成電信號(hào)。數(shù)字運(yùn)動(dòng)處理器DMP引擎可減少?gòu)?fù)雜的融合演算數(shù)據(jù)，以數(shù)字輸出6軸的 旋轉(zhuǎn)矩陣、四元數(shù)（quaternion)、歐拉角格式（Euler Angle forma)的融合演算數(shù)據(jù)，移除 加速器與陀螺儀軸間敏感度，降低設(shè)定給予的影響與感測(cè)器的漂移。用戶可以利用微處理 器控制單元12獲得所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元11輸出的四元數(shù)，進(jìn)行融合計(jì)算之后，便可得到 傳感器的角度。
[0052] 根據(jù)本發(fā)明的實(shí)施例，所述微處理器控制單元12根據(jù)獲得的所述起降甲板在X、 Y、Z三個(gè)方向的加速度和縱搖角速度向所述起降甲板發(fā)送控制信號(hào)。所述升降平臺(tái)13根 據(jù)所述控制信號(hào)控制所述直流減速電機(jī)2的轉(zhuǎn)向和轉(zhuǎn)速，使起降甲板達(dá)到設(shè)定的姿態(tài)角度 (通常設(shè)定成水平姿態(tài)），保證飛行器起降過(guò)程中安全性與穩(wěn)定性。
[0053] 上述實(shí)施例中，微處理器控制單元2可以采用具有Cotex M3內(nèi)核的微處理器 STM32。微處理器控制單元STM32系列芯片是為高性能、低成本、低功耗的嵌入式應(yīng)用專門 設(shè)計(jì)的芯片，增強(qiáng)型系列時(shí)鐘頻率達(dá)到72MHz，是同類產(chǎn)品中性能最高的產(chǎn)品。
[0054] 本發(fā)明還提供了一種用于控制如上文所述的艇載飛行器穩(wěn)定起降裝置的方法，如 圖5所示，所述方法包括以下步驟：
[0055] (1)系統(tǒng)進(jìn)行初始化，校準(zhǔn)運(yùn)動(dòng)姿態(tài)檢測(cè)單元。
[0056] 微處理器初始化I2C通信端口和定時(shí)器PffM輸出功能，微處理器通過(guò)I2C端口初 始化設(shè)置運(yùn)動(dòng)姿態(tài)檢測(cè)單元，校準(zhǔn)運(yùn)動(dòng)姿態(tài)檢測(cè)單元。
[0057] (2)在預(yù)定的時(shí)間間隔，微處理器控制單元讀取運(yùn)動(dòng)姿態(tài)檢測(cè)單元檢測(cè)到的起降 甲板在X、Y、Z三個(gè)方向的加速度和縱搖角速度數(shù)據(jù)。
[0058] 根據(jù)本發(fā)明的一個(gè)實(shí)施例，所述預(yù)定的時(shí)間間隔為5mS，5ms時(shí)間間隔到達(dá)，微處 理器控制單元就讀取運(yùn)動(dòng)姿態(tài)檢測(cè)單元中集成的加速度計(jì)所檢測(cè)的在X、Y、Z三個(gè)方向的 加速度數(shù)據(jù)和陀螺儀檢測(cè)的縱搖角速度數(shù)據(jù)。
[0059] 其中，起降甲板在X、Y、Z三個(gè)方向的加速度記為gnQ(n0 = X，y，z)，縱搖角速度記 為：橫搖角速度，縱搖角速度，艏搖角速度ιΓ。
[0060] (3)微處理器控制單元根據(jù)所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元檢測(cè)到的起降甲板在Χ、Υ、Ζ三 個(gè)方向的加速度和縱搖角速度數(shù)據(jù)，計(jì)算起降甲板當(dāng)前的縱搖姿態(tài)角。
[0061] 其中步驟（3)的具體計(jì)算過(guò)程如下：
[0062] (a)微處理器控制單元根據(jù)公
，計(jì)算 得到估計(jì)的橫搖角度Φ:和縱搖角度Θ i，其中，Gx，Gy，Gz分別為所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元檢 測(cè)到的起降甲板在X、Y、Z三個(gè)方向的加速度，需要說(shuō)明的是，因?yàn)轸紦u角度不影響平臺(tái)的 控制，所以此處不對(duì)艏搖角度Φ進(jìn)行估計(jì)。
[0063] (b)微處理器控制單元將接收到的陀螺儀的測(cè)量即橫搖角速度Φ '，縱搖角速度 Θ ^，艏搖角速度ιΓ進(jìn)行積分得到角度增量V Θ、V Φ、V Φ。
[0064] (c)微處理器控制單元將通過(guò)步驟（a)的計(jì)算所得到的估計(jì)的橫搖角度Φ和縱搖 角度Θ與通過(guò)步驟（b)的計(jì)算所得到的角度增量νθ、νΦ、νΦ進(jìn)行融合，得到起降平臺(tái)的 縱搖姿態(tài)角。
[0065] 由于加速度計(jì)測(cè)得的值中有大量平臺(tái)震蕩所產(chǎn)生的干擾加速度，所以得到的估計(jì) 值包含有噪聲，是不準(zhǔn)確的，需要陀螺儀來(lái)進(jìn)行校正。因?yàn)橥勇輧x本身存在溫度漂移的問(wèn) 題，因此其測(cè)得的值也不能直接反映起降甲板姿態(tài)的真實(shí)情況，需要和加速度計(jì)的值相互 融合，才能得到真實(shí)的姿態(tài)角。常用的融合方式卡爾曼濾波，但是卡爾曼濾波計(jì)算量大， 為其建立穩(wěn)定可靠的更新方程比較困難，對(duì)處理器的運(yùn)算速度和精度要求很到，不適合應(yīng) 用于本發(fā)明實(shí)驗(yàn)裝置。本發(fā)明采用了另外一種姿態(tài)融合方法一一互補(bǔ)濾波算法，互補(bǔ)濾 波算法簡(jiǎn)單可靠，對(duì)姿態(tài)傳感器的精度要求較低，其應(yīng)用越來(lái)越廣泛。其計(jì)算公式為g n = gnl*P+gn2*(l-p) iP ^ (0^ 1)} 〇
[0066] 其中，gn(n = x，y，z) = [ θ φ φ ]為最終計(jì)算得到的起降平臺(tái)姿態(tài)角，gnl(n = X，y，Ζ) = [ Θ 1 Φι 0]為加速度計(jì)估計(jì)得到的姿態(tài)角，gn2(n = X，y，z) = [V θ νφ νφ ] 為陀螺儀積分得到的姿態(tài)角度增量，其中，P為互補(bǔ)濾波權(quán)重因子，一般取〇. 95。
[0067] (4)微處理器控制單元根據(jù)步驟（3)計(jì)算得到的起降甲板當(dāng)前的縱搖姿態(tài)角和事 先設(shè)定的水平角作PID控制運(yùn)算得到輸出控制量。
[0068] 在PID控制策略當(dāng)中，比例調(diào)節(jié)是根據(jù)"偏差的大小"來(lái)動(dòng)作，它的輸出與輸入偏 差的大小成比例，比例調(diào)節(jié)及時(shí)有力但有余差。積分調(diào)節(jié)是根據(jù)"偏差是否存在"來(lái)動(dòng)作， 它的輸出與偏差對(duì)時(shí)間的積分成比例，其作用是消除余差。微分調(diào)節(jié)是根據(jù)"偏差的變化速 度"來(lái)動(dòng)作，它的輸出與偏差的變化速度成比例，其效果是阻止被調(diào)參數(shù)的一切變化，有超 前調(diào)節(jié)作用。
[0069] 本發(fā)明所采用的PID控制原理圖如圖6所示，其中，r(t)為期望角度，y(t)為實(shí)際 角度，e(t)為r(t)與y(t)的角度誤差，u(t)為計(jì)算的到減速電機(jī)轉(zhuǎn)速：
[0070] 其中，按照以下公式計(jì)算輸出控制量：
[0071] PffM_0UT = Kp*error+Ki* (error-Last_error)+Kd*(error-2*Last_error+Prev_ error)〇
[0072] 其中，PWM_0UT為輸出控制量，Kp、Ki和Kd分別為比例、積分和微分三項(xiàng)的系數(shù)， 這三個(gè)系數(shù)主要依靠設(shè)備裝置的實(shí)際參數(shù)進(jìn)行整定。首先設(shè)定一個(gè)Kp系數(shù)的初始值（可 以按照輸出控制量范圍與輸入變化范圍的比值，再乘以十分之一，作為初始參考），通過(guò)改 變給定值對(duì)系統(tǒng)加一個(gè)擾動(dòng)，觀察響應(yīng)曲線，不斷更改系數(shù)值，直到控制系統(tǒng)符合動(dòng)態(tài)過(guò)程 品質(zhì)要求為止，這時(shí)的系數(shù)值即為比較恰當(dāng)?shù)膮?shù)；然后繼續(xù)依照上面的過(guò)程對(duì)其余兩個(gè) 系數(shù)進(jìn)行整定。式中的error為控制偏差，由當(dāng)前傳感器角度與期望角度做差得到，Last_ error和Prev-error分別為上次誤差和上上次誤差，error、Last-error和Prev-error是 在每個(gè)控制周期末尾更迭得到的，即Last_error賦值給Prev_error，error賦值給Last_ error〇
[0073] (5)微處理器控制單元將計(jì)算得到的PID控制量調(diào)制成PffM信號(hào)輸出到起降平臺(tái)。
[0074] (6)起降平臺(tái)根據(jù)接收到的PffM信號(hào)相應(yīng)的使直流減速電機(jī)調(diào)整轉(zhuǎn)向和轉(zhuǎn)速帶動(dòng) 起降甲板旋轉(zhuǎn)，最終使起降甲板達(dá)到水平角度。
[0075] 以上內(nèi)容僅為本發(fā)明的較佳實(shí)施例，對(duì)于本領(lǐng)域的普通技術(shù)人員，依據(jù)本發(fā)明的 思想，在【具體實(shí)施方式】及應(yīng)用范圍上均會(huì)有改變之處，本說(shuō)明書內(nèi)容不應(yīng)理解為對(duì)本發(fā)明 的限制。
【主權(quán)項(xiàng)】
1. 一種艇載飛行器穩(wěn)定起降裝置，其特征在于，所述艇載飛行器穩(wěn)定起降裝置包括運(yùn) 動(dòng)姿態(tài)檢測(cè)單元、微處理器控制單元和起降平臺(tái)，所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元和所述起降平臺(tái) 與所述微處理器控制單元電連接，所述起降平臺(tái)包括起安裝支架、起降甲板、直流減速電 機(jī)、安裝板和電機(jī)支架，其中，所述安裝板置于所述安裝支架上，所述安裝板上安裝有兩個(gè) 軸承座和所述電機(jī)支架，所述電機(jī)支架上安裝有所述直流減速電機(jī)，所述兩個(gè)軸承座上連 接著旋轉(zhuǎn)軸，所述旋轉(zhuǎn)軸和所述直流減速電機(jī)的電機(jī)軸通過(guò)兩個(gè)法蘭盤對(duì)接連接，所述起 降甲板通過(guò)兩個(gè)連接件固定在所述旋轉(zhuǎn)軸上；另外， 所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元被設(shè)計(jì)為整合性6軸運(yùn)動(dòng)處理組件MPU6050和3軸電子羅盤 HMC5883L，其中6軸運(yùn)動(dòng)處理組件MPU6050集成了 3軸MEMS陀螺儀、3軸MEMS加速度計(jì)和 一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP(Digital Motion Processor)，并且可通過(guò)I2C接口連接 一個(gè)第三方的數(shù)字傳感器； 所述起降甲板由金屬框架和沖孔鋁板組成，并且所述沖孔鋁板上的孔型可以為弧形角 長(zhǎng)方形。2. 根據(jù)權(quán)利要求1所述的艇載飛行器穩(wěn)定起降裝置，其特征在于，所述安裝支架由支 撐臂、橫梁和懸臂組成，所述支撐臂下端開有用于將安裝支架連接到船艇尾部的安裝孔，所 述支撐臂通過(guò)所述懸臂連接到船艇。3. 根據(jù)權(quán)利要求2所述的艇載飛行器穩(wěn)定起降裝置，其特征在于，所述安裝支架的上 端通過(guò)法蘭盤安裝到所述安裝板的底部。4. 根據(jù)權(quán)利要求1所述的艇載飛行器穩(wěn)定起降裝置，其特征在于，所述兩個(gè)軸承座和 所述電機(jī)支架水平分布在同一軸線上。5. 根據(jù)權(quán)利要求1所述的艇載飛行器穩(wěn)定起降裝置，所述第三方的數(shù)字傳感器比如為 磁力計(jì)。6. -種用于控制如上述權(quán)利要求1-5中的任意一項(xiàng)所述的艇載飛行器穩(wěn)定起降裝置 的方法，所述方法包括以下步驟： (1) 系統(tǒng)進(jìn)行初始化，校準(zhǔn)運(yùn)動(dòng)姿態(tài)檢測(cè)單元。 (2) 在預(yù)定的時(shí)間間隔，微處理器控制單元讀取運(yùn)動(dòng)姿態(tài)檢測(cè)單元檢測(cè)到的起降甲板 在X、Y、Z三個(gè)方向的加速度和縱搖角速度數(shù)據(jù)。 其中，起降甲板在X、Y、Z三個(gè)方向的加速度記為gn()(nO = X，y，z)，縱搖角速度記為： 橫搖角速度，縱搖角速度Θ '，艏搖角速度φ '。 (3) 微處理器控制單元根據(jù)所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元檢測(cè)到的起降甲板在Χ、Υ、Ζ三個(gè)方 向的加速度和縱搖角速度數(shù)據(jù)，計(jì)算起降甲板當(dāng)前的縱搖姿態(tài)角。 (4) 微處理器控制單元根據(jù)步驟（3)計(jì)算得到的起降甲板當(dāng)前的縱搖姿態(tài)角和事先設(shè) 定的水平角作PID控制運(yùn)算得到輸出控制量。 (5) 微處理器控制單元將計(jì)算得到的PID控制量調(diào)制成PffM信號(hào)輸出到起降平臺(tái)。 (6) 起降平臺(tái)根據(jù)接收到的PffM信號(hào)相應(yīng)的使直流減速電機(jī)調(diào)整轉(zhuǎn)向和轉(zhuǎn)速帶動(dòng)起降 甲板旋轉(zhuǎn)，最終使起降甲板達(dá)到水平角度。7. 根據(jù)權(quán)利要求6所述的方法，其中，步驟（3)的具體計(jì)算過(guò)程如下： (a) 微處理器控制單元根據(jù)公j計(jì)算得到 估計(jì)的橫搖角度Φ:和縱搖角度Θ i，其中，Gx，Gy，&分別為所述運(yùn)動(dòng)姿態(tài)檢測(cè)單元檢測(cè)到 的起降甲板在X、Y、Z三個(gè)方向的加速度。 (b) 微處理器控制單元將接收到的陀螺儀的測(cè)量即橫搖角速度Φ '，縱搖角速度 Θ ^，艏搖角速度ιΓ進(jìn)行積分得到角度增量V Θ、V Φ、V Φ。 (c) 微處理器控制單元將通過(guò)步驟（a)的計(jì)算所得到的估計(jì)的橫搖角度Φ JP縱搖角 度S1與通過(guò)步驟（b)的計(jì)算所得到的角度增量νθ、νΦ、νΦ進(jìn)行融合，得到起降平臺(tái)的 縱搖姿態(tài)角，其計(jì)算公式為：g n= gnl*p+gn2*(l-p) {p e (〇，1)}。 其中，811(11 = 1，7,2) = [0(|)1])]為最終計(jì)算得到的起降平臺(tái)姿態(tài)角，8111(11 = 1，7， ζ) = [ Θ i (J)1 〇]為加速度計(jì)估計(jì)得到的姿態(tài)角，gn2(n = X，y，z) = [V θ νφ νφ ]為陀 螺儀積分得到的姿態(tài)角度增量，其中，P為互補(bǔ)濾波權(quán)重因子。8. 根據(jù)權(quán)利要求6所述的方法，其中，ρ -般取0. 95。9. 根據(jù)權(quán)利要求6所述的方法，其中，按照以下公式計(jì)算輸出控制量： PffM_OUT = Kp*error+Ki*(error-Last_error)+Kd*(error-2*Last_error+Prev_ error)〇 其中，PWM_0UT為輸出控制量，Kp、Ki和Kd分別為比例、積分和微分三項(xiàng)的系數(shù)，error 為控制偏差，由當(dāng)前傳感器角度與期望角度做差得到，Last_erro;r和Prev_erro;r分別為上 次誤差和上上次誤差，error、Last_erro;r和Prev_erro;r是在每個(gè)控制周期末尾更迭得到 的，即 Last_erro;r 賦值給 Prev_erro;r，error 賦值給 Last_erro;r。
【文檔編號(hào)】B64F1/00GK105836151SQ201510688098
【公開日】2016年8月10日
【申請(qǐng)日】2015年10月23日
【發(fā)明人】范云生, 馬建磊, 王國(guó)峰, 柳健, 白鳴, 白一鳴
【申請(qǐng)人】范云生