CN108508797A - 轮式机器人定位系统及其定位方法 - Google Patents

Abstract

本发明涉及轮式机器人定位系统及其定位方法，其特征在于，包括主控模块，以及与所述主控模块连接的SLAM模块、电机驱动模块、地磁传感器、陀螺仪和测速传感器；所述SLAM模块与激光雷达连接，所述电机驱动模块与各驱动轮电机连接。驱动轮有两个，分别是左驱动轮和右驱动轮，所述定位系统还包括前万向轮和后万向轮。本发明采用激光雷达、测速传感器、陀螺仪、地磁传感器，利用SLAM模块，获取机器人的姿态信息和环境信息，设计了多传感器信息融合的定位系统，实现地图绘制和定位功能，受农作物的影响较小，不必铺设导航线路。

Description

轮式机器人定位系统及其定位方法

技术领域

本发明属于农业机器人技术领域，具体涉及轮式机器人定位系统及其定位方法。

背景技术

目前的室内导航系统，有无线定位导航系统、自动寻线导航系统等。但是，在农业温室环境中，无线定位导航系统的信号传输会受到农作物的影响，自动寻线导航系统需要铺设导航线路，无法适应多变复杂的农业环境。

发明内容

为了解决上述存在的技术问题，本发明设计了轮式机器人定位系统及其定位方法，受农作物的影响较小，不必铺设导航线路，可以实现轮式机器人的在温室环境下自主定位的功能。

为了解决上述存在的技术问题，本发明采用了以下方案：

一种轮式机器人定位系统，包括主控模块，以及与所述主控模块连接的SLAM模块、电机驱动模块、地磁传感器、陀螺仪和测速传感器；所述SLAM模块与激光雷达连接，所述电机驱动模块与各驱动轮电机连接。

进一步，所述测速传感器是编码器，所述编码器与驱动轮一一对应。

进一步，驱动轮有两个，分别是左驱动轮和右驱动轮；所述定位系统还包括前万向轮和后万向轮。

进一步，所述陀螺仪采用MPU-6050模块。

进一步，所述主控模块的主体是以ARM架构的STM32F103VET6芯片。

进一步，所述主控模块还包括自主设计的电源转化电路、复位电路、时钟电路及去耦电路。

进一步，所述主控模块还包括自主设计的电源指示灯电路和蜂鸣器电路；所述蜂鸣器电路提示所述定位系统工作的相关进度。

进一步，所述主控模块、SLAM模块、电机驱动模块、激光雷达、陀螺仪和地磁传感器固定在轮式机器人的移动小车车体上。

一种轮式机器人定位方法，包括以下步骤：

S1，激光雷达扫描一个360度的二维平面地图数据并传送给SLAM模块，所述SLAM模块对该二维平面地图数据处理并提取地图中的特征点、判断机器人在环境中的位置；同时，陀螺仪采集轮式机器人与水平面的夹角信息、地磁传感器采集轮式机器人在水平面内的方向信息，并分别传送给所述SLAM模块，所述SLAM模块经过数据融合算法得到角度数据从而确定机器人的航向角；

S2，所述SLAM模块根据航向角将机器人应该向X轴向和Y轴向移动的速度量，及机器人转向时的角速度量传输给主控模块；所述主控模块根据所述SLAM模块传输来的速度量、以及各编码器、所述地磁传感器、所述陀螺仪适时采集的信息，发送电机控制信号给电机驱动模块，所述电机驱动模块驱动各驱动轮电机进行正转或反转或调速，实现机器人的前进或后退或左转前进或右转前进或左转后退或右转后退或速度调节或停止的基本运动动作。

进一步，上述S1中，所述SLAM模块将所述激光雷达扫描的二维平面地图这样的局部地图数据进行SLAM算法处理，并构建出增量式全局地图，同时确定机器人系统自身的位置；然后，所述SLAM模块通过以太网进行信号传输，将增量式全局地图和机器人系统自身位置及机器人的航向角显示在上位机上。

进一步，上述S2中，所述陀螺仪采集车轮与水平地面的夹角；当车轮因地面凹陷或凸起偏离水平位置时，所述陀螺仪采集两者之间的夹角并将数据传输到所述SLAM模块，经SLAM算法处理数据后继续传输到所述主控模块，进而由所述主控模块产生算法对轮式机器人姿态进行调整；当小车处于上坡、下坡或平路的不同路况时，所述陀螺仪采集车轮与水平地面的夹角，并将数据通过所述SLAM模块传输到所述主控模块，所述主控模块产生算法对轮式机器人的速度进行调控。

进一步，上述S2中，所述各编码器分别采集各驱动轮的速度并传送给所述主控模块，所述主控模块采用PID速度控制算法精确控制机器人各驱动轮的速度。

进一步，上述S2中，所述地磁传感器固定在轮式机器人的移动小车车体上；当移动小车需要转向时，所述地磁传感器首先记录初始的地磁传感器和地磁场方向的夹角，然后随着移动小车转向不断获取所述地磁传感器和地磁场方向之间的动态的夹角，所述适时动态的夹角与初始的夹角相减，得到移动小车转过的角度；所述主控模块产生控制机器人速度的PWM波，再通过PID方向控制算法，保证机器人以目标方向移动。

该轮式机器人定位系统及其定位方法具有以下有益效果：

（1）本发明采用激光雷达、测速传感器（编码器）、陀螺仪、地磁传感器，利用SLAM模块，获取机器人的姿态信息和环境信息，设计了多传感器信息融合的定位系统，实现地图绘制和定位功能，受农作物的影响较小，不必铺设导航线路。

（2）相较于现有的四个驱动轮农业机器人，本发明只涉及两个驱动电路，设计更加简单，避免了内耗提升了传递效率，同时，移动小车的两个驱动轮在转向时的路线可以看作是以任意一车轮为圆心的半径不一样的同心圆弧，再结合两个万向轮可使小车转向更简单。

（3）本发明系统研究了传感器融合算法和直流电机的增量式PID控制算法，编写了系统控制程序，实现了机器人的自主定位，并设计机器人行驶过程中的控制策略。最终，通过实地实验，验证了该系统能基本满足温室移动机器人的定位要求。

附图说明

图1：本发明实施方式中轮式机器人定位系统的模块化结构示意图；

图2：本发明实施方式中轮式机器人定位系统的布局结构示意图；

图3：本发明实施方式中主控模块芯片的接口示意图；

图4：本发明实施方式中电源转化电路图；

图5：本发明实施方式中复位电路图；

图6：本发明实施方式中时钟电路图；

图7：本发明实施方式中去耦电路图；

图8：本发明实施方式中SLAM模块接口示意图；

图9：本发明实施方式中部分接口示意图；

图10：本发明实施方式中电源指示电路图；

图11：本发明实施方式中蜂鸣器电路图；

图12：本发明实施方式中轮式机器人速度控制图；

图13：本发明实施方式中轮式机器人转向示意图；

图14：本发明实施方式中定位系统整体的程序流程图；

图15：本发明实施方式中SLAM模块通信程序流程图；

图16：本发明实施方式中地磁传感器通信程序流程图；

图17：本发明实施方式中速度控制程序流程图；

图18：本发明实施方式中方向控制的程序流程图；

图19：本发明实施方式中轮式机器人转向原理图。

附图标记说明：

1—主控模块；2—SLAM模块；3—电机驱动模块；41—电机Ⅰ；42—电机Ⅱ；5—激光雷达；6—陀螺仪；7—地磁传感器；81—编码器Ⅰ；82—编码器Ⅱ；91—前万向轮；92—后万向轮；101—左驱动轮；102—右驱动轮。

具体实施方式

下面结合附图，对本发明做进一步说明：

图1至图11示出了本发明轮式机器人定位系统的具体实施方式。图1是本实施方式中轮式机器人定位系统的模块化结构示意图；图2是本实施方式中轮式机器人定位系统的布局结构示意图。

如图1和图2所示，本实施方式中的轮式机器人定位系统，包括主控模块1，以及与主控模块1连接的SLAM模块2、电机驱动模块3、地磁传感器7、陀螺仪6和测速传感器；SLAM模块2与激光雷达5连接，电机驱动模块3与各驱动轮电机连接。本实施例中，主控模块1、SLAM模块2、电机驱动模块3、激光雷达5、陀螺仪6和地磁传感器7固定在轮式机器人的移动小车车体上。SLAM模块2的接口示意图如图8所示。

如图1和图2所示，本实施例中，驱动轮有两个，分别是左驱动轮101和右驱动轮102，左驱动轮101通过相应的联轴器与电机Ⅰ41连接，右驱动轮102通过相应的联轴器与电机Ⅱ42连接，电机驱动模块3与电机Ⅰ41和电机Ⅱ42连接；定位系统还包括前万向轮91和后万向轮92。

如图1和图2所示，所述测速传感器是编码器，编码器与驱动轮一一对应。本实施例中，编码器有两个，分别是编码器Ⅰ81和编码器Ⅱ82，编码器Ⅰ81及编码器Ⅱ82分别采集左驱动轮101和右驱动轮102的速度，编码器Ⅰ81及编码器Ⅱ82分别与主控系统模块1相对应的IO口线路连接。

本实施例中，陀螺仪6采用MPU-6050模块。此模块是一个6轴运动处理传感器。它集成了3轴MEMS陀螺仪， 3轴MEMS 加速度计，可以通过其 I2C接口输出一个6轴的信号，经过卡尔曼滤波算法与地磁传感器7进行整合校正，可以实时掌握机器人角度变化，同激光雷达5结合SLAM模块2确定机器人的位置及朝向。

本实施例中，主控模块1的主体是以ARM架构的STM32F103VET6芯片，该芯片的接口示意图如图3所示，图3式本实施方式中主控模块芯片的接口示意图。

主控模块1还包括自主设计的电源转化电路、复位电路、时钟电路及去耦电路。电源转化电路如图4所示，复位电路如图5所示，时钟电路如图6所示，去耦电路如图7所示。

主控模块1还包括自主设计的电源指示灯电路和蜂鸣器电路；所述蜂鸣器电路提示所述定位系统工作的相关进度。电源指示电路如图10所示，包含红色发光二极管D3的电路与主控模块1的VDD引脚连接，提示是否电源上电正常。当主控模块1正常通电，VDD引脚输出高电平，电源指示电路接通，此实红灯亮起表示电源上电正常。包含蓝色发光二极管D4、D5的电路分别与SLAM模块2的WLED引脚及MLED引脚连接，用于提示SLAM模块2是否正常工作，蓝灯亮起表示SLAM模块2正常工作。蜂鸣器电路如图11所示，提示系统工作的相关进度。蜂鸣器电路与主控模块1的BEEP_PWM引脚连接，当BEEP_PWM引脚输出高电平时，三极管截止，没有电流流过线圈；当BEEP_PWM引脚输出低电平，三极管导通。这样就在蜂鸣器两端产生了方波信号，驱动蜂鸣器发声。此时电机驱动模块3驱动电机Ⅰ41和电机Ⅱ42工作。

本实施例中，电机Ⅰ41、电机Ⅱ42为直流电机，每个电机需要3个控制信号IN1、IN2和EN1，EN1为使能信号。电机的两个电极IN1、IN2，是电机转动方向控制信号，分别为1，0时，电机正转，反之，电机反转。给EN1、EN2输入PWM信号，调整PWM的占空比，可以调整电机转速。左驱动轮101通过联轴器与电机Ⅰ41连接，右驱动轮102通过联轴器与电机Ⅱ42连接，主控模块1发送电机控制信号给电机驱动模块3，电机驱动模块3驱动电机Ⅰ41和电机Ⅱ42,进行正转、反转或调速，实现轮式机器人的前进、后退、左转前进、右转前进、左转后退、右转后退、速度调节和停止等基本运动动作。

工作时，如图14至图18，图14是本实施方式中定位系统整体的程序流程图；图15是本实施方式中SLAM模块通信程序流程图；图16是本实施方式中地磁传感器通信程序流程图；图17是本实施方式中速度控制程序流程图；图18是本实施方式中方向控制的程序流程图。

该轮式机器人定位系统的定位方法，包括以下步骤：

S1，激光雷达5扫描一个360度的二维平面地图数据并传送给SLAM模块2， SLAM模块2对该二维平面地图数据处理并提取地图中的特征点、判断机器人在环境中的位置；同时，陀螺仪6采集轮式机器人与水平面的夹角信息、地磁传感器7采集轮式机器人在水平面内的方向信息，并分别传送给SLAM模块2， SLAM模块2经过数据融合算法得到角度数据从而确定机器人的航向角；

S2，SLAM模块2根据航向角将机器人应该向X轴向（左右向）和Y轴向（前后向）移动的速度量，及机器人转向时的角速度量传输给主控模块1；主控模块1根据SLAM模块2传输来的速度量、以及各编码器、地磁传感器7、陀螺仪6适时采集的信息，发送电机控制信号给电机驱动模块3，电机驱动模块3驱动各驱动轮电机进行正转或反转或调速，实现机器人的前进或后退或左转前进或右转前进或左转后退或右转后退或速度调节或停止的基本运动动作。

优选地，上述S1中， SLAM模块2将激光雷达5扫描的二维平面地图这样的局部地图数据进行SLAM算法处理，并构建出增量式全局地图，同时确定机器人系统自身的位置；然后， SLAM模块2通过以太网进行信号传输，将增量式全局地图和机器人系统自身位置及机器人的航向角显示在上位机上，如图15所示。

优选地，上述S2中，主控模块1根据SLAM模块2传输来的移动机器人的X轴向速度量、Y轴向速度量和角速度量，输出移动机器人将要前进的方向量和左右驱动轮轮的线速度量，具体运动合成如图12、13所示。各编码器分别采集各驱动轮的速度并传送给主控模块1，主控模块1采用PID速度控制算法精确控制机器人各驱动轮的速度，如图17所示；地磁传感器7固定在轮式机器人的移动小车车体上；当移动小车需要转向时，地磁传感器7首先记录初始的地磁传感器7和地磁场方向的夹角，然后随着移动小车转向不断获取地磁传感器7和地磁场方向之间的动态的夹角，适时动态的夹角与初始的夹角相减，得到移动小车转过的角度，如图16所示。主控模块1产生控制机器人速度的PWM波，再通过PID方向控制算法，保证机器人以目标方向移动， 如图18所示。

优选地，上述S2中，陀螺仪6采集车轮与水平地面的夹角；当车轮因地面凹陷或凸起偏离水平位置时，陀螺仪6采集两者之间的夹角并将数据传输到SLAM模块2，经SLAM算法处理数据后继续传输到主控模块1，进而由主控模块1产生算法对轮式机器人姿态进行调整；当小车处于上坡、下坡或平路的不同路况时，陀螺仪6采集车轮与水平地面的夹角，并将数据通过SLAM模块2传输到主控模块1，主控模块1产生算法对轮式机器人的速度进行调控。

本发明采用激光雷达、编码器、陀螺仪、地磁传感器，设计了多传感器信息融合的定位系统，实现地图绘制和导航功能，受农作物的影响较小，不必铺设导航线路。

本发明可以在上位机上显示地图，具有定位、目的地选择、路径计算功能模块。首先，采用激光雷达对机器人周围的地图进行扫描。然后，采用SLAM模块对地图数据进行处理。陀螺仪选用MPU-6050模块，此模块是一个6轴运动处理传感器。它集成了3轴MEMS陀螺仪， 3轴MEMS 加速度计，以及一个可扩展的数字运动处理器，可用 I2C 接口连接地磁传感器。扩展之后就可以通过其 I2C接口 输出一个6轴的信号。经过卡尔曼滤波算法与地磁传感器进行整合校正，可以实时掌握机器人角度变化，同激光雷达结合SLAM模块确定机器人的位置及朝向。接着，将地图显示在上位机软件上，用户可直接在上位机界面上设置机器人移动的目标点。随后，SLAM模块对小车的路径进行计算，采用编码器采集机器人的速度，陀螺仪、地磁传感器采集机器人自身的方向，主控系统产生控制机器人速度的PWM波，并通过PID算法，调整机器人的运动速度和方向。主控系统将SLAM模块的数据信息，与采集的机器人姿态数据进行融合，指导机器人在温室环境中导航。

针对温室中的轮式作业机器人，本发明设计了一种轮式机器人定位系统，完成了系统电路和运动控制程序的设计，实现了轮式机器人自主定位的功能，受农作物的影响较小，不必铺设导航线路。

相较于现有的四个驱动轮农业机器人，本发明只涉及两个驱动电路，设计更加简单，避免了内耗提升了传递效率，同时，移动小车的两个驱动轮在转向时的路线可以看作是以任意一车轮为圆心的半径不一样的同心圆弧，如图19所示，再结合两个万向轮可使小车转向更简单。

本发明系统研究了传感器融合算法和直流电机的增量式PID控制算法，编写了系统控制程序，实现了机器人的自主定位，并设计机器人行驶过程中的控制策略。最终，通过实地实验，验证了该系统能基本满足温室移动机器人的定位要求。

上面结合附图对本发明进行了示例性的描述，显然本发明的实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围内。

Claims (10)

1.一种轮式机器人定位系统，其特征在于，包括主控模块，以及与所述主控模块连接的SLAM模块、电机驱动模块、地磁传感器、陀螺仪和测速传感器；所述SLAM模块与激光雷达连接，所述电机驱动模块与各驱动轮电机连接。

2.根据权利要求1所述的轮式机器人定位系统，其特征在于，所述测速传感器是编码器，所述编码器与驱动轮一一对应。

3.根据权利要求1或2所述的轮式机器人定位系统，其特征在于，驱动轮有两个，分别是左驱动轮和右驱动轮；所述定位系统还包括前万向轮和后万向轮。

4.根据权利要求1至3任一所述的轮式机器人定位系统，其特征在于，所述主控模块的主体是以ARM架构的STM32F103VET6芯片；所述主控模块还包括电源转化电路、复位电路、时钟电路、去耦电路电源指示灯电路和蜂鸣器电路；所述蜂鸣器电路提示所述定位系统工作的相关进度。

5.根据权利要求1至4任一所述的轮式机器人定位系统，其特征在于，所述主控模块、SLAM模块、电机驱动模块、激光雷达、陀螺仪和地磁传感器固定在轮式机器人的移动小车车体上。

6.一种轮式机器人定位方法，其特征在于，包括以下步骤：

S1，激光雷达扫描一个360度的二维平面地图数据并传送给SLAM模块，所述SLAM模块对该二维平面地图数据处理并提取地图中的特征点、判断机器人在环境中的位置；同时，陀螺仪采集轮式机器人与水平面的夹角信息、地磁传感器采集轮式机器人在水平面内的方向信息，并分别传送给所述SLAM模块，所述SLAM模块经过数据融合算法得到角度数据从而确定机器人的航向角；

S2，所述SLAM模块根据航向角将机器人应该向X轴向和Y轴向移动的速度量，及机器人转向时的角速度量传输给主控模块；所述主控模块根据所述SLAM模块传输来的速度量、以及各编码器、所述地磁传感器、所述陀螺仪适时采集的信息，发送电机控制信号给电机驱动模块，所述电机驱动模块驱动各驱动轮电机进行正转或反转或调速，实现机器人的前进或后退或左转前进或右转前进或左转后退或右转后退或速度调节或停止的基本运动动作。

7.根据权利要求6所述的轮式机器人定位方法，其特征在于，上述S1中，所述SLAM模块将所述激光雷达扫描的二维平面地图这样的局部地图数据进行SLAM算法处理，并构建出增量式全局地图，同时确定机器人系统自身的位置；然后，所述SLAM模块通过以太网进行信号传输，将增量式全局地图和机器人系统自身位置及机器人的航向角显示在上位机上。

8.根据权利要求6或7所述的轮式机器人定位方法，其特征在于，上述S2中，所述陀螺仪采集车轮与水平地面的夹角；当车轮因地面凹陷或凸起偏离水平位置时，所述陀螺仪采集两者之间的夹角并将数据传输到所述SLAM模块，经SLAM算法处理数据后继续传输到所述主控模块，进而由所述主控模块产生算法对轮式机器人姿态进行调整；当小车处于上坡、下坡或平路的不同路况时，所述陀螺仪采集车轮与水平地面的夹角，并将数据通过所述SLAM模块传输到所述主控模块，所述主控模块产生算法对轮式机器人的速度进行调控。

9.根据权利要求6或7所述的轮式机器人定位方法，其特征在于，上述S2中，所述各编码器分别采集各驱动轮的速度并传送给所述主控模块，所述主控模块采用PID速度控制算法精确控制机器人各驱动轮的速度。

10.根据权利要求6至9任一所述的轮式机器人定位方法，其特征在于，上述S2中，所述地磁传感器固定在轮式机器人的移动小车车体上；当移动小车需要转向时，所述地磁传感器首先记录初始的地磁传感器和地磁场方向的夹角，然后随着移动小车转向不断获取所述地磁传感器和地磁场方向之间的动态的夹角，所述适时动态的夹角与初始的夹角相减，得到移动小车转过的角度；所述主控模块产生控制机器人速度的PWM波，再通过PID方向控制算法，保证机器人以目标方向移动。