申请/专利权人：江苏领创星通卫星通信科技有限公司

申请日：2023-11-24

公开（公告）日：2024-07-02

公开（公告）号：CN117289322B

主分类号：G01S19/48

分类号：G01S19/48;G01S19/49;G01S19/39;H04W4/02;H04W64/00

优先权：

专利状态码：有效-授权

法律状态：2024.07.02#授权;2024.01.12#实质审查的生效;2023.12.26#公开

摘要：本发明公开了一种基于IMU、GPS与UWB的高精度定位算法，包括确定状态向量、惯导的姿态误差微分方程建模、惯导的速度误差微分方程建模、惯导的位置误差微分方程建模、传感器误差建模、惯导的系统状态误差微分方程建模、GPS速度误差观测方程建模、GPS位置误差观测方程建模、UWB位置误差观测方程建模和双分支卡尔曼循环定位；本发明属于勘测、导航、陀螺仪技术领域，具体为一种基于IMU、GPS与UWB的高精度定位算法，本发明的优点为：本发明结合IMU的高频惯导数据，使用双分支卡尔曼循环定位预测载体位置，使整个算法更加鲁棒和精确，并且本发明使用的传感器价格低廉，是一种低成本的定位方案。

主权项：1.一种基于IMU、GPS与UWB的高精度定位算法，包括确定状态向量、惯导的姿态误差微分方程建模、惯导的速度误差微分方程建模、惯导的位置误差微分方程建模、传感器误差建模、惯导的系统状态误差微分方程建模、GPS速度误差观测方程建模、GPS位置误差观测方程建模、UWB位置误差观测方程建模和双分支卡尔曼循环定位；所述确定状态向量，就是确定卡尔曼滤波方程中的状态向量，包括导航状态误差向量和传感器： ，上式中，δx表示状态误差，δrn表示惯导位置误差向量在n系下的投影，n系为当地水平坐标系，δvn表示惯导速度误差向量在n系下的投影，等号右边第三项表示惯导姿态误差向量，bg表示三轴陀螺仪零偏向量，ba表示三轴加速度计零偏向量，sg表示三轴陀螺仪比例因子误差向量，sa表示三轴加速度计比例因子误差向量，等号右边每一项都是三维的列向量，因此δx共有21维；所述惯导的姿态误差微分方程建模为： ，上式中，表示惯导的姿态误差的导数，表示惯导的姿态误差，上标n表示各物理量在n系下的投影；上标b表示各物理量在b系下的投影；ωin表示n系相对于i系的角速度；δωin表示n系相对于i系的角速度误差；δωib表示b系相对于i系的角速度误差；表示惯导所在的b系相对于n系的方向余弦矩阵；所述惯导的速度误差微分方程建模为： ，上式中，等号左边表示当地水平坐标系下的速度向量的一阶导数，表示加速度计测得的比力误差在b系的投影；fn表示加速度计测得的比力在n系的投影；ωie表示e系相对于i系的角速度；δωie表示e系相对于i系的角速度误差；ωen表示n系相对于e系的角速度；δωen表示n系相对于e系的角速度误差；υn表示当地水平坐标系下的速度向量；δυn表示当地水平坐标系下的速度误差向量；δgp的表示当地重力误差；当地重力的展开式为： ，上式中，g表示重力矢量；ωie表示e系相对于i系的角速度向量；r表示载体的位置矢量；从而有： ，上式中，上标n表示在n系的投影；所述双分支卡尔曼循环定位由误差状态外推方程、误差状态协方差外推方程、卡尔曼增益方程、GPS误差状态更新方程、UWB误差状态更新方程和误差状态协方差更新方程组成；以上方程均由误差状态外推方程、GPS观测方程和UWB位置误差观测方程推导得出，融合IMU、GPS和UWB多源数据，以IMU作为推导误差状态方程的主要传感器，将GPS的观测数据和UWB的观测数据作为推导观测方程的传感器；当GPS观测数据超过给定误差阈值或者GPS观测数据落在UWB信号覆盖范围时，使用UWB数据作为观测数据；所述传感器误差建模如下：第一步，加速度计误差建模： ，上面1式中，等号左边表示加速度计实际的比力测量输出；表示加速度计的理论输出；I为单位向量；Sa为加速度计的比例因子误差，ba为加速度计零偏；ωa表示加速度计测量值的白噪声；在2式中，表示加速度计测量误差；第二步，陀螺仪误差建模： ，式3中，等号左边表示陀螺实际的角速度测量输出，ωibb为b系下的理论角速度；Sg为陀螺仪的比例因子误差，bg为陀螺零偏；wg表示陀螺仪测量值的白噪声；δωibb表示陀螺仪测量误差；加速度计零偏、陀螺零偏、加速度计的比例因子误差和陀螺仪的比例因子误差建模为一阶高斯马尔科夫过程，有： ，上式中，Tgb、Tab、Tgs和Tas分别表示陀螺零偏、加速度计零偏、陀螺比例因子和加速度计比例因子的相关时间；ωgb、ωab、ωgs和ωas分别表示陀螺零偏驱动白噪声、加速度计零偏驱动白噪声、陀螺比例因子驱动白噪声和加速度计比例因子驱动白噪声；所述惯导的系统状态误差微分方程建模，具体步骤为：首先，结合确定状态向量、惯导的姿态误差微分方程建模、惯导的速度误差微分方程建模、惯导的位置误差微分方程建模和传感器误差建模的结果，构建出惯导的系统状态误差微分方程，有： ，上式中Ft表示系统矩阵；Gt表示输入方程；ωt表示连续时间的系统高斯白噪声；δxt表示t时刻的状态误差；其次，根据上述微分方程进一步推出误差状态外推方程，有： ，其中： ，上式中，Φk,k-1表示状态转移矩阵，ωk-1为系统高斯白噪声ωt的线性变换，δxk表示k时刻的状态误差；δxk-1表示k-1时刻的状态误差；记离散化时间间隔Δt=tk-tk-1，当Δt比较小时，Ft在时间间隔[tk-1，tk]内变化不大，此时有： ，能够证明ωk是零均值白噪声序列，满足： ，上式中，符号“N”表示高斯分布，符号“~”表示服从，也就是ωk服从高斯分布，Qk为系统状态噪声协方差矩阵，计算方法为： ，上式中，Qk的计算使用梯形积分公式；所述GPS位置误差观测方程建模，GPS的位置误差观测方程写成以下形式： ，上式中，δzr表示GPS位置观测误差；Hr表示GPS的位置观测矩阵，δrG表示GPS天线的位置误差，所述GPS速度误差观测方程建模为： ，上式中，δvG表示GPS速度误差，Hv表示GPS的速度观测矩阵，具体表达式为： ，上式中，lb为GPS相位中心的杆臂向量在b系下的投影； ，上式中，符号diag表示对角矩阵化操作，GPS速度误差观测方程建模和GPS位置误差观测方程建模共同构成了GPS的观测方程建模。

全文数据：

权利要求：

百度查询： 江苏领创星通卫星通信科技有限公司 一种基于IMU、GPS与UWB的高精度定位算法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD