CN109029445A - 一种用于室内的自主定位装置及方法 - Google Patents

Abstract

一种用于室内的自主定位装置，其特征在于：包括定位节点和中心总控平台调度系统，所述定位节点分为固定定位节点和移动定位节点两种工作模式，所述固定定位节点和移动定位节点是同一种设备，根据不同的情况设置成不同的工作模式，所述中心总控平台调度系统包含基于多模块室内定位算法。本发明电路结构简单，成本低，性能高，定位速度快，并且对UWB定位结果进行了算法优化，使得离散后算法运算量小，可以快速实现定位。

Description

一种用于室内的自主定位装置及方法

技术领域

本发明涉及电子通信领域，尤其涉及一种用于室内的自主定位装置及方法。

背景技术

全球定位系统和北斗系统的成功应用，解决了户外的精确定位需求，但是由于目前的建筑物主要是钢筋混凝土结构，室外的卫星定位信号难以穿透墙体等障碍物，使得室内没有很好的定位技术。随着目前人们生活水平的提高，需要越来越高的自动化服务设备，很多服务必须依托于高精度室内定位技术才能实现，定位精度要达到厘米级别。

针对室内定位人们提出了多种解决方案，比如红外线、WIFI、蓝牙、 RFID、Zigbee等技术，但是这些技术共同的缺陷在于定位精度达不到要求，大部分采用RSSI测量信号强度的方式来估计当前设备的位置，只能给出米级别的范围，而无法得到精确的定位信息。近几年提出的UWB超带宽定位技术很好的解决了精确测距的问题，但是该种定位技术也存在一定缺陷，就是波长短，无法对障碍物绕射测量，一旦有障碍物则测距结果就发生错误。

发明内容

针对上述问题，本发明提供了一种用于室内的自主定位装置及方法。

为解决上述技术问题，本发明采用的技术方案是：包括定位节点和中心总控平台调度系统，所述定位节点分为固定定位节点和移动定位节点两种工作模式，所述固定定位节点和移动定位节点是同一种设备，根据不同的情况设置成不同的工作模式，所述中心总控平台调度系统包含基于多模块室内定位算法。

进一步的，所述定位节点由双电源供电系统和测距系统两部分构成。

进一步的，所述双电源供电系统为整个定位节点提供电源，具备外部电源输入，所述双电源供电系统包括电源管理单元，所述电源管理单元直接将外部输入电压输出直流，所述电源管理单元分出一条支路给锂电池充电单元，用于设备电池的充电，在没有外部直流的情况下，锂电池通过 SPX3819低压差LDO放电，输出稳定的直流，所述双电源供电系统具备双回路，保证系统稳定供电。

进一步的，所述测距系统包括STM32控制核心、看门狗复位单元、拨码开关模式设置单元、TFT液晶显示单元、通信单元、DWM1000测距模组和 UWB测距技术。

进一步的，所述通信单元包括Zigbee通信单元、485总线单元和可扩展通信单元，所述Zigbee通信单元是无线通信方式，所述485总线单元是有线通信模式，所述可扩展通信单元为设备预留通信接口。

进一步的，所述通信接口包括两组全双工TTL232接口，一个用来与电脑调试通信，作为打印设备信息和测试设备的shell接口，另,一个为预留接口，当设备需要其它互联方式时，通过转到TTL串口直接互联。

进一步的，所述TFT液晶显示单元采用LCD5110显示模组，LCD5110显示模组为84×48的点阵，显示4行汉字，30个字符，设备开机后用来显示开机自检过程。。

进一步的，基于权利要求1至4任意一项所述的一种用于室内的自主定位装置，包括机载节点的位置定位信息步骤，所述记载节点的位置定位信息步骤包括如下步骤：

SS1.安装好定位装置，在一个房间内保证至少有3个固定节点，定位节点安装在距离地面2m的高度，同时远离墙体不低于10cm。

SS2.设置好所有定位节点的通信模式和通信频率，保持一致，设置好定位节点的功能和ID号，保证所有的定位节点ID号不重复，并开机自检。

SS3.开机自检后，显示模组没有显示错误信息(INIL ERROR)，则表明定位节点模块工作正常，提示错误信息，则需要复位重启。

SS4.打开机载节点电源，查看显示模组信息，模组没有显示错误，并且能够轮流显示与各个固定节点的距离，则表明整个硬件系统测距功能是完善的，否则重启模块，直至能够正确显示测距信息。

SS5.中心总控平台通过Zigbee发送shell指令”Zig getdistance Data”，将所有测距模块的测距信息上传到中心总控平台。中心总控平台相关软件统计得到的定位信息，根据定位算法测得机载节点的位置信息。

SS6.在步骤SS5的基础上，利用RANSAC算法剔除错误数据，得到记载节点的位置定位信息的正确数据。

进一步的，所述步骤SS6中RANSAC算法的计算方法如下：

SS61：从定位节点1到n中任取三组节点，这三组节点的距离利用所述步骤SS5中的定位算法的方程组求出一个定位结果(X'Y'Z')。

SS62：利用该定位结果(X'Y'Z')计算到剩余节点的距离(D'₁D'₂...D'_n-3)，算出剩余节点计算距离与测量距离(D₁D₂...D_n-3)的误差的平方取该误差之和的平局值作为距离阈值t²；若计算距离与测量距离的误差的平方E²小于t²，则认为该节点为内点，否则为外点，并统计对用内点的个数n₁。

SS63：再从定位节点1到n中任意取三组数据，重复步骤SS61和SS62，求出内点的个数n₂和对应的其它参数。

SS64：重复10次步骤SS61和SS62，得到10组数据，取内点数n最大的三组节点作为数据正确的定位节点。若出现内点两组相同，则取最大误差平方较小的那组数据作为正确的定位节点，若最大误差平方相同则比较第二大误差平方直至找出需要的定位节点数据，该组节点数据求出的代数中心命名为X_rY_rZ_r。

SS65：利用计算出的代数中心到剩余节点的距离来剔除错误节点，因测距传感器的精度为±10cm，误差结果超过这个范围的定位节点数据认为是错的。

SS66：得到代数中心后，利用代数中心数据的摄动，使代数中心与真实定位坐标的误差减小，得到优化的定位值，该摄动优化步骤如下：

SS661:根据RANSAC算法得到代数中心点R(X_rY_rZ_r)，利用剩余的i 个正确节点数据进行优化，根据空间距离公式可得其余正确节点到代数中心的误差为

SS662：求出e_i对t的导数：

令i＝1…i，列出除去野点节点外所有其它定位节点误差导数方程，令

得到公式

SS663：构造Lyapunov函数则求导得：选择控制律则有由此可知算法收敛于平衡点，对控制率离散化得到：因此按照设计的离散控制律迭代十次，得到精度更高的几何中心点。

由上述对本发明的描述可知，和现有技术相比，本发明具有如下优点：

1、本发明电路结构简单，成本低，性能高，定位速度快。

2、本发明对于UWB定位结果进行了算法优化，且离散后算法运算量小，可以快速实现定位。

3、本发明具备多种供电方式、通信方式，方便安装和组网。

4、本发明将固定节点和移动节点功能做成一体化，可以根据需求选择不同工作模式，方便简洁。

5、本发明还可以加装防水、防尘、防爆外壳，使用工作范围广。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明一种用于室内的自主定位装置的定位节点系统结构框图；

图2为本发明一种用于室内的自主定位装置的的单个房间内设备定位功能示意图；

图3为本发明一种用于室内的自主定位装置的移动设备节点坐标计算参考坐标系图；

图4为本发明一种用于室内的自主定位装置的DW1000外围接口及功能设置电路。

图5为本发明一种用于室内的自主定位装置的液晶显示电路和485通信电路。

图6为本发明一种用于室内的自主定位装置的STM32单片机控制核心电路。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例

参考图1-图6，一种用于室内的自主定位装置，包括定位节点和中心总控平台调度系统，定位节点分为固定定位节点和移动定位节点两种工作模式，该固定定位节点和移动定位节点是同一种设备，只是根据不同的情况设置成不同的工作模式，如果固定在一个位置，此时需要将节点设置为固定定位节点，如果安装在移动设备上，此时节点设置为移动定位节点。

优选的，定位节点主要包括两部分：双电源供电系统1和测距系统2，双电源供电系统1为整个定位节点提供电源，具备外部电源输入，电压范围是9～35V，双电源供电系统1包括电源管理单元11，该电源管理单元11 直接将外部输入电压输出为3.3V直流，同时电源管理单元11分出一条支路给锂电池充电单元12，用于设备电池的充电，如果没有外部直流电源，则锂电池组13通过SPX3819低压差LDO放电，输出稳定3.3V直流，双电源供电系统1具备双回路功能，外部电源输入和机载电池配合，保证系统稳定供电。

优选的，定位节点的测距系统2包括：STM32控制核心24、看门狗复位单元21、拨码开关模式设置单元22、TFT液晶显示单元23、Zigbee通信单元27、485总线单元28、可扩展通信单元26、DWM1000测距模组25和 UWB(3.1Ghz～6.5Ghz)。

STM32控制核心24用来实现定位节点的功能。

看门狗复位单元21检测STM32单片机的工作状态，如果发现STM32处于失控状态，立即复位单片机，保证系统可靠运行。

拨码开关模式设置单元22用来确定定位节点的模式、通信频率、节点

功能和节点ID号。

TFT液晶显示单元23用来显示测量的距离信息、设备自检信息等。

Zigbee通信单元27、485总线单元28和可扩展通信单元26是用来实

现设备的互联通信，包括设备与设备的通信，设备与中心总控平台的

通信。

DWM1000测距模组25实现定位节点间距离的测量。

定位节点的测距系统2的拨码开关模式设置单元22的功能如表1所述，总共有12个拨码开关，1号控制通信模式，如果拨码开关打开，表示处于高速通信6.8Mbps模式，如果拨码开关关闭处于低速通信模式110kbps，高速模式下通信速度快，测距所用的时间短，但是更容易受到障碍物遮挡，低速模式通信速度慢，但是通信距离可以更远。2号控制通信频率，如果拨码开关打开，通信频率为3993.6Mhz，是UWB频段的通道2，如果拨码开关关闭，则选用的通信频率为6489.6Mhz。3号控制定位节点的功能，如果拨码开关打开，表示定位节点为固定定位节点，用来锚定距离，如果关闭，表示当前设备工作在移动节点模式，用于接收固定节点的无线信号，测量到相应固定节点的距离。4、5、6、7、8五个拨码开关设置定位节点的ID号，这个ID号是用来区分各个定位节点的，同时也是定位节点的通信地址。

表1

优选的，定位节点的测距系统2的通信部分包括，Zigbee通信单元27，该单元是无线通信方式，采用Zigbee自组网的方式进行信息透传，只要设备启动，Zigbee单元自动进行组网，任何一台设备发出的信息都能通过网络传递到目的设备中，485总线单元28是有线通信方式，为了方便设备用于多个房间的定位场合，当设备处于多个房间时，Zigbee单元可能会由于环境原因，没有办法保证在穿透墙壁的时候数据能够可靠传输，此时最可靠的方式还是采用有线的方案进行信息的回传，固定定位节点通过485总线单元28回传定位距离信息，中心总控平台也可通过485总线单元28进行控制指令的传输。可扩展通信单元26为该设备预留的通信接口，包括两组全双工TTL232接口，一个用来与电脑调试通信，作为打印设备信息和测试设备的shell接口，另外一个为预留接口，当设备需要其它互联方式，比如Internet接口、CAN总线接口等，可以通过转到TTL串口直接互联。

优选的，定位节点的测距系统2的TFT显示模组采用LCD5110显示模组，LCD5110模组为84×48的点阵，可以显示4行汉字，30个字符。当设备开机后用来显示开机自检过程，设备正常运行起来后显示测距距离，如果设备运行中出错，显示出错信息。

优选的，定位节点的测距系统2的DWM1000测距模组25，能够在无遮挡的条件下达到±10cm的测距精度，通信距离可达300m，在采用短数据包通信时，在半径20m的区域内，标签密度可达11000个，功耗低，单个供电电压为2.8v～3.6v，发射模式电流最大为31mA，接收模式电流最大为64mA，空闲模式下电流为2uA，支持TOF(飞行时间)和TDOA(到达时间差)定位机制，该DWM1000测距模组25主要是通过计算无线电波在空中传输的时间来计算两个模组之间的距离。

基于上述所述的一种用于室内的自主定位装置，本发明公开了一种用于室内的自主定位方法，包括机载节点的位置定位信息步骤，所述记载节点的位置定位信息步骤包括如下步骤：

SS1.安装好定位装置，在一个房间内保证至少有3个固定节点，为了减少设备被遮挡造成无法测距的情况，推荐安装5个以上。定位节点安装在距离地面2m左右的高度，同时远离墙体10cm以上。

SS2.设置好所有定位节点的通信模式，保持一致，如果距离不远的应用场景，尽量选用高速通信模式，设置好所有定位节点的通信频率，保持一致。设置好定位节点的功能，如果是固定定位节点打开3号拨码开关，如果是移动定位节点，关闭3号拨码开关，设置好定位节点的ID号，保证所有的定位节点ID号不重复，并开机自检。

SS3.开机自检后，如果显示模组没有显示错误信息(INIL ERROR)，则表明定位节点模块工作正常，如果提示错误信息，则需要复位重启。

SS4.打开机载节点电源，查看显示模组信息，如果模组没有显示错误，并且能够轮流显示与各个固定节点的距离，例如：A0T01.62m，A0代表0 号固定节点，T0代表0号机载节点，则表明整个硬件系统测距功能是完善的，否则重启模块，直至能够正确显示测距信息。

SS5.在中心总控平平台通过Zigbee发送shell指令”Zig getdistance Data”，将所有测距模块的测距信息上传到中心总控平台。总控平台相关软件统计得到的定位信息，根据定位算法测得机载节点的位置信息，所述定位算法的计算方法如下：

首先将安装有定位节点的空间建立坐标系XYZ-O，并且测量相应定位节点的三维坐标(X₁Y₁Z₁、X₂Y₂Z₂、X₃Y₃Z₃……X_nY_nZ_n)，每一次测距得到的距离为D₁、D₂、D₃……D_n。任取三组测距数据(D₁、D₂、D₃)，根据立体几何可得方程1，对公式进行求解，由于定位平面上的一点，因此Z_T可以认为是0，得到方程唯一解(X_TY_TZ_T)。

所述室内定位算法最基本原理已经在方程1中阐述，但是如果仅仅基于这个方程定位，很多情况下得不到正确位置，DWM1000测量距离本身误差是±10cm，但是一旦中间有障碍物的时候，无线电波发生绕射，导致DWM1000 给出错误的距离，当把错误距离代入公式计算时，所得到的结果也将是错误值，针对这个缺陷，在所述定位算法的基础上提出了基于RANSAC算法进行数据处理。在同一空间下，布置多个定位传感器，如果空间中存在遮挡物体，由于采用多个传感器，可以认为大多数时间下，传感器都是正常测量距离的，有少数传感器回传的距离是错的，利用RANSAC算法剔除错误数据，得到正确距离数据。根据多个正确距离数据求出代数中心，再利用代数中心数据构造误差函数，通过设计合理的扰动规律，减小误差，最终求得理想范围内的几何中心数据，也就是优化后的定位坐标X_TY_TZ_T，这种定位方法基本上排除错误数据干扰，并能够减小定位误差。

SS6.在步骤SS5的基础上，利用RANSAC算法剔除错误数据，得到记载节点的位置定位信息的正确数据，所述RANSAC算法的计算方法步骤如下：

在同一空间下，布置多个定位传感器，如果空间中存在遮挡物体，由于采用多个传感器，可以认为大多数时间下，传感器都是正常测量距离的，有少数传感器回传的距离是错的，利用RANSAC算法剔除错误数据，得到正确距离数据，根据多个正确距离数据求出代数中心，再利用代数中心数据构造误差函数，通过设计合理的扰动规律，减小误差，最终求得理想范围内的几何中心数据，也就是优化后的定位坐标X_TY_TZ_T，这种定位方法基本上排除错误数据干扰，并能够减小定位误差。

SS61：从定位节点1到n中任取三组节点，用到这三组节点的距离利用方程1求出一个定位结果(X'Y'Z')。

SS62：利用该定位结果(X'Y'Z')计算到剩余节点的距离(D'₁D'₂...D'_n-3)，算出剩余节点计算距离与测量距离(D₁D₂...D_n-3)的误差的平方取该误差之和的平局值作为距离阈值t²。若计算距离与测量距离的误差的平方E²小于t²，则认为该节点为内点，否则为外点，并统计对用内点的个数n₁。

SS63：再从定位节点1到n中任意取三组数据，重复步骤SS61和SS62，求出内点的个数n₂和对应的其它参数。

SS64：重复10次步骤SS61和SS62，得到10组数据，取内点数n最大的三组节点作为数据正确的定位节点，如果出现内点两组相同，则取最大误差平方较小的那组数据作为正确的定位节点，如果最大误差平方相同则比较第二大误差平方直至找出需要的定位节点数据，该组节点数据求出的代数中心命名为X_rY_rZ_r。

SS65：利用计算出的代数中心到剩余节点的距离来剔除错误节点，因测距传感器的精度为±10cm，误差结果超过这个范围的定位节点数据认为是错的。

SS66：得到代数中心后，利用代数中心数据的摄动，使代数中心与真实定位坐标的误差减小，得到优化的定位值，该摄动优化步骤如下：

SS661:根据RANSAC算法得到代数中心点R(X_rY_rZ_r)，利用剩余的i 个正确节点数据进行优化，根据空间距离公式可得其余正确节点到代数中心的误差为

SS662：求出e_i对t的导数：

令i＝1…i，列出除去野点节点外所有其它定位节点误差导数方程，令

得到公式

SS663：构造Lyapunov函数则求导得：选择控制律则有由此可知算法收敛于平衡点，对控制率离散化得到：因此按照设计的离散控制律迭代十次，得到精度更高的几何中心点。

所述算法可分两种情况使用，如果要求得到高精度定位结果，需要按照RANSAC算法后继续优化得到精确度更高的几何中心点，所消耗时间为 10s，如果要求高速情况下得到定位结果，可不必进行优化，直接用代数中心点作为定位结果，误差最大±10cm。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种用于室内的自主定位装置，其特征在于：包括定位节点和中心总控平台调度系统，所述定位节点分为固定定位节点和移动定位节点两种工作模式，所述固定定位节点和移动定位节点是同一种设备，根据不同的情况设置成不同的工作模式，所述中心总控平台调度系统包含基于多模块室内定位算法。

2.根据权利要求1所述的一种用于室内的自主定位装置，其特征在于：所述定位节点由双电源供电系统和测距系统两部分构成。

3.根据权利要求2所述的一种用于室内的自主定位装置，其特征在于：所述双电源供电系统为整个定位节点提供电源，具备外部电源输入，所述双电源供电系统包括电源管理单元，所述电源管理单元直接将外部输入电压输出直流，所述电源管理单元分出一条支路给锂电池充电单元，用于设备电池的充电，在没有外部直流的情况下，锂电池通过SPX3819低压差LDO放电，输出稳定的直流，所述双电源供电系统具备双回路，保证系统稳定供电。

4.根据权利要求2所述的一种用于室内的自主定位装置，其特征在于：所述测距系统包括STM32控制核心、看门狗复位单元、拨码开关模式设置单元、TFT液晶显示单元、通信单元、DWM1000测距模组和UWB测距技术。

5.根据权利要求4所述的一种用于室内的自主定位装置，其特征在于：所述通信单元包括Zigbee通信单元、485总线单元和可扩展通信单元，所述Zigbee通信单元是无线通信方式，所述485总线单元是有线通信模式，所述可扩展通信单元为设备预留通信接口。

6.根据权利要求5所述的一种用于室内的自主定位装置，其特征在于：所述通信接口包括两组全双工TTL232接口，一个用来与电脑调试通信，作为打印设备信息和测试设备的shell接口，另,一个为预留接口，当设备需要其它互联方式时，通过转到TTL串口直接互联。

7.根据权利要求4所述的一种用于室内的自主定位装置，其特征在于：所述TFT液晶显示单元采用LCD5110显示模组，LCD5110显示模组为84×48的点阵，显示4行汉字，30个字符，设备开机后用来显示开机自检过程。

8.一种用于室内的自主定位方法，其特征在于：基于权利要求1至4任意一项所述的一种用于室内的自主定位装置，包括机载节点的位置定位信息步骤，所述记载节点的位置定位信息步骤包括如下步骤：

SS1.安装好定位装置，在一个房间内保证至少有3个固定节点，定位节点安装在距离地面2m的高度，同时远离墙体不低于10cm。

SS2.设置好所有定位节点的通信模式和通信频率，保持一致，设置好定位节点的功能和ID号，保证所有的定位节点ID号不重复，并开机自检。

SS3.开机自检后，显示模组没有显示错误信息(INIL ERROR)，则表明定位节点模块工作正常，提示错误信息，则需要复位重启。

SS4.打开机载节点电源，查看显示模组信息，模组没有显示错误，并且能够轮流显示与各个固定节点的距离，则表明整个硬件系统测距功能是完善的，否则重启模块，直至能够正确显示测距信息。

SS5.中心总控平台通过Zigbee发送shell指令”Zig getd istance Data”，将所有测距模块的测距信息上传到中心总控平台，中心总控平台相关软件统计得到的定位信息，根据定位算法测得机载节点的位置信息。

SS6.在步骤SS5的基础上，利用RANSAC算法剔除错误数据，得到记载节点的位置定位信息的正确数据。

9.根据权利要求5所述一种用于室内的自主定位方法，其特征在于：所述步骤SS6中RANSAC算法的计算方法如下：

SS61：从定位节点1到n中任取三组节点，这三组节点的距离利用所述步骤SS5中的定位算法的方程组求出一个定位结果(X'Y'Z')。

SS62：利用该定位结果(X'Y'Z')计算到剩余节点的距离(D'₁D'₂...D'_n-3)，算出剩余节点计算距离与测量距离(D₁D₂...D_n-3)的误差的平方取该误差之和的平局值作为距离阈值t²；若计算距离与测量距离的误差的平方E²小于t²，则认为该节点为内点，否则为外点，并统计对用内点的个数n₁。

SS63：再从定位节点1到n中任意取三组数据，重复步骤SS61和SS62，求出内点的个数n₂和对应的其它参数。

SS64：重复10次步骤SS61和SS62，得到10组数据，取内点数n最大的三组节点作为数据正确的定位节点；若出现内点两组相同，则取最大误差平方较小的那组数据作为正确的定位节点，若最大误差平方相同则比较第二大误差平方直至找出需要的定位节点数据，该组节点数据求出的代数中心命名为X_rY_rZ_r。

SS65：利用计算出的代数中心到剩余节点的距离来剔除错误节点，因测距传感器的精度为±10cm，误差结果超过这个范围的定位节点数据认为是错的。

SS66：得到代数中心后，利用代数中心数据的摄动，使代数中心与真实定位坐标的误差减小，得到优化的定位值，该摄动优化步骤如下：

SS661:根据RANSAC算法得到代数中心点R(X_rY_rZ_r)，利用剩余的i个正确节点数据进行优化，根据空间距离公式可得其余正确节点到代数中心的误差为

SS662：求出e_i对t的导数：

令i＝1…i，列出除去野点节点外所有其它定位节点误差导数方程，令

得到公式

SS663：构造Lyapunov函数则求导得：选择控制律则有由此可知算法收敛于平衡点，对控制率离散化得到：因此按照设计的离散控制律迭代十次，得到精度更高的几何中心点。