四旋翼飛行器仿真方法
【專利摘要】四旋翼飛行器仿真方法，涉及一種無人飛行器實(shí)時(shí)仿真方法。解決現(xiàn)有半實(shí)物仿真系統(tǒng)規(guī)模龐大，連接復(fù)雜、仿真存在虛擬的狀態(tài)變量導(dǎo)致的準(zhǔn)確度低、難以應(yīng)用基于模型的控制器設(shè)計(jì)方法和數(shù)值仿真系統(tǒng)機(jī)理建模得到的模型準(zhǔn)確度不高的問題。本發(fā)明把實(shí)際的飛行器作為被控對(duì)象接入仿真回路，為MATLAB/Simulink中機(jī)理建模得到的數(shù)值仿真模型提供了一個(gè)參考，使得飛行器機(jī)理建模準(zhǔn)確性可以快速得到驗(yàn)證；使用了參數(shù)辨識(shí)的方法確保了數(shù)值仿真中對(duì)象模型的準(zhǔn)確性，從而數(shù)值仿真中驗(yàn)證可行的控制器最終能夠有效地應(yīng)用到實(shí)際的物理對(duì)象上。本發(fā)明具體用于四旋翼飛行器仿真領(lǐng)域。
【專利說明】四旋翼飛行器仿真方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種無人飛行器實(shí)時(shí)仿真方法。
【背景技術(shù)】
[0002]四旋翼飛行器在動(dòng)力學(xué)上具有欠驅(qū)動(dòng)、強(qiáng)耦合和非線性的特點(diǎn)，這些都增加了飛行控制器設(shè)計(jì)的難度。
[0003]在控制器的設(shè)計(jì)階段，通常有兩種方式可以去驗(yàn)證控制器性能:第一種是實(shí)物飛行，這種驗(yàn)證方法可信度高，但具有效率低、風(fēng)險(xiǎn)大、成本高的缺點(diǎn)；第二種方法是數(shù)值仿真，這種方法效率高、成本低，但通常會(huì)由于數(shù)值模型中參數(shù)估計(jì)不準(zhǔn)確、未建模動(dòng)態(tài)和未考慮擾動(dòng)等因素導(dǎo)致即便是在數(shù)值仿真中驗(yàn)證可行的控制器也難以應(yīng)用到實(shí)際的飛行器上。
[0004]半實(shí)物仿真,又稱為硬件在回路中的仿真(Hardware in the Loop Simulat1n),是指在仿真實(shí)驗(yàn)系統(tǒng)的仿真回路中接入部分實(shí)物的實(shí)時(shí)仿真。實(shí)時(shí)性是進(jìn)行半實(shí)物仿真的必要前提。半實(shí)物仿真同其它類型的仿真方法相比具有經(jīng)濟(jì)地實(shí)現(xiàn)更高真實(shí)度的可能性，同時(shí)具有快速搭建和部署控制算法的優(yōu)勢(shì)。從系統(tǒng)的觀點(diǎn)來看，半實(shí)物仿真允許在系統(tǒng)中接入部分實(shí)物，意味著可以把部分實(shí)物放在系統(tǒng)中進(jìn)行考察，從而使部件能在滿足系統(tǒng)整體性能指標(biāo)的環(huán)境中得到檢驗(yàn)，因此半實(shí)物仿真是提高系統(tǒng)設(shè)計(jì)的可靠性和研制質(zhì)量的必要手段。
[0005]實(shí)現(xiàn)半實(shí)物仿真的一種手段是采用dSPACE實(shí)時(shí)仿真系統(tǒng)，它是由德國(guó)dSPACE公司開發(fā)的一套基于MATLAB/Simulink的控制系統(tǒng)在實(shí)時(shí)環(huán)境下的開發(fā)及測(cè)試工作平臺(tái)，實(shí)現(xiàn)了實(shí)際物理系統(tǒng)和MATLAB/Simulink的無縫連接，但價(jià)格昂貴。
[0006]另一種支持?jǐn)?shù)據(jù)可視化的基于xPC的半實(shí)物仿真系統(tǒng)要兩臺(tái)計(jì)算機(jī)(宿主機(jī)、目標(biāo)機(jī))才能完成仿真，連接復(fù)雜成本較高。
[0007]—種四旋翼飛行器的半實(shí)物仿真方法將飛行器固定在一個(gè)與地面固連的萬向節(jié)上，限制了其在空間平動(dòng)的三個(gè)自由度，從而保證其飛行的安全性；但這種方法使用了虛擬的位置狀態(tài)，仿真準(zhǔn)確度不高，而且并不適合用于驗(yàn)證高度控制算法。
【發(fā)明內(nèi)容】
[0008]本發(fā)明是為了解決現(xiàn)有半實(shí)物仿真系統(tǒng)規(guī)模龐大，連接復(fù)雜、仿真存在虛擬的狀態(tài)變量導(dǎo)致的準(zhǔn)確度低、難以應(yīng)用基于模型的控制器設(shè)計(jì)方法和數(shù)值仿真系統(tǒng)機(jī)理建模得到的模型準(zhǔn)確度不高的問題，本發(fā)明提供了一種四旋翼飛行器仿真方法。
[0009]四旋翼飛行器半實(shí)物仿真方法，它是基于下述裝置實(shí)現(xiàn)的，該裝置包括仿真計(jì)算機(jī)、機(jī)載微控制器、I號(hào)無線收發(fā)器、2號(hào)無線收發(fā)器和機(jī)載傳感器；
[0010]機(jī)載微控制器的數(shù)據(jù)信號(hào)輸入端與機(jī)載傳感器的數(shù)據(jù)信號(hào)輸出端連接，機(jī)載微控制器的通訊端與2號(hào)無線收發(fā)器的信號(hào)輸入輸出端連接，2號(hào)無線收發(fā)器通過無線通信的方式與I號(hào)無線收發(fā)器進(jìn)行數(shù)據(jù)傳輸，I號(hào)無線收發(fā)器的信號(hào)輸入輸出端與仿真計(jì)算機(jī)的信號(hào)輸入輸出端連接，
[0011]仿真計(jì)算機(jī)內(nèi)嵌入Matlab/Simulink仿真模塊,所述的Matlab/Simulink仿真模塊包括數(shù)據(jù)輸入模塊、狀態(tài)估計(jì)與濾波模塊、姿態(tài)與高度控制模塊、飛行狀態(tài)顯示模塊、數(shù)據(jù)輸出模塊、半實(shí)物仿真數(shù)據(jù)記錄模塊和控制指令生成模塊；
[0012]該半實(shí)物仿真方法的具體過程為:
[0013]首先，通過機(jī)載微控制器采集機(jī)載傳感器輸出的數(shù)據(jù)信號(hào)，且機(jī)載微控制器依次通過2號(hào)無線收發(fā)器、I號(hào)無線收發(fā)器將采集到的數(shù)據(jù)信號(hào)送至仿真計(jì)算機(jī)的數(shù)據(jù)輸入模塊，該數(shù)據(jù)信號(hào)包括測(cè)距讀數(shù)、三軸加速度讀數(shù)、三軸角速度讀數(shù)和三軸磁感應(yīng)強(qiáng)度信號(hào)；
[0014]通過數(shù)據(jù)輸入模塊接收輸入的數(shù)據(jù)信號(hào)，并將該數(shù)據(jù)送至狀態(tài)估計(jì)與濾波模塊，狀態(tài)估計(jì)與濾波模塊根據(jù)接收的數(shù)據(jù)信號(hào)利用互補(bǔ)濾波的方法獲得兩組數(shù)據(jù)，該兩組數(shù)據(jù)中的第一組數(shù)據(jù)為姿態(tài)歐拉角估計(jì)值和高度估計(jì)值，第二組數(shù)據(jù)為角速度的估計(jì)值和豎直方向上速度的估計(jì)值，
[0015]且將第一組數(shù)據(jù)送至飛行狀態(tài)顯示模塊，飛行狀態(tài)顯示模塊用于實(shí)時(shí)顯示四旋翼飛行器的飛行狀態(tài)，
[0016]將第一組和第二組數(shù)據(jù)送至姿態(tài)與高度控制模塊，姿態(tài)與高度控制模塊根據(jù)控制指令生成模塊產(chǎn)生的控制指令，計(jì)算獲得姿態(tài)和高度控制信號(hào)，然后經(jīng)過控制分配得到四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速，該四個(gè)電機(jī)的期望轉(zhuǎn)速通過數(shù)據(jù)輸出模塊依次經(jīng)過I號(hào)無線收發(fā)器、2號(hào)無線收發(fā)器后送至機(jī)載微控制器用于控制四旋翼飛行器的電子調(diào)速器及電機(jī)，
[0017]半實(shí)物仿真數(shù)據(jù)記錄模塊用于存儲(chǔ)狀態(tài)估計(jì)與濾波模塊輸出的兩組數(shù)據(jù)和四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速。
[0018]所述的機(jī)載傳感器包括三軸電子羅盤、六軸運(yùn)動(dòng)傳感器和超聲測(cè)距傳感器，
[0019]三軸電子羅盤用于采集三軸磁感應(yīng)強(qiáng)度信號(hào)，
[0020]六軸運(yùn)動(dòng)傳感器用于采集三軸加速度讀數(shù)和三軸角速度讀數(shù)，
[0021 ] 超聲測(cè)距傳感器用于采集測(cè)距讀數(shù)。
[0022]所述的三軸電子羅盤的采用型號(hào)為HMC5883L的集成電路實(shí)現(xiàn)，六軸運(yùn)動(dòng)傳感器的型號(hào)為MPU6050的集成電路實(shí)現(xiàn)，超聲測(cè)距傳感器采用型號(hào)為US-100的集成電路實(shí)現(xiàn)。
[0023]所述的機(jī)載微控制器采用TMS320F28335型DSP實(shí)現(xiàn)。
[0024]所述的姿態(tài)與高度控制模塊的優(yōu)化過程為，
[0025]在模型辨識(shí)子系統(tǒng)中，參數(shù)辨識(shí)器接收四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速Ω1; Ω2，Ω3，ω4、姿態(tài)歐拉角估計(jì)值(I, I, #)、高度估計(jì)值1、角速度的估計(jì)值
(ρ> I?卩)和豎直方向上速度的估計(jì)值該參數(shù)辨識(shí)器根據(jù)待辨識(shí)模型結(jié)構(gòu)，求得參數(shù)向量的辨識(shí)結(jié)果，進(jìn)而確定了飛行器運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型，從而根據(jù)數(shù)值仿真子系統(tǒng)完成對(duì)姿態(tài)與高度控制模塊的優(yōu)化，
[0026]所述的待辨識(shí)模型結(jié)構(gòu)通過下述的狀態(tài)空間方程描述:
φ- pcos$-f-r?ηθ
0= q-r cos Θι--χ φ + p sin θ tan φ
ψ-r 00Βθ$0€φ- ρ$?ηθ^οφ
/ j ___ j \ j j
P= θψ\ ^βZz^Qr+J^lf2
VIXX J IXXIXX
/ \
[0027]< ^=φψ LzIj1...+φ?^--ι +^?!^ (公式一)，
I J (π ,π
? I ~~ I \ ?
r-# ^^ +丄CZ4
I 4 J 4
1= w
命=g —(eos 叆 cos 句丄 IT1.m
^^^(?^+--+α；+^2)
C/3=li(?)
[0028]其中，<U,=h(￡\2-1i;)(公式二)，
U4 =d (-Q2 + — ?^2 +1?2)
Qr = ?\ + fi|| —Ω( —ζ\
[0029]所述的參數(shù)辨識(shí)器根據(jù)待辨識(shí)模型結(jié)構(gòu)，獲得參數(shù)向量的辨識(shí)結(jié)果的具體過程為:
[0030]當(dāng)四個(gè)電機(jī)的期望轉(zhuǎn)速Q(mào)1, Ω2, Ω3, Ω4、作為飛行器模型的輸入時(shí)，通過公式一和公式二求得部分預(yù)測(cè)狀態(tài)的數(shù)值解為
[0031 ] ξ (k) = [ Φ (k), Θ (k), ψ (k)，P (k)，q (k)，r (k), z(k), w(k)]T,
Λ>^
[0032]其次，采用代價(jià)函數(shù)匕('.)= Σ^(λ> —求取rQ，
[0033]r0 =argminFv(r),
[0034]A即為參數(shù)向量的辨識(shí)結(jié)果，r為待辨識(shí)參數(shù)向量，其中，r =[m，I，IssJ Iyyj Izzj Jrj b, d]，
[0035]待辨識(shí)參數(shù)向量r包括飛行器總質(zhì)量m,相對(duì)電機(jī)軸距離I,機(jī)體轉(zhuǎn)動(dòng)慣量(Ixx, Iyy, IJ、旋翼與電機(jī)轉(zhuǎn)子繞電機(jī)軸轉(zhuǎn)動(dòng)慣量1，升力系數(shù)b，扭矩系數(shù)d，N為記錄的采樣點(diǎn)數(shù)冰為采樣時(shí)間點(diǎn)，^%、4、仏和均為中間變量，g為重力加速度。
[0036]四旋翼飛行器的數(shù)值仿真方法，該方法的具體過程為，
[0037]首先，參數(shù)辨識(shí)器接收從半實(shí)物仿真系統(tǒng)獲得數(shù)據(jù)信息，該參數(shù)辨識(shí)器根據(jù)待辨識(shí)模型結(jié)構(gòu)，獲得參數(shù)向量的辨識(shí)結(jié)果，并通過數(shù)值仿真子系統(tǒng)根據(jù)獲得的參數(shù)向量的辨識(shí)結(jié)果，獲得飛行器運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型，從而完成四旋翼飛行器的數(shù)值仿真。
[0038]所述的參數(shù)辨識(shí)器接收從半實(shí)物仿真系統(tǒng)獲得數(shù)據(jù)信息包括四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速Ω1; Ω2，Ω3，Ω4、姿態(tài)歐拉角估計(jì)值(I O, #)、高度估計(jì)值f、角速度的估計(jì)值(#, I? ?)和豎直方向上速度的估計(jì)值命，
[0039]所述的待辨識(shí)模型結(jié)構(gòu)通過下述的狀態(tài)空間方程描述:
-..? = pm%0 + r%m$
0= q-r cos^tan^ + p sin Θ tan φ
ψ =拳—/? sin# see ^
? f/ —/、.j ι
ρ^θψ J^二 -e^ar^—ut
\ IXX J IXX^ XX
[0040]< ^=φψ ? _Ξ_十 φ^--^ζ2 + —?—(公式'~* )，
K ^yy J ^yy
?1-1 Λ ?
Γ = θφ\(chéng)^............^........................^.....+^tZ4
f = w
I
W = g-(cos φ COS θ)-U1
,.m
U1 =b (?^2..032 )
υ2=δ(-Ω:;+Ω:)
[0041]其中，?=Mq1-Qi2)(公式二)，
U4 =?(-?\2+?ξ~￡^+￡--)
Qr = ￡^+Ω4—?^—Ω^
[0042]所述的參數(shù)辨識(shí)器根據(jù)待辨識(shí)模型結(jié)構(gòu)，獲得參數(shù)向量的辨識(shí)結(jié)果的具體過程為:
[0043]當(dāng)四個(gè)電機(jī)的期望轉(zhuǎn)速Q(mào)1, Ω2, Ω3, Ω4、作為飛行器模型的輸入時(shí)，通過公式一和公式二求得部分預(yù)測(cè)狀態(tài)的數(shù)值解為ξ (k) = [ΦΟΟ，Θ (k), v(k),p(k),q(k),r(k),z(k),w(k)]T,
JV
[0044]其次，采用代價(jià)函數(shù)匕W = Z(Ha)W*))求取rQ，
h —I.f
[0045]r0 =BrgminPv (r),
[0046]A即為參數(shù)向量的辨識(shí)結(jié)果，r為待辨識(shí)參數(shù)向量，其中，r =[m，I，IssJ Iyyj Izzj Jrj b, d]，
[0047]待辨識(shí)參數(shù)向量r包括飛行器總質(zhì)量m,相對(duì)電機(jī)軸距離I,機(jī)體轉(zhuǎn)動(dòng)慣量(Ixx, Iyy, IJ、旋翼與電機(jī)轉(zhuǎn)子繞電機(jī)軸轉(zhuǎn)動(dòng)慣量1，升力系數(shù)b，扭矩系數(shù)d，N為記錄的采樣點(diǎn)數(shù)冰為采樣時(shí)間點(diǎn)，^%、4、仏和均為中間變量，g為重力加速度。
[0048]本發(fā)明建立了一種四旋翼飛行器半實(shí)物驗(yàn)證平臺(tái)，把實(shí)際的飛行器作為被控對(duì)象接入仿真回路，為MATLAB/Simulink中建好的數(shù)值仿真模型提供一個(gè)參考，使得飛行器數(shù)值建模準(zhǔn)確性可以快速得到驗(yàn)證，進(jìn)而確保數(shù)值仿真中驗(yàn)證可行的控制器最終能夠有效地應(yīng)用到實(shí)際的物理對(duì)象上。同時(shí)為飛行器模型分析和控制參數(shù)整定提供一個(gè)可視化的數(shù)據(jù)顯示平臺(tái)，提高狀態(tài)觀測(cè)器和控制器的設(shè)計(jì)開發(fā)效率。
[0049]系統(tǒng)包括組成部分與功能:
[0050]安裝有MATLAB/Simulink的仿真計(jì)算機(jī)作為半實(shí)物驗(yàn)證系統(tǒng)的核心部分，它運(yùn)行了飛行器實(shí)體對(duì)象和仿真環(huán)境的數(shù)學(xué)模型和程序，采用了層次化、模塊化的建模方法，可以以數(shù)據(jù)流的驅(qū)動(dòng)方式實(shí)時(shí)運(yùn)行。
[0051]仿真計(jì)算機(jī)中安裝了 MATLAB的Real-time Windows Target (RTWT)產(chǎn)品，它提供了一個(gè)實(shí)時(shí)引擎使得Simulink仿真模型的不同的模塊能夠在Windows系統(tǒng)下實(shí)時(shí)地與硬件輸入輸出板卡連接，同時(shí)實(shí)現(xiàn)信號(hào)的可視化與模型運(yùn)行時(shí)的參數(shù)調(diào)節(jié)功能。
[0052]外部模式則允許Simulink引擎作為客戶端(宿主)，RTffT生成的代碼作為服務(wù)器(目標(biāo))，客戶端和服務(wù)器之間以共享內(nèi)存的方式進(jìn)行通信。具體來看，Simulink引擎發(fā)送消息請(qǐng)求目標(biāo)接收數(shù)據(jù)并且上傳測(cè)量數(shù)據(jù)，而目標(biāo)則以執(zhí)行請(qǐng)求作為回應(yīng)。
[0053]在外部模式下，半實(shí)物仿真模型最高可以以5KHz的頻率執(zhí)行，Simulink模型就變成了一個(gè)圖形用戶界面，不需要重新編譯便可以調(diào)整模型參數(shù)。運(yùn)行在Simulink中的仿真模型包括:
[0054](I)仿真模型硬件接口包括數(shù)據(jù)輸入模塊和數(shù)據(jù)輸出模塊，分別可以完成串口通信的二進(jìn)制編碼和解碼的工作，與機(jī)載微控制器建立數(shù)據(jù)連接；
[0055](2)數(shù)值對(duì)象模型，包括飛行器的運(yùn)動(dòng)學(xué)方程、動(dòng)力學(xué)方程、傳感器的測(cè)量模型以及模擬的噪聲模型。傳感器包括三軸電子羅盤、六軸運(yùn)動(dòng)傳感器和超聲測(cè)距傳感器。此部分與實(shí)際的物理模型對(duì)應(yīng)，用于濾波算法和控制算法的初步驗(yàn)證；
[0056](3)狀態(tài)估計(jì)與濾波模塊，根據(jù)對(duì)地磁矢量和重力矢量的觀測(cè)和三軸方位讀數(shù)信號(hào)的積分，利用互補(bǔ)濾波的方法獲得對(duì)姿態(tài)信息的估計(jì)。根據(jù)高度的測(cè)量值、姿態(tài)的估計(jì)值與加速度的測(cè)量值，同樣使用互補(bǔ)濾波的方法獲得高度變化率的估計(jì)。
[0057](4)姿態(tài)與高度控制模塊，包括四個(gè)分別以滾轉(zhuǎn)角、俯仰角、偏航角和高度為被控變量的PID控制器，經(jīng)過控制分配，得到四個(gè)電機(jī)的期望轉(zhuǎn)速作為輸出。
[0058]以上⑵(3) (4)構(gòu)成控制回路可以實(shí)現(xiàn)數(shù)值仿真，以上(I) (3) (4)構(gòu)成控制回路并配合實(shí)際的物理模型可以完成半實(shí)物仿真，
[0059]四旋翼飛行器的機(jī)載控制板搭載了數(shù)字信號(hào)處理器(DSP)TMS320F28335用于采集飛行姿態(tài)和位置信息傳給仿真計(jì)算機(jī)并執(zhí)行仿真計(jì)算機(jī)發(fā)出的控制指令。
[0060]DSP的I2C接口分別與六軸運(yùn)動(dòng)傳感器MPU6050(包含三軸加速度計(jì)和三軸陀螺儀)和三軸電子羅盤HMC5883L連接。通過UART與超聲測(cè)距模塊US-100連接用以測(cè)量高度。通過SPI總線與運(yùn)行Simulink的仿真計(jì)算機(jī)交換數(shù)據(jù):按照收到的請(qǐng)求向仿真計(jì)算機(jī)發(fā)送采集到的傳感器數(shù)據(jù)，根據(jù)收到的控制信號(hào)生成PWM信號(hào)并發(fā)給電子調(diào)速器從而控制電機(jī)轉(zhuǎn)速。在仿真的開始階段，應(yīng)當(dāng)將飛行器重心固定在一個(gè)萬向節(jié)上從而保證試驗(yàn)的安全性。
[0061]射頻無線通信部分包含了一個(gè)同DSP連接的NRF24L01無線收發(fā)器，和一個(gè)與仿真計(jì)算機(jī)連接的兼容NRF24L01的USB無線收發(fā)器，實(shí)現(xiàn)了仿真計(jì)算機(jī)與DSP間的無線數(shù)據(jù)交換。
[0062]本發(fā)明所提出的四旋翼飛行器半實(shí)物仿真方法，可以直觀地針對(duì)各種在數(shù)值仿真中已經(jīng)驗(yàn)證成功的飛行控制算法結(jié)合實(shí)際對(duì)象進(jìn)行半實(shí)物仿真，既能真實(shí)地呈現(xiàn)四旋翼無人飛行器的狀態(tài)和控制量變化曲線，又能在線修改濾波器與控制器參數(shù)，能夠?yàn)榍度胧娇刂破鞯脑O(shè)計(jì)提供極具參考價(jià)值的測(cè)試結(jié)果，可極大地縮減研發(fā)周期，同時(shí)節(jié)約飛行實(shí)驗(yàn)成本。
[0063]本發(fā)明在仿真過程中，其被控對(duì)象采用實(shí)體四旋翼無人飛行器，而非純數(shù)值形式的動(dòng)力學(xué)模型，姿態(tài)傳感器可實(shí)時(shí)讀取到真實(shí)的飛行狀態(tài)數(shù)據(jù)，仿真效果貼近直實(shí)情況。
[0064]本發(fā)明使用半實(shí)物驗(yàn)證系統(tǒng)的擾動(dòng)響應(yīng)數(shù)據(jù)，將系統(tǒng)辨識(shí)的方法應(yīng)用到數(shù)值模型的修正工作上，為四旋翼飛行器的機(jī)理建模方法提供了有效的驗(yàn)證工具。
[0065]仿真系統(tǒng)的兩個(gè)數(shù)據(jù)終端(仿真計(jì)算機(jī)和實(shí)物平臺(tái))使用射頻無線通信，近距離情況下可以保證快速穩(wěn)定的數(shù)據(jù)交換，提高了仿真的靈活性。
[0066]由于所需要的控制周期在200Hz以內(nèi)，MATLAB提供的可以植入到Windows中的實(shí)時(shí)內(nèi)核完全可以勝任本發(fā)明所需要的數(shù)據(jù)帶寬，本發(fā)明所涉及的仿真計(jì)算機(jī)在提供人機(jī)交互功能的同時(shí)完成了實(shí)際飛行器模型實(shí)時(shí)控制的任務(wù)，省去了考慮專用計(jì)算機(jī)成本和兼容性問題的煩惱。
[0067]而本發(fā)明對(duì)現(xiàn)有基于xPC的半實(shí)物仿真系統(tǒng)進(jìn)行改進(jìn)，上述目標(biāo)機(jī)內(nèi)運(yùn)行的仿真程序轉(zhuǎn)移到宿主機(jī)中的MATLAB提供的實(shí)時(shí)內(nèi)核中運(yùn)行，既保證了系統(tǒng)運(yùn)行的實(shí)時(shí)性，又縮減了系統(tǒng)硬件的規(guī)模，降低了模塊連接的復(fù)雜度，提高了運(yùn)行的可靠性。由于本發(fā)明采用了全自由度的半實(shí)物仿真，沒有將飛行器限制在萬向節(jié)上，因此仿真得到的數(shù)據(jù)具有更高的真實(shí)性。
[0068]本發(fā)明把實(shí)際的飛行器作為被控對(duì)象接入仿真回路，為MATLAB/Simulink中建好機(jī)理建模得到的數(shù)值仿真模型提供了一個(gè)參考，對(duì)于確認(rèn)機(jī)理建模得到的數(shù)值仿真模型的有效性具有重要意義
[0069]對(duì)于四旋翼飛行器的控制問題，數(shù)據(jù)驅(qū)動(dòng)的控制器設(shè)計(jì)方法(比如PID)不依賴于具體模型的參數(shù)，因此參數(shù)辨識(shí)的過程沒有太大存在的意義。在過去的幾十年中，各種新型的基于模型的控制器設(shè)計(jì)方法不斷涌現(xiàn)。為了利用這些方法，首先必須獲得準(zhǔn)確的被控對(duì)象模型。與機(jī)理建模得到的仿真模型相比，本發(fā)明所提出的參數(shù)辨識(shí)器可以得到更為準(zhǔn)確的對(duì)象模型，這為基于模型的控制器設(shè)計(jì)方法的應(yīng)用提供了必要的條件。
【專利附圖】
【附圖說明】
[0070]圖1為本發(fā)明所述的四旋翼飛行器半實(shí)物仿真方法的原理示意圖；
[0071]圖2為【具體實(shí)施方式】六所述的四旋翼飛行器的數(shù)值仿真方法的原理示意圖。
【具體實(shí)施方式】
[0072]【具體實(shí)施方式】一:參見圖1說明本實(shí)施方式，本實(shí)施方式所述的四旋翼飛行器半實(shí)物仿真方法，它是基于下述裝置實(shí)現(xiàn)的，該裝置包括仿真計(jì)算機(jī)1、機(jī)載微控制器2、1號(hào)無線收發(fā)器3、2號(hào)無線收發(fā)器4和機(jī)載傳感器5 ；
[0073]機(jī)載微控制器2的數(shù)據(jù)信號(hào)輸入端與機(jī)載傳感器5的數(shù)據(jù)信號(hào)輸出端連接,機(jī)載微控制器2的通訊端與2號(hào)無線收發(fā)器4的信號(hào)輸入輸出端連接，2號(hào)無線收發(fā)器4通過無線通信的方式與I號(hào)無線收發(fā)器3進(jìn)行數(shù)據(jù)傳輸，I號(hào)無線收發(fā)器3的信號(hào)輸入輸出端與仿真計(jì)算機(jī)I的信號(hào)輸入輸出端連接，
[0074]仿真計(jì)算機(jī)I內(nèi)嵌入Matlab/Simulink仿真模塊,所述的Matlab/Simulink仿真模塊包括數(shù)據(jù)輸入模塊1-1、狀態(tài)估計(jì)與濾波模塊1-2、姿態(tài)與高度控制模塊1-3、飛行狀態(tài)顯示模塊1-4、數(shù)據(jù)輸出模塊1-5、半實(shí)物仿真數(shù)據(jù)記錄模塊1-6和控制指令生成模塊1-7 ；
[0075]該半實(shí)物仿真方法的具體過程為:
[0076]首先，通過機(jī)載微控制器2米集機(jī)載傳感器5輸出的數(shù)據(jù)信號(hào),且機(jī)載微控制器2依次通過2號(hào)無線收發(fā)器4、I號(hào)無線收發(fā)器3將采集到的數(shù)據(jù)信號(hào)送至仿真計(jì)算機(jī)I的數(shù)據(jù)輸入模塊1-1，該數(shù)據(jù)信號(hào)包括測(cè)距讀數(shù)、三軸加速度讀數(shù)、三軸角速度讀數(shù)和三軸磁感應(yīng)強(qiáng)度信號(hào)；
[0077]通過數(shù)據(jù)輸入模塊1-1接收輸入的數(shù)據(jù)信號(hào)，并將該數(shù)據(jù)送至狀態(tài)估計(jì)與濾波模塊1-2，狀態(tài)估計(jì)與濾波模塊1-2根據(jù)接收的數(shù)據(jù)信號(hào)利用互補(bǔ)濾波的方法獲得兩組數(shù)據(jù)，該兩組數(shù)據(jù)中的第一組數(shù)據(jù)為姿態(tài)歐拉角估計(jì)值和高度估計(jì)值，第二組數(shù)據(jù)為角速度的估計(jì)值和豎直方向上速度的估計(jì)值，
[0078]且將第一組數(shù)據(jù)送至飛行狀態(tài)顯示模塊1-4，飛行狀態(tài)顯示模塊1-4用于實(shí)時(shí)顯示四旋翼飛行器的飛行狀態(tài)，
[0079]將第一組和第二組數(shù)據(jù)送至姿態(tài)與高度控制模塊1-3，姿態(tài)與高度控制模塊1-3根據(jù)控制指令生成模塊1-7產(chǎn)生的控制指令，計(jì)算獲得姿態(tài)和高度控制信號(hào)，然后經(jīng)過控制分配得到四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速，該四個(gè)電機(jī)的期望轉(zhuǎn)速通過數(shù)據(jù)輸出模塊1-5依次經(jīng)過I號(hào)無線收發(fā)器3、2號(hào)無線收發(fā)器4后送至機(jī)載微控制器2用于控制四旋翼飛行器的電子調(diào)速器及電機(jī)，
[0080]半實(shí)物仿真數(shù)據(jù)記錄模塊1-6用于存儲(chǔ)狀態(tài)估計(jì)與濾波模塊1-2輸出的兩組數(shù)據(jù)和四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速。
[0081]【具體實(shí)施方式】二:本實(shí)施方式與【具體實(shí)施方式】一所述的四旋翼飛行器半實(shí)物仿真方法的區(qū)別在于，所述的機(jī)載傳感器5包括三軸電子羅盤5-1、六軸運(yùn)動(dòng)傳感器5-2和超聲測(cè)距傳感器5-3，
[0082]三軸電子羅盤5-1用于米集三軸磁感應(yīng)強(qiáng)度信號(hào)，
[0083]六軸運(yùn)動(dòng)傳感器5-2用于采集三軸加速度讀數(shù)和三軸方位讀數(shù)，
[0084]超聲測(cè)距傳感器5-3用于采集測(cè)距讀數(shù)。
[0085]【具體實(shí)施方式】三:本實(shí)施方式與【具體實(shí)施方式】二所述的四旋翼飛行器半實(shí)物仿真方法的區(qū)別在于，所述的三軸電子羅盤5-1的采用型號(hào)為HMC5883L的集成電路實(shí)現(xiàn)，六軸運(yùn)動(dòng)傳感器5-2的型號(hào)為MPU6050的集成電路實(shí)現(xiàn)，超聲測(cè)距傳感器5_3采用型號(hào)為US-100的集成電路實(shí)現(xiàn)。
[0086]【具體實(shí)施方式】四:本實(shí)施方式與【具體實(shí)施方式】一、二或三所述的四旋翼飛行器半實(shí)物仿真方法的區(qū)別在于，所述的機(jī)載微控制器2采用TMS320F28335型DSP實(shí)現(xiàn)。
[0087]【具體實(shí)施方式】五:本實(shí)施方式與【具體實(shí)施方式】一、二或三所述的四旋翼飛行器半實(shí)物仿真方法的區(qū)別在于，所述的姿態(tài)與高度控制模塊1-3的優(yōu)化過程為，
[0088]在模型辨識(shí)子系統(tǒng)中，參數(shù)辨識(shí)器接收四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速Ω1； Ω2, Ω3, Ω4、姿態(tài)歐拉角估計(jì)值(A §, #)、高度估計(jì)值￡、角速度的估計(jì)值
(#, I,卩)和豎直方向上速度的估計(jì)值#，該參數(shù)辨識(shí)器根據(jù)待辨識(shí)模型結(jié)構(gòu)，求得參數(shù)向量的辨識(shí)結(jié)果，進(jìn)而確定了飛行器運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型，從而根據(jù)數(shù)值仿真子系統(tǒng)完成對(duì)姿態(tài)與高度控制模塊1-3的優(yōu)化，
[0089]所述的待辨識(shí)模型結(jié)構(gòu)通過下述的狀態(tài)空間方程描述:
【權(quán)利要求】
1.四旋翼飛行器半實(shí)物仿真方法，其特征在于，它是基于下述裝置實(shí)現(xiàn)的，該裝置包括仿真計(jì)算機(jī)(I)、機(jī)載微控制器(2)、I號(hào)無線收發(fā)器(3)、2號(hào)無線收發(fā)器(4)和機(jī)載傳感器(5)； 機(jī)載微控制器(2)的數(shù)據(jù)信號(hào)輸入端與機(jī)載傳感器(5)的數(shù)據(jù)信號(hào)輸出端連接，機(jī)載微控制器(2)的通訊端與2號(hào)無線收發(fā)器(4)的信號(hào)輸入輸出端連接，2號(hào)無線收發(fā)器(4)通過無線通信的方式與I號(hào)無線收發(fā)器(3)進(jìn)行數(shù)據(jù)傳輸，I號(hào)無線收發(fā)器(3)的信號(hào)輸入輸出端與仿真計(jì)算機(jī)(I)的信號(hào)輸入輸出端連接， 仿真計(jì)算機(jī)(I)內(nèi)嵌入Matlab/Simulink仿真模塊,所述的Matlab/Simulink仿真模塊包括數(shù)據(jù)輸入模塊(1-1)、狀態(tài)估計(jì)與濾波模塊(1-2)、姿態(tài)與高度控制模塊(1-3)、飛行狀態(tài)顯示模塊(1-4)、數(shù)據(jù)輸出模塊(1-5)、半實(shí)物仿真數(shù)據(jù)記錄模塊(1-6)和控制指令生成模塊(1-7)； 該半實(shí)物仿真方法的具體過程為: 首先，通過機(jī)載微控制器(2)采集機(jī)載傳感器(5)輸出的數(shù)據(jù)信號(hào)，且機(jī)載微控制器(2)依次通過2號(hào)無線收發(fā)器(4)、1號(hào)無線收發(fā)器(3)將采集到的數(shù)據(jù)信號(hào)送至仿真計(jì)算機(jī)(I)的數(shù)據(jù)輸入模塊(1-1)，該數(shù)據(jù)信號(hào)包括測(cè)距讀數(shù)、三軸加速度讀數(shù)、三軸角速度讀數(shù)和三軸磁感應(yīng)強(qiáng)度信號(hào)； 通過數(shù)據(jù)輸入模塊(1-1)接收輸入的數(shù)據(jù)信號(hào)，并將該數(shù)據(jù)送至狀態(tài)估計(jì)與濾波模塊(1-2)，狀態(tài)估計(jì)與濾波模塊(1-2)根據(jù)接收的數(shù)據(jù)信號(hào)利用互補(bǔ)濾波的方法獲得兩組數(shù)據(jù)，該兩組數(shù)據(jù)中的第一組數(shù)據(jù)為姿態(tài)歐拉角估計(jì)值和高度估計(jì)值，第二組數(shù)據(jù)為角速度的估計(jì)值和豎直方向上速度的估計(jì)值， 且將第一組數(shù)據(jù)送至飛行狀態(tài)顯示模塊(1-4)，飛行狀態(tài)顯示模塊(1-4)用于實(shí)時(shí)顯示四旋翼飛行器的飛行狀態(tài)， 將第一組和第二組數(shù)據(jù)送至姿態(tài)與高度控制模塊(1-3)，姿態(tài)與高度控制模塊(1-3)根據(jù)控制指令生成模塊(1-7)產(chǎn)生的控制指令，計(jì)算獲得姿態(tài)和高度控制信號(hào)，然后經(jīng)過控制分配得到四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速，該四個(gè)電機(jī)的期望轉(zhuǎn)速通過數(shù)據(jù)輸出模塊(1-5)依次經(jīng)過I號(hào)無線收發(fā)器(3)、2號(hào)無線收發(fā)器(4)后送至機(jī)載微控制器(2)用于控制四旋翼飛行器的電子調(diào)速器及電機(jī)， 半實(shí)物仿真數(shù)據(jù)記錄模塊(1-6)用于存儲(chǔ)狀態(tài)估計(jì)與濾波模塊(1-2)輸出的兩組數(shù)據(jù)和四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速。
2.根據(jù)權(quán)利要求1所述的四旋翼飛行器半實(shí)物仿真方法，其特征在于，所述的機(jī)載傳感器(5)包括三軸電子羅盤(5-1)、六軸運(yùn)動(dòng)傳感器(5-2)和超聲測(cè)距傳感器(5-3)， 三軸電子羅盤(5-1)用于采集三軸磁感應(yīng)強(qiáng)度信號(hào)， 六軸運(yùn)動(dòng)傳感器(5-2)用于采集三軸加速度讀數(shù)和三軸角速度讀數(shù)， 超聲測(cè)距傳感器(5)用于采集測(cè)距讀數(shù)。
3.根據(jù)權(quán)利要求2所述的四旋翼飛行器半實(shí)物仿真方法，其特征在于，所述的三軸電子羅盤(5-1)的采用型號(hào)為HMC5883L的集成電路實(shí)現(xiàn)，六軸運(yùn)動(dòng)傳感器(5_2)的型號(hào)為MPU6050的集成電路實(shí)現(xiàn)，超聲測(cè)距傳感器(5)采用型號(hào)為US-1OO的集成電路實(shí)現(xiàn)。
4.根據(jù)權(quán)利要求1、2或3所述的四旋翼飛行器半實(shí)物仿真方法，其特征在于，所述的機(jī)載微控制器(2)采用TMS320F28335型DSP實(shí)現(xiàn)。
5.根據(jù)權(quán)利要求1、2或3所述的四旋翼飛行器半實(shí)物仿真方法，其特征在于，所述的姿態(tài)與高度控制模塊(1-3)的優(yōu)化過程為， 在模型辨識(shí)子系統(tǒng)中，參數(shù)辨識(shí)器接收四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速Ω1； Ω2, Ω3, Ω4、姿態(tài)歐拉角估計(jì)值(I §, #)、高度估計(jì)值|、角速度的估計(jì)值(#? ^和豎直方向上速度的估計(jì)值fv,該參數(shù)辨識(shí)器根據(jù)待辨識(shí)模型結(jié)構(gòu)，求得參數(shù)向量的辨識(shí)結(jié)果，進(jìn)而確定了飛行器運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型，從而根據(jù)數(shù)值仿真子系統(tǒng)完成對(duì)姿態(tài)與高度控制模塊(1-3)的優(yōu)化， 所述的待辨識(shí)模型結(jié)構(gòu)通過下述的狀態(tài)空間方程描述:
所述的參數(shù)辨識(shí)器根據(jù)待辨識(shí)模型結(jié)構(gòu)，獲得參數(shù)向量的辨識(shí)結(jié)果的具體過程為: 當(dāng)四個(gè)電機(jī)的期望轉(zhuǎn)速Q(mào)1, Ω2, Ω3, Ω4、作為飛行器模型的輸入時(shí)，通過公式一和公式二求得部分預(yù)測(cè)狀態(tài)的數(shù)值解為ξ (k) = [ΦΟΟ，Θ (k), V(k),p(k),q(k),r(k),z(k),w(k)]T， 其次，采用代價(jià)函數(shù)
求取r。，
r0即為參數(shù)向量的辨識(shí)結(jié)果，r為待辨識(shí)參數(shù)向量,其中，r = [m, I, Ixx, Iyy, Izz, Jr, b, d] (Φ, θ , Ψ)為姿態(tài)歐拉角，ζ為高度，(p，q，r)為角速度，w為豎直方向上速度，待辨識(shí)參數(shù)向量r包括飛行器總質(zhì)量m,相對(duì)電機(jī)軸距離I,機(jī)體轉(zhuǎn)動(dòng)慣量(Ixx, Iyy, IJ、旋翼與電機(jī)轉(zhuǎn)子繞電機(jī)軸轉(zhuǎn)動(dòng)慣量1，升力系數(shù)b，扭矩系數(shù)d，N為記錄的采樣點(diǎn)數(shù)冰為采樣時(shí)間點(diǎn)，^%、4、仏和均為中間變量，g為重力加速度。
6.四旋翼飛行器的數(shù)值仿真方法，其特征在于，該方法的具體過程為， 首先，參數(shù)辨識(shí)器接收從半實(shí)物仿真系統(tǒng)獲得數(shù)據(jù)信息，該參數(shù)辨識(shí)器根據(jù)待辨識(shí)模型結(jié)構(gòu)，獲得參數(shù)向量的辨識(shí)結(jié)果，并通過數(shù)值仿真子系統(tǒng)根據(jù)獲得的參數(shù)向量的辨識(shí)結(jié)果，獲得飛行器運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型，從而完成四旋翼飛行器的數(shù)值仿真。 所述的參數(shù)辨識(shí)器接收從半實(shí)物仿真系統(tǒng)獲得數(shù)據(jù)信息包括四旋翼飛行器的四個(gè)電機(jī)的期望轉(zhuǎn)速Q(mào)1, Ω2，Ω3，ω4、姿態(tài)歐拉角估計(jì)值(A Θ, #)、高度估計(jì)值f、角速度的估計(jì)值I,纟)和豎直方向上速度的估計(jì)值#， 所述的待辨識(shí)模型結(jié)構(gòu)通過下述的狀態(tài)空間方程描述:
所述的參數(shù)辨識(shí)器根據(jù)待辨識(shí)模型結(jié)構(gòu)，獲得參數(shù)向量的辨識(shí)結(jié)果的具體過程為: 當(dāng)四個(gè)電機(jī)的期望轉(zhuǎn)速Q(mào)1, Ω2, Ω3, Ω4、作為飛行器模型的輸入時(shí)，通過公式一和公式二求得部分預(yù)測(cè)狀態(tài)的數(shù)值解為ξ (k) = [ΦΟΟ，Θ (k), V(k),p(k),q(k),r(k),z(k),w(k)]T，.V.其次，采用代價(jià)函數(shù)
求取rQ，r0即為參數(shù)向量的辨識(shí)結(jié)果，r為待辨識(shí)參數(shù)向量,其中，r = [m, I, Ixx, Iyy, Izz, Jr, b, d]T
9 待辨識(shí)參數(shù)向量r包括飛行器總質(zhì)量m,相對(duì)電機(jī)軸距離I,機(jī)體轉(zhuǎn)動(dòng)慣量(Ixx, Iyy, IJ、旋翼與電機(jī)轉(zhuǎn)子繞電機(jī)軸轉(zhuǎn)動(dòng)慣量1，升力系數(shù)b，扭矩系數(shù)d，N為記錄的采樣點(diǎn)數(shù)冰為采樣時(shí)間點(diǎn)，^%、4、仏和均為中間變量，g為重力加速度。
【文檔編號(hào)】G05B17/02GK104133379SQ201410404036
【公開日】2014年11月5日 申請(qǐng)日期:2014年8月15日 優(yōu)先權(quán)日:2014年8月15日
【發(fā)明者】胡慶雷, 陳卓, 苗楠, 李波 申請(qǐng)人:哈爾濱工業(yè)大學(xué)