本發(fā)明實(shí)施例涉及多旋翼自主飛行控制技術(shù)領(lǐng)域，具體地說(shuō)，涉及一種考慮地效的多旋翼自主起降控制方法、考慮地效的多旋翼自主起降控制裝置。

背景技術(shù)：

近些年，多旋翼無(wú)人機(jī)以其簡(jiǎn)單的操作機(jī)理、穩(wěn)定的飛行性能、相比固定翼和直升機(jī)更低廉的價(jià)格而得到越來(lái)越廣泛的應(yīng)用。自主起飛和降落是多旋翼工作的重要階段，也是最容易造成墜機(jī)、側(cè)翻等飛行事故的階段，嚴(yán)重的還可能對(duì)作業(yè)人員造成人身傷害。

在離地高度較小的起降過(guò)程中，地效干擾對(duì)多旋翼的飛行動(dòng)態(tài)造成了嚴(yán)重的影響。盡管從理論上講，地效有助于增加螺旋槳產(chǎn)生的升力，但在許多復(fù)雜的地形環(huán)境和高精度的控制任務(wù)下，地效將大大降低多旋翼自主飛行的性能表現(xiàn)。地效的模型很難精確建立，受風(fēng)干擾、地形凹凸條件、地面硬度等起降環(huán)境影響嚴(yán)重，而這些因素存在許多不確定性，由此給操作員或自主控制器帶來(lái)了諸多挑戰(zhàn)，例如植保作業(yè)時(shí)要求飛行姿態(tài)平穩(wěn)，在移動(dòng)的平臺(tái)上起降時(shí)要求有精確的起降時(shí)間，帶有高精密載荷時(shí)要求軟著陸等，在這些應(yīng)用場(chǎng)合，地效干擾將對(duì)飛行器的控制表現(xiàn)產(chǎn)生嚴(yán)重影響。

技術(shù)實(shí)現(xiàn)要素：

為了解決現(xiàn)有技術(shù)中的上述問(wèn)題，本發(fā)明實(shí)施例提供一種考慮地效的多旋翼自主起降控制方法，以實(shí)現(xiàn)在復(fù)雜地形環(huán)境、高精度起降控制任務(wù)要求下的高性能自主起降控制。此外，本發(fā)明還提供一種考慮地效的多旋翼自主起降控制裝置。

為了實(shí)現(xiàn)上述目的，根據(jù)本發(fā)明的一個(gè)方面，提供以下技術(shù)方案：

一種考慮地效的多旋翼自主起降控制方法，該方法包括：

獲取多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)及上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量；其中，期望參數(shù)包括期望的起降速度；實(shí)時(shí)工作參數(shù)包括：實(shí)時(shí)高度、豎直速度、滾轉(zhuǎn)角和俯仰角；

根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量；

根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)和上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量；

根據(jù)標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量和地效干擾下的自主起降電機(jī)補(bǔ)償控制量，合成出下一時(shí)刻的電機(jī)控制量。

進(jìn)一步地，根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量，具體包括：

根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)以及多旋翼結(jié)構(gòu)參數(shù)，結(jié)合多旋翼標(biāo)稱高度動(dòng)力學(xué)模型，根據(jù)下式計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量：

其中，表示標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量；m表示多旋翼的質(zhì)量；表示期望的起降加速度；k₁表示控制增益，k₁＞0；k₂表示控制增益，k₂＞0；e₁＝z_d-z；z_d表示期望高度；z表示實(shí)時(shí)高度；表示期望的起降速度；v_d表示多旋翼豎直速度；g表示重力加速度常數(shù)；表示推控比的估計(jì)值；表示滾轉(zhuǎn)角；θ表示俯仰角。

進(jìn)一步地，推控比的估計(jì)值通過(guò)以下方式來(lái)確定：

獲取多旋翼進(jìn)行定點(diǎn)懸停飛行時(shí)上一時(shí)刻豎直方向的電機(jī)控制量和多旋翼所受重力；

基于上一時(shí)刻豎直方向的電機(jī)控制量和多旋翼所受重力，根據(jù)下式采用離線辨識(shí)方法計(jì)算推控比的估計(jì)值：

其中，表示推控比估計(jì)值；mg表示多旋翼所受重力；U表示上一時(shí)刻豎直方向的電機(jī)控制量。

進(jìn)一步地，期望的起降加速度通過(guò)以下方式來(lái)計(jì)算：

基于期望的起降速度，根據(jù)預(yù)設(shè)的控制周期，采用一階慣性環(huán)節(jié)構(gòu)成數(shù)字微分器，計(jì)算期望的起降加速度。

進(jìn)一步地，期望高度通過(guò)以下方式來(lái)計(jì)算：

基于期望的起降速度，根據(jù)預(yù)設(shè)的控制周期，采用數(shù)值積分的方法計(jì)算下一時(shí)刻期望的高度。

進(jìn)一步地，根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)和上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量，具體包括：

根據(jù)下式構(gòu)建擴(kuò)張狀態(tài)觀測(cè)器：

其中，表示高度估計(jì)值；表示豎直方向速度估計(jì)值；表示總和干擾量的估計(jì)值；l₁、l₂、l₃表示擴(kuò)張狀態(tài)觀測(cè)器增益，l₁＞0，l₂＞0，l₃＞0；U表示上一時(shí)刻豎直方向的電機(jī)控制量；表示推控比估計(jì)值；表示滾轉(zhuǎn)角；θ表示俯仰角；z表示實(shí)時(shí)高度；m表示多旋翼的質(zhì)量；g表示重力加速度常數(shù)；

利用總和干擾量的估計(jì)值，根據(jù)下式計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量：

其中，U_com表示地效干擾下的自主起降電機(jī)補(bǔ)償控制量。

進(jìn)一步地，根據(jù)標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量和地效干擾下的自主起降電機(jī)補(bǔ)償控制量，合成出下一時(shí)刻的電機(jī)控制量，具體包括：

根據(jù)下式確定下一時(shí)刻的電機(jī)控制量：

其中，U^*表示下一時(shí)刻的電機(jī)控制量；表示標(biāo)稱狀態(tài)下自主起降電機(jī)控制量；c表示補(bǔ)償控制量權(quán)系數(shù)；U_com表示地效干擾下的自主起降電機(jī)補(bǔ)償控制量。

進(jìn)一步地，補(bǔ)償控制量權(quán)系數(shù)根據(jù)以下方式來(lái)確定：

當(dāng)實(shí)時(shí)高度小于預(yù)定閾值時(shí)，將補(bǔ)償控制量權(quán)系數(shù)設(shè)定為1；

當(dāng)實(shí)時(shí)高度大于預(yù)定閾值時(shí)，將補(bǔ)償控制量權(quán)系數(shù)設(shè)定為0。

為了實(shí)現(xiàn)上述目的，根據(jù)本發(fā)明的再一個(gè)方面，還提供了以下技術(shù)方案：

一種考慮地效的多旋翼自主起降控制裝置，該裝置可以包括：

獲取模塊，用于獲取多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)及上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量；其中，期望參數(shù)包括期望的起降速度；實(shí)時(shí)工作參數(shù)包括：實(shí)時(shí)高度、豎直速度、滾轉(zhuǎn)角和俯仰角；

標(biāo)稱控制量計(jì)算模塊，用于根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量；

補(bǔ)償控制量計(jì)算模塊，用于根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)和上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量；

控制量合成模塊，用于根據(jù)標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量和地效干擾下的自主起降電機(jī)補(bǔ)償控制量，合成出下一時(shí)刻的電機(jī)控制量。

進(jìn)一步地，標(biāo)稱控制量計(jì)算模塊具體用于根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)以及多旋翼結(jié)構(gòu)參數(shù)，結(jié)合多旋翼標(biāo)稱高度動(dòng)力學(xué)模型，根據(jù)下式計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量：

其中，表示標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量；m表示多旋翼的質(zhì)量；表示期望的起降加速度；k₁表示控制增益，k₁＞0；k₂表示控制增益，k₂＞0；e₁＝z_d-z；z_d表示期望高度；z表示實(shí)時(shí)高度；表示期望的起降速度；v_d表示多旋翼豎直速度；g表示重力加速度常數(shù)；表示推控比的估計(jì)值；表示滾轉(zhuǎn)角；θ表示俯仰角。

進(jìn)一步地，標(biāo)稱控制量計(jì)算模塊還用于通過(guò)以下方式來(lái)確定推控比的估計(jì)值：

獲取多旋翼進(jìn)行定點(diǎn)懸停飛行時(shí)上一時(shí)刻豎直方向的電機(jī)控制量和多旋翼所受重力；

基于上一時(shí)刻豎直方向的電機(jī)控制量和多旋翼所受重力，根據(jù)下式采用離線辨識(shí)方法計(jì)算推控比的估計(jì)值：

其中，表示推控比估計(jì)值；mg表示多旋翼所受重力；U表示上一時(shí)刻豎直方向的電機(jī)控制量。

進(jìn)一步地，標(biāo)稱控制量計(jì)算模塊還用于通過(guò)以下方式來(lái)確定期望的起降加速度：

基于期望起降速度，根據(jù)預(yù)設(shè)的控制周期，采用一階慣性環(huán)節(jié)構(gòu)成數(shù)字微分器，計(jì)算期望的起降加速度。

進(jìn)一步地，標(biāo)稱控制量計(jì)算模塊還用于通過(guò)以下方式來(lái)確定期望高度：

基于期望起降速度，根據(jù)預(yù)設(shè)的控制周期，采用數(shù)值積分的方法計(jì)算下一時(shí)刻期望的高度。

進(jìn)一步地，補(bǔ)償控制量計(jì)算模塊具體用于：

根據(jù)下式構(gòu)建擴(kuò)張狀態(tài)觀測(cè)器：

其中，表示高度估計(jì)值；表示豎直方向速度估計(jì)值；表示總和干擾量的估計(jì)值；l₁、l₂、l₃表示擴(kuò)張狀態(tài)觀測(cè)器增益，l₁＞0，l₂＞0，l₃＞0；U表示上一時(shí)刻豎直方向的電機(jī)控制量；表示推控比估計(jì)值；表示滾轉(zhuǎn)角；θ表示俯仰角；z表示實(shí)時(shí)高度；m表示多旋翼的質(zhì)量；g表示重力加速度常數(shù)；

利用總和干擾量的估計(jì)值，根據(jù)下式計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量：

其中，U_com表示地效干擾下的自主起降電機(jī)補(bǔ)償控制量。

進(jìn)一步地，控制量合成模塊具體用于根據(jù)下式確定下一時(shí)刻的電機(jī)控制量：

其中，U^*表示下一時(shí)刻的電機(jī)控制量；表示標(biāo)稱狀態(tài)下自主起降電機(jī)控制量；c表示補(bǔ)償控制量權(quán)系數(shù)；U_com表示地效干擾下的自主起降電機(jī)補(bǔ)償控制量。

進(jìn)一步地，控制量合成模塊通過(guò)以下方式來(lái)確定補(bǔ)償控制量權(quán)系數(shù)：

當(dāng)實(shí)時(shí)高度小于預(yù)定閾值時(shí)，將補(bǔ)償控制量權(quán)系數(shù)設(shè)定為1；

當(dāng)實(shí)時(shí)高度大于預(yù)定閾值時(shí)，將補(bǔ)償控制量權(quán)系數(shù)設(shè)定為0。

本發(fā)明實(shí)施例提供一種考慮地效的多旋翼自主起降控制方法及考慮地效的多旋翼自主起降控制裝置。其中，該多旋翼自主起降控制方法可以包括獲取多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)及上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量；其中，期望參數(shù)包括期望的起降速度；實(shí)時(shí)工作參數(shù)包括：實(shí)時(shí)高度、豎直速度、滾轉(zhuǎn)角和俯仰角；根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量；根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)和上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量；根據(jù)標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量和地效干擾下的自主起降電機(jī)補(bǔ)償控制量，合成出下一時(shí)刻的電機(jī)控制量。本發(fā)明實(shí)施例針對(duì)未建模的地效干擾，通過(guò)計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量，將多旋翼在離地高度較小的起降過(guò)程中受到的地效干擾估計(jì)出來(lái)并進(jìn)行主動(dòng)補(bǔ)償控制，再將標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量和地效干擾下的自主起降電機(jī)補(bǔ)償控制量進(jìn)行合成，從而得到電機(jī)控制量；由此，實(shí)現(xiàn)了在復(fù)雜地形環(huán)境、高精度起降控制任務(wù)要求下的高性能自主起降控制。這樣還增強(qiáng)了起降飛行的穩(wěn)定性和操作體驗(yàn)，降低了不確定地效干擾帶來(lái)的起降過(guò)程中飛行器側(cè)翻、墜機(jī)甚至造成人身傷害等風(fēng)險(xiǎn)。

附圖說(shuō)明

圖1是根據(jù)本發(fā)明實(shí)施例的考慮地效的多旋翼自主起降控制方法的流程示意圖；

圖2是根據(jù)本發(fā)明實(shí)施例的考慮地效干擾的多旋翼自主起降電機(jī)控制量計(jì)算流程示意圖；

圖3是根據(jù)本發(fā)明實(shí)施例的考慮地效的多旋翼自主起降控制裝置的結(jié)構(gòu)示意圖；

圖4是根據(jù)本發(fā)明另一實(shí)施例的考慮地效的多旋翼自主起降控制裝置的結(jié)構(gòu)示意圖；

圖5是根據(jù)本發(fā)明實(shí)施例的考慮地效的多旋翼自主起降控制裝置的應(yīng)用示意圖。

具體實(shí)施方式

下面參照附圖來(lái)描述本發(fā)明的優(yōu)選實(shí)施方式。本領(lǐng)域技術(shù)人員應(yīng)當(dāng)理解的是，這些實(shí)施方式僅僅用于解釋本發(fā)明的技術(shù)原理，并非旨在限制本發(fā)明的保護(hù)范圍。

本發(fā)明實(shí)施例的基本思想是考慮未建模的、含有多種不確定性的地效干擾，采用主動(dòng)控制的方式為多旋翼自主起降過(guò)程設(shè)計(jì)標(biāo)稱控制器和擴(kuò)張狀態(tài)觀測(cè)器，并采用觀測(cè)出來(lái)的地效干擾設(shè)計(jì)補(bǔ)償控制器，從而分別計(jì)算豎直方向的電機(jī)標(biāo)稱控制量和抗地效干擾補(bǔ)償控制量，以實(shí)現(xiàn)在復(fù)雜地形環(huán)境、高精度起降控制任務(wù)要求下的高性能自主起降控制。

本發(fā)明實(shí)施例提出一種考慮地效的多旋翼自主起降控制方法。如圖1所示，該方法可以包括：

S100：獲取多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)及上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量。其中，期望參數(shù)包括期望的起降速度；實(shí)時(shí)工作參數(shù)包括：實(shí)時(shí)高度、豎直速度、滾轉(zhuǎn)角和俯仰角。

在實(shí)際應(yīng)用中，本步驟既可以分時(shí)也可以同時(shí)讀取期望的多旋翼的起降速度、實(shí)時(shí)高度、豎直速度、滾轉(zhuǎn)角、俯仰角以及上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量。其中，實(shí)時(shí)高度、豎直速度、滾轉(zhuǎn)角和俯仰角可以通過(guò)傳感器實(shí)時(shí)獲取。

S110：根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量。

其中，多旋翼結(jié)構(gòu)參數(shù)包括但不限于多旋翼的質(zhì)量和推控比。推控比為多旋翼豎直方向產(chǎn)生的向上的升力與豎直方向上所給的電機(jī)控制量之間的比值。

具體地，本步驟可以包括：

根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)以及多旋翼結(jié)構(gòu)參數(shù)，結(jié)合多旋翼標(biāo)稱高度動(dòng)力學(xué)模型，根據(jù)下式計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量：

其中，表示標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量；m表示多旋翼的質(zhì)量；表示期望的起降加速度(即下一時(shí)刻期望的起降加速度)；k₁表示控制增益，k₁＞0；k₂表示控制增益，k₂＞0；e₁＝z_d-z；z_d表示期望高度(即下一時(shí)刻期望的高度)；z表示實(shí)時(shí)高度；表示期望的起降速度；v_d表示多旋翼豎直速度；g表示重力加速度常數(shù)；表示推控比的估計(jì)值；表示滾轉(zhuǎn)角；θ表示俯仰角。

上述多旋翼標(biāo)稱高度動(dòng)力學(xué)模型可以根據(jù)下式來(lái)建立：

其中，表示z的二階導(dǎo)數(shù)，即豎直方向的加速度；U表示上一時(shí)刻豎直方向的電機(jī)控制量。

上述多旋翼標(biāo)稱高度動(dòng)力學(xué)模型不考慮任何干擾和建模誤差。鑒于此，本發(fā)明實(shí)施例設(shè)計(jì)反饋控制律，來(lái)計(jì)算標(biāo)稱狀態(tài)下自主起降電機(jī)控制量。

上述控制增益在大于0的情況下可以保證得到標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量。在具體實(shí)施過(guò)程中，控制增益可以根據(jù)實(shí)際情況，并考慮不同的系統(tǒng)參數(shù)以及多旋翼特性在調(diào)試中進(jìn)行參數(shù)整定。例如：一個(gè)自重1公斤的四旋翼和一個(gè)自重2公斤同時(shí)布局不同的四旋翼在考慮控制增益時(shí)，本領(lǐng)域技術(shù)人員可以根據(jù)具體情況進(jìn)行分析和有限的實(shí)驗(yàn)(其中包括但不限于反復(fù)試湊方法)即可整定得到，在此不再贅述。

在一個(gè)優(yōu)選的實(shí)施例中，通過(guò)以下方式來(lái)確定推控比的估計(jì)值：

步驟A1：獲取多旋翼進(jìn)行定點(diǎn)懸停飛行時(shí)上一時(shí)刻豎直方向的電機(jī)控制量和多旋翼所受重力。

步驟A2：基于上一時(shí)刻豎直方向的電機(jī)控制量和多旋翼所受重力，根據(jù)下式采用離線辨識(shí)方法估計(jì)推控比：

式中，表示推控比的估計(jì)值；mg表示多旋翼所受重力；U表示上一時(shí)刻豎直方向的電機(jī)控制量。

在實(shí)際應(yīng)用中，可以首先通過(guò)遙控操作使多旋翼進(jìn)行定點(diǎn)懸停飛行，記錄此時(shí)所給的控制量和多旋翼實(shí)際所受重力，通過(guò)離線辨識(shí)方法獲得推控比的估計(jì)值將該估計(jì)值用于在線控制量的計(jì)算。

在一個(gè)優(yōu)選的實(shí)施例中，通過(guò)以下方式來(lái)計(jì)算期望的起降加速度：

基于期望起降速度，根據(jù)預(yù)設(shè)的控制周期，采用一階慣性環(huán)節(jié)構(gòu)成數(shù)字微分器，計(jì)算出下一時(shí)刻期望的起降加速度。

在一個(gè)優(yōu)選的實(shí)施例中，通過(guò)以下方式來(lái)計(jì)算期望高度：

基于期望起降速度，根據(jù)預(yù)設(shè)的控制周期，采用數(shù)值積分的方法計(jì)算出下一時(shí)刻期望的高度。

S120：根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)和上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量。

本步驟在計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量時(shí)，考慮如下受擾動(dòng)的多旋翼高度動(dòng)力學(xué)模型：

其中，Δ表示總和干擾量，其代表所有建模誤差和外界擾動(dòng)的總和。示例性地，總和干擾量可以包括離線辨識(shí)推控比時(shí)產(chǎn)生的建模誤差、地效干擾產(chǎn)生的影響以及其他未知干擾。其中，在離地飛行距離較小時(shí)，地效干擾占主導(dǎo)作用。對(duì)此，本發(fā)明實(shí)施例通過(guò)構(gòu)造擴(kuò)張狀態(tài)觀測(cè)器來(lái)進(jìn)行估計(jì)。

具體地，本步驟可以通過(guò)步驟S121至步驟S122來(lái)實(shí)現(xiàn)。

S121：根據(jù)下式構(gòu)建擴(kuò)張狀態(tài)觀測(cè)器：

其中，表示高度估計(jì)值；表示豎直方向速度估計(jì)值；表示總和干擾量的估計(jì)值；l₁、l₂、l₃表示擴(kuò)張狀態(tài)觀測(cè)器增益，l₁＞0，l₂＞0，l₃＞0；U表示上一時(shí)刻豎直方向的電機(jī)控制量。

上述擴(kuò)張狀態(tài)觀測(cè)器增益大于0就可以實(shí)現(xiàn)擴(kuò)張狀態(tài)觀測(cè)器的構(gòu)建。在具體實(shí)施過(guò)程中，擴(kuò)張狀態(tài)觀測(cè)器增益可以根據(jù)實(shí)際情況，并考慮不同的系統(tǒng)參數(shù)以及多旋翼特性在調(diào)試中進(jìn)行參數(shù)整定。例如：一個(gè)自重1公斤的四旋翼和一個(gè)自重2公斤同時(shí)布局不同的四旋翼在考慮擴(kuò)張狀態(tài)觀測(cè)器增益時(shí)，本領(lǐng)域技術(shù)人員可以根據(jù)具體情況進(jìn)行分析和有限的實(shí)驗(yàn)(其中包括但不限于反復(fù)試湊方法)即可整定得到，在此不再贅述。

S122：利用總和干擾量的估計(jì)值，根據(jù)下式計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量：

其中，U_com表示地效干擾下的自主起降電機(jī)補(bǔ)償控制量；表示總和干擾量的估計(jì)值；m表示多旋翼的質(zhì)量；表示推控比估計(jì)值；表示滾轉(zhuǎn)角；θ表示俯仰角。

S130：根據(jù)標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量和地效干擾下的自主起降電機(jī)補(bǔ)償控制量，合成出下一時(shí)刻的電機(jī)控制量。

具體地，本步驟可以根據(jù)下式確定下一時(shí)刻的電機(jī)控制量：

其中，U^*表示下一時(shí)刻的電機(jī)控制量；表示標(biāo)稱狀態(tài)下自主起降電機(jī)控制量；c表示補(bǔ)償控制量權(quán)系數(shù)；U_com表示地效干擾下的自主起降電機(jī)補(bǔ)償控制量。

在一個(gè)優(yōu)選的實(shí)施例中，可以根據(jù)以下方式來(lái)確定補(bǔ)償控制量權(quán)系數(shù)：

步驟B1：當(dāng)實(shí)時(shí)高度小于預(yù)定閾值時(shí)，將補(bǔ)償控制量權(quán)系數(shù)設(shè)定為1。

步驟B2：當(dāng)實(shí)時(shí)高度大于預(yù)定閾值時(shí)，將補(bǔ)償控制量權(quán)系數(shù)設(shè)定為0。

在實(shí)際應(yīng)用中，本發(fā)明實(shí)施例可以重復(fù)上述步驟S100至步驟S130，計(jì)算出下一個(gè)控制周期內(nèi)的電機(jī)控制量。

本發(fā)明實(shí)施例針對(duì)未建模的地效干擾，通過(guò)采用標(biāo)稱電機(jī)控制量，采用擴(kuò)張狀態(tài)觀測(cè)器，估計(jì)干擾并計(jì)算補(bǔ)償電機(jī)控制量及地效干擾下的自主起降電機(jī)補(bǔ)償控制量，進(jìn)行加權(quán)合成得出下一時(shí)刻的豎直方向電機(jī)控制量，實(shí)現(xiàn)了在復(fù)雜地形環(huán)境、高精度起降控制任務(wù)要求下的高性能自主起降控制。

下面結(jié)合圖2以一優(yōu)選實(shí)施例來(lái)詳細(xì)說(shuō)明考慮地效干擾的多旋翼自主起降電機(jī)控制量計(jì)算流程。該流程可以包括如下步驟：

步驟21：根據(jù)讀取到的期望起降速度進(jìn)行指令預(yù)處理。

具體地，本步驟可以包括：根據(jù)預(yù)設(shè)的控制周期，采用一階慣性環(huán)節(jié)構(gòu)成數(shù)字微分器，計(jì)算出下一時(shí)刻期望的起降加速度，并采用數(shù)值積分的方法計(jì)算出下一時(shí)刻期望的高度。

步驟22：根據(jù)期望高度、期望起降速度與從傳感器讀取到的多旋翼實(shí)時(shí)高度、豎直速度作差、滾轉(zhuǎn)角、俯仰角以及計(jì)算得到的期望起降加速度，計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量。

步驟23：計(jì)算得到考慮地效干擾的自主起降電機(jī)補(bǔ)償控制量。

步驟24：將標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量和地效干擾的自主起降電機(jī)補(bǔ)償控制量加權(quán)求和，得到豎直方向電機(jī)控制量。

步驟25：利用豎直方向電機(jī)控制量用以控制受地效干擾所擾動(dòng)過(guò)的多旋翼高度動(dòng)力學(xué)模型，進(jìn)而控制多旋翼的電機(jī)轉(zhuǎn)速，從而實(shí)現(xiàn)多旋翼的自主起降控制。

上述實(shí)施例中雖然將各個(gè)步驟按照上述先后次序的方式進(jìn)行了描述，但是本領(lǐng)域技術(shù)人員可以理解，為了實(shí)現(xiàn)本實(shí)施例的效果，不同的步驟之間不必按照這樣的次序執(zhí)行，其可以同時(shí)(并行)執(zhí)行或以顛倒的次序執(zhí)行，這些簡(jiǎn)單的變化都在本發(fā)明的保護(hù)范圍之內(nèi)。

基于與上述方法實(shí)施例相同的技術(shù)構(gòu)思，本發(fā)明實(shí)施例還提供一種考慮地效的多旋翼自主起降控制裝置。該多旋翼自主起降控制裝置可以執(zhí)行上述多旋翼自主起降控制方法實(shí)施例。該裝置30包括：獲取模塊32、標(biāo)稱控制量計(jì)算模塊34、補(bǔ)償控制量計(jì)算模塊36和控制量合成模塊38。其中，獲取模塊32用于獲取多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)及上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量；其中，期望參數(shù)包括期望的起降速度；實(shí)時(shí)工作參數(shù)包括：實(shí)時(shí)高度、豎直速度、滾轉(zhuǎn)角和俯仰角。標(biāo)稱控制量計(jì)算模塊34用于根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量。補(bǔ)償控制量計(jì)算模塊36用于根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)和上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量以及多旋翼結(jié)構(gòu)參數(shù)，計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量?？刂屏亢铣赡K38用于根據(jù)標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量和地效干擾下的自主起降電機(jī)補(bǔ)償控制量，合成出下一時(shí)刻的電機(jī)控制量。

在一些可選的實(shí)施例中，上述標(biāo)稱控制量計(jì)算模塊具體用于根據(jù)多旋翼的期望參數(shù)、實(shí)時(shí)工作參數(shù)以及多旋翼結(jié)構(gòu)參數(shù)，結(jié)合多旋翼標(biāo)稱高度動(dòng)力學(xué)模型，根據(jù)下式計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量：

其中，表示標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量；m表示多旋翼的質(zhì)量；表示期望的起降加速度；k₁表示控制增益，k₁＞0；k₂表示控制增益，k₂＞0；e₁＝z_d-z；z_d表示期望高度；z表示實(shí)時(shí)高度；表示期望的起降速度；v_d表示多旋翼豎直速度；g表示重力加速度常數(shù)；表示推控比的估計(jì)值；表示滾轉(zhuǎn)角；θ表示俯仰角。

在一些可選的實(shí)施例中，上述標(biāo)稱控制量計(jì)算模塊還用于通過(guò)以下方式來(lái)確定推控比的估計(jì)值：

獲取多旋翼進(jìn)行定點(diǎn)懸停飛行時(shí)上一時(shí)刻豎直方向的電機(jī)控制量和多旋翼所受重力；

基于上一時(shí)刻豎直方向的電機(jī)控制量和多旋翼所受重力，根據(jù)下式采用離線辨識(shí)方法計(jì)算推控比的估計(jì)值：

其中，表示推控比估計(jì)值；mg表示多旋翼所受重力；U表示上一時(shí)刻豎直方向的電機(jī)控制量。

在一些可選的實(shí)施例中，上述標(biāo)稱控制量計(jì)算模塊還用于通過(guò)以下方式來(lái)確定期望的起降加速度：

基于期望起降速度，根據(jù)預(yù)設(shè)的控制周期，采用一階慣性環(huán)節(jié)構(gòu)成數(shù)字微分器，計(jì)算期望的起降加速度。

在一些可選的實(shí)施例中，上述標(biāo)稱控制量計(jì)算模塊還用于通過(guò)以下方式來(lái)確定期望高度：

基于期望起降速度，根據(jù)預(yù)設(shè)的控制周期，采用數(shù)值積分的方法計(jì)算下一時(shí)刻期望的高度。

在一些可選的實(shí)施例中，上述補(bǔ)償控制量計(jì)算模塊具體用于：

根據(jù)下式構(gòu)建擴(kuò)張狀態(tài)觀測(cè)器：

其中，表示高度估計(jì)值；表示豎直方向速度估計(jì)值；表示總和干擾量的估計(jì)值；l₁、l₂、l₃表示擴(kuò)張狀態(tài)觀測(cè)器增益，l₁＞0，l₂＞0，l₃＞0；U表示上一時(shí)刻豎直方向的電機(jī)控制量；表示推控比估計(jì)值；表示滾轉(zhuǎn)角；θ表示俯仰角；z表示實(shí)時(shí)高度；m表示多旋翼的質(zhì)量；g表示重力加速度常數(shù)；

利用總和干擾量的估計(jì)值，根據(jù)下式計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量：

其中，U_com表示地效干擾下的自主起降電機(jī)補(bǔ)償控制量。

在一些可選的實(shí)施例中，控制量合成模塊具體用于根據(jù)下式確定下一時(shí)刻的電機(jī)控制量：

其中，U^*表示下一時(shí)刻的電機(jī)控制量；表示標(biāo)稱狀態(tài)下自主起降電機(jī)控制量；c表示補(bǔ)償控制量權(quán)系數(shù)；U_com表示地效干擾下的自主起降電機(jī)補(bǔ)償控制量。

在一些可選的實(shí)施例中，上述控制量合成模塊通過(guò)以下方式來(lái)確定補(bǔ)償控制量權(quán)系數(shù)：

當(dāng)實(shí)時(shí)高度小于預(yù)定閾值時(shí)，將補(bǔ)償控制量權(quán)系數(shù)設(shè)定為1；

當(dāng)實(shí)時(shí)高度大于預(yù)定閾值時(shí)，將補(bǔ)償控制量權(quán)系數(shù)設(shè)定為0。

下面結(jié)合圖4，來(lái)對(duì)本發(fā)明實(shí)施例提出的考慮地效的多旋翼自主起降控制裝置進(jìn)行詳細(xì)說(shuō)明。如圖4所示，該多旋翼自主起降控制裝置可以包括期望值讀取模塊41、傳感器讀取模塊42、控制量讀取模塊43、標(biāo)稱控制量計(jì)算模塊44、補(bǔ)償控制量計(jì)算模塊45和控制量合成模塊46。其中，期望值讀取模塊41用于讀取期望的多旋翼起降速度，并根據(jù)預(yù)設(shè)的控制周期，計(jì)算出下一時(shí)刻期望的起降加速度和高度。傳感器讀取模塊42用于實(shí)時(shí)獲取多旋翼的高度、豎直速度、滾轉(zhuǎn)角和俯仰角?？刂屏孔x取模塊43用于讀取上一時(shí)刻多旋翼在豎直方向的電機(jī)控制量。標(biāo)稱控制量計(jì)算模塊44用于根據(jù)期望值讀取模塊41和傳感器讀取模塊42讀取到的數(shù)據(jù)，以及多旋翼的結(jié)構(gòu)參數(shù)，計(jì)算標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量。補(bǔ)償控制量計(jì)算模塊45用于根據(jù)期望值讀取模塊41、傳感器讀取模塊42、控制量讀取模塊43讀取到的數(shù)據(jù)，以及多旋翼的結(jié)構(gòu)參數(shù)，構(gòu)建狀態(tài)估計(jì)器，將地效干擾量估計(jì)出來(lái)，并利用地效干擾的估計(jì)值來(lái)計(jì)算地效干擾下的自主起降電機(jī)補(bǔ)償控制量?？刂屏亢铣赡K46用于將標(biāo)稱控制量計(jì)算模塊44計(jì)算出的標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量和補(bǔ)償控制量計(jì)算模塊45計(jì)算出的地效干擾下的自主起降電機(jī)補(bǔ)償控制量，以加權(quán)相加的方式合成為下一時(shí)刻的總電機(jī)控制量，還用于給多旋翼的高度設(shè)定一個(gè)閾值，當(dāng)高度低于這個(gè)閾值時(shí)，補(bǔ)償控制量切入起作用，當(dāng)高度高于該閾值時(shí)，補(bǔ)償控制量改出不起作用，從而控制多旋翼電機(jī)的轉(zhuǎn)速。其中，多旋翼基本結(jié)構(gòu)參數(shù)包括多旋翼的質(zhì)量和推控比，推控比為多旋翼豎直方向產(chǎn)生的向上的升力與豎直方向上所給的電機(jī)控制量之間的比值。根據(jù)多旋翼定點(diǎn)懸停時(shí)所給的控制量和多旋翼實(shí)際所受重力，通過(guò)離線辨識(shí)方法獲得推控比的估計(jì)值。

下面結(jié)合圖5，再以一優(yōu)選實(shí)施方式對(duì)本發(fā)明實(shí)施例提出的多旋翼自主起降控制裝置的應(yīng)用進(jìn)行詳細(xì)說(shuō)明。

如圖5所示，51為控制指令給定裝置，其包括遙控器或地面站，用以給出期望的起降速度，優(yōu)選的，遙控器可以選用Futaba T14SG遙控器，地面站可以采用250mW的915M數(shù)傳與多旋翼通信。52為傳感裝置，其包括IMU(三軸加速度計(jì)和三軸陀螺儀)、磁羅盤(pán)、GPS和氣壓高度計(jì)，各種傳感器通過(guò)擴(kuò)展卡爾曼濾波方法給出多旋翼的實(shí)時(shí)高度、豎直速度、滾轉(zhuǎn)角、俯仰角，優(yōu)選的，可以采用MPU6050IMU模塊，并選用LSM303D三軸磁強(qiáng)計(jì)、MS5611氣壓高度計(jì)和UBLOX M8N GPS模塊來(lái)實(shí)現(xiàn)。53為本發(fā)明實(shí)施例提出的考慮地效的多旋翼自主起降控制裝置，其通過(guò)將標(biāo)稱狀態(tài)下的自主起降電機(jī)控制量和地效干擾下的自主起降電機(jī)補(bǔ)償控制量加權(quán)合成，得到最終的豎直方向電機(jī)控制量，用以控制多旋翼電機(jī)54。

經(jīng)實(shí)驗(yàn)證明，本發(fā)明實(shí)施例提出的考慮地效的多旋翼自主起降控制方法和考慮地效的多旋翼自主起降控制裝置能有效抵抗地效干擾，增加自主起降過(guò)程中的姿態(tài)平穩(wěn)性，提升操作體驗(yàn)和飛行安全性。

本領(lǐng)域技術(shù)人員可以理解，上述多旋翼自主起降控制裝置和多旋翼的電機(jī)轉(zhuǎn)速控制系統(tǒng)還可以包括一些其他的公知結(jié)構(gòu)，例如處理器、控制器、存儲(chǔ)器和總線等，其中，存儲(chǔ)器包括但不限于隨機(jī)存儲(chǔ)器、閃存、可編程只讀存儲(chǔ)器、易失性存儲(chǔ)器、非易失性存儲(chǔ)器、串行存儲(chǔ)器、并行存儲(chǔ)器或寄存器等，處理器包括但不限于單核處理器、多核處理器、基于X86架構(gòu)的處理器、CPLD/FPGA、DSP、ARM處理器、MIPS處理器等，總線可以包括數(shù)據(jù)總線、地址總線和控制總線。為了不必要地模糊本公開(kāi)的實(shí)施例，這些公知的結(jié)構(gòu)未在圖3-5中示出。

應(yīng)指出的是，上面分別對(duì)本發(fā)明的多旋翼自主起降控制方法考慮地效的多旋翼自主起降控制裝置實(shí)施例進(jìn)行了描述，但是對(duì)一個(gè)實(shí)施例描述的細(xì)節(jié)也可應(yīng)用于另一個(gè)實(shí)施例。

以上對(duì)本發(fā)明的示例實(shí)施例的詳細(xì)描述是為了說(shuō)明和描述的目的而提供。不是為了窮盡或?qū)⒈景l(fā)明限制為所描述的精確形式。顯然，許多變型和改變對(duì)本領(lǐng)域技術(shù)人員而言是顯而易見(jiàn)的。實(shí)施例的選擇和描述是為了最佳地說(shuō)明本發(fā)明的原理及其實(shí)際應(yīng)用，從而使本領(lǐng)域其他技術(shù)人員能夠理解本發(fā)明的各種實(shí)施例和適于特定使用預(yù)期的各種變型。本發(fā)明的實(shí)施例可以省略上述技術(shù)特征中的一些技術(shù)特征，僅解決現(xiàn)有技術(shù)中存在的部分技術(shù)問(wèn)題。而且，所描述的技術(shù)特征可以進(jìn)行任意組合。本發(fā)明的保護(hù)范圍由所附權(quán)利要求及其等價(jià)物來(lái)限定，本領(lǐng)域技術(shù)其他人員可以對(duì)所附權(quán)利要求中所描述的技術(shù)方案進(jìn)行各種變型或替換和組合，這些更改或替換之后的技術(shù)方案都將落入本發(fā)明的保護(hù)范圍之內(nèi)。