动态避障方法、装置及移动机器人与流程

1.本技术涉及移动机器人技术领域，特别是涉及动态避障方法、装置及移动机器人。


背景技术：

2.移动机器人路径规划是指为移动机器人生成一条从起点到终点的无碰撞路径。对于室内移动机器人而言，路径规划算法决定移动机器人运行的智能化程度。按对环境的感知程度，移动机器人路径规划算法分为已知环境信息的全局路径规划算法和未知环境或部分未知环境下的局部避障算法。
3.对于全局路径规划算法，由于已知环境信息，只需对环境地图进行搜索即可得到全局规划路径，但当环境信息发生变化时需要重新更新全局地图并执行规划过程。而局部路径规划算法只获取局部地图信息，当环境信息发生改变时能快速做出反应。对于局部路径规划算法只需计算局部环境地图，相较全局路径规划算法，计算成本和响应时间能有效降低。然而，目前的局部路径规划算法很少考虑到动态障碍物的移动特性，当室内物体发生快速移动时，可能会与移动目标发生碰撞。概言之，现有的移动机器人在避障和路径规划过程中，对动态障碍物的避障效果较差且路径规划的优化率较低。


技术实现要素：

4.基于此，有必要针对上述现有的移动机器人在避障和路径规划过程中对动态障碍物的避障效果较差且路径规划优化率较低的问题，提供一种动态避障方法、装置及移动机器人。
5.为了实现上述目的，本发明实施例提供一种动态避障方法，应用于移动机器人，包括如下步骤：
6.获取环境点云信息；
7.根据环境点云信息，对障碍物场景进行识别，得到障碍物场景信息；
8.对障碍物场景信息中动态障碍物场景信息进行预测处理，得到第一障碍物地图信息，并将第一障碍物地图信息更新至环境点云信息，得到更新后环境点云信息；
9.基于vfh避障算法，对更新后环境点云信息进行处理，得到当前目标方向信息；根据当前目标方向信息，对当前运行路径进行振荡区域标记，得到各振荡区域信息；根据各振荡区域信息，对当前运行路径进行振荡区域约束，得到优化后路径信息。
10.在其中一个实施例中，在对障碍物场景进行识别的步骤之后还包括：
11.在障碍物场景信息包括特殊障碍物场景信息时，生成对应特殊障碍物场景信息的第二障碍物地图信息，并将第二障碍物地图信息更新至更新后环境点云信息。
12.在其中一个实施例中，在对障碍物场景进行识别的步骤之后还包括：
13.在障碍物场景信息包括静态障碍物场景信息时，生成对应静态障碍物场景信息的第三障碍物地图信息；并将第三障碍物地图信息更新至更新后环境点云信息。
14.在其中一个实施例中，获取环境点云信息的步骤包括：
15.获取移动机器人前进方向的相机采集图像；
16.对相机采集图像进行摄像机标定处理，得到相机内参和畸变参数；
17.根据相机内参和畸变参数，对相机采集图像进行处理，得到环境点云信息。
18.在其中一个实施例中，对相机采集图像进行摄像机标定处理的步骤包括：
19.基于张氏标定算法，对相机采集图像进行内参标定，得到相机内参；
20.对相机采集图像进行畸变矫正，得到畸变参数。
21.在其中一个实施例中，对更新后环境点云信息进行处理的步骤包括：
22.基于vfh避障算法，对更新后环境点云信息进行处理，得到二值障碍物密度直方图；
23.对二值障碍物密度直方图进行处理，得到候选波谷信息；
24.对候选波谷信息进行加权处理，得到加权后候选波谷信息；
25.根据加权后候选波谷信息和预设的最终目标方向信息，得到当前目标方向信息。
26.在其中一个实施例中，对当前运行路径进行振荡区域标记的步骤包括：
27.获取预先存储的轨迹信息；
28.将当前目标方向信息与预先存储的轨迹信息进行比对处理，并根据比对处理结果，计算当前运行路径点与相邻下一运行路径点的加权欧式距离差分，得到第一数值；
29.在第一数值落入预设阈值范围时，对下一运行路径点进行振荡标记，得到振荡区域信息。
30.在其中一个实施例中，对障碍物场景信息中动态障碍物场景信息进行预测处理的步骤包括：
31.初始化环境点云信息中各动态障碍物点的kf参数；
32.对相邻两帧图像中的动态障碍物点进行平移配准，获取对应动态障碍物点的移动信息；
33.将匹配到的动态障碍物点接入kf先验估计和后验更新模型进行kf预测处理，对未匹配到的动态障碍物点视为停止移动，并初始化对应未匹配到的动态障碍物点的kf参数；
34.根据kf预测处理的结果，得到第一障碍物地图信息。
35.另一方面，本发明实施例还提供一种动态避障装置，应用于移动机器人，包括：
36.点云信息获取单元，用于获取环境点云信息；
37.障碍物识别单元，用于根据环境点云信息，对障碍物场景进行识别，得到障碍物场景信息；
38.点云信息更新单元，用于对障碍物场景信息中动态障碍物场景信息进行预测处理，得到第一障碍物地图信息，并将第一障碍物地图信息更新至环境点云信息，得到更新后环境点云信息；
39.避障优化单元，用于基于vfh避障算法，对更新后环境点云信息进行处理，得到当前目标方向信息；根据当前目标方向信息，对当前运行路径进行振荡区域标记，得到各振荡区域信息；根据各振荡区域信息，对当前运行路径进行振荡区域约束，得到优化后路径信息。
40.另一方面，本发明实施例还提供一种移动机器人，包括移动机器人主体及设置在移动机器人上的控制器；控制器用于执行上述任意一项的动态避障方法。
41.上述任一技术方案具有如下优点和有益效果：
42.上述动态避障方法的各实施例中，获取环境点云信息；根据环境点云信息，对障碍物场景进行识别，得到障碍物场景信息；对障碍物场景信息中动态障碍物场景信息进行预测处理，得到第一障碍物地图信息，并将第一障碍物地图信息更新至环境点云信息，得到更新后环境点云信息；基于向量场直方图避障算法，对更新后环境点云信息进行处理，得到当前目标方向信息；根据当前目标方向信息，对当前运行路径进行振荡区域标记，得到各振荡区域信息；根据各振荡区域信息，对当前运行路径进行振荡区域约束，得到优化后路径信息。本技术基于对障碍物的类型进行划分，通过对获取到的环境点云信息进行障碍物识别，在识别到动态障碍物时，对动态障碍物场景信息进行预测处理，并将生成的第一障碍物地图信息更新至环境点云信息，结合vfh避障算法对动态障碍物进行避障处理，解决了多个动态障碍物下的避障问题，另外，通过对当前运行路径进行振荡区域标记及约束处理，进而使得移动机器人远离振荡区域，能有效避免移动机器人陷入死区，从而优化了移动机器人的路径规划，提高了对动态障碍物的避障效果。
附图说明
43.图1为一个实施例中动态避障方法的应用环境示意图；
44.图2为一个实施例中动态避障方法的第一流程示意图；
45.图3为一个实施例中获取环境点云信息步骤的流程示意图；
46.图4为一个实施例中对更新后环境点云信息进行处理步骤的流程示意图；
47.图5为一个实施例中现有vfh(左图)和改进vfh(右图)避障效果示意图；
48.图6为一个实施例中对当前运行路径进行振荡区域标记步骤的流程示意图；
49.图7为一个实施例中振荡场景示意图；
50.图8为一个实施例中基于卡尔曼滤波算法对动态障碍物场景信息进行预测处理的流程示意图；
51.图9为一个实施例中优化后路径信息的效果示意图；
52.图10为一个实施例中动态避障装置的方框示意图。
具体实施方式
53.为使本技术领域人员更好地理解本技术方案，下面结合本技术实施例中附图，对本技术实施例中技术方案进行清楚、完整描述。显然，所描述实施例仅仅是本技术一部分实施例，而不是全部实施例。基于本技术中实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都应属于本技术保护的范围。
54.需要说明的是，本技术的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便此处描述的本技术的实施例。此外，术语“包括”、“具有”及其任何变形，意图在覆盖不排它的包含，例如，包含一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，也可包括没有清楚地列出或对于这些过程、方法、产品或设备固有的其它步骤或单元。
55.另外，术语“多个”的含义应为两个及两个以上。
56.现有技术中针对动态障碍物场景，目前主要有两种处理方法，一种是通过加入约束、改进算法结构、组合优化算法等方法提高算法效率，进而提高路径规划算法在动态场景下的重规划能力。然而，障碍物发生快速变化时，重新规划仍存在收敛时间过长、无法及时对移动的障碍物做出反应而导致碰撞等问题。第二种是对移动障碍物的轨迹进行预测，使用预测后的栅格地图进行避障和路径规划，但存在多个障碍物发生移动时无法对障碍物进行有效匹配的问题。概言之，现有的移动机器人在动态障碍物场景的避障和路径规划过程中，存在避障效果较差，路径规划优化率较低的问题。为了解决以上问题，本技术提供如下动态避障方法。
57.本技术提供的动态避障方法，可应用于如图1所示应用环境中。其中，移动机器人包括移动机器人主体104和与之连接的控制器102。移动机器人可以但不限于是扫地机器人、拖地机器人和服务机器人等具备自行走功能的机器人。移动机器人还包括摄像机，摄像机可用于采集对应环境点云信息的环境图像，进而控制器可根据摄像机采集到的环境图像，处理得到相应环境点云信息。
58.如图2所示，在一个实施例中，以应用于图1中的控制器为例进行说明，该方法包括：
59.步骤s210，获取环境点云信息。
60.其中，环境点云信息为对应移动机器人所处环境图像的点云信息。点云信息是指在一个三维坐标系统中的一组向量的集合。这些向量可通过三维坐标形式表示。
61.例如，移动机器人上设置有摄像机，可通过摄像机采集移动机器人前进方向的环境图像，并将采集到的环境图像传输给控制器，控制器可对接收到的环境图像进行处理，进而得到环境点云信息。
62.步骤s220，根据环境点云信息，对障碍物场景进行识别，得到障碍物场景信息。
63.控制器可根据预设的障碍物数据库，对环境点云信息中的障碍物场景进行识别处理，得到障碍物场景信息。其中，障碍物场景信息包括普通障碍物场景信息和特殊障碍物场景信息。普通障碍物场景信息根据物体是否发生移动，划分为静态障碍物场景信息和动态障碍物场景信息。静态障碍物场景信息中包含的静态障碍物为静止不动的障碍物，例如，在室内家庭场景中，静态障碍物可以是桌子和椅子等。动态障碍物场景信息中包含的动态障碍物为可移动的障碍物，如猫、狗等宠物。特殊障碍物场景信息包括但不限于斜坡、沟壑、鞋带等障碍物场景信息。
64.示例性地，可基于移动机器人的应用场景，对障碍物根据场景划分为正常雷达可检测成功的普通障碍物和根据雷达无法正常检测的特殊障碍物，进而对障碍物场景划分为普通障碍物场景和特殊障碍物场景。
65.步骤s230，对障碍物场景信息中动态障碍物场景信息进行预测处理，得到第一障碍物地图信息，并将第一障碍物地图信息更新至环境点云信息，得到更新后环境点云信息。
66.其中，第一障碍物地图信息可以是相应的栅格地图信息。
67.控制器对基于障碍物场景划分，对障碍物场景信息类型进行判断，在障碍物场景信息包括动态障碍物场景信息时，对动态障碍物场景信息进行预测处理，进而得到第一障碍物地图信息。控制器可将处理得到的第一障碍物地图信息更新至环境点云信息，得到更新后环境点云信息。
68.示例性地，控制器获取至少两帧的障碍物场景信息，判断障碍物场景信息中障碍物坐标是否发生变化，并将坐标发生变化的障碍物判定为动态障碍物。
69.需要说明的是，本实施例可采用的卡尔曼滤波(kalman filter，kf)算法对动态障碍物进行预测处理，在其它实施例中还可采用extended kalman filter算法、unscented kalman filter算法或particle filter算法对动态障碍物进行预测处理。
70.步骤s240，基于向量场直方图(vector field histogram，vfh)避障算法，对更新后环境点云信息进行处理，得到当前目标方向信息；根据当前目标方向信息，对当前运行路径进行振荡区域标记，得到各振荡区域信息；根据各振荡区域信息，对当前运行路径进行振荡区域约束，得到优化后的路径信息。
71.控制器采用向量场直方图避障算法，对更新后的环境点云信息进行处理，计算相应目标方向，进而得到当前目标方向信息。控制器根据当前目标方向信息检测当前运行路径是否发生振荡，并根据检测结果，对当前运行路径进行振荡区域标记，进而得到相应的各个振荡区域信息。控制器对相应的振荡区域添加约束，避开或远离振荡区域，进而得到优化后的路径信息，由此有效避免移动机器人陷入死区，实现对移动机器人的动态避障。
72.上述实施例中，基于对障碍物的类型进行划分，通过对获取到的环境点云信息进行障碍物识别，在识别到动态障碍物时，对动态障碍物场景信息进行预测处理，并将生成的第一障碍物地图信息更新至环境点云信息，结合向量场直方图避障算法对动态障碍物进行避障处理，解决了多个动态障碍物下的避障问题。此外，通过对当前运行路径进行振荡区域标记和约束处理，进而使得移动机器人远离振荡区域，能有效避免移动机器人陷入死区，从而优化移动机器人的路径规划，提高了对动态障碍物的避障效果。
73.在一个实施例中，在对障碍物场景进行识别的步骤之后还包括：
74.在障碍物场景信息包括特殊障碍物场景信息时，生成对应特殊障碍物场景信息的第二障碍物地图信息，并将第二障碍物地图信息更新至更新后环境点云信息。
75.其中，第二障碍物地图信息可以是相应的栅格地图信息。
76.控制器对基于障碍物场景划分，对障碍物场景信息类型进行判断，在障碍物场景信息包括特殊障碍物场景信息时，生成对应特殊障碍物场景信息的第二障碍物地图信息。控制器可将处理得到的第三障碍物地图信息更新至环境点云信息，得到更新后环境点云信息。
77.需要说明的是，在一些应用场景中特殊障碍物不仅仅包括沟壑、斜坡、鞋带，在其它应用场景中对移动机器人形成障碍的激光雷达无法检测的物体都可作为特殊障碍物。
78.在一个实施例中，在对障碍物场景进行识别的步骤之后还包括：
79.在障碍物场景信息包括静态障碍物场景信息时，生成对应静态障碍物场景信息的第三障碍物地图信息，并将第三障碍物地图信息更新至更新后环境点云信息。
80.其中，第三障碍物地图信息可以是相应的栅格地图信息。
81.控制器对基于障碍物场景划分，对障碍物场景信息类型进行判断，在障碍物场景信息包括静态障碍物场景信息时，直接生成对应静态障碍物场景信息的第三障碍物地图信息，并将生成的第三障碍物地图信息更新至环境点云信息，得到更新后环境点云信息。
82.示例性地，控制器可获取至少两帧障碍物场景信息，判断障碍物场景信息中障碍物坐标是否发生变化，并将坐标未变化的障碍物判定为静态障碍物。
83.在一个实施例中，如图3所示，获取环境点云信息的步骤包括：
84.步骤s310，获取移动机器人前进方向的相机采集图像。
85.其中，相机采集图像指的是摄像机采集移动机器人前进方向的环境图像。摄像机设置在移动机器人上，移动机器人工作过程中，摄像机可对当前环境进行扫描，进而得到相机采集图像。
86.需要说明的是，摄像机可以是3d tof相机，3d tof相机采集的图像对应为tof图像。在一些实施例中，摄像机也可用其它深度相机替代，如双目相机、结构光相机等。
87.步骤s320，对相机采集图像进行摄像机标定处理，得到相机内参和畸变参数。
88.其中，相机内参是指从相机坐标系转为图像坐标系，即三维点到二维点转换得到的参数。畸变参数包括径向畸变参数和切向畸变参数。需要说明的是，摄像机标定处理过程中求解的摄像机参数为确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系而必须建立的摄像机成像的几何模型参数。
89.示例性地，对相机采集图像进行摄像机标定处理的步骤包括：
90.基于张氏(张正友)标定算法，对相机采集图像进行内参标定，得到相机内参；对相机采集图像进行畸变矫正，得到畸变参数。
91.其中，张氏标定算法是一种基于单平面棋盘格的摄像机标定算法。
92.步骤s330，根据相机内参和畸变参数，对相机采集图像进行处理，得到环境点云信息。
93.控制器根据处理得到的相机内参和畸变参数，对相机采集图像进行标定和畸变矫正处理，得到处理后的相机采集图像，并将处理后的相机采集图像进行转化得到环境点云信息，进而通过使用摄像机获取普通雷达无法感知的区域，弥补雷达的视野盲区。在一个实施例中，如图4所示，对更新后环境点云信息进行处理的步骤包括：
94.步骤s410，基于向量场直方图避障算法，对更新后环境点云信息进行处理，得到二值障碍物密度直方图。
95.控制器基于向量场直方图避障算法，对更新后环境点云信息进行处理，计算相应的障碍物密度，进而得到二值障碍物密度直方图。
96.步骤s420，对二值障碍物密度直方图进行处理，得到候选波谷信息。
97.控制器根据处理得到的二值障碍物密度直方图，计算候选波谷方向，进而得到候选波谷信息。
98.需要说明的是，候选波谷信息可用来表示障碍物信息。
99.步骤s430，对候选波谷信息进行加权处理，得到加权后候选波谷信息。
100.控制器可根据预设的权重，对候选波谷信息进行加权处理，进而得到加权后候选波谷信息。
101.步骤s440，根据加权后候选波谷信息和预设的最终目标方向信息，得到当前目标方向信息。
102.控制器根据预设的最终目标方向信息和处理得到的加权后候选波谷信息，计算当前目标方向，进而得到当前目标方向信息。
103.示例性地，现有vfh(或称原始vfh)和改进vfh算法静态障碍物避障效果如图5所示，其中圆圈表示移动机器人，方框为避障算法计算时的活动窗口，细点为障碍物，星点为
目标点。保存移动机器人每一步的位置形成图中所示运动轨迹。当目标点处于障碍物后方时，如图5的左图所示，现有vfh算法会使移动机器人来回移动陷入振荡。采用上述实施例的步骤s240的振荡检测和振荡抑制方法步骤后，图5的右图中移动机器人经一次来回移动，系统检测到移动机器人陷入振荡，添加振荡抑制条件后远离振荡区域。
104.在一个实施例中，如图6和7所示，对当前运行路径进行振荡区域标记的步骤包括：
105.步骤s610，获取预先存储的轨迹信息。
106.步骤s620，将当前目标方向信息与预先存储的轨迹信息进行比对处理，并根据比对处理结果，计算当前运行路径点与相邻下一运行路径点的加权欧式距离差分，得到第一数值。
107.步骤s630，在第一数值落入预设阈值范围时，对下一运行路径点进行振荡标记，得到振荡区域信息。
108.具体地，通过将当前目标方向信息与预先存储的轨迹信息进行对比并计算相邻点的加权欧式距离差分，进而根据计算得到第一数值判断当前运行路径是否发生振荡及振荡类型；进而根据不同振荡类型对可移动机器人通行方向进行约束限制，远离振荡区域，实现有效避免移动机器人陷入死区。
109.上述实施例中，通过对vfh算法的改进，解决vfh类算法在靠近障碍物的目标点附近和对称障碍物区域易发生振荡的问题，提高了vfh算法的路径稳定性，并结合卡尔曼滤波算法避开复杂环境下多个动态障碍物，从而优化移动机器人的路径规划，提高了对动态障碍物的避障效果。
110.在一个实施例中，如图8所示，对障碍物场景信息中动态障碍物场景信息进行预测处理的步骤包括：
111.步骤s810，初始化环境点云信息中各动态障碍物点的kf(kalman filter，卡尔曼滤波)参数。
112.步骤s820，对相邻两帧图像中的动态障碍物点进行平移配准，获取对应动态障碍物点的移动信息。
113.步骤s830，将匹配到的动态障碍物点接入kf算法先验估计和后验更新模型进行kf预测处理，对未匹配到的动态障碍物点视为停止移动，并初始化对应未匹配到的动态障碍物点的kf参数。
114.步骤s840，根据kf算法预测处理结果，得到第一障碍物地图信息。
115.具体地，控制器通过初始化环境地图中各动态障碍物点的kf参数，对相邻两帧图像中动态障碍物点进行平移配准，获取对应动态障碍物点的移动信息；将匹配动态障碍物点带入kf算法先验估计和后验更新模型进行kf预测处理，对未匹配到的动态障碍物点视为停止移动，并初始化对应的kf参数，进而输出kf算法预测后的下一步动态障碍物点地图信息，由此得到第一障碍物地图信息。
116.示例性地，基于kf算法对障碍物进行多点预测的vfh算法动态障碍物预测效果如图9所示，两个长方框表示相邻时刻的两个活动窗口，密集点为当前时刻检测到的移动障碍点，三角点为kf算法预测的障碍点，障碍物上方的虚线箭头代表当前动态障碍物的移动方向，米字点为移动机器人中心点。在经过图示区域时，由于障碍物处于移动状态，vfh规划方向可能在下一时刻与障碍物发生碰撞。使用kf算法对障碍物运动轨迹进行多点预测，将预
测后的轨迹添加到局部地图中，可进行下一步vfh规划以避开动态障碍物。将不同障碍物场景生成的障碍物地图信息更新到环境点云信息，使用相邻两帧地图进行平移配准，并使用kf算法对多个动态障碍物进行轨迹预测，使用vfh算法进行避障，解决多个动态障碍物下的避障问题。通过检测vfh规划过程中是否发生振荡，对于标记的振荡区域添加约束条件，远离振荡区域，能有效避免机器人陷入死区，从而优化了移动机器人的路径规划，提高了对动态障碍物的避障效果。
117.在一个具体示例中，基于3d tof相机，以该方法应用于图1中的控制器为例进行说明，具体步骤如下：
118.步骤1，对tof相机进行标定和矫正，采集tof图像生成点云信息。
119.其中，步骤1的具体实现过程划分为步骤1.1、步骤1.2和步骤1.3。
120.步骤1.1，使用tof相机从不同角度分别拍摄10次打印的棋盘格图片，并使用matlab的camera calibrator工具箱进行张正友标定，获得tof相机的内参矩阵如下：
[0121][0122]
其中f为相机焦距，d
x
,dy分别为像素坐标系中x和y方向的像素刻度。c
x
和cy分别为像素图像坐标系原点在像素坐标系中的坐标。
[0123]
实际标定获取的内参(单位为mm)为：
[0124][0125]
标定获取的畸变因子为：k1＝-0.05526，k2＝0.2604，k3＝-0.44089，
[0126]
步骤1.2，使用tof相机采集图像，对图像畸变进行矫正，矫正方法为：
[0127]
x
distorted
＝x [2p1xy p2(r2 2x2)]
[0128]ydistorted
＝y [2p2xy p1(r2 2y2)]
[0129][0130]
步骤1.3，将矫正后的图像利用内参转换为点云，其转换关系为：
[0131][0132]
其中(u,v)是像素坐标系，(xc,yc,zc)为世界坐标系。
[0133]
步骤2，根据环境点云信息将障碍物场景划分为普通障碍物场景和特殊场景(如斜坡、沟壑、鞋带等)，根据不同场景采用对应方式更新障碍物地图。
[0134]
其中，步骤2的具体实现过程划分为步骤2.1、步骤2.2、步骤2.3和步骤2.4。
[0135]
步骤2.1，输入步骤1获取的点云信息，对点云进行遍历，滤除20cm以上点云信息，判断是否有斜坡、鞋带、沟壑等特殊障碍物。对于沟壑，只需滤除地面以上部分，如存在地面以下部分的沟壑点，对沟壑点取均值得到其近似中心坐标，通过计算近似中心坐标与每个沟壑点的最小距离r，当2r＞d(d为车轮直径)时判断车轮无法通过；对于斜坡，首先滤除地面以下部分，检测障碍物高度发生突变的位置，若突变位置坡度小于30
°
，则将对应区域标记为可通行区域，否则标记为不可通行区域。
[0136]
步骤2.2，滤除步骤2.1中检测的特殊障碍物，对相邻时刻栅格地图求交集，并对相邻时刻栅格地图中的交集在各自区域部分做差分处理，若存在不为0的区域，判断为动态障碍物，反之则为静态障碍物。
[0137]
步骤2.3，对于步骤2.2中的静态障碍物，输出栅格地图，对于步骤2.2中的动态障碍物，进行如下kf预测流程：
[0138]
获取环境栅格地图并滤除栅格地图中静态障碍物区域，初始化动态障碍点的kf参数，初始化参数如下：
[0139][0140]
其中为初始时刻位置矢量的先验计算值，x0为初始时刻位置矢量的后验更新值，为初始时刻预估误差协方差矩阵，p0为初始时刻后验误差协方差矩阵。
[0141]
对相邻两帧地图中的运动点进行平移配准。首先计算相邻两帧地图中的交集，对两帧地图中的交集部分做差分处理，大于0的位置标记为 ，小于0的位置标记为-，计算每一标记为-的点最近的 点作为移动前后的匹配点，获取对应运动点的移动情况。
[0142]
将动态障碍点移动前后的匹配点代入kf算法进行更新预测，公式如下：
[0143]
先验估计为：
[0144]
先验预估误差协方差为：
[0145]
卡尔曼增益为：
[0146]
后验更新为：
[0147]
后验误差协方差为：
[0148]
其中x为位置矢量，x＝[x,y,v
x
,vy]，k为时刻，uk为过程噪声，a、b为先验更新参数矩阵，q为过程噪声协方差，r为测量噪声协方差，c为测量值y与位置x的变换矩阵。
[0149]
使用先验估计预测移动障碍点的下一步轨迹并输出预测轨迹栅格地图。
[0150]
步骤2.4，将特殊障碍物的栅格地图、静态障碍物栅格地图、动态障碍物栅格地图、预测轨迹栅格地图进行叠加输出。
[0151]
步骤3，使用vfh算法计算目标方向，并根据运行路径检测是否发生振荡，通过添加约束避开振荡区域。
[0152]
其中，步骤3的具体实现过程划分为步骤3.1、步骤3.2、步骤3.3和步骤3.4。
[0153]
步骤3.1，首先对步骤2输出的栅格地图进行分区，分区大小为10
°
，分别计算每个分区n内的障碍物密度，计算步骤如下：
[0154]
对于每个障碍点(xi,yi)，机器人中心点(x0,y0)，其对应的角度为：
[0155][0156]
对应的障碍物密度为：
[0157][0158]
其中c
i,j
为障碍点(xi,yi)出现障碍物的概率值，a,b为常数，d
i,j
为障碍点(xi,yi)与(x0,y0)之间的距离。
[0159]
分区n的障碍物密度为：
[0160][0161]
计算各个分区的障碍物密度得到障碍物密度直方图，然后设置阈值τh和τ
l
，将直角密度直方图转为二值密度直方图，转换公式为：
[0162][0163]
其中k为时刻。
[0164]
步骤3.2，通过二值障碍物密度直方图将障碍物密度为0的分区作为候选区域，大于0的区域作为不可通行区域，连续为0的区域作为候选波谷。
[0165]
步骤3.3，将候选波谷进行加权，并根据最终目标方向，计算当前目标方向。
[0166]
步骤3.4，将目标方向与存储的轨迹进行对比，计算相邻点加权欧式距离差分，计算公式为：
[0167]
点(xi,yi)距离目标点(x
goal
,y
goal
)和初始出发点(x
ini
,y
ini
)的欧式距离之和为：
[0168][0169]
其中α、β为对应加权因子。
[0170]
加权欧式距离差分为：
[0171][0172]
振荡类型及其差分曲线如图7所示，根据对应差分曲线可判断是否发生振荡及对应振荡类型。具体流程如下：
[0173]
记录每一次vfh算法输出的规划目标点；每一次到达规划目标点时计算与上一步的加权欧式距离并记录；根据记录的加权欧式距离记录绘制横坐标轴为时间k，纵坐标轴为k时刻获得的加权欧式距离，判断5个相邻时刻过零点的次数n；n≥4时为直线振荡，2≤n《4时为三角振荡，n《2时不发生振荡。
[0174]
步骤3.5，根据不同振荡类型对可通行方向进行限制，远离振荡区域。
[0175]
其中，对于图7中第一种振荡类型，可通过限制机器人转向角度限制每一次规划目标点的方向与当前运行方向角度之差小于3个分区；对于图7中第二种振荡类型甚至更复杂的振荡类型需通过距离差分曲线获取陷入振荡的区域，增加对应区域的障碍物密度，远离振荡区域。
[0176]
传统vfh和改进vfh算法静态障碍物避障效果如图5所示，其中圆圈表示移动机器人，方框为避障算法计算时的活动窗口，细点为障碍物，星点为目标点。保存移动机器人每一步的位置形成图中所示运动轨迹。当目标点处于障碍物后方时，如图5左图所示，现有vfh算法会使移动机器人来回移动陷入振荡。采用步骤3.4的振荡检测方法和步骤3.5的振荡抑制方法后，图5右图中移动机器人经一次来回移动，系统检测到移动机器人陷入振荡，添加振荡抑制条件后远离振荡区域。
[0177]
基于kf算法对障碍物进行多点预测的vfh算法动态障碍物预测效果如图9所示，两个长方框表示相邻时刻的两个活动窗口，密集点为当前时刻检测到的移动障碍点，三角点为kf预测的障碍点，障碍物上方的虚线箭头代表当前动态障碍物的移动方向，米字点为移动机器人中心点。在经过图示区域时，由于障碍物处于移动状态，vfh规划方向可能在下一时刻与障碍物发生碰撞。使用kf算法对障碍物运动轨迹进行多点预测，将预测后的轨迹添加到局部地图中，可进行下一步vfh规划以避开动态障碍物。
[0178]
应当理解的是，虽然图2～4、6、8的流程图中各个步骤按照箭头指示依次显示，但是这些步骤并非必然按照箭头指示的顺序依次执行。除非本文有明确说明，这些步骤执行并没有严格的顺序限制，可以以其它顺序执行。而且，图2～4、6、8中至少一部分步骤可包括多个子步骤或多个阶段，这些子步骤或阶段并非必然在同一时刻执行完成，而是可在不同时刻执行，这些子步骤或阶段的执行顺序也非必然是依次进行，而是可与其它步骤或其它步骤的子步骤或阶段的至少一部分轮流或交替执行。
[0179]
在一个实施例中，如图10所示，还提供了一种应用于移动机器人的动态避障装置，包括：
[0180]
点云信息获取单元110，用于获取环境点云信息。
[0181]
障碍物识别单元120，用于根据环境点云信息，对障碍物场景进行识别，得到障碍物场景信息。
[0182]
点云信息更新单元130，用于对障碍物场景信息中动态障碍物场景信息进行预测处理，得到第一障碍物地图信息，并将第一障碍物地图信息更新至环境点云信息，得到更新后环境点云信息。
[0183]
避障优化单元140，用于基于向量场直方图避障算法，对更新后环境点云信息进行处理，得到当前目标方向信息；根据当前目标方向信息，对当前运行路径进行振荡区域标记，得到各振荡区域信息；根据各振荡区域信息，对当前运行路径进行振荡区域约束，得到优化后路径信息。
[0184]
关于动态避障装置的具体限定可参见上文中对移动机器人动态避障方法的限定，在此不再赘述。上述动态避障装置中各个模块可全部或部分通过软件、硬件及其组合实现。上述各模块可以硬件形式内嵌或独立于移动机器人中的控制器中，也可以软件形式存储于移动机器人中的存储器中，以便控制器调用执行上述各个模块对应的操作。
[0185]
在一个实施例中，还提供一种移动机器人，包括移动机器人主体及设置在移动机
器人上的控制器；控制器用于执行上述任意一项的动态避障方法。
[0186]
其中，移动机器人可以是具有扫地、拖地等功能的清洁机器人。移动机器人主体设置有摄像机，摄像机用于采集移动机器人前进方向的图像，进而得到相机采集图像。
[0187]
控制器用于执行以下动态避障方法的步骤：
[0188]
获取环境点云信息；根据环境点云信息，对障碍物场景进行识别，得到障碍物场景信息；对障碍物场景信息中动态障碍物场景信息进行预测处理，得到第一障碍物地图信息，并将第一障碍物地图信息更新至环境点云信息，得到更新后环境点云信息；基于向量场直方图避障算法，对更新后环境点云信息进行处理，得到当前目标方向信息；根据当前目标方向信息，对当前运行路径进行振荡区域标记，得到各振荡区域信息；根据各振荡区域信息，对当前运行路径进行振荡区域约束，得到优化后路径信息。
[0189]
上述实施例中，基于对障碍物类型进行划分，通过对获取到的环境点云信息进行障碍物识别，在识别到动态障碍物时，对动态障碍物场景信息进行预测处理，并将生成的第一障碍物地图信息更新至环境点云信息，结合向量场直方图避障算法对动态障碍物进行避障处理，解决了多个动态障碍物下的避障问题。此外，通过对当前运行路径进行振荡区域标记和约束处理，进而使得移动机器人远离振荡区域，能有效避免移动机器人陷入死区，从而优化移动机器人的路径规划，提高了对动态障碍物的避障效果。
[0190]
在一个实施例中，还提供一种计算机可读存储介质，其上存储计算机程序，计算机程序被处理器执行时实现上述任一项动态避障方法的步骤。
[0191]
在一个示例中，计算机程序被处理器执行时实现以下步骤：
[0192]
获取环境点云信息；根据环境点云信息，对障碍物场景进行识别，得到障碍物场景信息；在障碍物场景信息包括动态障碍物场景信息时，对动态障碍物场景信息进行预测处理，得到第一障碍物地图信息，并将第一障碍物地图信息更新至环境点云信息，得到更新后环境点云信息；基于向量场直方图避障算法，对更新后环境点云信息进行处理，得到当前目标方向信息；根据当前目标方向信息，对当前运行路径进行振荡区域标记，得到各振荡区域信息；根据各振荡区域信息，对当前运行路径进行振荡区域约束，得到优化后路径信息。
[0193]
本领域普通技术人员可以理解为实现上述实施例方法中的全部或部分流程，可以通过计算机程序指令相关硬件完成，所述计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本技术所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(rom)、可编程rom(prom)、电可编程rom(eprom)、电可擦除可编程rom(eeprom)或闪存。易失性存储器可包括随机存取存储器(ram)或外部高速缓冲存储器。作为说明而非局限，ram以多种形式可得，诸如静态ram(sram)、动态ram(dram)、同步dram(sdram)、双数据率sdram(ddrsdram)、增强型sdram(esdram)、同步链路(synchlink)dram(sldram)、存储器总线(rambus)直接ram(rdram)、直接存储器总线动态ram(drdram)及存储器总线动态ram(rdram)等。
[0194]
以上所述实施例的各技术特征可以进行任意组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0195]
以上所述实施例仅表达了本技术的几种实施方式，其描述较为具体和详细，但并
不能因此而理解为对发明专利范围的限制。应当指出，对于本领域的普通技术人员而言，在不脱离本技术构思的前提下，还可做出若干变形和改进，这些都属于本技术的保护范围。因此，本技术专利的保护范围应以所附权利要求为准。