本發(fā)明涉及無人機技術(shù)領(lǐng)域,具體而言,涉及一種坐標計算方法及裝置、飛行控制方法及系統(tǒng)、無人機。
背景技術(shù):
相關(guān)技術(shù)中,無人機的飛行控制一般是基于全球定位系統(tǒng)(Global Positioning System,GPS)坐標,GPS坐標是利用GPS經(jīng)緯度坐標點進行無人機的軌跡飛行,這種技術(shù)存在一定的缺陷,例如,在GPS信號較差的地方,比如室內(nèi)等,就無法完成相應(yīng)的軌跡飛行,導(dǎo)致應(yīng)用場景較少,用戶體驗不佳。
技術(shù)實現(xiàn)要素:
本發(fā)明的目的在于提供一種坐標計算方法及裝置、飛行控制方法及系統(tǒng)、無人機,以解決無人機在GPS信號較差的地方無法完成飛行的技術(shù)問題。
為了實現(xiàn)上述目的,本發(fā)明實施例采用的技術(shù)方案如下:
第一方面,本發(fā)明實施例提供了一種坐標計算裝置,應(yīng)用于無人機,所述坐標裝置包括:建立模塊,用于建立基于所述無人機的臨時坐標系;獲取模塊,用于獲取所述無人機于所述臨時坐標系三個軸向的前一時刻速度、前一時刻坐標以及加速度值;計算模塊,用于基于所述前一時刻速度和加速度值,計算所述無人機于所述臨時坐標系三個軸向的實時速度;以及基于所述無人機于所述臨時坐標系三個軸向的實時速度和所述前一時刻坐標,計算所述無人機于所述臨時坐標系的實時坐標。
第二方面,本發(fā)明實施例還提供了一種坐標計算方法,應(yīng)用于無人機,所述坐標計算方法包括:建立基于所述無人機的臨時坐標系;獲取所述無人機于所述臨時坐標系三個軸向的前一時刻速度、前一時刻坐標以及加速度值;基于所述前一時刻速度和加速度值,計算所述無人機于所述臨時坐標系三個軸向的實時速度;基于所述無人機于所述臨時坐標系三個軸向的實時速度和所述前一時刻坐標,計算所述無人機于所述臨時坐標系的實時坐標。
第三方面,本發(fā)明實施例還提供了一種應(yīng)用前述的坐標計算裝置的飛行控制系統(tǒng),應(yīng)用于無人機,所述飛行控制系統(tǒng)還包括:映射模塊,用于將預(yù)設(shè)飛行軌跡映射到所述臨時坐標系;執(zhí)行模塊,用于依據(jù)所述實時坐標和映射到所述臨時坐標系的預(yù)設(shè)飛行軌跡,控制所述無人機在所述預(yù)設(shè)飛行軌跡上飛行。
第四方面,本發(fā)明實施例還提供了一種應(yīng)用前述的坐標計算方法的飛行控制方法,應(yīng)用于無人機,所述飛行控制方法還包括:將預(yù)設(shè)飛行軌跡映射到所述臨時坐標系;依據(jù)所述實時坐標和映射到所述臨時坐標系的預(yù)設(shè)飛行軌跡,控制所述無人機所述臨時坐標系中沿所述預(yù)設(shè)飛行軌跡飛行。
第五方面,本發(fā)明實施例還提供了一種無人機,所述無人機包括:存儲器;處理器;及飛行控制系統(tǒng);所述飛行控制系統(tǒng)包括坐標計算裝置,所述坐標計算裝置安裝于所述存儲器中并包括一個或多個由所述處理器執(zhí)行的軟件功能模塊,所述坐標計算裝置包括:建立模塊,用于建立基于所述無人機的臨時坐標系;獲取模塊,用于獲取所述無人機于所述臨時坐標系三個軸向的前一時刻速度、前一時刻坐標以及加速度值;計算模塊,用于基于所述前一時刻速度和加速度值,計算所述無人機于所述臨時坐標系三個軸向的實時速度;以及基于所述無人機于所述臨時坐標系三個軸向的實時速度和所述前一時刻坐標,計算所述無人機于所述臨時坐標系的實時坐標。
本發(fā)明實施例提供的坐標計算方法及裝置、飛行控制方法及系統(tǒng)、無人機,通過建立基于無人機的臨時坐標系,獲取所述無人機于所述臨時坐標系三個軸向的前一時刻速度、前一時刻坐標以及加速度值;基于所述前一時刻速度和加速度值,計算所述無人機于所述臨時坐標系三個軸向的實時速度;基于所述無人機于所述臨時坐標系三個軸向的實時速度和所述前一時刻坐標,計算所述無人機于所述臨時坐標系的實時坐標。本發(fā)明可以使無人機的飛行不限制在特定的場景,即使在GPS信號較弱的地方也能完成航線飛行,提高了用戶體驗。
為使本發(fā)明的上述目的、特征和優(yōu)點能更明顯易懂,下文特舉較佳實施例,并配合所附附圖,作詳細說明如下。
附圖說明
為了更清楚地說明本發(fā)明實施例的技術(shù)方案,下面將對實施例中所需要使用的附圖作簡單地介紹,應(yīng)當理解,以下附圖僅示出了本發(fā)明的某些實施例,因此不應(yīng)被看作是對范圍的限定,對于本領(lǐng)域普通技術(shù)人員來講,在不付出創(chuàng)造性勞動的前提下,還可以根據(jù)這些附圖獲得其他相關(guān)的附圖。
圖1示出了本發(fā)明實施例提供的無人機的結(jié)構(gòu)框圖。
圖2示出了本發(fā)明實施例提供的無人機坐標計算裝置的功能框圖。
圖3示出了本發(fā)明實施例提供的無人機坐標計算方法的流程圖。
圖4示出了本發(fā)明實施例提供的無人機飛行控制系統(tǒng)的功能框圖。
圖5示出了本發(fā)明實施例提供的無人機飛行控制方法的流程圖。
具體實施方式
如圖1所示,是無人機100的方框示意圖。所述無人機100包括飛行控制系統(tǒng)300(其包括坐標計算裝置200)、存儲器101、存儲控制器102、處理器103、外設(shè)接口104、輸入輸出單元105以及傳感器組件106及其他。所述存儲器101、存儲控制器102、處理器103以及外設(shè)接口104、輸入輸出單元105以及傳感器組件106各元件相互之間直接或間接地電性連接,以實現(xiàn)數(shù)據(jù)的傳輸或交互。例如,這些元件相互之間可通過一條或多條通訊總線或信號線實現(xiàn)電性連接。所述坐標計算裝置200包括至少一個可以軟件或固件(firmware)的形式存儲于所述存儲器101中的軟件功能模塊。所述處理器103用于執(zhí)行存儲器101中存儲的可執(zhí)行模塊,例如所述坐標計算裝置200包括的軟件功能模塊或計算機程序。
其中,存儲器101可以是,但不限于,隨機存取存儲器101(Random Access Memory,RAM),只讀存儲器101(Read Only Memory,ROM),可編程只讀存儲器101(Programmable Read-Only Memory,PROM),可擦除只讀存儲器101(Erasable Programmable Read-Only Memory,EPROM),電可擦除只讀存儲器101(Electric Erasable Programmable Read-Only Memory,EEPROM)等。其中,存儲器101用于存儲程序,所述處理器103在接收到執(zhí)行指令后,執(zhí)行所述程序,本發(fā)明實施例任一實施例揭示的流程定義的服務(wù)器/計算機所執(zhí)行的方法可以應(yīng)用于處理器103中,或者由處理器103實現(xiàn)。
處理器103可能是一種集成電路芯片,具有信號的處理能力。上述的處理器103可以是通用處理器,包括中央處理器(Central Processing Unit,簡稱CPU)、網(wǎng)絡(luò)處理器103(Network Processor,簡稱NP)等;還可以是數(shù)字信號處理器(DSP)、專用集成電路(ASIC)、現(xiàn)成可編程門陣列(FPGA)或者其他可編程邏輯器件、分立門或者晶體管邏輯器件、分立硬件組件??梢詫崿F(xiàn)或者執(zhí)行本發(fā)明實施例中的公開的各方法、步驟及邏輯框圖。處理器103可以是微處理器或者該處理器103也可以是任何常規(guī)的處理器等。
所述外設(shè)接口104將各種輸入/輸出裝置耦合至處理器103以及存儲器101。在一些實施例中,外設(shè)接口104,處理器103以及存儲控制器102可以在單個芯片中實現(xiàn)。在其他一些實例中,他們可以分別由獨立的芯片實現(xiàn)。
輸入輸出單元105用于提供給用戶輸入數(shù)據(jù)實現(xiàn)用戶與該無人機100的交互。所述輸入輸出單元105可以是,但不限于,按鍵,用于響應(yīng)用戶的操作而輸出對應(yīng)的信號。
傳感器組件106用于響應(yīng)用戶的操作而輸出對應(yīng)的信號。在本實施例中,該傳感器組件106可以是,但不限于,GPS傳感器、光流傳感器、超聲波傳感器、聲控傳感器、氣壓計、慣導(dǎo)系統(tǒng)等。上述慣導(dǎo)系統(tǒng)包括加速度計與陀螺儀兩部分,常用的加速度計包括壓電型、微型電子機械系統(tǒng)(Micro-Electro Mechanical System,MEMS)等,用于測量加速度;常用的陀螺儀包括微型電子機械系統(tǒng)(Micro-Electro Mechanical System,MEMS)、靜電陀螺、光纖陀螺等,用于測量角速度。
可以理解,圖1所示的結(jié)構(gòu)僅為示意,無人機100還可包括比圖1中所示更多或者更少的組件,或者具有與圖1所示不同的配置。圖1中所示的各組件可以采用硬件、軟件或其組合實現(xiàn)。
下面將結(jié)合本發(fā)明實施例中附圖,對本發(fā)明實施例中的技術(shù)方案進行清楚、完整地描述,顯然,所描述的實施例僅僅是本發(fā)明一部分實施例,而不是全部的實施例。通常在此處附圖中描述和示出的本發(fā)明實施例的組件可以以各種不同的配置來布置和設(shè)計。因此,以下對在附圖中提供的本發(fā)明的實施例的詳細描述并非旨在限制要求保護的本發(fā)明的范圍,而是僅僅表示本發(fā)明的選定實施例?;诒景l(fā)明的實施例,本領(lǐng)域技術(shù)人員在沒有做出創(chuàng)造性勞動的前提下所獲得的所有其他實施例,都屬于本發(fā)明保護的范圍。
應(yīng)注意到:相似的標號和字母在下面的附圖中表示類似項,因此,一旦某一項在一個附圖中被定義,則在隨后的附圖中不需要對其進行進一步定義和解釋。同時,在本發(fā)明的描述中,術(shù)語“第一”、“第二”等僅用于區(qū)分描述,而不能理解為指示或暗示相對重要性。
第一實施例
請參閱圖2,本發(fā)明第一實施例提供一種坐標計算裝置200,應(yīng)用于無人機100,坐標計算裝置200包括:GPS坐標系屏蔽模塊210、建立模塊220、檢測模塊230、獲取模塊240以及計算模塊250。
GPS坐標系屏蔽模塊210,用于在建立臨時坐標系之前屏蔽GPS坐標系。
由于無人機100通常情況使用的是GPS坐標系,在本發(fā)明實施例中,需要無人機100建立臨時坐標系以替代GPS坐標系,因此,在建立臨時坐標系之前,可以由GPS坐標系屏蔽模塊210屏蔽GPS坐標系。
建立模塊220,用于建立基于所述無人機100的臨時坐標系。
在本發(fā)明實施例中,用戶可以通過遙控終端發(fā)送啟用臨時坐標系的控制指令,無人機100收到該控制指令后,由建立模塊210建立臨時坐標系,該臨時坐標系是基于無人機100自身的位置臨時建立的,例如,無人機100在收到啟用臨時坐標系的控制指令時,以無人機100的當前位置作為臨時坐標系的坐標原點O(0,0,0),以正北方向為Y軸方向,以正東方向為X軸方向,以垂直于水平面向上的方向為Z軸方向,建立滿足右手定則的臨時坐標系(特殊需求下,坐標系X,Y,Z軸方向可任意定義,右手定則不是必要的)。
檢測模塊230,用于檢測無人機100的慣導(dǎo)系統(tǒng)是否工作正常。
在本發(fā)明實施例中,檢測模塊230檢測慣導(dǎo)系統(tǒng)是否工作正??梢园ㄒ韵聝煞N方法:1、檢測慣導(dǎo)系統(tǒng)是否輸出工作正常的標志;2、對慣導(dǎo)系統(tǒng)在預(yù)設(shè)時間段內(nèi)輸出的數(shù)據(jù)進行分析,判斷數(shù)據(jù)是否正常。
檢測模塊230還用于檢測無人機100的速度測量儀是否工作正常。
在本發(fā)明實施例中,無人機100除了設(shè)有慣導(dǎo)系統(tǒng)以外,還可以設(shè)有其他速度測量儀,例如光流、GPS測速等,檢測模塊230檢測速度測量儀是否工作正??梢园ㄒ韵聝煞N方法:1、檢測速度測量儀是否輸出工作正常的標志;2、對速度測量儀在預(yù)設(shè)時間段內(nèi)輸出的數(shù)據(jù)進行分析,判斷數(shù)據(jù)是否正常。
檢測模塊230還用于檢測無人機100的位移測量儀是否工作正常。
在本發(fā)明實施例中,無人機100除了設(shè)有慣導(dǎo)系統(tǒng)、速度測量儀以外,還可以設(shè)有其他位移測量儀,例如雙目測距、超聲測距等,檢測模塊230檢測位移測量儀是否工作正常可以包括以下兩種方法:1、檢測位移測量儀是否輸出工作正常的標志;2、對位移測量儀在預(yù)設(shè)時間段內(nèi)輸出的數(shù)據(jù)進行分析,判斷數(shù)據(jù)是否正常。
獲取模塊240,用于獲取無人機100于臨時坐標系三個軸向的前一時刻速度、前一時刻坐標以及加速度值。
在本發(fā)明實施例中,若檢測模塊230檢測到無人機100的慣導(dǎo)系統(tǒng)工作正常,則由獲取模塊230獲取無人機100于臨時坐標系三個軸向的前一時刻速度、前一時刻坐標以及加速度值。其中,獲取模塊230可以從慣導(dǎo)系統(tǒng)的加速度計獲取所述加速度值,以及從無人機100的存儲器101獲取所述前一時刻速度、前一時刻坐標。
計算模塊250,用于基于所述前一時刻速度和加速度值,計算無人機100于臨時坐標系三個軸向的實時速度;以及基于無人機于臨時坐標系三個軸向的實時速度和所述前一時刻坐標,計算無人機100于臨時坐標系的實時坐標。
在本發(fā)明實施例中,基于所述三個軸向的前一時刻速度和加速度值,計算無人機100于臨時坐標系三個軸向的實時速度包括速度測量儀工作異常和工作正常兩種情況下的不同計算方法。
當速度測量儀工作異常時,計算模塊250將所述三個軸向的前一時刻速度和加速度值進行積分計算,得到無人機100于臨時坐標系三個軸向的實時速度。計算公式如下:
vx(t)=vx(t-1)+ax·dt
vy(t)=vy(t-1)+ay·dt
vz(t)=vz(t-1)+az·dt
其中,vx(t)、vy(t)、vz(t)分別表示無人機100于臨時坐標系的X、Y、Z三個軸向?qū)崟r速度,vx(t-1)、vy(t-1)、vz(t-1)分別表示無人機100于臨時坐標系的X、Y、Z三個軸向的前一時刻速度,ax、ay、az分別表示無人機100于臨時坐標系的X、Y、Z三個軸向的加速度值。
當所述速度測量儀工作正常時,計算模塊250將所述三個軸向的前一時刻速度和加速度值進行積分計算得到的速度,與所述速度測量儀輸出的速度進行加權(quán)計算,得到所述無人機于所述臨時坐標系三個軸向的實時速度。計算公式如下(其中,速度測量儀為光流測量儀):
vx(t)=vix(t)·wix+vox(t)·wox
vy(t)=viy(t)·wiy+voy(t)·woy
vz(t)=viz(t)·wiz+voz(t)·woz
其中,vx(t)、vy(t)、vz(t)分別表示無人機100于臨時坐標系的X、Y、Z三個軸向的實時速度,vix(t)、viy(t)、viz(t)分別為所述三個軸向的前一時刻速度和加速度值進行積分計算得到的速度(亦即慣導(dǎo)系統(tǒng)得出的三個軸向的慣導(dǎo)速度),wix、wiy、wiz分別為所述三個軸向的慣導(dǎo)速度所占權(quán)值,wix、wiy、wiz的大小根據(jù)慣導(dǎo)系統(tǒng)的精度而定,vox(t)、voy(t)、voz(t)分別為光流測量儀輸出的所述三個軸向的光流速度,wox、woy、woy分別為所述三個軸向的光流速度所占權(quán)值,wox、woy、woy的大小根據(jù)光流測量儀的精度而定。
可以理解,速度測量儀還可以更多,只需將慣導(dǎo)系統(tǒng)得出的慣導(dǎo)速度與每一個速度測量儀輸出的速度進行加權(quán)計算即可得到無人機100于臨時坐標系的X、Y、Z三個軸向的實時速度。
無人機100在每一時刻的實時速度均可以被存儲于無人機100的存儲器101,可以理解的是,在某一時刻無人機100于所述臨時坐標系三個軸向的實時速度,可以作為下一時刻的前一時刻速度。
在本發(fā)明實施例中,基于無人機于臨時坐標系三個軸向的實時速度和所述前一時刻坐標,計算無人機100于臨時坐標系的實時坐標,包括位移測量儀工作異常和工作正常兩種情況下的不同計算方法。
當所述位移測量儀工作異常時,計算模塊250將所述無人機于所述臨時坐標系三個軸向的前一時刻坐標和實時速度進行積分計算,得到所述無人機于所述臨時坐標系的實時坐標。計算公式如下:
Lx(t)=Lx(t-1)+vx·dt
Ly(t)=Ly(t-1)+vy·dt
Lz(t)=Lz(t-1)+vz·dt
其中,Lx(t)、Ly(t)、Lz(t)分別表示所述無人機于所述臨時坐標系三個軸向的實時坐標,Lx(t-1)、Ly(t-1)、Lz(t-1)分別表示所述無人機于所述臨時坐標系三個軸向的前一時刻坐標,vx、vy、vz分別表示所述無人機于所述臨時坐標系三個軸向的實時速度。其中,vx、vy、vz可以為速度測量儀工作正常時的實時速度,也可以為速度測量儀工作異常時的實時速度。
當所述位移測量儀工作正常時,計算模塊250將所述無人機于所述臨時坐標系三個軸向的前一時刻坐標和實時速度進行積分計算,再與所述位移測量儀輸出的位移進行加權(quán)計算,得到所述無人機于所述臨時坐標系三個軸向的實時坐標。計算公式如下(其中,位移測量儀為雙目測量儀):
Lx(t)=Lx(t-1)+vx·dt·wvx+ΔLsx·wsx
Ly(t)=Ly(t-1)+vy·dt·wvy+ΔLsy·wsy
Lz(t)=Lz(t-1)+vz·dt·wvz+ΔLsz·wsz
其中,Lx(t)、Ly(t)、Lz(t)分別表示無人機于所述臨時坐標系三個軸向的實時坐標,Lx(t-1)、Ly(t-1)、Lz(t-1)分別表示無人機于所述臨時坐標系三個軸向的前一時刻坐標,vx、vy、vz分別表示無人機于臨時坐標系三個軸向的實時速度,vx·dt、vy·dt、vz·dt分別表示無人機于臨時坐標系三個軸向的速度積分得到的當前位移,wvx、wvy、wvz分別表示無人機于臨時坐標系三個軸向的速度積分得到的當前位移所占權(quán)值,ΔLsx、ΔLsy、ΔLsz分別表示雙目測量儀于臨時坐標系三個軸向的當前位移,wsx、wsy、wsz分別表示雙目測量儀于臨時坐標系三個軸向的當前位移所占權(quán)值,wsx、wsy、wsz的大小根據(jù)雙目測量儀的精度而定。
可以理解,位移測量儀還可以更多,只需將無人機于所述臨時坐標系三個軸向的前一時刻坐標和實時速度進行積分計算,然后與每一個位移測量儀輸出的位移進行加權(quán)計算,即可得到所述無人機于所述臨時坐標系三個軸向的實時坐標。
無人機100在每一時刻的實時坐標均可以被存儲于無人機100的存儲器101,可以理解的是,在某一時刻無人機100于所述臨時坐標系三個軸向的實時坐標,可以作為下一時刻的前一時刻坐標。
第二實施例
請參閱圖3,本發(fā)明第二實施例提供一種坐標計算方法,應(yīng)用于無人機100,坐標計算方法包括以下步驟:
步驟S1,檢測是否啟用臨時坐標系。
在本發(fā)明實施例中,步驟S1可以由檢測模塊230執(zhí)行。當用戶通過遙控終端發(fā)送啟用臨時坐標系的控制指令,無人機100的檢測模塊230收到該控制指令后,則判定需要啟用臨時坐標系,執(zhí)行步驟S2。若無人機100的檢測模塊230未收到該控制指令,則判定不需要啟用臨時坐標系,繼續(xù)執(zhí)行步驟S1。
步驟S2,屏蔽GPS坐標系。
在本發(fā)明實施例中,步驟S2可以由GPS坐標系屏蔽模塊210執(zhí)行。由于無人機100通常情況使用的是GPS坐標系,在本發(fā)明實施例中,需要無人機100建立臨時坐標系以替代GPS坐標系,因此,在建立臨時坐標系之前,可以由GPS坐標系屏蔽模塊210屏蔽GPS坐標系。
步驟S3,建立基于無人機的臨時坐標系。
在本發(fā)明實施例中,步驟S3可以由建立模塊220執(zhí)行。無人機100收到啟用臨時坐標系的控制指令后,由建立模塊220建立臨時坐標系,該臨時坐標系是基于無人機100自身的位置臨時建立的,例如,無人機100在收到啟用臨時坐標系的控制指令時,以無人機100的當前位置作為臨時坐標系的坐標原點O(0,0,0),以正北方向為Y軸方向,以正東方向為X軸方向,以垂直于水平面向上的方向為Z軸方向,建立滿足右手定則的臨時坐標系(特殊需求下,坐標系X,Y,Z軸方向可任意定義,右手定則不是必要的)。
步驟S4,檢測慣導(dǎo)系統(tǒng)是否工作正常。
在本發(fā)明實施例中,步驟S4可以由檢測模塊230執(zhí)行。檢測模塊230檢測慣導(dǎo)系統(tǒng)是否工作正常可以包括以下兩種方法:1、檢測慣導(dǎo)系統(tǒng)是否輸出工作正常的標志;2、對慣導(dǎo)系統(tǒng)在預(yù)設(shè)時間段內(nèi)輸出的數(shù)據(jù)進行分析,判斷數(shù)據(jù)是否正常。
若慣導(dǎo)系統(tǒng)工作異常,則流程結(jié)束;若慣導(dǎo)系統(tǒng)工作正常,則執(zhí)行獲取無人機100于臨時坐標系三個軸向的前一時刻速度、前一時刻坐標以及加速度值的步驟,其中,慣導(dǎo)系統(tǒng)包括加速度計,所述獲取所述無人機100于所述臨時坐標系三個軸向的加速度值的步驟包括:從所述加速度計獲取所述加速度值,以及從所述無人機獲取預(yù)先存儲的前一時刻速度和前一時刻坐標。然后,再執(zhí)行步驟S5。
步驟S5,檢測速度傳感器是否工作正常。
在本發(fā)明實施例中,步驟S5可以由檢測模塊230執(zhí)行。無人機100除了設(shè)有慣導(dǎo)系統(tǒng)以外,還可以設(shè)有其他速度測量儀,例如光流、GPS測速等,檢測模塊230檢測速度測量儀是否工作正??梢园ㄒ韵聝煞N方法:1、檢測速度測量儀是否輸出工作正常的標志;2、對速度測量儀在預(yù)設(shè)時間段內(nèi)輸出的數(shù)據(jù)進行分析,判斷數(shù)據(jù)是否正常。
若速度傳感器工作正常,則執(zhí)行步驟S7;若速度傳感器工作異常則執(zhí)行步驟S6。其中,步驟S6和步驟S7是基于所述前一時刻速度和加速度值,計算無人機100于臨時坐標系三個軸向的實時速度。
步驟S6,將所述前一時刻速度和加速度值進行積分計算,得到所述無人機于所述臨時坐標系三個軸向的實時速度。
在本發(fā)明實施例中,步驟S6可以由計算模塊250執(zhí)行。當速度測量儀工作異常時,計算模塊250執(zhí)行步驟S6。計算公式如下:
vx(t)=vx(t-1)+ax·dt
vy(t)=vy(t-1)+ay·dt
vz(t)=vz(t-1)+az·dt
其中,vx(t)、vy(t)、vz(t)分別表示無人機100于臨時坐標系的X、Y、Z三個軸向?qū)崟r速度,vx(t-1)、vy(t-1)、vz(t-1)分別表示無人機100于臨時坐標系的X、Y、Z三個軸向的前一時刻速度,ax、ay、az分別表示無人機100于臨時坐標系的X、Y、Z三個軸向的加速度值。
步驟S7,將所述前一時刻速度和加速度值進行積分計算得到的速度,與所述速度測量儀輸出的速度進行加權(quán)計算,得到所述無人機于所述臨時坐標系三個軸向的實時速度。
在本發(fā)明實施例中,步驟S7可以由計算模塊250執(zhí)行。當所述速度測量儀工作正常時,計算模塊250執(zhí)行步驟S7,計算公式如下:
vx(t)=vix(t)·wix+vox(t)·wox
vy(t)=viy(t)·wiy+voy(t)·woy
vz(t)=viz(t)·wiz+voz(t)·woz
其中,vx(t)、vy(t)、vz(t)分別表示無人機100于臨時坐標系的X、Y、Z三個軸向的實時速度,vix(t)、viy(t)、viz(t)分別為所述三個軸向的前一時刻速度和加速度值進行積分計算得到的速度(亦即慣導(dǎo)系統(tǒng)得出的三個軸向的慣導(dǎo)速度),wix、wiy、wiz分別為所述三個軸向的慣導(dǎo)速度所占權(quán)值,wix、wiy、wiz的大小根據(jù)慣導(dǎo)系統(tǒng)的精度而定,vox(t)、voy(t)、voz(t)分別為光流測量儀輸出的所述三個軸向的光流速度,wox、woy、woy分別為所述三個軸向的光流速度所占權(quán)值,wox、woy、woy的大小根據(jù)光流測量儀的精度而定。
無人機100在每一時刻的實時速度均可以被存儲于無人機100的存儲器101,可以理解的是,在某一時刻無人機100于所述臨時坐標系三個軸向的實時速度,可以作為下一時刻的前一時刻速度。
步驟S8,檢測位移傳感器是否工作正常。
在本發(fā)明實施例中,步驟S8可以由檢測模塊230執(zhí)行。無人機100除了設(shè)有慣導(dǎo)系統(tǒng)和速度傳感器以外,還可以設(shè)有其他位移測量儀,例如雙目測距、超聲測距等,檢測模塊230檢測位移測量儀是否工作正??梢园ㄒ韵聝煞N方法:1、檢測位移測量儀是否輸出工作正常的標志;2、對位移測量儀在預(yù)設(shè)時間段內(nèi)輸出的數(shù)據(jù)進行分析,判斷數(shù)據(jù)是否正常。
在速度測量儀工作正常的情況下,若位移傳感器工作正常,則執(zhí)行步驟S10,若位移傳感器工作異常,則執(zhí)行步驟S9;在速度測量儀工作異常的情況下,若位移傳感器工作正常,則執(zhí)行步驟S10,若位移傳感器工作異常,則執(zhí)行步驟S9。其中,步驟S9和步驟S10是基于無人機于臨時坐標系三個軸向的前一時刻坐標和實時速度,計算無人機100于臨時坐標系的實時坐標。
步驟S9,將所述無人機于所述臨時坐標系三個軸向的前一時刻坐標和實時速度進行積分計算,得到所述無人機于所述臨時坐標系的實時坐標。
在本發(fā)明實施例中,步驟S9可以由計算模塊250執(zhí)行。計算公式如下:
Lx(t)=Lx(t-1)+vx·dt
Ly(t)=Ly(t-1)+vy·dt
Lz(t)=Lz(t-1)+vz·dt
其中,Lx(t)、Ly(t)、Lz(t)分別表示所述無人機于所述臨時坐標系三個軸向的實時坐標,Lx(t-1)、Ly(t-1)、Lz(t-1)分別表示所述無人機于所述臨時坐標系三個軸向的前一時刻坐標,vx、vy、vz分別表示所述無人機于所述臨時坐標系三個軸向的實時速度。其中,vx、vy、vz可以為速度測量儀工作正常時(步驟S7)計算出的實時速度,也可以為速度測量儀工作異常時(步驟S6)計算出的實時速度。
步驟S10,將所述無人機于所述臨時坐標系三個軸向的前一時刻坐標和實時速度進行積分計算,再與所述位移測量儀輸出的位移進行加權(quán)計算,得到所述無人機于所述臨時坐標系三個軸向的實時坐標。
在本發(fā)明實施例中,步驟S10可以由計算模塊250執(zhí)行。計算公式如下(其中,位移測量儀為雙目測量儀):
Lx(t)=Lx(t-1)+vx·dt·wvx+ΔLsx·wsx
Ly(t)=Ly(t-1)+vy·dt·wvy+ΔLsy·wsy
Lz(t)=Lz(t-1)+vz·dt·wvz+ΔLsz·wsz
其中,Lx(t)、Ly(t)、Lz(t)分別表示無人機于所述臨時坐標系三個軸向的實時坐標,Lx(t-1)、Ly(t-1)、Lz(t-1)分別表示無人機于所述臨時坐標系三個軸向的前一時刻坐標,vx、vy、vz分別表示無人機于臨時坐標系三個軸向的實時速度,vx·dt、vy·dt、vz·dt分別表示無人機于臨時坐標系三個軸向的速度積分得到的當前位移,wvx、wvy、wvz分別表示無人機于臨時坐標系三個軸向的速度積分得到的當前位移所占權(quán)值,ΔLsx、ΔLsy、ΔLsz分別表示雙目測量儀于臨時坐標系三個軸向的當前位移,wsx、wsy、wsz分別表示雙目測量儀于臨時坐標系三個軸向的當前位移所占權(quán)值,wsx、wsy、wsz的大小根據(jù)雙目測量儀的精度而定。
步驟S11,將實時坐標發(fā)送給存儲器。
在本發(fā)明實施例中,步驟S11可以由發(fā)送模塊(圖中未示)執(zhí)行。無人機100在每一時刻的實時坐標均可以被發(fā)送至無人機100的存儲器101進行存儲,可以理解的是,在某一時刻無人機100于所述臨時坐標系三個軸向的實時坐標,可以作為下一時刻的前一時刻坐標。
步驟S12,檢測是否關(guān)閉臨時坐標系。
在本發(fā)明實施例中,步驟S12可以由檢測模塊230執(zhí)行。用戶可以通過遙控終端發(fā)送關(guān)閉臨時坐標系的控制指令。若無人機的檢測模塊230接收到關(guān)閉臨時坐標系的控制指令,則判定需要關(guān)閉臨時坐標系,流程結(jié)束;若無人機的檢測模塊230未接收到關(guān)閉臨時坐標系的控制指令,則判定不需要關(guān)閉臨時坐標系,繼續(xù)執(zhí)行步驟S4。
第三實施例
請參閱圖4,本發(fā)明第三實施例提供一種飛行控制系統(tǒng)300,其應(yīng)用實施例一中所述的坐標計算裝置200,該飛行控制系統(tǒng)300可以應(yīng)用于無人機。飛行控制系統(tǒng)300包括:
坐標計算裝置200,用于計算無人機100于臨時坐標系的實時坐標。
在本發(fā)明實施例中,坐標計算裝置200計算無人機100于臨時坐標系的實時坐標的過程如實施例一所述,在此不再贅述。
映射模塊310,用于將預(yù)設(shè)飛行軌跡映射到臨時坐標系。
在本發(fā)明實施例中,可以利用遙控終端預(yù)先設(shè)定無人機100的飛行軌跡,預(yù)設(shè)飛行軌跡可以為環(huán)繞飛行、直線飛行等等。
對于復(fù)雜的飛行軌跡,可以預(yù)先將飛行軌跡存儲于無人機100的存儲器101中,該預(yù)先存儲的飛行軌跡是基于原始坐標系,該原始坐標系是以軌跡開始點為原點,以正北方向為Y軸正方向,以正東為X軸正方向建立的符合右手定則的坐標系。
首先,計算將原始坐標系旋轉(zhuǎn)至臨時坐標系的旋轉(zhuǎn)矩陣,旋轉(zhuǎn)矩陣可以表示為:
其中Aa->b'表示原始坐標系a方向上單位矢量在臨時坐標系b'方向上的投影大小。
然后,將原始坐標系下的軌跡坐標乘以旋轉(zhuǎn)矩陣,得到臨時坐標系下的軌跡坐標,計算公式如下:
最后,將計算得到的臨時坐標系下的軌跡坐標加上設(shè)定的軌跡起始點坐標,得到臨時坐標系下的軌跡,實現(xiàn)了將預(yù)設(shè)飛行軌跡映射到臨時坐標系。
對于簡單的飛行軌跡,例如環(huán)繞飛行、直線飛行等,可以無需預(yù)先將飛行軌跡存儲于無人機100的存儲器101中,而可以臨時計算。
當無人機執(zhí)行環(huán)繞飛行時,由于在臨時坐標系下,無人機均為圍繞臨時坐標系的原點飛行,所以在無人機得到了臨時坐標系下的軌跡起始點(Ix',Iy',Iz')后,即可得到半徑:
然后,計算飛行軌跡為:
Ix(t)=R·cos(θ0+ω·t)
Iy(t)=R·sin(θ0+ω·t)
其中θ0為軌跡起始點相對于臨時坐標系的原點的方向與y軸的夾角。
執(zhí)行模塊320,用于依據(jù)所述實時坐標和映射到所述臨時坐標系的預(yù)設(shè)飛行軌跡,控制所述無人機在所述預(yù)設(shè)飛行軌跡上飛行。
在本發(fā)明實施例中,執(zhí)行模塊320首先依據(jù)實時坐標控制無人機飛行至預(yù)設(shè)飛行軌跡上的軌跡起始點,然后在收到遙控終端的開始指令后,依據(jù)實時坐標控制無人機依次到達預(yù)設(shè)飛行軌跡上的各個目標點,以實現(xiàn)無人機在所述預(yù)設(shè)飛行軌跡上飛行。
第四實施例
請參閱圖5,本發(fā)明第四實施例提供一種飛行控制方法,其應(yīng)用實施例二中所述的坐標計算方法,該飛行控制方法可以應(yīng)用于無人機。飛行控制方法包括以下步驟:
步驟S21,確認以當前點為坐標原點。
在本發(fā)明實施例中,步驟S21可以由坐標計算裝置200的檢測模塊230執(zhí)行。當檢測模塊230接收到遙控終端發(fā)送的啟用臨時坐標系的控制指令后,則判定以當前點為坐標原點。當無人機執(zhí)行環(huán)繞飛行時,坐標原點可以作為環(huán)繞點,無人機環(huán)繞坐標原點飛行。
步驟S22,屏蔽GPS坐標系。
在本發(fā)明實施例中,步驟S22可以由坐標計算裝置200的GPS坐標系屏蔽模塊210執(zhí)行。屏蔽GPS坐標系的方法與實施例二中所述的方法相同,在此不再贅述。
步驟S23,建立臨時坐標系。
在本發(fā)明實施例中,步驟S23可以由坐標計算裝置200的建立模塊220執(zhí)行。建立臨時坐標系的方法與實施例二中所述的方法相同,在此不再贅述。
步驟S24,計算無人機100于臨時坐標系的實時坐標。
在本發(fā)明實施例中,步驟S24可以由坐標計算裝置200執(zhí)行。坐標計算裝置200計算無人機100于臨時坐標系的實時坐標的方法如實施例二所述,在此不再贅述。
步驟S25,將預(yù)設(shè)飛行軌跡映射到臨時坐標系。
在本發(fā)明實施例中,步驟S25可以由映射模塊310執(zhí)行。
具體的,對于復(fù)雜的飛行軌跡,可以預(yù)先將飛行軌跡存儲于無人機100的存儲器101中,該預(yù)先存儲的飛行軌跡是基于原始坐標系,該原始坐標系是以軌跡開始點為原點,以正北方向為Y軸正方向,以正東為X軸正方向建立的符合右手定則的坐標系。
首先,計算將原始坐標系旋轉(zhuǎn)至臨時坐標系的旋轉(zhuǎn)矩陣,旋轉(zhuǎn)矩陣可以表示為:
其中Aa->b'表示原始坐標系a方向上單位矢量在臨時坐標系b'方向上的投影大小。
然后,將原始坐標系下的軌跡坐標乘以旋轉(zhuǎn)矩陣,得到臨時坐標系下的軌跡坐標,計算公式如下:
最后,將計算得到的臨時坐標系下的軌跡坐標加上設(shè)定的臨時坐標系下的軌跡起始點坐標,得到臨時坐標系下的軌跡,實現(xiàn)了將預(yù)設(shè)飛行軌跡映射到臨時坐標系。
對于簡單的飛行軌跡,例如環(huán)繞飛行、直線飛行等,可以無需預(yù)先將飛行軌跡存儲于無人機100的存儲器101中,而可以臨時計算。
當無人機執(zhí)行環(huán)繞飛行時,由于在臨時坐標系下,無人機均為圍繞臨時坐標系的原點飛行,所以在無人機得到了臨時坐標系下的軌跡起始點(Ix',Iy',Iz')后,即可得到半徑:
然后,計算飛行軌跡為:
Ix(t)=R·cos(θ0+ω·t)
Iy(t)=R·sin(θ0+ω·t)
其中θ0為軌跡起始點相對于臨時坐標系的原點的方向與y軸的夾角。
步驟S26,依據(jù)所述實時坐標和映射到所述臨時坐標系的預(yù)設(shè)飛行軌跡,控制所述無人機在所述預(yù)設(shè)飛行軌跡上飛行。
在本發(fā)明實施例中,步驟S26可以由執(zhí)行模塊320執(zhí)行。
本發(fā)明提供一種坐標計算方法及裝置、飛行控制方法及系統(tǒng)、無人機,通過建立基于無人機的臨時坐標系,獲取所述無人機于所述臨時坐標系三個軸向的前一時刻速度、前一時刻坐標以及加速度值;基于所述前一時刻速度和加速度值,計算所述無人機于所述臨時坐標系三個軸向的實時速度;基于所述無人機于所述臨時坐標系三個軸向的實時速度和所述前一時刻坐標,計算所述無人機于所述臨時坐標系的實時坐標。本發(fā)明可以使無人機的飛行不限制在特定的場景,即使在GPS信號較弱的地方也能完成航線飛行,提高了用戶體驗。
在本申請所提供的幾個實施例中,應(yīng)該理解到,所揭露的裝置和方法,也可以通過其它的方式實現(xiàn)。以上所描述的裝置實施例僅僅是示意性的,例如,附圖中的流程圖和框圖顯示了根據(jù)本發(fā)明的多個實施例的裝置、方法和計算機程序產(chǎn)品的可能實現(xiàn)的體系架構(gòu)、功能和操作。在這點上,流程圖或框圖中的每個方框可以代表一個模塊、程序段或代碼的一部分,所述模塊、程序段或代碼的一部分包含一個或多個用于實現(xiàn)規(guī)定的邏輯功能的可執(zhí)行指令。也應(yīng)當注意,在有些作為替換的實現(xiàn)方式中,方框中所標注的功能也可以以不同于附圖中所標注的順序發(fā)生。例如,兩個連續(xù)的方框?qū)嶋H上可以基本并行地執(zhí)行,它們有時也可以按相反的順序執(zhí)行,這依所涉及的功能而定。也要注意的是,框圖和/或流程圖中的每個方框、以及框圖和/或流程圖中的方框的組合,可以用執(zhí)行規(guī)定的功能或動作的專用的基于硬件的系統(tǒng)來實現(xiàn),或者可以用專用硬件與計算機指令的組合來實現(xiàn)。
另外,在本發(fā)明各個實施例中的各功能模塊可以集成在一起形成一個獨立的部分,也可以是各個模塊單獨存在,也可以兩個或兩個以上模塊集成形成一個獨立的部分。
所述功能如果以軟件功能模塊的形式實現(xiàn)并作為獨立的產(chǎn)品銷售或使用時,可以存儲在一個計算機可讀取存儲介質(zhì)中?;谶@樣的理解,本發(fā)明的技術(shù)方案本質(zhì)上或者說對現(xiàn)有技術(shù)做出貢獻的部分或者該技術(shù)方案的部分可以以軟件產(chǎn)品的形式體現(xiàn)出來,該計算機軟件產(chǎn)品存儲在一個存儲介質(zhì)中,包括若干指令用以使得一臺計算機設(shè)備(可以是個人計算機,服務(wù)器,或者網(wǎng)絡(luò)設(shè)備等)執(zhí)行本發(fā)明各個實施例所述方法的全部或部分步驟。而前述的存儲介質(zhì)包括:U盤、移動硬盤、只讀存儲器(ROM,Read-Only Memory)、隨機存取存儲器(RAM,Random Access Memory)、磁碟或者光盤等各種可以存儲程序代碼的介質(zhì)。
需要說明的是,在本文中,諸如第一和第二等之類的關(guān)系術(shù)語僅僅用來將一個實體或者操作與另一個實體或操作區(qū)分開來,而不一定要求或者暗示這些實體或操作之間存在任何這種實際的關(guān)系或者順序。而且,術(shù)語“包括”、“包含”或者其任何其他變體意在涵蓋非排他性的包含,從而使得包括一系列要素的過程、方法、物品或者設(shè)備不僅包括那些要素,而且還包括沒有明確列出的其他要素,或者是還包括為這種過程、方法、物品或者設(shè)備所固有的要素。在沒有更多限制的情況下,由語句“包括一個……”限定的要素,并不排除在包括所述要素的過程、方法、物品或者設(shè)備中還存在另外的相同要素。
以上所述僅為本發(fā)明的優(yōu)選實施例而已,并不用于限制本發(fā)明,對于本領(lǐng)域的技術(shù)人員來說,本發(fā)明可以有各種更改和變化。凡在本發(fā)明的精神和原則之內(nèi),所作的任何修改、等同替換、改進等,均應(yīng)包含在本發(fā)明的保護范圍之內(nèi)。應(yīng)注意到:相似的標號和字母在下面的附圖中表示類似項,因此,一旦某一項在一個附圖中被定義,則在隨后的附圖中不需要對其進行進一步定義和解釋。
以上所述,僅為本發(fā)明的具體實施方式,但本發(fā)明的保護范圍并不局限于此,任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi),可輕易想到變化或替換,都應(yīng)涵蓋在本發(fā)明的保護范圍之內(nèi)。因此,本發(fā)明的保護范圍應(yīng)所述以權(quán)利要求的保護范圍為準。