一种基于麦克纳姆轮自主导航全向车控制系统

技术领域

本发明涉及车间物流搬运与装配技术领域，具体涉及一种基于麦克纳姆轮自主导航全向车控制系统。

背景技术

航天器的装配技术是航天制造技术的重要组成部分,航天产品总装过程中正向自动化、数字化装配方向发展，传统的装配技术主要以人来完成，劳动成本高生产效率低；随着航天产品的批量化，简单模式下的人工装配越来越不适用与现代装配的要求。

麦克纳姆轮，作为一种全方位轮，通过四个轮子的转速转向适当组合，可以实现机器人在平面上三自由度的全方位移动，目前已广泛应用于军工、制造业、电线电缆车间、医院等行业中。麦克纳姆轮式AGV，利用其灵活的运动特性，在自动导引(循迹)过程中可以实现直角转弯，侧向移动的功能，较之传统的AGV小车，节省了空间，提高了物流效率。

AGV传统的导航方式主要为磁导航和视觉导航，由地面上铺设的磁条或色带作为参考，AGV在行进过程中不断动态修正其与预定轨迹的偏差。然而这种导航方式，虽然使得AGV可以不受人的控制自动行走，但固定的轨迹极大地限制了运动，尤其对于麦克纳姆轮式AGV而言，其全方位移动的特点无法发挥出来，因此，近年来，无轨导航技术的发展十分迅速，在某些领域已有实际的应用案例。

一般基于麦克纳姆轮全向转运车应用在数字化总装脉动生产线、数字化柔性装配工作站等项目中，能够在狭窄空间进行无线遥控全向移动，同时在调度管理系统路径规划下完成部件、成件和产品的自主导航转运、自动与工位对接等任务，能够提高生产装配的效率和降低劳动成本。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：如何设计一种基于麦克纳姆轮自主导航全向车控制系统，完成产品及零部件的自动转运及与装配工位自动对接，同时在狭窄空间采用无线遥控技术实现手动转运的功能。

(二)技术方案

为了解决上述技术问题，本发明提供了一种基于麦克纳姆轮自主导航全向车控制系统，包括：供电系统1、车体运动控制系统2、主控调度系统3、车体无线通讯模块、无线遥控器6、驱动系统7、自主导航系统8、自动对接系统9和安全防撞系统10；所述车体无线通讯模块包括主控无线通讯模块4和遥控无线通讯模块5；

所述供电系统1包括：电池组、逆变电源和直流转换电源；所述电池组与逆变电源电连接，逆变电源用于驱动系统7中四组伺服驱动器的供电；电池组与直流转换电源电连接，直流转换电源用于车体运动控制系统2、自主导航系统8、自动对接系统9和安全防撞系统10的供电；

所述车体运动控制系统2包括伺服运动控制器和信号扩展模块，所述信号扩展模块用于通过主控无线通讯模块4接收主控调度系统3的路径命令信息、自主导航系统8的反馈数据信息、自动对接系统9的反馈数据信息以及安全防撞系统10的反馈数据信息；所述伺服运动控制器用于对信号扩展模块接收的反馈数据信息进行处理，向驱动系统7实时发送执行命令信息，控制全向转运车的运动；

所述主控调度系统3包括上位机调度界面和无线收发通讯模块，主控调度系统3与主控无线通讯模块4连接实现与车体实时通信；上位机调度界面用于实现对全向转运车进行路线设定，完成多台全向转运车的调度规划；

所述主控无线通讯模块4用于主控调度系统3与车体运动控制系统2通信；

所述无线遥控器6通过遥控无线通讯模块5与车体运动控制系统2实现通信，用于手动控制全向转运车运动，实现的运动方向有前进、后退、左移、右移和原地旋转；

所述驱动系统7包括四组伺服驱动器、四组驱动电机，每组伺服驱动器与一驱动电机对应电连接，车体运动控制系统2分别与四组伺服驱动器连接，并向伺服驱动器发送运动指令同时接收伺服驱动器的内部编码器反馈信号，伺服驱动器用于按照运动指令中的给定速度和方向控制驱动电机进行转动，驱动电机通过减速器与全向转运车的车轮连接；

所述自主导航系统8包括图像采集模块、信号处理模块、颜色带、二维矩阵码和减速控制码；其中图像采集模块安装在车体中心位置，每间隔一定时间采集地面上颜色带、二维矩阵码或减速控制码的信息，颜色带、二维矩阵码和减速控制码铺设在地面上，车体直行时图像采集模块扫描颜色带的位置信息，在减速区扫描减速控制码的减速站点号信息，在转弯处扫描二维矩阵码的位置信息，信号处理模块对图像采集模块的扫描信息进行处理，即，将图像信息转换为二进制数字信息，并通过RS485协议向车体运动控制系统2发送处理后的数据，车体运动控制系统2对数据进行解算后得到车体的姿态信息、减速命令信息或转弯位置信息，其中姿态信息包括左右偏移值和偏转角度，减速命令信息包括规定减速站点号信息，转弯位置信息包括前后、左右坐标值和偏转角度；

当全向转运车导航直行时图像采集模块把采集的图像信息传送给车体运动控制系统2，车体运动控制系统2把解算后得到的车体中心左右偏移值d₁和偏转角β₁，与存储在车体运动控制系统2中作为给定值的左右偏移值d₂和偏转角β₂进行比较得出：d＝d₁-d₂,β＝β₁-β₂，其中d为左右偏差值，β为角度偏差值，进而对左右偏差值和角度偏差值分别进行PID控制；

当全向转运车进行转弯或停止时，图像采集模块首先检测到减速控制码的减速站点号信息，车体运动控制系统2通过减速站点号信息进行判断，如果此减速站点号信息为转弯或停止站点时，车体开始减速，直到图像采集模块检测到二维矩阵码的中心坐标原点位置时，车体停止或进行原地转弯；

所述自动对接系统9包括三个激光位移传感器和三个挡板，挡板安装在装配工位下方，其中第一、第二激光位移传感器安装在车前端两侧，激光检测方向为车体前进方向，第一、第二激光位移传感器分别检测自身到挡板的距离分别为d₁和d₂，第三激光位移传感器安装在车体前端中心位置，激光检测方向为车体横向方向，第一、第二激光位移传感器检测自身到对应挡板的距离为d₃，三个激光位移传感器测量的距离值反馈到车体运动控制系统2中；全向转运车到达装配工位站点后，第一、第二激光位移传感器测量前方距离为100mm时减速停止，开始与装配工位自动对接，车体运动控制系统2通过距离值计算得到Δd＝d₁-d₂为车体前端左右偏转误差，Δd₁＝d₃-d₄为车体前端左右横向偏移误差，其中d₄＝100mm为横向设定值，把Δd和Δd₁作为测量偏差值分别进行自动对接PID控制；

所述安全防撞系统10包括两个安全扫描仪，安装在车体前后两端，用于检测行进中前后方2米范围内是否有障碍物，当前方2米处出现障碍物时，安全扫描仪反馈信号至车体运动控制系统2中，车体运动控制系统2控制全向转运车开始减速，当检测到距离为1m时，控制全向转运车开始停止运动，直到障碍物消除控制全向转运车开始继续运动。

优选地，所述车体运动控制系统2通过以下方式对左右偏差值和角度偏差值分别进行PID控制：当-5mm<d<5mm时，左右偏差不调节；当-35mm<d<35mm时，进行左右PID控制输出不同的车体横向速度v_x，控制d在±5mm范围内；当d>35mm或d<-35mm时，车体超出轨道，停止运行，当β＝0°时，不进行车体角度调节，当β>0°或β<0°时，进行角度PID控制，输出相应的自转角速度ω，根据车体速度与每个车轮角速度的关系，计算出每个车轮调节的速度，进而调节角度偏差，当车体的d和β都调节时，同时进行PID控制，车体进行复合运动调节。

优选地，所述车体运动控制系统2进行自动对接PID控制的原理是：首先，车体进行原地调节姿态，把Δd和Δd₁的误差同时控制在±2mm内，当Δd>2mm或Δd<-2mm时，设定以车体前端中心点为转弯原点进行旋转调节，此时车体前端车轮角速度远小于后端车轮角速度(差值大于预设阈值)，当Δd₁>2mm或Δd₁<-2mm时，进行左右横向调节；其次，车体进行向前移动同时进行实时姿态调节，保证Δd和Δd₁的误差在±0.5mm内，调节原理与原地调节姿态一致，当第一、第二激光位移传感器测量前方距离变为70mm时，车体减速停止，车体与装配工位接触，完成自动对接任务。

优选地，所述电池组由8组6V电池串联组成，电源容量共计48V/200AH。

优选地，所述逆变电源输出端输出220V交流电。

优选地，所述直流转换电源输出24V直流电。

优选地，所述主控无线通讯模块4为无线WIFI模块。

优选地，所述信号扩展模块用于通过主控无线通讯模块4以Modbus Tcp协议接收主控调度系统3的路径命令信息，通过RS485通讯协议接收自主导航系统8的反馈数据信息，通过模拟量接口接收自动对接系统9的反馈数据信息，通过数字量接口接收安全防撞系统10的反馈数据信息。

优选地，所述无线遥控器6通过遥控无线通讯模块5以RS232协议与车体运动控制系统2实现通信。

优选地，图像采集模块离地高度为100mm，每间隔20ms采集地面上颜色带。

(三)有益效果

本发明采用车体伺服运动控制系统结合视觉导航定位技术和自动对接系统实现车体按照主控调度系统设定路线，完成产品及零部件的自动转运及与装配工位自动对接，同时在狭窄空间采用无线遥控技术实现手动转运的功能。

附图说明

图1是本发明的控制系统总体结构图；

图2是本发明的车体运动控制原理图；

图3是本发明的导航控制算法原理图；

图4是本发明的全向转运车路径设置原理图；

图5是本发明的自动对接控制原理图。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

如图1所示，本发明所述的一种麦克纳姆轮全向自主导航转运车(全向转运车)控制系统，全向转运车采用麦克纳姆轮设计，前轮具有悬挂系统，转运车上端连接带有导轨的托架，托架上端载有转运的产品和零部件，全向自主转运车控制系统采用车体伺服运动控制系统结合视觉导航定位系统和自动对接系统实现车体按照主控调度系统设定路线，完成产品及零部件的自动转运及与装配工位自动对接，同时在狭窄空间采用无线遥控系统实现手动转运的功能。该控制系统包括：供电系统1、车体运动控制系统2、主控调度系统3、车体无线通讯模块(包括主控无线通讯模块4和遥控无线通讯模块5)、无线遥控器6、驱动系统7、自主导航系统8、自动对接系统9、安全防撞系统10。

所述供电系统1包括：电池组、逆变电源、直流转换电源、电量管理模块；其中电池组由8组6V电池串联组成，电源容量共计48V/200AH；电池组与逆变电源电连接，逆变电源输出端输出220V交流电，用于驱动系统7中四组伺服驱动器的供电；电池组与直流转换电源电连接，直流转换电源输出24V直流电，用于车体运动控制系统2、自主导航系统8、自动对接系统9、安全防撞系统10的供电。

所述车体运动控制系统2包括伺服运动控制器和信号扩展模块，其中信号扩展模块用于通过主控无线通讯模块4(无线WIFI模块)以Modbus Tcp协议接收主控调度系统3的路径命令信息，用于通过RS485通讯协议接收自主导航系统8的反馈数据信息，用于通过模拟量接口接收自动对接系统9的反馈数据信息，用于通过数字量接口接收安全防撞系统10的反馈数据信息；伺服运动控制器对信号扩展模块接收的反馈信息进行处理，向驱动系统7实时发送执行命令信息，控制全向转运车的运动。

所述主控调度系统3包括上位机调度界面和无线收发通讯模块，主控调度系统3通过通讯模块与主控无线通讯模块4连接实现与车体实时通信；上位机调度界面实现对全向转运车路线设定，完成多台全向转运车的调度规划。

所述主控无线通讯模块4用于主控调度系统3与车体运动控制系统2通信，采用无线WIFI模块，通信协议为Modbus Tcp协议。

所述无线遥控器6通过遥控无线通讯模块5与车体运动控制系统2实现通信，通信协议为RS232协议，用于手动控制全向转运车运动，实现的运动方向有前进、后退、左移、右移、原地旋转等。

所述驱动系统7包括四组伺服驱动器、四组驱动电机，每组伺服驱动器与一驱动电机对应电连接，车体运动控制系统2分别与四组伺服驱动器连接，并向伺服驱动器发送运动指令同时接收伺服驱动器的内部编码器反馈信号，伺服驱动器按照运动指令中的给定速度和方向控制驱动电机进行转动，驱动电机通过减速器与全向转运车的车轮连接。

所述自主导航系统8包括图像采集模块、信号处理模块、颜色带、二维矩阵码、减速控制码，其中图像采集模块安装在车体中心位置，离地高度100mm，每间隔20ms采集地面上颜色带、二维矩阵码或减速控制码的信息，颜色带、二维矩阵码和减速控制码铺设在地面上，其中颜色带为蓝色，宽度为25mm，车体直行时图像采集模块扫描颜色带的位置信息，在减速区扫描减速控制码的减速站点号信息，在转弯处扫描二维矩阵码的位置信息，信号处理模块对图像采集模块的扫描信息进行处理(将图像信息转换为为二进制数字信息)并通过RS485协议向车体运动控制系统2发送处理后的数据，车体运动控制系统2对数据进行解算后得到车体的姿态信息、减速命令信息或转弯位置信息，其中姿态信息包括左右偏移值和偏转角度，减速命令信息包括规定减速站点号信息，转弯位置信息包括前后、左右坐标值和偏转角度；

全向转运车的运动原理基于麦克纳姆轮结构建立的数学模型。车轮半径为r，四个轮子(依次为前左、前右、后左、后右)角速度分别为ω1、ω2、ω3、ω4，车体自转角速度为ω，车体前进速度为v_y、车体横向速度为v_x，车体中心到车轮中心的前后距离为l_y，车体中心到车轮中心横向距离为l_x，经过推理可以得出公式1中车轮的角速度ω1、ω2、ω3、ω4与车体自转角速度ω，车体前进速度v_y、车体横向速度v_x的关系，因此要改变车体中任意方向速度，可以通过公式(1)计算出四个车轮角速度即可调节车体任意方向姿态。

当全向转运车导航直行时图像采集模块把采集的图像信息传送给车体运动控制系统2，车体运动控制系统2把解算后得到的车体中心左右偏移值d₁和偏转角β₁(图像采集模块的反馈值)，与作为给定值的、存储在车体运动控制系统2中的左右偏移值d₂和偏转角β₂进行比较得出：d＝d₁-d₂,β＝β₁-β₂，其中d为左右偏差值，β为角度偏差值，进而对左右和角度分别进行PID控制。根据车体稳定性分析，采用带死区PID控制，即当-5mm<d<5mm时，左右偏差不需调节；当-35mm<d<35mm时，进行左右PID控制输出不同的v_x，控制d在±5mm范围内；当d>35mm或d<-35mm时，车体超出轨道，停止运行。当β＝0°时，不进行车体角度调节，当β>0°或β<0°时，进行角度PID控制，输出相应的自转角速度ω，根据车体速度与每个车轮角速度的关系，计算出每个车轮调节的速度，进而调节角度偏差。当车体的d和β都调节时，可以同时进行PID控制，车体进行复合运动调节。

当全向转运车进行转弯或停止时，图像采集模块首先检测到减速控制码的减速站点号信息，车体运动控制系统2通过的减速站点号信息进行判断，如果此的减速站点号信息为转弯或停止点时，车体开始减速，直到图像采集模块检测到二维矩阵码的中心坐标原点位置时，车体停止或进行原地转弯。

所述自动对接系统9包括三个激光位移传感器和三个挡板(安装在装配工位下面)，其中第一、第二激光位移传感器安装在车前端两侧，激光检测方向为车体前进方向，第一、第二激光位移传感器分别检测自身到对应挡板的距离分别为d₁和d₂，第三激光位移传感器安装在车体前端中心位置，激光检测方向为车体横向方向，第一、第二激光位移传感器检测自身到对应挡板的距离为d₃，激光位移传感器测量的距离值反馈到车体运动控制系统2中；全向转运车到达装配工位站点后，第一、第二激光位移传感器测量前方距离为100mm时减速停止，开始与装配工位自动对接，要求车体上端导轨与装配工位导轨左右横向精度为±0.5mm，左右偏转精度±0.5mm，通过距离值计算得到Δd＝d₁-d₂为车体前端左右偏转误差，Δd₁＝d₃-d₄为车体前端左右横向偏移误差，其中d₄＝100mm为横向设定值，把Δd和Δd₁作为测量偏差值分别进行PID控制，自动对接PID控制的原理是：首先，车体进行原地调节姿态，把Δd和Δd₁的误差同时控制在±2mm内，当Δd>2mm或Δd<-2mm时，设定以车体前端中心点为转弯原点进行旋转调节，此时车体前端车轮角速度远小于后端车轮角速度，这样保证车体前端摆动幅度小，易于偏转精度控制，当Δd₁>2mm或Δd₁<-2mm时，还进行左右横向调节；其次，车体进行缓慢向前移动时进行实时姿态调节，保证Δd和Δd₁的误差在±0.5mm内，调节原理与原地调节相同，当第一、第二激光位移传感器测量前方距离变为70mm时，车体减速停止，车体轨与装配工位接触，完成自动对接任务。

所述安全防撞系统10包括2个安全扫描仪，安装在车体前后两端，用于检测行进中前后方2米范围内是否有障碍物，当前方2米处出现障碍物时，安全扫描仪反馈信号到车体运动控制系统2中，控制全向转运车开始减速，当检测距离为1m处时，全向转运车开始停止运动，直到障碍物消除全向转运车才开始继续运动。

所述车体运动控制原理图2和自主导航控制算法原理如图3所示，根据车体运动控制原理及自主导航系统反馈角度及左右偏移值计算得出实际车轮调节参数，全向转运车要实现沿导航颜色带运动并实时调节车体姿态，需要通过实时测量铺设地面颜色条与车体中心的相对位置，计算出左右偏移值d和角度偏移值β，再根据PID算法得出横向速度v_x和角速度ω的值，进而实时推算出轮子的角速度ω1、ω2、ω3、ω4，从而可以实现沿设定路线自主移动。

所述全向转运车路径设置原理如图4所示，是根据现场实际路线要求设定的转弯和停靠站点，要求转弯为原地旋转90°，转弯处铺设XY矩阵坐标二维码；停靠站点和转弯处附件铺设识别二维码，用于车体到达此处进行减速。

所述车体与装配工位自动对接原理如图5所示，是全向转运车到达装配工位处进行自动对接，其原理是在车架前端安装三个激光位移传感器，其中d₁为第一激光位移传感器测量车架前端面到工位的距离，d₂为第二激光位移传感器测量车架前端面到工位的距离，d₃为第三激光位移传感器测量车架到工位侧面偏移的距离，把偏移值经过数据处理反馈到车体运动控制系统2中，实时调节车体姿态偏差。

本发明具体实施步骤如图2-5所示:

1)启动转运车总开关，供电系统1向车体运动控制系统2、驱动系统7、自主导航系统8、自动对接姿态9、安全防撞系统10供电，运动控制器进行初始化，驱动系统车轮电机使能；

2)运动模式切换成手动操作模式，打开无线遥控器6开关，并与车体建立无线通信；把转运车手动移动到导航初始位置1号站点(图3中所述)；

3)运动模式切换到导航模式，打开主控调度系统3的上位机界面，选择起点、终点，设置导航运动速度，点击启动进行自主导航运动；

4)全向转运车按照地面铺设颜色带路线运动，自主导航系统8实时反馈车体中心偏移值d和偏转角β(图2中所示)通过RS485协议发送到车体运动控制系统2，车体ν_y为前进速度，作为固定值输出，运动控制器经过PID算法(图3中所示)计算出车体的ν_x、ω，进而计算出4组车轮不同的角速度；

5)在自主导航转运过程中，安全防撞系统10实时监测车体前后端是否有障碍物，当前后方2米处检测到障碍物时转运车开始减速，当前后方1米处检测到障碍物时转运车停止，直到障碍物移除后才能恢复导航运动；

6)当导航路径终点为5号或15号时，转运车自主导航到装配工位2米处进行减速，第一、二、第三激光位移传感器实时测量前端距离(图5中所示)，当第一、二、第三激光位移传感器同时测量到数据时，转运车停止运动；

7)全向转运车进行自动对接控制，对接原理如图5所示，首先进行原地调节，使偏差值控制±2mm内，然后车体缓慢前进同时实时姿态调节，控制精度保证在±0.5mm内，直到车体上导轨与装配工位导轨对齐；

8)当导航路径终点为停靠站点时，不需启动自动对接程序，到达附近先减速再停止即可。

9)当达到终点后，主控调度系统3显示已完成导航任务，如需继续进行导航，只要选择下一终点即可。

可以看出，本发明具有以下优点：

1)基于麦克纳姆轮全向转运车不仅可以通过无线遥控器实现远距离狭窄空间全向运动，而且在自动化装配生产线中作为自动物流配送完成产品和零部件从库房到装配工位的自动转运；

2)全向转运车通过视觉导航控制系统完成转运车按照主控调度设定路线前进、原地转弯，站点停靠，在载重2t下导航控制精度保证在5mm以内，运行平稳可靠；

3)转运车前后安全防撞系统能够实时监测前后方的障碍物，遇到障碍物能够实现紧急减速和停止，提高了自动转运时的安全性和可靠性；

4)转运车在装配工位处能够通过实时测量前方距离，进行闭环姿态调节，实现车体与装配工位自动对接，使得对接控制精度相比手动对接大大提高，快速完成高精度自动对接，对接最高精度为0.2mm，对接时间在2min内；

5)全向转运车在生产装配过程中缩短了转运时间、提高了生产效率，降低人力成本。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。