一种基于扩展卡尔曼滤波的激光雷达和UWB组合定位方法及系统

一种基于扩展卡尔曼滤波的激光雷达和uwb组合定位方法及系统
技术领域
1.本发明涉及机器人环境感知技术领域，尤其涉及一种基于扩展卡尔曼滤波的激光雷达和uwb组合定位方法及系统。


背景技术：

2.基于激光雷达的定位方法作为agv定位方式的一种，近年来被广泛应用于仓储物流等行业。基于激光雷达的定位主要依靠的是具有高反射强度的反光板与三点定位法来实现的。该方法通过将当前位置激光雷达扫描到的反光板与事先存储的全局地图中的反光板进行匹配，由于环境内所有反光板坐标已知，就可以根据匹配结果获得当前位置激光雷达扫描到的反光板的全局坐标，当获得的全局坐标至少有三个时，就可以根据激光雷达与每个反光板的距离通过三点定位法求得激光雷达当前的全局坐标。
3.但是，这种定位算法对信标的布置与检测有很高要求，如场景内的人工信标在空间上不可形成对称或局部相似、布置的数量在密度上要保证agv在任意时刻都能检测到至少三个信标，从而满足三点定位法的最低要求等。然而，在实际应用中如果不能保证达到这些要求，agv在定位或导航过程中就会出现误匹配、卡顿或轨迹缺失等现象，从而降低精度。因此这种定位方法存在一定的局限性。为了在控制成本的前提下，尽可能提高agv在复杂环境下的定位精度，因此，如何对传统激光雷达定位算法进行硬件及软件算法层面上的改进，以获得更好的定位效果，成为了需要研究的课题。


技术实现要素：

4.本发明的实施例提供一种基于扩展卡尔曼滤波的激光雷达和uwb组合定位方法及系统，能够进一步提高agv在复杂环境下的定位精度。
5.为达到上述目的，本发明的实施例采用如下技术方案：
6.第一方面，本发明的实施例提供的方法，包括：
7.s1、建立agv的组合定位模型，所述组合定位模型包括：agv的状态模型和利用所述状态模型建立的量测模型，其中，在agv活动的场地中部署有反光板，在每一个反光板上粘贴有一个uwb定位基站，uwb天线和激光雷达组成了agv的定位系统；
8.s2、通过所述uwb天线接收uwb脉冲信号，同时通过所述激光雷达接收反光板反射回来的激光信号，根据所接收到的信号得到agv的uwb定位信息和激光定位信息，其中，在agv运行的过程中周期性得执行步骤s2；
9.s3、将所得到的uwb定位信息和激光定位信息，输入所述组合定位模型，并更新观测矩阵；
10.s4、利用更新后的观测矩阵，通过扩展卡尔曼滤波进行迭代运算，并利用迭代运算的结果对agv进行状态预测和量测更新。
11.第二方面，本发明的实施例提供的定位系统，在agv活动的场地中部署有反光板，
在每一个反光板上粘贴有一个uwb定位基站，uwb天线和激光雷达组成了agv的定位系统并安装在agv上；所述uwb天线，用于接收uwb脉冲信号；所述激光雷达用于接收反光板反射回来的激光信号；agv上安装的计算模块，用于根据所接收到的信号得到agv的uwb定位信息和激光定位信息。
12.agv上安装的计算模块中导入了agv的组合定位模型，所述组合定位模型包括：agv的状态模型和利用所述状态模型建立的量测模型，其中，在agv活动的场地中部署有反光板，在每一个反光板上粘贴有一个uwb定位基站，uwb天线和激光雷达组成了agv的定位系统；所述计算模块，具体用于将所得到的uwb定位信息和激光定位信息，输入所述组合定位模型，并更新观测矩阵；之后利用更新后的观测矩阵，通过扩展卡尔曼滤波进行迭代运算，并利用迭代运算的结果对agv进行状态预测和量测更新。
13.本发明实施例提供的基于扩展卡尔曼滤波的激光雷达和uwb组合定位方法及系统，利用uwb和激光雷达组成双传感器组合定位系统，在观测方程中加入了uwb/激光雷达与某一个uwb之间的距离约束，因此即便某一个时刻激光雷达扫描获得的反光板中存在三角形与全局地图或局部地图中另一个三角形全等或相似的情况，但前后时刻uwb/激光雷达与某一个相同的uwb之间的距离却是不一样的，因此从理论上能够进一步降低系统发生误匹配的风险，从而进一步提高agv在复杂环境下的定位精度。
附图说明
14.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
15.图1-5为本发明实施例提供的具体实例中的场景示意图；
16.图6为本发明实施例提供的具体实例中的执行流程示意图；
17.图7为本发明实施例提供的系统架构示意图；
18.图8为本发明实施例提供的方法流程示意图。
具体实施方式
19.为使本领域技术人员更好地理解本发明的技术方案，下面结合附图和具体实施方式对本发明作进一步详细描述。下文中将详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的任一单元和全部组合。本技术领域
技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。
20.本发明实施例提供一种基于扩展卡尔曼滤波的激光雷达和uwb组合定位方法，如图8所示，包括：
21.s1、建立agv的组合定位模型，所述组合定位模型包括：agv的状态模型和利用所述状态模型建立的量测模型，其中，在agv活动的场地中部署有反光板，在每一个反光板上粘贴有一个uwb定位基站，uwb天线和激光雷达组成了agv的定位系统。本实施例中，通过引入uwb(ultra wide band,超宽带)与激光雷达组成双传感器组合定位系统，利用扩展卡尔曼滤波算法对激光雷达的定位结果进行状态约束和预测修正。
22.s2、通过所述uwb天线接收uwb脉冲信号，同时通过所述激光雷达接收反光板反射回来的激光信号，根据所接收到的信号得到agv的uwb定位信息和激光定位信息，其中，在agv运行的过程中周期性得执行步骤s2；
23.s3、将所得到的uwb定位信息和激光定位信息，输入所述组合定位模型，并更新观测矩阵；
24.s4、利用更新后的观测矩阵，通过扩展卡尔曼滤波进行迭代运算，并利用迭代运算的结果对agv进行状态预测和量测更新。
25.本实施例中，在s1中，包括：建立agv的状态模型xk＝akx
k-1
+w
k-1
作为uwb/激光雷达组合定位系统的状态方程，ak为定位系统的状态转移矩阵，xk为状态向量，w
k-1
为过程噪声，k表示时刻；其中，表示时刻；其中，和为所述定位系统在k时刻相对于k-1时刻在x和y方向的坐标变化量，和为所述定位系统在k时刻相对于k-1时刻在x和y方向的速度变化量。具体的，在s2中，所述通过所述激光雷达接收反光板反射回来的激光信号，包括：所述激光雷达发射激光信号扫描agv周围部署的反光板，并通过三角定位法得到所述激光雷达在第k时刻的坐标(xk,yk)，xk、yk分别表示两个坐标轴上的坐标参数，k表示时刻。例如：主传感器为激光雷达，副传感器为uwb，激光雷达负责本系统的主要定位任务，uwb负责在激光雷达出现失效情况时进行校正。因此，激光雷达扫描获取周围反光板坐标并通过三角定位法得出第k时刻激光雷达的坐标(xk,yk)(具体来说，这个过程可以通过导入至激光雷达内部的驱动程序实现)前一时刻激光雷达得出的坐标为(x
k-1
,y
k-1
)所以
26.中的：
[0027][0028][0029][0030]
这些参数都是由激光雷达得到的坐标数据求得
的。uwb由于安装在各个反光板上，是提前布置在环境中的，因此在本专利中这些uwb的坐标均已知，如第i个uwb的坐标为
[0031]
具体的，w
k-1
满足均值为0且方差为的高斯分布；步骤s2的周期为δt,则：所述利用所述状态模型建立的量测模型，包括：yk＝ckxk+vk，yk为输出矩阵，vk为量测噪声矩阵，ck为观测矩阵；
[0032]
其中，ξ
k，i
为在k时刻经过泰勒展开后的观测噪声，ξ
k，k-1
为在k-1和k时刻之间经过泰勒展开后的观测噪声，具体来说，ξ
k，i
为激光雷达/uwb系统在k时刻经过泰勒展开后的观测噪声；ξ
k，k-1
为激光雷达/uwb系统在k-1和k时刻之间经过泰勒展开后的观测噪声，可以理解为ξ
k，i
是相同时间内激光雷达/uwb系统之间的观测噪声，ξ
k，k-1
是激光雷达/uwb系统在不同时刻之间的观测噪声。i表示反光板的编号，d
k，i
表示在k时刻所述定位系统与第i个反光板上的uwb定位基站之间的距离，d
k，k-1
表示所述定位系统在k-1时刻所在位置，与在k时刻所在位置之间的距离，d
k0，i
表示k0时刻(即每一次计算过程的初始时刻)uwb/激光雷达系统与第i个反光板上的uwb之间的距离，d
k0，k-1
表示k0时刻uwb/激光雷达系统与k时刻之间的距离；
[0033]
x
k0
表示每一次计算过程的初始时刻系统的x坐标，x
k-1
表示k-1时刻系统的x坐标，y
k0
表示每一次计算过程的初始时刻系统的y坐标，y
k-1
表示k-1时刻系统的y坐标，表示第i个uwb的x坐标，表示第i个uwb的y坐标。
[0034]
在k时刻所述定位系统与第i个反光板上的uwb定位基站之间的距离所述定位系统在k-1时刻所在位置，与在k时刻所在位置之间的距离xk表示k时刻系统的x坐标，yk表示k时刻系统的y坐标，v
k，i
为激光雷达/uwb系统在k时刻的观测噪声(泰勒展开前的，与ξ
k，i
区分开)，v
k，k-1
为激光雷达/uwb系统在k-1和k时刻之间的观测噪声(也是泰勒展开前的，与ξ
k，k-1
区分开)。
[0035]
所述通过扩展卡尔曼滤波进行迭代运算，包括：通过递推公式进行迭代运算，其中，ekf(扩展卡尔曼滤波器)算法的递推公式为：
[0036][0037][0038][0039][0040]
pk＝(i-k
kck
)p
′k，且进行迭代运算时取初值和e表示求解数学期望(均值)运算符，如对x0求均值就是e[x0]；x0表示初始时刻系统的状态量；表示系统的状态初值；p0为系统的初始状态均方误差；为校正前状态量的估计值；为k时刻校正后状态量的估计值；为k-1时刻校正后的状态量的估计值；kk表示k时刻系统的卡尔曼增益；pk为k时刻校正后的状态量均方误差；p
k-1
为k-1时刻校正后的状态量均方误差；wk为过程噪声泰勒展开权重矩阵；q
k-1
为过程噪声方差；r
′k为校正前的状态量均方误差；rk为观测噪声方差。
[0041]
所述利用迭代运算的结果对agv进行状态预测和量测更新，包括：在每一次迭代中，通过所述迭代公式获取当前时刻的状态量的先验估计和先验方差，其中，agv的坐标作为输入所述迭代公式的状态量；利用所述先验方差获取当前时刻的卡尔曼增益，再利用所述先验估计和所述卡尔曼增益获取后验估计。具体来说，ekf算法的迭代运算过程包括了状态预测和量测更新两部分：首先系统状态量xk的初值取为初始状态均方误差为
[0042]
ekf公式如下：
[0043][0044][0045][0046][0047]
pk＝(i-k
kck
)p
′kꢀꢀꢀ
(5)
[0048]
其中，为校正前状态量的估计值，即状态量的先验估计；为校正后状态量的估计值，即状态量的后验估计；p
′k为校正前的状态量均方误差，即先验方差；pk为校正后的状态量均方误差，即后验方差；q
k-1
为过程噪声方差；rk为观测噪声方差。确定和后，由式(1)和式(4)可以求得k＝1时刻状态量的先验估计和先验方差p
′1(前面的两次计算也就是所说的状态预测，预测的就是状态量的先验估计和先验方差)，之后将p
′1待入式(3)得到k＝1时刻的卡尔曼增益k1，有
了k1和式(2)可以求出k＝1时刻的即k＝1时刻校正后状态量的估计，也就是状态量的后验估计，有了k1和p
′1，根据式(5)可以求得k＝1时刻校正后的状态量均方误差p1，即后验方差，(这两次计算即我所说的量测更新，更新的就是状态量的后验估计和后验方差)后面的运算就是前面过程的不断迭代，k＝2时刻状态量的先验估计和先验方差通过状态预测的两次运算求得，求得的结果再代入量测更新的两次计算求出状态量在k＝2时刻的后验估计和后验方差，k＝3，4，
…
，n等都是这样不断迭代更新获得的，n为正整数。总之，状态量就是系统的坐标，通过ekf算法不断进行迭代运算，获取到每一个时刻的一个最优的值。
[0049]
通过上面五个迭代公式(1)～(5)对系统进行状态预测和量测更新。利用uwb和激光雷达组成双传感器组合定位系统，在观测方程中加入了uwb/激光雷达与某一个uwb之间的距离约束，因此即便某一个时刻激光雷达扫描获得的反光板中存在三角形与全局地图或局部地图中另一个三角形全等或相似的情况，但前后时刻uwb/激光雷达与某一个相同的uwb之间的距离却是不一样的，因此从理论上能够进一步降低系统发生误匹配的风险。通过扩展卡尔曼滤波算法对状态值进行预测。扩展卡尔曼滤波相较于传统卡尔曼滤波对非线性系统的预测效果更好，与uwb/激光雷达硬件相结合形成互补，及时修正定位结果，降低误差。例如：在实际应用中，在一个相对理想的室内环境(结构化环境，无强反射物体干扰，比如窗户，其他的反光条等，无大型金属物体对uwb通信产生干扰)中悬挂一定数量的反光板，反光板由半径固定的圆柱形筒状载体以及覆盖在上面的高反射率反光膜构成，如图1所示。在反光板上粘贴uwb模块并安装在环境中的各个合适位置。合适位置的定义为：反光条的高度与激光雷达的扫描器处于同一水平面上、反光板与反光板之间相对激光雷达扫描器之间的角度差大于0.6度、反光板与其他可能造成干扰的反光源之间的距离大于0.3m、反光板在空间中的布置结构不能形成完全对称的情况。具体如图2、3、4所示：之后，利用激光雷达对周围环境进行不间断的扫描，获取环境中布置的各个反光板的坐标，利用三角定位法计算得到定位系统在k时刻的全局坐标(xk,yk)并利用公式：
[0050][0051][0052][0053][0054]
从而求得定位系统在k时刻相对于k-1时刻x和y方向上的位移以及速度变化量。将以上四个参数组成定位系统的状态量其中：和为所述定位系统在k时刻相对于k-1时刻在x和y方向的坐标变化量，和为所述定位系统在k时刻相对于k-1时刻在x和y方向的速度变化量。设定位系统状态方程为：xk＝akx
k-1
+w
k-1
，其中，ak为定位系统的状态转移矩阵；xk为状态向量，w
k-1
为过程噪声，并假设其满足均
值为0，方差为的高斯分布；k表示时刻。状态转移矩阵δt为定位系统采样时间差。之后，建立系统量测方程模型。设k时刻定位系统与第i个反光板上的uwb之间的距离为：
[0055][0056]
设k-1时刻uwb/激光雷达系统与k时刻之间的距离为：
[0057][0058]
式中：xk表示k时刻系统的x坐标，yk表示k时刻系统的y坐标，v
k，i
为激光雷达/uwb系统在k时刻的观测噪声；v
k，k-1
为激光雷达/uwb系统在k-1和k时刻之间的观测噪声。由于式(6)(7)均为非线性函数，对其进行二元泰勒展开,并忽略二阶及以上的高次项：
[0059][0060][0061]
计算得：
[0062][0063][0064]
式中：ξ
k，i
为定位系统在k时刻经过泰勒展开后的观测噪声；ξ
k，k-1
为定位系统在k-1和k时刻之间经过泰勒展开后的观测噪声；d
k0，i
表示k0时刻(即每一次计算过程的初始时刻)定位系统与第i个反光板上的uwb之间的距离，d
k0，k-1
表示k0时刻定位系统与k时刻之间的距离；x
k0
表示初始时刻系统的x坐标，x
k-1
表示k-1时刻系统的x坐标，y
k0
表示初始时刻系统的y坐标，y
k-1
表示k-1时刻系统的y坐标，表示第i个uwb的x坐标，表示第i个uwb的y坐标。由此可以写出定位系统的量测方程为：yk＝ckxk+vk[0065]
式中：
[0066]
观测矩阵ck:
[0067][0068]
至此，本定位系统的数学模型，即状态方程和量测方程建立完毕。之后，利用扩展卡尔曼滤波算法(ekf)对本系统的状态量，即进行状态预
测和量测更新，获取到任意k时刻定位系统状态量的最优值。其具体步骤如下：
[0069]
ekf算法的迭代运算过程包括了状态预测和量测更新两部分：首先，定位系统状态量xk的初值取为初始状态均方误差为此时系统处于最初始的状态，无任何先验值。ekf公式如下：
[0070][0071][0072][0073][0074]
pk＝(i-k
kck
)p
′kꢀꢀꢀ
(5)
[0075]
式中：为校正前状态量的估计值，即状态量的先验估计；为校正后状态量的估计值，即状态量的后验估计；p
′k为校正前的状态量均方误差，即先验方差；pk为校正后的状态量均方误差，即后验方差；q
k-1
为过程噪声方差；rk为观测噪声方差。确定和后，由式(1)和式(4)可以求得k＝1时刻状态量的先验估计和先验方差p
′1(前面的两次计算也就是所说的状态预测，预测的就是状态量的先验估计和先验方差)，之后将p
′1待入式(3)得到k＝1时刻的卡尔曼增益k1，有了k1和式(2)可以求出k＝1时刻的即k＝1时刻校正后状态量的估计，也就是状态量的后验估计，有了k1和p
′1，根据式(5)可以求得k＝1时刻校正后的状态量均方误差p1，即后验方差，(这两次计算即量测更新，更新的就是状态量的后验估计和后验方差)后面的运算就是前面过程的不断迭代。需要指出的是，k时刻ekf算法通过量测更新求得的状态量的后验估计值在迭代运算过程中均视为下一时刻(k+1时刻)运算过程中的状态量的先验估计值，每一次计算过程的结果反过来更新观测矩阵ck。经过不断的迭代更新获取每一时刻定位系统状态量的最优值，进而求得定位系统在每一时刻的定位结果。通过式(2)(3)和(5)可知，ekf算法的卡尔曼增益kk、校正后状态量的估计值和校正后的状态量均方误差pk均和观测矩阵ck相关，而观测矩阵ck的数学模型是根据激光雷达与uwb之间的距离方程等“约束项”建立的，因此该数学模型将激光雷达与uwb之间的距离作为约束部分耦合到了整个系统的量测方程中，并在ekf迭代过程中不断约束着最终运算得到的结果，从而达到对系统定位结果进行约束优化的效果，具体流程可以如图6所示。
[0076]
本实施例中还提供一种基于扩展卡尔曼滤波的激光雷达和uwb组合定位系统，如图7所示的，在agv活动的场地中部署有反光板，在每一个反光板上粘贴有一个uwb定位基站，uwb天线和激光雷达组成了agv的定位系统并安装在agv上；
[0077]
所述uwb天线，用于接收uwb脉冲信号；
[0078]
所述激光雷达用于接收反光板反射回来的激光信号；
[0079]
agv上安装的计算模块，用于根据所接收到的信号得到agv的uwb定位信息和激光定位信息。
[0080]
具体的，agv上安装的计算模块中导入了agv的组合定位模型，所述组合定位模型包括：agv的状态模型和利用所述状态模型建立的量测模型，其中，在agv活动的场地中部署有反光板，在每一个反光板上粘贴有一个uwb定位基站，uwb天线和激光雷达组成了agv的定位系统；
[0081]
所述计算模块，具体用于将所得到的uwb定位信息和激光定位信息，输入所述组合定位模型，并更新观测矩阵；之后利用更新后的观测矩阵，通过扩展卡尔曼滤波进行迭代运算，并利用迭代运算的结果对agv进行状态预测和量测更新。
[0082]
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于设备实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。