CN103914068A - 一种基于栅格地图的服务机器人自主导航方法 - Google Patents

Abstract

本发明属于机器人导航技术领域，涉及一种基于栅格地图的服务机器人自主导航方法，通过2D激光雷达实时地采集机器人所处的环境信息，进行环境特征提取，对采集的信息利用SLAM技术进行栅格地图的创建，同时实时地对机器人进行自主定位，并且在此基础上根据导航任务要求对机器人进行路径规划，然后根据规划的路径进行跟踪控制，使机器人完成导航任务。本发明同现有技术相比优点在于：针对导航中的三个关键技术进行了改进，使其更适合于服务机器人导航的实际需求，从而获得最佳的可行方案；其次本发明实时地对环境信息进行处理，产生动态地图，从而有效地实现自主导航，导航的精度和效率都大大提高。

Description

一种基于栅格地图的服务机器人自主导航方法

技术领域

本发明属于机器人及导航技术领域，涉及一种基于栅格地图的服务机器人自主导航方法。

背景技术

自主移动机器人是机器人研究领域的一个重要分支，它在军事、民用、科学研究等许多领域得到了广泛应用。近些年来，随着计算机、传感器和网络技术的发展，人们研究的重点也从结构式环境中的固定式机械臂、机械手转向非结构未知环境中的自主移动机器人。传统的机械臂空间环境建模以及示教运动的方法已无法满足自主移动机器人所面对的新任务。自主导航技术是移动机器人研究的核心，是实现自主移动的关键技术，主要包括自主定位、路径规划、跟踪控制等。目前，针对自主移动机器人导航问题已有很多的研究，但是现有的技术仍不够完善。自主定位是移动机器人导航的最基本环节，移动机器人要完成导航任务，就需要实时地知道自身相对于外界环境的位姿，常用的自主定位技术主要包括相对定位技术、绝对定位技术和组合定位技术。

相对定位也称为航迹推算，主要包括测距法和惯性导航法，其优点是不依赖于外部环境信息，可以提供完全的自主性和完备的导航信息，但是里程计、陀螺仪等传感器的误差会引来累积误差。绝对定位是利用外部参照***，通过测量移动机器人的绝对位置来实现定位，主要包括网络定位、路标定位、地图匹配定位，其优点是定位精度较高，不存在累计误差，但是技术复杂，且成本较高。组合定位是将相对定位和绝对技术结合起来进行定位，通常的做法是利用相对定位进行位姿估计，利用绝对定位对定位结果进行校正。

路径规划是移动机器人跟踪控制的基础，是移动机器人导航中最重要的任务之一。移动机器人路径规划主要可以分为模板匹配路径规划、基于环境模型的路径规划和基于行为的路径规划三种类型。模板匹配路径规划是将机器人当前状态与过去模板库中的事例相比较，寻找出一个最优匹配事例，修正该事例中的路径，从而得到一条新的路径。基于环境模型的路径规划是目前最为成熟的方法，根据掌握信息的完整程度可分为环境信息已知的全局路径规划和环境信息完全未知或部分未知的局部路径规划。基于行为的方法是由BROOKS在他的包容式结构中提出的，将其用于解决移动机器人路径规划问题是一种新的发展趋势。

根据控制目标的不同，移动机器人的跟踪控制问题包括点镇定问题、轨迹跟踪问题、路径跟踪问题。点镇定问题是指设计控制器，使移动机器人从任意的初始状态到达并稳定在任意的终止状态，其目的是获得一个反馈控制律，使移动机器人闭环***的一个平衡点是渐进稳定的。轨迹跟踪问题是指通过反馈控制，使得机器人从任意初始位置出发，都能够跟随预先设定好的期望轨迹。路径跟踪问题是指移动机器人以给定的速度或加速度，跟随预先设定好的期望路径。

以上三个关键技术是在机器人自主导航过程中必须解决的问题，但是由于服务机器人所处环境的复杂性，目前的一些技术已不能满足要求，因此需要对这些技术进行改进，或者研究新的导航技术。

发明内容

为了解决上述问题，本发明的目的在于针对现有技术的不足，提供一种基于栅格地图的服务机器人自主导航方法。

本发明基于栅格地图的服务机器人自主导航方法，包括自主定位、路径规划、轨迹跟踪，其特征在于：通过2D激光雷达实时地采集机器人所处的环境信息，进行环境特征提取，对采集的信息利用同时定位与地图构建技术进行栅格地图的创建，同时实时地对机器人进行自主定位，并在定位过程中对传感器误差实时地进行校正，在此基础上根据导航任务要求对机器人进行路径规划，然后根据规划的路径进行跟踪控制，使机器人完成导航任务，具体包括以下步骤：

本发明是通过以下技术方案实现的，本发明包括以下步骤：

步骤1：利用编码器采集机器人各轮的转速，利用2D激光雷达采集环境信息，从而得到服务机器人与周围环境的相对距离；

步骤2：同时定位与地图创建

步骤2.1：自主定位

步骤2.1.1：根据编码器反馈的各轮转速和服务机器人结构，建立机器人运动学模型；

步骤2.1.2：根据机器人位姿预测过程是马尔科夫过程，建立机器人位姿预测误差模型；

步骤2.1.3：根据2D激光雷达采集的环境信息，采用随机霍夫变换最小二乘算法提取局部环境直线特征和点特征，建立基于环境特征的观测模型；

步骤2.1.4：根据2D激光雷达原始数据与观测模型的关系，建立机器人观测误差模型；

步骤2.1.5：通过扩展卡尔曼滤波的定位算法进行自主定位。

步骤2.2：地图创建

步骤2.2.1：将局部环境特征通过鲁棒性的预测，从而生成全局环境特征；

步骤2.2.2：以服务机器人在二维平面内所占的投影面积的最小值为栅格，并以此栅格为单位将全局环境特征栅格化，生成了服务机器人所处环境的栅格地图；

所述的全局环境栅格是指：障碍物栅格标记为1，空白栅格标记为0；

步骤3：路径规划

步骤3.1：根据导航任务要求，设置路径规划的起始点和目标点；

步骤3.2：在栅格地图中采用一维编码方式表示可选择的路径，并建立有明确物理意义的适应度函数，进而采用遗传算法进行路径规划；

步骤3.3：采用极坐标下多项式曲线法对生成的路径进行平滑处理，从而得到适合机器人跟踪控制的平滑路径；

步骤4：跟踪控制

步骤4.1：根据移动机器人运动性能建立机器人在一定运动约束条件下的运动学模型及位姿误差状态方程；

步骤4.1：设计基于backstepping时变状态反馈方法的移动机器人轨迹跟踪控制器，并利用基于Lyapunov稳定性理论，对轨迹跟踪控制器的全局稳定性进行分析；

步骤4.2：根据规划出的服务机器人的运动路径，以及机器人当前的运动状态，采用轨迹跟踪控制器，控制机器人完成导航任务。

本发明同现有技术相比的有益效果：相对于现有的移动机器人自主导航方法，本发明的优点在于：首先本发明针对导航中的三个关键技术进行了改进，使其更适合于服务机器人导航的实际需求，从而获得最佳的可行方案；其次本发明实时地对环境信息进行处理，产生动态地图，从而有效地实现自主导航，导航的精度和效率都大大提高。

附图说明

图1为服务机器人自主导航方法示意图

图2为图1中步骤2.1的自主定位流程图

图3为图1中步骤2.2的地图更新流程图

图4为图1中步骤3的遗传算法路径规划流程图

图5为图1中步骤3的路径规划示意图

图6为图1中步骤3的路径编码方式示意图

图7为图1中步骤4的跟踪控制流程图

图8为图1中步骤4的跟踪控制***结构

具体实施方式

下面结合附图对本发明加以详细说明，应当指出的是，所描述的实施例仅旨在便于对本发明的理解，而不对其起任何限定作用。

图1是说明根据本发明的基于栅格地图的服务机器人自主导航方法的实施主要步骤的流程图。参照图1，该方法的主要流程为：

步骤1：控制机器人在已知的实验环境中运动，根据2D激光雷达采集并经特征提取的环境信息，进行地图创建，得到全局环境栅格地图；

步骤2：执行自主定位模块，确定机器人在全局坐标系中的位姿，并根据导航任务要求设定目标点；

步骤3：机器人在全局环境栅格地图中，根据自身位姿和目标点位置，执行路径规划模块，获得从机器人到目标点的最优路径，即一系列离散的路径点序列，并发送给路径跟踪模块；

步骤4：在执行路径跟踪模块过程中，根据2D激光雷达采集的环境信息实时更新全局环境地图。自主定位，并检测路径上是否存在障碍物，若存在障碍物则执行步骤3，否则继续执行路径跟踪，直到到达目标点。

图2为图1中步骤2.1的自主定位流程图。参照图2，自主定位的主要流程为：

步骤2.1.1：首先将500线编码器和2D激光雷达分别安装于服务器周围，然后设置500线编码器以30毫秒为周期反馈各轮的转速；再设置2D激光雷达以30毫秒为采样周期采集环境信息，从而得到服务机器人与其周围环境的相对距离；

步骤2.1.2：根据500线编码器和服务机器人各轮结构建立机器人运动学模型为：

式中[x(k) y(k) θ(k)]^T为机器人k时刻的位姿；L(k)为机器人从k时刻到k+1时刻运动的弧线距离；θ(k)为机器人坐标系与全局坐标系的夹角；Δθ(k)为机器人从k时刻到k+1时刻运动方向的变化量。

步骤2.1.3：分析采用运动学模型进行机器人位姿预测而引入的误差，根据机器人位姿预测过程是马尔科夫过程，得到机器人位姿预测协方差阵P(k)＝[p_ij]。

步骤2.1.4：根据服务机器人和全局环境特征的位置关系建立机器人的观测模型为：

$Z_j\left(k\right)=\left[\begin{array}{c}{\mathrm{\λ}}_j\\ {\mathrm{\δ}}_j\end{array}\right]=\left[\begin{array}{c}|{\mathrm{\ρ}}_j-\sqrt{x^2\left(k\right)+y^2\left(k\right)}\cos ({\mathrm{\θ}}_j-\arctan (y\left(k\right)/x\left(k\right)))|\\ {\mathrm{\θ}}_j-\mathrm{\θ}\left(k\right)\end{array}\right]$

式中Z_j(k)＝[λ_j δ_j]^T为第j个环境特征在机器人坐标系中的参数；(ρ_j，θ_j)为第j个环境特征在全局坐标系中的参数。

步骤2.1.5：根据2D激光雷达原始数据与观测模型的关系，得到机器人第j个环境特征的观测误差协方差阵为R_j；

步骤2.1.6：根据服务机器人的运动学模型和观测模型，通过滤波预测、观测预测、特征匹配、状态更新四步处理过程，即可得到更加精确的定位信息。

图3为图1中步骤2.2的地图更新流程图。

图4为图1中步骤3的遗传算法路径规划流程图，图5为图1中步骤3的路径规划示意图。参照图4、图5，路径规划的主要流程为：

步骤3.1：根据如图6所示的路径编码方式，路径编码形式为：

y1

y2

y3

……

yn

步骤3.2：计算种群个体适应度[f，p]＝objf(start，destination)，f为每个个体适应度，p为累积概率；

步骤3.3：根据导航任务要求，采用上述改进遗传算法进行路径规划，生成了从起始点到目标点的路径点序列；

步骤3.4：对生成的路径点序列进行关键点优化，得到从起始点到目标点的关键点序列；

步骤3.5：采用极坐标下多项式曲线法对由关键点序列表示的路径进行平滑处理，多项式曲线为：

式中为曲线上各点的极坐标；R为曲线所替代弧的半径；Φ为曲线的变化值。即可得到最终的平滑路径。

图7为图1中步骤4的跟踪控制流程图。轨迹跟踪的主要流程为：

步骤4.1：在考虑服务机器人所受的约束后，机器人位姿误差***状态方程为：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_e={\mathrm{\ω}}_{\mathrm{cz}}{y}_e-v_{\mathrm{ry}}{\sin \mathrm{\θ}}_e\\ {\stackrel{\·}{y}}_e=-{\mathrm{\ω}}_{\mathrm{cz}}{x}_e-v_{\mathrm{cy}}+v_{\mathrm{ry}}\cos {\mathrm{\θ}}_e\\ {\stackrel{\·}{\mathrm{\θ}}}_e={\mathrm{\ω}}_{\mathrm{rz}}-{\mathrm{\ω}}_{\mathrm{cz}}\end{array}\right.$

式中[x_e y_e θ_e]^T为k时刻在移动机器人坐标系中的位姿误差向量；[v_cx v_cy ω_cz]^T为机器人轨迹跟踪控制器的控制向量；[v_rx v_ry ω_rz]^T为机器人参考速度和角速度。

基于机器人运动学模型轨迹跟踪的定义为寻找有界速度控制量v_y和ω_z为：

$v_y=v_y(x_e,y_e,{\mathrm{\θ}}_e,v_{\mathrm{ry}},{\mathrm{\ω}}_{\mathrm{rz}},{\stackrel{\·}{v}}_{\mathrm{ry}},{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rz}})$

${\mathrm{\ω}}_z={\mathrm{\ω}}_z(x_e,y_e,{\mathrm{\θ}}_e,v_{\mathrm{ry}},{\mathrm{\ω}}_{\mathrm{rz}},{\stackrel{\·}{v}}_{\mathrm{ry}},{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rz}})$

步骤4.2：通过分步构造李雅普诺夫函数来设计控制律，机器人的控制律为：

$\left\{\begin{array}{c}{v}_y=v_{\mathrm{ry}}{\cos \mathrm{\θ}}_e+k_1\cos \left(\arctan \left({\mathrm{\ω}}_z\right)\right)\frac{1}{1+{\mathrm{\ω}}_z^2}{\stackrel{\·}{\mathrm{\ω}}}_z{x}_e+k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right){\mathrm{\ω}}_z{y}_e\\ -k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right)v_{\mathrm{ry}}s{\mathrm{in\θ}}_e+k_1(y_e+k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right)x_e)\\ {\mathrm{\ω}}_z={\mathrm{\ω}}_{\mathrm{rz}}-{2k}_3{x}_e{v}_{\mathrm{ry}}\cos \left(\frac{{\mathrm{\θ}}_e}{2}\right)+k_4\sin \left(\frac{{\mathrm{\θ}}_e}{2}\right)\end{array}\right.$

式中k₁、k₂、k₃、k₄为大于零的常数。

Claims (4)

1.一种基于栅格地图的服务机器人自主导航方法，包括自主定位、路径规划、轨迹跟踪，其特征在于：通过2D激光雷达实时地采集机器人所处的环境信息，进行环境特征提取，对采集的信息利用同时定位与地图构建技术进行栅格地图的创建，同时实时地对机器人进行自主定位，并在定位过程中对传感器误差实时地进行校正，在此基础上根据导航任务要求对机器人进行路径规划，然后根据规划的路径进行跟踪控制，使机器人完成导航任务，具体包括以下步骤：

步骤1：控制机器人在已知的实验环境中运动，根据2D激光雷达采集并经特征提取的环境信息，进行地图创建，得到全局环境栅格地图；所述的全局环境栅格是指障碍物栅格标记为1，空白栅格标记为0；

步骤2：执行自主定位模块，确定机器人在全局坐标系中的位姿，并根据导航任务要求设定目标点；

步骤3：机器人在全局环境栅格地图中，根据自身位姿和目标点位置，执行路径规划模块，获得从机器人到目标点的最优路径，即一系列离散的路径点序列，并发送给路径跟踪模块；

步骤4：在执行路径跟踪模块过程中，根据2D激光雷达采集的环境信息实时更新全局环境地图；自主定位，并检测路径上是否存在障碍物，若存在障碍物则执行步骤3，否则继续执行路径跟踪，直到到达目标点。

2.根据权利要求1所述的一种基于栅格地图的服务机器人自主导航方法，其特征在于：步骤2的具体步骤如下：

步骤2.1：自主定位

步骤2.1.1：首先将500线编码器和2D激光雷达分别安装于服务器周围，然后设置500线编码器以30毫秒为周期反馈各轮的转速；再设置2D激光雷达以30毫秒为采样周期采集环境信息，从而得到服务机器人与其周围环境的相对距离；

步骤2.1.2：根据500线编码器和服务机器人各轮结构建立机器人运动学模型为：

式中[x(k) y(k) θ(k)]^T为机器人k时刻的位姿；L(k)为机器人从k时刻到k+1时刻运动的弧线距离；θ(k)为机器人坐标系与全局坐标系的夹角；Δθ(k)为机器人从k时刻到k+1时刻运动方向的变化量。

步骤2.1.3：分析采用运动学模型进行机器人位姿预测而引入的误差，根据机器人位姿预测过程是马尔科夫过程，得到机器人位姿预测协方差阵P(k)＝[p_ij]。

步骤2.1.4：根据服务机器人和全局环境特征的位置关系建立机器人的观测模型为：

$Z_j\left(k\right)=\left[\begin{array}{c}{\mathrm{\λ}}_j\\ {\mathrm{\δ}}_j\end{array}\right]=\left[\begin{array}{c}|{\mathrm{\ρ}}_j-\sqrt{x^2\left(k\right)+y^2\left(k\right)}\cos ({\mathrm{\θ}}_j-\arctan (y\left(k\right)/x\left(k\right)))|\\ {\mathrm{\θ}}_j-\mathrm{\θ}\left(k\right)\end{array}\right]$

式中Z_j(k)＝[λ_j δ_j]^T为第j个环境特征在机器人坐标系中的参数；(ρ_j，θ_j)为第j个环境特征在全局坐标系中的参数；

步骤2.1.5：根据2D激光雷达原始数据与观测模型的关系，得到机器人第j个环境特征的观测误差协方差阵为R_j；

步骤2.1.6：根据服务机器人的运动学模型和观测模型，通过滤波预测、观测预测、特征匹配、状态更新四步处理过程，即可得到更加精确的定位信息；

步骤2.2：地图创建

步骤2.2.1：将局部环境特征通过鲁棒性的预测，从而生成全局环境特征；

步骤2.2.2：以服务机器人在二维平面内所占的投影面积的最小值为栅格，并以此栅格为单位将全局环境特征栅格化，生成了服务机器人所处环境的栅格地图。

3.根据权利要求1所述的一种基于栅格地图的服务机器人自主导航方法，其特征在于：步骤3的具体步骤如下：

步骤3.1：路径编码形式为：

  y₁   y₂   y₃ …… y_n

步骤3.2：计算种群个体适应度[f，p]＝objf(start，destination)，f为每个个体适应度，p为累积概率；

步骤3.3：根据导航任务要求，采用上述改进遗传算法进行路径规划，生成了从起始点到目标点的路径点序列；

步骤3.4：对生成的路径点序列进行关键点优化，得到从起始点到目标点的关键点序列；

步骤3.5：采用极坐标下多项式曲线法对由关键点序列表示的路径进行平滑处理，多项式曲线为：

式中为曲线上各点的极坐标；R为曲线所替代弧的半径；Φ为曲线的变化值。即可得到最终的平滑路径。

4.根据权利要求1所述的一种基于栅格地图的服务机器人自主导航方法，其特征在于：步骤4的具体步骤如下：

步骤4.1：在考虑服务机器人所受的约束后，机器人位姿误差***状态方程为：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_e={\mathrm{\ω}}_{\mathrm{cz}}{y}_e-v_{\mathrm{ry}}{\sin \mathrm{\θ}}_e\\ {\stackrel{\·}{y}}_e=-{\mathrm{\ω}}_{\mathrm{cz}}{x}_e-v_{\mathrm{cy}}+v_{\mathrm{ry}}\cos {\mathrm{\θ}}_e\\ {\stackrel{\·}{\mathrm{\θ}}}_e={\mathrm{\ω}}_{\mathrm{rz}}-{\mathrm{\ω}}_{\mathrm{cz}}\end{array}\right.$

式中[x_e y_e θ_e]^T为k时刻在移动机器人坐标系中的位姿误差向量；[v_cx v_cy ω_cz]^T为机器人轨迹跟踪控制器的控制向量；[v_rx v_ry ω_rz]^T为机器人参考速度和角速度。

基于机器人运动学模型轨迹跟踪的定义为寻找有界速度控制量v_y和ω_z为：

$v_y=v_y(x_e,y_e,{\mathrm{\θ}}_e,v_{\mathrm{ry}},{\mathrm{\ω}}_{\mathrm{rz}},{\stackrel{\·}{v}}_{\mathrm{ry}},{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rz}})$

${\mathrm{\ω}}_z={\mathrm{\ω}}_z(x_e,y_e,{\mathrm{\θ}}_e,v_{\mathrm{ry}},{\mathrm{\ω}}_{\mathrm{rz}},{\stackrel{\·}{v}}_{\mathrm{ry}},{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rz}})$

步骤4.2：通过分步构造李雅普诺夫函数来设计控制律，机器人的控制律为：

$\left\{\begin{array}{c}{v}_y=v_{\mathrm{ry}}{\cos \mathrm{\θ}}_e+k_1\cos \left(\arctan \left({\mathrm{\ω}}_z\right)\right)\frac{1}{1+{\mathrm{\ω}}_z^2}{\stackrel{\·}{\mathrm{\ω}}}_z{x}_e+k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right){\mathrm{\ω}}_z{y}_e\\ -k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right)v_{\mathrm{ry}}s{\mathrm{in\θ}}_e+k_1(y_e+k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right)x_e)\\ {\mathrm{\ω}}_z={\mathrm{\ω}}_{\mathrm{rz}}-{2k}_3{x}_e{v}_{\mathrm{ry}}\cos \left(\frac{{\mathrm{\θ}}_e}{2}\right)+k_4\sin \left(\frac{{\mathrm{\θ}}_e}{2}\right)\end{array}\right.$

式中k₁、k₂、k₃、k₄为大于零的常数。