CN112526991B - 机器人运动方法、装置、电子设备及存储介质 - Google Patents

机器人运动方法、装置、电子设备及存储介质 Download PDF

Info

Publication number
CN112526991B
CN112526991B CN202011353145.1A CN202011353145A CN112526991B CN 112526991 B CN112526991 B CN 112526991B CN 202011353145 A CN202011353145 A CN 202011353145A CN 112526991 B CN112526991 B CN 112526991B
Authority
CN
China
Prior art keywords
gap
robot
point
end point
passable
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011353145.1A
Other languages
English (en)
Other versions
CN112526991A (zh
Inventor
秦家虎
高炤
王帅
张展鹏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Science and Technology of China USTC
Original Assignee
University of Science and Technology of China USTC
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Science and Technology of China USTC filed Critical University of Science and Technology of China USTC
Priority to CN202011353145.1A priority Critical patent/CN112526991B/zh
Publication of CN112526991A publication Critical patent/CN112526991A/zh
Application granted granted Critical
Publication of CN112526991B publication Critical patent/CN112526991B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

本公开提供一种用于未知环境中的机器人运动方法、装置、电子设备及存储介质，应用于机器人技术领域，包括：确定机器人周围的可通行间隙集合；基于所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小，从所述可通行间隙集合中选择目标间隙；根据所述目标间隙计算期望运动方向；使用动态窗口法使所述机器人沿所述期望运动方向运动。使机器人能够沿着障碍边沿移动，使机器人受障碍引导，摆脱局部极小。

Description

机器人运动方法、装置、电子设备及存储介质
技术领域
本申请涉及机器人技术领域，尤其涉及一种用于未知环境中的机器人运动方法、装置、电子设备及存储介质。
背景技术
机器人导航方法的核心是路径规划，路径规划算法主要分为全局路径规划和局部路径规划。由于未知环境是指移动机器人没有关于环境的任何先验信息，所以未知环境的导航一般只能通过局部机器人运动方法实现。局部机器人运动方法是指利用传感器采集机器人周围的障碍分布，仅利用这些局部信息对机器人移动行为做出规划，使机器人能够安全到达终点。
现有的基于间隙的导航方法极少能够充分利用传感器数据，比如一些通过“开口”的角度大小来判断是否是间隙的算法，显然无法提取到两障碍存在交叠，但二者之间宽度足够大的间隙，再如一些基于开口的距离大小来判断是否是间隙的方法，由于算法考虑的不足，可能将无法使机器人通行的间隙判断为可通行间隙。或者，由于缺乏关于环境的先验知识，基于间隙的规划方法盲目地选择与终点方向最近的间隙，而在一些复杂的环境中，这往往会导致机器人陷入僵局，再无法靠近终点。
发明内容
本申请的主要目的在于提供一种用于未知环境中的机器人运动方法、装置、电子设备及存储介质，使机器人能够沿着障碍边沿移动，这种行为可以使机器人受障碍引导，摆脱局部极小。
为实现上述目的，本申请实施例第一方面提供一种种用于未知环境中机器人运动方法，包括：
确定机器人周围的可通行间隙集合；
基于所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小，从所述可通行间隙集合中选择目标间隙；
根据所述目标间隙计算期望运动方向；
使用动态窗口法使所述机器人沿所述期望运动方向运动。
可选的，所述方法还包括：
利用激光雷达传感器扫描当前环境，得到关于所述当前环境的扫描点集P＝{pi＝(ρi，θi)|i∈[0，Ns-1]}，其中，pi＝(ρi，θi)表示第i个扫描点的极坐标，ρi表示第i个扫描点的极坐标距离，θi表示第i个扫描点的极坐标角度，Ns表示扫描点总数，p0表示机器人正前方数据，下标沿逆时针方向递增；
连接所述机器人所述当前位置or和终点位置pgoal，形成直线orpgoal；
过所述当前位置or作垂直于所述直线orpgoal的垂线，将所述当前障碍环境分为四个区域，将位于线段orpgoal两侧的两个区域分别定义为区域I和区域II；
设所述区域I内的扫描点集为P1，所述区域II内的扫描点集为P2；
计算
Figure BDA0002799070540000021
pi＝(ρi，θi)∈P1，pj＝(ρj，θj)∈P2，其中，d表示分别属于P1和P2的两个点之间的最小距离，ρj表示第j个扫描点的极坐标距离，θj表示第j个扫描点的极坐标角度；
若d大于预设宽度阈值2Rs且所述终点在传感器探测范围内，则所述机器人可直接到达所述终点，将所述终点方向θgoal设为所述期望运动方向θmd，执行所述使用动态窗口法使所述机器人沿所述期望运动方向运动的步骤；
若d不大于所述预设宽度阈值2Rs或所述终点不在传感器探测范围内，则所述机器人不可直接到达所述终点，执行所述确定机器人周围的可通行间隙集合的步骤。
可选的，所述确定机器人周围的可通行间隙集合包括：
获取当前障碍环境中障碍物的位置数据；
根据所述障碍物的位置数据，提取所述当前障碍环境中存在的间隙，形成候选间隙集合Gt；
分别提取所述候选间隙集合Gt中各候选间隙的最窄宽度；
分别判断每个所述候选间隙的最窄宽度是否大于所述预设宽度阈值2Rs；
若存在候选间隙的最窄宽度大于所述预设宽度阈值2Rs，则将所述候选间隙添加到可通行间隙集合G中。
可选的，其中，设pi和pj为两个相邻扫描点，定义两类不连续边沿，其中，第一类不连续边沿ρi＜Rmax&&ρj＝Rmax，第二类不连续边沿ρj-ρi>2Rs，Rmax是所述传感器可感知的最大距离，若j＞i，则在pi处检测到了上升沿，若j＜i，则在pj处检测到了下降沿；
若在pi处检测到了第一类不连续边沿中的上升沿，则继续扫描，直到在pj，j∈[i+1，i+Ns]处检测到第一类不连续边沿中的下降沿，此时存在间隙g(i，j)，所述间隙g(i，j)两侧端点分别为是pi和pj，若所述间隙g(i，j)的角度宽度不小于180度，则将所述间隙g(i，j)纳入可通行间隙集合G，若所述间隙g(i，j)的角度宽度小于180度且间隙宽度大于所述预设宽度阈值2Rs，则将所述间隙g(i，j)纳入所述候选间隙集合Gt；
若在pi处检测到第二类不连续边沿中的上升沿或者下降沿，则存在间隙g(i，i+1)，若所述间隙宽度大于所述预设宽度阈值2Rs，则将所述间隙g(i，i+1)纳入所述候选间隙集合Gt；
执行所述分别提取所述候选间隙集合Gt中各候选间隙的最窄宽度的步骤。
可选的，其中，计算所述左侧搜索区间kl＝Int(dij，ρi)，所述右侧搜索区间kr＝Int(dij，ρj)，dij表示扫描点pj和pi的欧式距离，Int(d，ρ)定义为：
Figure BDA0002799070540000041
计算所述间隙g(i，j)的左侧点集Pl＝{k∈[j，i+kl]|dik＜dij}和所述间隙g(i，j)的右侧点集Pr＝{k∈[j-kr，i]|dkj＜dij}；
求所述间隙g(i，j)最窄宽度位置：
Figure BDA0002799070540000042
若di′j′＞2Rs，则将所述间隙添加至所述可通行间隙集合G中。
可选的，所述基于所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小，从所述可通行间隙集合中选择目标间隙包括：
规定所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小φg的初始时刻的正方向是使φg更小的方向；
若
Figure BDA0002799070540000043
计算虚拟目标方向θvg＝α·θgoal，α∈[0，1]表示待调参数，从所述可通行间隙集合G中选择与所述虚拟目标方向θvg最近的可通行隙作为目标间隙，并将所述目标间隙与所述虚拟目标方向θvg更近一侧的端点记为pc＝(ρc，θc)，另一侧端点记为pf＝(ρf，θf)；
若
Figure BDA0002799070540000044
从
Figure BDA0002799070540000045
方位开始，沿正方向搜索，将遇到的第一个间隙作为目标间隙，并将所述目标间隙首先遇到的一侧端点记为pc＝(ρc，θc)，另一侧端点记为pf＝(ρf，θf)。
可选的，所述根据所述目标间隙计算期望运动方向包括：
当
Figure BDA0002799070540000046
时，获取所述目标间隙与所述虚拟目标方向θvg更近一侧的端点pc＝(ρc，θc)，另一侧端点pf＝(ρf，θf)；
当
Figure BDA0002799070540000047
时，获取所述目标间隙首先遇到的一侧端点pc＝(ρc，θc)，另一侧端点pf＝(ρf，θf)；
根据pc＝(ρc，θc)以及pf＝(ρf，θf)，计算安全角度αc、αf以及安全区域，得到期望运动方向；
其中，
Figure BDA0002799070540000051
Figure BDA0002799070540000052
其中，θtg表示所述目标间隙的角度宽，
Figure BDA0002799070540000053
Figure BDA0002799070540000054
若pc是目标间隙左端点，则θ′c＝θc-αc，θ′f＝θf+αf，安全区域为[θ′f，θ′c]；
若pc是目标间隙右端点，则θ′c＝θc+αc，θ′f＝θf-αf，安全区域为[θ′c，θ′f]；
期望运动方向
Figure BDA0002799070540000055
本申请实施例第二方面提供一种用于未知环境中机器人运动装置，包括：
确定模块，用于确定机器人周围的可通行间隙集合；
选择模块，用于基于所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小，从所述可通行间隙集合中选择目标间隙；
计算模块，用于根据所述目标间隙计算期望运动方向；
运动模块，用于使用动态窗口法使所述机器人沿所述期望运动方向运动。
本申请实施例第三方面提供了一种电子设备，包括：
存储器，处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现本申请实施例第一方面提供的用于未知环境中的机器人运动方法。
本申请实施例第四方面提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现本申请实施例第一方面提供的用于未知环境中的机器人运动方法。
从上述本申请实施例可知，本申请提供的用于未知环境中的机器人运动方法、装置、电子设备及存储介质，以传感器扫描点的不连续为间隙的线索进行分析，可以检测到所有可能的间隙，以这些间隙的两侧端点为圆心，间隙宽度为半径作圆，可以确定该间隙对应的最窄宽度的两个端点所在的区间，此时，该间隙的最窄宽度即可确定，利用这一最窄宽度和机器人实际的尺寸，可以删除机器人无法通行的间隙，即提取出所有能使机器人通行的间隙。使机器人能够沿着障碍边沿移动，这种行为可以使机器人受障碍引导，摆脱局部极小。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为机器人处在障碍环境中的示意图；
图2为本申请一实施例提供的用于未知环境中的机器人运动方法的流程示意图；
图3为本申请一实施例提供的用于未知环境中的机器人运动方法的流程示意图；
图4为本申请一实施例提供的当前障碍环境划分的示意图；
图5为本申请一实施例提供的间隙提取的示意图；
图6为本申请一实施例提供的期望运动方向的示意图；
图7为本申请一实施例提供的机器人运动轨迹的示意图；
图8为本申请一实施例提供的利用用于未知环境中的机器人运动方法在MATLAB中仿真的示意图；
图9为本申请一实施例提供的用于未知环境中的机器人运动装置的结构示意图；
图10示出了一种电子设备的硬件结构示意图。
具体实施方式
为使得本申请的申请目的、特征、优点能够更加的明显和易懂，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而非全部实施例。基于本申请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
请参阅图1，图1为机器人处在障碍环境中的示意图。移动机器人在地面进行移动，装载了一个传感器，可以是360度的激光雷达，用于对局部环境的感知，而障碍环境中布置有许多散乱的障碍，采用本可以使机器人在不知环境的全局地图的前提下，仍然能够到达给定终点。
请参阅图2，图2为本申请一实施例提供的机器人运动方法的流程示意图，该方法主要包括以下步骤：
S201、确定机器人周围的可通行间隙集合；
S202、基于所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小，从所述可通行间隙集合中选择目标间隙；
S203、根据所述目标间隙计算期望运动方向；
S204、使用动态窗口法使所述机器人沿所述期望运动方向运动。
请参阅图3，图3为本申请一实施例提供的机器人运动方法的流程示意图，图3中，N表示否，Y表示是，该方法主要包括以下步骤：
步骤1、程序初始化。初始化算法中使用的一系列参数，检查各传感器是否正常工作。
步骤2、读取传感器数据，判断终点是否直接可达。
步骤2.1、读取传感器数据，并将扫描点集记为P＝{pi＝(ρi，θi)|i∈[0，Ns-1]}，其中pi＝(ρi，θi)表示第i个扫描点的极坐标，ρi表示第i个扫描点的极坐标距离，θi表示第i个扫描点的极坐标角度，Ns表示扫描点总数。第一个扫描点是移动机器人的正前方，扫描点下标沿逆时针方向递增。
步骤2.2、判断终点是否直接可达。连接机器人位置or和终点位置pgoal形成直线orpgoal，过当前位置or作垂直于直线orpgoal的垂线，此时，两条直线将当前障碍环境分为四部分；设线段orpgoal左右两侧的区间内的点集分别为P1和P2；计算
Figure BDA0002799070540000081
其中，d表示分别属于P1和P2的两个点之间的最小距离，ρj表示第j个扫描点的极坐标距离，θj表示第j个扫描点的极坐标角度；若d＞2Rs且终点在传感器探测范围内，则直接可达，否则，不可直接可达。
步骤2.3、若终点直接可达，将终点方向θgoal设为期望运动方向θmd，并直接执行步骤6，否则，继续执行后续步骤3。
步骤3、执行间隙提取算法，获取可通行间隙集合。
步骤3.1、遍历一次传感器数据，检测所有不连续边沿，并获取候选间隙集合。
步骤3.1.1、设两个相邻扫描点为pi和pj，则可以定义两类不连续边沿：
第一类不连续边沿：ρi＜Rmax&&ρj＝Rmax
第二类不连续边沿：ρj-ρi＞2Rs
其中，ρi和ρj分别是扫描点pi和pj的距离，Rmax是传感器可感知的最大距离。
步骤3.1.2、基于上述不连续边沿的定义再做进一步规定：若j＞i，则称在pi处检测到了上升沿，反之，则称在pj处检测到了下降沿。
步骤3.1.3、从扫描点P0开始，沿下标递增的顺序遍历扫描点集P：
若在pi处检测到了第一类类不连续边沿中的上升沿，则继续向后扫描，直到在pj，j∈[i+1，i+Ns]处检测到第一类不连续边沿中的下降沿，此时构成一个间隙g(i，j)，其两侧端点分别为是pi和pj。进一步分析：若g(i，j)的角度宽度不小于180度，则机器人必然可以通过该间隙，且间隙可视，因此将其纳入可通行间隙集合G。若g(i，j)的角度宽度小于180度且间隙宽度大于预设宽度阈值2Rs，需要经过步骤3.2提取其最窄宽度，以判断该间隙是否可通行，因此，将其纳入候选间隙集合Gt。
若在pi处检测到第二类不连续边沿中的上升沿或者下降沿，则存在间隙g(i，i+1)，将其纳入候选间隙集合Gt。
步骤3.1.4、至此，获得候选间隙集合Gt和间隙集合G，若Gt为空集，则返回间隙集合G，执行步骤4，否则，执行步骤3.2。
步骤3.2、提取全部候选间隙的最窄宽度，并与预设宽度阈值2Rs相比较，将能够使机器人顺利通行的间隙添加到可通行间隙集合G。
步骤3.2.1、以间隙g(i，j)为例，计算左侧搜索区间kl＝Int(dij，ρi)，右侧搜索区间kr＝Int(dij，ρj)，其中，dij表示扫描点pj和pi的欧式距离，函数Int(d，ρ)定义为：
Figure BDA0002799070540000091
步骤3.2.2、计算间隙g(i，j)左侧点集Pl＝{k∈[j，i+kl]dik＜dij}和右侧点集Pr＝{k∈[j-kr，i]|dkj＜dij}。
步骤3.2.3、求g(i，j)最窄宽度位置：
Figure BDA0002799070540000092
步骤3.2.4、若di′j′＞2Rs，将间隙g(i′，j′)添加至可通行间隙间隙集合G，得到全部可通行间隙集合G。
步骤4、从可通行间隙集合G中选择目标间隙。
步骤4.1、定义一个角度φg，用来表示机器人朝向和终点方向之间的夹角大小，并规定其初始时刻的正方向是使终点方向到机器人朝向的夹角更小的方向。当机器人沿此正方向旋转时，
Figure BDA0002799070540000101
增大，反之减小。当沿反方向旋转减小为零并继续旋转时，其正方向也发生改变，因此，
Figure BDA0002799070540000102
增大。
步骤4.2、若
Figure BDA0002799070540000103
则计算虚拟目标方向：θvg＝α·θgoal，α∈[0，1]表示待调参数。从间隙集合G中选择与θvg方向最近的间隙作为目标间隙。将该间隙与θvg方向更近一侧的端点记为pc＝(ρc，θc)，另一端点记为pf＝(ρf，θf)。
步骤4.3、若
Figure BDA0002799070540000104
则从
Figure BDA0002799070540000105
方位开始，沿φg正方向搜索，将遇到的第一个间隙作为目标间隙。将首先遇到的一侧端点记为pc＝(ρc，θc)，另一侧端点记为pf＝(ρf，θf)。
步骤5、根据目标间隙计算期望运动方向。
步骤5.1、计算安全角度αc和αf，定义如下：
Figure BDA0002799070540000106
Figure BDA0002799070540000107
式中，θtg表示目标间隙的角度宽，Dc和Df分别定义为：
Figure BDA0002799070540000108
Figure BDA0002799070540000109
步骤5.2、安全区域计算
若pc是目标间隙左端点，则θ′c＝θc-αc，θ′f＝θf+αf，安全区域为[θ′f，θ′c]。
若pc是目标间隙右端点，则θ′c＝θc+αc，θ′f＝θf-αf，安全区域为[θ′c，θ′f]。
步骤5.3、期望方向计算
Figure BDA0002799070540000111
步骤6、使用动态窗口法控制机器人沿期望方向移动。
步骤6.1、根据机器人自身约束及周围环境约束可确定速度采样空间
Vr＝Vs∩Va∩Vd
其中，Vs＝{(υ，ω)|υ∈[0，υmax]∧ω∈[ωmin，ωmax]}，υmax是最大线速度，ωmin和ωmax分别为最小角速度和最大角速度；
Figure BDA0002799070540000112
Figure BDA0002799070540000113
dist(υ，ω)为制动距离，
Figure BDA0002799070540000114
和
Figure BDA0002799070540000115
分别为最大加速度和最大角加速度；
Figure BDA0002799070540000116
υa和ωa为当前线速度和角速度。
步骤6.2、对速度空间内的每一组速度(υ，ω)进行轨迹预测，采用的评价函数：G(υ，ω)＝α·heading(υ，ω)+β·obs(υ，ω)+γ·vel(υ，ω)，式中，α，β，γ均为待调系数，其余各项指标heading(υ，ω)、obs(υ，ω)、vel(υ，ω)计算如下：
Figure BDA0002799070540000117
Figure BDA0002799070540000118
其中，θ为机器人在轨迹终点位置的朝向与目标方向的夹角。
Figure BDA0002799070540000119
其中，Robs为障碍到机器人轨迹中心的最小距离，Rrob为机器人半径，若Robs小于Rrob，将排除这条轨迹所对应的速度，Ds为安全距离，函数
Figure BDA00027990705400001110
定义：
Figure BDA00027990705400001111
步骤6.3、从中选择评价最高的轨迹所对应的速度作为机器人的控制输入。
步骤7、重复步骤2至步骤6，直至到达终点。
为了让本领域技术人员更加理解本发明的上述流程，下面结合具体实例对本发明做进一步描述，并在最后给出MATLAB仿真实例。
步骤a：进行程序初始化。
步骤b：读取一次激光雷达数据，判断终点是否直接可达，如图4所示的情况，在区域I和区域II中最小距离d＝0＜2Rs，因此终点不直接可达。
步骤c：执行间隙提取算法，如图5所示，沿逆时针方向从下标0开始扫描激光雷达数据，分别在
Figure BDA0002799070540000121
处有第一类上升沿，他们分别与
Figure BDA0002799070540000122
的第一类下降沿构成间隙g(i1，j1)，g(i3，j3)，g(i4，j4)，在
Figure BDA0002799070540000123
处有第二类上升沿，因此存在间隙g(i2，j2)，实际上有j2＝i2+1，这四个间隙保存为候选间隙集合Gt，进一步分析集合内各间隙的最窄距离(图中双箭头所示)。
步骤d：从图5可知，φg＜π/2，且正方向为顺时针方向，与θvg最近的间隙是g(i′1，j′1)，
Figure BDA0002799070540000124
是最近一侧的端点，
Figure BDA0002799070540000125
是另一侧端点。
步骤e：期望运动方向θmd如图6所示。
步骤f：采用动态窗口法控制机器人沿期望方向运动，如图7所示。
在MATLAB中搭建仿真环境，如图8所示，图8中半径最小的圆形表示移动机器人，半径最小的圆形中的箭头方向表示移动机器人的朝向，半径第二的圆形表示安全距离，半径第三的圆形表示传感器的最大感知范围，空白区域表示无障碍，灰色表示障碍，曲线表示轨迹，移动机器人没有任何关于环境的先验信息，只提供一个可达的终点。
请参阅图9，图9是本申请又一实施例提供的机器人运动装置的结构示意图，该装置可内置于电子设备中，该装置主要包括：
确定模块301，用于确定机器人周围的可通行间隙集合；
选择模块302，用于基于所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小，从所述可通行间隙集合中选择目标间隙；
计算模块303，用于根据所述目标间隙计算期望运动方向；
运动模块304，用于使用动态窗口法使所述机器人沿所述期望运动方向运动。
本公开其中一个实施例中，确定模块301包括：
获取子模块，用于获取当前障碍环境中障碍物的位置数据；
第一提取子模块，用于根据所述障碍物的位置数据，提取所述当前障碍环境中存在的间隙，形成候选间隙集合Gt；
第二提取子模块，用于分别提取所述候选间隙集合Gt中各候选间隙的最窄宽度；
判断子模块，用于分别判断每个所述候选间隙的最窄宽度是否大于所述预设宽度阈值2Rs；
请参见图10，图10示出了一种电子设备的硬件结构图。
本实施例中所描述的电子设备，该电子设备可以是运动机器人或其它具有运动功能的电子设备，包括：
存储器41、处理器42及存储在存储器41上并可在处理器上运行的计算机程序，处理器执行该程序时实现前述图1所示实施例中描述的用于未知环境中的机器人运动方法。
进一步地，该电子设备还包括：
至少一个输入设备43；至少一个输出设备44。
上述存储器41、处理器42输入设备43和输出设备44通过总线45连接。
其中，输入设备43具体可为摄像头、触控面板、物理按键或者鼠标等等。输出设备44具体可为机器人。
存储器41可以是高速随机存取记忆体(RAM，Random Access Memory)存储器，也可为非不稳定的存储器(non-volatile memory)，例如磁盘存储器。存储器41用于存储一组可执行程序代码，处理器42与存储器41耦合。
进一步地，本公开实施例还提供了一种计算机可读存储介质，该计算机可读存储介质可以是设置于上述各实施例中的电子设备中，该计算机可读存储介质可以是前述图10所示实施例中的电子设备。该计算机可读存储介质上存储有计算机程序，该程序被处理器执行时实现前述图1所示实施例中描述的用于未知环境中的机器人运动方法。进一步地，该计算机可存储介质还可以是U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
需要说明的是，在本公开各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。
所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来。
需要说明的是，对于前述的各方法实施例，为了简便描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其它顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定都是本发明所必须的。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其它实施例的相关描述。
以上为对本发明所提供的一种用于未知环境中的机器人运动方法、装置、电子设备及可读存储介质的描述，对于本领域的技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种用于未知环境中的机器人运动方法，其特征在于，包括：
确定机器人周围的可通行间隙集合；
基于所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小，从所述可通行间隙集合中选择目标间隙；
根据所述目标间隙计算期望运动方向；
使用动态窗口法使所述机器人沿所述期望运动方向运动；
利用激光雷达传感器扫描当前环境，得到关于所述当前环境的扫描点集P＝{pi＝(ρi，θi)|i∈[0，Ns-1]}，其中，pi＝(ρi，θi)表示第i个扫描点的极坐标，ρi表示第i个扫描点的极坐标距离，θi表示第i个扫描点的极坐标角度，Ns表示扫描点总数，p0表示机器人正前方数据，下标沿逆时针方向递增；
连接所述机器人当前位置or和终点位置pgoal，形成直线orpgoal；
过所述当前位置or作垂直于所述直线orpgoal的垂线，将当前障碍环境分为四个区域，将位于线段orpgoal两侧的两个区域分别定义为区域I和区域II；
设所述区域I内的扫描点集为P1，所述区域II内的扫描点集为P2；
计算
Figure FDA0003406862560000011
pi＝(ρi，θi)∈P1，pj＝(ρj，θj)∈P2，其中，d表示分别属于P1和P2的两个点之间的最小距离，ρj表示第j个扫描点的极坐标距离，θj表示第j个扫描点的极坐标角度；
若d大于预设宽度阈值2Rs且所述终点在传感器探测范围内，则所述机器人可直接到达所述终点，将所述终点方向θgoal设为所述期望运动方向θmd，执行所述使用动态窗口法使所述机器人沿所述期望运动方向运动的步骤；
若d不大于所述预设宽度阈值2Rs或所述终点不在传感器探测范围内，则所述机器人不可直接到达所述终点，执行所述确定机器人周围的可通行间隙集合的步骤；
其中，所述确定机器人周围的可通行间隙集合包括：
获取当前障碍环境中障碍物的位置数据；
根据所述障碍物的位置数据，提取所述当前障碍环境中存在的间隙，形成候选间隙集合Gt；
分别提取所述候选间隙集合Gt中各候选间隙的最窄宽度；
分别判断每个所述候选间隙的最窄宽度是否大于预设宽度阈值2Rs；
若存在候选间隙的最窄宽度大于所述预设宽度阈值2Rs，则将所述候选间隙添加到可通行间隙集合G中。
2.根据权利要求1所述的用于未知环境中的机器人运动方法，其特征在于，其中，
设pi和pj为两个相邻扫描点，定义两类不连续边沿，其中，第一类不连续边沿ρi＜Rmax&&ρj＝Rmax，第二类不连续边沿ρj-ρi＞2Rs，Rmax是传感器可感知的最大距离，若j＞i，则在pi处检测到了上升沿，若j＜i，则在pj处检测到了下降沿；
若在pi处检测到了第一类不连续边沿中的上升沿，则继续扫描，直到在pj，j∈[i+1，i+Ns]处检测到第一类不连续边沿中的下降沿，此时存在间隙g(i，j)，所述间隙g(i，j)两侧端点分别为是pi和pj，若所述间隙g(i，j)的角度宽度不小于180度，则将所述间隙g(i，j)纳入可通行间隙集合G，若所述间隙g(i，j)的角度宽度小于180度且间隙宽度大于所述预设宽度阈值2Rs，则将所述间隙g(i，j)纳入所述候选间隙集合Gt；
若在pi处检测到第二类不连续边沿中的上升沿或者下降沿，则存在间隙g(i，i+1)，若所述间隙宽度大于所述预设宽度阈值2Rs，则将所述间隙g(i，i+1)纳入所述候选间隙集合Gt；
执行所述分别提取所述候选间隙集合Gt中各候选间隙的最窄宽度的步骤。
3.根据权利要求2所述的用于未知环境中的机器人运动方法，其特征在于，其中，计算左侧搜索区间kl＝Int(dij，ρi)，右侧搜索区间kr＝Int(dij，ρj)，dij表示扫描点pj和pi的欧式距离，Int(d，ρ)定义为：
Figure FDA0003406862560000031
计算所述间隙g(i，j)的左侧点集Pl＝{k∈[j，i+kl]|dik＜dij}和所述间隙g(i，j)的右侧点集Pr＝{k∈[j-kr，i]|dkj＜dij}；
求所述间隙g(i，j)最窄宽度位置：
Figure FDA0003406862560000032
di′j′表示扫描点pi′和pj′的欧式距离；
若di′j′＞2Rs，则将所述间隙添加至所述可通行间隙集合G中。
4.根据权利要求3所述的用于未知环境中的机器人运动方法，其特征在于，所述基于所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小，从所述可通行间隙集合中选择目标间隙包括：
规定所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小φg的初始时刻的正方向是使φg更小的方向；
若
Figure FDA0003406862560000041
计算虚拟目标方向θvg＝α·θgoal，α∈[0，1]表示待调参数，θgoal表示所述机器人中心和所述终点的连线与所述机器人正前方方向的夹角，从所述可通行间隙集合G中选择与所述虚拟目标方向θvg最近的可通行隙作为目标间隙，并将所述目标间隙与所述虚拟目标方向θvg更近一侧的端点记为pc＝(ρc，θc)，另一侧端点记为pf＝(ρf，θf)；
若
Figure FDA0003406862560000042
从
Figure FDA0003406862560000043
方位开始，沿正方向搜索，将遇到的第一个间隙作为目标间隙，并将所述目标间隙首先遇到的一侧端点记为pc＝(ρc，θc)，另一侧端点记为pf＝(ρf，θf)。
5.根据权利要求4所述的用于未知环境中的机器人运动方法，其特征在于，所述根据所述目标间隙计算期望运动方向包括：
当
Figure FDA0003406862560000044
时，获取所述目标间隙与所述虚拟目标方向θvg更近一侧的端点pc＝(ρc，θc)，另一侧端点pf＝(ρf，θf)；
当
Figure FDA0003406862560000045
时，获取所述目标间隙首先遇到的一侧端点pc＝(ρc，θc)，另一侧端点pf＝(ρf，θf)；
根据pc＝(ρc，θc)以及pf＝(ρf，θf)，计算安全角度αc、αf以及安全区域，得到期望运动方向；
其中，
Figure FDA0003406862560000046
Figure FDA0003406862560000051
其中，θtg表示所述目标间隙的角度宽，
Figure FDA0003406862560000052
Figure FDA0003406862560000053
若pc是目标间隙左端点，则θ′c＝θc-αc，θ′f＝θf+αf，安全区域为[θ′f，θ′c]；
若pc是目标间隙右端点，则θ′c＝θc+αc，θ′f＝θf-αf，安全区域为[θ′c，θ′f]；
期望运动方向
Figure FDA0003406862560000054
θc′表示与终点方向最近的安全区域边界的方向，θf′表示与终点方向最远的安全区域边界的方向。
6.一种用于未知环境中机器人运动装置，其特征在于，包括：
确定模块，用于确定机器人周围的可通行间隙集合；
选择模块，用于基于所述机器人当前朝向与所述机器人朝向终点方向之间的夹角大小，从所述可通行间隙集合中选择目标间隙；
计算模块，用于根据所述目标间隙计算期望运动方向；
运动模块，用于使用动态窗口法使所述机器人沿所述期望运动方向运动；
利用激光雷达传感器扫描当前环境，得到关于所述当前环境的扫描点集P＝{pi＝(ρi，θi)|i∈[0，Ns-1]}，其中，pi＝(ρi，θi)表示第i个扫描点的极坐标，ρi表示第i个扫描点的极坐标距离，θi表示第i个扫描点的极坐标角度，Ns表示扫描点总数，p0表示机器人正前方数据，下标沿逆时针方向递增；
连接所述机器人当前位置or和终点位置pgoal，形成直线orpgoal；
过所述当前位置or作垂直于所述直线orpgoal的垂线，将当前障碍环境分为四个区域，将位于线段orpgoal两侧的两个区域分别定义为区域I和区域II；
设所述区域I内的扫描点集为P1，所述区域II内的扫描点集为P2；
计算
Figure FDA0003406862560000061
pi＝(ρi，θi)∈P1，pj＝(ρj，θj)∈P2，其中，d表示分别属于P1和P2的两个点之间的最小距离，ρj表示第j个扫描点的极坐标距离，θj表示第j个扫描点的极坐标角度；
若d大于预设宽度阈值2Rs且所述终点在传感器探测范围内，则所述机器人可直接到达所述终点，将所述终点方向θgoal设为所述期望运动方向θmd，执行所述使用动态窗口法使所述机器人沿所述期望运动方向运动的步骤；
若d不大于所述预设宽度阈值2Rs或所述终点不在传感器探测范围内，则所述机器人不可直接到达所述终点，执行所述确定机器人周围的可通行间隙集合的步骤；
其中，所述确定机器人周围的可通行间隙集合包括：
获取当前障碍环境中障碍物的位置数据；
根据所述障碍物的位置数据，提取所述当前障碍环境中存在的间隙，形成候选间隙集合Gt；
分别提取所述候选间隙集合Gt中各候选间隙的最窄宽度；
分别判断每个所述候选间隙的最窄宽度是否大于预设宽度阈值2Rs；
若存在候选间隙的最窄宽度大于所述预设宽度阈值2Rs，则将所述候选间隙添加到可通行间隙集合G中。
7.一种电子设备，包括：存储器，处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时，实现权利要求1至5中的任一项所述的用于未知环境中的机器人运动方法中的各个步骤。
8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现权利要求1至5中的任一项所述的用于未知环境中的机器人运动方法中的各个步骤。
CN202011353145.1A 2020-11-25 2020-11-25 机器人运动方法、装置、电子设备及存储介质 Active CN112526991B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011353145.1A CN112526991B (zh) 2020-11-25 2020-11-25 机器人运动方法、装置、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011353145.1A CN112526991B (zh) 2020-11-25 2020-11-25 机器人运动方法、装置、电子设备及存储介质

Publications (2)

Publication Number Publication Date
CN112526991A CN112526991A (zh) 2021-03-19
CN112526991B true CN112526991B (zh) 2022-05-24

Family

ID=74993940

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011353145.1A Active CN112526991B (zh) 2020-11-25 2020-11-25 机器人运动方法、装置、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN112526991B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115328211B (zh) * 2022-10-17 2022-12-27 复亚智能科技(太仓)有限公司 一种无人机局部路径规划方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103455034A (zh) * 2013-09-16 2013-12-18 苏州大学张家港工业技术研究院 一种基于最近距离向量场直方图的避障路径规划方法
CN103576686A (zh) * 2013-11-21 2014-02-12 中国科学技术大学 一种机器人自主导引及避障的方法
CN104898660A (zh) * 2015-03-27 2015-09-09 中国科学技术大学 一种提高机器人路径规划效率的室内地图构建方法
CN106969770A (zh) * 2017-05-31 2017-07-21 安科机器人有限公司 一种机器人及其导航方法、计算机可读存储介质
CN109062227A (zh) * 2018-09-25 2018-12-21 电子科技大学 一种基于动态窗口的多曲线局部路径规划方法
CN111813100A (zh) * 2019-07-04 2020-10-23 中国科学技术大学 一种局部路径规划算法及装置

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN100511326C (zh) * 2005-10-18 2009-07-08 中国科学技术大学 两轮遥控小车倒立摆及其平衡控制方法
US11314262B2 (en) * 2016-08-29 2022-04-26 Trifo, Inc. Autonomous platform guidance systems with task planning and obstacle avoidance
US10429847B2 (en) * 2017-09-22 2019-10-01 Locus Robotics Corp. Dynamic window approach using optimal reciprocal collision avoidance cost-critic
DE102017123491B4 (de) * 2017-10-10 2023-12-28 Deutsche Post Ag Fahrzeug zum autonomen Transportieren eines Objektes
JP2019175390A (ja) * 2018-03-29 2019-10-10 パナソニックＩｐマネジメント株式会社 搭乗管理システム、搭乗管理方法、プログラム、及び移動体

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103455034A (zh) * 2013-09-16 2013-12-18 苏州大学张家港工业技术研究院 一种基于最近距离向量场直方图的避障路径规划方法
CN103576686A (zh) * 2013-11-21 2014-02-12 中国科学技术大学 一种机器人自主导引及避障的方法
CN104898660A (zh) * 2015-03-27 2015-09-09 中国科学技术大学 一种提高机器人路径规划效率的室内地图构建方法
CN106969770A (zh) * 2017-05-31 2017-07-21 安科机器人有限公司 一种机器人及其导航方法、计算机可读存储介质
CN109062227A (zh) * 2018-09-25 2018-12-21 电子科技大学 一种基于动态窗口的多曲线局部路径规划方法
CN111813100A (zh) * 2019-07-04 2020-10-23 中国科学技术大学 一种局部路径规划算法及装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A Thin Electroadhesive Inchworm Climbing Robot Driven by an Electrostatic Film Actuator for Inspection in a Narrow Gap;Hongqiang Wang ，等;《IEEE》;20131231;第1-6页 *
全向移动机器人动态避障方法;张大志，等;《北京航空航天大学学报》;20200713;第1115-1123页 *

Also Published As

Publication number Publication date
CN112526991A (zh) 2021-03-19

Similar Documents

Publication Publication Date Title
CN112379679B (zh) 一种无人车辆局部路径规划方法
CN110018489B (zh) 基于激光雷达的目标追踪方法、装置及控制器和存储介质
US10198008B2 (en) Mobile robot system
CN109001757B (zh) 一种基于2d激光雷达的车位智能检测方法
CN112363513B (zh) 一种基于深度信息的障碍物分类避障控制方法
CN113110521B (zh) 移动机器人路径规划控制方法及其控制装置、存储介质
CN112526999B (zh) 速度规划方法、装置、电子设备和存储介质
KR102547274B1 (ko) 이동 로봇 및 이의 위치 인식 방법
CN113034579B (zh) 一种移动机器人基于激光数据的动态障碍物轨迹预测方法
WO2022001323A1 (zh) 目标车辆的控制方法、装置、电子设备及存储介质
US11719799B2 (en) Method for determining a collision free space
CN116576857A (zh) 一种基于单线激光雷达的多障碍物预测导航避障方法
CN112526991B (zh) 机器人运动方法、装置、电子设备及存储介质
CN114489050A (zh) 沿直线行驶的避障路线控制方法、装置、设备及存储介质
JP2022183692A (ja) 航行監視装置、航行監視方法および航行監視プログラム
CN114509079A (zh) 用于自主驾驶的地面投影的方法和系统
US20220229186A1 (en) Object shape detection apparatus and method
US20220413147A1 (en) Method for detecting ground using lidar sensor and ground detection device performing same
Rosas-Cervantes et al. Mobile robot 3D trajectory estimation on a multilevel surface with multimodal fusion of 2D camera features and a 3D light detection and ranging point cloud
Norouzi et al. Recursive line extraction algorithm from 2d laser scanner applied to navigation a mobile robot
Liang et al. Of-vo: Reliable navigation among pedestrians using commodity sensors
CN118226860B (zh) 机器人运动控制方法、装置、机器人和存储介质
US12020464B2 (en) Method of determining an orientation of an object and a method and apparatus for tracking an object
Park et al. Sensor fusion-based exploration in home environments using information, driving and localization gains
KR20240036903A (ko) 객체 검출 방법 및 시스템

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant