基于重力感应传感器的六自由度车载机械手控制系统设计

孟祥斌,张雅彬,田卫华,刘 炜

(1.沈阳工程学院自动化学院，辽宁沈阳 110136；2.哈尔滨工程大学智能科学与工程学院，黑龙江哈尔滨 150001)

0 引言

重力感应传感器是新型传感器技术范畴，实质是利用弹性敏感元件制成悬臂式位移器，与采用弹性敏感元件制成的储能弹簧来驱动电触点，实现从重力变化值到电信号的转变[1-2]。

重力感应传感器以其低功耗、小体积、高过载、使用简便等优势广泛应用于运动载体姿态测量中。由于重力感应传感器微惯性数据处理简单可靠，并不会改变传感器弹载结构，对其在车载测量应用方面有着重要意义。但是，重力感应传感器测量精度有限，具体应用需进一步研究。文献[3]提出重力加速度数值积分的多项式拟合算法，平台移动速度快，但定位精度不够。文献[4]研发了上位主控平台，根据需要实时接收车载机械手状态信息，实现遥控平台操控，定位误差精准，但车体移动速度较慢。文献[5]设计了基于MMA7260 加速度传感器的姿态平衡小车控制系统，可实现车体快速控制，但定位精度误差有待提高。

为解决以上问题，本文通过重力感应传感器MMA7660FC测量重力引起的加速度，计算出设备相对于水平面的倾斜角度。设计了一种基于遥控操作平台的双控制系统，该系统利用重力感应传感器对车体运行姿态进行粗调整，快速到达接近抓取目标位置时，采用按钮连接STM32控制模块进行车体速度、方向精准步长控制，可实现车载机械手位置的精准校准，实验表明车体移动速度和定位精度都满足设计要求。

1 系统设计

车载机械手控制系统由遥控操作平台、控制模块、无线通信模块、驱动模块、AD转换模块、重力感应模块、稳压滤波模块等组成，系统框架如图1所示。

图1 系统框架图

车载机械手是按照制订程序、轨迹与要求实现自行抓取、放置的自动化机械系统。车载机械手通常由2部分构成，即移动车体平台和机械手。依托固定车辆底座，在此底座上层放置机械手，利用机械手实现空间动作，完成抓取物体、机械手臂伸展、动作复位等动作。通过借助车体的快速移动性，扩展了机械手工作范围。

设计的遥控操作平台分别对车体和六自由度机械手机械控制。只有保证车体稳定静止后，才可以对机械手进行动作操作。车体除搭载机械手和传感器外，车体自身具有单独STM32控制模块。控制模块通过车体所携带超声波传感器和视觉传感器识别周围环境状态，将采集的信号处理后，通过无线传输模块发送给遥控操作平台，以便遥控操作平台进行及时处理。PID算法调整直流电机的转速，车体STM32控制器内含特殊安全机制，保证遇到突发路面障碍和阻碍物体时，及时暂停车辆行进，直到遇到遥控操作平台确认信息2次后(间隔时间短于2 s)，才可以继续车体行进和机械手操作动作。

2 硬件设计

车载机械手控制系统包含车体运行姿态的调控和六自由度机械手抓取控制，其中通过控制直流电机来实现车体移动，当车体静止后，再控制机械手上的6个舵机实现物体抓取的设计。

2.1 车体驱动设计

车体STM32控制模块通过无线模块，接收到遥控操作平台指令，据此执行相应的控制任务。由于控制模块STM32输出的PWM信号为3.3 V电平，不能直接用来驱动直流电机进行转动。因此，采用直流驱动电路来提高驱动能力。设计采用L298N芯片，L298N是双H桥电机驱动芯片，可以驱动2个直流电机。依据L298N设计的驱动电路可提供5～35 V范围的驱动电压，最大功率为25 W。L298N价格较低，性能可靠，满足本设计需求[6]。

车体STM32控制模块并产生稳定度较高PWM信号和I/O电流。电流信号方向的正负来确定使能端上的信号电平，利用使能端的信号状态来改变直流电动机的顺时针、逆时针旋转状态。PWM信号的占空比决定直流电机的运行速度。PWM信号经L298N驱动电路放大后，可以满足车体直流电机的运行功率。车体电机驱动电路图如图2所示。

图2 电机驱动电路图

2.2 重力感应传感器电路设计

重力感应传感器MMA7660FC是一个 1.5g的三轴加速度计，支持I2C数字输出。同时这款传感器是电容MEMS型的传感器，具有非常低的功耗，它内部具有低通滤波器，可用于补偿0g偏移和增益误差，用户可根据要求为该传感器配置为6位分辨率。重力感应传感器功能框图见图3。

图3 重力感应传感器功能框图

车轮的转速和方向由重力感应的倾角值决定。重力感应传感器电路见图4。遥控操作平台内控制模块将捕获重力感应传感器的姿态数值与设定程序进行匹配，并将匹配数值发送给车体控制模块。通过车体控制模块内的闭环PID控制算法确定车辆的车轮运动方向和角度。

图4 重力感应传感器电路

2.3 遥控操作平台通信电路设计

车载机械手遥控操作平台系统中，设计通信模块采用无线通信NRF24L01芯片，这款无线射频收发芯片的工作频率是2.4 GHz。该芯片内部自带CRC检验功能，可自动完成应答功能，内部硬件的CRC具有查错能力，可以实现一点对多点通信地址操控。同时，该款芯片在1.9～3.6 V之间进行工作，可实现低功耗处理数据，待机状态下电流为22 μA，掉电模式下电流为900 nA。内置2.4 GHz天线，该芯片有126个频道，符合多点通信和跳频通信的需求。此外具有高可靠性的数据传送[7-8]。通信模块与控制器STM32F103RBT6构成无线通信传输。无线模块采用3.3 V电源电压供电，因为STM32F103RBT6同样采用3.3 V供电，所以无线模块的通信引脚可以和控制器STM32的引脚直接相连。IRQ是可设置的中断标志的引脚，当接收有效数据时或者发送数据成功时，引脚会产生电平的变化。

2.4 车体无线通信电路设计

车体的无线通信模块主要功能是接收遥控操作平台发送的控制指令，并将车体搭载传感器采集的现场环境数据传送到遥控操作平台端。NRF24L01的外接天线传输距离提高到20 m，其车体无线通信接收电路见图5，受外界环境影响，传输距离有一定的降低。

图5 车体无线通信接收电路

2.5 液晶显示接口电路

显示模块使用低功耗NOKIA5110的LCD液晶显示屏。该屏是以点阵的方式来显示的，具有48像素×84像素的分辨率。传输最高速率可达4.0 Mbit/s，可以显示英文字符、汉字和图片。液晶显示模块采取SPI的通信方式。硬件使用了5个普通的IO口的资源作为显示屏的输出接口。通过液晶显示模块可以实时观测车体运行速度信息和方向角度数值[9]。图6为液晶接口图。

3 程序设计

该车载机械手系统的软件主要用C语言编写，开发环境为Keil MDK，软件采用模块化设计。车载机械手系统的主程序先进行初始化配置，确保车体和机械手正常工作。初始化主要包括系统变量初始化、中断初始化、传感器模块初始化、A/D转换程序初始化、机械手初始化等。系统变量初始化包含霍尔测速模块的设置，重力感应传感器、超声波模块测距离赋初值，视觉传感器Pixy 2代CMUcam5的各类函数的初始化，供电电源监测初始化等。系统初始化后，遥控操作平台中重力感应模块测定数值超过设定静止阈值后，通过遥控操作无线模块发出移动信息，启动车体。车体在移动后，利用超声波传感器和视觉传感器感知车体周围环境信息，如遇障碍物，及时将信息反馈给遥控操作平台端。遥控操作平台改变车体运行速度和方向，规避障碍物。当车体接近目标物体时，首先通过重力感应模块进行车体姿态调整，其次通过按钮进行位置微调修正。当满足设定误差范围±5 mm内时，保持车体静止。启动机械手控制，利用PID算法控制六自由度机械手舵机角度方向角度，进行目标物体抓取，抓取目标物体放入小车后，进行机械手归位。最后，重新启动小车运动。车载机械手主程序的流程图如图7所示。

车载机械手系统需要确保电量支持，各类传感器模块、无线通信模块、车体驱动电机和机械手舵机等均需要电能支持，为保证系统电量充足，需设置电源监测程序。通过一定延迟时间对车体携带电源两端电压监测，保证系统正常工作。当电压值低于设置电压(7.2 V的20%)，红灯闪烁，需要及时充电。正常时保持绿灯点亮。图8为电池电能监测子程序流程图。

图8 电池电能监测子程序流程

4 测试结果及分析

4.1 机械手臂夹紧力测试

抓取物体的夹紧力是设计机械手重要指标，夹紧力大会使物体在拾取物体过程中表面破损或形状变形，从而导致拾取失败；夹紧力过小，导致车辆行走过程中，物体滑落，也会导致拾取失败。夹紧力需要克服物体重力所引起的静载荷和物体运动状态变化产生的载荷，以保证机械手对物体稳定的夹紧状态。机械手对物体夹紧力依据式(1)计算[10]:

FN≥β1β2β3G

(1)

式中：β1为安全系数，通常取值1.2～2.0；β2为状况系数，主要考虑惯性力的作用；β3为方向系数，依据手臂与物体之间的不同位置进行判断；G为被抓取物件所受重力，N。

机械手夹紧力测试如表1所示。

表1 机械手夹紧力测试

由测试结果可知，机械手对于质量≤6.05 g物体，抓取率过低。对于质量>10.0 g物体，抓取率高。此外，测试中机械手所夹取物体质量不可超过300 g，否则会造成微型车载搬运机器人重心不稳。

4.2 车辆控制系统测试

遥控操作平台连接无线通信模块后，对车载机械手状态进行20次发送指令测试，观察搬运机器人运动状态的变化，再改变遥控操作平台与搬运机器人之间距离后重复上面的测试。不同距离控制指令准确性测试如表2所示。

表2 不同距离控制指令准确性测试

其中重力传感器粗略调整为模式1，按钮连接STM32控制器，进入模式2，实现精细调整。根据表2可以获得实测模式1和模式2的接收指令准确次数和该模式对应的准确率。

以上测试环境是在空旷的地面上，测试通信质量较好，在无线通信NRF24L01芯片有效范围20 m内，通信指令模式1准确率的平均值为95.6%,模式2准确率的平均值为98.8%。原因为：重力感应模式1下，数据变化过快，控制器对数据的处理和发送延迟。

4.3 六自由度机械手仿真及定位测试

控制STM32模块给机械手的6个舵机发送相应的旋转角度，控制机械手的关节运行，完成抓取工作。图9给出了基于Proteus软件仿真的PWM脉宽仿真波形图。

图9 PWM脉宽波形图

依据虚拟示波器显示的PWM波形，可以检验编写的PWM子程序是否满足舵机控制要求。舵机的转动角度是通过调节PWM信号的占空比来完成的。标准PWM信号的周期固定为20 ms(50 Hz)，理论上仿真脉宽分布应在1～2 ms之间，实测脉宽可为0.5～2.5 ms之间。

在设计中，每个数字舵机方向旋转角度设置为8个定值(6个舵机共有48个方向角)。受方向角在空间数量不足影响，存在定位误差，实测数值与理论值计算相比在-3～3 mm之间。图10为车载机械手实物图。

图10 车载机械手实物图

5 结论

本文设计基于重力感应传感器的车载机械手，完成了车辆确定的运动路径规划，实现运动路径连续可控。通过按钮可在20 m内，远程操作机械手进行抓取、运送等动作执行，并对夹紧力进行实测，实现了机械臂的精准定位与操控。