本發(fā)明涉及無人機技術及云臺控制技術，尤其是涉及一種基于圖像角度測算系統(tǒng)。

背景技術：

土地是立國之根本，在國民經(jīng)濟發(fā)展和基礎設施建設中占有十分重要的地位。隨著國民經(jīng)濟的快速發(fā)展，我國政府更是迫切需要一種技術手段能夠精確高效地完成土地信息的采集和繪制。目前我國城鎮(zhèn)土地調(diào)查主要使用第二次全國土地調(diào)查的GIS系統(tǒng)、GPS-RTK系統(tǒng)與慣導組合和光學全站儀。原有的偵測手段都有靈活機動性不足、精度不夠和受地形地貌的影響因素，影響土地調(diào)查的效率。

目前軍用無人機的研發(fā)已經(jīng)逐漸進入到了實戰(zhàn)測試階段，相比于軍用無人機續(xù)航里程遠、載荷大、電子對抗性強等特點，民用無人機要求低空作業(yè)、輕型小型便于操控、價格低廉、故障損毀后造成的損失較小等特點，因此民用無人機應用于商業(yè)市場上有著廣闊的前景。目前超小型無人機的機載云臺系統(tǒng)具有特殊的要求，例如對云臺的體積和重量的限制，以及在應對空中復雜氣流環(huán)境時需要云臺保持穩(wěn)定的角度和靈活的轉(zhuǎn)動；需要任何載荷能夠根據(jù)遙控指令進行調(diào)整或保持一個特定角度，從而保證任務載荷進行有效地觀測，即需要一種具有慣性姿態(tài)穩(wěn)定的云臺系統(tǒng)。而現(xiàn)有的云臺系統(tǒng)現(xiàn)有云臺往往利用加速度傳感器來檢測姿態(tài)特征，當攜帶云臺的無人機處于加速飛行過程中，加速度計的輸出將包含機體加速度信息，則單純利用加速度計獲得云臺姿態(tài)角將含有較大的誤差。

考慮到土地調(diào)查地形地貌和空中管制等因素，選擇使用無人機作為云臺的搭載設備。隨著近年來材料學和微控制技術的發(fā)展，無人機技術近年來發(fā)展迅速。從飛行方式來看無人機主要有固定翼、撲翼和多旋翼這三種方式。因固定翼體積較大而撲翼抗風能力較弱的原因，多旋翼飛機因其優(yōu)異的操控性成為目前中短距離無人機的首選。

在無人機飛行的過程中需要實時的對土地信息進行拍照采集，在圖像采集的過程中因其受到風力、自身機身抖動和路徑變更等影響會改變其飛行姿態(tài)，在沒有搭載云臺的情況下所拍攝的圖像會出現(xiàn)嚴重的對焦模糊的情況對后期的處理造成很大的難度。搭載云臺后會解決圖像不穩(wěn)的情況，但是為了保證根據(jù)像控點測量土地信息的精確度，必須保證攝像頭與土地保持垂直，這樣對云臺控制技術提出了更高的要求。

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

本發(fā)明所要解決的技術問題是針對上述現(xiàn)有技術存在的MEMS陀螺儀漂移和云臺控制的不足，而提供一種多控制系統(tǒng)融合的云臺實時控制系統(tǒng)及控制方法，解決了小型四旋翼飛機載荷小的情況下對于攝像頭的穩(wěn)態(tài)控制。通過將MEMS陀螺儀漂移自整定、模糊PID控制與自適應卡爾曼濾波的結(jié)合、在線電子攝像模塊對于PID參數(shù)的線下修正，提高了攝像的穩(wěn)定質(zhì)量，降低了圖像后期的處理難度，提高了作業(yè)環(huán)境的復雜程度。

為解決上述技術問題，本發(fā)明的技術方案是：

一種土地調(diào)查小型無人機的云臺實時控制系統(tǒng)，其特征在于，包含：

云臺三軸驅(qū)動系統(tǒng)、用于調(diào)整無人機的傾斜角度；

攝像機、用于獲取地面的圖像信息，并根據(jù)地面上設置的參考點計算攝像機的傾斜角度；

MEMS傳感器、用于獲取無人機的傾斜角度；

處理器，包括加權(quán)平均模塊及PID算法控制模塊，所述加權(quán)平均模塊對所述MEMS傳感器獲取的無人機的傾斜角度和攝相機得到的攝像機傾斜角度進行加權(quán)平均處理得到最終的無人機傾角；所述PID算法控制模塊將最終的無人機傾角作為輸入，計算出所述云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度；

以及核心控制器，根據(jù)所述處理器得到的所述云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度，控制所述云臺三軸驅(qū)動系統(tǒng)進行調(diào)整。

所述處理器還包括MEMS漂移自整定模塊以及自適應卡爾曼濾波算法模塊，所述MEMS漂移自整定模塊用于消除所述MEMS陀螺儀自身的漂移誤差；所述自適應卡爾曼濾波算法模用于對所述MEMS陀螺儀獲取的傾斜角度進行濾波去噪處理。

所述處理器還包括電子攝像模塊，該電子攝像模塊對所述對拍攝到的照片基于相控點進行角度解算的處理。

所述PID算法控制模塊為模糊PID算法控制模塊。

所述云臺三軸驅(qū)動系統(tǒng)為三軸電機。

一種土地調(diào)查小型無人機的云臺實時控制方法，其特征在于，步驟為：

S1、在需要測量的區(qū)域標定放置參考點，以此作為攝像機的傾角計算的參考點；

S2、無人機升空在垂直地面航拍的過程中攝像機根據(jù)參考點計算出其攝像頭的傾斜角度；同時無人機輸出MEMS傳感器獲取的無人機的傾斜角度；

S3、通過MEMS傳感器和攝相機得到的傾斜角度加權(quán)平均后作為最終的輸出角度輸入到PID算法控制模塊中；

S4、模糊PID算法控制模塊根據(jù)設定的模糊規(guī)則，計算出最終的云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度；云臺三軸驅(qū)動系統(tǒng)根據(jù)接收的需要調(diào)整的角度信息進行調(diào)整。

步驟S2中，還需對MEMS傳感器獲取的無人機的傾斜角度進行卡爾曼濾波處理。

所述云臺三軸驅(qū)動系統(tǒng)為三軸電機，所述需要調(diào)整的角度信息以PWM信號的形式驅(qū)動所述三軸電機。

核心控制器首先根據(jù)前一時刻的角度值算出此時刻角度偏差變換率，然后將角度偏差和角度偏差變化率作為模糊PID的輸入?yún)?shù)。模糊PID相較于普通PID控制有更快的響應速度和較小的系統(tǒng)超調(diào)，相較于其他復雜的控制算法應用于有限硬件資源的效率較高。模糊PID控制器根據(jù)模糊規(guī)則庫制定的標準實施的改變PID控制器的三個參數(shù)K_P、K_I和K_D。但是小型無人機飛行時受到的干擾很大其主要來自于測量干擾和控制干擾，當控制系統(tǒng)引入這些未知噪聲后會出現(xiàn)較大的穩(wěn)態(tài)誤差，因此需要加入自適應卡爾曼濾波來克服未知噪聲帶來的擾動。當模型存在攝動時，會引起噪聲統(tǒng)計特性的變化，可以通過噪聲的統(tǒng)計估值器來對過程噪聲和觀測噪聲的一、二階矩陣進行估計，從而調(diào)整卡爾曼濾波的增益，提高濾波精度。但是云臺系統(tǒng)如果本身存在很大的誤差其自身形成的閉環(huán)系統(tǒng)無法糾正，根據(jù)每幀拍攝的圖像進行在線的解算，比較每幅圖片同一特征點的重合度，建立矢量坐標系解算出幀與幀之間的重合度誤差，計算每幅圖像的外方元素，根據(jù)解算的外方元素求得攝像機與地面垂直偏差的角度。根據(jù)角度誤差大小實時更改模糊PID和自適應卡爾曼濾波的增益。

有益效果

本發(fā)明使得航拍測量無人機在飛行姿態(tài)控制和云臺穩(wěn)定控制兩方面都能做到理想的精度要求。特別是加入了獨特的線上圖像角度測算方法，真正根據(jù)實際拍攝的圖像特征對云臺進行控制，使得無人機能適應多重復雜環(huán)境；同時本發(fā)明將模糊PID控制與自適應卡爾曼濾波相結(jié)合，可以獲得穩(wěn)定的圖像，從而減輕圖像后期處理的難度。

附圖說明

圖1是本發(fā)明控制系統(tǒng)原理框圖。

圖2為本發(fā)明控制方法原理框圖。

具體實施方式

下面結(jié)合附圖，對本發(fā)明作詳細說明：

如圖1所示，本發(fā)明一種土地調(diào)查小型無人機的云臺實時控制系統(tǒng)，包含：

云臺三軸驅(qū)動系統(tǒng)、用于調(diào)整無人機的傾斜角度；

攝像機、用于獲取地面的圖像信息，并根據(jù)地面上設置的參考點計算攝像機的傾斜角度；

MEMS傳感器、用于獲取無人機的傾斜角度；

處理器，包括加權(quán)平均模塊、PID算法控制模塊、MEMS漂移自整定模塊、自適應卡爾曼濾波算法模塊以及電子攝像模塊，加權(quán)平均模塊對所述MEMS傳感器獲取的無人機的傾斜角度和攝相機得到的攝像機傾斜角度進行加權(quán)平均處理得到最終的無人機傾角；PID算法控制模塊將最終的無人機傾角作為輸入，計算出云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度；MEMS漂移自整定模塊用于消除MEMS陀螺儀自身的漂移誤差；自適應卡爾曼濾波算法模用于對MEMS陀螺儀獲取的傾斜角度進行濾波去噪處理；電子攝像模塊對對拍攝到的照片基于相控點進行角度解算的處理。

以及核心控制器，根據(jù)所述處理器得到的云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度，控制云臺三軸驅(qū)動系統(tǒng)進行調(diào)整。

PID算法控制模塊為模糊PID算法控制模塊。

云臺三軸驅(qū)動系統(tǒng)為三軸電機。

一種土地調(diào)查小型無人機的云臺實時控制方法，步驟為：

S1、在需要測量的區(qū)域標定放置參考點，以此作為攝像機的傾角計算的參考點；

S2、無人機升空在垂直地面航拍的過程中攝像機根據(jù)參考點計算出其攝像頭的傾斜角度；同時無人機輸出MEMS傳感器獲取的無人機的傾斜角度；

S3、通過MEMS傳感器和攝相機得到的傾斜角度加權(quán)平均后作為最終的輸出角度輸入到PID算法控制模塊中；

S4、模糊PID算法控制模塊根據(jù)設定的模糊規(guī)則，計算出最終的云臺三軸驅(qū)動系統(tǒng)需要調(diào)整的角度；云臺三軸驅(qū)動系統(tǒng)根據(jù)接收的需要調(diào)整的角度信息進行調(diào)整。

步驟S2中，還需對MEMS傳感器獲取的無人機的傾斜角度進行卡爾曼濾波處理。

云臺三軸驅(qū)動系統(tǒng)為三軸電機，需要調(diào)整的角度信息以PWM信號的形式驅(qū)動三軸電機。

實施例

本發(fā)明控制系統(tǒng)，在無人機機架上下方合適的位置安裝可以控制三個相互垂直方向的無刷電機，以此為攝像機搭設云臺的控制平臺。還需要在無人機上搭設最小飛控系統(tǒng)pixhawk，pixhawk自帶MEMS傳感器同時能夠在一定的程度上穩(wěn)定機身的平穩(wěn)。

在需要測量的區(qū)域標定放置參考點，以此作為單目攝像機的傾角計算的參考點。

準備工作完成后，無人機升空在垂直地面航拍的過程中單目攝像機根據(jù)參考點計算出其攝像頭的傾斜角度，同時無人機安裝的pixhawk通過串口輸出無人機的三個方向上的加速度、速度和傾斜角度，傾斜角度經(jīng)過卡爾曼濾波減少系統(tǒng)的白色噪聲誤差的影響。

以ARMA擬合建立陀螺儀輸出角度的一階模型為基礎，系統(tǒng)狀態(tài)方程為：

x(k)＝Ax(k-1)+ω(k-1)

系統(tǒng)的測量方程為：

y(k)＝Cx(k)+v(k)

濾波估計方程為：

濾波增益方程為：K(k)＝P₁(k)C^T[CP₁(k)C^T+R(k)]^-1

均方預測誤差：P₁(k)＝AP(k-1)A^T+Q(k-1)

均方濾波誤差：P(k)＝P₁(k)-K(k)CP₁(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ù)上述卡爾曼濾波方程計算各個參量值，并最終解算出濾波估計方程中的作為實際的MEMS陀螺儀輸出的角度值。

通過MEMS傳感器和單目攝相機得到的傾斜角度加權(quán)平均后作為最終的輸出角度。輸入到模糊PID控制系統(tǒng)中。

使用單向空間后方交會方法計算出外方元素的三個參數(shù)：航向傾角、旁向傾角和像片旋角。算法求解過程如下:

(a)在對準備進行航拍的土地上放置四個控制點地面坐標，在外業(yè)測量過程中獲取控制點的地面測量坐標，并轉(zhuǎn)化成地面攝影測量坐標：

X_S1,Y_S1,Z_S1,X_S2,Y_S2,Z_S2,X_S3,Y_S3,Z_S3,X_S4,Y_S4,Z_S4。

(b)從攝影資料中查取像片比例尺m，平均航高H，內(nèi)方位元素攝影機主距f。

(c)測量控制點的像點坐標：在拍攝的圖片中根據(jù)像素計算出像點坐標x、y。

(d)確定未知數(shù)的初始值：在豎直攝影的情況下角元素初始值為線元素初始值Z_S0＝H＝mf，

(e)將上一步得到的初始值帶入R矩陣，計算其各個參數(shù)

b₁＝cosωsinκ

b₂＝cosωcosκ

b₃＝-sinω

(f)根據(jù)上一步計算得到的各個參數(shù)帶入共線方程中計算得到x₀和y₀。

共線方程為：

$x=-f\frac{a_1(X-X_S)+b_1(Y-Y_S)+c_1(Z-Z_S)}{a_3(X-X_S)+b_3(Y-Y_S)+c_3(Z-Z_S)}$

$y=-f\frac{a_2(X-X_S)+b_2(Y-Y_S)+c_2(Z-Z_S)}{a_3(X-X_S)+b_3(Y-Y_S)+c_3(Z-Z_S)}$

(g)計算A矩陣，其中

$\frac{\∂x}{\∂X_S}=\frac{1}{\stackrel{\‾}{z}}(a_{1}f+a_{3}x),\frac{\∂x}{\∂Y_S}=\frac{1}{\stackrel{\‾}{z}}(b_{1}f+b_{3}x)$

$\frac{\∂x}{\∂\mathrm{\ω}}=-fsin\mathrm{\κ}-\frac{x}{f}(xsin\mathrm{\κ}+ycos\mathrm{\κ}),\frac{\∂x}{\∂\mathrm{\κ}}=y$

$\frac{\∂y}{\∂X_S}=\frac{1}{\stackrel{\‾}{z}}(a_{2}f+a_{3}y),\frac{\∂y}{\∂Y_S}=\frac{1}{\stackrel{\‾}{z}}(b_{2}f+b_{3}y)$

$\frac{\∂y}{\∂\mathrm{\ω}}=-fcos\mathrm{\κ}-\frac{x}{f}(xsin\mathrm{\κ}+ycos\mathrm{\κ}),\frac{\∂x}{\∂\mathrm{\κ}}=-x$

$\stackrel{\‾}{z}=a_3(X-X_S)+b_3(Y-Y_S)+c_3(Z-Z_S)$

(h)根據(jù)平差最小二乘法計算和上一步計算出的A矩陣，計算出近似值的改正數(shù)矩陣：

X_S＝X₀+dX_S

Y_S＝Y(jié)₀+dY_S

Z_S＝Z₀+dZ_S

ω＝ω₀+dω

κ＝κ₀+dκ

(i)判斷矩陣A中的參數(shù)是否滿足要求，如果滿足要求則結(jié)束，如果不滿足要求則根據(jù)上面計算得到的X_S,Y_S,Z_S,ω,κ，帶入到R矩陣中各個參數(shù)重復(d)-(i)步驟。

模糊PID控制系統(tǒng)根據(jù)之前制定的模糊規(guī)則，計算出最終3個無刷電機需要調(diào)整的角度，以PWM信號的形式驅(qū)動云臺上的三軸電機。

根據(jù)攝影測量和卡爾曼濾波計算得到的兩個角度值，計算兩個角度的平均值作為模糊PID的控制輸入。

給定角度減去測量角度得到誤差絕對值|E|和誤差變化絕對值|Ec|，|Ec|的模糊集合作為輸入語言變量，將Kp，Ki，Kd的模糊集合作為輸出語言變量。輸入、輸出語言變量的語言值分為“大(L)”、“中(M)”、“小(S)”三個等級：

(1)當誤差絕對值|E|較大時，為加快系統(tǒng)響應速度，應取較大的Kp；同時為了避免系統(tǒng)在初始時，由于誤差的瞬時增大可能出現(xiàn)的微分飽和而使控制作用超出允許的范圍，此時，應取較小的Kd。同時，為了防止系統(tǒng)響應出現(xiàn)較大的超調(diào)，產(chǎn)生積分飽和，對積分作用應加以限制，所以，此時應取Ki＝0。

(2)當誤差|E|適中時，為使系統(tǒng)具有較小的超調(diào)，應取稍小的Kp；此時Kd的取值對系統(tǒng)的影響較大，所以Kd的值要大小適中，以保證系統(tǒng)的響應速度。同時，可增加一些積分對控制的作用，Ki太大，易造成積分飽和，太小不能加快系統(tǒng)響應速度，所以Ki取值要適當。

(3)當誤差|E|較小時，為使系統(tǒng)具有良好的穩(wěn)態(tài)性能，應取較大的Kp、Ki；同時為避免系統(tǒng)在設定值附近產(chǎn)生振蕩，Kd值的選擇非常重要。一般情況下：當|E|較小時，Kd可取大些，當|E|變大時，Kd可取小些。

下表為基于以上分析和語言變量的定義總結(jié)出的Kp、Ki、Kd的自調(diào)整規(guī)則：

表1

表2

表3

系統(tǒng)某時刻誤差、誤差變化率的絕對值|E|、|Ec|已知，現(xiàn)在根據(jù)|E|、|Ec|要求PID調(diào)節(jié)器的Kp、Ki、Kd。以求Kp為例說明推理方法。

根據(jù)表1，可將每條Kp調(diào)整規(guī)則寫出來，如第一條可寫為：

R₁:if|E|＝L and|EC|＝L then K_p＝M

該規(guī)則隸屬度的計算方法為：

μ_kPi(c_p)＝μ_LE(|E|)∧μ_LEC(|EC|)

同理，可求出關于Kp的其他所有規(guī)則的隸屬度μK_pi(c_p)(i＝1，2，...，n)，其中，n為有關Kp的所有規(guī)則的條數(shù)，為第i條規(guī)則中Kp所取模糊集合的中心值。假定系統(tǒng)在某時刻誤差、誤差變化率的絕對值|E|、|EC|已知，Kp的計算公式為：

$K_P=\frac{\underset{i=1}{\overset{n}{\Σ}}\left({\mathrm{\μ}}_{K_{Pi}}\right(c_p)\×c_{pi})}{\underset{i=1}{\overset{n}{\Σ}}{\mathrm{\μ}}_{K_{Pi}}\left(c_p\right)}$

同理，可得到Ki、Kd計算公式，如下(其中，m、l分別為有關Ki、Kd的所有規(guī)則的條數(shù)。)：

$K_I=\frac{\underset{i=1}{\overset{m}{\Σ}}\left({\mathrm{\μ}}_{K_{Ii}}\right(d_I)\×d_I)}{\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\μ}}_{K_{Ii}}\left(d_I\right)}$

$K_D=\frac{\underset{i=1}{\overset{l}{\Σ}}\left({\mathrm{\μ}}_{K_{Di}}\right(g_D)\×g_{Di})}{\underset{i=1}{\overset{l}{\Σ}}{\mathrm{\μ}}_{K_{Di}}\left(g_D\right)}$

從上面兩個公式中可以看出，Kp、Ki、Kd與誤差|E|、|EC|之間建立了一種函數(shù)關系，滿足了系統(tǒng)在不同的E、EC狀態(tài)下對PID控制參數(shù)的不同要求。

在云臺不斷調(diào)整姿態(tài)的過程中其輸出角度不斷變化，因為現(xiàn)有的MEMS陀螺儀有自身的漂移誤差造成測量并不準確。通過航拍攝像和對MEMS陀螺儀卡爾曼濾波減小漂移誤差對系統(tǒng)的影響，最后將角度平均值輸入模糊PID控制器對云臺進行控制。