一种不依赖GNSS的无人车自主定位与环境探索方法

一种不依赖gnss的无人车自主定位与环境探索方法
技术领域
1.本发明涉及无人车导航与控制技术领域，特别是指一种不依赖gnss的无人车自主定位与环境探索方法。


背景技术：

2.随着信息技术、计算机技术、先进制造技术等高新技术的迅猛发展，无人车技术得到了快速发展，无人车能够替代人类在危险环境中作业，例如：矿井探测、深海探索、危险环境中的救援活动等，为实现未知环境探索，其中，定位与地图构建(simultaneous localization and mapping，slam)是最常用的关键技术之一，尤其是在全球卫星导航系统(global navigation satellite system，gnss)不可用的情况下。传统的被动slam是指无人车仅依靠自身配备的传感器获得环境信息以构建环境地图，并在该地图中实现精确定位，无人车需要在遥控作用下移动，在地图构建精度、计算复杂度、数据关联以及估计的一致性等方面存在一定问题。此外，实际应用中，无人车无法在进入未知环境前就规划出一条运动路径，但是，实际应用中常常需要无人车在没有人为干预的情况下根据传感器获得的环境信息和需要完成的任务自主规划运动路径，最终完成自身定位与地图构建。为解决这些问题，需要将无人车地图构建、定位和路径规划这三个基本任务结合起来，即主动slam。相比被动slam，主动slam研究更具有现实意义。
3.从已有的研究成果看，人们通常将主动slam中的路径规划问题转化成带约束的优化问题，即建立以无人车轨迹为自变量的优化目标项，通过极小化目标项得到无人车的最优轨迹。已有的主动slam方法主要解决如下两个问题：
4.1)如何快速覆盖尽可能大的环境面积；
5.2)如何通过对特征的合理选择降低定位和建图的不确定性。
6.然而，实际中主动slam的优化目标项之间往往是相互制约、相互矛盾的。例如，当无人车以提高定位与建图精度为主要目标时，无人车往往倾向于在已探索区域重复探索，当无人车以扩大探索区域为主要目标时，建图不确定性就会变大。而现有的研究中多采用多目标优化策略，但多目标优化策略存在计算量大，易陷入局部最优解等问题。


技术实现要素：

7.本发明实施例提供了不依赖gnss的无人车自主定位与环境探索方法，能够同时提高地图构建精度以及未知区域覆盖效率。所述技术方案如下：
8.本发明实施例提供了一种不依赖gnss的无人车自主定位与环境探索方法，包括：
9.基于无人车自主环境探索机理，建立无人车自主定位与环境探索模型；
10.根据地图构建精度要求，基于建立的无人车自主定位与环境探索模型，以极小化建图不确定性为第一优化目标项；
11.根据地图覆盖要求，以极大化环境探索增益为第二优化目标项，引导无人车不断向未探索区域运动；
12.采用粒子群优化算法对第一优化目标项和第二优化目标项进行综合求解，得到无人车最优的控制输入。
13.进一步地，所述无人车自主定位与环境探索模型包括：运动学模型以及观测模型；其中，所述运动学模型表示为：
[0014][0015]
其中，x
k
＝(x
′ꢀ
y
′ꢀ
θ
′
)
t
，x
k
表示无人车在第k步时的位姿，(x
′ꢀ
y
′ꢀ
θ
′
)表示无人车在第k步时的横坐标、纵坐标、车头与其横轴形成的角度，上标
t
表示矩阵转置；x
k
‑1＝(x y θ)
t
，x
k
‑1表示无人车在第k
‑
1步时的位姿，(x y θ)表示无人车在第k
‑
1步时的横坐标、纵坐标、车头与其横轴形成的角度；分别表示无人车运动受噪声影响时，实际输入的无人车速度、角速度；为的简写形式，分别表示平移噪声、旋转噪声，v
k
‑1、ω
k
‑1分别表示理论上输入的无人车的速度、角速度，u
k
‑1＝(v
k
‑
1 ω
k
‑1)
t
，u
k
‑1表示无人车在第k
‑
1步时的控制输入。
[0016]
进一步地，实际输入的无人车的速度、角速度表示为：
[0017][0018]
进一步地，所述观测模型表示为：
[0019][0020]
其中，p{z
k
|x
k
,m}表示无人车在当前位姿x
k
和已知之前运动观测的前提下，观测特征的概率，所述特征指在无人车探索的环境中的静止物体的位置；m表示已构建出的环境地图；表示无人车在当前位姿下观测到的第j个特征；z
k
表示无人车在当前位姿观测到的j个特征的集合；z
max
表示的最大值；服从均值为标准偏差为σ
hit
的正态分布
[0021]
进一步地，所述根据地图构建精度要求，基于建立的无人车自主定位与环境探索模型，以极小化建图不确定性为第一优化目标项包括：
[0022]
在模型预测控制框架中，通过建立的无人车自主定位与环境探索模型实现l步无人车位姿观测，其中，在位姿观测时，以极小化建图不确定性为第一优化目标项：
[0023]
[0024][0025]
其中，f
a
(u
k:k+l
‑1)表示为了提高地图构建精度、无人车定位精度的目标函数；u
k:k+l
‑1＝(u
k
,u
k+1
,...,u
k+l
‑1)，u
k:k+l
‑1表示无人车在第k步至第k+l
‑
1步的控制输入；表示无人车从第k步到第k+l步中所有最可能位姿；i
k+l
表示维度为k+l的费歇耳信息矩阵；f
v
(
·
)是运动学模型的简写形式。
[0026]
进一步地，第二优化目标项表示为：
[0027][0028][0029]
其中，f
c
(u
k:k+l
‑1)表示环境探索增益；o表示环境中所有特征的集合；o
det
是o的子集，o
det
表示无人车已经观测到的所有特征的集合；p表示二维积分变量；c(k+l
‑
1)表示无人车在第k+l
‑
1步时需探索的环境地图；v(k)表示无人车在第k步时已探索过的环境地图；x
min
、x
max
分别表示x的最小值和最大值；y
min
、y
max
分别表示y的最小值和最大值。
[0030]
进一步地，在采用粒子群优化算法对第一优化目标项和第二优化目标项进行综合求解时，采用的适应度函数为：
[0031][0032]
其中，j
fit
表示适应度值，λ
i
为权重系数，i＝1,2，j1为极小化建图不确定性，j2为最大化环境探索增益，即：
[0033][0034]
进一步地，适应度函数中权重系数λ
i
的取值采用序关系分析法确定。
[0035]
本发明实施例提供的技术方案带来的有益效果至少包括：
[0036]
本发明实施例中，结合实际中gnss不可用的情况，基于主动slam方法同时规划无人车运动轨迹并构建环境地图，能够在无人员干预的情况下进行自主环境探索，为同时提高地图构建精度以及未知区域覆盖效率，采用粒子群优化算法对构建的多目标优化问题进行综合求解，算法收敛速度快，计算高效，地图构建准确性高且用时短，能够避免陷入局部最优解。
附图说明
[0037]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0038]
图1为本发明实施例提供的不依赖gnss的无人车自主定位与环境探索方法的流程示意图；
[0039]
图2为本发明实施例提供的基于pso算法的多目标优化问题求解流程示意图。
具体实施方式
[0040]
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。
[0041]
如图1所示，本发明实施例提供了一种不依赖gnss的无人车自主定位与环境探索方法，包括：
[0042]
s101，基于无人车自主环境探索机理，建立无人车自主定位与环境探索模型，实现无人车环境探索机理建模；
[0043]
本实施例中，假设在未知环境中，无人车以恒定速度v进行探索，首先令无人车在第k
‑
1步时的控制输入u
k
‑1＝(v
k
‑
1 ω
k
‑1)
t
，则无人车运动轨迹的曲率半径其中，v
k
‑1表示无人车在第k
‑
1步时的速度，ω
k
‑1表示无人车在第k步时的角速度，上标
t
表示矩阵的转置。
[0044]
假设x
k
‑1＝(x y θ)
t
为无人车在k
‑
1步时的位姿，(x y θ)表示无人车在第k
‑
1步时的横坐标、纵坐标、车头与其横轴形成的角度，并假定在
△
t的时间内(v
k
‑
1 ω
k
‑1)
t
保持恒定，得到运动轨迹的曲率中心(x
c
、y
c
)位于
[0045][0046]
其中，
△
t表示无人车两步之间的间隔时间；
[0047]
运动
△
t时间后，理想情况下，无人车将位于x
k
＝(x
′ꢀ
y
′ꢀ
θ
′
)
t
，其中，(x
′ꢀ
y
′ꢀ
θ
′
)可以表示为：
[0048][0049]
其中，x
k
表示无人车在第k步时的位姿，(x
′
y
′
θ
′
)表示无人车在第k步时的横坐标、
纵坐标、车头与其横轴形成的角度；
[0050]
当无人车运动受噪声影响时，实际的控制输入可表示为
[0051][0052]
其中，分别表示无人车运动受噪声影响时，实际输入的无人车的速度、角速度，也就是说，v
k
‑1、ω
k
‑1是无人车运动没有受到噪声影响时的理论上的控制输入；和分别表示平移噪声和旋转噪声。
[0053]
本实施例中，在控制输入u
k
‑1＝(v
k
‑
1 ω
k
‑1)
t
的作用下，有
[0054][0055]
进一步，考虑更准确的噪声模型：
[0056][0057]
其中，为的简写形式，无具体的物理含义；
[0058]
则式(4)所示的运动学模型可更新为：
[0059][0060]
本实施例中，无人车的观测模型可示为：
[0061][0062]
其中，p{z
k
|x
k
,m}表示无人车在当前位姿x
k
和已知之前运动观测的前提下，观测特征的概率，这些特征可能是无人车之前已经观测到的，也可能是这次新观测到的，所述特征指在无人车探索环境中的静止物体的位置，例如，树木，房屋等，特征的意义在与让无人车认识这个环境，构建出这个环境的地图，并在构建地图的过程中定位自身的位置；m表示已经构建出的环境地图；z
k
表示无人车在当前位姿观测到的j个特征的集合；表示无人车在当前位姿观测到的第j个特征，的概率分布符合正态分布，表达式为：
[0063][0064]
式中，η表示归一化因子，z
max
表示的最大值，服从均值为标准偏差为σ
hit
的正态分布，具体表示为的正态分布，具体表示为可由x
k
和m经过射线投射得到，标准偏差σ
hit
是测量模型的一个固有的噪声参数；的表达式为：
[0065][0066]
归一化因子η表示为：
[0067][0068]
综上所述，测量噪声的观测模型可表示为：
[0069][0070]
其中，
[0071]
本实施例中，式(6)所示的运动学模型和式(10)所示的观测模型共同构成式(11)所示的无人车自主定位与环境探索模型：
[0072][0073]
s102，根据地图构建精度要求，基于建立的无人车自主定位与环境探索模型，以极小化建图不确定性为第一优化目标项；
[0074]
在模型预测控制(model predictive control，mpc)框架中，通过建立的无人车自主定位与环境探索模型实现l步无人车位姿观测并对建图不确定性进行描述，在[k,k+l]范围内，优化目标项j
k
(u
k:k+l
‑1)可表示为：
[0075][0076]
其中，gb(x
k+l
)为高斯函数，即数，即表示高斯函数，n表示正态分布，表示正态分布中的均值，表示正态分布中的方差，x
k+l
表示无人车从第k步到第k+l步中的所有可能位姿(注意是所有可能位姿，不是地图中的一系列点，而是一系列椭圆区域)，表示无人车从第k步到第k+l步中所有最可能位姿(是一系列点)；f
j
(gb(x
k+l
))表示高斯函数的外包函数；i
k+l
表示费歇耳信息矩阵，矩阵的下标
k+l
表示矩阵的维度，物理含义为无人车已行驶步数与预测步数的和；det表示行列式；b(k,l)＝z
1：k
,u
0:k
‑1,u
k:k+l
‑1表示k+l个控制输入(u
0:k
‑1,u
k:k+l
‑1)和无人车在前k步(具体指：第1步到第k步)中观测到的所有特征(z
1：k
)；z
k+1：k+l
表示无人车从k+1步到第k+l步观测到的所有特征。p(x
k+l
|b(k,l),z
k+1；k+l
)＝p(x
k+l
|z
1:k
,u
0:k
‑1,z
k+1:k+l
,u
k:k+l
‑1)，p(x
k+l
|b(k,l),z
k+1；k+l
)和p(x
k+l
|z
1:k
,u
0:k
‑1,z
k+1:k+l
,u
k:k+l
‑1)都表示在已知k+l个控制输入(u
0:k
‑1,u
k:k+l
‑1)和观测的前提下，无人车位姿出现在某个点的概率。
[0077]
本实施例中，应用贝叶斯定理，由(12)可得：
[0078][0079]
其中，p(z
k+1；k+l
|x
k+l
,b(k,l))表示在已知k+l个控制输入(u
0:k
‑1,u
k:k+l
‑1)和k+l个无人车位姿的前提下，无人车观测到特征，特征坐标是某个点的概率；p(x
k+l
|b(k,l))表示在已知k+l个控制输入(u
0:k
‑1,u
k:k+l
‑1)和k个无人车位姿的前提下，无人车位姿出现在某个点的概率；p(z
k+1；k+l
|b(k,l))表示在已知k+l个控制输入(u
0:k
‑1,u
k:k+l
‑1)和k个无人车位姿的前提下，无人车观测到特征，特征坐标是某个点的概率；
[0080]
因概率p(z
k+1:k+l
|b(k,l))是未知的，将(13)式改写为：
[0081][0082]
应用马尔可夫定理可得预测状态向量
[0083][0084]
综上，式(12)所示优化问题可转换为：
[0085][0086]
其中，f
a
(u
k:k+l
‑1)表示为了提高地图构建精度、无人车定位精度的目标函数；u
k:k+l
‑1＝(u
k
,u
k+1
,...,u
k+l
‑1)，u
k:k+l
‑1表示无人车在第k步至第k+l
‑
1步的控制输入；f
v
(
·
)是运动学模型的简写形式，具体指：
[0087]
s103，根据地图覆盖要求，以极大化环境探索增益为第二优化目标项，引导无人车不断向未探索区域运动；
[0088]
本实施例中，在mpc框架中，令c(k)和v(k)分别表示无人车在第k步时需探索的环境地图和已探索过的环境地图。为了极大化v(k)，即让无人车在最短时间内完成覆盖面积最大。为同时满足探索时间最短和覆盖面积最大这两个指标，可通过极大化如下环境探索增益：
[0089][0090]
其中，o表示环境中所有特征的集合；o
det
是o的子集，o
det
表示无人车已经观测到的所有特征的集合；p表示二维积分变量；c(k+l
‑
1)表示无人车在第k+l
‑
1步时需探索的环境地图；v(k)表示无人车在第k步时已探索过的环境地图；x
min
、x
max
分别表示x的最小值和最大值；y
min
、y
max
分别表示y的最小值和最大值。
[0091]
式(17)所示的极大化问题等价于：
[0092][0093]
其中，f
c
(u
k:k+l
‑1)表示环境探索增益。
[0094]
s104，采用粒子群优化算法对第一优化目标项和第二优化目标项进行综合求解，
得到无人车最优的控制输入。
[0095]
对于本实施例中的式(11)所示的无人车自主定位与环境探索模型，控制输入表示粒子群，通过粒子群的迭代求解得到控制输入u
k:k+l
‑1，如果在最大迭代次数之前达到最优解(全局最优位置)，终止迭代，如果先达到最大迭代次数，也终止迭代，如图2所示。
[0096]
本实施例中，将控制输入u
i
作为粒子，在四维目标空间中，粒子i的位置u
i
、速度σ
i
表示为：
[0097][0098]
其中，位置u
i
、速度σ
i
都是四维向量，u
i1
,u
i2
,u
i3
,u
i4
分别表示粒子位置的四个维度的值，具体物理含义分别为无人车速度的横坐标，无人车速度的纵坐标，无人车角速度的横坐标，无人车角速度的纵坐标；σ
i1
,σ
i2
,σ
i3
,σ
i4
表示粒子速度的四个维度的值，用于算法粒子迭代，没有具体的物理含义；
[0099]
将u
i
代入适应度函数求适应值j
fit
，其中，适应度函数的设计非常关键，在不依赖gnss的无人车自主定位与环境探索方法中，为了兼顾地图构建精度(即：建图不确定性)和环境探索增益，设计如下适应度函数为：
[0100][0101]
其中，j
fit
表示适应度值，λ
i
(i＝1,2)为权重系数，j1为步骤二所示的极小化地图构建精度，j2为步骤三所示的最大化环境探索增益，即：
[0102][0103]
适应度函数中的权重系数λ
i
(i＝1,2)的选取采用序关系分析法，具体可以包括以下步骤：
[0104]
首先，由指标j1、j2的重要程度确定序关系(即j1>j2)；
[0105]
其次，根据采用序关系分析法指定r1＝1.2(即)，其中，r1表示序关系分析法中定义的权重系数比值，
[0106]
本实施例中，粒子i的第d(1≤d≤4)维速度、位置更新公式分别为：
[0107]
[0108][0109]
其中，pbest
id
＝(p
i1
,p
i2
,p
i3
,p
i4
)表示粒子i经历过的最优位置；gbest＝(g1,g2,g3,g4)表示粒子群所经历过的最优位置；表示第t次迭代时，粒子i速度矢量的第d维分量；表示第t次迭代时，粒子i位置矢量的第d维分量；c1,c2表示加速度常数，调节学习最大步长；r1、r2表示两个随机函数，取值范围为[0,1]；ω表示惯性权重，采用线性递减权值策略，即
[0110][0111]
其中，ω
i
与ω
a
分别为最小和最大惯性权值，n
c
,n
a
分别为当前迭代次数和最大迭代次数。u
k
,u
k+1
,...,u
k+l
‑1分别表示pso算法迭代出的最优解。
[0112]
本发明实施例所述的不依赖gnss的无人车自主定位与环境探索方法，结合实际中gnss不可用的情况，基于主动slam方法同时规划无人车运动轨迹并构建环境地图，能够在无人员干预的情况下进行自主环境探索，为同时提高地图构建精度以及未知区域覆盖效率，采用粒子群优化算法对构建的多目标优化问题进行综合求解，算法收敛速度快，计算高效，地图构建准确性高且用时短，能够避免陷入局部最优解。
[0113]
综上，本发明实施例所述的不依赖gnss的无人车自主定位与环境探索方法具有以下优点：
[0114]
1)考虑无人车控制输入受到的噪声的影响，建立的无人车自主定位与环境探索模型具有较强的抗干扰能力，符合工程要求。
[0115]
2)兼顾建图不确定性极小化和环境探索增益极大化两大指标，在mpc框架下，综合设计无人车控制输入，具有实际应用价值。
[0116]
3)采用粒子群优化算法对构建的非线性多目标优化问题进行综合求解，需要设置的参数少，简单易行，算法收敛速度快，计算高效。
[0117]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。