一种自主定位方法与系统与流程

本发明涉及无人车或机器人技术领域，具体涉及一种自主定位方法与系统。

背景技术：

自主定位技术是指机器人或无人车等智能系统根据导航卫星信号、自身传感器信息、地图等先验信息获取其在运行环境中所处的位置与姿态信息。自主定位技术是机器人、无人车等智能系统自主作业中首先需要解决的关键技术问题。

复杂环境中，当导航卫星系统受到电磁干扰等环境影响时，机器人或无人车可能只能获取有限的导航卫星信号(导航卫星信号时有时无、导航卫星定位误差大、导航卫星信号定位信息无效)，在这种环境条件下，如何获取准确的位姿信息是智能系统执行作业任务时需要解决的首要问题。

技术实现要素：

为解决上述当导航卫星系统受到电磁干扰等环境影响时，机器人或无人车只能获取有限的导航卫星信号，不能准确进行定位的问题，本发明提供一种自主定位方法，所述方法包括：

当在设定时间期限内不能获取导航卫星定位时，基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息；

所述当前的位置信息包括：位置与姿态的最佳估计值。

优选的，所述基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息包括：

将所述第一位置信息设定为初始位置信息；

基于所述初始位置信息和当前多传感器数据计算定位信息；

将所述定位信息设为初始位置信息，持续根据初始位置信息和当前时刻多传感器数据迭代计算，更新定位信息。

优选的，所述将所述定位信息设为初始位置信息，持续根据初始位置信息和当前时刻多传感器数据迭代计算，更新定位信息包括：

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-nⁱ～ykⁱ,xk-n^o～xk^o,yk-n^g～yk^g,当系统长时间无法获取导航卫星定位信号时，将仅采取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表示三维空间下系统状态(含6个系统状态量位置与姿态)。

其中xk如式4所示：

xk＝[q,p,v,w,g,b]^t(4)

其中，q是姿态信息中旋转量的四元数表示，p＝[x,y,z]是位置信息的变换，v速度信息，w是角速度信息，g和b分别是增量和偏置信息，动态模型f的表述公式为：

其中，是一个四元数的乘法，qw是基于角速度的旋转四元数，rw是相应的旋转矩阵，v表示线速度；δt表示采样周期。本方法仅使用四元数的向量部分进行优化，因此，上述公式中仅有少量旋转矢量。n个状态的同步优化需要计算ekf大约n次。利用批处理优化问题的稀疏性，将有利于增加线性特征。

因此，根据ins、里程计、gnss的状态初值y0ⁱ、x0^o、y0^g和最新的n个最新时间状态的测量值yk-nⁱ～ykⁱ、xk-n^o～xk^o、yk-n^g～yk^g，带入公式3中的优化方程，并采用levenberg-marquardt(l-m)方法求取公式3中xk的最优解，即获取了公式4中所示的6个系统状态量位置与姿态值xk的最佳估计值

步骤3：传递步骤，向下一时刻即k+1时刻传递公式3中卡尔曼滤波(kf)方法的协方差p。

步骤4：预测下一状态xk+1和更新相应状态向量，去掉窗口中n个状态值中的第一个状态值，循环返回步骤1，直至最后时刻结束。

实施例2

本发明还涉及一种自主定位系统，包括：

位置获取模块：用于当在设定时间期限内不能获取导航卫星定位时，基于最近获取的导航卫星定位的第一位置信息和多传感器数据，采用扩展卡尔曼滤波方法进行计算得到当前的位置信息；

所述位置获取模块包括：定位信息更新单元；

所述定位信息更新单元用于将所述第一位置信息设定为初始位置信息；

基于所述初始位置信息和当前多传感器数据计算定位信息；

将所述定位信息设为初始位置信息，持续根据初始位置信息和当前时刻多传感器数据迭代计算，更新定位信息。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上仅为本发明的实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均包含在申请待批的本发明的权利要求范围之内。