基于UWB、光流和惯性导航的高精度室内三维定位方法技术

本发明专利技术涉及室内定位技术领域，特别涉及一种基于UWB、光流和惯性导航的高精度室内三维定位方法，包括对加速度计和陀螺仪进行误差补偿，完成初始对准；进入导航阶段，将加速度计和陀螺仪实时测量的参数进行导航解算，获得载体的实时速度、位置、以及姿态信息并与超宽带UWB定位信息、光流传感器的速度信息构建量测矩阵；构建载体的室内三维定位的状态方程，根据卡尔曼滤波算法以及不同时刻的量测矩阵，求解状态方程获得不同时刻的状态矩阵的估计值，该值即为载体的室内三维定位信息；本发明专利技术能实现全自主室内导航，比传蓝牙信标室内定位所需布设基站可以大幅度减少，比单纯UWB室内定位能大幅度减少基站布设以及定位更连续。

High precision Indoor 3D positioning method based on UWB, optical flow and inertial navigation

【技术实现步骤摘要】
基于UWB、光流和惯性导航的高精度室内三维定位方法
本专利技术涉及室内定位
，特别涉及一种基于UWB、光流和惯性导航的高精度室内三维定位方法。
技术介绍
目前的室内定位技术方面的技术，有基于UWB(Ultra-Wideband，UWB)超宽带，iBeacon蓝牙信标，RFID(RadioFrequencyIdentification)射频识别，WIFI无线网络等技术的室内定位技术比较成熟。目前主流的室内定位技术，均需要大量的在室内布置信标，这个工作量很大，且硬件成本和施工成本都比较高，在无法接收信号的地方无法实现室内有效定位。由于UWB信号的高时间分辨率，TOA(TimeofArriva,到达时间定位)和TDOA(TimeDifferenceofArriva,到达时间差定位)相对于其他算法具有更高的准确度。对目前UWB定位来说最有效的解决方案是采用TOA与TDOA的混合定位算法，因为结合了两种算法的优点。TOA技术是指由基站向移动站发出特定的测距命令或指令信号，并要求终端对该指令进行响应。基站会纪录下由发出测距指令到收到终端确认信号所花费的时间，该时间主要由射频信号在环路上的传播时延、终端的响应时延和处理时延、基站的处理时延组成。如果能够准确地得到终端和基站的响应和处理时延，就可以算出射频信号的环路传播时延。因为无线电波在空气中以光速传播，所以基站与终端之间的距离可以估算出来。当有三个基站参与测量时，就可以根据三角定位法来确定终端所在的区域。TDOA定位算法是一种利用时间差进行定位的方法，通过测量信号达到基站的时间，可以确定信号源的距离，利用信号源到多个无线电监测站的距离(以无线电基站为中心，距离为半径作圆)，就能确定信号的位置。通过比较信号到达多个基站的时间差，就能做出以检测站为焦点、距离差为长轴的双曲线的交点，该交点即为信号的位置。光流传感器通过图像采集系统以一定速率连续采集物体表面图像，再由DSP对所产生的图像数字矩阵进行分析。由于相邻的两幅图像总会存在相同的特征通过对比这些特征点的位置变化信息，便可以判断出物体表面特征的平均运动，这个分析结果最终被转换为二维的坐标偏移量，并以像素数形式存储在特定的寄存器中，实现对运动物体的检测。
技术实现思路
为了实现全自主室内导航，本专利技术提出一种基于UWB、光流和惯性导航的高精度室内三维定位方法，载体设置有惯性导航仪、惯性传感器、光流传感器、磁传感器以及超宽带定位，使用惯性导航仪融合光流传感器、磁传感器、超宽带定位等信息来测量载体的速度、位置、以及姿态信息；具体的室内三维定位过程包括：惯性传感器包括加速度计和陀螺仪，对加速度计和陀螺仪进行误差补偿，完成初始对准；进入导航阶段，将加速度计和陀螺仪实时测量的参数进行导航解算，获得载体的实时速度、位置、以及姿态信息；将载体的实时速度、位置、以及姿态信息与超宽带UWB定位信息、光流传感器的速度信息构建矩阵；构建载体的室内三维定位的状态方程，根据卡尔曼滤波算法以及不同时刻的量测矩阵，求解状态方程获得不同时刻的状态矩阵的估计值，该值即为载体的室内三维定位信息。进后验的，量测向量表示为：其中，Z为量测向量；VGE为光流传感器测得的东向速度；VIE为惯性传感器测得的东向速度；VGN为光流传感器测得的北向速度；VIN为惯性传感器测得的北向速度；LG为UWB定位得到的纬度值；LI为惯性传感器计算得到的纬度值；λG为UWB定位得到的经度值；λI为惯性传感器计算得到的经度值；hA为UWB定位得到的高度值；hI为惯性传感器计算得到的高度值；ψG为磁传感器计算得到的航向角；ψI为惯性传感器计算得到的航向角。进后验的，载体的室内三维定位的状态方程表示为：其中，Xk+1表示第k+1时刻的状态矩阵；Φk+1,k表示状态向量均方差矩阵；Wk表示第k时刻的状态矩阵噪声向量；Zk+1表示第k+1时刻的量测向量；Hk+1表示第k+1时刻的量测矩阵；Vk+1表示第k+1时刻的量测矩阵噪声向量。进后验的，状态矩阵X表示为：其中，δVE为东向速度误差，为惯性传感器测得的东向速度与光流传感器测得的东向速度的差值；δVN北向速度误差，为惯性传感器测得的北向速度与光流传感器测得的北向速度的差值；δVU为天向速度误差，为惯性传感器测得的天向速度与光流传感器测得的天向速度的差值；δL为纬度误差，为惯性导航仪测量的纬度值与UWB定位得到的纬度值的差；δλ为经度误差，为惯性导航仪测量的经度值与UWB定位得到的经度值的差；δh为高度误差，为惯性导航仪测量的高度值与UWB定位得到的高度值的差；为东向失准角误差，为惯性导航仪测得的东向失准角与陀螺仪测量并进行补偿之后的东向失准角的差值；为北向失准角误差，为惯性导航仪测得的北向失准角与陀螺仪测量并进行补偿之后的北向失准角的差值；为天向失准角误差，为惯性导航仪测得的天向失准角与陀螺仪测量并进行补偿之后的天向失准角的差值；εx为x轴陀螺零漂；εy为y轴陀螺零漂；εz为z轴陀螺零漂；为x轴加速度计零漂；为y轴加速度计零漂；为z轴加速度计零漂。进后验的，根据卡尔曼滤波算法以及不同时刻的量测矩阵，求解状态方程获得不同时刻的状态矩阵的估计值的过程包括：根据第k时刻的状态矩阵预测第k+1时刻的状态矩阵，获得第k+1时刻的后验预测状态矩阵：更新第k+1时刻的后验预测状态矩阵的状态，获得第k+1时刻的预测状态矩阵：第k+1时刻的卡尔曼滤波增益Kk+1为：根据第k时刻的均方误差预测第k+1时刻的均方误差，获得第k+1时刻的后验预测均方误差：更新第k+1时刻的后验预测均方误差的状态，获得第k+1时刻的预测均方误差：其中，为第k+1时刻的后验预测状态向量；为第k时刻的状态向量；Φk+1,k为状态向量的均方差矩阵；Zk+1为第k+1时刻的量测向量；Hk+1为第k+1时刻的量测矩阵；Pk+1/k为第k+1时刻的均方误差矩阵；Pk为第k时刻的均方误差矩阵；Qk为状态参数的噪声矩阵；I为单位阵；Rk+1为第k+1时刻的观测噪声矩阵；上标T表示矩阵的转置；上标-1表示矩阵的逆矩阵。本专利技术的有益效果在于：1.本专利技术提出的基于UWB、光流和惯性导航的高精度室内三维定位，不依赖于外部信号，能实现全自主室内导航，比传蓝牙信标室内定位所需布设基站可以大幅度减少，比单纯UWB室内定位能大幅度减少基站布设以及定位更连续；2.本专利技术提出的基于UWB、光流和惯性导航的高精度室内三维定位方法与传统的是室内方法相比较，具有更高的定位精度。附图说明图1为本专利技术根据定位基站根据到达时间定位TOA算法确认定位位置的经纬度以及高度的示意图；图2为本专利技术根据定位基站根据到达时间定位TOA算法确认定位位置的经纬度以及高度的示意图；图3为本专利技术基于UWB、光流和惯性导航的高精度室内三维定位方法的原理示意图。具体实施方式...

【技术保护点】
1.基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，载体设置有惯性导航仪、惯性传感器、光流传感器、磁传感器以及超宽带定位，使用惯性导航仪融合光流传感器、磁传感器、超宽带定位的信息来测量载体的速度、位置、以及姿态信息；具体的室内三维定位过程包括：/n惯性传感器包括加速度计和陀螺仪，对加速度计和陀螺仪进行误差补偿，完成初始对准；/n进入导航阶段，将加速度计和陀螺仪实时测量的参数进行导航解算，获得载体的实时速度、位置、以及姿态信息；/n将载体的实时速度、位置、以及姿态信息与超宽带UWB定位信息、光流传感器的速度信息构建量测矩阵；/n构建载体的室内三维定位的状态方程，根据卡尔曼滤波算法以及不同时刻的量测矩阵，求解状态方程获得不同时刻的状态矩阵的估计值，该值即为载体的室内三维定位信息。/n

【技术特征摘要】
1.基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，载体设置有惯性导航仪、惯性传感器、光流传感器、磁传感器以及超宽带定位，使用惯性导航仪融合光流传感器、磁传感器、超宽带定位的信息来测量载体的速度、位置、以及姿态信息；具体的室内三维定位过程包括：
惯性传感器包括加速度计和陀螺仪，对加速度计和陀螺仪进行误差补偿，完成初始对准；
进入导航阶段，将加速度计和陀螺仪实时测量的参数进行导航解算，获得载体的实时速度、位置、以及姿态信息；
将载体的实时速度、位置、以及姿态信息与超宽带UWB定位信息、光流传感器的速度信息构建量测矩阵；
构建载体的室内三维定位的状态方程，根据卡尔曼滤波算法以及不同时刻的量测矩阵，求解状态方程获得不同时刻的状态矩阵的估计值，该值即为载体的室内三维定位信息。


2.根据权利要求1所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，量测向量表示为：



其中，Z为量测向量；VGE为光流传感器测得的东向速度；VIE为惯性传感器测得的东向速度；VGN为光流传感器测得的北向速度；VIN为惯性传感器测得的北向速度；LG为UWB定位得到的纬度值；LI为惯性传感器计算得到的纬度值；λG为UWB定位得到的经度值；λI为惯性传感器计算得到的经度值；hA为UWB定位得到的高度值；hI为惯性传感器计算得到的高度值；ψG为磁传感器计算得到的航向角；ψI为惯性传感器计算得到的航向角。


3.根据权利要求2所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，定位基站根据到达时间定位TOA算法确认定位位置的经纬度以及高度包括：已知3个定位基站R1、R2以及R3R4的坐标，分别为：R1(x1,y1,z1)、R2(x2,y2,z2)以及R3(x3,y3,z3)，若脉冲信号从定位位置O到R1、R2以及R3这3个基站的时间分别为为t1、t2、t3，根据脉冲信号的传播速度以及脉冲信号从定位位置O到R1、R2以及R3的时间计算出3个定位基站与定位位置O的相对距离，每个基站以相对距离为半径画一个球形轨迹，利用三个球形方程能够计算出唯一的交点，该交点为定位位置O的坐标，计算过程包括：



其中，v为脉冲信号传播的速度。


4.根据权利要求2所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，定位基站根据到达时间定位TOA算法确认定位位置的经纬度以及高度包括：已知4个定位基站R1、R2、R3以及R4的坐标，分别为：R1(x1,y1,z1)、R2(x2,y2,z2)、R3(x3,y3,z3)以及R4(x4,y4,z4)，若脉冲信号从定位位置O到R1、R2、R3以及R4四个基站的时间分别为为t1、t2、t3以及t4，分别以(R1、R4)，(R2、R4)，(R3、R4)做为焦点定位位置O，发送的信号到两基站间的距离差为常数，可以得到3组双曲线，双曲线的交点即是定位标签O(x0,y0,z0)的坐标，根据其标签O(x0,y0,z0)的坐标确定定位位置O的经纬度以及高度，求解定位标签O的坐标的方程表示为：



其中，v为脉冲信号传播的速度。


5.根据权利要求2所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，光流传感器测得的东向速度以及北向速度的过程包括：由图像平面三维速度场的投影建立运动场，将相机参考帧中一个点P的像素坐标的投影在原点，获得投影p，将点P处的速度与图像平面中p的速...

【专利技术属性】
技术研发人员：李宝林，李星海，王浩，徐江，王梅，
申请(专利权)人：中电科技集团重庆声光电有限公司，
类型：发明
国别省市：重庆;50