室内机器人导航系统及方法
【专利摘要】本发明提供一种室内机器人导航系统及方法，其中系统包括：人机交互装置用于接收用户输入的终点位置坐标及终点位置姿态信息；数据采集装置用于获取当前位置坐标和当前位置姿态信息；数据分析装置用于采用第一算法生成全局参考路径；数据采集装置还用于实时更新局部区域地图，并通过坐标转换获取动态障碍物在二维全局静态地图中对应的第二坐标；数据分析装置还用于采用第二算法对机器人在下一刻的速度向量进行最优规划，以获得最优速度向量，用于供机器人根据所述最优速度向量进行移动。本发明提供一种室内机器人导航系统及方法，实用性更高。
【专利说明】
室内机器人导航系统及方法
技术领域
[0001] 本发明设及人工智能及自动化领域内的一种机器人导航系统及方法，尤其设及一 种室内机器人导航系统及方法。
【背景技术】
[0002] 导航技术是引导某一设备从指定航线的当前起始点运动到终点的方法。导航技术 无论是在交通运输，还是家庭服务、军事领域等方面都拥有广阔的应用前景，也是当下研究 的热口技术之一。在交通运输上，导航技术可W用来实现无人驾驶汽车的自主驾驶、规避障 碍物行为，也可完成大型仓库中的物流智能调度等;在家庭服务方面，可W帮助类似扫地机 器人的小型机器人完成室内的定点移动;在军事领域，导航技术可W运用在导弹的定点打 击、无人机、无人车的定点巡逻侦查规避威胁区域的场景。
[0003] 目前，机器人导航系统主要采用GPS、路标、机器人底部的码盘、光学运动捕捉系统 等的协同工作来提供机器人的位置姿态信息，W及，采用激光雷达和超声波的协同工作来 提供障碍物的深度信息，从而实现对机器人进行导航。但是，现有的机器人导航系统存在的 缺点是，机器人导航系统的静态路径规划算法完全依赖于静态全局地图进行路径规划，没 有将动态障碍物加 W考虑，实用性不高。
【发明内容】
[0004] 本发明要解决的技术问题是提供一种室内机器人导航系统及方法，采用全局静态 规划与局部动态避障规划相结合的路径规划算法对机器人的路径进行实时规划，使得机器 人可W根据局部近距离动态障碍物的更新做出路径的调整，实用性更高。
[0005] 为解决上述技术问题，本发明的技术方案是，
[0006] -方面，本发明提供一种室内机器人导航系统，应用于机器人，包括:人机交互装 置：用于接收用户输入的机器人的终点位置坐标及机器人的终点位置姿态信息;数据采集 装置:用于获取机器人的当前位置坐标和机器人的当前位置姿态信息;数据分析装置，用于 根据终点位置坐标，终点位置姿态信息，当前位置坐标，当前位置姿态信息W及预先获取的 二维全局静态地图，采用第一算法生成一条位于当前点与终点之间的代价最小的全局参考 路径;数据采集装置，还用于实时更新距离机器人固定半径内的局部区域地图，获取动态障 碍物在局部区域地图中的第一坐标，并根据第一坐标进行坐标转换W获取动态障碍物在二 维全局静态地图中对应的第二坐标;数据分析装置，还用于根据第二坐标，终点位置坐标， 当前位置坐标，当前位置姿态信息，终点位置姿态信息和全局参考路径，采用第二算法对机 器人在下一刻的速度向量进行最优规划，W获得最优速度向量，用于供机器人根据最优速 度向量进行移动。
[0007] 进一步地，人机交互装置包括一个可视化的交互界面。
[000引进一步地，数据采集装置包括= RGB-D传感器、超声波测距仪、惯性测量传感器。 [0009] 进一步地，第一算法为A*算法。
[0010] 进一步地，第二算法为DWA算法。
[0011] 另一方面，本发明提供一种室内机器人导航方法，应用于机器人，包括:人机交互 装置接收用户输入的机器人的终点位置坐标及机器人的终点位置姿态信息;数据采集装置 获取机器人的当前位置坐标和机器人的当前位置姿态信息;数据分析装置根据终点位置坐 标，终点位置姿态信息，当前位置坐标，当前位置姿态信息W及预先获取的二维全局静态地 图，采用第一算法生成一条位于当前点与终点之间的代价最小的全局参考路径;数据采集 装置实时更新距离机器人固定半径内的局部区域地图，获取动态障碍物在局部区域地图中 的第一坐标，并根据第一坐标进行坐标转换W获取动态障碍物在二维全局静态地图中对应 的第二坐标；当机器人没有被障碍物包围时，为正常导航状态，数据分析装置根据第二坐 标，终点位置坐标，当前位置坐标，当前位置姿态信息，终点位置姿态信息和全局参考路径， 采用第二算法对机器人在下一刻的速度向量进行最优规划，W获得最优速度向量，用于供 机器人根据最优速度向量进行移动。
[0012] 进一步地，当机器人被障碍物包围而无法进行导航时，还包括:数据分析装置发出 一个预设的速度向量给底盘W进行原地旋转;数据采集装置持续扫描并更新局部区域地 图，直到局部区域地图中的障碍物的分布允许机器人通过，返回正常导航状态，其中，障碍 物包括动态障碍物和静态障碍物。
[0013] 进一步地，二维全局静态地图的预先获取步骤包括:数据采集装置围绕当前环境 进行一次全局扫描W获得若干帖=维点云，并采用=维SLAM算法将若干帖=维点云拼接成 为=维全局静态地图；数据分析装置对=维全局静态地图进行降维度处理，W获得二维全 局静态地图。
[0014] 进一步地，第一算法为A*算法。
[0015] 进一步地，第二算法为DWA算法。
[0016] 本发明提供的室内机器人导航系统及方法，使用人机交互装置接收用户输入的导 航的终点位置坐标和终点位置姿态信息;数据采集装置获取机器人的当前位置坐标和当前 位置姿态信息;之后，数据分析装置，再根据终点位置坐标，终点位置姿态信息，当前位置坐 标，位置姿态信息W及预先获取的二维全局静态地图，采用第一算法生成一条位于当前点 与终点之间的代价最小的全局参考路径;在获得全局参考路径的同时，数据采集装置实时 更新距离机器人固定半径内的局部区域地图，并获取动态障碍物在局部区域地图中的第一 坐标，并通过坐标转换将得到的第一坐标转换到二维全局静态地图坐标系中W获取第二坐 标；当机器人没有被障碍物包围时，为正常导航状态，数据分析装置再根据第二坐标，终点 位置坐标，当前位置坐标，当前位置姿态信息，终点位置姿态信息和全局参考路径，采用第 二算法对机器人在下一刻的速度向量进行最优规划，W获得最优速度向量，W使得底盘根 据最优速度向量进行动作，从而使得机器人向终点位置进行移动。此外，需要说明的是，本 发明的室内机器人导航系统及方法是通过控制底盘的运动来实现对机器人的运动进行导 航。
[0017] 本发明提供的室内机器人导航系统及方法，先采用第一算法进行全局静态规划， 规划出一条位于当前点与终点之间的代价最小的全局参考路径，之后再采用第二算法进行 局部动态避障规划，根据局部近距离动态障碍物对路径进行实时调整。本发明提供的室内 机器人导航系统及方法，采用全局静态规划与局部动态避障规划相结合的路径规划算法对 机器人的路径进行实时规划，使得机器人可W根据局部近距离动态障碍物的更新做出路径 的调整，实用性高。
【附图说明】
[0018] 图1是本发明实施例提供的一种室内机器人导航系统的框图；
[0019] 图2是本发明实施例提供的一种室内机器人导航方法的流程图。
【具体实施方式】
[0020] 下面通过具体的实施例进一步说明本发明，但是，应当理解为，运些实施例仅仅是 用于更详细具体地说明之用，而不应理解为用于W任何形式限制本发明。
[0021 ]实施例一
[0022] 结合图1，本实施例提供一种室内机器人导航系统100,应用于机器人，包括；
[0023] 人机交互装置1:用于接收用户输入的机器人的终点位置坐标及机器人的终点位 置姿态信息；
[0024] 数据采集装置2:用于获取机器人的当前位置坐标和机器人的当前位置姿态信息；
[0025] 数据分析装置3,用于根据终点位置坐标，终点位置姿态信息，当前位置坐标，当前 位置姿态信息W及预先获取的二维全局静态地图，采用第一算法生成一条位于当前点与终 点之间的代价最小的全局参考路径；
[0026] 数据采集装置2,还用于实时更新距离机器人固定半径内的局部区域地图，获取动 态障碍物在局部区域地图中的第一坐标，并根据第一坐标进行坐标转换W获取动态障碍物 在二维全局静态地图中对应的第二坐标；
[0027] 数据分析装置3，还用于根据第二坐标，终点位置坐标，当前位置坐标，当前位置姿 态信息，终点位置姿态信息和全局参考路径，采用第二算法对机器人在下一刻的速度向量 进行最优规划，W获得最优速度向量，用于供底盘根据所述最优速度向量进行移动。
[0028] 本发明实施例提供的室内机器人导航系统100,使用人机交互装置1接收用户输入 的导航的终点位置坐标和终点位置姿态信息;数据采集装置2获取机器人的当前位置坐标 和当前位置姿态信息；之后，数据分析装置3，再根据终点位置坐标，终点位置姿态信息，当 前位置坐标，位置姿态信息W及预先获取的二维全局静态地图，采用第一算法生成一条位 于当前点与终点之间的代价最小的全局参考路径;在获得全局参考路径的同时，数据采集 装置2实时更新距离机器人固定半径内的局部区域地图，并获取动态障碍物在局部区域地 图中的第一坐标，并通过坐标转换将得到的第一坐标转换到二维全局静态地图坐标系中W 获取第二坐标;数据分析装置3再根据第二坐标，终点位置坐标，当前位置坐标，当前位置姿 态信息，终点位置姿态信息和全局参考路径，采用第二算法对机器人在下一刻的速度向量 进行最优规划，W获得最优速度向量，W使得底盘根据最优速度向量进行动作，从而使得机 器人向终点位置进行移动。此外，需要说明的是，本发明实施例的室内机器人导航系统100 是通过控制底盘的运动来实现对机器人的运动进行导航。
[0029] 本发明实施例提供的室内机器人导航系统100,先采用第一算法进行全局静态规 划，规划出一条位于当前点与终点之间的代价最小的全局参考路径，之后再采用第二算法 进行局部动态避障规划，根据局部近距离动态障碍物对路径进行实时调整。本发明实施例 提供的室内机器人导航系统100,采用全局静态规划与局部动态避障规划相结合的路径规 划算法对机器人的路径进行实时规划，使得机器人可W根据局部近距离动态障碍物的更新 做出路径的调整，实用性高。
[0030] 优选地，人机交互装置1包括一个可视化的交互界面。可W让用户自行设置导航终 点，并且将导航过程的地图信息、动态障碍物信息、规划出的全局参考路径、机器人的位置 姿态信息等实时显示在界面上，便于用户与机器人导航系统的交互。
[0031] 进一步优选地，数据采集装置2包括:RGB-D传感器、超声波测距仪、惯性测量传感 器。本发明实施例中，数据采集装置2为S维环境扫描设备，RGB-D传感器和超声波测距仪用 于采集室内环境颜色深度信息，惯性测量传感器用于采集机器人的位置姿态信息。
[0032] 此外，需要说明的是，与现有技术相比，本发明实施例中，数据采集装置2中的各设 备均为非激光雷达设备，成本更低。且本实施例没有使用现有技术中的路标和光学运动捕 捉系统，也就是说，本实施例不需要在环境中安装设置额外的装置来协助定位与导航，对外 部设备的依赖小，更加方便使用。同时，本实施例采用RGB-D传感器和超声波测距仪用于采 集室内环境颜色深度信息，与现有技术中的仅使用超声波、红外测距传感器相比，本实施例 采用视觉信息进行定位，精度更高，且对于障碍物实际位置的判断也更加精确，具有更高的 导航稳定性。
[0033] 优选地，第一算法为A*算法。本实施例的机器人依据二维全局静态地图、当前位置 坐标，当前位置姿态信息，目标位置坐标，目标位置姿态信息，在数据分析平台上利用A*算 法在全局静态地图上逐个迭代栅格点计算各条候选路径的代价，最终选择代价最小的候选 路径作为全局参考路径。
[0034] 此外，需要说明的是，本实施例中的A*算法为英译名称，在本领域还具有另外两个 名称，分别为A-Star算法和A星算法，但是，无论是称为A*算法，A-Star算法或A星算法，均是 本领域技术人员能够理解且接受的。
[00对进一步地，第二算法为DWA算法，其中，DWA算法又名：动态窗口方法（Dynamic Window ApproachiDWA)，需要说明的是，DWA算法是本领域技术人员能够理解并接收的通用 缩写简称。本实施例在获取全局参考路径的同时，数据采集装置2实时更新距离机器人固定 半径内的局部区域地图，并获取动态障碍物在局部区域地图中的第一坐标，并通过坐标转 换将得到的第一坐标转换到二维全局静态地图坐标系中W获取第二坐标，之后数据分析装 置3，再根据第二坐标，终点位置坐标，当前位置坐标，当前位置姿态信息，终点位置姿态信 息和全局参考路径，采用DWA算法对机器人在下一刻的速度向量进行最优规划，W获得最优 速度向量，W使得底盘根据最优速度向量进行动作，从而使得机器人向终点位置进行移动。
[0036]具体地，速度向量包括线速度和角速度。更加具体地，DWA算法的依据是:在一个较 短的时间间隔里机器人能够达到的线速度值变化范围和角速度值变化范围是一定的，且线 速度值变化范围与角速度值变化范围是根据实际所采用的电机的性能和地面摩擦系数决 定。一般的，为了便于计算，通常将线速度值变化范围和角速度值变化范围赋予选定的常数 进行计算。DWA算法根据线速度值变化范围和角速度值变化范围，通过构建一个动态窗口， 在动态窗口内部，通过计算下一时刻的机器人速度值来对可行的规划区域进行最终的调 整，最后在该可行的规划区域内遍历所有的线速度与角速度的取值，并将线速度与角速度 的值代入一个设定好的目标函数中，从而可W规划出机器人在下一时刻的速度向量。
[0037] 更加具体地，根据动力学原理，每一个时刻内通过执行DWA算法返回的线速度的模 与角速度可W得到一段圆弧轨迹，即每一个时刻行走的圆弧都可W独一无二地被一个速度 向量(vi，wi)所表示，其中i = l，2,3,…n。所W为了生成接下来n个时间间隔内当前点到目标 点P的轨迹，在不碰撞障碍物的前提下，数据分析需要在当前时刻决策ti到tn运n个时间间隔 内每一时刻的速度向量(Vi,Wi) dDWA算法的原理中指出运样的算法复杂度会随着时间间隔 数n成指数增长。
[0038] 为了让决策速度向量(vi，wi)的最优化问题具有可行性。DWA算法在当前时刻只考 虑接下来所有n个时刻的第一个时间间隔，从而假设接下来的n-1个时间间隔内的速度向量 与第一时刻决策的速度向量保持一致为一个常向量，即ti到tn运n-1个时刻的加速度为0。
[0039] 运样的简化是依据如下观测事实而提出的：
[0040] (1)简化后的规划空间是二维的，所W具有易处理的特点；
[0041] (2)每一个时刻的规划是重复进行的（没有必要在当前时刻计算接下来所有时刻 的最优化问题，否则复杂度急剧上升）；
[0042] (3)速度状态在没有新的控制指令传递下将会自动保持为一个常量。
[0043] 距离机器人周围较近的障碍物会对其线速度和角速度施加约束。最大的可行速度 取决于路径上下一个障碍物与机器人本身的距离，即当前的可行速度的集合是保证机器人 能够在接触障碍物前停下的速度范围（受到机器人底盘电机能够提供的最大加速度的影 响）。令对于速度向重(V ,W)的制动距罔为dist(v ,W),且^与如是加速度的临界值。由牛顿 第二定律可W计算出向量(V，w)的初步决策空间为：
[0044]
CD
[0045] 其中Va就是允许机器人不碰撞障碍物的所有速度向量(v，w)的集合。
[0046] 为了将电机等动力设备能够提供给机器人的极限加速度考虑进入对决策规划空 间的进一步约束。DWA算法将速度向量决策空间进一步筛选成下一个时间间隔内凭借加速 度极限值能够达到的速度范围。令t为时间间隔，巧日W为极限的加速度，（Va，Wa)为当前的速 度向量。所W动杰窗口 Vri被定父化下：
[0047]
(2)
[0048] 动态窗口是一块W当前速度为中屯、的W极限加速度为参数扩张成的一个矩形区 域。所有在动态窗口外的速度向量在下一个时间间隔内均不可能实现，所W都不被考虑。
[0049] 最后令线速度极限值与角速度极限值确定的决策空间外围为Vs。那么综上，最终 的决策区域Vr就被定义Vs、Va、Vd =个决策空间的交集：
[0050] Vr = Vs n Va n Vd (3)
[0051 ] Vr决策空间定义完成之后，将建立目标函数G( V，W)，通过在Vr向量空间中求解目标 函数G(V，w)的最大值来对下一时刻的速度向量(V，w)进行最终的决策。下面定义目标函数 为：
[0052] G(v,w)=曰? heading(v,w)+0 ? dist_ob(v,w)+丫 ? dist_gpath(v,w) (4)
[0053] 其中，目标函数是指向目标测量角函数heading(v，w)、与障碍物的间隙函数dist_ ob(v,w)、与全局参考路径的距离函数dist_gpath(v,w)的加权线性组合，且a，0，丫为S个 函数分别对应的权值，当=个权值分别取得合适的值时可W完成高效的动态避障任务。其 中指向目标测量角函数heading(v，w)描述了机器人朝向与目标点方向的同向程度，用180 ~0来表示，0是机器人正方向与目标点方向的夹角。由于运个方向对于不同的(v，w)都会有 所改变，e是针对机器人的下一预测位置计算得到的，通过假设机器人在下一间隔内W选中 的速度向量运动，从而求得下一预测位置坐标。实际的测量中，需要考虑到旋转过程的动力 学模型。所W0需要在下一个时间间隔后施加一个在最大减速度条件下最终到达的位置点 来计算得到。与障碍物的间隙函数dist_ob(v，w)描述了下一时刻机器人与路径上最近障碍 物的距离。如果路径上没有任何障碍物则方程返回一个较大的常数。与全局参考路径的距 离函数dist_gpath(v，w)返回了下一时刻机器人将处于的位置点与规划出的全局参考路径 的距离，表征了所规划的实时速度组合使得机器人下一时刻所处位置相对于最小代价位置 的差异。优选的，目标函数中的=个权值a，e，丫都被标准化到[0，1 ]之间。
[0054] 实施例二
[0055] 结合图2,本实施例提供一种室内机器人导航方法，应用于机器人，包括；
[0056] 步骤SI:人机交互装置1接收用户输入的机器人的终点位置坐标及机器人的终点 位置姿态信息；
[0057] 步骤S2:数据采集装置2获取机器人的当前位置坐标和机器人的当前位置姿态信 息；
[0058] 步骤S3:数据分析装置討良据终点位置坐标，终点位置姿态信息，当前位置坐标，当 前位置姿态信息W及预先获取的二维全局静态地图，采用第一算法生成一条位于当前点与 终点之间的代价最小的全局参考路径；
[0059] 步骤S4:数据采集装置2实时更新距离机器人固定半径内的局部区域地图，获取动 态障碍物在局部区域地图中的第一坐标，并根据第一坐标进行坐标转换W获取动态障碍物 在二维全局静态地图中对应的第二坐标；
[0060] 步骤S5:当机器人没有被障碍物包围时，为正常导航状态，数据分析装置3根据第 二坐标，终点位置坐标，当前位置坐标，当前位置姿态信息，终点位置姿态信息和全局参考 路径，采用第二算法对机器人在下一刻的速度向量进行最优规划，W获得最优速度向量，用 于供底盘根据所述最优速度向量进行移动。
[0061] 本发明实施例提供的室内机器人导航方法，使用人机交互装置1接收用户输入的 导航的终点位置坐标和终点位置姿态信息;数据采集装置2获取机器人的当前位置坐标和 当前位置姿态信息；之后，数据分析装置3，再根据终点位置坐标，终点位置姿态信息，当前 位置坐标，位置姿态信息W及预先获取的二维全局静态地图，采用第一算法生成一条位于 当前点与终点之间的代价最小的全局参考路径;在获得全局参考路径的同时，数据采集装 置2实时更新距离机器人固定半径内的局部区域地图，并获取动态障碍物在局部区域地图 中的第一坐标，并通过坐标转换将得到的第一坐标转换到二维全局静态地图坐标系中W获 取第二坐标；当机器人没有被障碍物包围时，数据分析装置3再根据第二坐标，终点位置坐 标，当前位置坐标，当前位置姿态信息，终点位置姿态信息和全局参考路径，采用第二算法 对机器人在下一刻的速度向量进行最优规划，W获得最优速度向量，W使得底盘根据最优 速度向量进行动作，从而使得机器人向终点位置进行移动。此外，需要说明的是，本发明实 施例的室内机器人导航方法是通过控制底盘的运动来实现对机器人的运动进行导航。
[0062] 本发明实施例提供的室内机器人导航方法，先采用第一算法进行全局静态规划， 规划出一条位于当前点与终点之间的代价最小的全局参考路径，之后再采用第二算法进行 局部动态避障规划，根据局部近距离动态障碍物对路径进行实时调整。本发明实施例提供 的室内机器人导航方法，采用全局静态规划与局部动态避障规划相结合的路径规划算法对 机器人的路径进行实时规划，使得机器人可W根据局部近距离动态障碍物的更新做出路径 的调整，实用性高。
[0063] 优选地，步骤S5中，当机器人被障碍物包围而无法进行导航时，还包括:数据分析 装置3发出一个预设的速度向量给底盘W进行原地旋转;数据采集装置2持续扫描并更新局 部区域地图，直到局部区域地图中的障碍物的分布允许机器人通过，返回正常导航状态，其 中，障碍物包括动态障碍物和静态障碍物。当动态障碍物或者动态障碍物与静态障碍物一 起形成包围将机器人围住时，机器人将进入原地旋转状态，持续扫描并更新周围的障碍物 信息，与此同时，数据分析装置3将会在局部区域地图中清除上一周期旋转扫描得到的周围 障碍物信息，并重建当前周期旋转扫描得到的障碍物信息。直到周围障碍物的分布允许机 器人通过后，终止运一循环，返回正常的导航状态。
[0064] 优选地，步骤S3中，二维全局静态地图的预先获取步骤包括:数据采集装置2围绕 当前环境进行一次全局扫描W获得若干帖=维点云，并采用=维SLAM算法将若干帖=维点 云拼接成为=维全局静态地图；数据分析装置3对=维全局静态地图进行降维度处理，W获 得二维全局静态地图。本实施例通过预先对=维点云地图进行投影并获得二维全局静态地 图，其目的在于，节省存储空间，简化数据存储的空间开销，W提高导航效率。
[0065] 优选地，第一算法为A*算法。本实施例的机器人依据二维全局静态地图、当前位置 坐标，当前位置姿态信息，目标位置坐标，目标位置姿态信息，在数据分析平台上利用A*算 法在全局静态地图上逐个迭代栅格点计算各条候选路径的代价，最终选择代价最小的候选 路径作为全局参考路径。
[0066] 此外，需要说明的是，本实施例中的A*算法为英译名称，在本领域还具有另外两个 名称，分别为A-Star算法和A星算法，但是，无论是称为A*算法，A-Star算法或A星算法，均是 本领域技术人员能够理解且接受的。
[0067] 进一步地，第二算法为DWA算法。其中，DWA算法又名：动态窗口方法（Dynamic Window ApproachiDWA)，需要说明的是，DWA算法是本领域技术人员能够理解并接收的通用 缩写简称。本实施例在获取全局参考路径的同时，数据采集装置2实时更新距离机器人固定 半径内的局部区域地图，并获取动态障碍物在局部区域地图中的第一坐标，并通过坐标转 换将得到的第一坐标转换到二维全局静态地图坐标系中W获取第二坐标，之后数据分析装 置3，再根据第二坐标，终点位置坐标，当前位置坐标，当前位置姿态信息，终点位置姿态信 息和全局参考路径，采用DWA算法对机器人在下一刻的速度向量进行最优规划，W获得最优 速度向量，W使得底盘根据最优速度向量进行动作，从而使得机器人向终点位置进行移动。
[0068] 具体地，速度向量包括线速度和角速度。更加具体地，DWA算法的依据是:在一个较 短的时间间隔里机器人能够达到的线速度值变化范围和角速度值变化范围是一定的，且线 速度值变化范围与角速度值变化范围是根据实际所采用的电机的性能和地面摩擦系数决 定。一般的，为了便于计算，通常将线速度值变化范围和角速度值变化范围赋予选定的常数 进行计算。DWA算法根据线速度值变化范围和角速度值变化范围，通过构建一个动态窗口， 在动态窗口内部，通过计算下一时刻的机器人速度值来对可行的规划区域进行最终的调 整，最后在该可行的规划区域内遍历所有的线速度与角速度的取值，并将线速度与角速度 的值代入一个设定好的目标函数中，从而可W规划出机器人在下一时刻的速度向量。
[0069] 更加具体地，根据动力学原理，每一个时刻内通过执行DWA算法返回的线速度的模 与角速度可W得到一段圆弧轨迹，即每一个时刻行走的圆弧都可W独一无二地被一个速度 向量(vi，wi)所表示，其中i = l，2,3,…n。所W为了生成接下来n个时间间隔内当前点到目标 点P的轨迹，在不碰撞障碍物的前提下，数据分析需要在当前时刻决策ti到tn运n个时间间隔 内每一时刻的速度向量(Vi,Wi) dDWA算法的原理中指出运样的算法复杂度会随着时间间隔 数n成指数增长。
[0070] 为了让决策速度向量(Vi，Wi)的最优化问题具有可行性。DWA算法在当前时刻只考 虑接下来所有n个时刻的第一个时间间隔，从而假设接下来的n-1个时间间隔内的速度向量 与第一时刻决策的速度向量保持一致为一个常向量，即ti到tn运n-1个时刻的加速度为0。
[0071] 运样的简化是依据如下观测事实而提出的：
[0072] (1)简化后的规划空间是二维的，所W具有易处理的特点；
[0073] (2)每一个时刻的规划是重复进行的（没有必要在当前时刻计算接下来所有时刻 的最优化问题，否则复杂度急剧上升）；
[0074] (3)速度状态在没有新的控制指令传递下将会自动保持为一个常量。
[0075] 距离机器人周围较近的障碍物会对其线速度和角速度施加约束。最大的可行速度 取决于路径上下一个障碍物与机器人本身的距离，即当前的可行速度的集合是保证机器人 能够在接触障碍物前停下的速度范围（受到机器人底盘电机能够提供的最大加速度的影 响）。令对于速度向量(V，W)的制动距离为diSt (V，W)，且砖与是加速度的临界值。由牛顿 第二定律可W计算出向量(v，w)的初步决策空间为：
[0076]
( 1 )
[0077] 其中Va就是允许机器人不碰撞障碍物的所有速度向量(v，w)的集合。
[0078] 为了将电机等动力设备能够提供给机器人的极限加速度考虑进入对决策规划空 间的进一步约束。DWA算法将速度向量决策空间进一步筛选成下一个时间间隔内凭借加速 度极限值能够达到的速度范围。令t为时间间隔，1)和W为极限的加速度，（Va，Wa)为当前的 速度向音。所W动杰窗口 W献吿公如下，
[0079]
(2)
[0080] 动态窗口是一块W当前速度为中屯、的W极限加速度为参数扩张成的一个矩形区 域。所有在动态窗口外的速度向量在下一个时间间隔内均不可能实现，所W都不被考虑。
[0081] 最后令线速度极限值与角速度极限值确定的决策空间外围为Vs。那么综上，最终 的决策区域Vr就被定义Vs、Va、Vd =个决策空间的交集：
[0082] Vr = Vs n Va n Vd (3)
[0083] Vr决策空间定义完成之后，将建立目标函数G( V，W)，通过在Vr向量空间中求解目标 函数G(V，w)的最大值来对下一时刻的速度向量(V，w)进行最终的决策。下面定义目标函数 为：
[0084] G(v,w)=曰? heading(v,w)+0 ? dist_ob(v,w)+丫 ? dist_gpath(v,w) (4)
[0085] 其中，目标函数是指向目标测量角函数heading(v，w)、与障碍物的间隙函数dist_ ob(v,w)、与全局参考路径的距离函数dist_gpath(v,w)的加权线性组合，且a，0，丫为S个 函数分别对应的权值，当=个权值分别取得合适的值时可W完成高效的动态避障任务。其 中指向目标测量角函数heading(v，w)描述了机器人朝向与目标点方向的同向程度，用180 ~0来表示，0是机器人正方向与目标点方向的夹角。由于运个方向对于不同的(v，w)都会有 所改变，e是针对机器人的下一预测位置计算得到的，通过假设机器人在下一间隔内W选中 的速度向量运动，从而求得下一预测位置坐标。实际的测量中，需要考虑到旋转过程的动力 学模型。所W0需要在下一个时间间隔后施加一个在最大减速度条件下最终到达的位置点 来计算得到。与障碍物的间隙函数dist_ob(v，w)描述了下一时刻机器人与路径上最近障碍 物的距离。如果路径上没有任何障碍物则方程返回一个较大的常数。与全局参考路径的距 离函数dist_gpath(v，w)返回了下一时刻机器人将处于的位置点与规划出的全局参考路径 的距离，表征了所规划的实时速度组合使得机器人下一时刻所处位置相对于最小代价位置 的差异。优选的，目标函数中的=个权值a，e，丫都被标准化到[0，1 ]之间。
[0086]尽管本发明已进行了一定程度的描述，明显地，在不脱离本发明的精神和范围的 条件下，可进行各个条件的适当变化。可W理解，本发明不限于所述实施方案，而归于权利 要求的范围，其包括所述每个因素的等同替换。
【主权项】
1. 一种室内机器人导航系统，应用于机器人，其特征在于，包括： 人机交互装置：用于接收用户输入的所述机器人的终点位置坐标及所述机器人的终点 位置姿态信息； 数据采集装置:用于获取所述机器人的当前位置坐标和所述机器人的当前位置姿态信 息； 数据分析装置，用于根据所述终点位置坐标，所述终点位置姿态信息，所述当前位置坐 标，当前位置姿态信息以及预先获取的二维全局静态地图，采用第一算法生成一条位于当 前点与终点之间的代价最小的全局参考路径； 数据采集装置，还用于实时更新距离机器人固定半径内的局部区域地图，获取动态障 碍物在所述局部区域地图中的第一坐标，并根据所述第一坐标进行坐标转换以获取所述动 态障碍物在所述二维全局静态地图中对应的第二坐标； 数据分析装置，还用于根据所述第二坐标，所述终点位置坐标，所述当前位置坐标，所 述当前位置姿态信息，所述终点位置姿态信息和所述全局参考路径，采用第二算法对机器 人在下一刻的速度向量进行最优规划，以获得最优速度向量，用于供机器人根据所述最优 速度向量进行移动。2. 根据权利要求1所述的室内机器人导航系统，其特征在于，所述人机交互装置包括一 个可视化的交互界面。3. 根据权利要求1所述的室内机器人导航系统，其特征在于，所述数据采集装置包括： RGB-D传感器、超声波测距仪、惯性测量传感器。4. 根据权利要求1至3中任一项所述的室内机器人导航系统，其特征在于，所述第一算 法为A*算法。5. 根据权利要求4所述的室内机器人导航系统，其特征在于，所述第二算法为DWA算法。6. -种室内机器人导航方法，应用于机器人，其特征在于，包括： 步骤SI:人机交互装置接收用户输入的所述机器人的终点位置坐标及所述机器人的终 点位置姿态信息； 步骤S2:数据采集装置获取所述机器人的当前位置坐标和所述机器人的当前位置姿态 信息； 步骤S3:数据分析装置根据所述终点位置坐标，所述终点位置姿态信息，所述当前位置 坐标，当前位置姿态信息以及预先获取的二维全局静态地图，采用第一算法生成一条位于 当前点与终点之间的代价最小的全局参考路径； 步骤S4:数据采集装置实时更新距离机器人固定半径内的局部区域地图，获取动态障 碍物在所述局部区域地图中的第一坐标，并根据所述第一坐标进行坐标转换以获取所述动 态障碍物在所述二维全局静态地图中对应的第二坐标； 步骤S5:当所述机器人没有被障碍物包围时，为正常导航状态，数据分析装置根据所述 第二坐标，所述终点位置坐标，所述当前位置坐标，所述当前位置姿态信息，所述终点位置 姿态信息和所述全局参考路径，采用第二算法对机器人在下一刻的速度向量进行最优规 划，以获得最优速度向量，用于供机器人根据所述最优速度向量进行移动。7. 根据权利要求6所述的室内机器人导航方法，其特征在于，所述步骤S5中，当所述机 器人被障碍物包围而无法进行导航时，还包括： 数据分析装置发出一个预设的速度向量给底盘以进行原地旋转； 数据采集装置持续扫描并更新所述局部区域地图，直到所述局部区域地图中的障碍物 的分布允许机器人通过，返回正常导航状态，其中，所述障碍物包括所述动态障碍物和静态 障碍物。8. 根据权利要求7所述的室内机器人导航方法，其特征在于，所述步骤S3中，所述二维 全局静态地图的预先获取步骤包括： 数据采集装置围绕当前环境进行一次全局扫描以获得若干帧三维点云，并采用三维 SLAM算法将所述若干帧三维点云拼接成为三维全局静态地图； 数据分析装置对所述三维全局静态地图进行降维度处理，以获得二维全局静态地图。9. 根据权利要求8所述的室内机器人导航系统，其特征在于，所述第一算法为A*算法。10. 根据权利要求9所述的室内机器人导航系统，其特征在于，所述第二算法为DWA算 法。
【文档编号】G05D1/02GK105955273SQ201610353911
【公开日】2016年9月21日
【申请日】2016年5月25日
【发明人】杭蒙, 陈震, 朱骋, 王皓月, 李经纬, 蒋申璐, 杨永全, 任海波, 杜新鹏
【申请人】速感科技（北京）有限公司