CN108919794B - 一种双轮差速型移动机器人的全局路径规划方法及其装置 - Google Patents

一种双轮差速型移动机器人的全局路径规划方法及其装置 Download PDF

Info

Publication number
CN108919794B
CN108919794B CN201810555370.XA CN201810555370A CN108919794B CN 108919794 B CN108919794 B CN 108919794B CN 201810555370 A CN201810555370 A CN 201810555370A CN 108919794 B CN108919794 B CN 108919794B
Authority
CN
China
Prior art keywords
point state
optimal
state
motion
mobile robot
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
CN201810555370.XA
Other languages
English (en)
Other versions
CN108919794A (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.)
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Original Assignee
Guangzhou Shiyuan Electronics Thecnology Co Ltd
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 Guangzhou Shiyuan Electronics Thecnology Co Ltd filed Critical Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority to CN201810555370.XA priority Critical patent/CN108919794B/zh
Publication of CN108919794A publication Critical patent/CN108919794A/zh
Application granted granted Critical
Publication of CN108919794B publication Critical patent/CN108919794B/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
    • G05D1/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria

Landscapes

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

Abstract

本发明涉及一种双轮差速型移动机器人的全局路径规划方法及其装置，其中方法包括：获取双轮差速型移动机器人的起点状态和终点状态；基于运动基元，获取起点状态到终点状态之间的各个中间点对应的最优中间点状态；获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到终点状态的最优运动基元；根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径。本发明实施例提出的全局路径规划方法及其装置不会陷入局部极值，每次规划的全局路径一致且是最优路径，规划的路径比上述算法更平滑，完成一次规划的时间短。

Description

一种双轮差速型移动机器人的全局路径规划方法及其装置
技术领域
本发明实施例属于移动机器人技术领域，具体涉及一种双轮差速型移动机器人的全局路径规划方法及其装置。
背景技术
路径规划是指移动机器人根据环境信息搜索一条从起点到目标点的无碰路径，该路径的优劣直接影响移动机器人搜索的时间复杂度和空间复杂度。移动机器人常用的路径规划算法包括：A*算法、Dijk算法、人工势场法、RRT(快速扩展随机树，rapidly-exploringrandom tree)算法等。这些算法存在各自的局限性，例如，人工势场法容易陷入局部极值，导致路径规划失败；RRT算法每次规划的路径都是随机的，并且规划的路径一般不是最优路径；A*算法和Dijk算法也有明显的局限性，例如，没有考虑移动机器人本身的运动学模型限制，导致规划出的路径不平滑，而且在面积较大的环境中，由于搜索的空间过大，导致路径规划完成时间过长。
发明内容
为了解决上述路径规划算法存在的容易陷入局部极值、规划的全局路径随机且不是最优或者规划出的路径不平滑、完成时间过长等技术问题，本发明实施例提出了一种双轮差速型移动机器人的全局路径规划方法及其装置。
一种双轮差速型移动机器人全局路径规划方法，该方法包括以下步骤：
获取双轮差速型移动机器人的起点状态和终点状态；
基于根据双轮差速型移动机器人的运动学模型构造的运动基元，获取起点状态到终点状态之间的各个中间点对应的最优中间点状态，记录各个最优中间点状态对应的运动基元；
获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到终点状态的最优运动基元；
根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径。
进一步地，至少基于双轮差速型移动机器人的角速度和位姿，构造双轮差速型移动机器人的运动学模型。
进一步地，所述根据双轮差速型移动机器人的运动学模型构造运动基元时，构造运动基元的输入用(V,W)表示，V表示移动机器人的线速度，W表示双轮差速型移动机器人的角速度；根据双轮差速型移动机器人的角速度W的值，构造相应的运动基元。
进一步地，从起点状态到终点状态之间的各个相邻中间点状态之间的运动时间为固定运动时间，起点状态到与起点状态相邻的中间点状态之间的运动时间为所述固定运动时间，且所述固定运动时间等于控制采样时间Δt。
进一步地，采用启发式搜索算法获取起点状态到终点状态之间的各个中间点对应的最优中间点状态。
进一步地，所述获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到达终点状态的最优运动基元，包括：将所述各个最优中间点状态中距离终点状态最近的最优中间点状态作为序号最大的最优中间点状态，通过M个不同的输入U，分别构造序号最大的最优中间点状态到达终点状态的M种运动基元，寻找不会发生碰撞且代价函数最小的运动基元，作为序号最大的最优中间点状态到达终点状态的最优运动基元，记录序号最大的最优中间点状态到达终点状态的最优运动基元。
进一步地，根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径，包括：取出各个最优中间点状态，通过序号最大的最优中间点状态到达终点状态的最优运动基元以及每个最优中间点状态记录的运动基元，倒序连接终点状态、各个最优中间点以及起点状态，所得轨迹即为最终的双轮差速型移动机器人的全局路径。
一种双轮差速型移动机器人全局路径规划装置，该装置包括：
初始状态获取模块，用于获取双轮差速型移动机器人的起点状态和终点状态；
最优中间点状态获取模块，用于基于根据双轮差速型移动机器人的运动学模型构造的运动基元，获取起点状态到终点状态之间的各个中间点对应的最优中间点状态，记录各个最优中间点状态对应的运动基元；
最优运动基元获取模块，用于获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到终点状态的最优运动基元；以及，
全局路径构建模块，用于根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径。
进一步地，至少基于双轮差速型移动机器人的角速度和位姿，构造双轮差速型移动机器人的运动学模型。
进一步地，所述根据双轮差速型移动机器人的运动学模型构造运动基元时，构造运动基元的输入用(V,W)表示，V表示移动机器人的线速度，W表示双轮差速型移动机器人的角速度；根据双轮差速型移动机器人的角速度W的值，构造相应的运动基元。
进一步地，从起点状态到终点状态之间的各个相邻中间点状态之间的运动时间为固定运动时间，起点状态到与起点状态相邻的中间点状态之间的运动时间为所述固定运动时间，且所述固定运动时间等于控制采样时间Δt。
进一步地，采用启发式搜索算法获取起点状态到终点状态之间的各个中间点对应的最优中间点状态。
进一步地，所述最优运动基元获取模块获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到达终点状态的最优运动基元，包括：将所述各个最优中间点状态中距离终点状态最近的最优中间点状态作为序号最大的最优中间点状态，通过M个不同的输入U，分别构造序号最大的最优中间点状态到达终点状态的M种运动基元，寻找不会发生碰撞且代价函数最小的运动基元，作为序号最大的最优中间点状态到达终点状态的最优运动基元，记录序号最大的最优中间点状态到达终点状态的最优运动基元。
进一步地，所述全局路径构建模块根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径，包括：取出各个最优中间点状态，通过序号最大的最优中间点状态到达终点状态的最优运动基元以及每个最优中间点状态记录的运动基元，倒序连接终点状态、各个最优中间点以及起点状态，所得轨迹即为最终的双轮差速型移动机器人的全局路径。
本发明实施例的有益效果：本发明实施例提出的双轮差速型移动机器人的全局路径规划方法及其装置，考虑了移动机器人本身的运动学模型限制，并基于运动基元构造搜索空间，将路径规划问题转化为图搜索问题，并且采用启发式搜索算法求解图搜索问题，因此与传统A*算法、Dijk算法、人工势场法和RRT算法等相比，本发明实施例提出的双轮差速型移动机器人的全局路径规划方法及其装置具备如下优势：不会陷入局部极值，每次规划的全局路径一致且是最优路径，规划的路径比上述算法更平滑，完成一次规划的时间比A*算法和Dijk算法更短。
附图说明
图1是本发明实施例提出的双轮差速型移动机器人全局路径规划方法的流程图；
图2是本发明实施例提出的采用启发式搜索算法获取起点状态到终点状态之间的各个中间点对应的最优中间点状态的方法的流程图；
图3是本发明实施例提出的以起点状态S0为例的运动基元和搜索状态的示意图；
图4是本发明实施例提出的双轮差速型移动机器人全局路径规划装置的结构示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。但本领域技术人员知晓，本发明并不局限于附图和以下实施例。
本发明实施例提出了一种基于运动基元的双轮差速型移动机器人全局路径规划方法，如图1所示，该方法包括以下步骤：
步骤S11、获取双轮差速型移动机器人的起点状态和终点状态；
步骤S12、基于根据双轮差速型移动机器人的运动学模型构造的运动基元，获取起点状态到终点状态之间的各个中间点对应的最优中间点状态，记录各个最优中间点状态对应的运动基元；
步骤S13、获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到终点状态的最优运动基元；
步骤S14、根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径。
其中，在步骤S11中，本发明实施例双轮差速型移动机器人的状态用位姿(x,y,θ)表示，x和y表示双轮差速型移动机器人的位置，θ表示双轮差速型移动机器人的姿式。
起点状态S0表示为(x0,y0,θ0)，终点状态Send表示为(xend,yend,θend)。
在本发明实施例中，至少基于双轮差速型移动机器人的角速度和位姿，构造双轮差速型移动机器人的运动学模型。
优选地，在本发明实施例中，双轮差速型移动机器人的运动学模型为：
考虑到双轮差速型移动机器人的运动轨迹只能为直线和圆弧，当移动机器人的角速度W小于预设值时，则认为双轮差速型移动机器人的运动轨迹为直线，双轮差速型移动机器人的直线运动学模型为公式1：
Figure BDA0001682202500000051
式中，x和y表示双轮差速型移动机器人的位置，θ表示双轮差速型移动机器人的姿式，V表示移动机器人的线速度，Δt表示控制采样时间，n取非负整数。
优选地，预设值可以设置为0.001。
当移动机器人的角速度大于等于预设值时，则认为双轮差速型移动机器人的运动轨迹为圆弧，双轮差速型移动机器人的圆弧运动学模型为公式2：
Figure BDA0001682202500000052
式中，x和y表示双轮差速型移动机器人的位置，θ表示双轮差速型移动机器人的姿式，V表示移动机器人的线速度，W表示移动机器人的角速度，Δt表示控制采样时间，n取非负整数。
在步骤S12中，所述根据双轮差速型移动机器人的运动学模型构造运动基元的方法为：
构造运动基元的输入U用(V,W)表示，V表示移动机器人的线速度，W表示双轮差速型移动机器人的角速度。根据双轮差速型移动机器人的角速度W的值，选用上述公式1或公式2，构造相应的运动基元P，输入U的V和W的不同取值，可对应不同的运动基元。可参考图3所示，通过10组不同的(V,W)组合，可以构造出10个不同的运动基元。
在步骤S12中，从起点状态到终点状态之间的各个相邻中间点状态之间的运动时间为固定运动时间，起点状态到与起点状态相邻的中间点状态之间的运动时间也为固定运动时间，且所述固定运动时间等于控制采样时间Δt。
优选地，控制采样时间Δt可以根据计算精度、从起点到终点的最大搜索步数限制等要求进行设置。
进一步地，在步骤S12中，从起点状态出发，基于根据双轮差速型移动机器人的运动学模型构造的运动基元，构建起点状态的搜索状态，从构建的起点状态的搜索状态中，获取第一个中间点对应的第一个最优中间点状态，记录第一个最优中间点状态对应的运动基元；然后从第一个最优中间点状态出发，基于根据双轮差速型移动机器人的运动学模型构造的运动基元，构建第一个最优中间点状态的搜索状态，从构建的第一个最优中间点状态的搜索状态中，获取第二个中间点对应的第二个最优中间点状态，记录第二个最优中间点状态对应的运动基元；以此类推，获取各个中间点对应的最优中间点状态，记录各个最优中间点状态对应的运动基元，直到序号最大的最优中间点状态到达终点状态的距离在预设范围内。
更进一步地，在步骤S12中，所述获取起点状态到终点状态之间的各个中间点对应的最优中间点状态，采用启发式搜索算法，求解图搜索问题。
每个运动基元P的代价函数设为j(U)，
j(U)＝V2+W2 公式3，
则运动基元连接的两个状态，当前点状态S和当前点状态的下一个中间点状态的任一搜索状态S’间的代价函数g(s,s’)＝j(U)。
每个状态s的启发函数设为h(s)，启发函数h(s)采用公式4进行计算：
Figure BDA0001682202500000061
式中，(xS,yS,θS)表示状态s的位姿，(xend,yend,θend)表示双轮差速型移动机器人在终点状态的位姿。
当前点状态S的下一个中间点状态的任一搜索状态S’的筛选函数设为f(s’)，筛选函数采用公式5进行计算：
f(s’)＝g(s’)+h(s’)＝g(s)+g(s,s’)+h(s’) 公式5，
h(s’)为当前点状态S的下一个中间点状态的任一搜索状态S’的启发函数，g(s’)为当前点状态S的下一个中间点状态的任一搜索状态S’的代价函数，g(s)为当前点状态S的代价函数，g(s,s’)为运动基元连接的当前点状态S和当前点状态的下一个中间点状态的任一搜索状态S’间的代价函数。
采用启发式搜索算法获取起点状态到终点状态之间的各个中间点对应的最优中间点状态的方法如图2所示，包括以下步骤：
步骤S1200、构造OPEN列表和CLOSED列表，OPEN列表和CLOSED列表都设为空；
步骤S1201、起点状态用S0表示，终点状态用Send表示，将起点状态S0和终点状态Send加入CLOSED列表，起点状态S0的代价函数g(s0)＝0，则f(S0)＝h(S0)；将起点状态S0作为当前点状态S；
步骤S1202、判断当前点状态S是否为终点状态，如果是，则进入步骤S14；如果否，则进入步骤S1203；
步骤S1203、从双轮差速型移动机器人的当前点状态出发，通过M个不同的输入，分别按照上述公式1或公式2构造当前点状态S的M种运动基元，将构造的M种运动基元在固定运动时间处的所有点状态，作为下一个中间点状态的M个搜索状态，进入步骤S1204；
步骤S1204、从所述下一个中间点状态的M个搜索状态中提取一个搜索状态S’；
步骤S1205、判断提取的该搜索状态对应的运动基元是否发生碰撞，如果是，返回步骤S1204；如果否，判断该搜索状态是否被访问过，如果被访问过，进入步骤S1206；如果未被访问过，则f(s’)＝g(s’)＝无穷大，进入步骤S1206；
步骤S1206、判断g(s’)>g(s)+g(s,s’)是否成立，如果是，则进入步骤S1207；如果否，则进入步骤S1208；
步骤S1207、计算该搜索状态的代价函数g(s’)，g(s’)＝g(s)+g(s,s’)，利用公式4计算该搜索状态的启发函数h(s’)，利用公式5计算该搜索状态的筛选函数f(s’)，进入步骤S1208；
步骤S1208、将该搜索状态加入OPEN列表，返回步骤S1204，直到所述下一个中间点状态的M个搜索状态中的所有搜索状态提取完毕，进入步骤S1209；
步骤S1209、从加入OPEN列表中的所述下一个中间点状态的M个搜索状态中提取的未发生碰撞的搜索状态中，选取筛选函数值最小的搜索状态，作为下一个中间点对应的最优中间点状态，记录下一个中间点对应的最优中间点状态对应的运动基元，将下一个中间点对应的最优中间点状态加入CLOSED列表中，并将下一个中间点对应的最优中间点状态从OPEN列表移除；
优选地，M＝10。图3给出了以起点状态S0为例的运动基元和搜索状态的示意图，通过10个不同的输入，分别按照上述公式1或公式2构造起点状态S0的10种运动基元，将构造的起点状态S0的10种运动基元在固定运动时间处的所有点状态，作为起点状态S0的下一个中间点状态的10个搜索状态，即第一个中间点状态S1的10个搜索状态。以此类推，可以构造其余中间点状态的运动基元和搜索状态；
步骤S1210、将下一个中间点对应的最优中间点状态作为当前点状态，返回步骤S1202，直到所述下一个中间点对应的最优中间点状态到达终点状态的距离在预设范围内。
其中，在步骤S13中，所述获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到达终点状态的最优运动基元，包括：将所述各个最优中间点状态中距离终点状态最近的最优中间点状态作为序号最大的最优中间点状态，通过M个不同的输入U，分别按照上述公式1或公式2构造序号最大的最优中间点状态到达终点状态的M种运动基元，根据公式3寻找不会发生碰撞且代价函数最小的运动基元，作为序号最大的最优中间点状态到达终点状态的最优运动基元，记录序号最大的最优中间点状态到达终点状态的最优运动基元。
步骤S14中，根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径，包括：从CLOSED列表中依次取出各个最优中间点状态，通过序号最大的最优中间点状态到达终点状态的最优运动基元以及每个最优中间点状态记录的运动基元，倒序连接终点状态、各个最优中间点以及起点状态，所得轨迹即为最终的双轮差速型移动机器人的全局路径。
本发明实施例还提出了一种基于运动基元的双轮差速型移动机器人全局路径规划装置，如图4所示，该装置包括：初始状态获取模块、最优中间点状态获取模块、最优运动基元获取模块和全局路径构建模块；
其中，所述初始状态获取模块获取双轮差速型移动机器人的起点状态和终点状态；
所述最优中间点状态获取模块基于根据双轮差速型移动机器人的运动学模型构造的运动基元，获取起点状态到终点状态之间的各个中间点对应的最优中间点状态，记录各个最优中间点状态对应的运动基元；
所述最优运动基元获取模块获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到终点状态的最优运动基元；
所述全局路径构建模块根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径。
其中，在本发明实施例中，双轮差速型移动机器人的状态用位姿(x,y,θ)表示，x和y表示双轮差速型移动机器人的位置，θ表示双轮差速型移动机器人的姿式。
起点状态S0表示为(x0,y0,θ0)，终点状态Send表示为(xend,yend,θend)。
在本发明实施例中，至少基于双轮差速型移动机器人的角速度和位姿，构造双轮差速型移动机器人的运动学模型。
优选地，所述双轮差速型移动机器人的运动学模型为：
考虑到双轮差速型移动机器人的运动轨迹只能为直线和圆弧，当移动机器人的角速度W小于预设值时，则认为双轮差速型移动机器人的运动轨迹为直线，双轮差速型移动机器人的直线运动学模型为公式1：
Figure BDA0001682202500000091
式中，x和y表示双轮差速型移动机器人的位置，θ表示双轮差速型移动机器人的姿式，V表示移动机器人的线速度，Δt表示控制采样时间，n取非负整数。
优选地，预设值可以设置为0.001。
当移动机器人的角速度大于等于预设值时，则认为双轮差速型移动机器人的运动轨迹为圆弧，双轮差速型移动机器人的圆弧运动学模型为公式2：
Figure BDA0001682202500000092
式中，x和y表示双轮差速型移动机器人的位置，θ表示双轮差速型移动机器人的姿式，V表示移动机器人的线速度，W表示移动机器人的角速度，Δt表示控制采样时间，n取非负整数。
所述根据双轮差速型移动机器人的运动学模型构造运动基元的方法为：
构造运动基元的输入U用(V,W)表示，V表示移动机器人的线速度，W表示双轮差速型移动机器人的角速度。根据双轮差速型移动机器人的角速度W的值，选用上述公式1或公式2，构造相应的运动基元P，输入U的V和W的不同取值，可对应不同的运动基元。可参考图3所示，通过10组不同的(V,W)组合，可以构造出10个不同的运动基元。
在本发明实施例中，从起点状态到终点状态之间的各个相邻中间点状态之间的运动时间为固定运动时间，起点状态到与起点状态相邻的中间点状态之间的运动时间也为固定运动时间，且所述固定运动时间等于控制采样时间Δt。
优选地，控制采样时间Δt可以根据计算精度、从起点到终点的最大搜索步数限制等要求进行设置。
进一步地，在本发明实施例中，所述最优中间点状态获取模块从起点状态出发，基于根据双轮差速型移动机器人的运动学模型构造的运动基元，构建起点状态的搜索状态，从构建的起点状态的搜索状态中，获取第一个中间点对应的第一个最优中间点状态，记录第一个最优中间点状态对应的运动基元；然后从第一个最优中间点状态出发，基于根据双轮差速型移动机器人的运动学模型构造的运动基元，构建第一个最优中间点状态的搜索状态，从构建的第一个最优中间点状态的搜索状态中，获取第二个中间点对应的第二个最优中间点状态，记录第二个最优中间点状态对应的运动基元；以此类推，获取各个中间点对应的最优中间点状态，记录各个最优中间点状态对应的运动基元，直到序号最大的最优中间点状态到达终点状态的距离在预设范围内。
更进一步地，所述最优中间点状态获取模块获取起点状态到终点状态之间的各个中间点对应的最优中间点状态，可以采用启发式搜索算法，求解图搜索问题。
每个运动基元P的代价函数设为j(U)，
j(U)＝V2+W2 公式3，
则运动基元连接的两个状态，当前点状态S和当前点状态的下一个中间点状态的任一搜索状态S’间的代价函数g(s,s’)＝j(U)。
每个状态s的启发函数设为h(s)，启发函数h(s)采用公式4进行计算：
Figure BDA0001682202500000101
式中，(xS,yS,θS)表示状态s的位姿，(xend,yend,θend)表示双轮差速型移动机器人在终点状态的位姿。
当前点状态S的下一个中间点状态的任一搜索状态S’的筛选函数设为f(s’)，筛选函数采用公式5进行计算：
f(s’)＝g(s’)+h(s’)＝g(s)+g(s,s’)+h(s’) 公式5，
h(s’)为当前点状态S的下一个中间点状态的任一搜索状态S’的启发函数，g(s’)为当前点状态S的下一个中间点状态的任一搜索状态S’的代价函数，g(s)为当前点状态S的代价函数，g(s,s’)为运动基元连接的当前点状态S和当前点状态的下一个中间点状态的任一搜索状态S’间的代价函数。
采用启发式搜索算法获取起点状态到终点状态之间的各个中间点对应的最优中间点状态的方法如图2所示，包括以下步骤：
步骤S1200、构造OPEN列表和CLOSED列表，OPEN列表和CLOSED列表都设为空；
步骤S1201、起点状态用S0表示，终点状态用Send表示，将起点状态S0和终点状态Send加入CLOSED列表，起点状态S0的代价函数g(s0)＝0，则f(S0)＝h(S0)；将起点状态S0作为当前点状态S；
步骤S1202、判断当前点状态S是否为终点状态，如果是，则通知所述全局路径构建模块执行；如果否，则进入步骤S1203；
步骤S1203、从双轮差速型移动机器人的当前点状态出发，通过M个不同的输入，分别按照上述公式1或公式2构造当前点状态S的M种运动基元，将构造的M种运动基元在固定运动时间处的所有点状态，作为下一个中间点状态的M个搜索状态，进入步骤S1204；
步骤S1204、从所述下一个中间点状态的M个搜索状态中提取一个搜索状态S’；
步骤S1205、判断提取的该搜索状态对应的运动基元是否发生碰撞，如果是，返回步骤S1204；如果否，判断该搜索状态是否被访问过，如果被访问过，进入步骤S1206；如果未被访问过，则f(s’)＝g(s’)＝无穷大，进入步骤S1206；
步骤S1206、判断g(s’)>g(s)+g(s,s’)是否成立，如果是，则进入步骤S1207；如果否，则进入步骤S1208；
步骤S1207、计算该搜索状态的代价函数g(s’)，g(s’)＝g(s)+g(s,s’)，利用公式4计算该搜索状态的启发函数h(s’)，利用公式5计算该搜索状态的筛选函数f(s’)，进入步骤S1208；
步骤S1208、将该搜索状态加入OPEN列表，返回步骤S1204，直到所述下一个中间点状态的M个搜索状态中的所有搜索状态提取完毕，进入步骤S1209；
步骤S1209、从加入OPEN列表中的所述下一个中间点状态的M个搜索状态中提取的未发生碰撞的搜索状态中，选取筛选函数值最小的搜索状态，作为下一个中间点对应的最优中间点状态，记录下一个中间点对应的最优中间点状态对应的运动基元，将下一个中间点对应的最优中间点状态加入CLOSED列表中，并将下一个中间点对应的最优中间点状态从OPEN列表移除；
优选地，M＝10。图3给出了以起点状态S0为例的运动基元和搜索状态的示意图，通过10个不同的输入，分别按照上述公式1或公式2构造起点状态S0的10种运动基元，将构造的起点状态S0的10种运动基元在固定运动时间处的所有点状态，作为起点状态S0的下一个中间点状态的10个搜索状态，即第一个中间点状态S1的10个搜索状态。以此类推，可以构造其余中间点状态的运动基元和搜索状态；
步骤S1210、将下一个中间点对应的最优中间点状态作为当前点状态，返回步骤S1202，直到所述下一个中间点对应的最优中间点状态到达终点状态的距离在预设范围内。
其中，所述最优运动基元获取模块获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到达终点状态的最优运动基元，包括：将所述各个最优中间点状态中距离终点状态最近的最优中间点状态作为序号最大的最优中间点状态，通过M个不同的输入U，分别按照上述公式1或公式2构造序号最大的最优中间点状态到达终点状态的M种运动基元，根据公式3寻找不会发生碰撞且代价函数最小的运动基元，作为序号最大的最优中间点状态到达终点状态的最优运动基元，记录序号最大的最优中间点状态到达终点状态的最优运动基元。
所述全局路径构建模块根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径，包括：从CLOSED列表中依次取出各个最优中间点状态，通过序号最大的最优中间点状态到达终点状态的最优运动基元以及每个最优中间点状态记录的运动基元，倒序连接终点状态、各个最优中间点以及起点状态，所得轨迹即为最终的双轮差速型移动机器人的全局路径。
本发明实施例还提出一种计算机可读存储介质，存储有执行前述方法的计算机程序。
本发明实施例还提出一种计算机设备，包括处理器和操作上与所述处理器连接的上述计算机可读存储介质，所述处理器运行执行计算机可读介质中的计算机程序。
本领域技术人员可以理解，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或它们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
以上，对本发明的实施方式进行了说明。但是，本发明不限定于上述实施方式。凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (14)

1.一种双轮差速型移动机器人全局路径规划方法，其特征在于，该方法包括以下步骤：
获取双轮差速型移动机器人的起点状态和终点状态；
基于根据双轮差速型移动机器人的运动学模型构造的运动基元，获取起点状态到终点状态之间的各个中间点对应的最优中间点状态，记录各个最优中间点状态对应的运动基元；
获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到终点状态的最优运动基元；
根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径；
其中，至少基于双轮差速型移动机器人的角速度和位姿，构造双轮差速型移动机器人的运动学模型。
2.根据权利要求1所述的方法，其特征在于，根据双轮差速型移动机器人的运动轨迹为直线和圆弧，分别构造双轮差速型移动机器人的运动学模型。
3.根据权利要求1或2所述的方法，其特征在于，所述根据双轮差速型移动机器人的运动学模型构造运动基元时，构造运动基元的输入用（V,W）表示，
Figure 199286DEST_PATH_IMAGE001
表示移动机器人的线速度，
Figure 989388DEST_PATH_IMAGE002
表示双轮差速型移动机器人的角速度；根据移动机器人的线速度
Figure 150242DEST_PATH_IMAGE001
和双轮差速型移动机器人的角速度
Figure 418412DEST_PATH_IMAGE002
的值，构造相应的运动基元。
4.根据权利要求3所述的方法，其特征在于，从起点状态到终点状态之间的各个相邻中间点状态之间的运动时间为固定运动时间，起点状态到与起点状态相邻的中间点状态之间的运动时间为所述固定运动时间，且所述固定运动时间等于控制采样时间
Figure 613901DEST_PATH_IMAGE003
。
5.根据权利要求4所述的方法，其特征在于，采用启发式搜索算法获取起点状态到终点状态之间的各个中间点对应的最优中间点状态。
6.根据权利要求5所述的方法，其特征在于，所述获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到达终点状态的最优运动基元，包括：将所述各个最优中间点状态中距离终点状态最近的最优中间点状态作为序号最大的最优中间点状态，通过M个不同的输入U，分别构造序号最大的最优中间点状态到达终点状态的M种运动基元，寻找不会发生碰撞且代价函数最小的运动基元，作为序号最大的最优中间点状态到达终点状态的最优运动基元，记录序号最大的最优中间点状态到达终点状态的最优运动基元。
7.根据权利要求6所述的方法，其特征在于，根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径，包括：取出各个最优中间点状态，通过序号最大的最优中间点状态到达终点状态的最优运动基元以及每个最优中间点状态记录的运动基元，倒序连接终点状态、各个最优中间点以及起点状态，所得轨迹即为最终的双轮差速型移动机器人的全局路径。
8.一种双轮差速型移动机器人全局路径规划装置，其特征在于，该装置包括：
初始状态获取模块，用于获取双轮差速型移动机器人的起点状态和终点状态；
最优中间点状态获取模块，用于基于根据双轮差速型移动机器人的运动学模型构造的运动基元，获取起点状态到终点状态之间的各个中间点对应的最优中间点状态，记录各个最优中间点状态对应的运动基元；
最优运动基元获取模块，用于获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到终点状态的最优运动基元；以及，
全局路径构建模块，用于根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径；
其中，至少基于双轮差速型移动机器人的角速度和位姿，构造双轮差速型移动机器人的运动学模型。
9.根据权利要求8所述的装置，其特征在于，根据双轮差速型移动机器人的运动轨迹为直线和圆弧，分别构造双轮差速型移动机器人的运动学模型。
10.根据权利要求8或9所述的装置，其特征在于，所述根据双轮差速型移动机器人的运动学模型构造运动基元时，构造运动基元的输入用（V,W）表示，
Figure 453200DEST_PATH_IMAGE001
表示移动机器人的线速度，
Figure 225984DEST_PATH_IMAGE002
表示双轮差速型移动机器人的角速度；根据移动机器人的线速度
Figure 173211DEST_PATH_IMAGE001
和双轮差速型移动机器人的角速度
Figure 347840DEST_PATH_IMAGE002
的值，构造相应的运动基元。
11.根据权利要求10所述的装置，其特征在于，从起点状态到终点状态之间的各个相邻中间点状态之间的运动时间为固定运动时间，起点状态到与起点状态相邻的中间点状态之间的运动时间为所述固定运动时间，且所述固定运动时间等于控制采样时间
Figure 761635DEST_PATH_IMAGE003
。
12.根据权利要求11所述的装置，其特征在于，采用启发式搜索算法获取起点状态到终点状态之间的各个中间点对应的最优中间点状态。
13.根据权利要求12所述的装置，其特征在于，所述最优运动基元获取模块获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到达终点状态的最优运动基元，包括：将所述各个最优中间点状态中距离终点状态最近的最优中间点状态作为序号最大的最优中间点状态，通过M个不同的输入U，分别构造序号最大的最优中间点状态到达终点状态的M种运动基元，寻找不会发生碰撞且代价函数最小的运动基元，作为序号最大的最优中间点状态到达终点状态的最优运动基元，记录序号最大的最优中间点状态到达终点状态的最优运动基元。
14.根据权利要求13所述的装置，其特征在于，所述全局路径构建模块根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径，包括：取出各个最优中间点状态，通过序号最大的最优中间点状态到达终点状态的最优运动基元以及每个最优中间点状态记录的运动基元，倒序连接终点状态、各个最优中间点以及起点状态，所得轨迹即为最终的双轮差速型移动机器人的全局路径。
CN201810555370.XA 2018-06-01 2018-06-01 一种双轮差速型移动机器人的全局路径规划方法及其装置 Active CN108919794B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810555370.XA CN108919794B (zh) 2018-06-01 2018-06-01 一种双轮差速型移动机器人的全局路径规划方法及其装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810555370.XA CN108919794B (zh) 2018-06-01 2018-06-01 一种双轮差速型移动机器人的全局路径规划方法及其装置

Publications (2)

Publication Number Publication Date
CN108919794A CN108919794A (zh) 2018-11-30
CN108919794B true CN108919794B (zh) 2021-08-17

Family

ID=64419964

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810555370.XA Active CN108919794B (zh) 2018-06-01 2018-06-01 一种双轮差速型移动机器人的全局路径规划方法及其装置

Country Status (1)

Country Link
CN (1) CN108919794B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109445444B (zh) * 2018-12-25 2021-05-11 同济大学 一种障碍物集中环境下的机器人路径生成方法
CN109883430A (zh) * 2019-02-13 2019-06-14 平安科技（深圳）有限公司 导航路线推荐方法、装置、存储介质及计算机设备
CN110727271A (zh) * 2019-10-30 2020-01-24 北京科技大学 一种机器人运动基元确定方法及装置
CN114003030B (zh) * 2021-10-09 2023-08-08 北京科技大学 考虑质心变化的两轮差动移动机器人路径跟踪控制方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5341459A (en) * 1991-05-09 1994-08-23 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Generalized compliant motion primitive
CN103646035B (zh) * 2013-11-14 2017-07-07 北京锐安科技有限公司 一种基于启发式方法的信息搜索方法
CN106444738B (zh) * 2016-05-24 2019-04-09 武汉科技大学 基于动态运动基元学习模型的移动机器人路径规划方法
CN106767860B (zh) * 2016-11-21 2019-12-03 江苏大学 一种基于启发式搜索算法来缩短智能汽车路径规划搜索时间的方法
CN106989748A (zh) * 2017-05-16 2017-07-28 南京农业大学 一种基于云模型的农业移动机器人人机合作路径规划方法
CN107214701B (zh) * 2017-06-12 2019-10-18 南京理工大学 一种基于运动基元库的带电作业机械臂自主避障路径规划方法

Also Published As

Publication number Publication date
CN108919794A (zh) 2018-11-30

Similar Documents

Publication Publication Date Title
CN108919794B (zh) 一种双轮差速型移动机器人的全局路径规划方法及其装置
CN108256574B (zh) 机器人定位方法及装置
Poirson et al. Fast single shot detection and pose estimation
US9993923B2 (en) Trajectory generation device, moving object, trajectory generation method
CN109163722B (zh) 一种仿人机器人路径规划方法及装置
Park et al. Classification based grasp detection using spatial transformer network
US11113574B1 (en) Methods for performing self-supervised learning of deep-learning based detection network by using deep Q-network and devices using the same
Murthy et al. Shape priors for real-time monocular object localization in dynamic environments
CN111694917A (zh) 一种车辆异常轨迹检测、模型训练方法及装置
Denny et al. Lazy toggle PRM: A single-query approach to motion planning
CN111723983B (zh) 一种未知环境下无人机的时间参数化航路规划方法与系统
CN111015656A (zh) 一种机器人主动避障的控制方法、装置及存储介质
CN109341698B (zh) 一种移动机器人的路径选择方法及装置
Maddern et al. Towards persistent localization and mapping with a continuous appearance-based topology
Martinez-Carranza et al. Enhancing 6D visual relocalisation with depth cameras
Garcia-Fidalgo et al. Probabilistic appearance-based mapping and localization using visual features
CN112097772B (zh) 机器人及其地图构建方法和装置
CN109003329A (zh) 一种目标货物堆监控设备及存储介质
CN110749325B (zh) 航迹规划方法和装置
CN114577217B (zh) 基于冯洛诺伊图的路径规划方法、装置、设备及存储介质
CN113723180B (zh) 构建服务机器人主动物品检测模型数据集的方法和系统
CN112634395B (zh) 一种基于slam的地图构建方法及装置
Hassouna et al. Robust robotic path planning using level sets
Teshnizi et al. Planning motions for a planar robot attached to a stiff tether
Saleem et al. Obstacle-avoidance algorithm using deep learning based on rgbd images and robot orientation

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