一种混杂环境下差速移动机器人局部路径规划导引方法

1.本发明涉及一种混杂环境下差速移动机器人局部路径规划导引方法，属于机器人路径规划领域。


背景技术：

2.在工业、军事、民用领域，自主机器人导航是一个备受关注的复杂问题，高效且鲁棒性强的机器人自主导航系统亟待研究和面世。虽然导航技术在与时俱进的发展，并且取得了先进成果，但是在实际应用过程中还会源源不断产生新的需求，以适应更加针对性的工程应用。因此，需要进一步研究更加灵活可靠的导航策略，使其能够适应新的场景环境与任务需求。
3.近年来，应用于自主移动机器导航的反应式和认知式导航架构被广泛研究。快速搜索随机树(rrt)、局部窗口法(dwa)、人工势场法、teb等局部路径规划导引方法被广泛应用。但是，在复杂混乱的环境中，这些算法不能兼顾运算效率、安全避障和准确、稳定导引的需求。反应式导航借鉴了“刺激-响应”原理，可以对所处的环境信息进行实时响应，不需要完全已知整个环境地图信息进行其他复杂的任务规划，从而可以胜任在更加复杂的环境中安全导航。
4.在事先无法知道全局地图的混杂环境中，移动机器人需要结合自身传感器的感知输入，对当前局部视野内的环境建模分析，规划局部无碰撞路径，并应用跟踪控制算法进行准确导引。因此，研究一种可以实时地环境建模、高效稳定地规划、准确鲁棒地导引的局部路径规划导引方法架构十分必要。


技术实现要素：

5.本发明所要解决的技术问题在于，在全局环境未知情况下，基于移动机器人自身传感器的感知输入，对视野内局部环境障碍物进行基于椭圆几何的优化建模，通过平行椭圆极限环方法进行局部路径规划，应用预瞄距离自适应纯跟踪导引算法准确跟踪局部路径，从而能够在环境混杂、障碍物密集、机器人自身定位鲁棒性低和精度差的条件下，快速规划出一条安全的局部路径，并且快速高效的沿着局部路径跟踪导引。
6.为解决上述技术问题，本发明提供一种混杂环境下差速移动机器人局部路径规划导引方法，该方法包括如下步骤：
7.s1:基于机器人当前视角传感器(包括激光雷达、相机等)的感知输入，经过分割检测算法识别出障碍物，生成局部的障碍栅格占据图；
8.s2:对步骤s1输入的障碍栅格占据图，提取障碍物连通域，构成障碍连通图；
9.s3:对步骤s2生成的障碍连通图，计算每个障碍物区域的质心和方向；
10.s4:基于步骤s3计算的障碍物质心、方向，结合当前移动机器人的位置，对障碍物影响等级进行排序，确定最影响当前运动的障碍物；
11.s5:对步骤s4得到的最影响当前运动的障碍物，进一步确定质心(椭圆中心)，确定
方向，确定长轴，确定短轴，偏移量；
12.s6:基于步骤s5确定的相关参数，构建平行椭圆极限环微分方程，求解微分方程得到局部路径序列点信息；
13.s7:基于步骤s6生成的局部路径序列点信息，结合环境，确定阶段性局部目标点；
14.s8:对步骤s7确定的阶段性局部目标点，结合当前位置点、局部路径序列点，应用基于预瞄距离自适应的纯跟踪控制算法对局部路径准确跟踪导引。
15.优选的，所述步骤s1具体包括以下过程：
16.(1-1)利用硬件连接进行多传感器数据时间同步，也可以利用微处理的外部中断来实现多传感器的时间同步；
17.(1-2)对采集得到的融合信息进行二值化投影，得到局部障碍栅格占据图。
18.优选的，所述步骤s2具体包括以下过程：
19.对于步骤s1局部障碍栅格占据图，采用基于连接区域提取的两步快速扫描算法进行连通域提取。
20.(2-1)第一步遍历所有行列数据，分配临时标签和记录等效项；
21.(2-2)第二步遍历第一步产生的每个临时标签，将其替换成等效类的最小标签。
22.优选的，所述步骤s3具体包括以下过程：
23.(3-1)基于步骤s2产生的连通图，对每个连通域采用计算几何距算法确定质心，其计算公式如下：
[0024][0025]
其中，n为行数，m为列数，(xi,yj)(i＝0,1,...,n-1；j＝0,1,...,m-1)为像素坐标。
[0026]
(3-2)基于步骤s2产生的连通图，采用最小二阶矩轴(最小惯量轴在二维平面上的等效轴)作为联通域的方向。公式为：
[0027]
e＝∫∫r2f(x,y)dxdy
ꢀꢀꢀ
(2)
[0028]
式中，r是点(x,y)到直线的垂直距离。
[0029]
优选的，所述步骤s4具体包括以下过程：
[0030]
(4-1)依据局部环境占据连通图的每个障碍连通域的质心计算当前机器人位置(x
now
,y
now
)距离每个质心的距离：其中m＝1,2,...n，n为连通图上连通域的总数量；
[0031]
(4-2)依据计算得到的距离集合{dis1,dis2,dis3,...,disn}从小到大进行排序，确定最小距离min{dis1,dis2,dis3,...,dism}和障碍物质心将第k个障碍连通域作为运动视野内最影响机器人的障碍物。
[0032]
优选的，所述步骤s5具体包括以下过程：
[0033]
(5-1)基于步骤s4确定的最影响机器人移动的障碍物k，计算其质心到连通域边界的最大距离(其中j为连通域边界点序列),将其作为包络椭圆长半轴a＝lena，计算质心到连通域边界的最短距离
将其乘上大于1的系数增益k
cof
后作为包络椭圆短半轴b＝k
cof
*lenb；
[0034]
(5-2)结合机器人物理尺寸以及其对周围障碍最小安全碰撞距离safe
dis
的考虑，确定平行椭圆的偏移量k
p
＝safe
dis
。
[0035]
优选的，所述步骤s6具体包括以下过程：
[0036]
(6-1)基于步骤s5确定的长半轴、短半轴、偏移量，建立平行椭圆极限环方程。
[0037][0038]
其中，其中，z3＝(abk
p
)2；a和b分别表示包络椭圆的长半轴和短半轴；k
p
∈r
+
(k
p
≠0)表示的是椭圆平行曲线相对于包络椭圆的偏移量。
[0039]
(6-2)求解该微分方程，得到该椭圆的平行轨迹；
[0040]
(6-3)基于求得的轨迹，依据质心坐标进行平移，依据联通域的方向旋转得到最终路径轨迹。
[0041]
优选的，所述步骤s7具体包括以下过程：
[0042]
基于步骤s6求出的平行椭圆极限环轨迹，结合任务导向与避障的需求，确定极限环上的阶段目标点。
[0043]
优选的，所述步骤s8具体包括以下过程：
[0044]
(8-1)基于步骤s6求得的平行椭圆极限环轨迹和基于步骤s7确定的阶段目标点，从机器人当前位置应用基于预瞄距离自适应的纯跟踪控制算法对局部路径准确跟踪导引；
[0045]
(8-2)在每次控制周期中，计算路径当前点结合未来4个点构成曲线的曲率，依据曲率建立自适应预瞄距离函数，并进行求解确定当前时刻的预瞄距离和预瞄路径点；
[0046]
(8-3)将自适应的预瞄距离和预瞄路径点输入到纯跟踪算法中，计算、输出每个控制周期的速度和角速度信息，发送给机器人底盘电机，实现平滑、准确、稳定的跟踪导引。
[0047]
相对于现有技术，本发明具有如下优点：
[0048]
本发明提出的基于平行椭圆极限环的局部路径规划导引方法能够解决全局环境未知情况下，机器人在局部环境中安全规划、高效避障、准确导引的问题。首先，基于平行椭圆极限环的认知导航方式，借助于极限环周期性轨迹思想，凭借其固有的系统稳定性，在无需复杂计算的情况下，能够始终保持移动机器人相对于感知视野内包络障碍物椭圆的有效安全距离，从而保证机器人尽可能地躲避障碍。其次，在混杂环境中通过平行椭圆极限环微分方程建模可以灵活的调整机器人向自身轨道的收敛性和距离障碍物的安全偏移量，在不同的环境中都能获得平顺的导航路径。最后，基于预瞄距离自适应的纯跟踪导引算法，通过计算机器人未来一段待追踪路径的曲率，自适应计算预瞄距离和预瞄点，形成平滑的控制量，保证机器人稳定、准确的跟送路径，得到平顺灵活的导航结果。
附图说明
[0049]
图1本发明的方法流程示意图；
[0050]
图2平行椭圆极限环求解结果示意图；
[0051]
图3基于预瞄距离自适应的纯跟踪算法方案流程图。
具体实施方式
[0052]
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施仅用于说明本发明而不用限制本发明的范围。
[0053]
实施例1：如图1所示，一种混杂环境下差速移动机器人局部路径规划导引方法，该方法包括如下步骤：
[0054]
步骤s1:基于机器人当前视角传感器(包括激光雷达、相机等)的感知输入，经过分割检测算法识别出障碍物，生成局部的障碍栅格占据图；
[0055]
(1-1)利用硬件连接进行多传感器数据时间同步，也可以利用微处理的外部中断来实现多传感器的时间同步；
[0056]
(1-2)对采集得到的融合信息进行二值化投影，得到局部障碍栅格占据图。
[0057]
步骤s2:对步骤s1输入的障碍栅格占据图，提取障碍物连通域，构成障碍连通图。
[0058]
对于步骤s1局部障碍栅格占据图，采用基于连接区域提取的两步快速扫描算法进行连通域提取。
[0059]
(2-1)第一步遍历所有行列数据，分配临时标签和记录等效项；
[0060]
(2-2)第二步遍历第一步产生的每个临时标签，将其替换成等效类的最小标签。
[0061]
步骤s3:对步骤s2生成的障碍连通图，计算每个障碍物区域的质心和方向；
[0062]
(3-1)基于步骤s2产生的连通图，对每个连通域采用计算几何距算法确定质心，其计算公式如下：
[0063][0064]
其中，n为行数，m为列数，(xi,yj)(i＝0,1,...,n-1；j＝0,1,...,m-1)为像素坐标。
[0065]
(3-2)基于步骤s2产生的连通图，采用最小二阶矩轴(最小惯量轴在二维平面上的等效轴)作为联通域的方向。公式为：
[0066]
e＝∫∫r2f(x,y)dxdy
ꢀꢀꢀ
(2)
[0067]
式中，r是点(x,y)到直线的垂直距离。
[0068]
步骤s4:基于步骤s3计算的障碍物质心、方向，结合当前移动机器人的位置，对障碍物影响等级进行排序，确定最影响当前运动的障碍物；
[0069]
(4-1)依据局部环境占据连通图的每个障碍连通域的质心计算当前机器人位置(x
now
,y
now
)距离每个质心的距离：其中m＝1,2,...n，n为连通图上连通域的总数量；
[0070]
(4-2)依据计算得到的距离集合{dis1,dis2,dis3,...,disn}从小到大进行排序，确定最小距离min{dis1,dis2,dis3,...,dism}和障碍物质心将第k个障碍连通域作为运动视野内最影响机器人的障碍物。
[0071]
步骤s5:对步骤s4得到的最影响当前运动的障碍物，进一步确定椭圆中心(h,k)，确定椭圆方向ω，确定长半轴a，确定短半轴b，偏移量k
p
。
[0072]
(5-1)基于步骤s4确定的最影响机器人移动的障碍物k，计算其质心到连通域边界的最大距离(其中j为连通域边界点序列),将其作为包络椭圆长半轴a＝lena，计算质心到连通域边界的最短距离将其乘上大于1的系数增益k
cof
后作为包络椭圆短半轴b＝k
cof
*lenb；
[0073]
(5-2)结合机器人物理尺寸以及其对周围障碍最小安全碰撞距离safe
dis
的考虑，确定平行椭圆的偏移量k
p
＝safe
dis
。
[0074]
步骤s6:基于步骤s5确定的相关参数，构建平行椭圆极限环微分方程，求解微分方程得到局部路径序列点信息；
[0075]
(6-1)基于步骤s5确定的长半轴、短半轴、偏移量，建立平行椭圆极限环方程。
[0076][0077]
其中，其中，z3＝(abk
p
)2；a和b分别表示包络椭圆se的长半轴和短半轴；k
p
∈r
+
(k
p
≠0)表示的是椭圆平行曲线相对于包络椭圆的偏移量。μ∈r
+
是一个正的常量，它能用来调节平行椭圆极限环向其自身轨道的收敛性。r＝1表示顺时针轨迹，r＝-1表示逆时针轨迹。
[0078]
(6-2)求解该微分方程，得到该椭圆的平行轨迹；
[0079]
(6-3)基于求得的轨迹，依据质心坐标(h,k)进行平移，依据联通域的方向ω旋转得到最终路径轨迹path。
[0080]
在本实施例中，完成以上步骤生成的效果如图2所示的平行椭圆极限环求解结果示意图。
[0081]
步骤s7:基于步骤s6生成的平行椭圆极限环局部路径序列点信息，结合任务导向与避障的需求，确定阶段性局部目标点goal。
[0082]
步骤s8:对步骤s7确定的阶段性局部目标点goal，基于当前点p和局部路径path，应用预瞄距离自适应的纯跟踪控制算法对局部路径准确跟踪导引。
[0083]
(8-1)基于步骤s6求得的平行椭圆极限环轨迹和基于步骤s7确定的阶段目标点，从机器人当前位置应用基于预瞄距离自适应的纯跟踪控制算法对局部路径准确跟踪导引；
[0084]
(8-2)在每次控制周期中，首先判断当前点位置a
now
与目标点goal的阈值，如果阈值小于设定的导航成功的阈值，则认为车到达目标点，执行完毕。
[0085]
否则，根据以下公式计算路径当前点结合未来4个点构成曲线的曲率，
[0086][0087]
依据曲率建立以下自适应预瞄距离函数，并进行求解得到当前的预瞄距离。
[0088][0089]
其中，ld为预瞄距离，c∈r
+
(c≠0)为增益系数，e为正常数，low是预瞄距离下界，up是预瞄距离上界。
[0090]
然后基于预瞄距离ld确定预瞄点c。
[0091]
(8-3)如图3所示，将自适应的预瞄距离ld和预瞄点c输入到纯跟踪算法中，基于移动机器人当前点位置a
now
和车体航向角θ
pre
计算本次车体需要的航向角增量α：
[0092]
α＝atan2((c
y-a
now_y
),(c
x-a
now_x
))-θ
pre
ꢀꢀꢀꢀ
(6)
[0093]
依据航向角增量α和预瞄距离ld按照以下公式计算转弯半径r。
[0094][0095]
基于转弯半径r，依照既定速度v计算角速度ω或者依照既定角速度ω计算速度v，计算公式为：
[0096][0097]
将输出的每个控制周期的速度v和角速度ω信息，发送给机器人底盘电机，实现平滑、准确、稳定的跟踪导引。
[0098]
控制周期执行完毕后，更新机器人的当前位置a
now
，继续执行步骤(8-2)。
[0099]
需要说明的是上述实施例，并非用来限定本发明的保护范围，在上述技术方案的基础上所作出的等同变换或替代均落入本发明权利要求所保护的范围。