[發明專利]一種移動機器人3D定位系統

有效

• 申請號: 201810326546.4
• 申請日: 2018-04-12
• 公開（公告）號: CN108759822B
• 公開（公告）日: 2021-04-30
• 發明（設計）人: 平雪良;高文研;劉瀟瀟;王昕煜;蔣毅
• 申請（專利權）人: 江南大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C22/00
• 代理公司: 南京禹為知識產權代理事務所(特殊普通合伙) 32272
• 代理人: 王曉東

1.一種移動機器人3D定位系統，其特征在于：包括，

同步定位建圖單元(100)和導航單元(200)，所述同步定位建圖單元(100)和所述導航單元(200)之間可以切換，所述同步定位建圖單元(100)是所述導航單元(200)的2D位姿，所述導航單元(200)顯示所述同步定位建圖單元(100)的角度和初始位姿；

所述導航單元(200)包括算法(201)，所述算法(201)進行信息融合，所述信息包括多種傳感器的輸入和所述同步定位建圖單元(100)算法結果融合；

所述算法結果融合，先將移動機器人在空間所處的狀態用15維向量X進行描述：

X＝[x,y,z,θ_r,θ_p,θ_y,v_x,v_y,v_z,ω_r,ω_p,ω_y,a_x,a_y,a_z]^T

其中，x，y，z分別代表三維位置，θ_r,θ_p,θ_y表示各個位置的角度，v_x，v_y，v_z分別代表各個位置的線速度，w_r，w_p，w_y分別代表各個位置的角速度，a_x，a_y，a_z分別代表各個位置的加速度；

所述同步定位建圖單元(100)包括移動機器人的定位和坐標設置；

所述定位，通過全局定位對移動機器人的當前位置進行準確估計，再通過相對定位方式對其運動狀態進行預測，并不斷修正；

所述坐標，其包括世界坐標系、里程計坐標系、移動機器人中心坐標系、四個驅動輪的坐標系和各個傳感器坐標系，所述里程計坐標系由安裝在所述四個驅動輪上的編碼器計算里程計數據，從而確定其相對于世界坐標系的相對關系；

所述移動機器人以所述移動機器人中心坐標系為參考系，所述移動機器人在所述世界坐標系中與其當前θ_r,θ_p,θ_y和當前所處平面有關；

其中，以s表示函數sin，c表示函數cos，r、p、y分別表示所述移動機器人中心坐標系相對于所述世界坐標系繞x、y、z軸旋轉的角度；

若通過傳感器配置和相關算法，已經獲得15維向量X的所有成員，將移動機器人在所述移動機器人中心坐標系中各個方向上的運動向世界坐標系進行投影，在采樣時間為Δt時，可得融合后的預測；

2.根據權利要求1所述的移動機器人3D定位系統，其特征在于：確定系統的模型，所述系統的模型是將三式整理成矩陣形式得到：

其中，

3.根據權利要求2所述的移動機器人3D定位系統，其特征在于：所述系統的模型求偏導數，得到算法迭代所需雅各比矩陣并得到十五維空間上，傳感器和算法結果的融合過程為：

4.根據權利要求1～3任一所述的移動機器人3D定位系統，其特征在于：所述四個驅動輪的坐標系包括，后右輪鏈接、后左輪鏈接、前右輪鏈接和前左輪鏈接。

5.根據權利要求1～3任一所述的移動機器人3D定位系統，其特征在于：所述同步定位建圖單元(100)還包括數據處理模塊(101)、掃描匹配模塊(102)和構圖(103)。

6.根據權利要求5所述的移動機器人3D定位系統，其特征在于：所述移動機器人采用多機分布式控制和分布式計算。