一种自主定位方法与系统技术方案

本发明专利技术涉及一种不完全依赖导航卫星信号的自主定位方法与系统，包括：当在设定时间期限内不能获取导航卫星定位时，基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息。本发明专利技术为一种不完全依赖导航卫星信号的自主定位方法与系统，当导航卫星系统受到电磁干扰等环境影响时，机器人或无人车智能通过获取有限的导航卫星信号，仍然能够准确进行定位。

An Autonomous Location Method and System

The present invention relates to an autonomous positioning method and system that does not depend entirely on navigation satellite signals, including: when the navigation satellite positioning can not be obtained within a set time limit, based on the first position information and multi-sensor data of the recently acquired navigation satellite positioning, the current position information can be calculated by using the extended Kalman filter method. The invention provides an autonomous positioning method and system that does not rely entirely on navigation satellite signals. When the navigation satellite system is affected by electromagnetic interference and other environmental factors, the robot or unmanned vehicle can still accurately locate by acquiring limited navigation satellite signals intelligently.

【技术实现步骤摘要】
一种自主定位方法与系统
本专利技术涉及无人车或机器人
，具体涉及一种自主定位方法与系统。
技术介绍
自主定位技术是指机器人或无人车等智能系统根据导航卫星信号、自身传感器信息、地图等先验信息获取其在运行环境中所处的位置与姿态信息。自主定位技术是机器人、无人车等智能系统自主作业中首先需要解决的关键技术问题。复杂环境中，当导航卫星系统受到电磁干扰等环境影响时，机器人或无人车可能只能获取有限的导航卫星信号(导航卫星信号时有时无、导航卫星定位误差大、导航卫星信号定位信息无效)，在这种环境条件下，如何获取准确的位姿信息是智能系统执行作业任务时需要解决的首要问题。
技术实现思路
为解决上述当导航卫星系统受到电磁干扰等环境影响时，机器人或无人车只能获取有限的导航卫星信号，不能准确进行定位的问题，本专利技术提供一种自主定位方法，所述方法包括：当在设定时间期限内不能获取导航卫星定位时，基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息；所述当前的位置信息包括：位置与姿态的最佳估计值。优选的，所述基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息包括：将所述第一位置信息设定为初始位置信息；基于所述初始位置信息和当前多传感器数据计算定位信息；将所述定位信息设为初始位置信息，持续根据初始位置信息和当前时刻多传感器数据迭代计算，更新定位信息。优选的，所述将所述定位信息设为初始位置信息，持续根据初始位置信息和当前时刻多传感器数据迭代计算，更新定位信息包括：S101：基于初始位置信息和当前时刻卡尔曼滤波的协方差得到当前位置信息；S102：当不满足第一条件和第二条件时，将所述当前位置信息设为初始位置信息，执行S101，直到满足第一条件或第二条件；所述第一条件为：达到预定时间；所述第二条件为：当接收到的传感器数据为空时，终止。优选的，所述基于所述初始位置信息和当前多传感器数据计算定位信息的公式如下所示：式中，xk-N表示k-N时刻的系统状态值；P-1表示先验协方差矩阵的逆矩阵；f(xk-1)表示k-1时刻时系统状态转移方程；xk表示k时刻的系统状态值；x表示三维空间下系统状态。优选的，所示xk的系统状态值如下所示：xk＝[q,p,v,w,g,b]T式中，q表示姿态信息中旋转量的四元数；p表示位置信息的变换；v表示速度；w表示角速度；g表示增量；b表示偏置。优选的，所述f动态转移方程如下所示：式中，q表示姿态的四元数；qw表示基于角速度的旋转四元数；Rw表示相应的旋转矩阵；v表示线速度；δt表示采样周期。优选的，所述计算定位信息的公式基于多个时间状态的批处理优化函数和第一个时间点的传统EKF优化方程获得；所述多个时间状态的批处理优化函数如下所示：式中，x0表示系统先验信息；xk-1表示k-1时刻的系统状态；Q-1表示状态转移协方差的逆矩阵；R-1表示观测值噪声协方差矩阵的逆矩阵；yk表示表示k时刻的传感器测量值；h(xk)表示k时刻时系统的测量方程。所述第一个时间点的传统EKF优化方程如下所示：J(k)＝(xk-x)P-1(xk-x)T+(yk-h(xk))R-1(yk-h(xk))T式中，P表示先验协方差。优选的，所述获取的多传感器数据包括：系统自身惯性导航系统数据、里程计。一种自主定位系统，包括：位置获取模块：用于当在设定时间期限内不能获取导航卫星定位时，基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息。优选的，所述位置获取模块包括：定位信息更新单元；所述定位信息更新单元用于将所述第一位置信息设定为初始位置信息；基于所述初始位置信息和当前多传感器数据计算定位信息；将所述定位信息设为初始位置信息，持续根据初始位置信息和当前时刻多传感器数据迭代计算，更新定位信息。与最接近的现有技术相比，本申请具有如下有益效果：1、本专利技术为一种自主定位方法与系统，当在设定时间期限内不能获取导航卫星定位时，基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息，当导航卫星系统受到电磁干扰等环境影响时，机器人或无人车智能通过获取有限的导航卫星信号，仍然能够准确进行定位；2、本专利技术为一种自主定位方法与系统，通过采用基于扩展卡尔曼滤波的定位方法，能够融合智能系统自身INS、里程计与有限的GNSS信号，实现智能系统不完全依赖导航卫星信号的准确自主定位。附图说明图1为本专利技术的方法流程示意图；图2为本专利技术的自主定位方法流程图。具体实施例下面结合具体的实施例，对
技术实现思路
做进一步解释说明：实施例1如图1所示，一种自主定位方法主要包括以下步骤：当在设定时间期限内不能获取导航卫星定位时，基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息；所述当前的位置信息包括：位置与姿态的最佳估计值；下面的具体实施例是对上述内容的解释说明。如图2所示，步骤1：获取多传感器数据，获取INS、里程计、GNSS的N个最新时间状态的测量值：yk-NI～ykI,xk-NO～xkO,yk-NG～ykG,当系统长时间无法获取导航卫星定位信号时，将仅采取INS、里程计的测量值。步骤2：优化步骤，根据BOEKF方法，获得N个时间状态的批处理优化函数，获取6个系统状态量位置与姿态的当前最佳估计BOEKF方法是基于EKF和批处理优化的一种改进优化算法，BOEKF最突出优点是当EKF由于高不确定性和严重的非线性导致不能很好的建模滤波时，BOEKF的批处理优化迭代过程将提高收敛性，从而保证结果的质量与精度。在EKF的基础上，BOEKF方法将批处理优化作用于移动窗口内的N个最新状态。公式1中所示即为通用的批处理优化代价函数为：式中，x0表示系统先验信息(含6个系统状态量位置与姿态值)；x表示系统状态(含6个系统状态量位置与姿态值)；P-1表示先验协方差矩阵的逆矩阵；xk表示k时刻的系统状态(含6个系统状态量位置与姿态值)；f(·)表示系统状态转移方程；xk-1表示k-1时刻的系统状态(含6个系统状态量位置与姿态值)；Q-1表示状态转移协方差的逆矩阵；R-1表示观测值噪声协方差矩阵的逆矩阵；yk表示表示k时刻的传感器测量值；h(·)表示系统的测量方程。在解决递归问题时，代价函数中当前所有的测量值会使用瞬时高斯分布和中值来近似表示，当N＝1时，传统EKF的更新采用对同一时间的批处理优化的方法来解决，如公式2所示：J(k)＝(xk-x)P-1(xk-x)T+(yk-h(xk))R-1(yk-h(xk))T(2)其中，P是由传统EKF方法预测步骤中计算所得的协方差。BOEKF方法中，这种近似被推导N步，并用以解决N个时刻的批量优化问题。k-N信息被不断抽化为一个均值和扩展卡尔曼滤波的高斯分布，而N时刻的测量值也被同时使用。因此，由公式1和公式2得到BOEKF方法中N个时间状态的批处理优化函数，如公式3所示：式中，xk-N表示k-N时刻的系统状态值(含6个系统状态量的位置与姿态值)；f(·)表示系统状态转移方程；xk表示k时刻的，系统状态值；x表示三维空间下系本文档来自技高网...

【技术保护点】
1.一种自主定位方法，其特征在于，所述方法包括：当在设定时间期限内不能获取导航卫星定位时，基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息；所述当前的位置信息包括：位置与姿态的最佳估计值。

【技术特征摘要】
1.一种自主定位方法，其特征在于，所述方法包括：当在设定时间期限内不能获取导航卫星定位时，基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息；所述当前的位置信息包括：位置与姿态的最佳估计值。2.如权利要求1所述的一种自主定位方法，其特征在于，所述基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息包括：将所述第一位置信息设定为初始位置信息；基于所述初始位置信息和当前多传感器数据计算定位信息；将所述定位信息设为初始位置信息，持续根据初始位置信息和当前时刻多传感器数据迭代计算，更新定位信息。3.如权利要求2所述的一种自主定位方法，其特征在于，所述将所述定位信息设为初始位置信息，持续根据初始位置信息和当前时刻多传感器数据迭代计算，更新定位信息包括：S101：基于初始位置信息和当前时刻卡尔曼滤波的协方差得到当前位置信息；S102：当不满足第一条件和第二条件时，将所述当前位置信息设为初始位置信息，执行S101，直到满足第一条件或第二条件；所述第一条件为：达到预定时间；所述第二条件为：当接收到的传感器数据为空时，终止。4.如权利要求2所述的一种自主定位方法，其特征在于，所述基于所述初始位置信息和当前多传感器数据计算定位信息的公式如下所示：式中，xk-N表示k-N时刻的系统状态值；P-1表示先验协方差矩阵的逆矩阵；f(xk-1)表示k-1时刻时系统状态转移方程；xk表示k时刻的系统状态值；x表示三维空间下系统状态。5.如权利要求4所述的一种自主定位方法，其特征在于，所示xk的系统状态值如下所示：xk＝[q,p,v,w,g,b]T式中，q表示姿态信息中...

【专利技术属性】
技术研发人员：康晓，吴越，靳璐，靳保，余雪玮，马睿璘，罗涛，陶进，苏治宝，苏波，
申请(专利权)人：中国北方车辆研究所，
类型：发明
国别省市：北京,11