動模塊4的一端與電機3相連，另一端與控制芯片8的脈沖寬帶調制單元 相連，用于在脈沖寬帶調制單元輸出的PWM脈沖信號的控制下驅動4路電機3工作。
[0071] 控制芯片8的中央處理器CPU發出控制指令，由脈沖寬帶調制單元輸出PWM脈沖 信號，電機驅動模塊4根據PWM脈沖信號的驅動4路電機3工作。
[0072] 多個超聲波傳感器5與控制芯片8的監測端口相連，用于測量超聲波信號并傳送 至控制芯片8。
[0073] 如圖3所示，無人機為四旋翼無人機，多個超聲波傳感器5分別固定于四旋翼無人 機的四個機翼和中心位置處。
[0074] 溫度傳感器6與控制芯片8的監測端口相連，用于測量室內的溫度數據并傳送至 控制芯片8。
[0075] 陀螺儀7與控制芯片8的SPI接口相連，用于測量無人機的角速度并傳送至控制 芯片8。其中，陀螺儀7采用型號為MPU6050的陀螺儀。
[0076] 控制芯片8根據室內的溫度數據對測量的超聲波信號進行溫度補償。
[0077] 在本發明的一個實施例中，超聲波溫度補償的公式為：C = C0+0. 607XT°C。
[0078] 其中，T為測量的溫度數據，C0為測量超聲波信號，C為補償后的超聲波信號。
[0079] 控制芯片8利用溫度補償后的超聲波信號分別計算無人機與室內的四壁、天花板 和地板的距離，并根據無人機與室內的四壁、天花板和地板的距離反向定位出無人機的位 置。
[0080] 超聲波在空氣中的傳播速度為340m/s，根據計時器記錄的時間t，就可以計算出 發射點距障礙物（四壁、天花板和地板）的距離（s)，即：s = 340X At/2。
[0081] 需要說明的是，本發明通過溫度補償后的超聲信號測距，其誤差較小，S = 340X0. 000001/2 = 0· 17mm。
[0082] 然后，控制芯片8根據無人機與室內的四壁、天花板和地板的距離反向定位出無 人機在室內的位置。
[0083] 控制芯片8還用于根據無人機的角速度，計算無人機的姿態角，包括：建立機體坐 標系和導航坐標系，然后根據無人機的角速度計算無人機在機體坐標系和導航坐標系下， 機體轉動的旋轉矩陣，并根據旋轉矩陣計算無人機的飛機姿態矩陣，并根據飛機姿態矩陣 計算無人機的飛行姿態角。
[0084] 具體地，首先坐標系建立如下：機體坐標系定義為：X軸表示橫滾軸，對應前方；Y 軸表示俯仰軸，對應右方；Z軸表示航向，對應下方。導航坐標系定義為北東地坐標系。
[0085] 根據無人機的角速度計算無人機在機體坐標系和導航坐標系下，機體轉動的旋轉 矩陣，并根據旋轉矩陣計算無人機的飛機姿態矩陣。
[0086] 飛行姿態角為機體按照以下順序轉動時的角度：繞Z軸轉動的角度Φ，繞Y軸轉 動的角度Θ，繞X軸轉動的角度Φ。
[0087] 機體轉動按照繞航向軸（Z軸）轉動Φ，俯仰軸（Y軸）轉動Θ，橫滾軸（X軸）轉 動Φ的順序轉動時，旋轉矩陣為（cl，c2, c3)，
[0088] 根據繞z軸轉動Φ角，計算得到
[0089] 根據繞y軸轉動Θ角，計算得到
[0090] 根據繞X軸轉動Φ角，計算得到
[0091] 基于此，根據旋轉矩陣計算無人機的飛機姿態矩陣，包括如下步驟：
[0092] 首先，根據旋轉矩陣將參考體系變化至載體系，
[0093] cnb= c3c2cl ； (4)
[0094] 然后，計算飛機姿態矩陣Cbn為：
[0097] 根據飛機姿態矩陣計算無人機的飛行姿態角為：
[0099] 在本發明的一個實施例中，控8制芯片使用12MHz晶體作時鐘基準的89C51單片 機，89C51單片機定時器精度達到到1 μ s的精度，因此系統誤差在1mm的測量范圍內。
[0100] 進一步，本發明實施例的無人機還包括：燈光閃爍模塊9。該燈光閃爍模塊9與控 制芯片8相連，用于在無人機飛行時發出彩色燈光的閃爍光以提示周邊。
[0101] 此外，本發明實施例的無人機還包括：無線通信模塊10。該無線通信模塊與控制 芯片8相連，用于與外部的終端設備進行通信交互，以由外部的終端設備遠程控制無人機。
[0102] 在本發明的一個實施例中，無線通信模塊可以為WiFi模塊或GPRS通信模塊，實現 與外部的終端設備，例如手機的通信。
[0103] 在本發明的一個實施例中，本發明的無人機可以為微型四旋翼無人機、尺寸僅為 185. 5mmX 185. 5mm?
[0104] 根據本發明實施例的無人機，采用多個方向的超聲波傳感器，可計算無人機相對 于室內四壁、天花板、地板的位置，然后反向定位出無人機在室內的位置。本發明應用較少 的傳感器，實現對室內及無人機相關數據采集，生產成本較低，適合推廣使用。此外，本發明 可以通過測量的無人機的三軸角速度，計算出無人機的飛行姿態角，精確度高且算法簡單。
[0105] 在本說明書的描述中，參考術語"一個實施例"、"一些實施例"、"示例"、"具體示 例"、或"一些示例"等的描述意指結合該實施例或示例描述的具體特征、結構、材料或者特 點包含于本發明的至少一個實施例或示例中。在本說明書中，對上述術語的示意性表述不 一定指的是相同的實施例或示例。而且，描述的具體特征、結構、材料或者特點可以在任何 的一個或多個實施例或示例中以合適的方式結合。
[0106] 盡管上面已經示出和描述了本發明的實施例，可以理解的是，上述實施例是示例 性的，不能理解為對本發明的限制，本領域的普通技術人員在不脫離本發明的原理和宗旨 的情況下在本發明的范圍內可以對上述實施例進行變化、修改、替換和變型。本發明的范圍 由所附權利要求極其等同限定。
【主權項】
1. 一種室內無人機導航方法，其特征在于，包括如下步驟： 步驟S1，采用三軸陀螺儀測量無人機的角速度，并采用多組超聲波測距傳感器測量超 聲波信號、采用溫度傳感器測量室內的溫度數據； 步驟S2,根據所述室內的溫度數據對測量的超聲波信號進行溫度補償； 步驟S3,利用溫度補償后的超聲波信號分別計算所述無人機與室內的四壁、天花板和 地板的距離，并根據無人機與室內的四壁、天花板和地板的距離反向定位出所述無人機的 位置； 步驟S4,根據所述無人機的角速度，計算所述無人機的姿態角，包括如下步驟： 步驟S41，建立機體坐標系和導航坐標系； 步驟S42,根據所述無人機的角速度計算所述無人機在所述機體坐標系和導航坐標系 下，機體轉動的旋轉矩陣，并根據所述旋轉矩陣計算所述無人機的飛機姿態矩陣； 步驟S43,根據所述飛機姿態矩陣計算所述無人機的飛行姿態角。2. 如權利要求1所述的室內無人機導航方法，其特征在于，所述超聲波測距傳感器為6 組，分別安裝于所述無人機上多個固定位置處。3. 如權利要求1所述的室內無人機導航方法，其特征在于，在所述步驟S42中，所述飛 行姿態角為機體按照以下順序轉動時的角度：繞所述Z軸轉動的角度Φ，繞所述Y軸轉動 的角度Θ，繞所述X軸轉動的角度Φ。4. 如權利要求1所述的室內無人機導航方法，其特征在于，在所述步驟S42中，所述旋 轉矩陣為（cl，c2, c3)， 根據繞z軸轉動Φ角，計算得到根據繞y軸轉動θ角，計算得到根據繞X軸轉動Φ角，計算得到5. 如權利要求4所述的室內無人機導航方法，其特征在于，在所述步驟S42中，根據所 述旋轉矩陣計算所述無人機的飛機姿態矩陣，包括如下步驟： 根據所述旋轉矩陣將參考體系變化至載體系，cnb= c3c2cl ; 然后計算所述飛機姿態矩陣Cbn為：6. 如權利要求5所述的室內無人機導航方法，其特征在于，在所述步驟S43中，所述無 人機的飛行姿態角分別為：7. -種無人機，其特征在于，包括：電源模塊、電池、電機、電機驅動模塊、多個超聲波 測距傳感器、溫度傳感器、陀螺儀和控制芯片，其中， 所述電池與所述電源模塊、所述電機和所述控制芯片分別相連， 所述電機驅動模塊的一端與所述電機相連，另一端與所述控制芯片的脈沖寬帶調制單 元相連，用于在所述脈沖寬帶調制單元輸出的PWM脈沖信號的控制下驅動所述電機工作； 所述多個超聲波傳感器與所述控制芯片的監測端口相連，用于測量超聲波信號并傳送 至所述控制芯片； 所述溫度傳感器與所述控制芯片的監測端口相連，用于測量室內的溫度數據并傳送至 所述控制芯片； 所述陀螺儀與所述控制芯片的SPI接口相連，用于測量所述無人機的角速度并傳送至 所述控制芯片； 所述控制芯片根據所述室內的溫度數據對測量的超聲波信號進行溫度補償，利用溫度 補償后的超聲波信號分別計算所述無人機與室內的四壁、天花板和地板的距離，并根據無 人機與室內的四壁、天花板和地板的距離反向定位出所述無人機的位置，以及所述控制芯 片還用于根據所述無人機的角速度，計算所述無人機的姿態角，包括：建立機體坐標系和導 航坐標系，然后根據所述無人機的角速度計算所述無人機在所述機體坐標系和導航坐標系 下，機體轉動的旋轉矩陣，并根據所述旋轉矩陣計算所述無人機的飛機姿態矩陣，并根據所 述飛機姿態矩陣計算所述無人機的飛行姿態角。8. 如權利要求7所述的無人機，其特征在于，所述無人機為四旋翼無人機，所述多個超 聲波傳感器分別固定于所述四旋翼無人機的四個機翼和中心位置處。9. 如權利要求7所述的無人機，其特征在于，還包括：燈光閃爍模塊，所述燈光閃爍模 塊與所述控制芯片相連，用于在所述無人機飛行時發出閃爍光以提示周邊。10. 如權利要求7所述的無人機，其特征在于，還包括：無線通信模塊，所述無線通信模 塊與所述控制芯片相連，用于與外部的終端設備進行通信交互，以由外部的終端設備遠程 控制所述無人機。
【專利摘要】本發明提出了一種室內無人機導航方法及無人機，該導航方法包括：采用三軸陀螺儀測量無人機的角速度，并采用多組超聲波測距傳感器測量超聲波信號、采用溫度傳感器測量室內的溫度數據；根據所述室內的溫度數據對測量的超聲波信號進行溫度補償；利用溫度補償后的超聲波信號分別計算所述無人機、與室內的四壁、天花板和地板的距離，并根據無人機與室內的四壁、天花板和地板的距離反向定位出所述無人機的位置；根據所述無人機的角速度，計算所述無人機的姿態角。本發明應用較少的傳感器，實現對室內及無人機相關數據采集，生產成本較低，適合推廣使用。
【IPC分類】G01C21/16, G01C21/20
【公開號】CN105352505
【申請號】CN201510888634
【發明人】黃磊, 王根勇, 劉聰, 李學風
【申請人】北京健德乾坤導航系統科技有限責任公司
【公開日】2016年2月24日
【申請日】2015年12月8日