买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：南京理工大学

摘要：本发明公开了一种带观测时滞的无人车多源融合卡尔曼滤波方法及系统，具体为：无人车多源导航系统上电，对惯性导航元件进行初始对准；以捷联惯性导航系统作为时间、空间基准，对视觉惯性里程计、全球导航卫星系统进行时空同步；构建带时滞的SINSGNSS联邦子滤波器和SINSVIO联邦子滤波器，使用各联邦子滤波器进行实时观测修正，得到子滤波器的局部最优估计值；将子滤波器的结果输入主滤波器，在联邦主滤波器中根据信息分配因子对各子滤波器的局部最优估计值进行全局融合，实现高精度和可靠性的定位与导航。本发明方法降低了多源导航时滞系统的速度误差和位置误差，提高了定位和导航精度，并且结构简单，易于实现，工作稳定，可靠性高。

主权项：1.一种带观测时滞的无人车多源融合卡尔曼滤波方法，其特征在于，包括以下步骤：步骤1、无人车多源导航系统上电，对惯性导航元件进行初始对准；步骤2、以捷联惯性导航系统作为时间、空间基准，对视觉惯性里程计、全球导航卫星系统进行时空同步；步骤3、惯性导航元件分别与全球导航卫星系统、视觉惯性里程计进行组合，构建带时滞的SINSGNSS联邦子滤波器和SINSVIO联邦子滤波器，使用各联邦子滤波器进行实时观测修正，得到子滤波器的局部最优估计值；步骤4、将子滤波器的结果输入主滤波器，在联邦主滤波器中根据信息分配因子对各子滤波器的局部最优估计值进行全局融合，实现最终的定位与导航；步骤3所述的惯性导航元件分别与全球导航卫星系统、视觉惯性里程计进行组合，构建带时滞的SINSGNSS联邦子滤波器和SINSVIO联邦子滤波器，使用各联邦子滤波器进行实时观测修正，得到子滤波器的局部最优估计值，具体如下：步骤3.1、使用带时滞的SINSGNSS联邦子滤波器和SINSVIO联邦子滤波器分别进行卡尔曼滤波时间更新；步骤3.2、利用带时滞的SINSGNSS联邦子滤波器和SINSVIO联邦子滤波器分别进行卡尔曼滤波观测更新；步骤3.1所述的使用带时滞的SINSGNSS联邦子滤波器和SINSVIO联邦子滤波器分别进行卡尔曼滤波时间更新，具体如下：步骤3.1.1、系统状态量为： 其中φ表示惯导的俯仰角、横滚角和航向角的失准角误差，δvn表示惯导的东向、北向和天向速度误差，δp表示惯导的纬度、经度和高度误差，εb表示陀螺仪xyz三轴的零偏，表示加速度计xyz三轴的零偏；步骤3.1.2、状态一步转移矩阵为：Φkk-1＝I+Fk-1Δt2其中Φkk-1为状态一步转移矩阵，I为单位阵，Δt为惯导的采样间隔时间，Fk-1为k-1时刻的状态转移矩阵；步骤3.1.3、状态先验估计为： 其中为k时刻的先验状态估计值，为k-1时刻的后验状态估计值；步骤3.1.4、状态一步预测协方差矩阵为： 其中为k时刻状态一步预测协方差矩阵，Pk-1为k-1时刻状态估计协方差矩阵，Qk为系统噪声协方差矩阵；步骤3.2所述的利用带时滞的SINSGNSS联邦子滤波器和SINSVIO联邦子滤波器分别进行卡尔曼滤波观测更新，具体如下：步骤3.2.1、卡尔曼增益为： 其中Kk为卡尔曼增益系数，Hk为子滤波器中对应的观测矩阵，Rk为观测噪声协方差矩阵；步骤3.2.2、状态协方差矩阵为： 其中Pk为k时刻状态估计均方误差阵，I为单位阵；步骤3.2.3、状态后验估计为： 其中为k时刻状态量的最优估计，为k时刻状态量的先验估计，子滤波器的每次更新中，先验估计值都来自于联邦主滤波器输入，Zk为子滤波器中对应的观测量；步骤3.2.4、由于视觉惯性里程计、全球导航卫星系统的解算、传输存在时间滞后，因此需要对观测时滞进行处理，得到带时滞的观测量，具体如下：根据公式7，为k-1时刻的先验估计，设定k-1时刻具有无时滞的观测量Zk-1，则k-1时刻的后验估计为因此在k时刻，根据已知量Zk-1、建立对带时滞的观测量Zk的估计为： 即利用前一误差状态关系，结合当前状态量，构建当前带时滞的观测量；对于联邦卡尔曼滤波器，全球导航卫星系统、视觉惯性里程计k时刻的观测量分别为： 其中表示捷联惯导系统与全球导航卫星系统的速度差位置差表示捷联惯导系统与视觉惯性里程计的位置差得到k时刻的带时滞的全球导航卫星系统、视觉惯性里程计的观测量为： 其中表示捷联惯导系统与带时滞的全球导航卫星系统的速度差位置差表示捷联惯导系统与带时滞的视觉惯性里程计的位置差通过带观测时滞的SINSVIO、SINSGNSS子系统，实现联邦卡尔曼滤波多源融合结果的实时性；步骤3.2.5、全球导航卫星系统、视觉惯性里程计的观测矩阵分别为： HVIO＝[03×303×3I3×303×303×3]14其中HGNSS为全球导航卫星系统的观测矩阵，HVIO为视觉惯性里程计的观测矩阵。

全文数据：

权利要求：

百度查询： 南京理工大学 一种带观测时滞的无人车多源融合卡尔曼滤波方法及系统