一种基于多源融合的机器人定位方法

1.本发明涉及计算机和位置服务领域，尤其涉及的是一种基于多源融合的机器人定位方法、智能终端及存储介质。


背景技术：

2.随着科技的发展和人们生活水平的不断提高，各种智能终端的使用越来越普及。由于人们大部分时间都待在室内环境，对室内机器人定位和导航服务的需求也越来越大。与此同时，定位技术是机器人应用的关键技术，例如导航机器人、巡逻机器人、清洁机器人和送餐机器人，所有这些都需要全球导航卫星系统(global navigation satellite system，gnss)定位技术，它可以在室外场景中提供准确的定位结果。但是，由于室内环境存在信号受阻问题，基于gnss的定位技术无法用于室内定位。因此，室内机器人的定位仍然是一个挑战。
3.现有的机器人室内定位方法大部分是通过专业的传感器来获取定位结果，例如摄像头、激光器，超宽带、惯性传感器等。然而，现有技术中这些专业的设备价格昂贵，没有一个单一的设备可以解决所有室内场景下机器人的定位问题，无法降低整体成本。
4.因此，现有导航技术还有待改进和发展。


技术实现要素：

5.本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种基于多源融合的机器人定位方法、智能终端及存储介质，本发明解决了所有室内场景下机器人的定位问题，并降低了整体成本，为用户的使用提供了方便。
6.本发明解决问题所采用的技术方案如下：
7.一种基于多源融合的机器人定位方法，其中，所述方法包括：
8.通过构建的轮式编码器辅助的惯性导航定位系统获取位置惯性信息和线性速度信息，采用误差状态卡尔曼滤波、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息；
9.通过构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息；
10.基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果。
11.所述的基于多源融合的机器人定位方法，其中，所述通过构建的轮式编码器辅助的惯性导航定位系统获取位置惯性信息和线性速度信息，采用误差状态卡尔曼滤波、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息的步骤包括：
12.通过设置在机器人上的惯性导航系统，测量机器人的加速度和角速度，以获得机
器人的姿态、速度和位置惯性信息；以及通过设置在机器人行驶车轮上的编码器，测量车轮的线性速度，以构建轮式编码器辅助的惯性导航定位系统；
13.设置一误差状态卡尔曼滤波eskf；
14.采用所述误差状态卡尔曼滤波eskf、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息。
15.所述的基于多源融合的机器人定位方法，其中，所述通过构建的轮式编码器辅助的惯性导航定位系统获取位置惯性信息和线性速度信息，采用误差状态卡尔曼滤波、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息的步骤之前包括：
16.预先在机器人上设置的惯性导航系统，用于测量机器人的加速度和角速度，以获得机器人的姿态、速度和位置惯性信息；
17.以及预先在机器人行驶的车轮上设置编码器，用于测量车轮的线性速度。
18.所述的基于多源融合的机器人定位方法，其中，所述通过构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息的步骤包括：
19.设置通过基于射频测距的定位系统，获取射频定位信息，并设置采用自适应扩展卡尔曼滤波器对获取的射频定位信息进行滤波，以构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息。
20.所述的基于多源融合的机器人定位方法，其中，所述基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果的步骤包括：
21.获取所述轮式编码器辅助的惯性导航定位系统融合处理的第一融合位置信息；
22.获取基于自适应的扩展卡尔曼滤波的射频定位系统滤波后的射频定位信息；
23.基于所述第一融合位置信息，以及所述射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果。
24.所述的基于多源融合的机器人定位方法，其中，所述第一融合位置信息，为位置信息的最优估计；所述滤波后的射频定位信息包括定位信息和测距值信息。
25.所述的基于多源融合的机器人定位方法，其中，所述基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果的步骤包括：
26.将所述滤波后的射频定位信息的测距值用作滤波约束，获得轮式编码器辅助的惯性导航定位系统的惯性导航定位误差，称为最优估计的惯性导航定位误差；
27.通过rts平滑器进一步平滑所述惯性导航定位误差；
28.在惯导系统的定位结果中对平滑后的惯性导航定位误差进行补偿，以校正惯导系统的定位误差，得到最终的所述融合定位结果。
29.一种基于多源融合的机器人定位装置，其中，包括：
30.惯导轮式编码器处理模块，用于通过构建的轮式编码器辅助的惯性导航定位系统获取位置惯性信息和线性速度信息，采用误差状态卡尔曼滤波、对来自惯性导航定位系统
的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息；
31.射频定位处理模块，用于通过构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息；
32.融合处理模块，用于基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果。
33.一种智能终端，其中，包括有存储器，以及一个或者一个以上的程序，其中一个或者一个以上程序存储于存储器中，且经配置以由一个或者一个以上处理器执行所述一个或者一个以上程序包含用于执行任意一项所述的方法。
34.一种非临时性计算机可读存储介质，当所述存储介质中的指令由电子设备的处理器执行时，使得电子设备能够执行任意一项所述的方法。
35.本发明提供了一种基于多源融合的机器人室内定位方法，它可适用基于智能手机的机器人平台，也可适用于基于其他终端的机器人平台。本发明首先构建轮式编码器辅助的惯性导航定位系统，以抑制惯性导航的长时间累积误差问题。在此基础上，构建基于自适应的扩展卡尔曼滤波的射频定位系统，以融合射频测距信息。同时，为了克服轮式编码器辅助的惯性导航定位系统的累积误差问题与基于射频测距信息的定位系统的不稳定问题，本发明实现一个基于惯导、轮式编码器、射频信号的紧组合导航系统。本发明利用imu和轮式编码器对室内机器人进行定位，可以有效抑制惯性导航系统的累积误差，保持高精度定位。
附图说明
36.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
37.图1是本发明实施例提供的基于多源融合的机器人定位方法的流程示意图。
38.图2是本发明实施例提供的基于多源融合的机器人定位方法的机器人平台结构示意图。
39.图3是本发明实施例提供的基于多源融合的机器人定位方法的紧组合的定位系统功能原理框图。
40.图4是本发明实施例提供的基于多源融合的机器人定位装置的原理框图。
41.图5是本发明实施例提供的智能终端的内部结构原理框图。
具体实施方式
42.为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
43.需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后
……
)，
则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
44.由于人们大部分时间都待在室内环境，对室内机器人定位和导航服务的需求也越来越大。与此同时，定位技术是机器人应用的关键技术，例如导航机器人、巡逻机器人、清洁机器人和送餐机器人，所有这些都需要全球导航卫星系统(global navigation satellite system，gnss)定位技术，它可以在室外场景中提供准确的定位结果。但是，由于室内环境存在信号受阻问题，基于gnss的定位技术无法用于室内定位。因此，室内机器人的定位仍然是一个挑战。
45.现有的机器人室内定位方法大部分是通过专业的传感器来获取定位结果，例如摄像头、激光器，超宽带、惯性传感器等。然而，这些专业设备价格昂贵，没有一个单一的设备可以解决所有室内场景下机器人的定位问题。对于复杂的室内环境，多传感器融合定位是解决室内移动机器人定位问题的关键技术。针对于上述的现状，需要一个融合多传感器的室内定位技术，以满足可面向大众推广的室内移动机器人定位需求。
46.随着智能手机的不断发展，智能手机的内置传感器的质量也在不断提高。私人的智能手机配备了各种嵌入式传感器，并且功能强大。因此，智能手机有望成为实现室内机器人定位的载体。
47.为了解决现有技术的问题，本发明提供一种基于多源融合的机器人定位方法，本发明可适用基于智能手机的机器人平台，也可适用于基于其他终端的机器人平台。本发明利用imu和轮式编码器对室内机器人进行定位，可以有效抑制惯性导航系统的累积误差，保持高精度定位。
48.本发明首先构建轮式编码器辅助的惯性导航定位系统，以抑制惯性导航的长时间累积误差问题。在此基础上，构建基于自适应的扩展卡尔曼滤波的射频定位系统，以融合射频测距信息。同时，为了克服轮式编码器辅助的惯性导航定位系统的累积误差问题与基于射频测距信息的定位系统的不稳定问题，结合轮式编码器辅助的惯性导航定位系统与射频定位系统得到的位置信息，实现一个基于惯导、轮式编码器、射频信号的紧组合导航系统。
49.示例性方法
50.如图1中所示，本发明实施例提供一种基于多源融合的机器人定位方法，所述基于多源融合的机器人定位方法可以应用于智能终端。在本发明实施例中，所述方法包括如下步骤：
51.步骤s100、通过构建的轮式编码器辅助的惯性导航定位系统获取位置惯性信息和线性速度信息，采用误差状态卡尔曼滤波、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息；
52.本发明实施例的一种基于多源融合的机器人室内定位方法，可适用基于智能手机的机器人平台，也可适用于基于其他终端的机器人平台。本发明在机器人上构建轮式编码器辅助的惯性导航定位系统，以抑制惯性导航的长时间累积误差问题。
53.具体地，如图2所示，本发明通过设置在机器人10上的惯性导航系统，测量机器人的加速度和角速度，以获得机器人10的姿态、速度和位置惯性信息；以及通过设置在机器人行驶车轮上的编码器11，测量车轮的线性速度，以构建轮式编码器辅助的惯性导航定位系统；
54.设置一误差状态卡尔曼滤波eskf；
55.采用所述误差状态卡尔曼滤波eskf、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息。
56.本发明实施例中，采用的惯性导航系统(inertial navigation system，ins)是一种自主导航系统，它可以在没有外部信息的情况下完成导航任务。因为，随着时间的推移，惯性导航系统容易受到很大的累积误差的限制。为了克服上述问题，如图3所示，本发明还设置了一个误差状态卡尔曼滤波(error-state kalman filter，eskf)来融合来自惯性测量单元(inertial measurement unit，imu)的位置惯性信息和来自轮式编码器辅助的惯性导航定位系统的测量值。
57.步骤s200、通过构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息；
58.如图3所示，本发明设置通过基于射频测距的定位系统，获取射频定位信息，并设置采用自适应扩展卡尔曼滤波器对获取的射频定位信息进行滤波，以构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息。
59.即本步骤在此基础上，构建基于自适应的扩展卡尔曼滤波的射频定位系统，以融合射频测距信息。
60.步骤s300、基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果。
61.具体地，本步骤获取所述轮式编码器辅助的惯性导航定位系统融合处理的第一融合位置信息；获取基于自适应的扩展卡尔曼滤波的射频定位系统滤波后的射频定位信息；基于所述第一融合位置信息，以及所述射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果。
62.本步骤s300为了克服轮式编码器辅助的惯性导航定位系统的累积误差问题与基于射频测距信息的定位系统的不稳定问题，结合轮式编码器辅助的惯性导航定位系统与射频定位系统得到的位置信息，实现一个基于惯导、轮式编码器、射频信号的紧组合导航系统。基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果。
63.本发明具体实施例中，在具体实施前，参考图2和图3所示，需要预先在机器人10上设置的惯性导航系统，用于测量机器人的加速度和角速度，以获得机器人的姿态、速度和位置惯性信息；以及预先在机器人行驶的车轮上设置编码器11，用于测量车轮的线性速度。
64.进一步地，所述步骤s100的通过构建的轮式编码器辅助的惯性导航定位系统获取位置惯性信息和线性速度信息，采用误差状态卡尔曼滤波、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息具体包括：
65.步骤s101、通过设置在机器人上的惯性导航系统，测量机器人的加速度和角速度，以获得机器人的姿态、速度和位置惯性信息；以及通过设置在机器人行驶车轮上的编码器，
测量车轮的线性速度，以构建轮式编码器辅助的惯性导航定位系统；
66.步骤s102、设置一误差状态卡尔曼滤波eskf；
67.步骤s103、采用所述误差状态卡尔曼滤波eskf、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息。
68.本发明实施例中，关于轮式编码器辅助的惯性导航定位系统的构建，首先基于现有惯性导航系统进行构建，通过在机器人系统装载惯性导航系统，惯性导航系统(inertial navigation system，ins)是一种自主导航系统，它可以在没有外部信息的情况下完成导航任务；在机器人行驶车轮上设置轮式编码器辅助测量。
69.本发明实施例中，通过所述惯性导航系统(ins定位系统)可以测量机器人的加速度和角速度。ins定位系统的ins定位算法通过对惯性测量单元(inertial measurement unit，imu)的测量的比力和角速度进行积分，以获得机器人的姿态、速度和位置惯性信息。ins定位系统的速度更新方程与位置更新方程可以表示为如下：
[0070][0071][0072][0073][0074]
其中，表示k时刻的速度，表示k时刻的位置，表示k时刻的角增量，表示k时刻的加速度增量，gn＝[0 0 g]
t
，g表示重力加速度，与分别表示在k时刻的角速度与比力，bg与bf分别表示陀螺仪的偏差与加速度计的偏差，δt表示单位采样时间间隔。
[0075]
如图2所示，本发明在具体实施前需要预先在机器人10上设置的惯性导航系统，用于测量机器人的加速度和角速度，以获得机器人的姿态、速度和位置惯性信息；以及预先在机器人10行驶的车轮上设置编码器11，用于测量车轮的线性速度。
[0076]
本发明实施例中，关于在机器人行驶车轮上设置轮式编码器辅助测量，如图2所示，本发明实施例提出的机器人10，两个车轮编码器11安装在两个前轮上。车轮编码器11用于测量车轮的线性速度。左右车轮的角速度可以分别用ω
l
和ωr表示。通过这些测量值的简单计算，可以得到每个车轮的线速度。
[0077]
本发明实施例中，通过在机器人行驶车轮上设置轮式编码器辅助测量只能辅助测量获取机器人的线速度，而惯性导航系统的ins定位系统可以推导出位置、速度和姿态。因为，随着时间的推移，惯性导航系统会存在较大的累积误差。为了克服这些限制，本发明设置一误差状态卡尔曼滤波(error-state kalman filter，eskf)；采用所述误差状态卡尔曼滤波eskf、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息，以抑制ins累积的误差。误差状态卡尔曼滤波eskf算法由时间更新于测量值更新两部分组成。
[0078]
在时间更新阶段，本发明定义了误差状态向量δxk＝[δpk,δvk,δφk,δbg,δba]
t
，它包含位置误差、速度误差、姿态误差、陀螺仪偏差和加速度计偏差。陀螺仪和加速计偏差的
变化可以用一阶马尔可夫过程来描述。离散时间系统状态方程可以表示为公式(5)：
[0079]
δx
k,k-1
＝φ
k,k-1
δx
k-1
+ω
k-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0080][0081]
其中是相对于导航坐标系的特定力的斜对称矩阵。τg＝diag(τ
gx
,τ
gy
,τ
gz
)和τa＝diag(τ
ax
,τ
ay
,τ
az
)表示相关时间。
[0082]
相应的误差协方差矩阵p
k,k-1
可以表示为：
[0083][0084]
其中，q
k-1
表示系统噪声协方差矩阵。
[0085]
在测量值更新阶段，如图2所示，本发明的机器人10是一个差分驱动机器人，受非完整约束(nonholonomic constraints，nhc)。nhc基于这样一个事实，即在车辆正常移动过程中不会发生跳跃或侧向滑动，这意味着车辆的侧向和垂向速度可以假定为零。
[0086]
车辆坐标系下的速度可通过以下公式表示：
[0087][0088]
其中，δke表示编码器比例因子误差，v
encoder
表示轮式编码器解算的线性速度。
[0089]
同时，在车辆坐标系中由ins定位系统得到的速度可以表示为：
[0090][0091]
其中，其中表示从车身坐标系到车辆坐标系的方向余弦矩阵(direction cosine matrix，dcm)。表示从导航坐标系到车身框架的dcm。是由ins定位系统在导航坐标系下计算的速度。ωb是车身坐标系中陀螺仪的三轴角速度。(
×
)表示斜对称操作。lb表示杆臂，它是imu中心和车辆坐标系下原点之间的位移。
[0092]
速度误差测量方程可表示为：
[0093][0094]
进一步地，本发明实施例中，关于步骤s200的通过构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息的步骤的解释具体如下：
[0095]
本发明实施例中，为了减少构建的轮式编码器辅助的惯性导航定位系统随着时间的推移，存在的少量累积定位误差，克服累积定位误差问题，采用融合定位的方法。进一步地，在构建紧组合导航系统前，提供另一种相对稳定的定位系统，本发明采用基于射频测距的定位系统。具体地，设置通过基于射频测距的定位系统，获取射频定位信息，并设置采用自适应扩展卡尔曼滤波器对获取的射频定位信息进行滤波，以构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息。
[0096]
因为一般在复杂的室内环境中，射频定位系统面临多路径干扰和数据包丢失等挑
战，影响其定位精度。为了提高定位性能，本发明采用自适应扩展卡尔曼滤波器(adaptive extended kalman filter，aekf)对定位信息进行滤波，以构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息。它通过一系列随时间变化的测量来获得精确的定位结果。它包括时间更新阶段与测量值更新阶段。
[0097]
在时间更新阶段，给定采样时间tc和采样指数k，状态向量xk可以表示为：
[0098]
xk＝[p
x,k p
y,k v
x,k v
y,k a
x,k a
y,k
]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0099]
其中p
x,k
和p
y,k
表示移动设备的位置。v
x,k
和v
y,k
表示相应的速度向量，而a
x,k
和a
y,k
表示相应的加速度向量。
[0100]
状态向量转移模型可以描述为：
[0101]
xk＝fkx
k-1
+ωkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0102]
其中，fk是状态转移矩阵，ωk是时间指数k处的过程噪声向量。
[0103]
在状态更新期间，如果距离附近基站的测距距离可用，本发明可以使用这些测量值来校正预测的状态。本发明将dk＝[d
1,k
,...,d
n,k
]
t
定义为在k时刻下来自n个附近基站的距离测量向量，其可表示为：
[0104][0105]
其中，vk＝[v
1,k
,...,v
n,k
]
t
表示均值为零的距离测量误差向量。p
a,k
是移动设备在k时刻下的实际位置，而p
i,k
表示第i个基站的位置。如果实际可用的基站少于n个，则dk的维数将减少到可用基站的数量。
[0106]
在测量值更新阶段，测量值模型ζk可以用以下公式表示：
[0107][0108]
其中，p
k|k-1
表示移动设备的估计位置。
[0109]
对于测量噪声，本发明采用sage-husa自适应卡尔曼滤波器对其进行更新，该滤波器可以通过测量输出来估计系统的噪声参数。测量噪声矩阵rk的更新方程如下：
[0110][0111][0112]
其中，b是衰减因子，β0设置为零。衰落系数b通常为0.9到0.999。hk是一个雅可比矩阵，可由以下公式表示：
[0113][0114]
本发明实施例中关于步骤s300的所述基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果的步骤解释如下：
[0115]
本发明实施例中，如图3所示，基于所述轮式编码器辅助的惯性导航定位系统得到的所述第一融合位置信息，为位置信息的最优估计；基于自适应的扩展卡尔曼滤波的射频定位系统获得的滤波后的射频定位信息包括定位信息和测距值信息。
[0116]
如图3所示，然后将所述滤波后的射频定位信息的测距值用作滤波约束，获得轮式编码器辅助的惯性导航定位系统的惯性导航定位误差，称为最优估计的惯性导航定位误差；
[0117]
通过rts平滑器进一步平滑所述惯性导航定位误差；
[0118]
在惯导系统的定位结果中对平滑后的惯性导航定位误差进行补偿，以校正惯导系统的定位误差，得到最终的所述融合定位结果。
[0119]
具体地，本发明实施例中，在得到轮式编码器辅助的惯性导航系统与射频定位系统的位置信息后，通过融合定位的方式将上述信息进行融合，融合定位分为松组合融合定位与紧组合融合定位。松组合模式可以实现比单一导航技术更高的定位精度。本发明实施例中，射频的测距信息和编码器的测速信息可以抑制累积的ins定位系统的误差；即使射频定位系统由于测距异常而无法正常工作或者信号遮挡，紧组合的定位系统也可以计算准确的定位结果，所以本发明实施例中采用紧组合融合定位可用在复杂环境中也可以计算准确的定位结果。
[0120]
紧组合的定位系统的结构如图3所示。它主要包括两部分：ins(惯导)/轮式编码器融合定位系统和射频定位系统。ins/编码器融合定位系统包括设置在机器人平台的惯性导航系统以及设置在机器人车轮上的轮式编码器；通过惯性导航系统测量机器人的加速度和角速度，以获得机器人的姿态、速度和位置惯性信息；通过设置在机器人行驶车轮上的编码器，测量车轮的线性速度，以构建的轮式编码器辅助的惯性导航定位系统(即ins/编码器融合定位系统)；并设置一误差状态卡尔曼滤波eskf；采用所述误差状态卡尔曼滤波eskf、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息。本发明的ins/编码器融合定位系统通过eskf获得最优估计。
[0121]
并设置通过基于射频测距的定位系统，获取射频定位信息，并设置采用自适应扩展卡尔曼滤波器对获取的射频定位信息进行滤波，以构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息。本发明中通过射频定位系统根据测距信息应用ekf，以获得定位和测距值。
[0122]
基于上述结果，将射频测距值用作滤波约束，以获得ins定位误差的最优估计。通
过rts(rauch-tung-striebel)平滑器进一步平滑ins定位误差。最后，在惯导系统的定位结果中对平滑后的误差进行补偿，以校正惯导系统的定位误差。即本发明实施例中获取所述轮式编码器辅助的惯性导航定位系统融合处理的第一融合位置信息；以及获取基于自适应的扩展卡尔曼滤波的射频定位系统滤波后的射频定位信息；基于所述第一融合位置信息，以及所述射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果。
[0123]
具体地，本发明中关于紧组合的定位系统中的扩展卡尔曼滤波分为时间更新与测量值更新。
[0124]
在时间更新阶段，本发明定义ins/轮式编码器定位系统的位置和速度误差作为状态向量xk，可由以下公式表示：
[0125]
xk＝[δp
x,k δv
x,k δp
y,k δv
y,k
]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0126]
系统状态方程可表示为：
[0127]
x
k+1
＝f
tight
xk+w
tight
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0128]
其中，f
tight
是状态转移矩阵，w
tight
是过程噪声向量。
[0129]
机器人与第i个基站之间的距离可表示为：
[0130][0131]
其中，表示导航坐标系下的真实位置，(xi,yi,zi)表示导航坐标系下的第i个基站的位置。
[0132]
ins和基站之间的相关距离可以用以下公式表示：
[0133][0134]
其中，表示ins/轮式编码器定位系统计算的位置。
[0135]
在测量值更新阶段，测量值模型zk可以用以下公式表示：
[0136][0137]
其中，vk是观测噪声。
[0138]
与ekf只能利用过去和当前信息来估计当前状态相比，平滑算法可以利用未来信息来估计当前状态，从而显著提高状态估计的准确性。rts平滑器可由以下公式表示：
[0139][0140]
δx
s,k
＝δxk+k
s,k
(δx
s,k+1-δx
k+1,k
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(24)
[0141][0142]
其中，k
s,k
表示滤波增益，δx
s,k
表示平滑后的误差状态向量，p
s,k
表示误差协方差矩阵。
[0143]
由上可见，本发明实施例提出的紧组合的定位系统模型，融合射频信息、惯性信息
与轮式编码器信息，在可视距环境与非可视距环境下维持高精度的定位。所提出的模型不仅可适用于基于智能手机的机器人平台，也可适用于基于其他终端的机器人平台。本发明利用imu和轮式编码器对室内机器人进行定位，可以有效抑制惯性导航系统的累积误差，保持高精度定位。
[0144]
示例性设备
[0145]
如图4中所示，本发明实施例提供一种基于多源融合的机器人定位装置，该装置包括：
[0146]
惯导轮式编码器处理模块510，用于通过构建的轮式编码器辅助的惯性导航定位系统获取位置惯性信息和线性速度信息，采用误差状态卡尔曼滤波、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息；
[0147]
射频定位处理模块520，用于通过构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息；
[0148]
融合处理模块530，用于基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果，具体如上所述。
[0149]
基于上述实施例，本发明还提供了一种智能终端，其原理框图可以如图5所示。该智能终端包括通过系统总线连接的处理器、存储器、网络接口、显示屏、传感器。其中，该智能终端的处理器用于提供计算和控制能力。该智能终端的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该智能终端的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种基于多源融合的机器人定位方法。该智能终端的显示屏可以是液晶显示屏或者电子墨水显示屏。
[0150]
本领域技术人员可以理解，图5中示出的原理框图，仅仅是与本发明方案相关的部分结构的框图，并不构成对本发明方案所应用于其上的智能终端的限定，具体的智能终端可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
[0151]
在一个实施例中，提供了一种智能终端，包括有存储器，以及一个或者一个以上的程序，其中一个或者一个以上程序存储于存储器中，且经配置以由一个或者一个以上处理器执行所述一个或者一个以上程序包含用于进行以下操作的指令：
[0152]
通过构建的轮式编码器辅助的惯性导航定位系统获取位置惯性信息和线性速度信息，采用误差状态卡尔曼滤波、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息；
[0153]
通过构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息；
[0154]
基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果。
[0155]
所述的基于多源融合的机器人定位方法，其中，所述通过构建的轮式编码器辅助
的惯性导航定位系统获取位置惯性信息和线性速度信息，采用误差状态卡尔曼滤波、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息的步骤包括：
[0156]
通过设置在机器人上的惯性导航系统，测量机器人的加速度和角速度，以获得机器人的姿态、速度和位置惯性信息；以及通过设置在机器人行驶车轮上的编码器，测量车轮的线性速度，以构建轮式编码器辅助的惯性导航定位系统；
[0157]
设置一误差状态卡尔曼滤波eskf；
[0158]
采用所述误差状态卡尔曼滤波eskf、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息。
[0159]
所述的基于多源融合的机器人定位方法，其中，所述通过构建的轮式编码器辅助的惯性导航定位系统获取位置惯性信息和线性速度信息，采用误差状态卡尔曼滤波、对来自惯性导航定位系统的位置惯性信息和来自所述轮式编码器测量的线性速度信息进行融合，得到第一融合位置信息的步骤之前包括：
[0160]
预先在机器人上设置的惯性导航系统，用于测量机器人的加速度和角速度，以获得机器人的姿态、速度和位置惯性信息；
[0161]
以及预先在机器人行驶的车轮上设置编码器，用于测量车轮的线性速度。
[0162]
所述的基于多源融合的机器人定位方法，其中，所述通过构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息的步骤包括：
[0163]
设置通过基于射频测距的定位系统，获取射频定位信息，并设置采用自适应扩展卡尔曼滤波器对获取的射频定位信息进行滤波，以构建基于自适应的扩展卡尔曼滤波的射频定位系统，获得滤波后的射频定位信息。
[0164]
所述的基于多源融合的机器人定位方法，其中，所述基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果的步骤包括：
[0165]
获取所述轮式编码器辅助的惯性导航定位系统融合处理的第一融合位置信息；
[0166]
获取基于自适应的扩展卡尔曼滤波的射频定位系统滤波后的射频定位信息；
[0167]
基于所述第一融合位置信息，以及所述射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果。
[0168]
所述的基于多源融合的机器人定位方法，其中，所述第一融合位置信息，为位置信息的最优估计；所述滤波后的射频定位信息包括定位信息和测距值信息。
[0169]
所述的基于多源融合的机器人定位方法，其中，所述基于所述轮式编码器辅助的惯性导航定位系统得到的第一融合位置信息，以及所述基于自适应的扩展卡尔曼滤波的射频定位系统获得的射频定位信息，通过融合定位的方式将所述第一融合位置信息和所述射频定位信息进行融合定位，并输出融合定位结果的步骤包括：
[0170]
将所述滤波后的射频定位信息的测距值用作滤波约束，获得轮式编码器辅助的惯性导航定位系统的惯性导航定位误差，称为最优估计的惯性导航定位误差；
[0171]
通过rts平滑器进一步平滑所述惯性导航定位误差；
[0172]
在惯导系统的定位结果中对平滑后的惯性导航定位误差进行补偿，以校正惯导系
统的定位误差，得到最终的所述融合定位结果。
[0173]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本发明所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(rom)、可编程rom(prom)、电可编程rom(eprom)、电可擦除可编程rom(eeprom)或闪存。易失性存储器可包括随机存取存储器(ram)或者外部高速缓冲存储器。作为说明而非局限，ram以多种形式可得，诸如静态ram(sram)、动态ram(dram)、同步dram(sdram)、双数据率sdram(ddrsdram)、增强型sdram(esdram)、同步链路(synchlink)dram(sldram)、存储器总线(rambus)直接ram(rdram)、直接存储器总线动态ram(drdram)、以及存储器总线动态ram(rdram)等。
[0174]
综上所述，本发明公开了一种基于多源融合的机器人定位方法、智能终端及存储介质，本发明可适用基于智能手机的机器人平台，也可适用于基于其他终端的机器人平台。本发明首先构建轮式编码器辅助的惯性导航定位系统，以抑制惯性导航的长时间累积误差问题。在此基础上，构建基于自适应的扩展卡尔曼滤波的射频定位系统，以融合射频测距信息。同时，为了克服轮式编码器辅助的惯性导航定位系统的累积误差问题与基于射频测距信息的定位系统的不稳定问题，本发明实现一个基于惯导、轮式编码器、射频信号的紧组合导航系统。本发明利用imu和轮式编码器对室内机器人进行定位，可以有效抑制惯性导航系统的累积误差，保持高精度定位。
[0175]
应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。