CN107741745B - 一种实现移动机器人自主定位与地图构建的方法 - Google Patents

Abstract

本发明涉及机器人导航技术领域，旨在提供一种实现移动机器人自主定位与地图构建的方法。本发明提出机器人同步定位与地图创建的算法，在复杂环境下将机器人运动轨迹分为直线和弧线相融合的位姿估算方式，并在得到机器人位姿的基础上发明一种环境特征的处理方法更新地图特征，从而得到更为精确的机器人位姿和环境地图。本发明在复杂环境下具有良好的适应性和可靠性。能够在长时间运动中保证机器人位姿误差维持在一个很小的稳定的范围内，并在已确立机器人精确位姿的基础上，获取周围环境的特征，得到环境地图，然后基于环境地图实现自主定位与导航。本发明避免了因机器人误差导致整个地图出现偏差的可能，在各种复杂环境中都有良好的适应性。

Description

一种实现移动机器人自主定位与地图构建的方法

技术领域

本发明属于机器人导航技术领域，涉及一种实现移动机器人自主定位与地图构建的 方法。

背景技术

移动机器人自主定位与地图构建是实现未知环境下机器人自主导航的关键性技术， 具有广泛的应用前景，也是目前机器人研究的热点问题之一。在未知环境中，机器人依靠其自身携带的传感器所提供的信息对环境进行描述，然后构建地图特征模型，以得到 地图特征的精确位置，但环境地图的建立又取决于机器人各时刻的观测位置。因此，在 机器人的运动过程中面临着一个两难的情况：1)为了构建环境地图，机器人需要知道 各个时刻机器人所处的实际位置；2)机器人若要知道各个时刻的实际位置，则必须知 道机器人周围环境的地图特征。

目前，为了解决上述难题，很多研究人员提出将机器人的定位与地图构建结合在一 起同步交叉进行，根据已创建的地图特征校正里程计的误差，得到机器人的精确位姿估计，再以得到的机器人位姿去处理环境特征，得到环境地图。这种联合机器人定位与地 图构建的方法是基本可行的，但是在众多方法中忽略了在不同场景下机器人的位姿估计 存在较大偏差，而当机器人的位姿估计存在较大误差时，会导致地图特征的位置不够精 确，从而对整个地图精度造成极大的影响。

发明内容

本发明要解决的技术问题是，克服现有技术中存在不足，提供一种实现移动机器人 自主定位与地图构建的方法。

为解决技术问题，本发明的解决方案是：

提供一种实现移动机器人自主定位与地图构建的方法，是在移动机器人上安装里程 计、罗盘和激光雷达，利用里程计的信息获取机器人位姿，利用罗盘获取初始目标航向并在拐角处借助罗盘数据的差值获取机器人的偏转角，利用激光雷达获取周围环境特征的距离和角度信息；

该方法具体包括以下步骤：

步骤一、对已知t-1时刻的机器人位姿s_t-1、控制量u_t-1和环境特征m_t-1，去估算 得到t时刻机器人的位姿s_k；

该步骤具体包含：

(1)在t＝1时刻，机器人的初始位姿为s₀，获取的初始环境特征集为 表示t＝1时刻第i个特征点的位置；

要获得机器人下一时刻的最优位姿估计，先对机器人进行下一时刻的状态进行预测：

对上式取t＝1时，为初始时刻对下一时刻机器人位姿的估计；

在上式中，为机器人的预测位置，p(·)表示随机变量的概率密度函数，p(·|·)表示 条件概率密度函数；

上式中的递归公式是利用贝叶斯滤波的方式得到机器人在t时刻的位姿的后验概率； 利用粒子滤波器对后验概率去进行近似估计，得到一个提议分布(1.2)：机器人在t时刻 时，每个粒子位姿的后验概率分布满足：

上式中，为第j(j＝1,2…,m)个粒子在t时刻的位姿，s_t-1为t-1时刻机器人的位置；

(2)在上述状态预测方程，加入t时刻对周围环境的观测值z_t，利用环境观测量去修正机器人的预测值，得到更为精确的机器人位姿：

上式中为加入观测量修正机器人预测位置之后的最佳机器人位姿；

将对外的观测量z_t加入后验概率分布之后，能对机器人的预测位姿进行更新；在对 粒子的预测位姿进行更新的同时，对t时刻第j个粒子计算其权重

(3)得到每个粒子在加入观测值之后的一个权重之后，根据其权重的大小进行判断，权重小于0.5的粒子被丢弃，得到一组新的粒子集将这组粒 子进行归一化：

将粒子权重归一化之后，即对粒子进行加权求和得到机器人的最优估计位姿：

(4)当t＝1时，通过上述过程计算得到机器人下一时刻(t＝2)的最优位姿估计和一 组保留下来的粒子集依次迭代即能得到t时刻机器人的最优位姿估计

但在每一次迭代过程需要计算判断变量N_eff：

若N_eff>m/2，则表明粒子数足够去预测下一时刻的机器人位姿，则进入下一步；若N_eff≤m/2，则表明粒子数太少，无法对下一时刻的机器人预测状态提供足够的信息， 因此需要对机器人的位姿进行重采样；

步骤二、在得到机器人的最优估计位姿后，再利用激光雷达获取周围的环境特征点， 并将获取到的环境特征点利用扩展卡尔曼滤波进行处理，再与上一时刻的环境特征点进 行数据关联，从而更新环境地图；

该步骤具体包括：

(1)在t＝1时，机器人获取得到一组初始环境特征集m₀；在下一时刻，机器人运动到一个新的位置，要获取环境特征的最优位置估计，先利用t-1时刻的状态对t时刻的 环境特征进行预测：

在上式中t＝1时，表示从初始位置对下一位置的预测，表示t时刻环境特征的预测量，为过程预测函数，W_t-1为过程高斯噪声，满足如下条件：

E[W_t]＝0

在上式中，Q_k为噪声协方差；

(2)在得到环境特征的预测量后，再计算环境特征的观测量如下：

为运动观测函数，V_t为t时刻的测量高斯噪声，满足如下条件：

E[V_t]＝0

在上式中，R_k为噪声协方差；

(3)将得到的环境特征预测量与观测量z_t进行融合得到环境特征的最优位置估计 如下：

上式中，为t时刻环境特征点的最优估计量，z_t为t时刻环境特征的观测量，K_t为卡尔曼增益，P_t,t-1为t-1时刻预测t时刻的误差矩阵，R_t为观测噪声的协方差矩阵，H_t为 测量协方差矩阵，A_t-1为过程函数的雅克比矩阵；当t＝1时，其初始误差矩阵P₀，观 测噪声R₀，测量协方差矩阵H₁，测量雅克比矩阵A₀已知；

在得到环境特征的最优位置估计之后，为下一迭代过程的延续，对误差矩阵更新如 下：

P_t＝[I-K_tH_t]P_t.t-1

(4)在得到最优环境特征点的位置之后，需要将此刻的环境与上一时刻的环境进 行关联以得到更新后的地图，因此利用马氏距离来判别两个时刻所获取的特征是否为同 一环境特征；在计算马氏距离时，为了更快更方便的得出数据关联特征，直接计算t-1时刻对t时刻的环境特征预测量和t时刻环境特征观测量的距离：

其中为协方差矩阵，ε为门限常数，取值0.95；

当D²小于ε时，判定两个环境特征具有关联性，即为同一个环境，从而对旧地图进行更新；当D²大于限定值ε时，判定两个环境特征不具有关联性，即为两个不同的环境 特征，从而将此刻观测到的环境特征添加到旧地图中，对原地图进行扩展；

关联之后得到的一组环境特征集为再以这组环境特征和 此刻得到最优机器人位姿即能按同样方式对t＝3,4..时刻进行更新迭代，得到t时刻 机器人的位姿为和其所获取的环境特征为

发明原理描述：

本发明提出一种新型的机器人同步定位与地图创建的算法，即在复杂环境下，将机 器人运动轨迹分为直线和弧线相融合的位姿估算方式，并在得到机器人位姿的基础上发 明一种环境特征的处理方法更新地图特征，从而得到更为精确的机器人位姿和环境地图。 该算法在复杂环境下，具有良好的适应性和可靠性。该算法能够在长时间运动中，保证机器人的位姿误差维持在一个很小的稳定的范围内，并在已确立机器人精确位姿的基础上，获取周围环境的特征，得到环境地图，然后基于环境地图实现自主定位与导航。

与现有技术相比，本发明的优点在于：

(1)本发明对机器人的位姿的估算提供了在不同情境下的估算，以达到更为精确的效果，避免了因机器人误差较大从而导致整个地图出现偏差的可能。

(2)本发明考虑了在复杂环境条件下的机器人的运动模型，则该发明在各种复杂环境中都具有良好的适应性。

(3)在对机器人求取最优位姿时，会利用上一时刻得到环境特征对机器人位置进行修正；而在机器人位姿确定之后，对地图特征进行优化的过程中，会对上一时刻的地 图特征进行更新；两者互相矫正，从而使机器人的运动会收敛到一个相对稳定的状态， 误差较小，即本算法具有极大的可靠性。

附图说明

图1是本发明从t-1时刻到t时刻对机器人位姿和地图特征进行更新的示意图；

图2是在将机器人的运动路径通过直线估计得到机器人的观测位置估计示意图；

图2中，A：机器人上一时刻的位姿，在这个位置扫描路标C得到路标的距离和角 度B：机器人当前时刻的位姿，在这个位置扫描路标C得到路标的距离和角 度C：上一时刻和当前时间测量得到的同一路标；

图3是将机器人的运动路径通过曲线估计得到机器人的观测位置估计示意图；

图3中，A：机器人上一时刻的位姿，在这个位置扫描路标C得到路标的距离和角 度(d1，θ₁)；B：机器人当前时刻的位姿，在这个位置扫描路标C得到路标的距离和角 度(d2，θ₂)；C：上一时刻和当前时间测量得到的同一路标；

图4是本发明在t时刻对环境特征进行处理的示意图。

具体实施方式

下面以一个带有里程计、罗盘和激光雷达三种传感器的机器人在一个未知环境中的 运动说明本发明的方法。

机器人利用里程计的信息预测机器人位姿；用罗盘获取初始目标航向并在拐角处， 借助罗盘数据的差值获取机器人的偏转角；利用激光雷达获取周围环境特征的距离和角 度信息；本发明利用上述三种传感器对机器人的位置进行定位的同时获取周围环境地图。 上述公式(1.1)---(1.4)，(2.1)---(2.3)给出的是一般计算形式，具体计算形式与其设计 的概率分布和机器人运动有关。下面结合实例说明公式(1.1)---(1.4)，(2.1)---(2.3)中变量、 函数及概率密度函数的具体含义：

设机器人满足的运动方程为：

s_t＝f(s_t-1,u_t-1)+w_t (1)

其中s_t＝(x,y,θ)为t时刻机器人的位姿；u_t-1＝(v,ω)为t-1时刻为机器人的控制量，由里程计获得，v为机器人的线速度，ω为机器人的角速度；w_t为高斯噪声，满足 如下条件：

E[W_t]＝0

E[W_kW_j ^T]＝Q_kδ_kj

利用概率密度函数的方式预估机器人的位姿的时是用每个粒子的形式去预估机器 人所在位置的可能性，同时对每一种可能性进行评估，其具体评估方式如下： 将t时刻每个粒子所代表的机器人位姿分解为x-y方向坐标上的运动：

x_t＝x_t-1+v_t-1*cos(θ_t)+w_t (2)

y_t＝y_t-1+v_t-1*sin(θ_t)+w_t (3)

θ_t＝θ_t-1+ω_t-1*△t+w_t (4)

即从初始时刻开始，当初始位姿已知时，机器人的下一时刻的预测位姿，根据上述运动模型即可得到。但由于单独靠里程计获取的信息去预测机器人下一时刻的位姿存在着误差和干扰噪声w_t。因此，在本专利中利用激光雷达获取周围环境特征的位置，以估 算此刻机器人的观测位姿，选择相同特征概率最大的特征点，通过其位置来计算机器 人的观测位姿，具有该特征点的性质的点以下称为路标：

当机器人在直道上行走时，以示意图2的形式计算机器人的观测位姿通过在A，B两个位置测量路标C的位置即可得到此刻机器人的运动距离，即AB的长:

此时，机器人在t时刻的纵坐标未发生改变横坐标为当机器人在转角处行走时，以示意图3的形式计算机器人的观测位置由上图所示， 当机器人偏转原来的运动方向之后，此时根据激光雷达测量得到的路标C的夹角存在着 偏差角θ₀，因此第一种直线运动距离的估算方式可能会存在较大误差，故本专利提出第 二种情况下的运动距离估算方式：

其中为相邻时刻罗盘的角度差值，计算距离AB和

进一步可得即可根据θ₀和AB得到机器人运动t时刻的坐标为：

在得到机器人的观测位姿之后，再以观测位姿去修正由粒子所代表的机器人预测 位姿，而每一个粒子预测位姿的概率分布满足以观测位姿为均值，以干扰噪声Q_k为均差的正态分布，由此即可计算每一个粒子在加入观测位姿修正之后的权重为：

由于每个粒子的权重不一，部分粒子权重太小，即偏离观测位置太远 时，这部分粒子加入到评估最优位姿时会导致位置的偏离，故丢弃这部 分粒子。再将剩余的粒子进行归一化之后加权求和得到机器人的最优位姿估计：

在确认机器人的位姿之后，再利用激光雷达扫描周围的环境特征，利用扩展卡尔 曼滤波对环境特征进行处理，以获得精确的地图特征信息：

在t-1时刻，机器人获得一组环境特征集m_t-1；到t时刻，机器人运动到一个新的 位置，通过上一个运动过程保留下来的一组代表机器人可能位姿的粒子集对地图特征进行预测：

表示第j个粒子对环境特征集的预测位置，z_t为 一组环境特征的观测量，将这组观测量添加到每一个粒子上，可得到相同的一个环境特征在不同的粒子位姿下具有不同的位置。

对每一个环境特征在不同的粒子位姿下的位置求其均值为μ_i，假设测量数据的误 差为则对每一个环境特征的估算可近似为满足如下分布：

由每个环境特征在不同粒子下具有不同的位置，则其所代表的环境特征具有的重要 性程度也会不一样，即通过每个环境特征满足相应的正态分布得到每个环境特征所对 应的权重对每个环境特征在不同粒子下的位置进行加权求和得到环境特征的最优预测位置：

其中，为第i个环境特征在第j个粒子下的权重，为第i个环境特征的最优 预测位置。

在得到环境特征的最优预测位置之后，再将其与测量得到的环境特征z_t进行融合 得到环境特征的最优状态估计：

其中的参数计算更新如下：

P_t＝[I-K_tH_t]P_t.t-1 (22)

而R_t和Q_t-1为协方差矩阵；

对获得的环境特征进行误差处理之后再与前一时刻的环境特征进行数据关联，实现 对地图特征的更新：

在上式中，门限值ε取值为0.95，若马氏距离D²满足上式(23)，则该环境特征观测值与地图中已有的环境特征为同一环境特征，对此环境特征进行更新；若马氏距离D²不 满足上式(23)，则该环境特征为新的环境特征，即添加的到地图中，更新环境地图。

通过上述计算过程的迭代，当机器人达到目标位置时，机器人停止运动，从而得到机器人的运动路径为其遍历所形成的地图为 m＝{m₀,m₁,m₂,…,m_t}。

Claims (1)

1.一种实现移动机器人自主定位与地图构建的方法，其特征在于，是在移动机器人上安装里程计、罗盘和激光雷达，利用里程计的信息预测机器人位姿，利用罗盘获取初始目标航向并在拐角处借助罗盘数据的差值获取机器人的偏转角，利用激光雷达获取周围环境特征的距离和角度信息；

该方法具体包括以下步骤：

步骤一、对已知t-1时刻的机器人位姿s_t-1、控制量u_t-1和环境特征m_t-1，去估算得到t时刻机器人的位姿s_k；

该步骤具体包含：

(1)在t＝1时刻，机器人的初始位姿为s₀，获取的初始环境特征集为 表示t＝1时刻第i个特征点的位置；

要获得机器人下一时刻的最优位姿估计，先对机器人进行下一时刻的状态进行预测：

对上式取t＝1时，为初始时刻对下一时刻机器人位姿的估计；

在上式中，为机器人的预测位置，p(·)表示随机变量的概率密度函数，p(·|·)表示条件概率密度函数；

上式中的递归公式是利用贝叶斯滤波的方式得到机器人在t时刻的位姿的后验概率；利用粒子滤波器对后验概率进行近似估计，得到一个提议分布(1.2)：机器人在t时刻时，每个粒子位姿的后验概率分布满足：

上式中，为第j(j＝1，2...，m)个粒子在t时刻的位姿，s_t-1为t-1时刻机器人位置；

(2)在上述状态预测方程，加入t时刻对周围环境的观测量z_t，利用环境观测量去修正机器人的预测值，得到更为精确的机器人位姿：

上式中为加入观测量修正机器人预测位置之后的最佳机器人位姿；

将对外的观测量z_t加入后验概率分布之后，能对机器人的预测位姿进行更新；在对粒子的预测位姿进行更新的同时，对t时刻第j个粒子计算其权重

(3)得到每个粒子在加入观测量之后的一个权重之后，根据其权重的大小进行判断，权重小于0.5的粒子被丢弃，得到一组新的粒子集将这组粒子进行归一化：

将粒子权重归一化之后，即对粒子进行加权求和得到机器人的最优估计位姿：

(4)当t＝1时，通过上述过程计算得到机器人下一时刻(t＝2)的最优位姿估计和一组保留下来的粒子集依次迭代即能得到t时刻机器人的最优位姿估计但在每一次迭代过程需要计算判断变量N_eff：

若N_eff＞m/2，则表明粒子数足够去预测下一时刻的机器人位姿，则进入下一步；若N_eff≤m/2，则表明粒子数太少，无法对下一时刻的机器人预测状态提供足够的信息，因此需要对机器人的位姿进行重采样；

步骤二、在得到机器人的最优估计位姿后，再利用激光雷达获取周围的环境特征点，并将获取到的环境特征点利用扩展卡尔曼滤波进行处理，再与上一时刻的环境特征点进行数据关联，从而更新环境地图；

该步骤具体包括：

(1)在t＝1时，机器人获取得到一组初始环境特征集m₀；在下一时刻，机器人运动到一个新的位置，要获取环境特征的最优位置估计，先利用t-1时刻的状态对t时刻的环境特征进行预测：

在上式中t＝1时，表示从初始位置对下一位置的预测，θ_t表示t时刻环境特征的预测量，为过程预测函数，W_t-1为过程高斯噪声，满足如下条件：

E[W_t]＝0

在上式中，Q_k为噪声协方差；

(2)在得到环境特征的预测量θ_t后，再计算环境特征的观测量如下：

z_t＝h(θ_t)+V_t (2.2)

h(θ_t)为运动观测函数，V_t为t时刻的测量高斯噪声，满足如下条件：

E[V_t]＝0

在上式中，R_k为噪声协方差；

(3)将得到的环境特征预测量θ_t与观测量z_t进行融合得到环境特征的最优位置估计如下：

上式中，为t时刻环境特征点的最优估计量，z_t为t时刻环境特征的观测量，K_t为卡尔曼增益，P_t，t-1为t-1时刻预测t时刻的误差矩阵，R_t为观测噪声的协方差矩阵，H_t为测量协方差矩阵，A_t-1为过程函数的雅克比矩阵；当t＝1时，其初始误差矩阵P₀，观测噪声R₀，测量协方差矩阵H₁，测量雅克比矩阵A₀已知；

在得到环境特征的最优位置估计之后，为下一迭代过程的延续，对误差矩阵更新如下：

P_t＝[I-K_tH_t]P_t，t-1

(4)在得到最优环境特征的位置之后，需要将此刻的环境与上一时刻的环境进行关联以得到更新后的地图，因此利用马氏距离来判别两个时刻所获取的特征是否为同一环境特征；在计算马氏距离时，为了更快更方便的得出数据关联特征，直接计算t-1时刻对t时刻的环境特征预测量θ_t和t时刻环境特征观测量的距离：

其中为协方差矩阵，ε为门限常数，取值0.95；

当D²小于ε时，判定两个环境特征具有关联性，即为同一个环境，从而对旧地图进行更新；当D²大于限定值ε时，判定两个环境特征不具有关联性，即为两个不同的环境特征，从而将此刻观测到的环境特征添加到旧地图中，对原地图进行扩展；

关联之后得到的一组环境特征集为再以这组环境特征和此刻得到最优机器人位姿即能按同样方式对t＝3，4..时刻进行更新迭代，得到t时刻机器人的位姿为和其所获取的环境特征为