[發(fā)明專利]一種基于二維碼的導航定姿定位方法和系統(tǒng)

在審

• 申請?zhí)枺?/td>: 201910733720.1
• 申請日: 2019-08-09
• 公開（公告）號: CN110345937A
• 公開（公告）日: 2019-10-18
• 發(fā)明（設計）人: 王峰;潘觀潮;劉進輝;王宏武;王曉灑
• 申請（專利權）人: 東莞市普靈思智能電子有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16;G01C11/00;G01C1/00;G01B11/00;G06K17/00
• 代理公司: 東莞市啟信展華知識產(chǎn)權代理事務所(普通合伙) 44579
• 代理人: 馮蓉

本發(fā)明公開了一種基于二維碼的導航定姿定位方法和系統(tǒng)，所屬導航定姿定位方法包括以下步驟：(1)設置若干個二維碼；(2)單目相機拍攝到二維碼；(3)對拍攝到的二維碼進行IPM映射；(4)獲取該二維碼在世界坐標系中的位置、姿態(tài)角和幾何尺寸信息；計算出單目相機在二維碼坐標系中的位置和姿態(tài)角；(5)計算出單目相機在世界坐標系中的位置和姿態(tài)角，實現(xiàn)定位和定姿。本發(fā)明通過使用二維碼，再增加從二維碼中提取的位置信息和姿態(tài)角信息，不僅能夠?qū)崿F(xiàn)對自動駕駛汽車和移動機器人等裝置的三維定位和定姿，且易于實現(xiàn)，成本低廉。

技術領域

本發(fā)明涉及導航定姿定位技術領域，特別涉及一種基于二維碼的導航定姿定位方法和系統(tǒng)。

背景技術

自動駕駛汽車和機器人必須知道自身的位置和姿態(tài)角，才能實現(xiàn)可控制的移動。目前自動駕駛汽車和移動機器人普遍使用激光掃描雷達、攝像頭傳感器和同時定位和建圖(Simultaneous Location And Mapping，SLAM)算法實現(xiàn)車輛和機器人的定位和定姿，再增加全球?qū)Ш蕉ㄗ硕ㄎ恍l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)和慣性導航協(xié)助。激光掃描雷達精度較高，但成本也很高，不適應于大規(guī)模商業(yè)化應用。由于車輛和機器人周圍的環(huán)境的動態(tài)特征，SLAM算法也并不可靠。全球?qū)Ш叫l(wèi)星系統(tǒng)也容易受到遮擋，在室內(nèi)或者隧道內(nèi)完全不能定位。慣性導航也不能長時間的保存車輛的位置和姿態(tài)信息，在失去復雜環(huán)境下定位和定姿的精度。

目前，二維碼定位已經(jīng)被廣泛用于室內(nèi)自動導航車輛(Automatic GuidedVehicle，AGV)的定位和定姿。通常二維碼是貼在地板上，當安裝在車輛底部的攝像頭拍到二維碼時，AGV根據(jù)二維碼上的信息計算出自己的平面位置(x,y)和偏航角(θ)。這個方法對于室內(nèi)封閉環(huán)境的AGV非常實用，但它并不能擴展到自動駕駛汽車。首先，在開放環(huán)境下，比如室外道路，地面的二維碼很容易被車輪和人類活動污染，失去導航定姿定位的能力。其次，該方法只能定出AGV的二維位置坐標和偏航角。但是，自動駕駛汽車和移動機器人并不限于在水平的地面上行駛。因此，有必要提出一種新的基于二維碼的導航定姿定位方法和系統(tǒng)，能夠計算出自動駕駛汽車或者機器人等裝置在任何時間的三維空間位置(x，y，z)和三個姿態(tài)角(俯仰角、偏航角、滾動角)，實現(xiàn)三維定位。

可見，現(xiàn)有技術還有待改進和提高。

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

鑒于上述現(xiàn)有技術的不足之處，本發(fā)明的目的在于提供一種基于二維碼的導航定姿定位方法和系統(tǒng)，旨在解決現(xiàn)有技術中基于二維碼的導航定姿定位方法只能進行二維定位的技術問題。

為了達到上述目的，本發(fā)明采取了以下技術方案：

一種基于二維碼的導航定姿定位方法，其特征在于，包括以下步驟：

(1)設置若干個二維碼；

(2)單目相機拍攝到二維碼；

(3)對拍攝到的二維碼進行逆透視映射；

(4)獲取該二維碼在世界坐標系中的位置、姿態(tài)角和幾何尺寸信息；計算出單目相機在二維碼坐標系中的位置和姿態(tài)角；

(5)計算出單目相機在世界坐標系中的位置和姿態(tài)角，實現(xiàn)定位和定姿。

進一步地，所述的基于二維碼的導航定姿定位方法中，每個二維碼分別設置有位置標識符、時間標識符和擬合標識符。

進一步地，所述的基于二維碼的導航定姿定位方法中，步驟(3)中，拍攝到的二維碼的二維像素空間和世界坐標滿足逆透視映射關系；所述逆透視映射和世界坐標到單目相機坐標的三維平移和三軸轉動相關。

進一步地，所述的基于二維碼的導航定姿定位方法中，單目相機到二維碼的距離利用二維碼的大小，焦距和二維碼的成像大小計算得到。