一种车辆定位系统及其定位方法
【技术领域】
[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定位标签为原点，竖直向 上为距离正方向，显示屏显示该距离。使得用户了解汽车与用户之间的在竖直方向上的位 置关系，汽车与