多传感器数据融合处理方法及机器人避障方法与流程

1.本发明涉及机器人避障，特别是多传感器数据融合处理方法及机器人避障方法。


背景技术：

2.随着工业4.0时代来临，将由大量的机器人取代人工做重复性、单一性的工作。市面上大部分机器人配备了不少高精度传感器，除开本身传感器价格高昂以外，需要匹配相应的算法以及高算力的主控芯片cpu、gpu。当前芯片产量紧缩、价格飞涨数倍的情况下，在简单、重复性、单一性工作环境中，高昂的cpu处理器配置是没有太大的必要的，且在大量布置的情况下，性价比很低。
3.现行的多种避障方法依赖于大量的数据运算，cpu性能要求高。
4.基于以上条件，在简单、重复性、单一性工作情况下，低硬件成本是送货机器人的关键问题，而要降低硬件成本，则需要算法上的大量精简，目前流行算法并不能大量优化，因此需要一款新的算法来解决运算量的问题。
5.由于任务需求、对于环境数据的探测，目前越来越多的机器人涉及到使用多个传感器，目前主流的融合算法可以概括为随机、人工智能两大类。随机类融合算法有加权平均法、卡尔曼滤波、多贝叶斯估计法等，人工智能类融合算法有模糊逻辑、神经网络等，但是由于这些算法使用了很多复杂的运算，导致系统开销高、实时性低、无法下放到低成本cpu中运行，导致在此基础上要求cpu的性能高、价格昂贵。因此在满足避障要求的前提下，传感器数据高效率、低开销的融合、处理，是目前比较难以实现的问题。
6.目前主流的机器人避障方法有势场法(pfm)、向量场直方图(vfh)、可视图法、rrt算法、a*路径搜索等算法等，这些算法本身需要大量的数据(需要整个空间的数据)，所带来的问题是大量的计算，并且还有一些算法无法跨越大的u形障碍物，因此在满足避障条件下，减少避障时数据需求、减少计算复杂度、能跨越大u形障碍物是所需要解决的问题


技术实现要素：

7.本发明的目的在于克服现有技术的缺点，提供多传感器数据融合处理方法及机器人避障方法。
8.本发明的目的通过以下技术方案来实现：多传感器数据融合处理方法，建立机体坐标系；机器人中心为坐标原点，正右方为x轴正方向，函正上方为y轴正方向，传感器机体坐标系中信息可以表示为：其中x、y、z为机体坐标系中的安装位置，θ为该传感器在机体坐标系中围绕z轴旋转安装的角度。对于同一个传感器，绕z轴角度θ，在x，y参数一定
的情况下，无论其z方向有多少个数据点，都将其数据叠加压缩为一点，则可以忽略z方向坐标，上式变化为：因此激光雷达在机体坐标系中的表示为超声波表示为红外测距：tof表示
9.测算障碍物分别在传感器坐标系的坐标；传感器探测到某个方向距离为d，此为障碍物距离，在传感器坐标系上有对应的角度可以算出该点障碍物在传感器坐标系中的表示：
[0010][0011]
根据不同传感器的探测的特性，得到障碍物坐标合集：
[0012][0013]
其中：为某一个有效距离d在传感器坐标系中的角度，范围为传感器探测范围(min～max)；代表传感器坐标系下角度下距离原点为d的点的坐标；
[0014]
将障碍物在传感器坐标系的坐标转换到机体坐标系中，转换公式为：
[0015][0016]
其中：x,y为传感器在机体坐标系中的坐标，θ为机体坐标系中传感器围绕z轴的安装角度；
[0017]
在传感器坐标系中任意一点距离数据转换到机体坐标系中表示为：
[0018][0019]
从而得到基于机器人机体坐标系的原始坐标点合集表示为：
[0020][0021]
数据融合；扩充传感器测量的距离数据直到机器人测量最大边界(x
max
，y
max
)，传感器探测到某个方向距离为d，对应的角度则该点坐标为在机体坐标系中不变，d以e
step
步进增加，某一角度探测到障碍物的点向该角度远处延伸后坐标点的扩展坐标合集则为：
[0022][0023]
其中：
[0024]
1、max为该传感器到机器人测量边界的最大距离
[0025]
2、d为初始距离d0～max，步进为e
step
[0026]
则该传感器拓展后所有的数据坐标合集修改为：
[0027][0028]
为某一个有效距离d在传感器坐标系中的角度，范围为传感器探测范围(min～max)。
[0029]
即得到优化后传感器融合数据：
[0030][0031]
传感器包括激光雷达、超声波、红外测距、tof传感器的一种或多种。
[0032]
机器人避障方法，机体中心为坐标原点，机器人正右方为x正方向，正上方为y正方向，通过权利要求1～2任意一项的多传感器数据融合处理方法获取距离机器人中心距离为d的全向360度障碍物概率统计数组，对于某一个方向α，机器人机身宽度w，距离中心的距离为d，圆弧对应的角度扫过的范围β：
[0033][0034]
则机体坐标系下求取采集点合集的弧对应的角度范围为
[0035]
在经过该弧所有经过点的所有坐标依次求解，求解所有数据点的坐标[x,y]，求解公式如下：
[0036][0037]
其中：
[0038]
因为数据离散，取离散化数据，以步进e
step
取值，其中心点[x，y]
center
对应机体坐标系中心，width为二维数组宽，height为其高，如下：
[0039][0040]
存储的数据值1/0分别代表该点有/无障碍物；
[0041]
定义实际坐标位置对应二维数组坐标点的比例为:s
scale
，则角度a，距离d的障碍物的坐标转换到二维数组中坐标计算为：
[0042][0043]
其中：[x，y]
center
为二维数组中心点的坐标
[0044]
该弧获取的障碍物数据求和表示为：
[0045][0046]
其中：p([x，y]a)为该点在二维数组中的值(0或1)；[x，y]a为距离d，α角度的障碍物在二维数组中的坐标，且不超过二维数组边界[width，height]，单位为1；以步进e
step
取值；通过弧度扫过的角度β得到数据点个数：
[0047]
然后根据公式：获取到某一个α角度的通行概率，将该数据填充到全向360度障碍物概率统计数组中，因此，机器人在每个角度方向对应一个通行概率，使用该通行概率数据与机器人的期望目标对机器人的行进方向进行影响，即可以完成避障。
[0048]
将360度分成若干个方向，每个方向采集一段圆弧障碍物通行概率数据。
[0049]
将360度均匀分成20个方向。
[0050]
本发明具有以下优点：本发明多传感器数据融合处理方法，将单线激光雷达、超声波测距、红外测距、tof等测距模块的测量得到的数据以障碍物的叠加状态存储于二维数组中，并且以某一距离数据的测量角度向外延伸直到二维数组边界，在此二维数组中可以快速获取任意距离的障碍物情况，同时包含了该距离之内的障碍物信息；本发明中的机器人避障方法，结合多传感器数据融合处理方法可以轻松越过普通障碍物以及u/v等特殊形状的障碍物，该多传感器融合避障方法使用的传感器数据量小，算法复杂程度低，运算速度快，可以很快的假设到更低性能的处理器，相比于目前市面上普遍使用的方案需要配备高性能的处理器，直接降低了电路硬件的成本。
附图说明
[0051]
图1为机器人传感器安装示意图；
[0052]
图2为激光雷达原始数据显示示意图；
[0053]
图3为各种传感器融合到机体坐标系上的图形化显示示意图；
[0054]
图4为优化后的各种传感器融合到机体坐标系上的图形化显示示意图；
[0055]
图5为本发明计算障碍物概率时使用到的数据点标记示意图；
[0056]
图6为通行概率统计为某一方向的数据示意图；
[0057]
图7为机器人遇到障碍物的结构示意图；
[0058]
图8为障碍场景1的示意图；
[0059]
图9为障碍场景2的示意图；
[0060]
图10为障碍场景3的示意图；
[0061]
图11为障碍场景4的示意图；
[0062]
图12为障碍场景5的示意图；
[0063]
图13为障碍场为u形的示意图；
[0064]
图14为机器人转完u形障碍场的示意图；
[0065]
图15为机器人遇到u形障碍场边缘的示意图；
[0066]
图16为机器人转向完成后遇到u形障碍场边缘的示意图；
[0067]
图17为机器人位于u形障碍外侧的示意图。
具体实施方式
[0068]
为使本发明实施方式的目的、技术方案和优点更加清楚，下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式是本发明一部分实施方式，而不是全部的实施方式。通常在此处附图中描述和示出的本发明实施方式的组件可以以各种不同的配置来布置和设计。
[0069]
因此，以下对在附图中提供的本发明的实施方式的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施方式。基于本发明中的实施方式，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。
[0070]
需要说明的是，在不冲突的情况下，本发明中的实施方式及实施方式中的特征可以相互组合。
[0071]
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
[0072]
在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该发明产品使用时惯常摆放的方位或位置关系，或者是本领域技术人员惯常理解的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
[0073]
在本发明的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
[0074]
对于机器人实体虽然是三维空间的运动，但是最终通过分解都可以分解为上下左右前后六个方向三个维度的运动，而对于地面移动的机器人，其高度方向的运动可以忽略不计(最多是随着地面起伏)，在单一、结构化地形中移动机器人使用的是比较简易的传感器，并且在机器人周围局部的避障运动，可以化简为前后左右的二维方向的运动，因此，基于此简化，将所有的传感器数据压缩到一个二维空间中，并且对数据进行处理，最终机器人可以调用该二维空间任一段数据的合集对机器人某个通行方向的障碍物概率进行判断，此融合处理保证了机器人在此数据中采集任意距离的障碍物数据都能包含近距离障碍物的信息，即可通行方向的必然能让机器人通过。
[0075]
多传感器数据融合处理方法，传感器安装在机器上的位置如图1所示，依据图1中传感器的安装位置，建立机体坐标系，机器人中心为坐标原点，正右方为x轴正方向，函正上方为y轴正方向，传感器机体坐标系中信息可以表示为：其中x、y、z为机体坐标系中的安装位置，θ为该传感器在机体坐标系中围绕z轴旋转安装的角度，其他角度在本例中不会应用到计算中，可以忽略。在本方法中，对于同一个传感器，绕z轴角度θ，在x，y参数一定的情况下，无论其z方向有多少个数据点，都将其数据叠加压缩为一点，则可以忽略z方向坐标，上式变化为：因此激光雷达在机体坐标系中的表示为(lr＝lidar),超声波表示为(u＝ultrasonic),红外测距：tof表示
[0076]
传感器探测到某个方向距离为d，此为障碍物距离，在传感器坐标系上有对应的角度可以算出该点障碍物在传感器坐标系中的表示：
[0077][0078]
根据不同传感器的探测的特性，得到障碍物坐标合集：
[0079][0080]
其中：为某一个有效距离d在传感器坐标系中的角度，范围为传感器探测范围(min～max)；代表传感器坐标系下角度下距离原点为d的点的坐标；
[0081]
对于机械式单线激光雷达360度，按以上公式计算合集。对于多线雷达，在固定的某一角度方向上，z轴的所有点数据都累加到z＝0从原点向外辐射的射线上相同x/y坐标的点上，再使用以上方法计算合集。
[0082]
对于选用的超声波探测范围为25度，但是其只返回一个数值，在本方法中将超声波的数据等效为一个圆锥，又忽略了z轴方向数据，因此简化为一面扇形区域，即当超声波探测距离为d时，其经过计算得到的实际是一段弧坐标点的集合。红外测距、tof等探测范围是一个面的传感器和超声波传感器处理方法类似，不再赘述。
[0083]
在本实施例中，传感器包括激光雷达、超声波、红外测距、tof传感器的一种或多种，依据以上规则，得到激光雷达/超声波/红外测距/tof传感器的数据合集分别为：种，依据以上规则，得到激光雷达/超声波/红外测距/tof传感器的数据合集分别为：
[0084]
绕z轴旋转，传感器坐标系转换到机体坐标系的转换矩阵为：
[0085][0086]
其中：x,y为传感器在机体坐标系中的坐标
[0087]
θ为机体坐标系中传感器围绕z轴的安装角度
[0088]
则在传感器坐标系中任意一点距离数据(s＝sensor，代表基于传感器坐标系)转换到机体坐标系中表示为：
[0089][0090]
(b＝base,基于机体坐标系)
[0091]
基于以上公式，得到基于机器人机体坐标系的原始坐标点合集表示为：
[0092][0093]
以激光雷达为例，激光雷达原始数据如图2所示，灰色点是激光雷达传感器采集的数据点，多传感器数据融合到机体坐标系上的图形化显示如图3所示，大面积的黑色代表无障碍物，中间灰色方块为机器人示意，周边灰色方块为传感器测量到的障碍物的坐标合集cb，其中一个方块代表一个数据点：
[0094]
但是仅仅基于上述数据点，作为机器人可控距离的避障需要是不够的，如果要达到理想的近距离避障效果，需要增加大量算法对障碍物数据进行判断，因此为了减少运算资源的消耗，本方法基于此数据进行优化，扩充传感器测量的距离数据直到机器人测量最大边界(x
max
，y
max
)，具体实施如下：
[0095]
根据以上描述：传感器探测到某个方向距离为d，对应的角度则该点坐标为在机体坐标系中不变，d以e
step
步进增加，求取坐标合集，e
step
值取决于要求精度(精度要求高取值适当缩小)以及坐标的重复率(算出坐标重复率高可以适当取较大值)。求出某一角度探测到障碍物的点向该角度远处延伸后坐标点的扩展坐标合集为：
[0096][0097]
其中：max为该传感器到机器人测量边界的最大距离；d为初始距离d0～max，步进为e
step
[0098]
则该传感器拓展后所有的数据坐标合集修改为：则该传感器拓展后所有的数据坐标合集修改为：为某一个有效距离d在传感器坐标系中的角度，范围为传感器探测范围(min～max)。
[0099]
即得到优化后传感器融合数据：
[0100][0101]
优化后的多传感器融合到机体坐标系上的图形化显示如图4所示；
[0102]
通过该优化后的融合数据，在任意采样距离(距离机体中心)条件下，能采集到该距离障碍物的数据再叠加该距离内到机体中心的所有障碍物信息，而不用再次对近处数据进行判断，数据融合完成后，就可以以角度方向对障碍物附近求可通行的方向。此方法减少了对近处数据的判断过程，并且运算量小，速度运算快，可靠性高。
[0103]
本实施计算障碍物概率时使用到的数据点标记如图5所示，，对距离机体中心30、40、50、60、70、80、90(cm)几个范围的数据进行采样，如图5所示，九个白色的圆环是为使用到的数据点，使用时，取其中任意半径为r的一段弧的数据集合求其障碍物概率即可判断该方向是否可以越过障碍。
[0104]
一种机器人避障方法，本方法，相较于市面上通用的避障方法，系统开销很小，很容易的布置到低速嵌入式cpu中，直接降低了产品的硬件成品。能适应各种复杂的障碍环境，对于外轮廓边缘杂乱不规则的障碍物本方法也可以进行避障操作，并且对于u、v等机器人中常见的容易被陷入障碍物中震荡的障碍也能良好的处理，且结合对传感器数据的处理，保证避障的同时得到的可通行方向必然能让机器人通过的效果，经实测能达到良好避障效果。
[0105]
在机器人机身建立坐标系，机体中心为坐标原点，机器人正右方为x正方向，正上方为y正方向，获取距离机器人中心距离为d的全向360度障碍物概率统计数组passpercent[]，该数组的每个数据已经融合了距离圆心为d到距离圆心为0之间的障碍物信息。如图6所示，
[0106]
将360度分为0、18、36、54
…
342度共20个方向，每个方向采集一段圆弧障碍物数据，具体算法如下：
[0107]
对于某一个方向α，机器人机身宽度w，距离中心的距离为d，圆弧对应的角度扫过的范围β：(其中e为余量，确保机器人能完全通过、甚至能掉头且留有一定的活动空间)
[0108][0109]
则机体坐标系下求取采集点合集的弧对应的角度范围为
[0110]
如图7所示，要收集该弧所经过的所有的数据，需要对经过点的所有坐标依次求解，本实施取e
step
＝2度为步进，求解所有数据点的坐标[x，y]，求解公式如下：
[0111][0112]
其中：
[0113]
因为数据离散，取离散化数据，以步进e
step
取值
[0114]
同时有存储结构例如二维数组，其中心点[x，y]
center
对应机体坐标系中心，width为二维数组宽，height为其高，如下：
[0115][0116]
存储的数据值1/0分别代表该点有/无障碍物；
[0117]
定义实际坐标位置对应二维数组坐标点的比例为:s
scale
(cm/point，厘米/点)
[0118]
则角度a，距离d的障碍物的坐标转换到二维数组中坐标计算为：
[0119][0120]
其中：[x，y]
center
为二维数组中心点的坐标
[0121]
因此一段弧获取的障碍物数据求和表示为：
[0122][0123]
其中：
[0124]
1、p([x，y]a)为该点在二维数组中的值(0或1)
[0125]
2、[x，y]a为距离d，a角度的障碍物在二维数组中的坐标，且不超过二维数组边界[width，height]，单位为1
[0126]
3、以步进e
step
取值
[0127]
通过弧度扫过的角度β得到数据点个数：
[0128]
因此根据以上信息，获取到某一个α角度的通行概率：使用该数据填充passpercent[]数组，获得一个通行概率数组，机器人每个角度方向对应一个通行概率，使用该数据与机器人的期望目标对机器人的行进方向进行影响，即可以完成避障。
[0129]
以下对为该方法具体实施：
[0130]
机器人全向360度分为20份，此时20个方向点的通行概率分布数组passpercent[20](该数组为传感器融合最终得到的概率数组)，机器人正右方为0度，对应passpercent[0]数据，正前方为90度，对应passpercent[5]数据，正左方为180度，对应passpercent[10]数据，正后方为270度，对应passpercent[15]数据。(解释：在本实施中，从passpercent[0]～passpercent[20]对应机体坐标系角度依次为{0,18,36,54,72,90,108,126,144,162,180,198,216,234,252,270,288,306,324,342}，在后面的计算直接带出，不另计算，单位为度)。
[0131]
排序规则：以期望方向最前，与期望方向相差180度的角度最远进行排序。
[0132]
①
对于普通的障碍物避障如下(基于机体坐标系)：
[0133]
障碍场景1，如图8所示，passpercent＝{100,100,100,100,91,91,91,93,100,100,100,100,100,100,100,100,100,100,100,100}(单位％)，取通行概率100％为可通行
点，假设当前机器人目标点期望角度(机体坐标系)，按排序规则排序后得到排序后通行角度方向集合：sort＝{54,144,36,162,18,180,0,198,342
……
}(单位度)，因此此时机器人会向右旋转到当前机体的54度方向后再向前行，另在排序算法中角度以右边优先。
[0134]
障碍场景2，如图9所示：passpercent＝{100,100,100,100,81,71,71,71,79,100,100,100,100,100,100,100,100,100,100,100}(单位％)，期望角度按排序规则排序后得到排序后通行角度方向集合：sort＝{54,36,18,162,0
……
}(单位度)，因此此时机器人会向右旋转到当前机体的54度方向后再向前行。
[0135]
障碍场景3，如图10所示：passpercent＝{88,66,47,44,44,66,88,98,100,100,100,100,100,100,100,100,100,100,100,100}(单位％)，期望角度按排序规则排序后得到排序后通行角度方向集合：sort＝{144,162,180,342,198,324
……
}(单位度)，因此此时机器人会向左旋转到144度方向后再向前行。
[0136]
障碍场景4，如图11所示，：passpercent＝{100,100,88,66,44,37,37,59,81,100,100,100,100,100,100,100,100,100,100,100}(单位％)，期望角度按排序规则排序后得到排序后通行角度方向集合：sort＝{18,162,0,180,342,198,324
……
}(单位度)，因此此时机器人会向右旋转到18度方向后再向前行。实际上该状态下会进入u形障碍物处理方法中，该方法将在下面的章节详细描述。
[0137]
障碍场景5，如图12所示：passpercent＝{100,100,91,88,79,79,79,88,98,100,100,100,100,100,100,100,100,100,100,100}(单位％)，期望角度按排序规则排序后得到排序后通行角度方向集合：sort＝{18,162,0,180,342,198,324
……
}(单位度)，因此此时机器人会向右旋转到18度方向后再向前行。实际上该状态下也会进入u形障碍物处理方法中，该方法将在下面的章节详细描述。
[0138]
②
对于u、v型障碍物避障：
[0139]
初始状态：世界坐标系和机体坐标系重合，模型简化为二维运动，机器人正前方为y轴正方形，正右方为x轴正方形，记机器人在世界坐标系下为p
robot
＝[x，y]
robot
，在世界坐标系下绕z轴旋转的角度为θ。机器人全向360度分为20份，期望方向p
des
＝[x，y]
des
。备注:涉及到角度的单位都是度(减少浮点运算)。
[0140]
机器人进入u形障碍物避障的逻辑为：行走时，检测到期望方向已经被完全遮挡，即遮挡是一个平面时，因此不仅是u、v型障碍物，只要是其遮挡平面垂直于机器人与期望目的地连线时，且机器人已经走到该位置，触发u形障碍物避障。
[0141]
当前机器人遇到了需要进行u形避障的障碍物如下图13所示：
[0142]
此时passpercent[20]＝{100,100,88,66,54,54,54,76,98,100,100,100,100,100,100,100,100,100,100,100,}(％)
[0143]
算出此时世界坐标系下机器人去往期望目标点的角度：(算法中处理了tan函数无解问题，在此不赘述)
[0144][0145]
将该角度转换到机器人机体坐标系下，转换公式：
[0146][0147]
该实施中，算出来的基于机体坐标系中角度为与其临近的角度为54/72/90/108/126(度)，对应在passpercent中数据{88,66,54,54,54,76}，均不为100％，因此判断进入u形障碍物处理模式，初始进入时，会强制要求机器人向右旋转90度(实际是根据机器人左右两边空旷程度判断左右转)，转完之后如下图14所示：
[0148]
旋转完成后，机器人不再以期望目的地为期望方向，而是以正左方为期望方向，即在整个u形障碍物处理流程中，此时的通行概率分布：passpercent[20]＝{100，100，100，100，100，100，81,79,57,47,47,37,59,81,81,86,91,91,100,100}，基于则将passpercent按照规则排序后得到90/72/54/36/18
……
(度)，因此此时机器人将以90度角度行驶，即直行，运行到u障碍物边缘，如下图15所示：
[0149]
此时：passpercent[20]＝{100，91，88，66，44，22，0,0,0,20,42,64，80,100,100,100,100,100,100,100}，基于经过规则排序后得到0/342/324/306/288
……
，则机器人向机体坐标系0度方向旋转，则向右转，转向完成的数据省略，如图16所示为走到u形边缘示意图：
[0150]
此时：passpercent[20]＝{100,100,100,100,100,100,100,88,66,44,44,66,88,100,100,100,100,100,100,100}，基于经过规则排序后得到234/108/252/90/270
……
，此时机器人本来应该旋转到234度，但是这样会导致震荡无法出u形障碍，因此这里为了不陷入局部无解(即沿着墙壁远离期望目标时，机器人后方对着期望方向，会判断为退出u形避障模式并掉头行走)，最终机器人转向的角度不会超过机体坐标系的0～180范围，因此抛弃234度，取108度，机器人将向左转，之后直行、再左转、再直行(省略)，行进到下图17所示情况时：
[0151]
此时，根据公式
[0152]
得到世界坐标系下期望方向角度此时机体在世界坐标系下的角度θ＝160，经过转换，得到机体坐标系上期望角度此时通行概率passpercent[20]＝{100,100,100,100,100,100,100,83,83,83,91,91,100,100,100,100,100,100,100,100}，发现基于的临近角度108度方向为可行方向，且处于机器人正前方，则判定退出u形避障处理(这里判断依据为：在世界坐标系下，相对于机体位置的期望目标方向处在机器人正前方一定范围内(45～135度)，且此时没有被遮挡)，最后进入普通避障模式到达目标点。
[0153]
尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换,凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。