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

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

Info

Publication number
CN108919794A
CN108919794A CN201810555370.XA CN201810555370A CN108919794A CN 108919794 A CN108919794 A CN 108919794A CN 201810555370 A CN201810555370 A CN 201810555370A CN 108919794 A CN108919794 A CN 108919794A
Authority
CN
China
Prior art keywords
state
optimal
dotted state
mobile robot
dotted
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.)
Granted
Application number
CN201810555370.XA
Other languages
English (en)
Other versions
CN108919794B (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

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：
式中，x和y表示双轮差速型移动机器人的位置，θ表示双轮差速型移动机器人的姿式，V表示移动机器人的线速度，Δt表示控制采样时间，n取非负整数。
优选地，预设值可以设置为0.001。
当移动机器人的角速度大于等于预设值时，则认为双轮差速型移动机器人的运动轨迹为圆弧，双轮差速型移动机器人的圆弧运动学模型为公式2：
式中，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进行计算：
式中，(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：
式中，x和y表示双轮差速型移动机器人的位置，θ表示双轮差速型移动机器人的姿式，V表示移动机器人的线速度，Δt表示控制采样时间，n取非负整数。
优选地，预设值可以设置为0.001。
当移动机器人的角速度大于等于预设值时，则认为双轮差速型移动机器人的运动轨迹为圆弧，双轮差速型移动机器人的圆弧运动学模型为公式2：
式中，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进行计算：
式中，(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)表示，V表示移动机器人的线速度，W表示双轮差速型移动机器人的角速度；根据双轮差速型移动机器人的角速度W的值，构造相应的运动基元。
4.根据权利要求3所述的方法，其特征在于，从起点状态到终点状态之间的各个相邻中间点状态之间的运动时间为固定运动时间，起点状态到与起点状态相邻的中间点状态之间的运动时间为所述固定运动时间，且所述固定运动时间等于控制采样时间Δt。
5.根据权利要求4所述的方法，其特征在于，采用启发式搜索算法获取起点状态到终点状态之间的各个中间点对应的最优中间点状态。
6.根据权利要求5所述的方法，其特征在于，所述获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到达终点状态的最优运动基元，包括：将所述各个最优中间点状态中距离终点状态最近的最优中间点状态作为序号最大的最优中间点状态，通过M个不同的输入U，分别构造序号最大的最优中间点状态到达终点状态的M种运动基元，寻找不会发生碰撞且代价函数最小的运动基元，作为序号最大的最优中间点状态到达终点状态的最优运动基元，记录序号最大的最优中间点状态到达终点状态的最优运动基元。
7.根据权利要求6所述的方法，其特征在于，根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径，包括：取出各个最优中间点状态，通过序号最大的最优中间点状态到达终点状态的最优运动基元以及每个最优中间点状态记录的运动基元，倒序连接终点状态、各个最优中间点以及起点状态，所得轨迹即为最终的双轮差速型移动机器人的全局路径。
8.一种双轮差速型移动机器人全局路径规划装置，其特征在于，该装置包括：
初始状态获取模块，用于获取双轮差速型移动机器人的起点状态和终点状态；
最优中间点状态获取模块，用于基于根据双轮差速型移动机器人的运动学模型构造的运动基元，获取起点状态到终点状态之间的各个中间点对应的最优中间点状态，记录各个最优中间点状态对应的运动基元；
最优运动基元获取模块，用于获取所述各个最优中间点状态中距离终点状态最近的最优中间点状态到终点状态的最优运动基元；以及，
全局路径构建模块，用于根据获取的起点状态到终点状态的各个最优中间点状态及记录的对应的运动基元，以及距离终点状态最近的最优中间点状态到终点状态的最优运动基元，构建双轮差速型移动机器人的全局路径。
9.根据权利要求8所述的装置，其特征在于，至少基于双轮差速型移动机器人的角速度和位姿，构造双轮差速型移动机器人的运动学模型。
10.根据权利要求8或9所述的装置，其特征在于，所述根据双轮差速型移动机器人的运动学模型构造运动基元时，构造运动基元的输入用(V,W)表示，V表示移动机器人的线速度，W表示双轮差速型移动机器人的角速度；根据双轮差速型移动机器人的角速度W的值，构造相应的运动基元。
11.根据权利要求10所述的装置，其特征在于，从起点状态到终点状态之间的各个相邻中间点状态之间的运动时间为固定运动时间，起点状态到与起点状态相邻的中间点状态之间的运动时间为所述固定运动时间，且所述固定运动时间等于控制采样时间Δt。
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 true CN108919794A (zh) 2018-11-30
CN108919794B 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)

Cited By (4)

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

Citations (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
CN103646035A (zh) * 2013-11-14 2014-03-19 北京锐安科技有限公司 一种基于启发式方法的信息搜索方法
CN106444738A (zh) * 2016-05-24 2017-02-22 武汉科技大学 基于动态运动基元学习模型的移动机器人路径规划方法
CN106767860A (zh) * 2016-11-21 2017-05-31 江苏大学 一种基于启发式搜索算法来缩短智能汽车路径规划搜索时间的方法
CN106989748A (zh) * 2017-05-16 2017-07-28 南京农业大学 一种基于云模型的农业移动机器人人机合作路径规划方法
CN107214701A (zh) * 2017-06-12 2017-09-29 南京理工大学 一种基于运动基元库的带电作业机械臂自主避障路径规划方法

Patent Citations (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
CN103646035A (zh) * 2013-11-14 2014-03-19 北京锐安科技有限公司 一种基于启发式方法的信息搜索方法
CN106444738A (zh) * 2016-05-24 2017-02-22 武汉科技大学 基于动态运动基元学习模型的移动机器人路径规划方法
CN106767860A (zh) * 2016-11-21 2017-05-31 江苏大学 一种基于启发式搜索算法来缩短智能汽车路径规划搜索时间的方法
CN106989748A (zh) * 2017-05-16 2017-07-28 南京农业大学 一种基于云模型的农业移动机器人人机合作路径规划方法
CN107214701A (zh) * 2017-06-12 2017-09-29 南京理工大学 一种基于运动基元库的带电作业机械臂自主避障路径规划方法

Cited By (6)

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

Also Published As

Publication number Publication date
CN108919794B (zh) 2021-08-17

Similar Documents

Publication Publication Date Title
CN108919794A (zh) 一种双轮差速型移动机器人的全局路径规划方法及其装置
Hong et al. Rules of the road: Predicting driving behavior with a convolutional model of semantic interactions
Nguyen et al. Help, anna! visual navigation with natural multimodal assistance via retrospective curiosity-encouraging imitation learning
Sadeghian et al. Sophie: An attentive gan for predicting paths compliant to social and physical constraints
KR102177412B1 (ko) 이미지와 텍스트간 유사도 매칭 시스템 및 방법
Sud et al. Real-time path planning in dynamic virtual environments using multiagent navigation graphs
CN106845430A (zh) 基于加速区域卷积神经网络的行人检测与跟踪方法
CN110009718A (zh) 一种三维高精度地图生成方法及装置
CN111142522A (zh) 一种分层强化学习的智能体控制方法
US20120306741A1 (en) System and Method for Enhancing Locative Response Abilities of Autonomous and Semi-Autonomous Agents
WO2022007867A1 (zh) 神经网络的构建方法和装置
JP7465048B2 (ja) 視覚入力に対する形式的安全シンボリック強化学習
CN112231967A (zh) 基于深度强化学习的人群疏散仿真方法及系统
Loiseau et al. Online segmentation of lidar sequences: Dataset and algorithm
KR20210075619A (ko) 자율주행 로봇, 자율주행 로봇의 위치 추정 서버 및 이를 이용한 자율주행 로봇의 위치 추정 방법
Katyal et al. Occupancy map prediction using generative and fully convolutional networks for vehicle navigation
Bigazzi et al. Out of the box: embodied navigation in the real world
CN107704549A (zh) 语音搜索方法、装置及计算机设备
Wu et al. Analysing cinematography with embedded constrained patterns
CN110975291B (zh) 一种路径提取方法及系统
CN117576149A (zh) 一种基于注意力机制的单目标跟踪方法
CN110162655A (zh) 一种基于草图的三维模型局部检索方法、装置、终端设备
Archetti et al. Neural weighted a*: Learning graph costs and heuristics with differentiable anytime a
CN111832602B (zh) 基于地图的特征嵌入方法、装置、存储介质和电子设备
Zaech Vision for Autonomous Systems: From Tracking and Prediction to Quantum Computing

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