CN113359748A - 一种融合预测的改进Multi-RRT路径规划方法及AGV小车 - Google Patents

Abstract

本发明公开了一种融合预测的改进Multi‑RRT路径规划方法及AGV小车，本发明通过改进Multi‑RRT与稀疏矩阵的多目标路径规划算法生成实时最优路径指令，同时将该运行指令发送给智能物流车，由智能物流车进行执行。本发明可应用于智能物流仓储或智能工厂等多机器人多目标点协同工作路径规划场景中。在保留路径选择随机性的同时能够加快算法收敛速度，尤其适应于大型空旷环境下多传感器节点的场景。

Description

一种融合预测的改进Multi-RRT路径规划方法及AGV小车

技术领域

本发明涉及路径规划技术领域，特别涉及一种融合预测的改进Multi-RRT路径规划方法及AGV小车。

背景技术

RRT是一种多维空间中有效率的规划方法。它以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。在此基础上，提出基于RRT的多目标路径规范方法即为Multi-RRT方法，但该方法的计算量较大，冗余度较高，效率较为低下。

发明内容

本发明的目的在于，提供一种融合预测的改进Multi-RRT路径规划方法及AGV小车。本发明提高了多目标路径的计算效率，减少计算冗余，具有高效性的优点。

本发明的技术方案：一种融合预测的改进Multi-RRT路径规划方法,包括如下步骤：

步骤A：首先获取多个AGV小车与多个目标点的位置信息，将其转化为位置矩阵P，并将多个AGV小车的位置信息构成出发点集合，从出发点集合中选取出发点生成规划路径；再把不同的位置看作不同节点，AGV小车在某一个位置i会选择下一个节点位置为j的可能性表示为节点间转移概率，遍历所有的节点间转移概率构成转移概率矩阵A；

步骤B：转移概率矩阵A为对称矩阵，使用Cholesky算法将转移概率矩阵A解析分解得到环境矩阵L，环境矩阵L中元素代表各个节点之间相互选择的转移概率P_i，j；

步骤C:初始化环境矩阵L中多个AGV小车到各目标点的转移概率P_i，j，得到初始转移矩阵L¹；

步骤D:根据任意一个AGV小车当前位置和该AGV小车下一步或若干步后选择的节点对应的路径和概率计算路径期望，通过路径期望最大化得到当前节点到下一个节点的生长方向的参考值

步骤E:在生成的规划路径中找到距离步骤D中的AGV小车当前位置最近的节点X_near，在这两者连线上随机确定一个新节点X_rand，根据节点X_rand确定随机方向

步骤F:把参考值

和随机方向

结合，确定下一节点生长方向

步骤G:根据总路径代价最小的原则，迭代计算AGV小车每一步需走的步长；

步骤H:判断当前AGV小车在生长路径中的当前节点能否连接到目标点，如果不能则进入下一个节点，再重复上述步骤A-G，迭代计算出多个AGV出发点到多个目标点的路径；如果已经连接到终点，则从连接目标点的节点开始不断找出其父节点，直到找到起点，从而确定从起点开始连接目标点的路径；

步骤I：判断是否已遍历出发点集合，若否则从步骤A开始选取新出发点，若是则停止计算。

上述的融合预测的改进Multi-RRT路径规划方法，步骤D中在某个节点预测下个节点或者若干步后节点对应的概率计算步骤如下：

D.1：计算以第i个节点为出发点到达若干步后的第j个节点的概率P_i，j：

P_i，j＝Prob(X_n+1＝j|X_n＝i)； 式(1)；

式中：P_i，j是以第i个节点为出发点到达若干步后的第j个节点的概率，Prob表示根据先验得到的转移概率值；X_n表示当前位置；

D.2：计算第X_n步，位置为i节点的出发点经下两步X_n+2能够到达第k个节点的概率，在这之中的中间步X_n+1会产生多个选择，因此通过条件概率公式求和：

D.3：把条件概率公式转化成联合概率和先验概率的比值:

D.4：把式(3)中的求和项构造成两个式子的乘积，两个式子分别代表两个联合概率和先验概率的比值，即两个条件概率值：

D.5：根据事件的马尔可夫特性，以从出发点i出发经过节点j为条件到达目标k点概率等于从出发点j出发到达k点概率：

Prob(X_n+2＝k|X_n+1＝j and X_n＝i)＝Prob(X_n+2＝k|X_n+1＝j)； 式(5)，

式中，X_n表示出发点i，X_n+1表示中间点j；X_n+2表示目标点k；

将式(5)代入式(4)，得：

根据步骤B中的对称性简化式(6)得到：

根据式(7)从而得到对于从Xn出发时经过m步后的公式：

Prob(X_n+m＝j|X_n＝i)＝(P_i，j)^m。

前述的融合预测的改进Multi-RRT路径规划方法，对于多起点多目标点，在确定从某一个出发点出发的路径后，对比不同出发点间的路径并选择实际出发点，即步骤I判断是否已遍历出发点集合，遍历后采用EM算法选择收敛后概率最大的起始点对应的路径作为行进路径：

输入：观测变量数据Y＝Y₁,Y₂,……Y_n，隐变量Z＝Z₁,Z₂……Z_n，联合分布P(Y，Z|θ)，条件分布P(Z|Y，θ)；

输出：模型参数θ＝θ₁,θ₂,……θ_m，具体为出发点位置；

按以下步骤进行：

步骤1：选择参数的初始值θ⁽⁰⁾，开始迭代；

步骤2：在第E步时：在第i次迭代中，计算Q函数值：

式中：Q函数表示所求的代价函数；logP(Y，Z|θ)|Y，θ⁽ⁱ⁾表示对条件概率求对数值，增大转移概率；E_Z表示对求完对数后的概率值求期望；

步骤3：在第M步时：求使得Q函数最大化的θ⁽ⁱ⁺¹⁾：

重复步骤2和步骤3，直到收敛，得到模型参数θ的值，即得到多个出发点中与目标点最匹配的出发点。

前述的融合预测的改进Multi-RRT路径规划方法，步骤G中，通过Cost函数计算AGV小车每一步需走的步长；

式中：f^(x)为列举所有路径长度的期望值，S_ij是指从节点i到节点j路径的长度，P_ij是指选择从节点i到节点j的路径的概率。

一种执行所述的融合预测的改进Multi-RRT路径规划方法的AGV小车，包括底板，底板的下方四周设有直流电机，直流电机的输出端连接有轮胎；所述底板的中部设有顶板，顶板上设有多自由度的机械臂，机械臂的前端设有机械手；所述机械手上设有摄像头；所述顶板上设有车体控制器和通讯模块；所述底板的底面前端设有定位模块；所述底板的侧部设有物料夹持台。

上述的融合预测的改进Multi-RRT路径规划方法的AGV小车，所述物料夹持台4包括固定体，所述固定体的上端设有连杆连接件，固定体上铰接有底部具有齿牙的主动臂以及从动臂，主动臂的齿牙与从动臂的齿牙相啮合；所述主动臂的前端铰接有主动加持臂，主动加持臂的上端连接有主动夹持手；所述主动夹持手经主动辅助连杆与连杆连接件连接；所述从动臂的前端铰接有从动加持臂，从动加持臂的上端连接有从动夹持手，从动夹持手经从动辅助连杆与连杆连接件连接。

前述的融合预测的改进Multi-RRT路径规划方法的AGV小车，所述通讯模块为ZigBee模块。

与现有技术相比，本发明通过将多个AGV与多个目标点的位置信息转化为位置矩阵，特别对于空旷环境下，该位置矩阵具有稀疏特性，该稀疏特性可以为后续步骤的计算去掉没有信息的特征，使得可以在计算时进行特征的自动选择，降低了学习任务的难度，因此本发明提高了多目标路径的计算效率，减少计算冗余，具有高效性的优点。本发明可应用于智能物流仓储或智能工厂等多机器人多目标点协同工作路径规划场景中，在保留路径选择随机性的同时能够加快算法收敛速度，尤其适应于大型空旷环境下多传感器节点的场景。

附图说明

图1为本发明的方法流程图；

图2是获取多个AGV小车与多个目标点的位置信息的示意图；

图3是转移概率示意图；

图4是路径中提出中间点的示意图；

图5是AGV小车每一步需走的步长示意图；

图6是AGV小车的结构示意图；

图7是物料夹持台的结构示意图；

图8是物料夹持台夹持物流的结构示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的说明，但并不作为对本发明限制的依据。

实施例：一种融合预测的改进Multi-RRT路径规划方法，如图1所示，包括如下步骤：

步骤A：如图2所示，获取多个AGV小车(智能物流车)与多个目标点的位置信息，将其转化为位置矩阵P，并将多个AGV小车的位置信息构成出发点集合，从出发点集合中选取出发点生成规划路径；再把不同的位置看作不同节点，AGV小车在某一个位置i会选择下一个节点位置为j的的可能性表示为节点间转移概率，遍历所有的节点间转移概率构成转移概率矩阵A；

步骤B：假设矩阵A为对称矩阵，则根据概率非负性，矩阵A为对称正定矩阵，使用Cholesky算法将位置矩阵A解析分解得到环境矩阵L：A＝L*L^T

环境矩阵L中元素为各个点之间相互选择的转移概率P_i，j；本实施例中，如图3所示，假设有n个车，m个目标点，目标点之间存在时序性(即存在单向性)，该问题可以转化为有向图问题，即转化为环境矩阵L；由于环境稀疏性，该矩阵中有较多元素为0；

矩阵L中横向为出发点，竖向为目标点(或中间点)；矩阵L中元素为各个点之间互相选择的转移概率P_i，j(指从i出发到j点的概率)，矩阵L中对角线上每一点的值为“1”。本实施例中中A、B、C为出发点，D、E、F为目标点。该矩阵中所有元素大于等于0，矩阵每列之和为1。

步骤C:初始化环境矩阵L中多个AGV小车到各目标点的转移概率P_i，j，得到初始转移矩阵L¹；

步骤D:根据任意一个AGV小车当前位置和该AGV小车下一步或若干步后预测的节点对应的路径和概率计算路径期望，通过路径期望最大化得到当前节点到下一个节点的生长方向的参考值

步骤D中在某个节点预测下个节点或者若干步后节点对应的概率计算步骤如下：如图4所示，提出中间点i，j，k；

D.1：计算以第i个节点为出发点到达若干步后的第j个节点的概率P_i，j：

P_i，j＝Prob(X_n+1＝j|X_n＝i)； 式(1)；

式中：P_i，j是以第i个节点为出发点到达若干步后的第j个节点的概率，Prob表示根据先验得到的转移概率值；X_n表示当前位置；

D.2：计算第X_n步，位置为i节点的出发点经下两步X_n+2的能够到达第k个节点的概率，在这之中的中间步X_n+1会产生多个选择，因此通过条件概率公式求和：

D.3：把条件概率公式转化成联合概率和先验概率的比值:

D.4：把式(3)中的求和项构造成两个式子的乘积，两个式子分别代表两个联合概率和先验概率的比值，即两个条件概率值：

D.5：根据时间的马尔可夫性，以从出发点i出发经过节点j为条件到达目标k点概率等于从出发点j出发到达k点概率：

Prob(X_n+2＝k|X_n+1＝j and X_n＝i)＝Prob(X_n+2＝k|X_n+1＝j)； 式(5)，

式中，X_n表示出发点i，X_n+1表示中间点j；X_n+2表示目标点k；

将式(5)代入式(4)，得：

根据步骤B中的假设对称性化简式(6)后得：

根据式(7)得到对于从Xn出发时经过m步时的公式：

Prob(X_n+m＝j|X_n＝i)＝(P_i，j)^m。

步骤E:在生成的规划路径中找到距离步骤D中的AGV小车当前位置最近的节点X_near，在这两者连线上随机确定一个新节点X_rand，根据节点X_rand确定随机方向

步骤F:把参考值

和随机方向

结合，确定下一节点生长方向

根据随机性调节因子ε确定：

步骤G:通过总路径代价最小迭代计算AGV小车每一步需走的步长；本实施例中，如图5所示，通过Cost函数计算AGV小车每一步需走的步长；

式中：f(x)为列举所有路径长度的期望值，S_ij是指从节点i到节点j路径的长度，P_ij是指选择从节点i到节点j的路径的概率。

约束：

步骤H:判断当前AGV小车在生长路径中的当前节点能否连接到目标点，如果不能则进入下一个节点，再重复上述步骤A-F，迭代计算出多个AGV出发点到多个目标点的路径；如果已经连接到终点，则从连接目标点的树节点开始不断找出其父节点，直到找到起点，从而确定从起点开始连接目标点的路径；

步骤I：判断是否已遍历出发点集合，若否则从步骤A开始选取新出发点，若是则停止计算。对于多起点多目标点，在确定从某一个出发点出发的路径后，对比不同出发点间的路径并选择实际出发点，即步骤I判断是否已遍历出发点集合，遍历后采用EM算法选择收敛后概率最大的起始点对应的路径作为行进路径：

输入：观测变量数据Y＝Y₁,Y₂,……Y_n，隐变量Z＝Z₁,Z₂……Z_n，联合分布P(Y，Z|θ)，条件分布P(Z|Y，θ)；

输出：模型参数θ＝θ₁,θ₂,……θ_m，具体为出发点位置；

按以下步骤进行：

步骤1：选择参数的初始值θ⁽⁰⁾，开始迭代；

步骤2：在第E步时：在第i次迭代中，计算Q函数值：

式中：Q函数表示所求的代价函数；logP(Y，Z|θ)|Y，θ⁽ⁱ⁾表示对条件概率求对数值，增大转移概率；F_Z表示对求完对数后的概率值求期望；

步骤3：在第M步时：求使得Q函数最大化的θ⁽ⁱ⁺¹⁾：

重复步骤2和步骤3，直到收敛，得到模型参数θ的值，即得到多个出发点中与目标点最匹配的出发点。

一种执行前述的融合预测的改进Multi-RRT路径规划方法的AGV小车，包括底板3，底板3的下方四周设有直流电机1，直流电机的输出端连接有轮胎2；所述底板3的中部设有顶板5，顶板5上设有多自由度的机械臂8，机械臂8的前端设有机械手6，所述机械手6用于夹取物料，将物料从货架上夹起；所述机械手6上设有摄像头7，所述摄像头7用于采集所抓取物料信息与其位置；所述顶板5上设有车体控制器9和通讯模块10，所述通讯模块10为ZigBee模块；所述底板3的底面前端设有定位模块11；所述底板3的侧部设有物料夹持台4。用于夹持物料，将物料牢牢固定在智能物流车中，防止智能物流车在行进中发生物料掉落现象。所述机械臂8用于将物料从货架上转移至物料夹持台4或将物料从物料夹持台4转移至货架上，所述车体控制器9用于整台智能物流车的控制，所述通讯模块10用于和控制中心进行通讯连接，通过通讯模块10上传相关信息和获取相关任务，各智能物流车均可通过通讯模块10具有与其他车体控制器9通信连接，通过通讯模块10相互之间实现通信连接，以交换各自的位置信息，所述通讯模块10与车体控制器9之间设有隔离电路；所述定位模块11用于获取智能物流车当前所处的位置。控制中心，与智能物流车的通讯模块10通信连接，用于接收智能物流车当前所在位置以及所抓取或放置物料的相关信息，并结合RRT*与稀疏矩阵的多目标路径规划算法生成实时最优路径指令，同时将该运行指令发送给智能物流车。

所述物料夹持台4包括固定体12，所述固定体12的上端设有连杆连接件17，固定体12上铰接有底部具有齿牙的主动臂13以及从动臂21，主动臂13的齿牙与从动臂21的齿牙相啮合，主动臂13与电机进行连接，由电机进行驱动；所述主动臂13的前端铰接有主动加持臂14，主动加持臂14的上端连接有主动夹持手16；所述主动夹持手16经主动辅助连杆15与连杆连接件17连接；所述从动臂21的前端铰接有从动加持臂20，从动加持臂20的上端连接有从动夹持手18，从动夹持手18经从动辅助连杆19与连杆连接件17连接。本发明的所述物料夹持台4通过构建平行四边形四连杆机构，实现物料的稳固加持，将物料牢牢固定在智能物流车中，防止智能物流车在行进中发生物料掉落现象。其中所述的固定体12、主动臂13、主动加持臂14、主动辅助连杆15构成平行四边形四连杆机构，通过主动臂13的转动，带动主动加持臂14向物料移动，通过主动辅助连杆15保证主动加持臂14在移动中始终保持垂直，实现主动夹持手16整个平面完整贴合于物料的一侧，增加了主动夹持手16和物料的接触平面，从而增大了摩擦力。所述的固定体12、从动辅助连杆19、从动加持臂20、从动臂21构成平行四边形四连杆机构，其从动臂21和主动臂13通过齿轮啮合传递动力，在主动臂13转动时从动臂21同步转动，通过从动臂21的转动，带动从动加持臂20向物料移动，通过从动辅助连杆19保证从动加持臂20在移动中始终保持垂直，实现从动夹持手18整个平面完整贴合于物料的一侧，增加了从动夹持手18和物料的接触平面，从而增大了摩擦力。主动臂13和从动臂21通过齿牙啮合，既传递了动力，同时保证了主动臂13和从动臂21同步转动，通过两边的平行四边形四连杆机构，实现了主动夹持手16和从动夹持手18同时接触到物料22两侧，保证了物料22不会发生偏移，也保证了加持物料22时的稳固性。

本发明可应用于智能物流仓储或智能工厂等多机器人多目标点协同工作路径规划场景中，在保留路径选择随机性的同时能够加快算法收敛速度，尤其适应于大型空旷环境下多传感器节点的场景，具有高效性的优点。

Claims (7)

1.一种融合预测的改进Multi-RRT路径规划方法,其特征在于：包括如下步骤：

步骤A：首先获取多个AGV小车与多个目标点的位置信息，将其转化为位置矩阵P，并将多个AGV小车的位置信息构成出发点集合，从出发点集合中选取出发点生成规划路径；再把不同的位置看作不同节点，AGV小车在某一个位置i会选择下一个节点位置为j的可能性表示为节点间转移概率，遍历所有的节点间转移概率构成转移概率矩阵A；

步骤B：转移概率矩阵A为对称矩阵，使用Cholesky算法将转移概率矩阵A解析分解得到环境矩阵L，环境矩阵L中元素代表各个节点之间相互选择的转移概率P_i，j；

步骤C:初始化环境矩阵L中多个AGV小车到各目标点的转移概率P_i，j，得到初始转移矩阵L¹；

步骤D:根据任意一个AGV小车当前位置和该AGV小车下一步或若干步后预测的节点对应的路径和概率计算路径期望，通过路径期望最大化得到当前节点到下一个节点的生长方向的参考值

步骤E:在生成的规划路径中找到距离步骤D中的AGV小车当前位置最近的节点X_near，在这两者连线上随机确定一个新节点X_rand，根据节点X_rand确定随机方向

步骤F:把参考值

和随机方向

结合，确定下一节点生长方向

步骤G:根据总路径代价最小的原则，迭代计算AGV小车每一步需走的步长；

步骤H:判断当前AGV小车在生长路径中的当前节点能否连接到目标点，如果不能则进入下一个节点，再重复上述步骤A-G，迭代计算出多个AGV出发点到多个目标点的路径；如果已经连接到终点，则从连接目标点的节点开始不断找出其父节点，直到找到起点，从而确定从起点开始连接目标点的路径；

步骤I：判断是否已遍历出发点集合，若否则从步骤A开始选取新出发点，若是则停止计算。

2.根据权利要求1所述的融合预测的改进Multi-RRT路径规划方法，其特征在于：步骤D中在某个节点预测下个节点或者若干步后节点对应的概率计算步骤如下：

D.1：计算以第i个节点为出发点到达若干步后的第j个节点的概率P_i，j：

P_i，j＝Prob(X_n+1＝j|X_n＝i)； 式(1)；

式中：P_i，j是以第i个节点为出发点到达若干步后的第j个节点的概率，Prob表示根据先验得到的转移概率值；X_n表示当前位置；

D.2：计算第X_n步，位置为i节点的出发点经下两步X_n+2能够到达第k个节点的概率，在这之中的中间步X_n+1会产生多个选择，因此通过条件概率公式求和：

D.3：把条件概率公式转化成联合概率和先验概率的比值:

D.4：把式(3)中的求和项构造成两个式子的乘积，两个式子分别代表两个联合概率和先验概率的比值，即两个条件概率值：

D.5：根据事件的马尔可夫特性，以从出发点i出发经过节点j为条件到达目标k点概率等于从出发点j出发到达k点概率：

Prob(X_n+2＝k|X_n+1＝j and X_n＝i)＝Prob(X_n+2＝k|X_n+1＝j)； 式(5)，

式中，X_n表示出发点i，X_n+1表示中间点j；X_n+2表示目标点k；

将式(5)代入式(4)，得：

根据步骤B中的对称性简化式(6)得到：

根据式(7)从而得到对于从Xn出发时经过m步后的公式：

Prob(X_n+m＝j|X_n＝i)＝(P_i，j)^m。

3.根据权利要求1所述的融合预测的改进Multi-RRT路径规划方法，其特征在于：对于多起点多目标点，在确定从某一个出发点出发的路径后，对比不同出发点间的路径并选择实际出发点，即步骤I判断是否已遍历出发点集合，遍历后采用EM算法选择收敛后概率最大的起始点对应的路径作为行进路径：

输入：观测变量数据Y＝Y₁,Y₂,……Y_n，隐变量Z＝Z₁,Z₂……Z_n，联合分布P(Y，Z|θ)，条件分布P(Z|Y，θ)；

输出：模型参数θ＝θ₁,θ₂,……θ_m，具体为出发点位置；

按以下步骤进行：

步骤1：选择参数的初始值θ⁽⁰⁾，开始迭代；

步骤2：在第E步时：在第i次迭代中，计算Q函数值：

式中：Q函数表示所求的代价函数；logP(Y，Z|θ)Y，θ⁽ⁱ⁾表示对条件概率求对数值，增大转移概率；E_Z表示对求完对数后的概率值求期望；

步骤3：在第M步时：求使得Q函数最大化的θ⁽ⁱ⁺¹⁾：

重复步骤2和步骤3，直到收敛，得到模型参数θ的值，即得到多个出发点中与目标点最匹配的出发点。

4.根据权利要求1所述的融合预测的改进Multi-RRT路径规划方法，其特征在于：步骤G中，通过Cost函数计算AGV小车每一步需走的步长；

式中：f(x)为列举所有路径长度的期望值，S_ij是指从节点i到节点j路径的长度，P_ij是指选择从节点i到节点j的路径的概率。

5.一种执行所述的融合预测的改进Multi-RRT路径规划方法的AGV小车，其特征在于：包括底板(3)，底板(3)的下方四周设有直流电机(1)，直流电机的输出端连接有轮胎(2)；所述底板(3)的中部设有顶板(5)，顶板(5)上设有多自由度的机械臂(8)，机械臂(8)的前端设有机械手(6)；所述机械手(6)上设有摄像头(7)；所述顶板(5)上设有车体控制器(9)和通讯模块(10)；所述底板(3)的底面前端设有定位模块(11)；所述底板(3)的侧部设有物料夹持台(4)。

6.根据权利要求5所述的融合预测的改进Multi-RRT路径规划方法的AGV小车，其特征在于：所述物料夹持台4包括固定体(12)，所述固定体(12)的上端设有连杆连接件(17)，固定体(12)上铰接有底部具有齿牙的主动臂(13)以及从动臂(21)，主动臂(13)的齿牙与从动臂(21)的齿牙相啮合；所述主动臂(13)的前端铰接有主动加持臂(14)，主动加持臂(14)的上端连接有主动夹持手(16)；所述主动夹持手(16)经主动辅助连杆(15)与连杆连接件(17)连接；所述从动臂(21)的前端铰接有从动加持臂(20)，从动加持臂(20)的上端连接有从动夹持手(18)，从动夹持手(18)经从动辅助连杆(19)与连杆连接件(17)连接。

7.根据权利要求5所述的融合预测的改进Multi-RRT路径规划方法的AGV小车，其特征在于：所述通讯模块(10)为ZigBee模块。

Images