发明内容
本申请的目的在于一种割草机器人的避障巡回方法及系统,实现割草机器人在任意位置均能以最短平滑曲线路径返回指定点。
为实现上述目的,本申请所采取的技术方案为:
一种割草机器人的避障巡回方法,用于割草机器人在指定的割草区域中完成割草工作,所述割草机器人的避障巡回方法,包括:
步骤1、取割草区域基于GPS坐标的拓扑地图,该拓扑地图中包括割草机器人起始位置,充电站位置以及各障碍物边界;
步骤2、将拓扑地图中的障碍物边界按照割草机器人的转弯半径进行膨胀,并在膨胀后采用近似单元分解法将膨胀后的障碍物边界近似为多边形区域,将多边形区域作为对应障碍物的障碍物区域;
步骤3、以割草机器人当前位置为起点,以充电站位置为终点,以起点、终点以及各障碍物区域的顶点作为位置点,采用线段连接相邻位置点得到拓扑地图对应的可视图;
步骤4、基于可视图中的各线段采用Dijkstra算法得到割草机器人由起点至终点的最短折线路径;
步骤5、以机器人转弯半径为垂直距离在最短折线路径的每一折线的两边生成平行线,连接最短折线路径同侧的平行线,得到相对于最短折线路径的两条外围路径;
步骤6、取最短折线路径中属于障碍物区域的顶点的折点,确定两条外围路径中与该折点对应的两个点,选取两个点中不在障碍物区域内的点作为参考点;
步骤7、以起点、终点、步骤6中所选取的参考点作为控制点,使用De Casteljau算法绘制一条贝塞尔曲线,以所绘制的贝塞尔曲线作为割草机器人由当前位置返回充电站位置的最短平滑曲线路径。
以下还提供了若干可选方式,但并不作为对上述总体方案的额外限定,仅仅是进一步的增补或优选,在没有技术或逻辑矛盾的前提下,各可选方式可单独针对上述总体方案进行组合,还可以是多个可选方式之间进行组合。
作为优选,所述基于可视图中的各线段采用Dijkstra算法得到割草机器人由起点至终点的最短折线路径,包括:
步骤41、取可视图中一个标记为未访问的节点v;
步骤42、获取可视图中节点v所有的相邻点,并将节点v标记为已访问;
步骤43、取所有相邻点中的一个相邻点u,计算节点v到相邻点u的成本cost=dist(v)+length(u,v),其中,dist(v)为节点v到起点的最短距离,length(u,v)为相邻点u到节点v的距离;
步骤44、若成本cost<dist(u),则dist(u)=cost,其中dist(u)为相邻点u到起点的最短距离,并记录该相邻点u为路径节点,执行步骤45;否则返回步骤43取所有相邻点中的下一个相邻点作为相邻点u重新进行计算;
步骤45、判断节点v所有的相邻点是否已全部遍历,若是则执行步骤46;否则返回步骤43取所有相邻点中的下一个相邻点作为相邻点u;
步骤46、判断可视图中是否还存在未访问的节点,若是则返回步骤41;否则以记录的所有路径节点作为折点生成最短折线路径。
作为优选,所述参考点的坐标求解过程为:
令待求解的两个相邻参考点为参考点C
1和参考点C
2,且设参考点C
1和参考点C
2的坐标为
和
已知与参考点C
1对应的顶点A
1的坐标为
与参考点C
2对应的顶点A
2的坐标为
割草机器人的转弯半径为R;
则计算对应顶点与参考点之间的距离为:
根据二维平面直角坐标系两点直线方程y=kx+b,以及平行线与两顶点连接的直线的平行关系推导出经过参考点C1的平行线1与x轴的夹角α关系为:
联立表达式|A
1C
1|、tanα、sinα、
求解得到参考点C
1坐标
的值,同理联立表达式|A
2C
2|、tanα、sinα、
求解得参考点C
2坐标
的值。
作为优选,所述以起点、终点、步骤6中所选取的参考点作为控制点,使用DeCasteljau算法绘制一条贝塞尔曲线,以所绘制的贝塞尔曲线作为割草机器人由当前位置返回充电站位置的最短平滑曲线路径,包括:
设从割草机器人当前位置到充电站位置的路径上经过的控制点坐标为P0到Pn,n为控制点的总个数,则该贝塞尔曲线函数B(t)为:
取贝塞尔曲线为拥有两个控制点的三阶贝塞尔曲线,即取n=2,则根据贝塞尔曲线函数B(t)得到三阶贝塞尔曲线的展开二项式为:
C(t)=(1-t)3P0+3(1-t)2tP1+3(1-t)t2P2+t3P3,t∈[0,1]
式中,C(t)为三阶贝塞尔曲线的展开二项式,且C(t)即为割草机器人由当前位置返回充电站位置的最短平滑曲线路径。
本申请还提供一种割草机器人的避障巡回系统,包括处理器和存储器,所述存储器存储有计算机程序,所述处理器读取所述存储器中的计算机程序并运行以实现所述的割草机器人的避障巡回方法的步骤。
本申请提供的割草机器人的避障巡回方法及系统,结合使用Dijkstra算法与贝塞尔曲线控制割草机器人运动轨迹,通过以割草机器人底盘形状以及转弯半径为依据拓宽最短路径的方法选择出适用于绘制贝塞尔曲线的控制点来达到避障的目的,其中的关键点为在空旷草坪环境下如何选取机器人运动轨迹中的贝塞尔曲线控制点,对应的曲率值决定曲线的弯曲程度,进而影响机器人运动过程的平滑度和平稳度,本发明采用的控制点选择策略,实现局部递归路径下机器人的平滑稳定运行。割草机器人在实现最优路径运动的同时,能够有效绕过障碍物。
具体实施方式
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
除非另有定义,本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的,不是在于限制本申请。
其中一个实施例中,提供一种割草机器人的避障巡回方法,用于割草机器人在指定的割草区域中完成割草工作,旨在提出适用于基于GPS与距离传感器结合定位避障的割草机器人的平滑曲线避障及在任意位置都能适用的返回指定点的最优路径规划方法,可用于使割草机器人完成工作后准确返回充电站(或称充电桩)位置,也可用于割草机器人快速运行到指定位置。根据规划的最优路径实现自适应调节割草机器人的运行速度,进而合理规划割草工作时间与提高效率。
需要说明的是,本实施例提出的避障巡回方法以割草机器人为例进行说明,但并不限制该避障巡回方法仅适用于割草机器人,本实施例的避障巡回方法可应用于需要进行路径规划的其他类型机器人,例如扫地机器人、巡检机器人等。
如图1所示,本实施例的割草机器人的避障巡回方法,包括以下步骤:
步骤1、取割草区域基于GPS坐标的拓扑地图,该拓扑地图中包括割草机器人起始位置,充电站位置以及各障碍物边界。
步骤2、将拓扑地图中的障碍物边界按照割草机器人的转弯半径进行膨胀,并在膨胀后采用近似单元分解法将膨胀后的障碍物边界近似为多边形区域,将多边形区域作为对应障碍物的障碍物区域。
本实施例的割草机器人使用GPS定位与距离传感器,在初始化时绘制基于GPS坐标的拓扑地图。由于草坪中的大型障碍物,如池塘、花坛一般不具有规则边沿,故在拓扑地图中使用近似单元分解法(Approximate Cell Decomposition)把障碍物近似为地图上由适当大小的正方形组成的多边形区域(如图2所示,图2中的黑色实线为障碍物边界,灰色范围为使用近似单元分解法后得到的多边形区域),正方形的大小由草坪面积、机器人宽度与障碍物大小决定,网格顶点以位置坐标形式储存在系统中。
需要说明的是,为了简化视图,图2中的黑色实线为实际的障碍物边界向外扩大机器人转弯半径(或以上)距离后得到的膨胀缓冲区(即膨胀后的障碍物边界),以此保证机器人不会在运动过程中碰撞到障碍物。
步骤3、以割草机器人当前位置为起点,以充电站位置为终点,以起点、终点以及各障碍物区域的顶点作为位置点,采用线段连接相邻位置点得到拓扑地图对应的可视图。
可视图(Visibility Graph):在欧几里得平面中将路径起始点、各个障碍物顶点用直线连接,直线必不经过障碍,即连线的两点间在光学意义上互为可视的。如图3所示,图3为基于图2得到的可视图,图中的虚线为可视图中对应的各线段。
步骤4、基于可视图中的各线段采用Dijkstra算法得到割草机器人由起点至终点的最短折线路径。
本实施例通过时间复杂度为O(n2)的朴素Dijkstra算法(戴克斯特拉演算法(Dijkstra Algorithm)是一种在图形平面上,有多个节点的路径,求出最低通过成本的演算法)计算可视图中各线段的长度后,遍历路径网络,从所有路径中搜索出割草机器人从当前位置返回充电站位置的最短折线路径(基于图3中的可视图搜索到的最短折线路径如图4中的加粗虚线所示)。
如图5所示,本实施例采用的的Dijkstra算法具体步骤如下:
首先设定以下变量:创建dist、visit和route三个数组,其中dist:数组存储源点(起点)s到其它所有节点的最短距离,即各个顶点的权重,初始化源点的距离为0,其余顶点的距离为无穷大;visit:储存所有顶点访问状态,第一个数据为源点,值为0或1;route:储存访问路径;v,u代表顶点和相邻节点;length(u,v)则表示u到v的权重(距离)。
则本实施例基于Dijkstra算法搜索最短折线路径的过程如下:
步骤41、取可视图中一个标记为未访问的节点v。可视图中的节点按照顺序进行编排,在取节点v时按照编排的顺序从数组visit中进行取用。
步骤42、获取可视图中节点v所有的相邻点(复数个),并将节点v标记为已访问。
步骤43、取所有相邻点中的一个相邻点u,计算节点v到相邻点u的成本cost=dist(v)+length(u,v),其中,dist(v)为节点v到起点的最短距离,length(u,v)为相邻点u到节点v的距离。
步骤44、若成本cost<dist(u),则dist(u)=cost,其中dist(u)为相邻点u到起点的最短距离,并记录该相邻点u为路径节点,在数组route进行记录,执行步骤45;否则返回步骤43取所有相邻点中的下一个相邻点作为相邻点u重新进行计算。
步骤45、判断节点v所有的相邻点是否已全部遍历,若是则执行步骤46;否则返回步骤43取所有相邻点中的下一个相邻点作为相邻点u。
步骤46、判断可视图中是否还存在未访问的节点,若是则返回步骤41;否则以记录的所有路径节点作为折点生成最短折线路径。
步骤5、以机器人转弯半径为垂直距离在最短折线路径的每一折线的两边生成平行线,连接最短折线路径同侧的平行线,得到相对于最短折线路径的两条外围路径。
基于图4中的最短折线路径得到的两条外围路径如图6中两条加粗实线所示(即折线1和折线2),其中特例是机器人底盘转弯半径等于机器人半径,此时两平行线距离等于两倍机器人半径。
步骤6、取最短折线路径中属于障碍物区域的顶点的折点,确定两条外围路径中与该折点对应的两个点,选取两个点中不在障碍物区域内的点作为参考点。
步骤7、以起点、终点、步骤6中所选取的参考点作为控制点,使用De Casteljau算法绘制一条贝塞尔曲线(贝塞尔曲线(Bezier curve):由多个控制点坐标作为方程参数所绘制的平滑曲线),以所绘制的贝塞尔曲线作为割草机器人由当前位置返回充电站位置的最短平滑曲线路径。
在拓扑地图中,由计算可得的折线坐标可知,在每个经过的障碍物顶点处,该位置所对应的外围路径总会有一条在障碍物区域中,则在该顶点对应位置,选择不与障碍物区域覆盖面重合的点作为贝塞尔曲线的控制点(如图7中的控制点C1、控制点C2,控制点C1、控制点C2也就是步骤6选取的参考点,即参考点C1和参考点C2)。
根据对称平滑原则,曲率确定情况下,参考点C1和参考点C2与对应的顶点的连线和两平行折线垂直,唯一确定参考点C1和参考点C2的位置坐标。根据本实施例中起点至终点的路径规划,确定本实施例的贝塞尔曲线为三阶贝塞尔曲线,在此基础下参考点的坐标求解过程为:
如图8所示,为割草机器人三阶贝塞尔曲线的一段轨迹运动示意图,根据GPS差分定位原理建立二维平面直角坐标系,令待求解的两个相邻参考点为参考点C
1和参考点C
2,且设参考点C
1和参考点C
2的坐标为
和
根据二维栅格地图单元格已知与参考点C
1对应的顶点A
1的坐标为
与参考点C
2对应的顶点A
2的坐标为
割草机器人的转弯半径为R。
贝塞尔曲线中与控制点C1对应的曲点B1、与控制点C2对应的B2处的曲率一定的情况下,则计算对应顶点与参考点之间的距离为:
根据二维平面直角坐标系两点直线方程y=kx+b,以及平行线与两顶点连接的直线的平行关系推导出经过参考点C1的平行线1(即折线1)与x轴的夹角α关系为:
联立表达式|A
1C
1|、tanα、sinα、
(即公式(1)、(3)、(4)、(5))求解得到参考点C
1坐标
的值,方程联立求解为数学计算常规过程,本实施例中不展开详细描述。同理可得参考点C
2坐标
的值,由于折线1与折线2平行,因此折线2与x轴的夹角α关系、折线1与x轴的夹角α关系相同,因此联立表达式|A
2C
2|、tanα、sinα、
(即公式(2)、(3)、(4)、(6))即可求解得参考点C
2坐标
的值,并且以此类推可求解的得到所有参考点的坐标。
在得到所有参考点的坐标后,基于起点、终点和参考点可绘制得到一条贝塞尔曲线,根据本方案避障过程中的最短路径和贝塞尔曲线控制点选择的原则,将割草机器人从起点做曲线运动到终点的过程中控制点坐标设为P0到Pn,n为控制点的总个数(其中控制点的个数最大值由算法根据实际应用场景以及硬件平台等因素设定),则该贝塞尔曲线函数B(t)为:
在障碍物比较简单的空旷草坪上,拥有两个控制点的三阶贝塞尔曲线(CubicBezier Curve)因为适当的计算复杂度和较好的效果而最为适用,因此本实施例取贝塞尔曲线为拥有两个控制点的三阶贝塞尔曲线,即取n=2,则三阶贝塞尔曲线的展开二项式为:
C(t)=(1-t)3P0+3(1-t)2tP1+3(1-t)t2P2+t3P3,t∈[0,1] (8)
式中,C(t)为三阶贝塞尔曲线的展开二项式,且C(t)即为割草机器人由当前位置返回充电站位置的最短平滑曲线路径(如图7中的虚线曲线贝塞尔曲线1所示)。
本实施例在两个控制点的三阶贝塞尔曲线情景下经数学模拟验证可行,贝塞尔曲线不与障碍物所占面重合;在更多控制点的复杂情景下由于高阶贝塞尔曲线缺乏数值稳定性,可以引入连续贝塞尔或者B样条曲线来获得更好的结果,同时计算复杂度也会增加。在花园草坪这种相对空旷环境中认为三阶贝塞尔曲线足够处理绝大部分的情景,因此本申请提供的避障巡回方法完全适用于割草机器人。
本实施例的避障巡回方法为割草机器人运动到指定位置并实现自主避障提供了智能化计算最优路径的综合解决方法,且考虑到了马达控制的特性,旨在提升割草机器人的运动能力和割草效率。在较为空旷的草坪中使用本实施例的方法不需要提供太高的算力支持,而且在使用GPS差分定位可以得到较为准确的实时位姿达到即使在狭小的间隙中也能精确避障巡回的目的。
应该理解的是,虽然图1的流程图中的各个步骤按照箭头的指示依次显示,但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明,这些步骤的执行并没有严格的顺序限制,这些步骤可以以其它的顺序执行。而且,图1中的至少一部分步骤可以包括多个子步骤或者多个阶段,这些子步骤或者阶段并不必然是在同一时刻执行完成,而是可以在不同的时刻执行,这些子步骤或者阶段的执行顺序也不必然是依次进行,而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在另一个实施例中,提供一种割草机器人的避障巡回系统,即一种计算机设备,该计算机设备可以是终端。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中,该计算机设备的处理器用于提供计算和控制能力。
该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种割草机器人的避障巡回方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏,该计算机设备的输入装置可以是显示屏上覆盖的触摸层,也可以是计算机设备外壳上设置的按键、轨迹球或触控板,还可以是外接的键盘、触控板或鼠标等。
以上所述实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。