用于电测实验室仪表校验的AGV小车驱动电路及其驱动方法与流程

本发明涉及agv车辆技术领域，更具体地，涉及一种用于电测实验室仪表校验的agv小车驱动电路及其驱动方法。

背景技术：

电网企业地市供电局的电测实验室，是供电局各类电气测量仪表的最高计量标准，主要负责各类仪表的量值传递工作。每年需要定期检定的仪表，不仅数量庞大，而且类型也非常多。江门供电局电测实验室每年的仪表检定工作超过2500项。主要的仪表类型包括各类电测仪表、接地电阻表、绝缘电阻表、电压监测仪、频率表、电能质量监测终端、直流电桥等等共18种。

日常的仪表定期检定工作，主要流程包括：1.送检仪表的签收及台账录入；2.将待检仪表分类存放至待检区；3.将各类待检仪表搬运至对应的校验工位；4.对仪表进行校验；5.将已检仪表分类存放至已检区；6.已检仪表交付出库。

由于整个流程还未实现智能化管理，对仪表的查找、清点、分类、签收和搬运都需要试验人员全程参与。特别是仪表的多次搬运，需要消耗大量人力和时间。如果搬运时发生仪表掉落，不仅会对仪表造成损坏，还可能伤害到试验人员。另外，在仪表校验过程中，还存在待检仪表与已检仪表摆放杂乱无章的现象，导致试验人员需要花费大量时间进行仪表的查找和清点，而且容易造成送检仪表、已检仪表的丢失、漏试或者重复试验，最终造成仪表校验工作效率低下问题，与精益化管理的要求严重不符。仪表校验过程中，仪表需要在待检区、检定工位和已检区之间经过多次搬运。

第一次搬运：对送检仪表进行签收和分类，送检仪表进入待检状态后搬运至待检区(按照检定规程要求，仪表进入实验室环境后，待检时间不能低于24小时)。

第二次搬运：试验人员对仪表进行校验时，待检仪表需要从待检区搬运至对应校验工位上。

第三次搬运：校验完毕后，已检仪表需要再从工位上搬运至已检区。

由于整箱的仪表重量可能达到10kg至20kg，而箱子不易于单人搬抬，需要2人甚至多人协助搬运，既耗费时间，也不符合人机功效，且需要人工对仪表进行清点和筛选。对于日常生产的时效性非常差，影响了工作效率。当务之急就是实现仪表的智能化分类输送和分区管理。

要提高班组生产力，就要解决仪表的搬运问题，将试验人员从繁重的体力劳动中解放出来。如果能够借助高新技术和全新的管理理念，实现仪表分类输送和分区存放的智能化管理，将从根本上降低校验人员的劳动强度。

目前，随着无人驾驶技术和设备相关技术的提高，自动驾驶小车(agv)已经开始在工业的各个领域得到应用。agv小车的智能化特性，同样可以应用到电测实验室的仪表校验工作中，帮助校验人员分担仪表分类和输送工作。

技术实现要素：

本发明为克服上述现有技术所述的至少一种缺陷，提供一种用于电测实验室仪表校验的agv小车驱动电路及其驱动方法。

本发明旨在至少在一定程度上解决上述技术问题。

本发明的首要目的是提供一种较低成本的agv小车的驱动电路及驱动方法，实现自动完成电测实验室的仪表智能化输送。

为解决上述技术问题，本发明的技术方案如下：

一种用于电测实验室仪表校验的agv小车驱动电路，包括小车，还包括电源电路、地磁传感器hmc5883、姿态测量系统mpu-6050、cpu1、cpu2和电机驱动电路，其中：

地磁传感器hmc5883与姿态测量系统mpu-6050相连，姿态测量系统安装小车的车体平板下方，与小车的车轴在同一垂线上，姿态测量系统mpu-6050与cpu1相连，cpu1完成车体姿态测量和传感器处理工作，通过串口与cpu2相连；

电源电路分别与地磁传感器hmc5883、姿态测量系统mpu-6050、cpu1、cpu2和电机驱动电路相连；

电机驱动电路与cpu2相连。

优选地，其中cpu1和cpu2互为备份，当其中一个因外因不能正常工作时，另外一个便独自承担起系统的所有测量和控制任务。

优选地，电源电路包括第一电压转换电路和第二电压转换电路，其中：

第二电压转换电路包括高质电压转换电路和粗糙电压转换电路，第一转换电路的输入为24v锂电池，其输出分别接高质电压转换电路的输入端和粗糙电压转换电路的输入端，高质电压转换电路的输出端接姿态测量系统mpu-6050和地磁传感器hmc5883，粗糙电压转换电路的输出端接cpu1、cpu2和电机驱动电路，由于姿态测量系统对电压品质要求较高，粗糙的电源品质会对测量的精度产生影响。因此在主控电路中通过两颗asm-1117提供了2部分3.3v电源，一路专门为姿态传感器提供质量较高的3.3v电源，第二路为系统其它电路提供3.3v电源，避免了电源交叉影响，给姿态解算带来了更高的精确度。

优选地，第一电压转换电路包括lm2596s芯片、电解电容c9、稳压二极管d8、电感l1、电解电容c10和电容c11，其中：

lm2596s芯片的第1端口vin分别接24伏锂电池vccbtin和电解电容c9的阳极，第2端口vout分别接稳压二极管d8的负极和电感l1的一端，第3端口gnd接地，第4端口fb分别接电感l1的另一端与电解电容c10的阳极和电容c11的一端，第5端口on/off接地；

电解电容c9的阴极接地；

稳压二极管d8的正极接地；

电解电容c10的阴极接地；

电容c11的一端作为输出vcc-5v-bt输出，其另一端接地。

优选地，粗糙电压转换电路包括第一ams1117芯片、电容c14、电解电容c13、电容c15、电容c16、电感l2、电阻r16、电容c17和电容c18，其中：

第一ams1117芯片的第1端口gnd接地，第2端口vout分别接电解电容c13的阳极、电容c15的一端、电容c16的一端和电感l2的一端，第3端口vin为分别接第一电压转换电路的输出端和电容c14的一端；

电容c14的另一端接地；

电解电容c13的阴极接地；

电容c15的另一端接地；

电容c16的另一端接地；

电感l2的另一端分别接电容c17、电容c18；

电容c17的另一端分别接电阻r16和地；

电阻r16的另一端接地；

电容c18的一端为粗糙电压转换电路的输出，另一端接地。

优选地，高质电压转换电路包括第二ams1117芯片、电容c20、电阻r20、电解电容c19、电容c21、电容c22，其中：

第二ams1117芯片的第1端口gnd接地，第2端口vout分别接电解电容c19的阳极、电容c21的一端和电容c22的一端，第3端口vin分别接第一电压转换电路的输出端和电容c20的一端；

电容c20的另一端接地；

电解电容c19的阴极分别接地与电阻r20的一端；

电阻r20的另一端接地；

电容c21的另一端接地；

电容c22的一端为高质电压转换电路的输出，另一端接地。

优选地，电源电路还包括一电源检测电路，电源检测电路输入为24v锂电池，其输出接至一单片机的adc输入端口，包括电阻r14、电阻r15、电容c12，其中：

电阻r14的一端作为电源检测电路的输入，其另一端分别接电阻r15的一端、电容c12的一端；

电阻r15和电容c12的另一端接地；

电容c12的一端接入单片机的adc输入端口。

优选地，姿态测量系统mpu-6050包括一mpu6050芯片、电阻r1、电阻r2、电容c3、电容c4、电容c6、电容c8、电解电容c10，其中：

mpu6050芯片的第1端口clkin接地、第23端口scl/sclk接电阻r1的一端，第24端口sda/sdi接电阻r2的一端，第9端口ad0/sdo接地，第8端口分别接高质电压转换电路的输出和电容c3的一端，第20端口cpout接电容c4的一端，第10端口regout接电容c6的一端，第13端口vdd分别接电容c8的一端、电解电容c10的阳极和高质电压转换电路的输出；

电阻r1的另一端接高质电压转换电路的输出，电阻r2的另一端接高质电压转换电路的输出；

电容c3的另一端、电容c4的另一端、电容c6的另一端、电容c8的另一端和电解电容c10的阴极均接地。

优选地，电机驱动电路采用4片btn7960驱动芯片搭建成h桥式驱动电路，h桥式驱动电路分为两侧，互为镜像，其中一侧包括第一btn7970驱动芯片、第二btn7970驱动芯片、电阻r4、电阻r5、电阻r6、电阻r7、电解电容c1、第一跳线焊盘hearder2和第二跳线焊盘hearder2，其中：

第一btn7970驱动芯片的第2端口in和第三端口inh分别接cpu2，第5端口sr接电阻r4的一端，第6端口is分别接电阻r5和h桥式驱动电路的另一侧，第1端口gnd接地，第4端口out和第8端口out均接至第一跳线焊盘hearder2的第2接口上，第7端口vs分别接电解电容c1的阳极和第二跳线焊盘hearder2的第1接口上；

电阻r4的另一端和电阻r5的另一端均接地，电解电容c1的阴极接地；

第二btn7970驱动芯片的第2端口in和第三端口inh分别接cpu2，第5端口sr接电阻r7的一端，第6端口is分别接电阻r6和h桥式驱动电路的另一侧，第1端口gnd接地，第4端口out和第8端口out均接至第一跳线焊盘hearder2的第1接口上，第7端口vs分别接电解电容c1的阳极和第二跳线焊盘hearder2的第1接口上；

电阻r6的另一端和电阻r7的另一端均接地，第二跳线焊盘hearder2的第2接口接地。

一种用于电测实验室仪表校验的agv小车驱动方法，包括以下步骤：

s1：将cpu1和cpu2上电运行并初始化；

s2：判断cpu1的中断周期和cpu2的中断周期是否结束，直到结束，进入步骤s3；

s3：cpu1读取姿态测量系统mpu-6050的姿态数据并进行数据融合，通过串口发送姿态数据至cpu2，同时返回步骤s2；

s4：判断cpu2的串口是否中断，直到串口中断且cpu2的中断周期结束，由cpu2利用pid算法计算小车的直立控制、速度检测、速度控制和方向控制的数据，其中利用编码器实现速度检测，同时根据pid算法的结果控制定时器输出脉宽调制信号至驱动电路上；

s5：驱动电路完成电机驱动。

与现有技术相比，本发明技术方案的有益效果是：

本发明采用双cpu设计，分工合作，共同完成对车体的控制，达到良好的控制效果，同时双cpu互为备份，不至于使系统系统因为一个cpu的故障而失控，大大增加系统的可靠性。利用btn7960芯片能够在电机转向时自动产生一个死区时间，因此能免除软件设计死区时间的麻烦，不用考虑换电机方向是的死区时间。mpu-6050的数据传输速度最高至400khz满足小车队对系统实时性的要求。利用两级电压变换，将24v锂电池的电压转换为主控板需要的电压，且通过电源检测电路对电源电压进行检测，防止电源电压不足。

附图说明

图1为一种用于电测实验室仪表校验的agv小车驱动系统框架图。

图2为第一电压转换电路的电路图。

图3为粗糙电压转换电路的电路图。

图4为高质电压转换电路的电路图。

图5为电源检测电路的电路图。

图6为姿态测量系统mpu-6050的电路图。

图7为h桥式驱动电路的一侧的电路图。

图8为一种用于电测实验室仪表校验的agv小车驱动方法的流程图。

图9为pid控制图。

图10为利用四元数求解体感平衡车姿态角的流程图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1

一种用于电测实验室仪表校验的agv小车驱动电路，包括小车，其框架如图1所示，还包括电源电路、地磁传感器hmc5883、姿态测量系统mpu-6050、cpu1、cpu2和电机驱动电路，其中：

地磁传感器hmc5883与姿态测量系统mpu-6050相连，姿态测量系统安装小车的车体平板下方，与小车的车轴在同一垂线上，姿态测量系统mpu-6050与cpu1相连，cpu1完成车体姿态测量和传感器处理工作，通过串口与cpu2相连；

电源电路分别与地磁传感器hmc5883、姿态测量系统mpu-6050、cpu1、cpu2和电机驱动电路相连；

电机驱动电路与cpu2相连。

其中cpu1和cpu2互为备份，当其中一个因外因不能正常工作时，另外一个便独自承担起系统的所有测量和控制任务。

电源电路包括第一电压转换电路和第二电压转换电路，其中：

第二电压转换电路包括高质电压转换电路和粗糙电压转换电路，第一转换电路的输入为24v锂电池，其输出分别接高质电压转换电路的输入端和粗糙电压转换电路的输入端，高质电压转换电路的输出端接姿态测量系统mpu-6050和地磁传感器hmc5883，粗糙电压转换电路的输出端接cpu1、cpu2和电机驱动电路。

第一电压转换电路如图2所示，包括lm2596s芯片、电解电容c9、稳压二极管d8、电感l1、电解电容c10和电容c11，其中：

lm2596s芯片的第1端口vin分别接24伏锂电池vccbtin和电解电容c9的阳极，第2端口vout分别接稳压二极管d8的负极和电感l1的一端，第3端口gnd接地，第4端口fb分别接电感l1的另一端与电解电容c10的阳极和电容c11的一端，第5端口on/off接地；

电解电容c9的阴极接地；

稳压二极管d8的正极接地；

电解电容c10的阴极接地；

电容c11的一端作为输出vcc-5v-bt输出，其另一端接地。

粗糙电压转换电路如图3所示，包括第一ams1117芯片、电容c14、电解电容c13、电容c15、电容c16、电感l2、电阻r16、电容c17和电容c18，其中：

第一ams1117芯片的第1端口gnd接地，第2端口vout分别接电解电容c13的阳极、电容c15的一端、电容c16的一端和电感l2的一端，第3端口vin为分别接第一电压转换电路的输出端和电容c14的一端；

电容c14的另一端接地；

电解电容c13的阴极接地；

电容c15的另一端接地；

电容c16的另一端接地；

电感l2的另一端分别接电容c17、电容c18；

电容c17的另一端分别接电阻r16和地；

电阻r16的另一端接地；

电容c18的一端为粗糙电压转换电路的输出，另一端接地。

高质电压转换电路如图4所示，包括第二ams1117芯片、电容c20、电阻r20、电解电容c19、电容c21、电容c22，其中：

第二ams1117芯片的第1端口gnd接地，第2端口vout分别接电解电容c19的阳极、电容c21的一端和电容c22的一端，第3端口vin分别接第一电压转换电路的输出端和电容c20的一端；

电容c20的另一端接地；

电解电容c19的阴极分别接地与电阻r20的一端；

电阻r20的另一端接地；

电容c21的另一端接地；

电容c22的一端为高质电压转换电路的输出，另一端接地。

电源电路还包括一电源检测电路，如图5所示，电源检测电路输入为24v锂电池，其输出接至一单片机的adc输入端口，包括电阻r14、电阻r15、电容c12，其中：

电阻r14的一端作为电源检测电路的输入，其另一端分别接电阻r15的一端、电容c12的一端；

电阻r15和电容c12的另一端接地；

电容c12的一端接入单片机的adc输入端口，如图5，vcc_btin为电池正极，adc12_in5为单片机adc输入端口。设单片机adc模块转换结果为adc_dr则adc12_in5端的电压vin为：

单位为v，则电池电压为：

cpu2在主程序中循环测量电池的电压，当检测到电池电压低于设定阀值时，驱动蜂鸣器报警。防止因电池电量过低而导致系统工作失常而发生意外事故。

姿态测量系统mpu-6050如图6所示，包括一mpu6050芯片、电阻r1、电阻r2、电容c3、电容c4、电容c6、电容c8、电解电容c10，其中：

mpu6050芯片的第1端口clkin接地、第23端口scl/sclk接电阻r1的一端，第24端口sda/sdi接电阻r2的一端，第9端口ad0/sdo接地，第8端口分别接高质电压转换电路的输出和电容c3的一端，第20端口cpout接电容c4的一端，第10端口regout接电容c6的一端，第13端口vdd分别接电容c8的一端、电解电容c10的阳极和高质电压转换电路的输出；

电阻r1的另一端接高质电压转换电路的输出，电阻r2的另一端接高质电压转换电路的输出；

电容c3的另一端、电容c4的另一端、电容c6的另一端、电容c8的另一端和电解电容c10的阴极均接地。

电机驱动电路采用4片btn7960驱动芯片搭建成h桥式驱动电路，h桥式驱动电路分为两侧，互为镜像，其中一侧如图7所示，包括第一btn7970驱动芯片、第二btn7970驱动芯片、电阻r4、电阻r5、电阻r6、电阻r7、电解电容c1、第一跳线焊盘hearder2和第二跳线焊盘hearder2，其中：

第一btn7970驱动芯片的第2端口in和第三端口inh分别接cpu2，第5端口sr接电阻r4的一端，第6端口is分别接电阻r5和h桥式驱动电路的另一侧，第1端口gnd接地，第4端口out和第8端口out均接至第一跳线焊盘hearder2的第2接口上，第7端口vs分别接电解电容c1的阳极和第二跳线焊盘hearder2的第1接口上；

电阻r4的另一端和电阻r5的另一端均接地，电解电容c1的阴极接地；

第二btn7970驱动芯片的第2端口in和第三端口inh分别接cpu2，第5端口sr接电阻r7的一端，第6端口is分别接电阻r6和h桥式驱动电路的另一侧，第1端口gnd接地，第4端口out和第8端口out均接至第一跳线焊盘hearder2的第1接口上，第7端口vs分别接电解电容c1的阳极和第二跳线焊盘hearder2的第1接口上；

电阻r6的另一端和电阻r7的另一端均接地，第二跳线焊盘hearder2的第2接口接地。

一种用于电测实验室仪表校验的agv小车驱动方法，如图8所示，包括以下步骤：

s1：将cpu1和cpu2上电运行并初始化；

s2：判断cpu1的中断周期和cpu2的中断周期是否结束，直到结束，进入步骤s3；

s3：cpu1读取姿态测量系统mpu-6050的姿态数据并进行数据融合，通过串口发送姿态数据至cpu2，同时返回步骤s2；

s4：判断cpu2的串口是否中断，直到串口中断且cpu2的中断周期结束，由cpu2利用pid算法计算小车的直立控制、速度检测、速度控制和方向控制的数据，其中利用编码器实现速度检测，同时根据pid算法的结果控制定时器输出脉宽调制信号至驱动电路上；

s5：驱动电路完成电机驱动。

在具体实施过程中，根据pwm输出量与角度和角加速度按照式：

pwm＝kp_angle·angle+kd_angle·angle_dot

增加位置反馈后输出结果为：

pwm＝kp_angle·angle+kd_angle·angle_dot+kp_position·position+kd_position·position_dot_fil

系统的pid控制框图如图9所示，根据系统的框图，系统中角度环的控制使用比例和微分控制，比例控制增加系统的响应性，微分控制使系统具有更好的预测性，减弱系统震荡。安装在电机主轴上的编码器可以检测车体的速度。增加速度反馈，由于本发明中只需要车体平衡，速度反馈只用到比例环节，不再增加加积分环节。

利用四元数结算车体姿态角的具体步骤如图10所示：

(1)四元数初始化

cpu1将获取到的若干组mpu-6050所输出的陀螺仪和加速度数据进行均值滤波后作为四元数算法的基础数据。初始化过程，主要是对四元数变量进行赋值。对应程序如下：

(2)修正角速度误差

重力加速计容易受到车体振动的干扰，去瞬时值误差较大，陀螺仪比较稳定，但是积分会有累积误差的缺陷，所以需要用加速度计得到的数据来修正陀螺仪得到的数据。经叉积求得误差。agv车体坐标系上重力加速度为：

机体加速度值为：

ab＝(abxabyabz)

由加速度计测得重力向量为：

ab＝(abxabyabz)

陀螺仪积分得到的重力向量是vx，vy，vz；ex，ey，ez为重力向量的积,用叉积来修正陀螺仪输出数据得：

程序如下：

norm＝invsqrt(ax*ax+ay*ay+az*az)；

ax＝ax*norm；

ay＝ay*norm；

az＝az*norm；

//把加速度计的三维向量转化成单位向量

norm＝invsqrt(mx*mx+my*my+mz*mz)；

mx＝mx*norm；

my＝my*norm；

mz＝mz*norm；

bx＝sqrt((hx*hx)+(hy*hy))；

vx＝2*(q1q3-q0q2)；

vy＝2*(q0q1+q2q3)；

vz＝q0q0-q1q1-q2q2+q3q3；

wx＝2*bx*(0.5-q2q2-q3q3)+2*bz*(q1q3-q0q2)；

wy＝2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3)；

wz＝2*bx*(q0q2+q1q3)+2*bz*(0.5-q1q1-q2q2)；

//把加速度计的测量矢量和参考矢量做叉积，用于修正陀螺仪。

ex＝(ay*vz-az*vy)；

ey＝(az*vx-ax*vz)；

ez＝(ax*vy-ay*vx)；

(3)数据融合

根据上面求得的叉积误差，需要用到参数kp和ki，本设计中通过实验得到当kp＝2.0ki＝0.01时修正效果较好。

exint＝exint+ex*ki*halft；

eyint＝eyint+ey*ki*halft；

ezint＝ezint+ez*ki*halft；

gx＝gx+kp*ex+exint；

gy＝gy+kp*ey+eyint；

gz＝gz+kp*ez+ezint；

(4)四元数更新

四元数更新，即为求解四元数的微分方程

tempq0＝q0+(-q1*gx-q2*gy-q3*gz)*halft；

tempq1＝q1+(q0*gx+q2*gz-q3*gy)*halft；

tempq2＝q2+(q0*gy-q1*gz+q3*gx)*halft；

tempq3＝q3+(q0*gz+q1*gy-q2*gx)*halft；

(5)规范化处理^[15]。

由于使用单片机计算有误差，因此在单次四元数更新后需要进行规范化处理。根据规范化方程。

程序如下：

norm＝invsqrt(tempq0*tempq0+tempq1*tempq1+tempq2*tempq2+tempq3*tempq3)；

q0＝tempq0*norm；

q1＝tempq1*norm；

q2＝tempq2*norm；

q3＝tempq3*norm；

(6)欧拉角转换

依据变换方程把四元数变化成欧拉角

俯仰角：pitch＝-arcsin(2(q1q3-q0q2))

横滚角：

航向角：

对应程序如下：

航向角：angles[0]＝-atan2(2*q[1]*q[2]+2*q[0]*q[3],-2*q[2]*q[2]-2*q[3]*q[3]+1)*180/m_pi；

俯仰角：angles[1]＝-asin(-2*q[1]*q[3]+2*q[0]*q[2])*180/m_pi；

横滚角：angles[2]＝atan2(2*q[2]*q[3]+2*q[0]*q[1],-2*q[1]*q[1]-2*q[2]*q[2]+1)*180/m_pi；

通过以上步骤完成四元数求解姿态角，根据实测结果，可以完美得到agv车姿态数据。为cpu2进行pid控制提供可靠稳定的数据输入。

相同或相似的标号对应相同或相似的部件；

附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。