一种基于UWB测距的惯导原点定位方法及系统与流程

本发明涉及UWB定位与惯导定位
技术领域：
，尤其涉及一种基于UWB测距的惯导原点定位方法及系统。
背景技术：
：随着现代社会日新月异的发展，在各个领域内，需要被精确定位的目标随之越来越多，因此人们对定位技术也提出了更高层次的要求，尤其是对动态移动目标的精确定位，逐渐成为了当代人们关注的关键问题。然而在现代社会某些领域中，一些我们常用的动态定位方式已经无法满足当前日益增强的精确定位的需求，在这种背景下，超宽带无线通讯UWB(UltraWideband，超宽带)定位系统应运而生，使得对动态目标的精确定位成为可能。UWB是一种全新的、与传统通信技术有极大差异的通信新技术。它不需要使用传统通信体制中的载波，而是通过发送和接收具有纳秒或纳秒级以下的极窄脉冲来传输数据，从而具有GHz量级的带宽。UWB可用于室内精确定位，例如战场士兵的位置发现、机器人运动跟踪等，UWB系统与传统的窄带系统相比，具有穿透力强、功耗低、抗多径效果好、安全性高、系统复杂度低、能提供精确定位精度等优点。惯导系统(InertialNavigationSystem，INS)是以陀螺和加速度计为敏感器件的导航参数解算系统，该系统根据陀螺的输出建立导航坐标系，根据加速度计输出解算出运载体在导航坐标系中的速度和位置，属于一种推算导航方式。其中捷联式惯导系统(Strap-downInertialNavigationSystem，SINS)是将加速度计和陀螺仪直接安装在载体上，然后根据导航参数进行相关导航计算，由于其具有可靠性高、功能强、重量轻、成本低、精度高以及使用灵活等优点，使得SINS已经成为当今惯导系统发展的主流。现有技术中有独立使用惯导定位的系统和惯导协同UWB共同实现定位的系统。在独立使用惯导定位应用中，惯导初始化时所选原点与方向尽可能与实际环境相匹配，多个惯导同时工作时的初始化原点和方向必须相同，确保指挥中心能准确的掌握多个惯导的位置，做出正确判断和合理的指挥。在协同UWB技术的定位应用中，在移动目标上安装捷联惯导模块、超宽带无线接收器和协同定位模块，在定位区域内布置超宽带无线发射器，利用捷联惯导模块和超宽带无线传感器对移动目标的参数进行感知，建立捷联惯导和超宽带无线耦合模型，由捷联惯导模块输出移动目标高精度姿态信息，在捷联惯导模块输出的运动参数补偿下，超宽带无线传感器输出移动目标高精度的位置信息，并对捷联惯导模块输出的位置信息进行校正，从而实现封闭环境下定位区域内移动目标输出位置和姿态信息。但是，现有惯导协同UWB协同定位技术，主要是解决在没有地面或空中常用的GPS校准的封闭空间中，惯导传感器漂移和累计误差造成的长时间后位置信息严重失信的问题，没有涉及惯导的初始化原点及方向问题，且必须事先在封闭空间中安装多个位置已知且固定的无线传感器及其它配套设施，无法适应室外的情况，灵活性及移动性差。因此，对惯导协同UWB定位中的惯导原点的初始化问题进行研究具有现实的意义。技术实现要素：本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种可任意选择惯导初始化状态，惯导初始化灵活、方便，在有多个惯导设备时，不需要保持多个惯导设备初始化时的一致性，可方便的适用于室内、室外各种环境，简单实用的基于UWB测距的惯导原点定位方法及系统。为解决上述技术问题，本发明提出的技术方案为：一种基于UWB测距的惯导原点定位方法，通过UWB定位用户节点和惯导定位设备对同一目标进行定位，包括如下步骤：S1.布设UWB定位锚节点，并确定UWB定位坐标系和UWB定位锚节点的坐标，设置惯导定位设备，确定惯导定位坐标系；S2.获取UWB定位用户节点的定位数据和惯导定位设备的定位数据；S3.通过所述UWB定位用户节点的定位数据和惯导定位设备的定位数据解算惯导定位坐标系的原点参数。作为本发明的进一步改进，所述步骤S1中UWB定位锚节点的个数至少为2个；所述UWB定位锚节点优选布置在同一水平面上；确定所述UWB定位锚节点在UWB定位坐标系中的坐标；所述惯导定位坐标系为所述惯导设备初始化时所确定的坐标系。作为本发明的进一步改进，所述步骤S2中所述UWB定位用户节点的定位数据包括UWB定位用户节点的UWB定位坐标；所述UWB定位坐标至少包括2组，包括第一定位时刻获取的UWB定位坐标和第二定位时刻获取的UWB定位坐标；所述惯导定位设备的定位数据包括惯导定位设备的惯导定位坐标；所述惯导定位坐标至少包括2组，包括第一定位时刻获取的惯导定位坐标和第二定位时刻获取的惯导定位坐标。作为本发明的进一步改进，所述UWB定位坐标由UWB定位用户节点通过TOF测距得到所述UWB定位用户节点与所述UWB定位锚节点之间的距离，并根据所述距离与所述UWB定位锚节点的坐标计算得到。作为本发明的进一步改进，所述步骤S3的具体步骤包括：S3.1.通过所述第一定位时刻的UWB定位坐标、第二定位时刻的UWB定位坐标、第一定位时刻的惯导定位坐标、第二定位时刻的惯导定位坐标计算所述UWB定位坐标系与所述惯导定位坐标系之间的夹角；S3.2.根据所述UWB定位坐标系与所述惯导定位坐标系之间的夹角、UWB定位坐标和惯导定位坐标计算所述惯导定位坐标系的原点相对于所述UWB定位坐标系的原点之间的相对平移，从而确定所述惯导定位坐标系的原点。一种基于UWB测距的惯导原点定位系统，包括UWB定位模块、惯导定位模块、融合控制模块和中心处理模块；所述UWB定位模块包括UWB定位锚节点和UWB定位用户节点；所述UWB定位锚节点用于确定UWB定位坐标系；所述UWB定位用户节点用于获取UWB定位信息，并发送至所述中心处理模块；所述惯导定位模块用于获取惯导定位信息，并发送至所述中心处理模块；所述融合控制模块用于控制所述UWB定位用户节点和所述惯导定位模块获取同一定位时刻的UWB定位信息和惯导定位信息；所述中心处理模块用于设置UWB定位坐标系信息及UWB定位锚节点的坐标，并根据所述UWB定位锚节点的坐标、所述UWB定位信息和所述惯导定位信息解算获得所述惯导定位模块的原点。作为本发明的进一步改进，所述UWB定位模块包括至少2个UWB定位锚节点；所述UWB定位锚节点优选布置在同一水平面上；所述2个UWB定位锚节点优选设置在具有已知长度的支架上。作为本发明的进一步改进，所述融合控制模块还用于对所述UWB定位信息和惯导定位信息进行融合处理，所述UWB定位用户节点将所述UWB定位信息发送至所述融合控制模块，所述惯导定位模块将所述惯导定位信息发送至所述融合控制模块，所述融合控制模块对所述UWB定位信息和所述惯导定位信息进行融合打包后发送至所述中心处理模块。作为本发明的进一步改进，所述中心处理模块还包括数据存储单元，用于存储所接收到的UWB定位用户节点的UWB定位信息，以及惯导定位设备的惯导定位信息。作为本发明的进一步改进，所述UWB定位用户节点、惯导定位模块、融合控制模块、中心处理模块之间通过无线通信方式进行数据通信交互，所述无线方式包括蓝牙、Wifi、移动通信等方式。与现有技术相比，本发明的优点在于：1、本发明在惯导设备初始化时，可任意选择惯导设备的原点位置和方向，惯导设备初始化难度低，系统灵活度高；当同时包含有多个惯导设备时，无需要保持多个惯导设备在初始化时的一致性，从而避免初始化偏差，消除因突发状况对整个定位系统产生的不利影响，降低系统的复杂度。2、本发明的UWB定位锚节点可灵活布设，可移动、操作简单，可方便的适用于室内、室外各种环境，从而快速建立UWB定位与惯导定位协同的定位系统。附图说明图1为本发明具体实施例流程示意图。图2为本发明具体实施例结构示意图。图3为本发明具体实施例布设情况府视示意图。具体实施方式以下结合说明书附图和具体优选的实施例对本发明作进一步描述，但并不因此而限制本发明的保护范围。如图1所示，本实施例基于UWB测距的惯导原点定位方法，通过UWB定位用户节点和惯导定位设备对同一目标进行定位，包括如下步骤：S1.布设UWB定位锚节点，并确定UWB定位坐标系和UWB定位锚节点的坐标，设置惯导定位设备，确定惯导定位坐标系；S2.获取UWB定位用户节点的定位数据和惯导定位设备的定位数据；S3.通过UWB定位用户节点的定位数据和惯导定位设备的定位数据解算惯导定位坐标系的原点参数。在本实施例中，步骤S1中UWB定位锚节点的个数至少为2个；UWB定位锚节点优选布置在同一水平面上；确定UWB定位锚节点在UWB定位坐标系中的坐标；惯导定位坐标系为惯导设备初始化时所确定的坐标系。如图3所示，在本实施例中，包括2个UWB定位锚节点M1和M2，UWB定位锚节点M1和M2布置在同一水平面上，以M1与M2的中点为UWB定位坐标系的原点O1，以M1与M2的连接线所在的水平线为X轴，以垂直与X轴的水平线为Y轴，X轴与Y轴所在的平面为水平平面，与地面平行。设本实施例中M1与M2之间的距离为2米，则可设定UWB定位锚节点M1的坐标为(-1，0)，UWB定位锚节点M2的坐标为(1，0)。当然，当具有多个UWB定位锚节点时，可以选择以UWB定位锚节点的几何中心点为UWB定位坐标系的原点。在本实施例中，UWB定位用户节点和惯导定位设备将加载在同一载体上，如图3中A，惯导定位坐标系为惯导定位设备初始化时所确定的坐标系，在本实施例中，该坐标系即载体坐标系，该载体坐标系以初始化时载体的质心为载体坐标系的原点，载体坐标系的X轴为载体的水平右侧，Y轴为与X轴垂直，水平向前，Z轴竖直向上，其中，载体坐标系中的X轴与Y轴所在的平面为与地面平等的水平平面。在本实施例中，惯导定位坐标系的X轴方向如图3中所示的X′，Y轴方向如图3中所示的Y′。当然，UWB定位坐标系和惯导定位坐标系并不仅限于本实施例中所记载的方法，可根据实际需要灵活设置。在本实施例中，步骤S2中UWB定位用户节点的定位数据包括UWB定位用户节点的UWB定位坐标；UWB定位坐标至少包括2组，包括第一定位时刻获取的UWB定位坐标和第二定位时刻获取的UWB定位坐标；惯导定位设备的定位数据包括惯导定位设备的惯导定位坐标；惯导定位坐标至少包括2组，包括第一定位时刻获取的惯导定位坐标和第二定位时刻获取的惯导定位坐标。UWB定位坐标由UWB定位用户节点通过TOF测距得到UWB定位用户节点与UWB定位锚节点之间的距离，并根据距离与UWB定位锚节点的坐标计算得到。在本实施例中，通过式(1)所示公式即可计算UWB定位用户节点在UWB定位坐标系中的UWB定位坐标，(xT-x1)2+(yT-y1)2=d1(xT-x2)2+(yT-y2)2=d2---(1)]]>式(1)中，(xT,yT)为UWB定位坐标，(x1,y1)为UWB定位锚节点M1的坐标，(x2,y2)为UWB定位锚节点M1的坐标，d1为UWB定位用户节点与UWB定位锚节点M1之间的距离，d2为UWB定位用户节点与所述第二UWB定位锚节点M2之间的距离。TOF是基于信号飞行时间的测距算法，在本实施例中，采用两次双边测距求信号飞行时间平均值的方式进行距离计算，用于减少由于UWB定位用户节点和UWB定位锚节点的晶振不同引起的误差。在本实施例中，步骤S3的具体步骤包括：S3.1.通过第一定位时刻的UWB定位坐标、第二定位时刻的UWB定位坐标、第一定位时刻的惯导定位坐标、第二定位时刻的惯导定位坐标计算UWB定位坐标系与惯导定位坐标系之间的夹角；S3.2.根据UWB定位坐标系与惯导定位坐标系之间的夹角、UWB定位坐标和惯导定位坐标计算惯导定位坐标系的原点相对于UWB定位坐标系的原点之间的相对平移，从而确定惯导定位坐标系的原点。如图3所示，在本实施例中，根据式(1)所示公式，可计算得到在第一定位时刻的UWB定位坐标A(xT1,yT1)和在第二定位时刻的UWB定位坐标A′(xT2,yT2)。同时，通过惯导定位设备可获得在第一定位时刻的惯导定位坐标A(xgd1,ygd1)和在第二定位时刻的惯导定位坐标A′(xgd2,ygd2)。设惯导定位坐标系的原点相对于UWB定位坐标系的原点的相对平移位移为(Δx,Δy)，Δx为X轴平移量，Δy为Y轴平移量。则在第一定位时刻UWB定位坐标与惯导定位坐标之间的关系可通过式(2)所示公式表示，xT1=Δx+ygd1sinθ+xgd1cosθyT1=Δy+ygd1cosθ-xgd1sinθ---(2)]]>式(2)中，θ为所述UWB定位坐标系与惯导定位坐标系之间的夹角，(xT1,yT1)为第一定位时刻的UWB定位坐标，(xgd1,ygd1)为第一定位时刻的惯导定位坐标，(Δx,Δy)为惯导定位坐标系的原点相对于UWB定位坐标系的原点的相对平移位移。在第二定位时刻UWB定位坐标与惯导定位坐标之间的关系可通过式(3)所示公式表示，xT2=Δx+ygd2sinθ+xgd2cosθyT2=Δy+ygd2cosθ-xgd2sinθ---(3)]]>式(3)中，θ为所述UWB定位坐标系与惯导定位坐标系之间的夹角，(xT2,yT2)为第二定位时刻的UWB定位坐标，(xgd2,ygd2)为第二定位时刻的惯导定位坐标，(Δx,Δy)为惯导定位坐标系的原点相对于UWB定位坐标系的原点的相对平移位移。根据式(2)和式(3)可以得到式(4)，(xT1-xT2)=(ygd1-ygd2)sinθ+(xgd1-xgd2)cosθ(yT1-yT2)=(ygd1-ygd2)cosθ-(xgd1-xgd2)sinθ---(4)]]>式(4)中，θ为所述UWB定位坐标系与惯导定位坐标系之间的夹角，(xT1,yT1)为第一定位时刻的UWB定位坐标，(xgd1,ygd1)为第一定位时刻的惯导定位坐标，(xT2,yT2)为第二定位时刻的UWB定位坐标，(xgd2,ygd2)为第二定位时刻的惯导定位坐标。由式(4)可得式(5)，cosθ=(xT1-xT2ygd1-ygd2+yT1-yT2xgd1-xgd2)/(ygd1-ygd2xgd1-xgd2+xgd1-xgd2ygd1-ygd2)---(5)]]>式(5)中对各参数的定义与式(4)中相同。通过式(5)则可以解算得出UWB定位坐标系与惯导定位坐标系之间的夹角θ的值。再将UWB定位坐标系与惯导定位坐标系之间的夹角θ的值代入到式(2)或者式(3)中，即可计算得到惯导定位坐标系的原点相对于UWB定位坐标系的原点的相对平移位移中Δx和Δy的值，从而即可以确定惯导定位坐标系的原点。如图2所示，本实施例基于UWB测距的惯导原点定位系统，包括UWB定位模块、惯导定位模块、融合控制模块和中心处理模块；UWB定位模块包括UWB定位锚节点和UWB定位用户节点；UWB定位锚节点用于确定UWB定位坐标系；UWB定位用户节点用于获取UWB定位信息，并发送至中心处理模块；惯导定位模块用于获取惯导定位信息，并发送至中心处理模块；融合控制模块用于控制UWB定位用户节点和惯导定位模块获取同一定位时刻的UWB定位信息和惯导定位信息；中心处理模块用于设置UWB定位坐标系信息及UWB定位锚节点的坐标，并根据UWB定位锚节点的坐标、UWB定位信息和惯导定位信息解算获得惯导定位模块的原点。在本实施例中，如图3所示，UWB定位模块包括至少2个UWB定位锚节点；UWB定位锚节点优选布置在同一水平面上；2个UWB定位锚节点优选设置在具有已知长度的支架上。在本实施例中，UWB定位锚节点可以根据需要进行布置，为了更快的确定UWB定位坐标系的原点，以及UWB定位锚节点的坐标，优选将UWB定位锚节点布置在预设设计好的具有已知长度的支架上。本实施例所选用的支架长度为2米，将2个UWB定位锚节点分别布置在支架的两端，在布设时，只需要将该支架按照需要布设的方位水平布设，根据需要设定支架的中心UWB定位坐标系的原点，那么2个UWB定位锚节点的坐标分别为(-1，0)和(1，0)。当然，为了更方便的实现水平布设，还可以在支架上设置一个水平指示装置。同时，该支架可设置为可升缩或可折叠的支架，以更方便的携带。或者，在支架上设置距离刻度表，在UWB定位锚节点的安装时，可根据需要方便的设置UWB定位锚节点之间的距离。在UWB定位锚节点布设完成后，将UWB定位锚节点的坐标信息输入至中心处理模块，确定UWB定位坐标系。在本实施例中，融合控制模块还用于对UWB定位信息和惯导定位信息进行融合处理，UWB定位用户节点将UWB定位信息发送至融合控制模块，惯导定位模块将惯导定位信息发送至融合控制模块，融合控制模块对UWB定位信息和惯导定位信息进行融合打包后发送至中心处理模块。中心处理模块还包括数据存储单元，用于存储所接收到的UWB定位用户节点的UWB定位信息，以及惯导定位设备的惯导定位信息。UWB定位用户节点、惯导定位模块、融合控制模块、中心处理模块之间通过无线通信方式进行数据通信交互，无线方式包括蓝牙、Wifi、移动通信等方式。移动通信方式包括3G、4G等移动通信。在本实施例中，如图3所示，M1和M2是布设在支架上的2个UWB定位锚节点，A为UWB定位用户节点和惯导定位设备的载体，即被定位的人或物，其中A为载体在第一定位时刻时的位置，A’为载体在第二定位时刻时的位置。通过融合控制模块，分别在第一定位时刻获取载体的UWB定位坐标(xT1,yT1)和惯导定位坐标(xgd1,ygd1)，在第二定位时刻获取载体的UWB定位坐标(xT2,yT2)和惯导定位坐标(xgd2,ygd2)。其中，UWB定位坐标是UWB定位用户节点接收UWB定位锚节点发送的UWB信号，采用TOF算法测距由式(1)所示公式计算得到的UWB定位坐标，在第一定位时刻UWB定位用户节点距两个UWB定位锚节点的距离分别为d1和d2，在第二定位时刻UWB定位用户节点距两个UWB定位锚节点的距离分别为d1′和d2′。UWB定位用户节点将UWB定位坐标发送至融合控制模块，惯导定位设备将惯导定位坐标发送至融合控制模块，由融合控制模块进行融合打包后，发送至中心处理模块。当然，也可以采用其它的方式，如由UWB定位用户节点直接将UWB定位坐标发送至中心处理模块，惯导定位设备直接将惯导定位坐标发送至中心处理模块来实现定位坐标数据的发送。中心处理模块接收获得UWB定位坐标和惯导定位坐标，并根据所输入的UWB定位坐标系和UWB定位锚节点坐标，通过式(2)或式(3)，以及式(5)所示的公式，计算惯导定位坐标系的原点相对于UWB定位坐标系的原点的相对平移位移，从而确定惯导定位坐标系的原点。同时，中心处理模块还可以将所接收到的数据以及计算得到的数据存储至数据存储单元。在本实施例中，融合控制模块可以是智能手机，中心处理模块可以是智能手机、平板电脑、笔记本电脑等。上述只是本发明的较佳实施例，并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明。因此，凡是未脱离本发明技术方案的内容，依据本发明技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均应落在本发明技术方案保护的范围内。当前第1页1 2 3