机器人滚动路径规划方法、系统、存储介质、设备及应用技术方案

本发明专利技术属于机器人路径规划技术领域，公开了一种机器人滚动路径规划方法、系统、存储介质、设备及应用，包括：初始化；构建滚动路径规划区域并计算各非障碍物节点所受的综合静态势场；计算当前节点周围八节点的动态斥力势场；判断当前节点是否为目标点，判断当前点是否位于本次滚动规划的窗口边界；输出当前滚动窗口内规划好的路径；判断当前滚动窗口内规划出的可行路径长度是否大于机器人行进步长；在每轮滚动规划所得的局部路径的基础上，对路径节点的连通性检测进行路径平滑；将平滑处理后的路径点保存在PointList。本发明专利技术使用基于计算势场强度方法来代替矢量合成方法，避免传统人工势场法的局部极值问题，更满足动态避障的要求。

【技术实现步骤摘要】
机器人滚动路径规划方法、系统、存储介质、设备及应用
本专利技术属于机器人路径规划
，尤其涉及一种机器人滚动路径规划方法、系统、存储介质、设备及应用。
技术介绍
目前，在存在动态障碍物的未知环境中，移动机器人只能通过传感器获取到瞬时“静态”的局部环境信息，通过若干个“静”态的局部环境信息便可获得环境的动态信息，主要是动态障碍物信息，充分利用这些信息便可以实现一个局部环境的路径规划，若干个这样的局部规划前后衔接就可以完成一项全局路径规划任务，所以利用滚动窗口法进行路径规划是研究这类问题的一种自然选择。多数有关移动机器人路径规划的研究主要限于静态环境，但在实际情况中存在移动障碍物，并且传感器系统很难给出障碍物的细节描述，所以应用传统人工势场的避撞方法效果并不理想。人工势场法旨在跨越机器人的地图建立一个场或梯度，在场的作用下机器人从多个先验位置到达目标位置。这种方法最初是为机器人机械手的路径规划而创建的，使用频繁，并在移动机器人学范畴中，从属多个版本。人工势场法把机器人处理成在人工势场U(X)影响下的一个点。像球在重力场作用下滚下山一样，机器人在人为设计的场的影响下移动。目标(势场中的极小值点)表现为对机器人的引力，障碍物表现为势场的高峰或机器人所受的斥力。所有这些力的叠加，施加于机器人。这样一种人工势场平滑地引导机器人趋向目标，同时避免碰撞已知的障碍物。重要的是，这不只是路径规划，最终产生的场也是机器人的控制律。假定机器人能够相对于地图和势场定位自身的位置，那么根据势场，它也可确定其下一个动作。人工势场法算法高效、生成路径平滑，适用于路径实时性及安全性要求较高的路径规划任务。但本身也存在局部极值、目标不可达等缺陷。下面列举四个人工势场法的缺点：(1)动态环境路径规划能力差传统人工势场法只考虑了机器人与目标点，以及障碍物之间的距离信息，因此通常用于静态环境的路径规划，对动态环境的适应性较差。当出现动态障碍物或动态目标点时，传统人工势场法会因为实时性较差而无法顺利到达目标点。(2)目标不可达问题当机器人与目标点距离很近时，若目标点附近有障碍物，机器人所受引力相对于此时极大的斥力近似为零，那么机器人将会很难达到目标点。(3)局部极小值问题机器人在运动过程中，除目标点外，当斥力与引力的合力为0时，会陷入局部极值，最简单的情况如障碍物处于移动机器人与目标点的连线上时，机器人会出现受力为0，从而停止运动或震荡不前。该问题也被称为局部最优解问题或者局部稳定问题。(4)路径振荡问题在障碍物前发生振荡现象，呈锯齿形状况前进。因此，亟需一种新的机器人滚动路径规划方法。通过上述分析，现有技术存在的问题及缺陷为：(1)多数有关移动机器人路径规划的研究主要限于静态环境，但在实际情况中存在移动障碍物，并且传感器系统很难给出障碍物的细节描述，所以应用传统人工势场的避撞方法效果并不理想。(2)传统人工势场的动态环境路径规划能力差，只考虑了机器人与目标点，以及障碍物之间的距离信息，因此通常用于静态环境的路径规划，对动态环境的适应性较差。当出现动态障碍物或动态目标点时，传统人工势场法会因为实时性较差而无法顺利到达目标点。(3)传统人工势场存在目标不可达问题。当机器人与目标点距离很近时，若目标点附近有障碍物，机器人所受引力相对于此时极大的斥力近似为零，那么机器人将会很难达到目标点。(4)传统人工势场存在局部极小值问题。机器人在运动过程中，除目标点外，当斥力与引力的合力为0时，会陷入局部极值，最简单的情况如障碍物处于移动机器人与目标点的连线上时，机器人会出现受力为0，从而停止运动或震荡不前，该问题也被称为局部最优解问题或者局部稳定问题。(5)传统人工势场存在路径振荡问题，即在障碍物前发生振荡现象，呈锯齿形状况前进。进一步导致的技术问题是机器人所规划出的路径不便于实际运行场景下的移动。解决以上问题及缺陷的难度为：针对传统人工势场法的上述缺陷，需要进行综合考虑，对势场进行重新设计。解决以上问题及缺陷的意义为：对传统人工势场法进行改进，使机器人更适应实际场景的需求。
技术实现思路
针对现有技术存在的问题，本专利技术提供了一种机器人滚动路径规划方法、系统、存储介质、设备及应用。本专利技术是这样实现的，一种机器人滚动路径规划方法，所述机器人滚动路径规划方法包括以下步骤：初始化；构建滚动路径规划区域并求出各非障碍物节点所受的综合静态势场Usum，并将起始点设为当前节点；计算当前节点周围八节点的动态斥力势场Urepd，并将邻域Usum+Urepd小的值设为当前点；当前窗口内若无动态障碍物，则所述动态斥力势场为0；判断当前节点是否为目标点，若是，则转输出当前滚动窗口内规划好的路径；否则，则转判断当前点是否位于本次滚动规划的窗口边界；判断当前点是否位于本次滚动规划的窗口边界，若是，则转将当前节点记为当前滚动窗口下的子目标点；否则，则转计算当前节点周围八节点的动态斥力势场Urepd，并将邻域Usum+Urepd小的值设为当前点；将当前节点记为当前滚动窗口下的子目标点；输出当前滚动窗口内规划好的路径；判断当前滚动窗口内规划出的可行路径长度是否大于机器人行进步长，若是，则转从本次滚动规划的起始点起按步长截取局部路径的路径点，即切片处理，并对截取出的路径点平滑处理；否则，对规划出的局部路径的路径点平滑处理，然后转平滑处理后的路径点保存在PointList，并将所述PointList的第一个节点设为机器人当前节点；从本次滚动规划的起始点起按步长截取局部路径的路径点，即切片处理，并对截取出的路径点平滑处理；平滑处理后的路径点保存在PointList，并将所述PointList的第一个节点设为机器人当前节点。进一步，所述机器人滚动路径规划方法，还包括：(1)在每一次滚动规划区域，即滚动窗口内计算出所述范围内各个自由节点受到的引力势场；(2)计算出该范围内各个自由节点与动静态障碍物点的距离，并根据所述距离计算出所述范围内各个自由节点的斥力场强度；(3)若当前滚动窗口内探测到有动态障碍物的存在，则需计算由机器人与动态障碍物的相对运动速度引起的速度斥力势场；其中，在一个滚动窗口内，每次只能计算机器人当前所占节点的相邻八个节点对应的速度斥力场强度。进一步，所述人工势场法的基本思想是：机器人被吸引向目标，同时被先前已知的障碍物所排斥；如果在机器人移动中出现新的障碍物，为了集成这个新的信息，可以更新势场；在最简单的情况下，假设机器人是一个点，忽略机器人的运动方向θ，最后所得的势场是关于(x,y)的二元函数，换言之，势场是三维的；假定一个可微的势场函数U(X)，则可得到作用于位置X＝(x,y)的相关人工力F(X)：其中，表示在位置点X处U的梯度向量；作用于机器人的势场的计算值是目标的引力场和障碍物的斥力场之和：U(X)本文档来自技高网...

【技术保护点】
1.一种机器人滚动路径规划方法，其特征在于，所述机器人滚动路径规划方法包括以下步骤：/n初始化；/n构建滚动路径规划区域并求出各非障碍物节点所受的综合静态势场U

【技术特征摘要】
1.一种机器人滚动路径规划方法，其特征在于，所述机器人滚动路径规划方法包括以下步骤：
初始化；
构建滚动路径规划区域并求出各非障碍物节点所受的综合静态势场Usum，并将起始点设为当前节点；
计算当前节点周围八节点的动态斥力势场Urepd，并将邻域Usum+Urepd小的值设为当前点；当前窗口内若无动态障碍物，则所述动态斥力势场为0；
判断当前节点是否为目标点，若是，则转输出当前滚动窗口内规划好的路径；否则，则转判断当前点是否位于本次滚动规划的窗口边界；
判断当前点是否位于本次滚动规划的窗口边界，若是，则转将当前节点记为当前滚动窗口下的子目标点；否则，则转计算当前节点周围八节点的动态斥力势场Urepd，并将邻域Usum+Urepd小的值设为当前点；
将当前节点记为当前滚动窗口下的子目标点；
输出当前滚动窗口内规划好的路径；
判断当前滚动窗口内规划出的可行路径长度是否大于机器人行进步长，若是，则转从本次滚动规划的起始点起按步长截取局部路径的路径点，即切片处理，并对截取出的路径点平滑处理；否则，对规划出的局部路径的路径点平滑处理，然后转平滑处理后的路径点保存在PointList，并将所述PointList的第一个节点设为机器人当前节点；
从本次滚动规划的起始点起按步长截取局部路径的路径点，即切片处理，并对截取出的路径点平滑处理；
平滑处理后的路径点保存在PointList，并将所述PointList的第一个节点设为机器人当前节点。


2.如权利要求1所述的机器人滚动路径规划方法，其特征在于，所述机器人滚动路径规划方法，还包括：
(1)在每一次滚动规划区域，即滚动窗口内计算出所述范围内各个自由节点受到的引力势场；
(2)计算出该范围内各个自由节点与动静态障碍物点的距离，并根据所述距离计算出所述范围内各个自由节点的斥力场强度；
(3)若当前滚动窗口内探测到有动态障碍物的存在，则需计算由机器人与动态障碍物的相对运动速度引起的速度斥力势场；其中，在一个滚动窗口内，每次只能计算机器人当前所占节点的相邻八个节点对应的速度斥力场强度。


3.如权利要求2所述的机器人滚动路径规划方法，其特征在于，所述人工势场法的基本思想是：机器人被吸引向目标，同时被先前已知的障碍物所排斥；如果在机器人移动中出现新的障碍物，为了集成这个新的信息，可以更新势场；在最简单的情况下，假设机器人是一个点，忽略机器人的运动方向θ，最后所得的势场是关于(x,y)的二元函数，换言之，势场是三维的；假定一个可微的势场函数U(X)，则可得到作用于位置X＝(x,y)的相关人工力F(X)：



其中，表示在位置点X处U的梯度向量；



作用于机器人的势场的计算值是目标的引力场和障碍物的斥力场之和：
U(X)＝Uatt(X)+Urep(X)；
相似地，力也可被分离成吸引和排斥两部分：





4.如权利要求2所述的机器人滚动路径规划方法，其特征在于，步骤(1)中，所述引力势场的构建，包括：
将引力势场函数定义为抛物线函数：



式中，Katt是一个正的比例因子，这里也称为引力常数；ρ(X,Xgoal)表示欧氏距离||X-Xg||；引力势场是可微的，引力为引力势场对距离求导，形成吸引力Fatt：



当机器人到达目标点时，该力线性地收敛至0。


5.如权利要求2所述的机器人滚动路径规划方法，其特征在于，步骤(2)中，所述斥力势场函数为：



式中，Krep是一个比例因子，这里称之为距离斥力常数；ρ(X,Xobs)代表移动机器人与障碍物之间距离，严格意义上，ρ(X,Xobs)表示一种最小距离，它是正常数，即表示某障碍物所占区域Cobs与位置X的关系，即对于所有的X′∈Cobs，有ρ(X,Xobs)＝min||X-X′||，从而，势场区域就被局限于障碍物的表面上；ρo为斥力势场的作用范围，通常称为障碍物的影响半径；斥力势场函数Urep(X)是正的或零，当位置点X特别接近障碍物时，它趋于无穷大；
斥力即是斥力场对距离求导：



用Xobs表示某障碍物所占区域Cobs上距离X最近的位置，即ρ(X,Xobs)＝||X-Xobs||，则是由Xobs指向X的单位向量，即：



一个承受引力和斥力的点状机器人，其所受合力为F(X)＝Fatt(X)+Frep(X)，它使机器人避开障碍物趋向目标移动；在理想条件下，通过设置一个正比于合势场力向量的机器人速度向量，与球绕过障碍物并向山下滚动一样，可平滑地引导机器人趋向目标；
将障碍物的边界表示为一组点障碍物，并通过一控制点i来表示机器人；将代表机器人的控制点的笛卡尔位置写为X∈R2，障碍物的边界点j为cj∈R2；那么这两点之间的距离为||X-cj||，在控制点X处由障碍点j而引起的势能为：



它在控制点处产生排斥力：



因此，所述作用在机器人上的总力F(X)，即为吸引力Fatt(X)和代表机器人的控制点i与障碍物所有边界点j对应的排斥力Frepj(X)的总和；
步骤(2)中，所述根据所述距离计算出该范围内各个自由节点的斥力场强度，包括：



式中，ρn(X,Xg)为移动机器人与目标点距离的n次方，n为正系数，通常取2；
相应斥力变为：









式中，Frep1(X)方向由障碍物指向移动机器人，Frep2(X)方向由移动机器人指向目标点；
步骤(3)中，所述速度斥力势场的构建，包括：
将移动机器人与动态障碍物的速度分别用vr和vo来表示，用Urepv表示相对速度(vr-vo)产生的速度斥力势场，构建的速度斥力势场函数表示为：



式中，Krepv为速度斥力常量；vor＝vr-vo矢量，是智能机器人与动态障碍物的相对速度，方向与斥力方向相反；θ为相对速度vor的方向与移动机器人和动态障碍物的中心连线形成的夹角，当时，移动机器人向着远离障碍物方向运动，速度斥力势场作用力减小为零；设移动机器人、动态障碍物的位置坐标分别为X＝(x,y)，Xobs＝(xo,yo)，二者相对位置坐标(Xobs-X)＝(xor,yor)；
由构建的速度斥力势场函数可得速度斥力表达式：



移动机器人与障碍物速度分解过程中，φ为vor与X轴之间夹角；
cosθ＞0表示移动机器人在朝向动态障碍物运动，此时需要相对速度斥力；cosθ≤0表示移动机器人在远离动态障碍物运动，此时就算没有速度斥力也不会与动态障碍物发生碰撞，因此cosθ≤0时Urepv＝0；
距离和速度产生的总斥力势场函数Urepd，即动态斥力势场函数为：



动态人工势场法即在传统人工势场模型中加入速度斥力函数，改进后移动机器人受到的斥力函数为：
Frep(X,vor)＝Frep(X)+Frepv(X)；
移动机器人受到斥力和引力合力函数为：
F(X,vor)＝Fatt(X)+Frep(X)+Frepv(X)。


6.如权利要求1所述的机器人滚动路径规划方法，其特征在于，通过如下公式计算出当前滚动窗口范围内各个自由节点受到的静态综合势场Usum：



其中，MAX(Uatt(Xma))为机器人运行环境中当前滚动路径规划区域，即滚动窗口内的最大引力势场强度，将Usum+Urepd作为A*算法中的估价函数，机器人总是向当前节点八个方向邻域中Usum+Urepd最小值点移动，倘若有多个可选节点，选择使机器人转角中最小的节点；在静态综合势场Usum的计算公式中引入MAX(Uatt(Xma))可以使机器人移动过程中能优先避开障碍物，有效避免出现如下问题：当机器人运行环境范围过大且当前点距离目标点过远时Uatt值相对Urep过大，机器人向八个方向邻域Usum+Urepd值最小点移动时会向目标点移动而不是优先避开障碍物；
在每轮滚动规划所得的局部路径的基础上，通过添加路径平滑模块对路径节点的连通性检测进行路径平滑，包括：
(1)设路径Path＝P(1,2,...,N)，平滑迭代次数设为M；令k＝1:M，开始路径平滑循环；

【专利技术属性】
技术研发人员：李泽坤，胡核算，马艳，
申请(专利权)人：西安电子科技大学，
类型：发明
国别省市：陕西;61