CN109572857A - 一种麦克纳姆轮智能仓储agv及其路径规划方法 - Google Patents

Abstract

本发明涉及一种麦克纳姆轮智能仓储AGV及其路径规划方法，其包括：构建AGV运动模型以及麦克纳姆轮运动学模型；实时测算AGV的运行速度和角速度，同时由激光雷达传感器采集数据并回传至主控制器；构建多帧子地图；对所有子地图数据进行基于子地图快速关联算法的关联分析，并对AGV的状态进行预测以及环境地图预测，对环境地图进行更新或环境地图增广；超声波传感器实时检测前方是否有障碍并把结果回传至主控制器，计算出AGV的运行轨迹；将AGV运行轨迹代入麦克纳姆轮运动学公式，计算出各轮转向和转速值；各轮转向和转速值代入模糊PID控制器，结合AGV麦克纳姆轮的行星减速电机上的编码器回传的数据，驱动行星减速电机，最终实现AGV的精准定位和快速有效的搬运。

Description

一种麦克纳姆轮智能仓储AGV及其路径规划方法

技术领域

本发明涉及一种仓储AGV，特别是关于一种基于子地图快速关联算法的麦克纳姆轮智能仓储AGV及其路径规划方法。

背景技术

目前，智能仓储AGV(自动导引运输车)的开发和使用是一个非常热门的领域，许多公司都有着一套独特的AGV系统。最出名的代号为“小黄人”的机器人在仓储货物分拣上表现卓越，依靠着强大的主机终端，实时调度几十台“小黄人”的运行，实现物流分拣工作。但该款AGV只适合运行在较为平整的路面，适用场景也非常有限。另外，也有很多公司推出了相对“小黄”来说体积更大，功能更齐全的AGV系统，其自主导航绝大多数使用的是激光导航、惯性导航以及磁条导航。其中，激光导航占有很大的一部分，基于激光雷达的SLAM技术是目前最稳定、可靠性最高的方式。最早用于解决移动机器人同时定位与建图的方法是EKF－SLAM。在该方法中，最近邻数据关联法用于获取数据关联结果，扩展卡尔曼滤波用于完成对移动机器人位姿和环境地图的估计。然而EKF－SLAM在实际应用中存在很多问题，包括算法复杂度高、数据关联题难处理以及估计精度差等问题。

与此同时，AGV运行时，个别轮子转速不理想或打滑会直接对AGV运行的稳定性造成很大的影响，行业内解决此类问题的措施大部分是保证地面的平整或增大轮子的抓地力，但在一些特殊场合，这些措施将会无济于事。

发明内容

针对上述问题，本发明的目的是提供一种基于子地图快速关联算法的麦克纳姆轮智能仓储AGV及其路径规划方法，其依靠SLAM(同时定位与建图)技术，实现有效的路径规划，完成物品运输或是到达指定位置的功能。此外，依靠麦克纳姆轮的高机动性，提高作业效率并能在狭小的空间正常作业。

为实现上述目的，本发明采取以下技术方案：一种麦克纳姆轮智能仓储AGV路径规划方法，其包括以下步骤：1)预先设定AGV的搬运目标地点、仓储货物的放置地点以及行进速度；2)由AGV中的主控制器构建AGV运动模型以及麦克纳姆轮运动学模型；3)由AGV中的与主控制器连接的九轴陀螺仪实时测算AGV的运行速度和角速度，同时由设置在AGV上的激光雷达传感器开始采集数据并回传至主控制器；4)主控制器分别读取多帧激光雷达数据，构建多帧子地图，并将所有子地图存入主控制器的板载非易失存储器中；5)对存储器中的所有子地图数据进行基于子地图快速关联算法的关联分析，并对AGV的状态进行预测以及环境地图预测，结合预测结果，对环境地图进行更新或环境地图增广；6)设置在AGV上的超声波传感器实时检测前方是否有障碍并把结果回传至主控制器，主控制器根据更新后的环境地图和超声波传感器回传的数据计算出AGV的运行轨迹；7)将AGV运行轨迹代入麦克纳姆轮运动学公式，计算出各轮转向和转速值；8)将各轮转向和转速值代入模糊PID控制器，结合AGV麦克纳姆轮的行星减速电机上的编码器回传的数据，驱动行星减速电机，最终实现AGV的精准定位和快速有效的搬运。

进一步，所述步骤5)中，对所有子地图数据进行基于子地图快速关联算法的关联分析，包括以下步骤：5.1)主控制器连续读取由多帧的激光雷达数据构建成的子地图数据；5.2)对每一时刻的子地图数据设定局部关联区域，具体公式如下：

式中，(x_v,y_v,θ)表示当前时刻AGV的位姿，x_v表示AGV沿x方向的位姿、y_v表示AGV沿y方向的位姿、θ表示AGV的方向角；abs表示开根方；(x_i,y_i)表示子地图上某个特征的坐标值；r为激光雷达的最大扫描距离；d为补偿距离；由此得到的局部关联区域即为以激光雷达为圆心的半径为r+d的半圆；5.3)对t时刻的子地图划分完局部关联区域后，对子地图上的特征值点G_t＝{g_s|w′＝1,…m}按密度分为M组，即其中，表示t时刻的第k个小组，g_s表示第k组中的特征点，m_k表示第k组中的第m个特征点，w′表示存放特征点的变量；5.4)完成局部关联区分割和数据分组后，利用JCBB得到每组数据与全局地图Q关联的最优解U^l，最后对每组数据对应的关联最优解进行组合，得到最终的关联结果其中，u_n表示第n个最优解，t表示当前时刻，JCBB为联合兼容分支定界算法；5.5)根据关联结果，对环境地图进行更新或增广：u_n＝q表示该观测点与全局地图中第q个特征相匹配，该观测点则用于地图更新，u_n＝0表示该观测点为新特征值用于全局地图的增广。

进一步，所述步骤2)中，AGV运动模型为：

式中，o_v,t表示AGV在t时刻的位姿；L为车轮轴距；dt表示输入量的单次采样时间；v和θ表示t时刻的输入量，v表示AGV的速度，θ表示AGV的方向角；x_t-1表示t-1时刻AGV相对于x方向的位姿；y_t-1表示t-1时刻AGV相对于y的位姿；θ_t-1表示t-1时刻AGV的角位姿。

进一步，所述步骤2)中，麦克纳姆轮运动学模型为：

式中，w₁、w₂、w₃、w₄分别表示四个麦克纳姆轮的转速，v_x表示AGV沿x方向的速度，v_y表示AGV沿y方向的速度，w表示AGV绕四轮中心的转速，L₁表示沿x方向轮子中轴线到四轮中心的距离，L₂表示沿y方向轮子中轴线到四轮中心的距离，R为轮子半径。

一种实现上述规划方法的麦克纳姆轮智能仓储AGV，其包括车身，位于所述车身底部两侧分别设置有一对麦克纳姆轮，位于所述车身顶部设置有机械臂固定柱和主控系统仓，所述主控系统仓位于所述机械臂固定柱后部；位于所述车身前端顶部中央位置处设置有激光雷达传感器，位于所述车身前端下部两侧还分别设置有用于AGV局部避障的超声波传感器；所述机械臂固定柱的顶部通过旋转平台与机械臂结构连接，位于所述机械臂结构的端部设置有末端执行器；所述主控系统仓内设置有主控制器、以及与所述主控制器连接的九轴陀螺仪和麦克纳姆轮驱动电机；所述激光雷达传感器、超声波传感器和九轴陀螺仪将检测到的信号都传输至所述主控制器内，由所述主控制器控制所述旋转平台和麦克纳姆轮驱动电机动作。

进一步，所述机械臂结构包括机械大臂、机械中臂、第一步进电机、机械小臂、关节和第二步进电机；所述机械大臂一端通过所述旋转平台与所述机械臂固定柱连接，所述机械大臂另一端通过所述第一步进电机与所述机械中臂一端连接；所述机械中臂另一端通过所述关节与所述机械小臂一端连接，所述机械小臂该端端部设置有所述第二步进电机，且所述机械小臂的末端连接有所述末端执行器；所述第一步进电机和第二步进电机均与所述主控制器连接。

进一步，所述末端执行器内设置有驱动电机，该驱动电机与所述主控制器连接，该驱动电机带动所述末端执行器绕所述机械小臂转动。

进一步，所述机械臂结构还包括驱动连杆；所述驱动连杆一端与所述第一步进电机连接，另一端与所述机械小臂一端连接。

进一步，所述车身后部设置有动力电池仓，所述动力电池仓内设置有锂电池，所述锂电池外层包裹阻燃材料；所述动力电池仓后部还设置有充电插口。

进一步，所述麦克纳姆轮驱动电机采用带编码器的行星减速电机，该行星减速电机固定在所述车身的底盘上。

本发明由于采取以上技术方案，其具有以下优点：1、本发明采用基于子地图快速关联算法，对传统关联算法进行了优化，提高了AGV的同时定位与建图能力，减少了主控系统的运算负担，使AGV能够实现仓储货物的有效快速移送。2、本发明采用模糊PID算法，解决了因个别轮子打滑引起的移动不稳定问题。3、本发明采用基于投影法改进的麦克纳姆轮，优化了麦克纳姆轮运动单元的参数设计，使AGV运行更稳定。4、本发明主控系统使用的中央处理器模块采用ARM Cortex-M4FRDM-K64型号的单片机，具有成本低和可靠性高的优点。

附图说明

图1是本发明的AGV结构示意图；

图2是本发明的使用麦克纳姆轮的底盘示意图；

图3是本发明的基于子地图快速关联算法的关联分析流程示意图；

图4是本发明的划分局部关联区域和观测点分组的示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明提供一种麦克纳姆轮智能仓储AGV，其包括车身1，位于车身1底部两侧分别设置有一对麦克纳姆轮2，位于车身1顶部设置有机械臂固定柱3和主控系统仓4，主控系统仓4位于机械臂固定柱3后部。位于车身1前端顶部中央位置处设置有激光雷达传感器5，位于车身1前端下部两侧还分别设置有用于AGV局部避障的超声波传感器6。机械臂固定柱3的顶部通过旋转平台7与机械臂结构连接，位于机械臂结构的端部设置有末端执行器8。主控系统仓4内设置有主控制器、以及与主控制器连接的九轴陀螺仪和麦克纳姆轮驱动电机。激光雷达传感器5、超声波传感器6和九轴陀螺仪将检测到的信号都传输至主控制器内，由主控制器控制旋转平台7和麦克纳姆轮驱动电机动作，进而控制两对麦克纳姆轮2工作。

在一个优选的实施例中，机械臂结构包括机械大臂9、第一步进电机10、机械中臂11、关节12、机械小臂13和第二步进电机14。机械大臂9一端通过旋转平台7与机械臂固定柱3连接，机械大臂9另一端通过第一步进电机10与机械中臂11一端连接。机械中臂11另一端通过关节12与机械小臂13一端连接，机械小臂该端端部设置有第二步进电机14，且机械小臂13的末端连接有末端执行器8。其中，第一步进电机10和第二步进电机14均与主控制器连接，由主控制器控制步进电机动作。使用时，机械大臂9由旋转平台7带动转动，机械中臂11由第一步进电机10和机械大臂9带动，机械小臂13由机械中臂11通过关节12带动其动作，且机械小臂13在第二步进电机14的驱动下可以带动末端执行器8一同旋转。

上述实施例中，末端执行器8内还设置有驱动电机，该驱动电机与主控制器连接，由主控制器控制其动作。使用时，该驱动电机带动末端执行器8绕机械小臂13进行转动，使得机械臂结构具有5个自由度。

在一个优选的实施例中，机械臂结构还包括驱动连杆15。驱动连杆15一端与第一步进电机10连接，另一端与机械小臂13一端连接，用于辅助机械中臂11支撑、带动机械小臂13动作。

在一个优选的实施例中，位于车身1后部还设置有动力电池仓16，用于为主控系统仓4和各传感器供电。动力电池仓16内设置有大功率的锂电池，锂电池外层包裹阻燃材料。在动力电池仓16后部还设置有充电插口，以实现AGV的自动充电功能。

在一个优选的实施例中，麦克纳姆轮驱动电机采用带编码器的行星减速电机，该行星减速电机固定在车身底盘上。

上述各实施例中，超声波传感器6设置在靠近地面处，且不高过激光雷达传感器5的扫描平面，以防止在路径规划时引入噪声点。优选的，超声波传感器6由四组型号为US-100的超声波模块构成；激光雷达传感器5的型号为杉川A0602。

上述各实施例中，末端执行器8上还设置有视觉模块，以辅助机械臂结构准确抓取物料。

上述各实施例中，车身1采用碳纤维板制成，质量轻且硬度较高。

上述各实施例中，主控制器采用型号为ARM Cortex-M4FRDM K64F的单片机，同时主控系统仓4内还设置有与单片机连接的GPS模块，用于实时反馈当前AGV所在的位置。其中，单片机运行Ubuntu系统，并在linux架构中运行ROS机器人操作系统。

本发明还提供一种麦克纳姆轮智能仓储AGV路径规划方法，其包括以下步骤：

1)预先设定AGV的搬运目标地点、仓储货物的放置地点以及行进速度；

2)由主控制器构建AGV运动模型以及麦克纳姆轮运动学模型；

3)与主控制器连接的九轴陀螺仪实时测算AGV的运行速度和角速度，同时激光雷达传感器5开始采集数据并回传至主控制器；

4)主控制器分别读取多帧激光雷达数据，构建多帧子地图，并将所有子地图存入主控制器的板载非易失存储器中；

5)对存储器中的所有子地图数据进行基于子地图快速关联算法的关联分析，在此基础上对AGV的状态进行预测以及环境地图预测，结合预测结果，对环境地图进行更新或环境地图增广；

6)超声波传感器6实时检测前方是否有障碍并把结果回传至主控制器，主控制器根据更新后的环境地图和超声波传感器6回传的数据计算出AGV的运行轨迹；

7)将AGV运行轨迹代入麦克纳姆轮运动学公式，计算出各轮转向和转速值；

8)将各轮转向和转速值代入模糊PID控制器，结合行星减速电机上的编码器回传的数据，驱动行星减速电机，最终实现AGV的精准定位和快速有效的搬运。

上述步骤2)中，AGV运动模型为：

式中，o_v,t表示AGV在t时刻的位姿；L为车轮轴距；dt表示输入量的单次采样时间；v和θ表示t时刻的输入量，v表示AGV的速度，θ表示AGV的方向角，v和θ均由主控系统仓4内的九轴陀螺仪测得；x_t-1表示t-1时刻AGV相对于x方向的位姿；y_t-1表示t-1时刻AGV相对于y的位姿；θ_t-1表示t-1时刻AGV的角位姿；其中x和y方向如图2所示。

麦克纳姆轮运动学模型为：

式中，如图2所示，w₁、w₂、w₃、w₄分别表示四个麦克纳姆轮的转速，v_x表示AGV沿x方向的速度，v_y表示AGV沿y方向的速度，w表示AGV绕四轮中心的转速，L₁表示沿x方向轮子中轴线到四轮中心的距离，L₂表示沿y方向轮子中轴线到四轮中心的距离，R为轮子半径。

上述步骤5)中，如图3所示，对所有子地图数据进行基于子地图快速关联算法的关联分析，包括以下步骤：

5.1)主控制器连续读取由多帧的激光雷达数据构建成的子地图数据，单次读取的帧数一般在2至5帧左右；

子地图由多帧激光数据组成的目的在于，原始的对帧-帧数据进行关联的算法具有信息量小，匹配可信度不高的缺点且易受到噪声点的影响，故本实施例中利用激光雷达回传的多帧数据来构建子地图，提高了单次关联分析的信息量和关联可靠度；

5.2)但单纯地通过多帧数据构建子地图会使得子地图数据存在非常大的冗余性，反而会增加运算负担。因此，对每一时刻的子地图数据设定局部关联区域，具体公式如下：

式中，(x_v,y_v,θ)表示当前时刻AGV的位姿，x_v表示AGV沿x方向的位姿、y_v表示AGV沿y方向的位姿、θ表示AGV的方向角；abs表示开根方；(x_i,y_i)表示子地图上某个特征的坐标值；r为激光雷达的最大扫描距离；d为补偿距离。

由此得到的局部关联区域即为以激光雷达为圆心的半径为r+d的半圆。划分局部关联区域的目的在于对子地图进行分割，排除没有必要进行关联的数据，这样，数据关联的运算负担只于划分区域的大小有关，加快关联运算；

5.3)对t时刻的子地图划分完局部关联区域后，如图4所示，对子地图上的特征值点G_t＝{g_s|w′＝1,…m}按密度分为M组，即其中，表示t时刻的第k个小组，g_s表示第k组中的特征点，m_k表示第k组中的第m个特征点，w′表示存放特征点的变量，如图4所示的Group1，Group2,Group3,Group4；

分组的目的在于：传统的关联算法是对同一时刻采集到的所有数据点进行数据关联分析，虽然再此步骤之前进行了划分局部关联区域，但直接进行数据关联的计算复杂度很高，若将子地图中的数据按密度进行分组，对每一组单独进行数据关联分析，将大大提高运算速度；

5.4)完成局部关联区分割和数据分组后，利用联合兼容分支定界算法(JCBB)得到每组数据与全局地图Q关联的最优解U^l，最后对每组数据对应的关联最优解进行组合，得到最终的关联结果其中，u_n表示第n个最优解，t表示当前时刻；

5.5)根据关联结果，对环境地图进行更新或增广：u_n＝q表示该观测点与全局地图中第q个特征相匹配，该观测点则用于地图更新，u_n＝0表示该观测点为新特征值用于全局地图的增广。

由上述步骤可知，针对直接进行JBCC的原始算法和子地图快速关联算法进行复杂度分析可知：子地图快速关联算法的复杂度由变为其中N_i表示每一子地图划分的局部关联区域内需要与第i个已有特征进行关联分析的观测点。由于N＜n且j_n＜j，因此子地图快速关联算法降低了运算复杂度，有效的提高了运算速度，与此同时多帧子地图提供的信息较多，使算法结果更为可靠。

上述步骤8)中，利用模糊PID控制算法，实现对K_P、K_I、K_D这3个PID参数进行自行修改。其中K_P为比例常数，K_I为积分常数，K_D为微分常数，当主控制器检测到偏差值Δe以及偏差值的变化率e_r后，通过模糊PID控制算法便能实时对K_P、K_I、K_D这3个参数进行调整，以实现AGV的稳定运行。

上述各实施例仅用于说明本发明，各部件的结构、尺寸、设置位置及步骤都是可以有所变化的，在本发明技术方案的基础上，凡根据本发明原理对个别部件及步骤进行的改进和等同变换，均不应排除在本发明的保护范围之外。

Claims (10)

1.一种麦克纳姆轮智能仓储AGV路径规划方法，其特征在于包括以下步骤：

1)预先设定AGV的搬运目标地点、仓储货物的放置地点以及行进速度；

2)由AGV中的主控制器构建AGV运动模型以及麦克纳姆轮运动学模型；

3)由AGV中的与主控制器连接的九轴陀螺仪实时测算AGV的运行速度和角速度，同时由设置在AGV上的激光雷达传感器开始采集数据并回传至主控制器；

4)主控制器分别读取多帧激光雷达数据，构建多帧子地图，并将所有子地图存入主控制器的板载非易失存储器中；

5)对存储器中的所有子地图数据进行基于子地图快速关联算法的关联分析，并对AGV的状态进行预测以及环境地图预测，结合预测结果，对环境地图进行更新或环境地图增广；

6)设置在AGV上的超声波传感器实时检测前方是否有障碍并把结果回传至主控制器，主控制器根据更新后的环境地图和超声波传感器回传的数据计算出AGV的运行轨迹；

7)将AGV运行轨迹代入麦克纳姆轮运动学公式，计算出各轮转向和转速值；

8)将各轮转向和转速值代入模糊PID控制器，结合AGV麦克纳姆轮的行星减速电机上的编码器回传的数据，驱动行星减速电机，最终实现AGV的精准定位和快速有效的搬运。

2.如权利要求1所述规划方法，其特征在于：所述步骤5)中，对所有子地图数据进行基于子地图快速关联算法的关联分析，包括以下步骤：

5.1)主控制器连续读取由多帧的激光雷达数据构建成的子地图数据；

5.2)对每一时刻的子地图数据设定局部关联区域，具体公式如下：

式中，(x_v,y_v,θ)表示当前时刻AGV的位姿，x_v表示AGV沿x方向的位姿、y_v表示AGV沿y方向的位姿、θ表示AGV的方向角；abs表示开根方；(x_i,y_i)表示子地图上某个特征的坐标值；r为激光雷达的最大扫描距离；d为补偿距离；

由此得到的局部关联区域即为以激光雷达为圆心的半径为r+d的半圆；

5.3)对t时刻的子地图划分完局部关联区域后，对子地图上的特征值点G_t＝{g_s|w′＝1,…m}按密度分为M组，即其中，表示t时刻的第k个小组，g_s表示第k组中的特征点，m_k表示第k组中的第m个特征点，w′表示存放特征点的变量；

5.4)完成局部关联区分割和数据分组后，利用JCBB得到每组数据与全局地图Q关联的最优解U^l，最后对每组数据对应的关联最优解进行组合，得到最终的关联结果其中，u_n表示第n个最优解，t表示当前时刻，JCBB为联合兼容分支定界算法；

5.5)根据关联结果，对环境地图进行更新或增广：u_n＝q表示该观测点与全局地图中第q个特征相匹配，该观测点则用于地图更新，u_n＝0表示该观测点为新特征值用于全局地图的增广。

3.如权利要求1或2所述规划方法，其特征在于：所述步骤2)中，AGV运动模型为：

式中，o_v,t表示AGV在t时刻的位姿；L为车轮轴距；dt表示输入量的单次采样时间；v和θ表示t时刻的输入量，v表示AGV的速度，θ表示AGV的方向角；x_t-1表示t-1时刻AGV相对于x方向的位姿；y_t-1表示t-1时刻AGV相对于y的位姿；θ_t-1表示t-1时刻AGV的角位姿。

4.如权利要求1或2所述规划方法，其特征在于：所述步骤2)中，麦克纳姆轮运动学模型为：

式中，w₁、w₂、w₃、w₄分别表示四个麦克纳姆轮的转速，v_x表示AGV沿x方向的速度，v_y表示AGV沿y方向的速度，w表示AGV绕四轮中心的转速，L₁表示沿x方向轮子中轴线到四轮中心的距离，L₂表示沿y方向轮子中轴线到四轮中心的距离，R为轮子半径。

5.一种实现如权利要求1至4任一项所述规划方法的麦克纳姆轮智能仓储AGV，其特征在于：包括车身，位于所述车身底部两侧分别设置有一对麦克纳姆轮，位于所述车身顶部设置有机械臂固定柱和主控系统仓，所述主控系统仓位于所述机械臂固定柱后部；位于所述车身前端顶部中央位置处设置有激光雷达传感器，位于所述车身前端下部两侧还分别设置有用于AGV局部避障的超声波传感器；所述机械臂固定柱的顶部通过旋转平台与机械臂结构连接，位于所述机械臂结构的端部设置有末端执行器；

所述主控系统仓内设置有主控制器、以及与所述主控制器连接的九轴陀螺仪和麦克纳姆轮驱动电机；所述激光雷达传感器、超声波传感器和九轴陀螺仪将检测到的信号都传输至所述主控制器内，由所述主控制器控制所述旋转平台和麦克纳姆轮驱动电机动作。

6.如权利要求5所述AGV，其特征在于：所述机械臂结构包括机械大臂、机械中臂、第一步进电机、机械小臂、关节和第二步进电机；所述机械大臂一端通过所述旋转平台与所述机械臂固定柱连接，所述机械大臂另一端通过所述第一步进电机与所述机械中臂一端连接；所述机械中臂另一端通过所述关节与所述机械小臂一端连接，所述机械小臂该端端部设置有所述第二步进电机，且所述机械小臂的末端连接有所述末端执行器；所述第一步进电机和第二步进电机均与所述主控制器连接。

7.如权利要求6所述AGV，其特征在于：所述末端执行器内设置有驱动电机，该驱动电机与所述主控制器连接，该驱动电机带动所述末端执行器绕所述机械小臂转动。

8.如权利要求6或7所述AGV，其特征在于：所述机械臂结构还包括驱动连杆；所述驱动连杆一端与所述第一步进电机连接，另一端与所述机械小臂一端连接。

9.如权利要求5所述AGV，其特征在于：所述车身后部设置有动力电池仓，所述动力电池仓内设置有锂电池，所述锂电池外层包裹阻燃材料；所述动力电池仓后部还设置有充电插口。

10.如权利要求5所述AGV，其特征在于：所述麦克纳姆轮驱动电机采用带编码器的行星减速电机，该行星减速电机固定在所述车身的底盘上。