CN103558856A - 动态环境下服务动机器人导航方法 - Google Patents

Abstract

本发明涉及移动机器人自主导航技术领域。本发明公开了一种动态环境下服务动机器人导航方法。包括如下步骤：1)利用室内环境下多台全局摄像机与机器人车载激光传感器实现人的位置跟踪；2)根据采集样本对特定室内环境场所下人的运动模式进行训练，并对人的运动趋势进行预测；3)根据人的当前位置和预测位置，与环境静态障碍栅格地图相融合，生成导航风险概率地图；4)采用全局路径规划-局部避障控制层次化结构的机器人导航运动控制器，实现机器人导航行为控制，通过控制确保机器人在与人共处的复杂动态环境下安全高效的导航行为。

Description

动态环境下服务动机器人导航方法

技术领域

本发明涉及移动机器人导航技术领域，特别是涉及一种多约束动态环境下服务动机器人导航方法。服务机器人在与人共处的动态环境下导航，除了常规的避障以外还存在多种约束条件相互叠加，而其中由于人的运动对机器人造成的干扰是不完全已知的。本发明对这种动态干扰进行量化和一定预测，并将机器人的其它多种运动规则转换为约束的形式融入可达代价导航栅格，通过建立两层级联的运动控制器，实现了多约束条件制约下的机器人优化导航行为控制，从而有助于提高服务机器人未来进入家庭后的可靠导航作业问题。

背景技术

移动机器人的点到点导航，需要依据某些优化准则(时间或行程最短)，在其位形空间(ConfigurationSpace，CSpace)中搜索一条合理、完备、最优、可实时计算、能适应环境变化的路径。从机器人获取环境信息的程度可分为三种情况：

(1)环境信息完全已知，这是基于环境模型的全局路径规划的前提；

(2)环境信息完全未知或部分已知，这是基于传感器信息的局部路径规划所适用的情况；

(3)环境中存在以可知或可预知方式移动的障碍物(例如人)，此时传统的局部规划法虽然能保证避障，但是局部性的避障行为难以提高导航的全局效率。

对于动态环境，为了提高动态路径重规划的计算效率，一般采用基于滚动窗口的移动机器人路径规划方法，充分利用机器人实时测得的局部环境信息，以滚动方式进行在线规划，对动态环境具有良好的适应性。Minguez采用了全局/局部规划两层控制结构（参见“Sensor-basedrobotmotiongenerationinunknown,dynamicandtroublesomescenarios,RoboticsandAutonomousSystems,2005”），将wavefront路径规划与ND反应式避障算法组成级联，提高了在存在人的动态环境下机器人避碰运动的路径寻优能力与快速响应能力。

动态环境下对运动物体的预测导航(PredictiveNavigation)是提高导航全局效率的新方法。预测导航需要恢复避让对象的位置速度等状态信息，通过将CSpace扩展为完整的状态－时间描述或者把障碍物运动轨迹直接映射到速度空间进行全局运动规划。例如针对足球机器人，Gupta（参见“Real-TimeIdentificationandPredictiveControlofFastMobileRobotsusingGlobalVisionSensing,IEEETransactionsonInstrumentationandMeasurement,2005”）线性预测了直线运动的障碍物（足球）的运动速度变化。

服务型移动机器人工作在与人共处的动态环境下。对运动人/物体的动态性加以预测，是机器人改善避障导航性能的有效途径。现有预测避让导航方法一般建立在动态物体运动路线已知、速度已知、匀速运动、或者均加速运动等各种假设上。由于人的运动状态不可知，人的运动预测不同于运动物体的预测，假设其运动路线与具体运动速度已知在实际应用中是不现实的。近年来国外研究在针对人的机器人预测导航方面出现了一些有益的尝试，Bennewitz（参见“UsingEMtoLearnMotionBehaviorsofPersonswithMobileRobots,IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems,2002”）提出的方法建模并预测了人的运动路线模式，却没有考虑人沿着某路线行走的速度变化对预测人机相遇冲突带来的影响。

经专利检索查新，王耀南等人申请了中国发明专利第200910044273.5号，名称为“未知环境下移动机器人导航安全的方法”。该发明公开了一种借助栅格地图和模糊神经网络控制器来确保移动机器人在未知环境下安全探索导航的方法。但是该方法没有考虑环境中的动态物体及人对机器人在栅格地图中导航所造成的影响，其自主导航系统也仅采用了机器人车载测距传感器来感知环境障碍物，而车载测距传感器在探测范围上具有一定局限性。

梁华为等人申请了中国发明专利第200610096976.9号，名称为“一种机器人导航定位系统及导航定位方法”。该发明公开了一种机器人导航定位系统及导航定位方法。系统包括应用于机器人导航定位系统的无线传感器网络和机器人，无线传感器网络节点由传感单元、处理单元、定位单元、无线模块和电源模块组成；导航定位是通过布撒于监控区域内的无线传感器网络节点辅助机器人进行环境建模、定位、路径规划和导航。虽然该方法采用了分布式感知技术来辅助机器人导航，但也没有考虑通过对人的跟踪和运动预测来指导机器人对其合理避让。

贾庆轩等人申请了中国发明专利第201210355341.1号，名称为“预选择最小距离指标冗余机器人动态避障方法”。该方法考虑了障碍物随机运动的情况下，预选择最小距离指标冗余机器人动态避障路径规划，提高了动态避障的规划效率，满足实时性的要求。该方法适用于一般随机运动的障碍物情况，但是对于家庭室内环境下机器人导航，该方法不适合用于对人的运动进行跟踪和预测。此外，一般现有方法大多采用避障路径重规划方式来机械地避让，而没有考虑采取绕行、减速、等待等其它避让策略，往往会导致机器人徘徊运动，降低了机器人导航作业的效率。

根据以上调研，虽然机器人避障和导航控制已有较为广泛的研究，目前尚未特别针对与人共存机器人(Human-symbioticrobots)的导航控制问题建立相应的技术方法。服务机器人在与人共处的动态环境下导航，除了常规的避障以外还存在多种约束条件相互叠加，其中由于人的运动对机器人造成的干扰是不完全已知的，而目前已有的技术方案大多没有考虑这些因素，这制约了服务机器人的安全可靠导航性能。

发明内容

技术问题：针对与人共处动态环境下服务动机器人导航，本发明提出了一种多约束动态环境下服务动机器人导航方法。

技术方案：多约束动态环境下服务动机器人导航方法，所述方法包括：

利用室内环境下多台全局摄像机与机器人车载激光传感器组成的分布式传感器网络，实现全局地图中动态人的定位与跟踪；

通过采集人的长期运动轨迹学习上述室内环境下人的运动模式；而机器人在导航过程中，通过对人运动的长期运动规律预测和短期运动速度、方向预测相结合，对人的时空运动趋势进行估计；

根据人的当前位置和上述估计得到的预测位置，与环境静态障碍栅格地图相融合，生成导航风险概率地图；

采用全局路径规划-局部避障控制层次化结构的机器人导航运动控制器，实现机器人导航行为控制。通过控制并最终确保机器人在与人共处的复杂动态环境下安全高效的导航行为。

其中所述的人的定位与跟踪，是在室内环境下采用多台全局摄像机与机器人车载的单台激光扫描器实现人的位置跟踪，通过标定全局摄像机与全局栅格地图的坐标转换关系，获取栅格地图中人的全局位姿及运动速度。

其中所述的对人的时空运动趋势进行估计，步骤如下：

1)首先在人的定位与跟踪基础上进行人的运动轨迹模式训练和预测：长期采集的行人位置数据，采用EM算法学习其运动模式（轨迹），对每种运动轨迹模式在x-y平面中用混合高斯分布加以建模，在实时对人运动位置进行跟踪的基础上，依据人的运动历史估计出当前运动遵循轨迹模式的概率；

2)其次进行人的短期运动预测：假设人的运动速度和方向角每过一定时刻速度和方向角变化一次，且在一定范围内变化，各个时刻上的速度和方向角分别为相应范围内的随机变量且相互独立，对人沿着该预测路径前进的短期运动速度和方向角进行预测。

生成所述导航风险概率地图还包括对运动人的预测避让可达代价的导航约束条件，即当人机运动可能在狭窄通道内出现冲突时，通过预测人的运动位置来估计人的时空可达代价，并以降低人机运动冲突或阻塞风险为目标，控制机器人提前绕行或减速避让。

生成所述导航风险概率地图还包括带裕量的机器人四周安全范围可达代价的导航约束条件，即在机器人坐标位置的四周，用高斯分布的点云表示该点处障碍对机器人安全威胁概率。

生成所述导航风险概率地图还包括受建筑物遮挡或警戒区域绕行可达代价的导航约束条件，即控制机器人缓慢地走出建筑物遮挡范围。

生成所述导航风险概率地图还包括遵守行驶靠向习惯可达代价的导航约束条件，即将靠向准则引入机器人运动规划，确保机器人在与人相遇时双方遵守相同的靠向规则，达到最优的导航效率。

将环境中已知静态障碍物采用栅格占有地图描述，与按上述四条新的导航约束条件计算所得的可达代价相融合，更新导航风险概率地图。

其中所述的机器人导航运动控制器包括全局规划器和局部反应式壁障控制器两个子模块：

1)全局规划器在导航风险概率地图上进行全局路径规划，得到全局最优的路径点及每个点上的运动方向。

2)局部反应式避障控制器确保机器人在线运动时，以当前点的目标朝向为参考，根据以0.1cm为分辨率的动态窗口中激光传感器实际获取的障碍物情况，计算避障方向并求出平滑变化的控制量，包括平移速度υ和旋转速度ω。

采用运动规划-避障控制层次化结构的机器人导航运动控制器，实现机器人在与人共处的复杂动态环境下安全高效的导航行为。

本发明采用上述技术方案，具有以下有益效果：

1、当机器人与人位于两个相对独立的空间下、使用机器人车载传感器无法获知人的位置时，利用分布式感知进行人的定位与跟踪，并对人的全局运动进行预测，从而确保机器人对人的运动具有一定预见性。

2、建立了一种融合多约束的导航方法，其中对人动态避让利用了特定环境下人的运动路径具有一定规律的特性，预测避让的效果相比传统地机械式避障方法，能在确保机器人自主导航效率的基础上，提高机器人导航行为对人的安全性、预见性和友好性，有助于解决该类服务机器人未来进入家庭后的人性化作业问题。

附图说明

图1为本发明实施实例的动态环境下服务动机器人导航流程；

图2为本发明实施实例的分布式传感器下人的定位与跟踪，采用包括全局摄像机和机器人车载激光传感器组成的分布式传感器网络；

图3为本发明实施实例的人的运动轨迹模式学习，（a）为人的运动轨迹样本，（b）为训练得到的几种典型轨迹模式；

图4为本发明实施实例的人的短期运动方向（a）与速度预测（b）示意图；

图5是本发明实施实例中人的运动模式预测，（a）（b）（c）对应了人运动过程中的三个位置情况，上半图是摄像机对人的跟踪，下半图是栅格地图中人的位置及运动趋势预测，其中字母表示地点路标，轨迹旁的数字表示人沿着该路径模式的概率；

图6是本发明实施实例中，障碍物及行人四周膨胀安全区域；

图7为本发明实施实例中，室内环境下的可达受遮挡区域；

图8为本发明实施实例中，以走廊环境为例，说明右向行驶规则相应的代价栅格；

图9为本发明实施实例中，导航风险概率地图的更新与导航控制器结构；

图10在一个较为狭窄、含多个通道的实例房间环境中，本发明提出的导航结果与传统的重规划式避障导航控制方式结果对比。

图11是机器人靠近人的导航行为实例，(a)场景示意;(b)三条路径的选择。

具体实施方式

下面结合具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

本发明实施实例的方法步骤是：

利用室内环境下多台全局摄像机与机器人车载激光传感器组成的分布式传感器网络，实现全局地图中动态人的位置跟踪；

通过采集人的长期运动轨迹学习特定室内环境下人的运动模式；而机器人在导航过程中，通过对人运动的长期运动规律预测和短期运动速度、方向预测相结合，对人的时空运动趋势进行估计；

根据人的当前位置和预测位置，将包含对人动态避让在内的多种机器人导航规则转换为导航约束条件，与环境静态障碍栅格地图相融合，生成导航风险概率地图；

采用全局路径规划-局部避障控制层次化结构的机器人导航运动控制器，实现机器人导航行为控制，并最终确保机器人在与人共处的复杂动态环境下安全高效的导航行为。

具体实现方式为：

如图1所示为本实施例的机器人预测导航流程步骤。总体分为训练和运行两个阶段：在训练阶段，首先系统利用分布式摄像机与机器人车载激光组成的分布式传感器网络进行人的定位和跟踪（S1），将定位结果保存在机器人栅格地图坐标系下，同时采集并保存人的位置数据；然后判断是否采集了足够的样本并完成了训练（S2）；如果未完成训练，则进行人的运动轨迹模式样本训练（S3），训练后返回S1继续执行，否则进入运行阶段。在运行阶段，首先对人的长期运动模式和短期运动进行预测（S4），然后进行融合多约束的导航风险概率地图更新（S5）；接着通过层次化的导航运动控制器（S6），向机器人底轮输出运动控制量（S7），机器人利用车载定位系统判断自身位置，直到其到达目的地（S8）。

人机共处运动情况下，人的未知动态性是机器人自定位系统的主要干扰，恢复人的位置信息有利于提高机器人定位的准确性；而机器人车载激光测距器对人的检测又极易受到混杂背景中的干扰。为此，在实施例子中首先采用同时定位与人的跟踪（SLAP）算法获取人的位置信息，从而为人的运动估计与预测等后续工作提供依据。在实施例子中，采用如图2所示的全局摄像机和机器人激光传感器组成的分布式传感器网络，将各个传感器数据通过局域网传送到中心服务器，在中心服务器上利用融合分布式多源感知的Rao-Blackwellized粒子滤波算法(RBPF)进行同时定位与人的跟踪，即同时更新人-机状态的后验分布估计，获得机器人全局位姿r_k＝(x_r,_yr,θ_r)及运动速度(v_r,w_r)、人的全局位置h_k＝(x_h,y_h,θ_h)及运动速度(v_h,w_h)。其中x_r和y_r是机器人在全局x-y地图中的位置坐标，θ_r是机器人姿态角，v_r和w_r分别是机器人运动线速度和角速度。x_h和y_h是人在全局x-y地图中的位置坐标，θ_h是人的即时运动方向与地图坐标系x轴正方向的夹角。而下标k表示第k个时刻。

在实施例子中，机器人车载的测距激光传感器获得的数据是激光在测距高度35cm平面上扫描得到的环境中障碍物上各个点相对于移动机器人的距离和角度，在0°～180°范围内每1°分辨率获得一个激光束数据，共计181个激光束。环境中的分布式全局摄像机为CCD摄像头，采集RGB图像分辨率320*240；在实时例子中，在约80平米的室内环境下共布置了5台不同视角的全局摄像机。

人的运动轨迹模式分析与运动预测的实施步骤如下：

1)首先在人的定位与跟踪基础上进行人的运动轨迹模式训练和预测。利用长期采集的行人位置数据，采用EM算法学习其运动轨迹模式（轨迹）共M种，在实施实例环境中轨迹模式的类型数目M一般满足4≤M≤20。这M种运动轨迹模式的集合记为Ψ={Ψ₁,...,Ψ_M}。每种运动轨迹模式Ψ_m在x-y平面中用L_m个分量的高斯分布链来描述，

是第l个混合分量，

是混合系数。图3（a）为在某实验环境下采集到的行人运动轨迹实例。经过模型训练，共得到

共M种模型，p_pattern(Ψ_m|h_k)为依据人的运动历史观测估计其当前运动遵循某一类模式Ψ_m的概率。图3（b）是以这些历史轨迹数据为样本训练得到轨迹模式（仅显示了其中四种），每个轨迹模型用一系列高斯分布表示，图中的椭圆即代表x-y平面中的一个高斯分布。可见在该室内环境下，人的长期运动规律与室内场所语义上下文有一定联系。人的运动轨迹呈现为重要地点（例如饮水机、桌子、沙发、打印机等）之间的联通路线。

在实时对人运动位置进行跟踪的基础上，依据人的运动历史估计出当前运动遵循轨迹模式的概率。假设人的平面位置h_k，其属于给定某个轨迹模式Ψ_m中第l个高斯分布的概率记为，按式（1）计算为：

$p\left(h_k\right|{\mathrm{\ψ}}_m^l)=\frac{1}{\sqrt{2\mathrm{\π}}{\mathrm{\Σ}}_m^l}\·\exp (-{\left|\right|h_k-{\mathrm{\μ}}_m^l\left|\right|}^2/2{\left({\mathrm{\Σ}}_m^l\right)}^2)---\left(1\right)$

其中和

分别为第l个高斯分布的均值和协方差。

如不考虑行人沿该路径模式运动的短期速度变化和方向角变化，则根据人的历史位置信息h_1:k预测人在k时刻位置h_k属于某个运动轨迹模式Ψ_m的概率计算为：

p(Ψ_m,l,l′|h_1:k)＝ηp(h_1:k|Ψ_m,l,l′)p(Ψ_m)p(l,l′|Ψ_m)    （2）

$p_{\mathrm{pattern}}\left(h_t\right|{\mathrm{\ψ}}_m)=\underset{l=1}{\overset{l_m}{\mathrm{\Σ}}}\underset{l^{\′}=k}{\overset{l_m}{\mathrm{\Σ}}}p\left(h_k\right|{\mathrm{\ψ}}_m,l,l^{\′},h_{l:k})p({\mathrm{\ψ}}_m,l,{l^{\′}}_{}|h_{l:k})---\left(3\right)$

概率p(h_t|Ψ_m,l,l′,h_1:k)估计的是给定历史位置信息h_1:k、且假设h_1:k从

开始到

结束。概率p(Ψ_m,l,l′,h_1:k)可以按照贝叶斯公式进行分解：

p(Ψ_m,l,l′,h_1:k)＝ηp(h_1:k|Ψ_m,l,l′)p(Ψ_m)p(l,l′|Ψ_m)    （4）

其中η是归一化因子，p(h_1:k|Ψ_m,l,l′)是h_1:k的观测似然概率，p(Ψ_m)和p(l,l′|Ψ_m)是关于轨迹模式训练结果的两个先验分布。

2)其次进行人的短期运动预测，实施方法如下：

假设人的运动速度和方向角变化范围分别为[v_min,v_max]与[θ_min,θ_max]，每过ΔT时刻速度和方向角变化一次，各个时刻上的速度和方向角分别为上述范围内的随机变量且相互独立。这是对室内人运动的一种简单而有效的模型，即速度和方向角均在一定范围内随机变化，大致保持匀速运动。以人的当前位置h₀=(x₀,y₀)为原点、当前人的运动方向(Person’sInstantaneousOrientation，PIO)为对称轴、Λ＝θ_max为最大偏角的扇形区域表示了当前行人运动的方向不确定性。假设任意点极坐标h＝(r,α)，其方向不确定性可以简单计算为p_orien(h|h₀)＝exp(-α²)，直观地表示运动方向偏离当前PIO的角度α越大，其可能性越小。如图4（a）所示。而行人若沿着直线路径行走，则在某一时刻k能到达直线上位置h_k的概率记为p_vel(h_k;k)，如图4（b）所示。首先仅考虑速度不确定性，记h₀=(x₀,y₀)为人的当前位置，θ_h,0为当前人的运动方向角，

为当前人的位置分布方差，v_i为第i时刻人的运动速度，估计第k时刻的位置为：

$x_k=x_0+{\mathrm{\Σ}}_{i=1}^k{v}_i\mathrm{\Δ}T\cos {\mathrm{\θ}}_{h,0},y_k=y_0+{\mathrm{\Σ}}_{i=1}^k{v}_i\mathrm{\Δ}T\sin {\mathrm{\θ}}_{h,0}\·---\left(5\right)$

其中x_k和y_k分别为第k时刻人在全局地图坐标系下位置的x与y方向分量。每过一个单位时间增加的位置方差

计算为：

${\mathrm{\σ}}_{\mathrm{step}}^2=\frac{1}{12}{(v_{\max }-v_{\min })}^2\·---\left(6\right)$

人在第k时刻运动到位置h_k的概率分布为：

$p_{\mathrm{vel}}(h_k;k_{})=\frac{1}{2{\mathrm{\π\σ}}_{x,k}{\mathrm{\σ}}_{y,k}}\exp \{-\frac{1}{2}(\frac{{(x_k-{\stackrel{\_}{x}}_k)}^2}{{\mathrm{\σ}}_{x,y}^2}+\frac{{(y_k-{\stackrel{\_}{y}}_k)}^2}{{\mathrm{\σ}}_{y,k}^2})\}\·---\left(7\right)$

其中变量 ${\stackrel{\_}{x}}_k=x_0+k\stackrel{\_}{v}\mathrm{\Δ}T\cos {\mathrm{\θ}}_{h,0},$ ${\stackrel{\_}{y}}_k=y_0+k\stackrel{\_}{v}\mathrm{\Δ}T\sin {\mathrm{\θ}}_{h,0},$ ${\mathrm{\σ}}_{x,k}^2={\mathrm{\σ}}_{y,k}^2={\mathrm{\σ}}_0^2+k{\mathrm{\σ}}_{\mathrm{step}}^{20},$

$\stackrel{\_}{v}=\frac{v_{\max }+v_{\min }}{2}.$

综合考虑人运动的长/短期不确定性，用方向预测概率密度作为速度预测概率密度的指数折扣因子γ，递推估计从0时刻位置h₀运动到k时刻位置h_k的概率计算如下式所示，α为归一化因子。

p_predict(h_k;k)＝αp_pattern(Ψ_m|h_k)p_vel(h_k;k)^γ,γ＝p_orien(h_k|h_k-1).    （8）

在具体实施中，融合多约束的导航风险概率地图更新步骤为：

1)计算规则一“对运动人的预测避让”的可达代价。当人机运动可能在狭窄通道内出现冲突时，机器人应当让人先行通过而不是堵住通道的入/出口。这不仅能避免人机运动的正面冲突，更重要的是能够给予人充分的尊重。将短期未来某时刻人的预测位置融入栅格地图，称为“运动预测代价栅格”，记作Cost_predict。根据公式（8），令Cost_predict＝p_predict(h_k;k)即可，其中h_k是沿着机器人当前已规划路径上某个有待考察的栅格点，k是预测时间长度，在实施中一般取作5～10秒。该点上的Cost_predict值越大，表示k时刻后越有可能在此处发生人机相遇(co-occurrence)。图5是本发明实施实例中人的运动预测，其中k选取为5秒。

2)计算规则二“带裕量的机器人四周安全范围”的可达代价。

为了使得机器人在导航中，与障碍物之间留有一定裕度地进行避障，对环境中所有已知静态障碍物（即已有栅格地图中的被占有点）轮廓和已跟踪的行人位置点（用点h_k表示人的位置坐标）进行膨胀处理，即以高斯分布建模每个障碍物点外部的安全范围，以及这个范围内每个点的被占有概率。该步骤生成的安全栅格记为Cost_safe，其中地图中的任意点坐标(x,y)对应的安全概率Cost_safe(x,y)与常规表示空间被障碍物占用的概率类似，概率值越高表示该点处障碍对机器人通过的安全威胁（代价）越大，图6显示了障碍物与行人四周膨胀处理后的安全区域。

3)计算规则三“受建筑物遮挡或警戒区域绕行”的可达代价。

当机器人前进过程中，遇到建筑物背向遮挡区域，或者环境中存在设定的警戒区域，这两种环境区域约束的情况下，机器人应将此类区域对应的栅格加以处理以便控制绕行此类区域。第一种区域约束情况下，机器人绕行的原因是为了避免从建筑物遮挡区域背后突然出现而影响人的运动安全性。这种区域称为可达受遮挡区域(ReachableOccludedRegions，RORs)定义为未被障碍物占用但是易被柜子、墙体等高大物体所遮挡的区域，根据环境先验知识从栅格地图中一次性地提取环境中的ROR区域，相应栅格的ROR代价为Cost_RORs，其中地图中的任意点坐标(x,y)对应的概率Cost_RORs(x,y)为[0,1]间的某个常值。将Cost_RORs引入机器人运动规划器，可以驱使机器人离开受遮挡区域或者警戒区域，实现绕行。图7为实施实例的室内环境下可达受遮挡区域。而第二种区域约束需要由人工标记出地图中的警戒区域，机器人直接将地图中该区域对应的栅格标记为已占有即可。

4)计算规则四“遵守行驶靠向习惯”的可达代价。

以靠右习惯为例，将机器人左侧内墙壁的栅格代价值适当增大，称为“靠向代价栅格”，如图8所示。该概率分布计算如式（9）所示，某点(x,y)的概率值为Cost_side(x,y)，随着离开墙面内侧的距离增大而减小。当人机相遇过后，机器人应继续保持一段靠右行驶的路程而不是立即回到原有路径，保证了机器人在人身后运动的安全性。

${\mathrm{Cost}}_{\mathrm{side}}(x,y)=\min \{1-\frac{\mathrm{dist}((x,y),\mathrm{Wall})}{\mathrm{\η}{d}_{\mathrm{corr}}},0\}\·---\left(9\right)$

其中dist((x,y),Wall)是该点(x,y)与走廊一侧墙体Wall的距离函数，d_corr是走廊宽度，η是归一化因子。

5)可达代价融合，更新导航风险概率地图。将上述四种代价与障碍物占用的概率进行加权平均融合，其结果是考虑当前人机相对位置、人的运动预测及机器人运动目的地的可达代价栅格。其中权系数w_safe、w_RORs、w_side与w_predict可在实验中确定，或者在长期交互过程中赋予机器人在线参数学习的能力，以优化对人的个性化适应程度。与已有的障碍物栅格地图gridmap(x,y)的融合则采用取大值方法，生成最终导航风险概率地图Cost_final，如公式（10）（11）所示，并按比例归一化到0～1之间的相应值，如公式（12）所示。

Cost(x,y)＝w_safeCost_safe(x,y)+w_RORsCost_RORs(x,y)

+w_sideCost_side(x,y)+w_predictCost_predict(x,y)    （10）

Cost_final(x,y)＝max{gridmap(x,y),Cost(x,y)}    （11）

normalize(Cost_final(x,y))→[0,1]    （12）

在具体实施中，机器人导航运动控制器遵循全局规划与局部反应式避障结合的层次化结构，具体包括两个子模块：

1)全局规划器在创建的导航风险概率地图Cost_final上，采用波传播(wavefront全局路径规划法，在起点与终点栅格之间传播一条最短的、避免陷入受困障碍物内部、避免往复运动(cyclicalmotion)的最短全局路径进行全局路径规划，得到全局最优的路径点及每个点上的运动方向。

2)局部反应式避障控制器确保机器人在线运动时，以当前点的目标朝向为参考，根据较高分辨率的动态窗口中传感器实际获取的障碍物情况，采用平滑NearnessDiagram避障算法，计算避障方向并求出平滑变化的控制量，分别是平移速度υ和旋转速度ω。机器人车载计算机将平移速度和旋转速度作为运动控制量，输出到机器人低层运动控制单片机，从而驱动机器人完成导航和避障运动。在机器人导航同时，车载定位系统连续进行栅格地图中的自定位，最终确保机器人到达目的地。导航风险概率地图的更新与导航控制器结构如图9所示。

在图10所示的实施实例场景下，融合多约束条件的导航方式可避免机器人在多个候选路径之间来回切换徘徊，显著提高了机器人导航的效率与安全性。图10的(4)小图所示的左右房间有两处狭窄连接通道，机器人欲从图中的A处运动到B处，而与此同时，人将反方向穿越通道II。若采用普通的避障导航策略，由于人机相对运动的冲突情况不断变化，机器人规划的路径在通道I、II之间来回切换，如图10(a)的(2)～(4)小图所示，导致两次不必要的反转，出现了徘徊运动(共用时633个运行周期)。而本发明方法预测出人机阻塞情况，机器人在图10的(4)小图所示C处选择等待行为并在D处停止，等行人离开通道II后继续沿原有路径行驶，如图10(b)所示。最终运动轨迹较为光滑，实际上节省了到达目的地的耗时(仅用时342个运行周期)，提高了导航效率。

又如在图11所示情况下，机器人前进过程中贴近建筑物遮挡区域。此时本发明所采取的控制策略是：机器人既不采用path1从人的身后接近，也不采用path2紧贴墙角进入人的视野，而应沿着path3绕过高大障碍物的ROR区域，以一个较大偏角从人前侧方平缓地进入人的视野范围。这种导航策略的实施能够提高机器人导航行为对人的安全性。

Claims (8)

1.动态环境下服务动机器人导航方法，其特征在于，所述方法包括：

利用室内环境下多台全局摄像机与机器人车载激光传感器组成的分布式传感器网络，实现全局地图中动态人的定位与跟踪；

通过采集人的长期运动轨迹学习上述室内场所环境下人的运动模式；而机器人在导航过程中，通过对人运动的长期运动规律预测和短期运动速度、方向预测相结合，对人的时空运动趋势进行估计；

根据人的当前位置和上述估计得到的预测位置，与环境静态障碍栅格地图相融合，生成导航风险概率地图；

采用全局路径规划与局部避障控制层次化结构的机器人导航运动控制器，实现机器人导航行为控制。

2.根据权利要求1所述的动态环境下服务动机器人导航方法，其特征在于：所述的人的定位与跟踪，是在室内环境下采用多台全局摄像机与机器人车载的单台激光扫描器实现人的位置跟踪，通过标定全局摄像机与全局栅格地图的坐标转换关系，获取栅格地图中人的全局位姿及运动速度。

3.根据权利要求1所述的动态环境下服务动机器人导航方法，其特征在于：所述的对人的时空运动趋势进行估计，步骤如下：

1)首先在人的定位与跟踪基础上进行人的运动轨迹模式训练和预测：长期采集的行人位置数据，采用EM算法学习其运动模式类型，对每种类型的运动模式在x-y平面中用混合高斯分布加以建模，在实时对人运动位置进行跟踪的基础上，依据人的运动历史估计出当前运动遵循轨迹模式的概率；

2)其次进行人的短期运动预测：假设人的运动速度和方向角每过一定时刻速度和方向角变化一次，且在一定范围内变化，各个时刻上的速度和方向角分别为相应范围内的随机变量且相互独立，对人沿着该预测路径前进的短期运动速度和方向角进行预测。

4.根据权利要求1所述的动态环境下服务动机器人导航方法，其特征在于：生成所述导航风险概率地图还包括对运动人预测避让可达代价的导航约束条件，即当人机运动可能在狭窄通道内出现冲突时，通过预测人的运动位置来估计人的时空可达代价，并以降低人机运动冲突或阻塞风险为目标，控制机器人提前绕行或减速避让。

5.根据权利要求1所述的动态环境下服务动机器人导航方法，其特征在于：生成所述导航风险概率地图还包括带裕量的机器人四周安全范围可达代价的导航约束条件，即在机器人坐标位置的四周，用高斯分布的点云表示该点处障碍对机器人安全威胁概率。

6.根据权利要求1所述的动态环境下服务动机器人导航方法，其特征在于：生成所述导航风险概率地图还包括受建筑物遮挡或警戒区域绕行可达代价的导航约束条件，即控制机器人缓慢地绕行走出建筑物遮挡范围。

7.根据权利要求1所述的动态环境下服务动机器人导航方法，其特征在于：生成所述导航风险概率地图还包括遵守行驶靠向习惯可达代价的导航约束条件，即将靠向准则引入机器人运动规划，确保机器人在与人相遇时双方遵守相同的靠向规则，达到最优的导航效率。

8.根据权利要求1所述的动态环境下服务动机器人导航方法，其特征在于：所述的机器人导航运动控制器包括全局规划器和局部反应式壁障控制器两个子模块：

1)全局规划器在所述导航风险概率地图上，进行全局路径规划，得到全局最优的路径点及每个点上的运动方向；

2)局部反应式避障控制器确保机器人在线运动时，以当前点的目标朝向为参考，根据以0.1cm为分辨率的动态窗口中激光传感器实际获取的障碍物情况，计算避障方向并求出平滑变化的控制量，包括平移速度υ和旋转速度ω。

Images