一種車輛定位系統及其定位方法
【技術領域】
[0001] 本發明涉及汽車定位技術領域，尤其涉及一種車輛定位系統及其定位方法。
【背景技術】
[0002] 隨著城市化的快速發展，近年來國內的大中城市出現了大量的MALL(商業綜合 體），這些MALL-般都配備有大型的停車場，停車場可能會有多達上千個車位，有些還不止 一層。車主在停車場泊車完畢后，要找回時，有些人會忘了自己的車具體停在哪個位置。為 找回車子，車主往往會花費大量的時間和精力。如果有一種便攜型、離線式的車輛定位系 統，能幫助車主準確、快速地鎖定自己車輛的所在位置，并在找回的過程中給予導引，這必 將極大地提高找回車輛的效率，節省車主的時間和精力。

【發明內容】

[0003] 本發明的目的是克服車主有時忘了車輛具體的停車位置后需要花費大量時間才 能找到汽車，尤其是在大型停車場內，需要花費的時間和精力更多的技術問題，提供了一種 車輛定位系統及其定位方法，其能夠幫助車主快速準確的鎖定車輛的位置，并給予車主引 導，使車主能夠快速的找到自己的汽車，節省了車主的時間和精力。
[0004] 為了解決上述問題，本發明采用以下技術方案予以實現：
[0005] 本發明的一種車輛定位系統，包括設置在汽車上的定位裝置和用戶攜帶的指示裝 置，定位裝置包括底座以及可帶動底座上下左右轉動的電動云臺，底座上設有升降器，升降 器的頂端設有可水平轉動的旋轉機構，旋轉機構上設有可沿水平方向伸縮運動的第一伸縮 桿和第二伸縮桿，底座上設有中央處理單元、電子水平儀、第一無線通信模塊、第一電子羅 盤模塊和第一UWB定位傳感器，旋轉機構上設有第二UWB定位傳感器，第一伸縮桿前端設有 第三UWB定位傳感器，第二伸縮桿前端設有第四UWB定位傳感器，第一UWB定位傳感器和第 二UWB定位傳感器的連線與底座垂直，第二UWB定位傳感器和第三UWB定位傳感器的連線 與底座平行，第二UWB定位傳感器和第四UWB定位傳感器的連線與底座平行，指示裝置包括 控制器、第二無線通信模塊、UWB定位標簽、顯示屏、按鍵組和第二電子羅盤模塊，中央處理 單元分別與電動云臺、升降器、旋轉機構、第一伸縮桿、電子水平儀、第一無線通信模塊、第 一電子羅盤模塊、第一UWB定位傳感器、第二UWB定位傳感器、第三UWB定位傳感器、第二伸 縮桿和第四UWB定位傳感器電連接，控制器分別與第二無線通信模塊、UWB定位標簽、顯示 屏、按鍵組和第二電子羅盤模塊電連接。
[0006] 在本技術方案中，第一伸縮桿和第二伸縮桿相垂直。當用戶在大型停車場內忘了 車輛具體的停車位置時，啟動隨身攜帶的指示裝置工作，控制器通過第二無線通信模塊向 定位裝置發送定位指令。
[0007] 定位裝置接收到定位指令后，中央處理單元先通過電子水平儀檢測底座是否水 平，如果底座不水平，則中央處理單元控制電動云臺將底座轉動到水平位置。接著，中央處 理單元通過第一電子羅盤模塊獲取地磁正北的方向，以第一UWB定位傳感器為原點，以地 磁正北方向為Y軸正方向，以Y軸在水平面順時針轉動90度后的指向為X軸正方向，以豎 直向上方向為Ζ軸正方向，建立ΧΥΖ三維直角坐標系，第一UWB定位傳感器和第二UWB定位 傳感器的連線與Ζ軸重合。然后，中央處理單元控制升降器將旋轉機構升到設定的高度，使 得第二UWB定位傳感器、第三UWB定位傳感器和第四UWB定位傳感器的Ζ軸坐標都為zl， 接著控制旋轉機構將第一伸縮桿轉動到指向地磁正北的方向，控制第一伸縮桿和第二伸縮 桿伸長設定的長度，使得第三UWB定位傳感器的Y軸坐標為y2,第一伸縮桿和第二伸縮桿 始終保持相互垂直，第四UWB定位傳感器的X軸坐標為x3,此時，第一UWB定位傳感器的坐 標為（〇,〇,〇)，第二UWB定位傳感器的坐標為（0,0，zl)，第三UWB定位傳感器的坐標為（0， y2,zl)，第四UWB定位傳感器的坐標為（x3,0,zl)，第一UWB定位傳感器檢測到其與UWB定 位標簽之間的距離為R。，第二UWB定位傳感器檢測到其與UWB定位標簽之間的距離為札，第 三UWB定位傳感器檢測到其與UWB定位標簽之間的距離為R2，第四UWB定位傳感器檢測到 其與UWB定位標簽之間的距離為R3，假設UWB定位標簽的坐標為（X，y，ζ)，建立方程組，計 算出X，y，ζ的值，得到UWB定位標簽的坐標值。
[0008] 中央處理單元根據X，y的值計算出UWB定位標簽在X軸和Y軸所在的平面上的投 影指向第一UWB定位傳感器的方向與Y軸正方向（即地磁正北方向）之間的角度值α，將 該角度值α和UWB定位標簽的坐標值發送到指示裝置。控制器通過第二電子羅盤模塊獲取 地磁正北的方向，根據地磁正北的方向和接收到的角度值α確定顯示屏上顯示的指針的 指向，顯示屏顯示出指針，以地磁正北方向為起始，逆時針轉動為角度增大方向，該指針的 指向與地磁正北之間的角度值始終為α。控制器根據ζ的值確定UWB定位標簽與第一UWB 定位傳感器在豎直方向上的距離，以UWB定位標簽為原點，豎直向上為距離正方向，顯示屏 顯示該距離D，該距離D= -ζ，距離D為正數表示汽車在車主上方，距離D為負數表示汽車 在車主下方。定位裝置實時檢測角度值α和UWB定位標簽的坐標值，并發送到指示裝置， 指示裝置實時調整指針方向。
[0009] 作為優選，指示裝置還包括紅外測距儀，所述紅外測距儀與控制器電連接。車主可 通過指示裝置上的紅外測距儀檢測停車場一層的高度，控制器根據距離D估算出汽車與車 主之間相隔的樓層數，供車主參考。
[0010] 作為優選，所述定位裝置設置在汽車頂部。
[0011] 作為優選，所述第一伸縮桿和第二伸縮桿之間的夾角為45-135度。
[0012] 本發明的一種車輛定位系統的定位方法，包括以下步驟：
[0013] S1 :啟動指示裝置工作，控制器通過第二無線通信模塊向定位裝置發送定位指 令；
[0014] S2 :定位裝置接收到定位指令后，中央處理單元通過電子水平儀檢測底座是否水 平，如果底座不水平，則中央處理單元控制電動云臺將底座轉動到水平位置；
[0015] S3 :中央處理單元通過第一電子羅盤模塊獲取地磁正北的方向，以第一UWB定位 傳感器為原點，以地磁正北方向為Υ軸正方向，以Υ軸在水平面順時針轉動90度后的指向 為X軸正方向，以第一UWB定位傳感器指向第二UWB定位傳感器的方向為Ζ軸正方向，建立 ΧΥΖ三維直角坐標系；
[0016] S4 :中央處理單元控制升降器將旋轉機構升到設定的高度，使得第二UWB定位傳 感器、第三UWB定位傳感器、第四UWB定位傳感器的Ζ軸坐標都為zl，接著控制旋轉機構將 第一伸縮桿轉動到指向地磁正北的方向，控制第一伸縮桿和第二伸縮桿伸長設定的長度， 使得第三UWB定位傳感器的Y軸坐標為y2,第四UWB定位傳感器的X軸坐標為x3,Y軸坐 標為y3,此時，第一UWB定位傳感器的坐標為（0,0,0)，第二UWB定位傳感器的坐標為（0， 0,zl)，第三UWB定位傳感器的坐標為（0,y2,zl)，第四UWB定位傳感器的坐標為（x3,y3， zl)，第一UWB定位傳感器檢測到其與UWB定位標簽之間的距離為R。，第二UWB定位傳感器 檢測到其與UWB定位標簽之間的距離為&，第三UWB定位傳感器檢測到其與UWB定位標簽 之間的距離為R2，第四UWB定位傳感器檢測到其與UWB定位標簽之間的距離為R3，假設UWB 定位標簽的坐標為（X，y，z)，得到方程組：
[0017]
[0018] 采用基于泰勒級數展開的最小二乘法進行迭代求解，求出X，y，z的值，得到UWB定 位標簽的坐標值；
[0019] S5 :中央處理單元根據UWB定位標簽的坐標值，以Y軸正方向為起始，逆時針轉動 為角度增大方向，計算出Y軸正方向與第一UWB定位傳感器指向UWB定位標簽在X軸和Y 軸所在的平面上的投影的方向之間的角度值Θ，從而計算出UWB定位標簽在X軸和Y軸所 在的平面上的投影指向第一UWB定位傳感器的方向與Y軸正方向之間的角度值α，定位裝 置將計算出的角度值α以及UWB定位標簽的坐標值發送到指示裝置；
[0020] S6 :控制器通過第二電子羅盤模塊獲取地磁正北的方向，以地磁正北方向為起始， 逆時針轉動為角度增大方向，根據接收到的角度值α確定顯示屏上顯示的指針的指向，顯 示屏顯示出指針，該指針的指向與地磁正北之間的角度值始終為α。
[0021] 作為優選，所述步驟S6還包括以下步驟：控制器根據UWB定位標簽的坐標值確定 UWB定位標簽與第一UWB定位傳感器在豎直方向上的距離，以UWB定位標簽為原點，豎直向 上為距離正方向，顯示屏顯示該距離。使得用戶了解汽車與用戶之間的在豎直方向上的位 置關系，汽車與