本發(fā)明涉及無人機(jī)技術(shù)及云臺控制技術(shù),尤其是涉及一種基于圖像角度測算系統(tǒng)。
背景技術(shù):
土地是立國之根本,在國民經(jīng)濟(jì)發(fā)展和基礎(chǔ)設(shè)施建設(shè)中占有十分重要的地位。隨著國民經(jīng)濟(jì)的快速發(fā)展,我國政府更是迫切需要一種技術(shù)手段能夠精確高效地完成土地信息的采集和繪制。目前我國城鎮(zhèn)土地調(diào)查主要使用第二次全國土地調(diào)查的GIS系統(tǒng)、GPS-RTK系統(tǒng)與慣導(dǎo)組合和光學(xué)全站儀。原有的偵測手段都有靈活機(jī)動性不足、精度不夠和受地形地貌的影響因素,影響土地調(diào)查的效率。
目前軍用無人機(jī)的研發(fā)已經(jīng)逐漸進(jìn)入到了實(shí)戰(zhàn)測試階段,相比于軍用無人機(jī)續(xù)航里程遠(yuǎn)、載荷大、電子對抗性強(qiáng)等特點(diǎn),民用無人機(jī)要求低空作業(yè)、輕型小型便于操控、價(jià)格低廉、故障損毀后造成的損失較小等特點(diǎn),因此民用無人機(jī)應(yīng)用于商業(yè)市場上有著廣闊的前景。目前超小型無人機(jī)的機(jī)載云臺系統(tǒng)具有特殊的要求,例如對云臺的體積和重量的限制,以及在應(yīng)對空中復(fù)雜氣流環(huán)境時(shí)需要云臺保持穩(wěn)定的角度和靈活的轉(zhuǎn)動;需要任何載荷能夠根據(jù)遙控指令進(jìn)行調(diào)整或保持一個(gè)特定角度,從而保證任務(wù)載荷進(jìn)行有效地觀測,即需要一種具有慣性姿態(tài)穩(wěn)定的云臺系統(tǒng)。而現(xiàn)有的云臺系統(tǒng)現(xiàn)有云臺往往利用加速度傳感器來檢測姿態(tài)特征,當(dāng)攜帶云臺的無人機(jī)處于加速飛行過程中,加速度計(jì)的輸出將包含機(jī)體加速度信息,則單純利用加速度計(jì)獲得云臺姿態(tài)角將含有較大的誤差。
考慮到土地調(diào)查地形地貌和空中管制等因素,選擇使用無人機(jī)作為云臺的搭載設(shè)備。隨著近年來材料學(xué)和微控制技術(shù)的發(fā)展,無人機(jī)技術(shù)近年來發(fā)展迅速。從飛行方式來看無人機(jī)主要有固定翼、撲翼和多旋翼這三種方式。因固定翼體積較大而撲翼抗風(fēng)能力較弱的原因,多旋翼飛機(jī)因其優(yōu)異的操控性成為目前中短距離無人機(jī)的首選。
在無人機(jī)飛行的過程中需要實(shí)時(shí)的對土地信息進(jìn)行拍照采集,在圖像采集的過程中因其受到風(fēng)力、自身機(jī)身抖動和路徑變更等影響會改變其飛行姿態(tài),在沒有搭載云臺的情況下所拍攝的圖像會出現(xiàn)嚴(yán)重的對焦模糊的情況對后期的處理造成很大的難度。搭載云臺后會解決圖像不穩(wěn)的情況,但是為了保證根據(jù)像控點(diǎn)測量土地信息的精確度,必須保證攝像頭與土地保持垂直,這樣對云臺控制技術(shù)提出了更高的要求。
技術(shù)實(shí)現(xiàn)要素:
本發(fā)明所要解決的技術(shù)問題是針對上述現(xiàn)有技術(shù)存在的MEMS陀螺儀漂移和云臺控制的不足,而提供一種多控制系統(tǒng)融合的云臺實(shí)時(shí)控制系統(tǒng)及控制方法,解決了小型四旋翼飛機(jī)載荷小的情況下對于攝像頭的穩(wěn)態(tài)控制。通過將MEMS陀螺儀漂移自整定、模糊PID控制與自適應(yīng)卡爾曼濾波的結(jié)合、在線電子攝像模塊對于PID參數(shù)的線下修正,提高了攝像的穩(wěn)定質(zhì)量,降低了圖像后期的處理難度,提高了作業(yè)環(huán)境的復(fù)雜程度。
為解決上述技術(shù)問題,本發(fā)明的技術(shù)方案是:
一種土地調(diào)查小型無人機(jī)的云臺實(shí)時(shí)控制系統(tǒng),其特征在于,包含:
云臺三軸驅(qū)動系統(tǒng)、用于調(diào)整無人機(jī)的傾斜角度;
攝像機(jī)、用于獲取地面的圖像信息,并根據(jù)地面上設(shè)置的參考點(diǎn)計(jì)算攝像機(jī)的傾斜角度;
MEMS傳感器、用于獲取無人機(jī)的傾斜角度;
處理器,包括加權(quán)平均模塊及PID算法控制模塊,所述加權(quán)平均模塊對所述MEMS傳感器獲取的無人機(jī)的傾斜角度和攝相機(jī)得到的攝像機(jī)傾斜角度進(jìn)行加權(quán)平均處理得到最終的無人機(jī)傾角;所述PID算法控制模塊將最終的無人機(jī)傾角作為輸入,計(jì)算出所述云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度;
以及核心控制器,根據(jù)所述處理器得到的所述云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度,控制所述云臺三軸驅(qū)動系統(tǒng)進(jìn)行調(diào)整。
所述處理器還包括MEMS漂移自整定模塊以及自適應(yīng)卡爾曼濾波算法模塊,所述MEMS漂移自整定模塊用于消除所述MEMS陀螺儀自身的漂移誤差;所述自適應(yīng)卡爾曼濾波算法模用于對所述MEMS陀螺儀獲取的傾斜角度進(jìn)行濾波去噪處理。
所述處理器還包括電子攝像模塊,該電子攝像模塊對所述對拍攝到的照片基于相控點(diǎn)進(jìn)行角度解算的處理。
所述PID算法控制模塊為模糊PID算法控制模塊。
所述云臺三軸驅(qū)動系統(tǒng)為三軸電機(jī)。
一種土地調(diào)查小型無人機(jī)的云臺實(shí)時(shí)控制方法,其特征在于,步驟為:
S1、在需要測量的區(qū)域標(biāo)定放置參考點(diǎn),以此作為攝像機(jī)的傾角計(jì)算的參考點(diǎn);
S2、無人機(jī)升空在垂直地面航拍的過程中攝像機(jī)根據(jù)參考點(diǎn)計(jì)算出其攝像頭的傾斜角度;同時(shí)無人機(jī)輸出MEMS傳感器獲取的無人機(jī)的傾斜角度;
S3、通過MEMS傳感器和攝相機(jī)得到的傾斜角度加權(quán)平均后作為最終的輸出角度輸入到PID算法控制模塊中;
S4、模糊PID算法控制模塊根據(jù)設(shè)定的模糊規(guī)則,計(jì)算出最終的云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度;云臺三軸驅(qū)動系統(tǒng)根據(jù)接收的需要調(diào)整的角度信息進(jìn)行調(diào)整。
步驟S2中,還需對MEMS傳感器獲取的無人機(jī)的傾斜角度進(jìn)行卡爾曼濾波處理。
所述云臺三軸驅(qū)動系統(tǒng)為三軸電機(jī),所述需要調(diào)整的角度信息以PWM信號的形式驅(qū)動所述三軸電機(jī)。
核心控制器首先根據(jù)前一時(shí)刻的角度值算出此時(shí)刻角度偏差變換率,然后將角度偏差和角度偏差變化率作為模糊PID的輸入?yún)?shù)。模糊PID相較于普通PID控制有更快的響應(yīng)速度和較小的系統(tǒng)超調(diào),相較于其他復(fù)雜的控制算法應(yīng)用于有限硬件資源的效率較高。模糊PID控制器根據(jù)模糊規(guī)則庫制定的標(biāo)準(zhǔn)實(shí)施的改變PID控制器的三個(gè)參數(shù)KP、KI和KD。但是小型無人機(jī)飛行時(shí)受到的干擾很大其主要來自于測量干擾和控制干擾,當(dāng)控制系統(tǒng)引入這些未知噪聲后會出現(xiàn)較大的穩(wěn)態(tài)誤差,因此需要加入自適應(yīng)卡爾曼濾波來克服未知噪聲帶來的擾動。當(dāng)模型存在攝動時(shí),會引起噪聲統(tǒng)計(jì)特性的變化,可以通過噪聲的統(tǒng)計(jì)估值器來對過程噪聲和觀測噪聲的一、二階矩陣進(jìn)行估計(jì),從而調(diào)整卡爾曼濾波的增益,提高濾波精度。但是云臺系統(tǒng)如果本身存在很大的誤差其自身形成的閉環(huán)系統(tǒng)無法糾正,根據(jù)每幀拍攝的圖像進(jìn)行在線的解算,比較每幅圖片同一特征點(diǎn)的重合度,建立矢量坐標(biāo)系解算出幀與幀之間的重合度誤差,計(jì)算每幅圖像的外方元素,根據(jù)解算的外方元素求得攝像機(jī)與地面垂直偏差的角度。根據(jù)角度誤差大小實(shí)時(shí)更改模糊PID和自適應(yīng)卡爾曼濾波的增益。
有益效果
本發(fā)明使得航拍測量無人機(jī)在飛行姿態(tài)控制和云臺穩(wěn)定控制兩方面都能做到理想的精度要求。特別是加入了獨(dú)特的線上圖像角度測算方法,真正根據(jù)實(shí)際拍攝的圖像特征對云臺進(jìn)行控制,使得無人機(jī)能適應(yīng)多重復(fù)雜環(huán)境;同時(shí)本發(fā)明將模糊PID控制與自適應(yīng)卡爾曼濾波相結(jié)合,可以獲得穩(wěn)定的圖像,從而減輕圖像后期處理的難度。
附圖說明
圖1是本發(fā)明控制系統(tǒng)原理框圖。
圖2為本發(fā)明控制方法原理框圖。
具體實(shí)施方式
下面結(jié)合附圖,對本發(fā)明作詳細(xì)說明:
如圖1所示,本發(fā)明一種土地調(diào)查小型無人機(jī)的云臺實(shí)時(shí)控制系統(tǒng),包含:
云臺三軸驅(qū)動系統(tǒng)、用于調(diào)整無人機(jī)的傾斜角度;
攝像機(jī)、用于獲取地面的圖像信息,并根據(jù)地面上設(shè)置的參考點(diǎn)計(jì)算攝像機(jī)的傾斜角度;
MEMS傳感器、用于獲取無人機(jī)的傾斜角度;
處理器,包括加權(quán)平均模塊、PID算法控制模塊、MEMS漂移自整定模塊、自適應(yīng)卡爾曼濾波算法模塊以及電子攝像模塊,加權(quán)平均模塊對所述MEMS傳感器獲取的無人機(jī)的傾斜角度和攝相機(jī)得到的攝像機(jī)傾斜角度進(jìn)行加權(quán)平均處理得到最終的無人機(jī)傾角;PID算法控制模塊將最終的無人機(jī)傾角作為輸入,計(jì)算出云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度;MEMS漂移自整定模塊用于消除MEMS陀螺儀自身的漂移誤差;自適應(yīng)卡爾曼濾波算法模用于對MEMS陀螺儀獲取的傾斜角度進(jìn)行濾波去噪處理;電子攝像模塊對對拍攝到的照片基于相控點(diǎn)進(jìn)行角度解算的處理。
以及核心控制器,根據(jù)所述處理器得到的云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度,控制云臺三軸驅(qū)動系統(tǒng)進(jìn)行調(diào)整。
PID算法控制模塊為模糊PID算法控制模塊。
云臺三軸驅(qū)動系統(tǒng)為三軸電機(jī)。
一種土地調(diào)查小型無人機(jī)的云臺實(shí)時(shí)控制方法,步驟為:
S1、在需要測量的區(qū)域標(biāo)定放置參考點(diǎn),以此作為攝像機(jī)的傾角計(jì)算的參考點(diǎn);
S2、無人機(jī)升空在垂直地面航拍的過程中攝像機(jī)根據(jù)參考點(diǎn)計(jì)算出其攝像頭的傾斜角度;同時(shí)無人機(jī)輸出MEMS傳感器獲取的無人機(jī)的傾斜角度;
S3、通過MEMS傳感器和攝相機(jī)得到的傾斜角度加權(quán)平均后作為最終的輸出角度輸入到PID算法控制模塊中;
S4、模糊PID算法控制模塊根據(jù)設(shè)定的模糊規(guī)則,計(jì)算出最終的云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度;云臺三軸驅(qū)動系統(tǒng)根據(jù)接收的需要調(diào)整的角度信息進(jìn)行調(diào)整。
步驟S2中,還需對MEMS傳感器獲取的無人機(jī)的傾斜角度進(jìn)行卡爾曼濾波處理。
云臺三軸驅(qū)動系統(tǒng)為三軸電機(jī),需要調(diào)整的角度信息以PWM信號的形式驅(qū)動三軸電機(jī)。
實(shí)施例
本發(fā)明控制系統(tǒng),在無人機(jī)機(jī)架上下方合適的位置安裝可以控制三個(gè)相互垂直方向的無刷電機(jī),以此為攝像機(jī)搭設(shè)云臺的控制平臺。還需要在無人機(jī)上搭設(shè)最小飛控系統(tǒng)pixhawk,pixhawk自帶MEMS傳感器同時(shí)能夠在一定的程度上穩(wěn)定機(jī)身的平穩(wěn)。
在需要測量的區(qū)域標(biāo)定放置參考點(diǎn),以此作為單目攝像機(jī)的傾角計(jì)算的參考點(diǎn)。
準(zhǔn)備工作完成后,無人機(jī)升空在垂直地面航拍的過程中單目攝像機(jī)根據(jù)參考點(diǎn)計(jì)算出其攝像頭的傾斜角度,同時(shí)無人機(jī)安裝的pixhawk通過串口輸出無人機(jī)的三個(gè)方向上的加速度、速度和傾斜角度,傾斜角度經(jīng)過卡爾曼濾波減少系統(tǒng)的白色噪聲誤差的影響。
以ARMA擬合建立陀螺儀輸出角度的一階模型為基礎(chǔ),系統(tǒng)狀態(tài)方程為:
x(k)=Ax(k-1)+ω(k-1)
系統(tǒng)的測量方程為:
y(k)=Cx(k)+v(k)
濾波估計(jì)方程為:
濾波增益方程為:K(k)=P1(k)CT[CP1(k)CT+R(k)]-1
均方預(yù)測誤差:P1(k)=AP(k-1)AT+Q(k-1)
均方濾波誤差:P(k)=P1(k)-K(k)CP1(k)
其中Q(k)=E[ω(k)ω(k)T],R(k)=E[v(k)v(k)T],ω(k),v(k)都是白噪聲。
將MEMS陀螺儀輸出的角度經(jīng)過AD采樣后輸入至處理器中,從而得到系統(tǒng)的測量值y(k)。根據(jù)上述卡爾曼濾波方程計(jì)算各個(gè)參量值,并最終解算出濾波估計(jì)方程中的作為實(shí)際的MEMS陀螺儀輸出的角度值。
通過MEMS傳感器和單目攝相機(jī)得到的傾斜角度加權(quán)平均后作為最終的輸出角度。輸入到模糊PID控制系統(tǒng)中。
使用單向空間后方交會方法計(jì)算出外方元素的三個(gè)參數(shù):航向傾角、旁向傾角和像片旋角。算法求解過程如下:
(a)在對準(zhǔn)備進(jìn)行航拍的土地上放置四個(gè)控制點(diǎn)地面坐標(biāo),在外業(yè)測量過程中獲取控制點(diǎn)的地面測量坐標(biāo),并轉(zhuǎn)化成地面攝影測量坐標(biāo):
XS1,YS1,ZS1,XS2,YS2,ZS2,XS3,YS3,ZS3,XS4,YS4,ZS4。
(b)從攝影資料中查取像片比例尺m,平均航高H,內(nèi)方位元素?cái)z影機(jī)主距f。
(c)測量控制點(diǎn)的像點(diǎn)坐標(biāo):在拍攝的圖片中根據(jù)像素計(jì)算出像點(diǎn)坐標(biāo)x、y。
(d)確定未知數(shù)的初始值:在豎直攝影的情況下角元素初始值為線元素初始值ZS0=H=mf,
(e)將上一步得到的初始值帶入R矩陣,計(jì)算其各個(gè)參數(shù)
b1=cosωsinκ
b2=cosωcosκ
b3=-sinω
(f)根據(jù)上一步計(jì)算得到的各個(gè)參數(shù)帶入共線方程中計(jì)算得到x0和y0。
共線方程為:
(g)計(jì)算A矩陣,其中
(h)根據(jù)平差最小二乘法計(jì)算和上一步計(jì)算出的A矩陣,計(jì)算出近似值的改正數(shù)矩陣:
XS=X0+dXS
YS=Y(jié)0+dYS
ZS=Z0+dZS
ω=ω0+dω
κ=κ0+dκ
(i)判斷矩陣A中的參數(shù)是否滿足要求,如果滿足要求則結(jié)束,如果不滿足要求則根據(jù)上面計(jì)算得到的XS,YS,ZS,ω,κ,帶入到R矩陣中各個(gè)參數(shù)重復(fù)(d)-(i)步驟。
模糊PID控制系統(tǒng)根據(jù)之前制定的模糊規(guī)則,計(jì)算出最終3個(gè)無刷電機(jī)需要調(diào)整的角度,以PWM信號的形式驅(qū)動云臺上的三軸電機(jī)。
根據(jù)攝影測量和卡爾曼濾波計(jì)算得到的兩個(gè)角度值,計(jì)算兩個(gè)角度的平均值作為模糊PID的控制輸入。
給定角度減去測量角度得到誤差絕對值|E|和誤差變化絕對值|Ec|,|Ec|的模糊集合作為輸入語言變量,將Kp,Ki,Kd的模糊集合作為輸出語言變量。輸入、輸出語言變量的語言值分為“大(L)”、“中(M)”、“小(S)”三個(gè)等級:
(1)當(dāng)誤差絕對值|E|較大時(shí),為加快系統(tǒng)響應(yīng)速度,應(yīng)取較大的Kp;同時(shí)為了避免系統(tǒng)在初始時(shí),由于誤差的瞬時(shí)增大可能出現(xiàn)的微分飽和而使控制作用超出允許的范圍,此時(shí),應(yīng)取較小的Kd。同時(shí),為了防止系統(tǒng)響應(yīng)出現(xiàn)較大的超調(diào),產(chǎn)生積分飽和,對積分作用應(yīng)加以限制,所以,此時(shí)應(yīng)取Ki=0。
(2)當(dāng)誤差|E|適中時(shí),為使系統(tǒng)具有較小的超調(diào),應(yīng)取稍小的Kp;此時(shí)Kd的取值對系統(tǒng)的影響較大,所以Kd的值要大小適中,以保證系統(tǒng)的響應(yīng)速度。同時(shí),可增加一些積分對控制的作用,Ki太大,易造成積分飽和,太小不能加快系統(tǒng)響應(yīng)速度,所以Ki取值要適當(dāng)。
(3)當(dāng)誤差|E|較小時(shí),為使系統(tǒng)具有良好的穩(wěn)態(tài)性能,應(yīng)取較大的Kp、Ki;同時(shí)為避免系統(tǒng)在設(shè)定值附近產(chǎn)生振蕩,Kd值的選擇非常重要。一般情況下:當(dāng)|E|較小時(shí),Kd可取大些,當(dāng)|E|變大時(shí),Kd可取小些。
下表為基于以上分析和語言變量的定義總結(jié)出的Kp、Ki、Kd的自調(diào)整規(guī)則:
表1
表2
表3
系統(tǒng)某時(shí)刻誤差、誤差變化率的絕對值|E|、|Ec|已知,現(xiàn)在根據(jù)|E|、|Ec|要求PID調(diào)節(jié)器的Kp、Ki、Kd。以求Kp為例說明推理方法。
根據(jù)表1,可將每條Kp調(diào)整規(guī)則寫出來,如第一條可寫為:
R1:if|E|=L and|EC|=L then Kp=M
該規(guī)則隸屬度的計(jì)算方法為:
μkPi(cp)=μLE(|E|)∧μLEC(|EC|)
同理,可求出關(guān)于Kp的其他所有規(guī)則的隸屬度μKpi(cp)(i=1,2,...,n),其中,n為有關(guān)Kp的所有規(guī)則的條數(shù),為第i條規(guī)則中Kp所取模糊集合的中心值。假定系統(tǒng)在某時(shí)刻誤差、誤差變化率的絕對值|E|、|EC|已知,Kp的計(jì)算公式為:
同理,可得到Ki、Kd計(jì)算公式,如下(其中,m、l分別為有關(guān)Ki、Kd的所有規(guī)則的條數(shù)。):
從上面兩個(gè)公式中可以看出,Kp、Ki、Kd與誤差|E|、|EC|之間建立了一種函數(shù)關(guān)系,滿足了系統(tǒng)在不同的E、EC狀態(tài)下對PID控制參數(shù)的不同要求。
在云臺不斷調(diào)整姿態(tài)的過程中其輸出角度不斷變化,因?yàn)楝F(xiàn)有的MEMS陀螺儀有自身的漂移誤差造成測量并不準(zhǔn)確。通過航拍攝像和對MEMS陀螺儀卡爾曼濾波減小漂移誤差對系統(tǒng)的影響,最后將角度平均值輸入模糊PID控制器對云臺進(jìn)行控制。