無(wú)人機(jī)控制方法
【專利摘要】本發(fā)明實(shí)施例公開(kāi)一種無(wú)人機(jī)控制方法，視覺(jué)傳感器采集環(huán)境圖像信息；圖像采集模塊獲取視覺(jué)傳感器采集的環(huán)境圖像信息，并將環(huán)境圖像信息傳輸至處理模塊；處理模塊根據(jù)環(huán)境圖像信息，得到無(wú)人機(jī)的高度、位置和姿態(tài)估計(jì)，并將視覺(jué)位姿估計(jì)通過(guò)特定消息形式發(fā)布到主題，同時(shí)發(fā)布地圖主題輸出地圖信息；飛行控制板將無(wú)人機(jī)當(dāng)前位置、姿態(tài)和速度等信息通過(guò)MAVLink協(xié)議發(fā)送給通信接口模塊；控制模塊分析訂閱得到的無(wú)人機(jī)當(dāng)前各個(gè)狀態(tài)，并將控制指令發(fā)布到主題，供通信接口節(jié)點(diǎn)訂閱。通信接口模塊整合從圖像采集模塊訂閱得到的視覺(jué)估計(jì)位姿信息以及從控制模塊訂閱得到的控制命令，并通過(guò)MAVLink協(xié)議發(fā)送給飛行控制板，飛行控制板對(duì)無(wú)人機(jī)進(jìn)行控制。
【專利說(shuō)明】
無(wú)人機(jī)控制方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及無(wú)人飛行器領(lǐng)域，尤其涉及一種無(wú)人機(jī)控制方法。
【背景技術(shù)】
[0002] 多旋翼無(wú)人機(jī)(Multi-rotor aircraft)通過(guò)無(wú)線遙控設(shè)備進(jìn)行控制，能夠在無(wú)人 駕駛的狀態(tài)下執(zhí)行一些特殊的任務(wù)。其垂直起降、靈活機(jī)動(dòng)和可靠性高等特點(diǎn)，能夠攜帶更 多的探測(cè)和處理設(shè)備。
[0003] 隨著微電子技術(shù)的發(fā)展，計(jì)算機(jī)硬件和微電子傳感器的小型化和多樣化以及控制 算法的不斷優(yōu)化，增強(qiáng)了多旋翼無(wú)人機(jī)信息處理能力和速度，從而其執(zhí)行任務(wù)的類型也越 來(lái)越多樣。
[0004] 目前的無(wú)人機(jī)廣泛采用GPS和慣性導(dǎo)航系統(tǒng)進(jìn)行定位和導(dǎo)航，然而在建筑物和峽 谷等GPS信號(hào)缺失的環(huán)境中，需要對(duì)無(wú)人機(jī)進(jìn)行精準(zhǔn)控制，如何提高無(wú)人機(jī)控制精度是當(dāng)前 需要解決的問(wèn)題。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明實(shí)施例提供一種無(wú)人機(jī)控制方法，能夠提高無(wú)人機(jī)控制精度。
[0006] 本發(fā)明實(shí)施例采用如下技術(shù)方案：
[0007] -種無(wú)人機(jī)控制方法，所述方法應(yīng)用于無(wú)人機(jī)控制系統(tǒng)，所述系統(tǒng)包括:視覺(jué)傳感 器、飛行控制板、主控計(jì)算機(jī)、無(wú)人機(jī)，所述主控計(jì)算機(jī)包括依次連接的圖像采集模塊、處理 模塊、通信接口模塊、控制模塊，所述方法包括：
[0008] 所述視覺(jué)傳感器采集環(huán)境圖像信息；
[0009]所述圖像采集模塊獲取所述視覺(jué)傳感器采集的環(huán)境圖像信息，并將所述環(huán)境圖像 信息傳輸至所述處理模塊；
[0010]所述處理模塊根據(jù)所述環(huán)境圖像信息，得到無(wú)人機(jī)的高度、位置和姿態(tài)估計(jì)，并將 視覺(jué)位姿估計(jì)通過(guò)特定消息形式發(fā)布到主題，同時(shí)發(fā)布地圖主題輸出地圖信息；
[0011]所述飛行控制板將無(wú)人機(jī)當(dāng)前位置、姿態(tài)和速度等信息通過(guò)MAVLink協(xié)議發(fā)送給 所述通信接口模塊；
[0012]所述控制模塊分析訂閱得到的無(wú)人機(jī)當(dāng)前各個(gè)狀態(tài)，并將控制指令發(fā)布到主題， 供通信接口節(jié)點(diǎn)訂閱。
[0013]所述通信接口模塊整合從所述圖像采集模塊訂閱得到的視覺(jué)估計(jì)位姿信息以及 從所述控制模塊訂閱得到的控制命令，并通過(guò)MAVLink協(xié)議發(fā)送給所述飛行控制板，所述飛 行控制板對(duì)所述無(wú)人機(jī)進(jìn)行控制，使得所述無(wú)人機(jī)按照控制命令結(jié)合視覺(jué)位姿估計(jì)結(jié)果完 成相應(yīng)的自主飛行。
[0014] 可選的，還包括:所述通信接口模塊從所述處理模塊訂閱定位結(jié)果，根據(jù)所述定位 結(jié)果進(jìn)行定位。
[0015] 可選的，還包括：
[0016] 所述通信接口模塊訂閱自主控制命令主題，根據(jù)所述自主控制命令主題實(shí)現(xiàn)對(duì)無(wú) 人機(jī)進(jìn)行控制。
[0017] 可選的，還包括:所述通信接口模塊通過(guò)所述控制板獲取無(wú)人機(jī)當(dāng)前局部位置。
[0018] 可選的，還包括：
[0019]所述通信接口模塊設(shè)置有第一接口、第二接口，所述通信接口模塊通過(guò)所述第一 接口獲取飛控心跳包、位置數(shù)據(jù)包、姿態(tài)角數(shù)據(jù)包，所述通信接口模塊通過(guò)所述第二接口獲 取視覺(jué)位姿估計(jì)結(jié)果和控制命令。
[0020] 可選的，所述控制模塊獲取所述無(wú)人機(jī)當(dāng)前位置，生成位置控制指令，將所述位置 控制命令發(fā)布到所述通信接口模塊，并通過(guò)監(jiān)測(cè)所述無(wú)人機(jī)的狀態(tài)調(diào)整所述位置控制指 令。
[0021] 可選的，所述處理模塊對(duì)所述無(wú)人機(jī)當(dāng)前的位置和姿態(tài)進(jìn)行估計(jì)，所述處理模塊 實(shí)時(shí)向所述飛行控制板提供當(dāng)前的位置和姿態(tài)；
[0022] 所述飛行控制板通過(guò)訂閱主題的方式獲取當(dāng)前的位置估計(jì)結(jié)果(x，y，z)和四元 素(x，y，z，w)形式的姿態(tài)估計(jì)結(jié)果。
[0023] 可選的，所述處理模塊對(duì)每個(gè)新的關(guān)鍵幀I完成局部地圖構(gòu)建，步驟包括：
[0024] S1、插入關(guān)鍵幀；
[0025] S2、淘汰最近地圖點(diǎn)；
[0026] S3、創(chuàng)建新的地圖點(diǎn)；
[0027] S4、局部光束平差；
[0028] S5、局部關(guān)鍵幀淘汰。
[0029]可選的，所述處理模塊針對(duì)局部地圖構(gòu)建線程中最后處理的關(guān)鍵幀Ki進(jìn)行環(huán)路閉 合檢測(cè)線程，所述處理模塊通過(guò)關(guān)鍵幀1^的詞袋向量計(jì)算其與系統(tǒng)中關(guān)鍵幀的相似性，完 成候選的閉環(huán)檢測(cè)，如果有多個(gè)位置的關(guān)鍵幀與1的場(chǎng)景外觀相似，則有多個(gè)閉環(huán)候選；
[0030] 所述處理模塊計(jì)算當(dāng)前幀1與閉環(huán)幀心之間的相似變換，對(duì)回路中的累計(jì)誤差進(jìn) 行修正，同時(shí)相似性變換將作為閉環(huán)的幾何驗(yàn)證；
[0031] 所述處理模塊確定閉環(huán)后，通過(guò)計(jì)算得到的相似性變換對(duì)當(dāng)前關(guān)鍵幀的位姿進(jìn)行 修正，同時(shí)這個(gè)修正將影響到與當(dāng)前關(guān)鍵幀相鄰的關(guān)鍵幀節(jié)點(diǎn)，進(jìn)行位姿圖優(yōu)化達(dá)到全局 一致性，從而每個(gè)地圖點(diǎn)根據(jù)修正的關(guān)鍵幀進(jìn)行變換。
[0032] 可選的，所述無(wú)人機(jī)包括位置控制器、姿態(tài)控制器、執(zhí)行電機(jī)，所述位置控制器接 收飛行控制信息，進(jìn)行姿態(tài)的解算后，通過(guò)所述姿態(tài)控制器控制所述執(zhí)行電機(jī)，從而對(duì)所述 無(wú)人機(jī)進(jìn)行飛行控制。
[0033] 基于上述技術(shù)方案的無(wú)人機(jī)控制方法，利用視覺(jué)傳感器獲取環(huán)境圖像數(shù)據(jù)后，傳 輸?shù)酱钶d在無(wú)人機(jī)上的主控計(jì)算機(jī)，主控計(jì)算機(jī)對(duì)接收到的圖像進(jìn)行處理，完成位置和姿 態(tài)估計(jì)以及地圖構(gòu)建，之后將相對(duì)位置作為飛行參數(shù)直接送到飛行控制板，飛行控制板解 算當(dāng)前飛行器的位置、高度和姿態(tài)數(shù)據(jù)后，對(duì)所述無(wú)人機(jī)進(jìn)行控制，使得所述無(wú)人機(jī)按照控 制命令結(jié)合視覺(jué)位姿估計(jì)結(jié)果完成相應(yīng)的自主飛行。
[0034] 應(yīng)當(dāng)理解的是，以上的一般描述和后文的細(xì)節(jié)描述僅是示例性和解釋性的，并不 能限制本公開(kāi)。
【附圖說(shuō)明】
[0035] 此處的附圖被并入說(shuō)明書(shū)中并構(gòu)成本說(shuō)明書(shū)的一部分，示出了符合本發(fā)明的實(shí)施 例，并與說(shuō)明書(shū)一起用于解釋本發(fā)明的原理。
[0036] 圖1為本發(fā)明實(shí)施例提供的一種無(wú)人機(jī)控制系統(tǒng)的結(jié)構(gòu)示意圖之一；
[0037] 圖2為本發(fā)明實(shí)施例提供的一種無(wú)人機(jī)控制系統(tǒng)的結(jié)構(gòu)示意圖之二；
[0038]圖3為本發(fā)明實(shí)施例提供的一種無(wú)人機(jī)控制系統(tǒng)的結(jié)構(gòu)示意圖之三；
[0039] 圖4為本發(fā)明實(shí)施例提供的一種無(wú)人機(jī)控制系統(tǒng)的結(jié)構(gòu)示意圖之四。
【具體實(shí)施方式】
[0040] 這里將詳細(xì)地對(duì)示例性實(shí)施例進(jìn)行說(shuō)明，其示例表示在附圖中。下面的描述涉及 附圖時(shí)，除非另有表示，不同附圖中的相同數(shù)字表示相同或相似的要素。以下示例性實(shí)施例 中所描述的實(shí)施方式并不代表與本發(fā)明相一致的所有實(shí)施方式。相反，它們僅是與如所附 權(quán)利要求書(shū)中所詳述的、本發(fā)明的一些方面相一致的裝置和方法的例子。
[0041] 圖1為本發(fā)明實(shí)施例提供的一種無(wú)人機(jī)控制系統(tǒng)，該系統(tǒng)包括:視覺(jué)傳感器11、飛 行控制板12、主控計(jì)算機(jī)13，無(wú)人機(jī)14。
[0042] 本發(fā)明施例中，飛行控制板12是執(zhí)行自主命令和控制飛行的樞紐，主控計(jì)算機(jī)13 通過(guò)飛行控制板12獲取飛行數(shù)據(jù)和狀態(tài)，也是通過(guò)飛行控制板12來(lái)完成自主控制飛行。 [0043] 本發(fā)明一個(gè)實(shí)施例中，飛行控制板12可以采用如下配置：STM32F427Cortex M4168MHz微處理器，RAM為256KB，f lash存儲(chǔ)為2MB。除了主要的CPU外，還有一個(gè)STM32F103 處理器，避免主處理器失效時(shí)無(wú)人機(jī)失控。為拓展自主能力，飛行控制板12可以提供多個(gè)接 口來(lái)連接外部傳感器。
[0044] 本發(fā)明一個(gè)實(shí)施例中，飛行控制板12可以具有多種飛行模式滿足不同的任務(wù)需 求，包括:手動(dòng)模式、高度和位置穩(wěn)定模式、任務(wù)模式、懸停模式和離板模式。其中，離板模 式可以獲取外部的（如通過(guò)串口連接的主控計(jì)算機(jī))設(shè)置點(diǎn)和控制命令，包括位置、姿態(tài)、速 度和加速度等，飛行控制板的控制器將根據(jù)給定的目標(biāo)值進(jìn)行解算和飛行。
[0045] 本發(fā)明實(shí)施例中，視覺(jué)傳感器11可以完成對(duì)環(huán)境圖像的采集，根據(jù)視覺(jué)定位和地 圖構(gòu)建算法的需要，可以采用能提供640*480像素分辨率的圖像，并以每秒最高60幀的采集 頻率完成圖像的獲取(或者320*240分辨率下120幀每秒），有一個(gè)75度的廣角攝像模式的視 覺(jué)傳感器。
[0046] 主控計(jì)算機(jī)13主要處理視覺(jué)傳感器11的數(shù)據(jù)并對(duì)整個(gè)平臺(tái)進(jìn)行自主控制。主控計(jì) 算機(jī)11的配置硬件參數(shù)可以如表1所示，主控計(jì)算機(jī)13通過(guò)USB hub擴(kuò)展連接TP-link-WN823N USB Wi-Fi模塊和飛行控制板TELEM2端口，建立在板主控計(jì)算機(jī)13與飛行控制板12 以及地面站計(jì)算機(jī)的可靠通信。另一個(gè)USB 口則直接連接PS3Eye單目視覺(jué)傳感器，在板主控 計(jì)算機(jī)搭載Ubuntu 12.04LTS操作系統(tǒng)、Hydro版的機(jī)器人操作系統(tǒng)(R0S)和OpenCV圖像處 理庫(kù)。
[0047] 表1主控計(jì)算機(jī)13配置硬件參數(shù)
[0048]
[0049]
[0050] 本發(fā)明實(shí)施例提供一種無(wú)人機(jī)控制方法，所述方法應(yīng)用于圖1所示的無(wú)人機(jī)控制 系統(tǒng)，該系統(tǒng)包括:視覺(jué)傳感器11、飛行控制板12、主控計(jì)算機(jī)13、無(wú)人機(jī)14,如圖2所示，所 述主控計(jì)算機(jī)13包括依次連接的圖像采集模塊131、處理模塊132、通信接口模塊133、控制 模塊134，所述方法包括：
[0051 ]所述視覺(jué)傳感器11采集環(huán)境圖像信息；
[0052]所述圖像采集模塊131獲取所述視覺(jué)傳感器11采集的環(huán)境圖像信息，并將所述環(huán) 境圖像信息傳輸至所述處理模塊132;
[0053]所述處理模塊132根據(jù)所述環(huán)境圖像信息，得到無(wú)人機(jī)的高度、位置和姿態(tài)估計(jì)， 并將視覺(jué)位姿估計(jì)通過(guò)特定消息形式發(fā)布到主題，同時(shí)發(fā)布地圖主題輸出地圖信息；
[0054]所述飛行控制板12將無(wú)人機(jī)當(dāng)前位置、姿態(tài)和速度等信息通過(guò)MAVLink協(xié)議發(fā)送 給所述通信接口模塊133;
[0055] 所述控制模塊134分析訂閱得到的無(wú)人機(jī)當(dāng)前各個(gè)狀態(tài)，并將控制指令發(fā)布到主 題，供通信接口節(jié)點(diǎn)訂閱。
[0056] 所述通信接口模塊133整合從所述圖像采集模塊訂閱得到的視覺(jué)估計(jì)位姿信息以 及從所述控制模塊134訂閱得到的控制命令，并通過(guò)MAVLink協(xié)議發(fā)送給所述飛行控制板 12,所述飛行控制板12對(duì)所述無(wú)人機(jī)進(jìn)行控制，使得所述無(wú)人機(jī)14按照控制命令結(jié)合視覺(jué) 位姿估計(jì)結(jié)果完成相應(yīng)的自主飛行。
[0057] 本發(fā)明實(shí)施例的無(wú)人機(jī)控制方法，利用視覺(jué)傳感器獲取環(huán)境圖像數(shù)據(jù)后，傳輸?shù)?搭載在無(wú)人機(jī)上的主控計(jì)算機(jī)，主控計(jì)算機(jī)對(duì)接收到的圖像進(jìn)行處理，完成位置和姿態(tài)估 計(jì)以及地圖構(gòu)建，之后將相對(duì)位置作為飛行參數(shù)直接送到飛行控制板，飛行控制板解算當(dāng) 前飛行器的位置、高度和姿態(tài)數(shù)據(jù)后，對(duì)所述無(wú)人機(jī)進(jìn)行控制，使得所述無(wú)人機(jī)按照控制命 令結(jié)合視覺(jué)位姿估計(jì)結(jié)果完成相應(yīng)的自主飛行。
[0058]本發(fā)明實(shí)施例中，視覺(jué)傳感器11可以為單目視覺(jué)傳感器，視覺(jué)傳感器11采集環(huán)境 圖像信息傳輸給主控計(jì)算機(jī)13的圖像采集模塊131，完成圖像預(yù)處理。
[0059]通過(guò)處理模塊132根據(jù)視覺(jué)定位與地圖構(gòu)建算法對(duì)圖像幀進(jìn)行處理，完成無(wú)人機(jī) 14的高度、位置和姿態(tài)估計(jì)，并將視覺(jué)位姿估計(jì)通過(guò)特定消息形式發(fā)布到主題，同時(shí)發(fā)布地 圖主題輸出地圖信息。
[0060] 飛行控制板12將無(wú)人機(jī)當(dāng)前位置、姿態(tài)和速度等信息通過(guò)MAVLink協(xié)議發(fā)送給通 信接口模塊133,主控計(jì)算機(jī)13獲取無(wú)人機(jī)14的當(dāng)前飛行狀態(tài)。
[0061] 控制模塊134分析訂閱得到的無(wú)人機(jī)當(dāng)前各個(gè)狀態(tài)，并將控制指令發(fā)布到主題，供 通信接口節(jié)點(diǎn)訂閱。
[0062]通信接口模塊133整合從圖像采集模塊131訂閱得到的視覺(jué)估計(jì)位姿信息以及從 控制模塊134訂閱得到的控制命令，并通過(guò)MAVLink協(xié)議發(fā)送給飛行控制板12,無(wú)人機(jī)14將 按照控制命令結(jié)合視覺(jué)位姿估計(jì)結(jié)果完成相應(yīng)的自主飛行。
[0063] 本發(fā)明實(shí)施例中，可選的，所述通信接口模塊133從所述處理模塊訂閱定位結(jié)果， 根據(jù)所述定位結(jié)果進(jìn)行定位。
[0064] 可選的，所述通信接口模塊133訂閱自主控制命令主題，根據(jù)所述自主控制命令主 題實(shí)現(xiàn)對(duì)無(wú)人機(jī)進(jìn)行控制。
[0065] 可選的，還包括:所述通信接口模塊133通過(guò)所述控制板獲取無(wú)人機(jī)當(dāng)前局部位 置。
[0066] 可選的，所述通信接口模塊133設(shè)置有第一接口、第二接口，所述通信接口模塊通 過(guò)所述第一接口（/mavlink/to)獲取飛控心跳包、位置數(shù)據(jù)包、姿態(tài)角數(shù)據(jù)包，所述通信接 口模塊通過(guò)所述第二接口（/mavlink/from)獲取視覺(jué)位姿估計(jì)結(jié)果和控制命令。
[0067]本發(fā)明實(shí)施例中，通信接口模塊133可以通過(guò)Mavlink協(xié)議接收無(wú)人機(jī)14的心跳 包、姿態(tài)角數(shù)據(jù)包和位置數(shù)據(jù)包等，并按類別以特定消息格式將數(shù)據(jù)發(fā)布到不同的主題上。 同時(shí)訂閱了 vis ion和posit ion等主題，再以Mavlink協(xié)議的形式將這些主題數(shù)據(jù)發(fā)送到飛 行控制板12，獲取外部傳感器的數(shù)據(jù)并實(shí)現(xiàn)自主飛行控制。
[0068] 本發(fā)明實(shí)施例中，通信接口模塊133可以訂閱的不同主題，也可以發(fā)布不同的主 題，通信接口模塊133可以與飛行控制板12之間信息傳輸?shù)耐ㄐ艆f(xié)議，通信接口模塊133設(shè) 置有/mavl ink/to接口、/mavlink/from接口，/mavlink/to是通信接口模塊133獲取飛控心 跳包、位置數(shù)據(jù)包、姿態(tài)角數(shù)據(jù)包等信息的通道，/mavlink/from是飛行控制板12從通信接 口模塊133獲取視覺(jué)位姿估計(jì)結(jié)果和控制命令等信息的通道。
[0069] 本發(fā)明實(shí)施例中，通信接口模塊133可以訂閱/mavros/position/vision、/ mavros/setpoint/local_position、/mavros/rc/override、/mavros/safety_area/set、/ mavros/mocap/pose等主題。在訂閱的主題中，/mavros/position/vision為視覺(jué)定位與地 圖構(gòu)建方法中的定位結(jié)果，將跟蹤線程中的位置估計(jì)結(jié)果主題數(shù)據(jù)發(fā)布到該主題，即飛行 控制板12獲取視覺(jué)傳感器的當(dāng)前位置，實(shí)現(xiàn)定位。/mavros/setpoint/local_position為自 主控制命令主題，將位置控制命令發(fā)布到該主題實(shí)現(xiàn)對(duì)無(wú)人機(jī)的自主控制。
[0070] 本發(fā)明實(shí)施例中，通信接口模塊133可以發(fā)布/11^￥1'08/^08；[1：；[011/10031、/1]^￥1'08/ imu/data、/mavros/optical_flow、/mavros/state、/mavros/rc、/mavros/battery、/ ma vros/camera_image、/mavros/miss ion/waypoint s、/mavros/imu/aum_pressure 等主題。 在發(fā)布的主題中，/11^￥1'08/^08；[1：；[011/10031是飛行控制板12在獲取/11^￥1'08/^08；[1：；[011/ vision主題視覺(jué)定位結(jié)果后識(shí)別的無(wú)人機(jī)當(dāng)前局部位置，/mavros/imu/data為慣性測(cè)量單 元數(shù)據(jù)。其它還包括氣壓計(jì)、遙控?cái)?shù)據(jù)和飛行任務(wù)等主題信息。
[0071] 可選的，所述控制模塊133獲取所述無(wú)人機(jī)當(dāng)前位置，生成位置控制指令，將所述 位置控制命令發(fā)布到所述通信接口模塊，并通過(guò)監(jiān)測(cè)所述無(wú)人機(jī)的狀態(tài)調(diào)整所述位置控 制指令。
[0072] 本發(fā)明實(shí)施例中，控制模塊134訂閱飛行器的當(dāng)前位置主題，提供位置反饋，再將 位置控制命令發(fā)布到通信接口模塊的/mavros/setpoint/local_position主題上。通過(guò)對(duì) 無(wú)人機(jī)狀態(tài)的實(shí)時(shí)監(jiān)測(cè)調(diào)整位置控制信息。
[0073]可選的，所述處理模塊133對(duì)所述無(wú)人機(jī)當(dāng)前的位置和姿態(tài)進(jìn)行估計(jì)，所述處理模 塊實(shí)時(shí)向所述飛行控制板提供當(dāng)前的位置和姿態(tài)；
[0074]所述飛行控制板12通過(guò)訂閱主題的方式獲取當(dāng)前的位置估計(jì)結(jié)果(x，y，z)和四元 素(x，y，z，w)形式的姿態(tài)估計(jì)結(jié)果。
[0075]本發(fā)明實(shí)施例中，跟蹤線程是對(duì)相機(jī)獲取的每一幀圖像進(jìn)行處理，完成對(duì)每一幀 圖像的位姿估計(jì)，跟蹤線程框架如圖3所示。
[0076]本發(fā)明實(shí)施例中，跟蹤線程主要對(duì)當(dāng)前飛行器的位置和姿態(tài)進(jìn)行估計(jì)，在主控計(jì) 算機(jī)處理跟蹤線程的同時(shí)，需要實(shí)時(shí)地使飛行控制板獲取當(dāng)前的位置和姿態(tài)。所以需要在 跟蹤線程中完成位姿估計(jì)結(jié)果的主題發(fā)布。飛行器的飛行控制板通過(guò)訂閱主題的方式獲取 當(dāng)前傳感器的位置估計(jì)結(jié)果(x，y，z)和四元素(x，y，z，w)形式的姿態(tài)估計(jì)結(jié)果，所以跟蹤線 程在R0S中通過(guò)PoseStamped消息類型發(fā)布一個(gè)名為pose的主題。
[0077]可選的，所述處理模塊132對(duì)每個(gè)新的關(guān)鍵幀Ki完成局部地圖構(gòu)建，步驟包括：
[0078] S1、插入關(guān)鍵幀；
[0079] S2、淘汰最近地圖點(diǎn)；
[0080] S3、創(chuàng)建新的地圖點(diǎn)；
[00811 S4、局部光束平差；
[0082] S5、局部關(guān)鍵幀淘汰。
[0083]可選的，所述處理模塊132針對(duì)局部地圖構(gòu)建線程中最后處理的關(guān)鍵幀Ki進(jìn)行環(huán) 路閉合檢測(cè)線程，所述處理模塊通過(guò)關(guān)鍵幀I的詞袋向量計(jì)算其與系統(tǒng)中關(guān)鍵幀的相似 性，完成候選的閉環(huán)檢測(cè)，如果有多個(gè)位置的關(guān)鍵幀與心的場(chǎng)景外觀相似，則有多個(gè)閉環(huán)候 選；
[0084] 所述處理模塊132計(jì)算當(dāng)前幀1與閉環(huán)幀心之間的相似變換，對(duì)回路中的累計(jì)誤差 進(jìn)行修正，同時(shí)相似性變換將作為閉環(huán)的幾何驗(yàn)證；
[0085] 所述處理模塊132確定閉環(huán)后，通過(guò)計(jì)算得到的相似性變換對(duì)當(dāng)前關(guān)鍵幀的位姿 進(jìn)行修正，同時(shí)這個(gè)修正將影響到與當(dāng)前關(guān)鍵幀相鄰的關(guān)鍵幀節(jié)點(diǎn)，進(jìn)行位姿圖優(yōu)化達(dá)到 全局一致性，從而每個(gè)地圖點(diǎn)根據(jù)修正的關(guān)鍵幀進(jìn)行變換。
[0086] 本發(fā)明實(shí)施例中，環(huán)路閉合檢測(cè)線程主要是針對(duì)局部地圖構(gòu)建線程中最后處理的 關(guān)鍵幀I來(lái)進(jìn)行。通過(guò)關(guān)鍵幀1的詞袋向量計(jì)算其與系統(tǒng)中關(guān)鍵幀的相似性，完成候選的閉 環(huán)檢測(cè)。倘若有多個(gè)位置的關(guān)鍵幀與1的場(chǎng)景外觀相似，則有多個(gè)閉環(huán)候選。
[0087] 在無(wú)人機(jī)14的視覺(jué)定位與地圖構(gòu)建中，地圖的偏移有7個(gè)自由度，3個(gè)旋轉(zhuǎn)、3個(gè)平 移和1個(gè)尺度。所以判定是否為一個(gè)閉合回路，需要計(jì)算當(dāng)前幀I與閉環(huán)幀心之間的相似變 換，對(duì)回路中的累計(jì)誤差進(jìn)行修正。同時(shí)相似性變換將作為閉環(huán)的幾何驗(yàn)證。
[0088] 在確定了閉環(huán)后，則需要完成閉環(huán)修正，通過(guò)計(jì)算得到的相似性變換對(duì)當(dāng)前關(guān)鍵 幀的位姿進(jìn)行修正，同時(shí)這個(gè)修正將影響到與當(dāng)前關(guān)鍵幀相鄰的關(guān)鍵幀節(jié)點(diǎn)。最后進(jìn)行位 姿圖優(yōu)化達(dá)到全局一致性，優(yōu)化后，每個(gè)地圖點(diǎn)根據(jù)修正的關(guān)鍵幀進(jìn)行變換。
[0089] 本發(fā)明實(shí)施例中，如圖4所示，該系統(tǒng)還可以包括地面站計(jì)算機(jī)15。本發(fā)明實(shí)施例 中，為了搭建地面站系統(tǒng)完成程序的啟動(dòng)和初始化，并實(shí)現(xiàn)對(duì)無(wú)人機(jī)飛行狀態(tài)的實(shí)時(shí)監(jiān)控， 采用一臺(tái)無(wú)線路由器組網(wǎng)，自動(dòng)分配IP地址到主控計(jì)算機(jī)和地面站計(jì)算機(jī)，使得兩者完成 關(guān)聯(lián)，完成主控計(jì)算機(jī)畫面的無(wú)線傳輸。
[0090] 地面站計(jì)算機(jī)15基于虛擬網(wǎng)絡(luò)計(jì)算機(jī)(VNC)遠(yuǎn)程控制工具完成，在板主控計(jì)算機(jī) 為服務(wù)端(vncserver)，搭載Ubuntu操作系統(tǒng)，地面站計(jì)算機(jī)為客戶端(vncviewer)，搭載 WIN7操作系統(tǒng)。
[0091] 地面站計(jì)算機(jī)15的VNC的客戶端訪問(wèn)工具選用VNCViewer，啟動(dòng)工具后需要對(duì)IP地 址進(jìn)行設(shè)置，Server窗口為主控計(jì)算機(jī)服務(wù)端的IP地址。連接成功后可看到服務(wù)端一一在 板主控計(jì)算機(jī)的畫面，同時(shí)在地面站客戶端能對(duì)服務(wù)端的主控計(jì)算機(jī)實(shí)時(shí)進(jìn)行啟動(dòng)、設(shè)置 等操作。
[0092] 如圖4所示，所述無(wú)人機(jī)14包括位置控制器141、姿態(tài)控制器142、執(zhí)行電機(jī)143,所 述位置控制器141接收飛行控制信息，進(jìn)行姿態(tài)的解算后，通過(guò)所述姿態(tài)控制器控142制所 述執(zhí)行電機(jī)143,從而對(duì)所述無(wú)人機(jī)14進(jìn)行飛行控制。
[0093] 本發(fā)明實(shí)施例的無(wú)人機(jī)控制方法，利用單目視覺(jué)傳感器獲取環(huán)境圖像數(shù)據(jù)后，傳 輸?shù)酱钶d在無(wú)人機(jī)上的在板主控計(jì)算機(jī)，主控計(jì)算機(jī)對(duì)接收到的圖像進(jìn)行處理，完成位置 和姿態(tài)估計(jì)以及地圖構(gòu)建，之后將相對(duì)位置作為飛行參數(shù)直接送到底層的飛行控制板，飛 行控制板解算當(dāng)前飛行器的位置、高度和姿態(tài)數(shù)據(jù)后傳送回主控計(jì)算機(jī)，主控計(jì)算機(jī)將控 制命令參數(shù)發(fā)送到飛行器的位置控制器，形成控制回路。位置控制器則完成姿態(tài)的解算后， 通過(guò)姿態(tài)控制器對(duì)飛行器進(jìn)行自主飛行控制。
[0094]以上已經(jīng)描述了本發(fā)明的各實(shí)施例，上述說(shuō)明是示例性的，并非窮盡性的，并且也 不限于所披露的各實(shí)施例。在不偏離所說(shuō)明的各實(shí)施例的范圍和精神的情況下，對(duì)于本技 術(shù)領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)許多修改和變更都是顯而易見(jiàn)的。本文中所用術(shù)語(yǔ)的選擇，旨 在最好地解釋各實(shí)施例的原理、實(shí)際應(yīng)用或?qū)κ袌?chǎng)中的技術(shù)的改進(jìn)，或者使本技術(shù)領(lǐng)域的 其它普通技術(shù)人員能理解本文披露的各實(shí)施例。
[0095]以上所述，僅為本發(fā)明的【具體實(shí)施方式】，但本發(fā)明的保護(hù)范圍并不局限于此，任何 熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到變化或替換，都應(yīng)涵 蓋在本發(fā)明的保護(hù)范圍之內(nèi)。因此，本發(fā)明的保護(hù)范圍應(yīng)以權(quán)利要求的保護(hù)范圍為準(zhǔn)。
【主權(quán)項(xiàng)】
1. 一種無(wú)人機(jī)控制方法，其特征在于，所述方法應(yīng)用于無(wú)人機(jī)控制系統(tǒng)，所述系統(tǒng)包 括:視覺(jué)傳感器、飛行控制板、主控計(jì)算機(jī)、無(wú)人機(jī)，所述主控計(jì)算機(jī)包括依次連接的圖像采 集模塊、處理模塊、通信接口模塊、控制模塊，所述方法包括： 所述視覺(jué)傳感器采集環(huán)境圖像信息； 所述圖像采集模塊獲取所述視覺(jué)傳感器采集的環(huán)境圖像信息，并將所述環(huán)境圖像信息 傳輸至所述處理模塊； 所述處理模塊根據(jù)所述環(huán)境圖像信息，得到無(wú)人機(jī)的高度、位置和姿態(tài)估計(jì)，并將視覺(jué) 位姿估計(jì)通過(guò)特定消息形式發(fā)布到主題，同時(shí)發(fā)布地圖主題輸出地圖信息； 所述飛行控制板將無(wú)人機(jī)當(dāng)前位置、姿態(tài)和速度等信息通過(guò)MAVLink協(xié)議發(fā)送給所述 通信接口模塊； 所述控制模塊分析訂閱得到的無(wú)人機(jī)當(dāng)前各個(gè)狀態(tài)，并將控制指令發(fā)布到主題，供通 信接口節(jié)點(diǎn)訂閱。 所述通信接口模塊整合從所述圖像采集模塊訂閱得到的視覺(jué)估計(jì)位姿信息以及從所 述控制模塊訂閱得到的控制命令，并通過(guò)MAVLink協(xié)議發(fā)送給所述飛行控制板，所述飛行控 制板對(duì)所述無(wú)人機(jī)進(jìn)行控制，使得所述無(wú)人機(jī)按照控制命令結(jié)合視覺(jué)位姿估計(jì)結(jié)果完成相 應(yīng)的自主飛行。2. 根據(jù)權(quán)利要求1所述的系統(tǒng)，其特征在于，還包括:所述通信接口模塊從所述處理模 塊訂閱定位結(jié)果，根據(jù)所述定位結(jié)果進(jìn)行定位。3. 根據(jù)權(quán)利要求1或2所述的系統(tǒng)，其特征在于，還包括： 所述通信接口模塊訂閱自主控制命令主題，根據(jù)所述自主控制命令主題實(shí)現(xiàn)對(duì)無(wú)人機(jī) 進(jìn)行控制。4. 根據(jù)權(quán)利要求1至3中任一項(xiàng)所述的系統(tǒng)，其特征在于，還包括:所述通信接口模塊通 過(guò)所述控制板獲取無(wú)人機(jī)當(dāng)前局部位置。5. 根據(jù)權(quán)利要求1至4中任一項(xiàng)所述的系統(tǒng)，其特征在于，還包括： 所述通信接口模塊設(shè)置有第一接口、第二接口，所述通信接口模塊通過(guò)所述第一接口 獲取飛控心跳包、位置數(shù)據(jù)包、姿態(tài)角數(shù)據(jù)包，所述通信接口模塊通過(guò)所述第二接口獲取視 覺(jué)位姿估計(jì)結(jié)果和控制命令。6. 根據(jù)權(quán)利要求1至5中任一項(xiàng)所述的系統(tǒng)，其特征在于，所述控制模塊獲取所述無(wú)人 機(jī)當(dāng)前位置，生成位置控制指令，將所述位置控制命令發(fā)布到所述通信接口模塊，并通過(guò)監(jiān) 測(cè)所述無(wú)人機(jī)的狀態(tài)調(diào)整所述位置控制指令。7. 根據(jù)權(quán)利要求1至6中任一項(xiàng)所述的系統(tǒng)，其特征在于， 所述處理模塊對(duì)所述無(wú)人機(jī)當(dāng)前的位置和姿態(tài)進(jìn)行估計(jì)，所述處理模塊實(shí)時(shí)向所述飛 行控制板提供當(dāng)前的位置和姿態(tài)； 所述飛行控制板通過(guò)訂閱主題的方式獲取當(dāng)前的位置估計(jì)結(jié)果（x，y，z)和四元素（X， y，z，w)形式的姿態(tài)估計(jì)結(jié)果。8. 根據(jù)權(quán)利要求1至7中任一項(xiàng)所述的系統(tǒng)，其特征在于，所述處理模塊對(duì)每個(gè)新的關(guān) 鍵幀Ki完成局部地圖構(gòu)建，步驟包括： 51、 插入關(guān)鍵幀； 52、 淘汰最近地圖點(diǎn)； 53、 創(chuàng)建新的地圖點(diǎn)； 54、 局部光束平差； 55、 局部關(guān)鍵幀淘汰。9. 根據(jù)權(quán)利要求8所述的系統(tǒng)，其特征在于，所述處理模塊針對(duì)局部地圖構(gòu)建線程中最 后處理的關(guān)鍵幀1進(jìn)行環(huán)路閉合檢測(cè)線程，所述處理模塊通過(guò)關(guān)鍵幀I的詞袋向量計(jì)算其 與系統(tǒng)中關(guān)鍵幀的相似性，完成候選的閉環(huán)檢測(cè)，如果有多個(gè)位置的關(guān)鍵幀與1的場(chǎng)景外 觀相似，則有多個(gè)閉環(huán)候選； 所述處理模塊計(jì)算當(dāng)前幀1與閉環(huán)幀心之間的相似變換，對(duì)回路中的累計(jì)誤差進(jìn)行修 正，同時(shí)相似性變換將作為閉環(huán)的幾何驗(yàn)證； 所述處理模塊確定閉環(huán)后，通過(guò)計(jì)算得到的相似性變換對(duì)當(dāng)前關(guān)鍵幀的位姿進(jìn)行修 正，同時(shí)這個(gè)修正將影響到與當(dāng)前關(guān)鍵幀相鄰的關(guān)鍵幀節(jié)點(diǎn)，進(jìn)行位姿圖優(yōu)化達(dá)到全局一 致性，從而每個(gè)地圖點(diǎn)根據(jù)修正的關(guān)鍵幀進(jìn)行變換。10. 根據(jù)權(quán)利要求1至9中任一項(xiàng)所述的系統(tǒng)，其特征在于，所述無(wú)人機(jī)包括位置控制 器、姿態(tài)控制器、執(zhí)行電機(jī)，所述位置控制器接收飛行控制信息，進(jìn)行姿態(tài)的解算后，通過(guò)所 述姿態(tài)控制器控制所述執(zhí)行電機(jī)，從而對(duì)所述無(wú)人機(jī)進(jìn)行飛行控制。
【文檔編號(hào)】G05D1/10GK105974932SQ201610269672
【公開(kāi)日】2016年9月28日
【申請(qǐng)日】2016年4月27日
【發(fā)明人】王國(guó)勝, 呂強(qiáng), 郭峰, 張洋, 林輝燦, 馬建業(yè)
【申請(qǐng)人】中國(guó)人民解放軍裝甲兵工程學(xué)院