[發明專利]基于超聲和磁力計的機器人室內定位系統及方法

在審

• 申請號: 201710641542.0
• 申請日: 2017-07-31
• 公開（公告）號: CN107314766A
• 公開（公告）日: 2017-11-03
• 發明（設計）人: 李銘;李浩
• 申請（專利權）人: 成都楷模電子科技有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/20
• 代理公司: 成都行之專利代理事務所(普通合伙)51220
• 代理人: 王記明

1.基于超聲和磁力計的機器人室內定位系統，包括位于天花板上的信標節點和位于機器人上的接收節點，其特征在于，所述信標節點的數量至少為一個，每個信標節點由一個超聲波發射探頭組成，每個接收節點包括至少三個超聲波接收探頭，在機器人上還設置有地磁傳感器，所述超聲波接收探頭和地磁傳感器均與接收控制系統連接，其中：

超聲波發射探頭：發射超聲波信號給超聲波接收探頭；

地磁傳感器：探測地磁方向信號，并將地磁方向信號傳輸給接收控制系統；

超聲波接收探頭：接收超聲波發射探頭發射的超聲波信號，并將超聲波信號傳輸給接收控制系統；

接收控制系統：接收超聲波接收探頭傳輸的超聲波信號和地磁傳感器傳輸的地磁方向信號。

2.根據權利要求1所述的基于超聲和磁力計的機器人室內定位系統，其特征在于，當每個接收節點由三個超聲波接收探頭組成時，三個超聲波接收探頭圍成三角形接收陣列。

3.根據權利要求1所述的基于超聲和磁力計的機器人室內定位系統，其特征在于，當每個接收節點由四個超聲波接收探頭組成時，四個超聲波接收探頭位于同一平面，且四個探頭中相對的兩個探頭的連線與另外兩個相對的探頭的連線相互垂直。

4.根據權利要求1所述的基于超聲和磁力計的機器人室內定位系統，其特征在于，所述接收控制系統內的存儲器采用FIFO存儲器。

5.基于超聲和磁力計的機器人室內定位方法，其特征在于，包括以下步驟：

A：接收節點接收信標節點發射的超聲波信號，根據陣列測向理論，測得信標節點到接收節點的俯仰角水平角θ；

B：測量出信標節點所在平面到接收節點所在平面的垂直高度h，根據信標節點到接收節點的俯仰角通過計算得到接收節點距離信標節點在水平面上的投影長度l；

C：地磁傳感器探測地磁方向信號，獲得機器人在水平面的偏離角度θ₀；設定兩個超聲波接收探頭的基線方向與地磁傳感器的探測方向重合時的方向為初始方向，根據信標節點到接收節點的水平角θ，可計算出機器人與信標節點之間的真實角度θ′＝θ₀±θ；

D：由于信標節點的坐標已知，設為(x₀,y₀)，再根據方程組x₁＝x₀+l·cosθ′，y₁＝y₀+l·sinθ′，可計算得到機器人的坐標為(x₁,y₁)。