基于PID算法的辅助进食桌面机械手系统研究与设计*

刘祚时, 罗 斌, 张 平, 饶 远

(江西理工大学 机电工程学院,江西 赣州 341000)

0 引 言

伴随着微电子工艺技术的突破,现有技术已经能够实现纳米的加工工艺;同时传感器技术的革新、现代控制理论的出现以及人工智能和计算机技术的融合,使得对机器人技术的研究已经过渡到前沿综合学科的研究过程中[1],而服务机器人作为未来机器人技术产业化的重要发展方向,可用于向老人或残疾人提供助老助残的帮助、为家庭做清洁卫生工作以及一些其他特殊应用场合[2]。近年来,机器人技术得到了广泛应用,国内学者应用机器人技术实现了地板清洁与空气净化、微创外科手术[3]、自主种牙[4]以及家中巡逻[5]等。国外的研究机构则研制了自动喂饭机器人,可通过手按按键或使用脚踏模式,使得装有不同用餐食物的餐盘相互切换,该机械手不仅可以用来为残疾人辅助进食,还能够示教定位,并具有避障的功能;而Dapra资助的功能假肢项目DEKA Arm[6,7],利用人体神经界面把大脑皮层的神经运动的电信号转换为一种能够操作辅助装置的控制信号,从而使得瘫痪者恢复自理能力和运动能力,完成自主饮食。虽然机器人技术被广泛运用在医疗服务领域中,但实际针对上肢肢体不健全的特殊群体所应用的辅助进食类的机器人较少,且该服务类机器人购买成本较高、占用的空间位置较大,不能够很好地完成喂食任务。

为了解决上述问题,本文将结合语音与机器视觉等传感器技术设计一种辅助进食的桌面机械手控制系统,利用传感器技术将系统的反馈信息与用户进行数据交互,达到让机械手能够按照预期效果辅助上身有缺陷的人群完成辅助喂食等一系列功能的目的。

1 桌面机械手数学模型的建立

本文中实验所采用的桌面机械手平台由5个TBS2701数字舵机所组成,其控制单元采用STM32F103ZET6作为主控芯片,并结合LD3320A人声语音识别模块与OV7725摄像头模块实现本文的预期效果。

1.1 桌面机械手正运动学分析

现采用D-H方法并结合表1中连杆参数,建立机械手的连杆坐标系,如图1所示。

图1 辅助进食机械手连杆坐标系

连杆数ai/mmαi/(°)di/mmθi关节变量范围/(°)1109010θ1-135～135210500θ2-90～90310000θ3-135～13540900θ4-135～135500160θ5-90～90

其中,关节1～关节5为转动关节,基座偏移量为H=72 mm。当所有连杆坐标系确定后,可确定相邻连杆间的相对关系。现就图2中连杆i和i-1连杆来说明转换关系:

1) 选择坐标系与坐标系i-1一致,令其沿轴zi-1平移di,并且绕轴zi-1旋转θi,使当前坐标系与坐标系i′重合;

2) 将和坐标系i′一致的坐标系沿x′i平移ai,并且绕x′i旋转αi,使当前坐标与坐标系i重合。

上述变换用总的变换矩阵表示为

=Trans(0,0,di)Rot(zi-1,θi)Trans(ai,0,0)Rot(xi′,αi)=

(1)

(2)

将机械手的每个坐标转换矩阵代入式(2)中即可求出该机械手的正运动方程。

1.2 桌面机械手逆运动学分析

逆运动学的求解能够将末端执行器在操作空间的运动转化成关节空间的运动[8]。当给定末端执行器的位置pw去求解关节变量的θ1,θ2,θ3,θ4,θ5。

由于机械手存在三轴平行的特征,故采用反变换法或几何解法。在计算关节转角时采用双参变量的反正切函数关节变量,求得各个关节变角如下

θ1=atan 2(py,px)

θ2=atan 2(-a3sinθ3,a2+a3cosθ3)-

atan 2(cz-d1-H,ct+a1)

θ3=atan 2(a3sinθ3,a2+a3cosθ3)+

atan 2(cz-d1-H,ct+a1)

θ4=atan 2(-a3sinθ3+a4sinθ4,a2cosθ2+a4cosθ4)-

2atan (cz-d1-H,ct+a1)

θ5=atan 2(s1r11-c1r21,s1r12-c1r22)

1.3 运动学仿真分析

为验证桌面机械手的正运动学方程的建立和逆运动学求解的正确性,现将借助MATLAB Robotics Toolbox[9]工具箱对机械手建模,随后进行正逆运动学校验。首先根据表1中D-H参数表进行建模,如图2所示。

图2 机械手模型

然后根据Robotics Toolbox中的函数代入关节范围内的任意关节角计算正运动学矩阵值,最后由逆运动学公式将已知矩阵值代入求解出关节角并与前向运动学的关节角比较。

其中与AB两组关节角对应的正运动学位姿矩阵分别为

2 辅助进食控制系统的设计

2.1 对目标人脸的追踪控制设计

该辅助进食机械手适用于动态运动中的辅助进食任务,因此机械手的运动需要进行动态调整。故该机械手的工作时序是先等待人的输入,再去执行取食物的动作,接着寻找人脸位置,等待人嘴型张开动作的发生(也就是进食动作的完成)。由于该过程中,人是运动的而非静止状态,所以针对等待人进食的过程,需利用机器视觉实现人脸的动态跟随,使得取食物的器具(勺子)一直朝向人脸方向。

在进行人脸动态跟随之前,需完成查找人脸的搜索动作,但由于机械手的摆放位置是确定的。因此机械手取完食物后会运动到默认位置(面向人脸一侧),并在指定工作空间开始人脸第一次搜索,这时人脸的位置作为人脸动态跟随的初始位置,直至完成进食动作为止。

2.1.1 比例—积分—微分控制对象分析

比例—积分—微分(PID)的控制目标(输入信号)是OpenMV采集人脸中心相对于图像中心的像素差,并可通过输出脉冲宽度调制(pulse width modulation,PWM)来控制舵机机械手运动,实现机械手动态跟随人脸的效果。在控制过程中可将误差转换为期望PWM值输出,同时对采集识别到的人脸图像进行处理,而识别到的人脸图像在图片区域中表示为4个参数(x,y,w,h),即人脸对象的起点为(x,y),人脸区域宽度为w,高度为h,所有的单位为像素值。选用的分辨率为160像素×120像素(QQVGA),像素格式为灰度图,如图3所示。

图3 人脸坐标分析

图3中,当人脸对象被人脸检测算法识别出来之后,返回人脸对象在图像中的感兴趣区域(region of interest,ROI)。该区域由4个参数表征,人脸对象的中心坐标如图中十字型标记所示,则人脸对象的中心坐标为

cx=x+w/2,cy=y+h/2

(3)

而人脸对象中心坐标和整个图像的中心坐标的差值就作为舵机云台PID控制器的控制量,此时方向误差分别为

Δx=x+w/2-W/2,Δy=y+h/2-H/2

(4)

2.1.2 舵机位置PID控制器设计

设计舵机位置PID控制器是为了在视觉单元检测到人脸之后,顺利完成对目标人脸的搜索任务,随后等待人的交互动作(如张嘴动作)。考虑到目标用户为行动不便的特殊人群,所以需避免因人动作变化而导致人脸的位置发生改变的情况发生,防止机械手重新在工作空间中进行人脸搜索。因此,设计该舵机位置PID控制器需在机械手完成取食物的动作且搜索到人脸,并在人张嘴进食动作发生之前完成目标人脸的追踪,同时确保末端操作器始终在人脸正前方。

控制器设计规则：1)关节0与关节1使用相同的PID控制器;2)当控制变量较小时,PID控制器不生效;3)控制器的响应速度快。

由上设计了位置型PID控制器,可通过调节PID控制器的参数,满足实际的控制要求与执行元件的响应需要。

2.1.3 舵机位置型PID控制器实际控制效果测试及分析

本文进行2组实验来测试机械手PID控制器在水平方向上的跟随效果,并分别用机械手检测在人脸相对于机械手进行左偏和右偏运动的情况下,对PID控制器像素误差数据进行采集和分析。其中误差像素数据是由控制器串行外设接口传到上位机得到的。

从图4可以看出:控制量为像素值,经过PID参数整定后,敏感变量PID控制器的响应速度很快,能够达到人脸移动的追踪要求,并达到预期的控制效果。

图4 人脸PID控制效果

2.2 对机械手的语音控制设计

由于语音交互便于使用,且适合应用在人机交互要求较高的场合,同时语音交互也便于学习使用。通过LD3320A人声语音识别模块,输入有效的语音时,语音模块会根据输入的语音词条匹配预定义词条,完成相应的功能动作。

本文采用LD3320的循环识别模式,能够满足语音随机输入的要求,同时通过口令触发作为触发源。LD3320驱动包括以下步骤：1)芯片复位;2)语音识别;3)声音播放。其操作顺序如图5所示。

图5 语音识别操作顺序

语音识别通用初始化中主要是对LD3320进行软复位、数字信号处理器(DSP)休眠低功耗设置、模/数转换器(ADC)初始化以及时钟率的初始化设置。在语音识别初始化中主要是使控制寄存器进入自动语音识别(automatic speech recognition,ASR)模块,然后对添加词条的先入先出(FIFO)缓存器进行初始化,如图6(a)所示。

选择语音循环识别模式,可以让用户随时根据需求发出语音指令。文中的程序在FreeRTOS系统中创建一个优先级较高的语音识别任务,使得系统能够让每个任务在任务调度器的管理下高效运行。当有语音输入产生识别结果时;同时产生中断。因此需要设置中断处理,具体流程如图6(b)所示。

图6 词条写入与循环识别模式

3 实验调试过程

在完成整个辅助进食机械手系统的软硬件设计与测试后,对机械手进行系统调试,如图7所示。

图7 辅助进食机械手系统工作流程

在系统调试过程中,分别截取了机械手取食物、机械手喂食物和动态人脸跟随的实验照片,如图8所示。

图8 三组动作实验照

在实验过程中,辅助进食机械手能够先去指定的食物储存器中取食物(实验中食物用燕麦片代替),然后人脸动态跟随送达目的位置,在嘴张开且喂食动作结束后,继续重复上次动作,经实验证实该机械手可以达到辅助进食的效果。

4 结束语

本文设计的五自由度辅助进食桌面机械手系统,主要服务特殊人群,在考虑人机交互体验的前提下,采用了语音识别作为系统的输入,能够满足大部分场合的应用需求;同时在喂食的环节采用了人脸检测和模板匹配技术,并在取食物之后、等待进食之前使用了人脸追踪,通过PID算法实现机械手的动态人脸跟随,大大地提升了人机交互体验。而面对多传感器数据融合以及多任务应用场景的问题,本文选择嵌入式系统(FreeRTOS)开发软件应用程序能够提高系统的实时性,并使用信号量等机制解决任务同步、共享资源访问的问题,提高了开发效率。