一种六自由度咀嚼机器人的控制系统

摘要

一种六自由度咀嚼机器人的控制系统，所要解决的问题为，传统的点到点的3次多项式插值所需力矩大，所需设备体积较大的问题。由于机器人机械加工精度的因素,每个咀嚼机器人都存在个体差异和六个驱动杆对称性,很难得到一致的PID控制参数的问题。其采用以下方案：步骤1：规划咀嚼机器人的各分支杆的运动轨迹，通过切牙点的位置和位姿求出各驱动杆的伸缩，过路径点的3次多项式插值方法求出各驱动杆期望位置和速度；步骤2：根据各分支杆的运动轨迹对机器人的咀嚼过程进行控制，咀嚼过程包括张开、闭合、咬合三个阶段；在张开和闭合两个阶段，使用模糊控制，咬合阶段采用模糊PID控制。

权利要求

1.一种六自由度咀嚼机器人的控制系统，包括步骤：步骤1：规划咀嚼机器人的各分支杆的运动轨迹，通过切牙点的位置和位姿求出各驱动杆的伸缩，过路径点的3次多项式插值方法求出各驱动杆期望位置和速度，其特征在于，还包括以下步骤：步骤2：根据各分支杆的运动轨迹对机器人的咀嚼过程进行控制，咀嚼过程包括张开、闭合、咬合三个阶段；

在张开和闭合两个阶段，使用模糊控制，通过电机光编码器求解电机在张开闭合时的转速，在张开和闭合两个阶段，使用模糊控制，根据预设的张开闭合的电机的速度值，判断无刷直流电机转子转速，电机转子转速达到预设位置后，执行张开和闭合动作；

咬合阶段采用模糊PID控制：以驱动杆给定位置和实际位置的偏差作为输入，利用PID控制器的参数当前值，经过运算产生位置环的速度输出，位置环的速度输出和前馈通道的速度输出，经过叠加后作为总的输出，用作电机的速度给定。

2.根据权利要求1所述的一种六自由度咀嚼机器人的控制系统，其特征在于，所述过路径点的3次多项式插值方法如下：在t0时刻的运动速度为 在tf时刻的运动速度为 于是可以得到机器人驱动杆运动的边界条件：q(0)＝q0，q(tf)＝qf  (1)

令驱动杆的位置为：

2 3

q(t)＝a0+a1t+a2t+a3t  (3)

对其求一阶导数得到驱动杆速度：

将式(1)和式(2)代入式(3)和式(4)，可以求解出系数a0～a3a0＝q0；

得到过路径点的三次多项式插值的期望位置表达式(5)和速度表达式(6)：

3.根据权利要求1所述的一种六自由度咀嚼机器人的控制系统，其特征在于，PID控制器的参数当前值是PID控制器的参数以设定值为主分量，以模糊控制器产生的△Kp,△Ki和△Kd参数修正值为次要分量，两者相加构成的，△Kp比例参数变化量,△Ki微分参数变化量、△Kd积分参数变化量。

说明书

一种六自由度咀嚼机器人的控制系统

技术领域

[0001] 本专利基于对正常状态下成年人咀嚼运动中咀嚼肌、颞下颌关节、韧带及参与咀嚼运动相关组织结构进行生理特征研究分析，设计一种咀嚼机器人的控制方法及系统，属于机器人领域。具体设计涉及基于Cortex-M3控制器的机器人控制系统。

背景技术

[0002] 口腔生物材料和新型食品开发需要对颗粒度、质地、咀嚼性等方面进行评估。目前，对食物或材料的评估主要采用感官评价法和仪器评价法，感官评价法主要依赖品尝专家的主观评估，过程费时费力，评价结果主观性强、而且不稳定；常用的仪器评价法是简单的机械压缩试验，多属于半经验或模拟测定，评价结果与人类感知相差较大，而且不能对多种质地特性做出准确表达，故不能反映真实的咀嚼过程。随着食品工业的快速发展和人们对食品品质要求的日益提高，食品质地评价显得越来越重要，市场上急需快速、客观、准确、类人的食品质地评价方法。咀嚼机器人是一类可以模拟人类咀嚼行为的机器人，它可以真实的复现人类的咀嚼动作并采集分析咀嚼的信息,包括咀嚼力、位移、速度等内容。咀嚼机器人研究是集机构学、运动学、动力学、感知系统、实时控制、食品科学、生物力学及机械电子学于一体的工程科学。咀嚼系统的机器人化可为上述工作提供一种科学的手段。

[0003] 为了使咀嚼机器人运动轨迹平滑，需要对驱动杆的位置进行规划时，驱动杆运动位置的插值多采用3次多项式插值，它需要力矩大，力矩过大，同等减速条件下所要求匹配的电机功率就越大，电机功率越大，电机尺寸跟着增大，价格也可能增加，安装电机所需要的空间增大，增大机器人的体积。力矩过大，同等力臂条件下导致受力过大，需要增加驱动杆及机构整体的强度，提高设计成本。过路径点的3次多项式插值在同样起始点位置和同样的目标位置的情况下，比点到点的3次多项式插值所需力矩小。

[0004] 目前对多自由度机器人控制多采用4轴或6轴驱控一体机如ADT-QC600。或者采用ARM7及DSP等高速处理芯片该类芯片设计机器人系统要求在一个控制周期内完成数据的采集，处理，控制电机同指令的输出。同时需要将采集到的数据上传到上位机，过重的负荷影响到了机器人控制系统的可靠性及实时性并且成本较高。传统的PID控制器结构简单、实现简单、控制效果良好,所以在咀嚼机器人中广泛应用。但由于机器人机械加工精度问题,每个机器人都存在个体差异,很难得到一致的PID控制参数。针对传统PID控制器的特点,将摸糊控制技术引入到PID参数自整定过程中,根据不同情况在线自整定PID参数。通过和传统的PID控制器比较,模糊自适应PID控制器具有具有灵活性好,控制适应性强,动态、静态性能好等优点。模糊控制无需建立精确的数学模型,具有较强的鲁棒性,咀嚼机器人的咬合运动，不同阶段采用不同的控制，张开闭合阶段采用模糊控制，响应更快，咬合阶段采用模糊自适应PID控制，使轨迹更精确，适应性更强，更能满足咀嚼机器人的要求。针对上述问题对咀嚼机器人的控制系统设计采用性价比高STM32F103芯片分为主从分工协作，完成对下颌处压力传感器信号采集AD转换传输任务的同时控制6轴电机完成机器人动作。

发明内容

[0005] 本发明所要解决的技术问题是：

[0006] 1、传统的点到点的3次多项式插值所需力矩大，所需设备体积较大。

[0007] 2、由于机器人机械加工精度问题,每个咀嚼机器人都存在个体差异和六个驱动杆对称性,很难得到一致的PID控制参数的问题。

[0008] 为了解决上述技术问题,本发明才有以下技术方案：

[0009] 一种六自由度咀嚼机器人的控制系统，其包括以下步骤：

[0010] 步骤1：规划咀嚼机器人的各分支杆的运动轨迹，通过切牙点的位置和位姿求出各驱动杆的伸缩，过路径点的3次多项式插值方法求出各驱动杆期望位置和速度；

[0011] 步骤2：根据各分支杆的运动轨迹对机器人的咀嚼过程进行控制，咀嚼过程包括张开、闭合、咬合三个阶段；咬合阶段采用模糊PID控制。

[0012] 在张开和闭合两个阶段，使用模糊控制，通过电机光编码器求解电机在张开闭合时的转速，在张开和闭合两个阶段，使用模糊控制，根据预设的张开闭合的电机的速度值，判断无刷直流电机转子转速，电机转子转速达到预设位置后，执行张开和闭合动作；

[0013] 咬合阶段采用模糊PID控制：以驱动杆给定位置和实际位置的偏差作为输入，利用PID控制器的参数当前值，经过运算产生位置环的速度输出。位置环的速度输出和前馈通道的速度输出，经过叠加后作为总的输出，用作电机的速度给定。

[0014] 上述技术方案中，所述过路径点的3次多项式插值方法如下：

[0015] 在t0时刻的运动速度为 在tf时刻的运动速度为 于是可以得到机器人驱动杆运动的边界条件：

[0016] q(0)＝q0，q(tf)＝qf  (1)

[0017]

[0018] 令驱动杆的位置为：

[0019] q(t)＝a0+a1t+a2t2+a3t3  (3)

[0020] 对其求一阶导数得到驱动杆速度：

[0021]

[0022] 将式(1)和式(2)代入式(3)和式(4)，可以求解出系数a0～a3

[0023]

[0024] 得到过路径点的三次多项式插值的期望位置表达式(5)和速度表达式(6)：

[0025]

[0026]

[0027] 上述技术方案中，PID控制器的参数当前值是PID控制器的参数以设定值为主分量，以模糊控制器产生的△Kp,△Ki和△Kd参数修正值为次要分量，两者相加构成的，△Kp比例参数变化量·△Ki微分参数变化量、△Kd积分参数变化量。

[0028] 与现有咀嚼机器人相比，本咀嚼机器人的显著优点是：

[0029] (1)力矩大，同等减速条件下所要求匹配的电机功率就越大，电机功率越大，电机尺寸跟着增大，价格也可能增加，安装电机所需要的空间增大，增大机器人的体积。在同样起始点位置和同样的目标位置的情况下，过路径点的3次多项式插值比点到点的3次多项式插值所需力矩小。

[0030] (2)不同阶段采用不同控制方法，张开闭合阶段采用模糊控制，速度更快，由于机器人机械加工精度问题,每个咀嚼机器人都存在个体差异和六个驱动杆对称性,很难得到一致的PID控制参数，另外位置变化会导致机器人本体重心的变化，从而使被控对象的参数发生变化，所以应该根据对象参数的变化调整PID控制器的参数。咬合阶段采用模糊自适应PID控制，将摸糊控制技术引入到PID参数自整定过程中,根据不同情况在线自整定PID参数，既保证了系统控制的动态效果,在偏差较大时控制速度较快,采用模糊控制提高控制的快速性,又保证了较好的稳态性能。在设定值附近,采用PID控制算法可以有效地克服系统不稳定因素,使系统控制在稳定中趋于精确,弥补了以前单一的控制算法的缺陷。

附图说明

[0031] 图1表示咀嚼机器人控制系统硬件；

[0032] 图2表示系统框图；

[0033] 图3表示上位机控制界面；

[0034] 图4表示一个驱动杆实际伸缩量曲线

[0035] 图5表示模糊PID控制系统原理图；

[0036] 图6表示位置环变参数模糊PID控制器框图；

[0037] 图7表示电机速度控制图；

[0038] 图8表示无刷直流电机调速系统控制框图。

具体实施方式

[0039] 以下，基于附图对本发明的实施方式进行详细说明：

[0040] 本发明目的在于发明一种仿真人类咀嚼动作以研究食品力学性能的咀嚼机器人控制系统。人类咀嚼运动是二十多块咀嚼肌在收缩受到诸多非确定因素如食物的形状尺寸和韧脆程度的影响，咀嚼肌在神经控制下产生收缩，为下颌运动提供源动力。基于人类咀嚼肌解剖学得知，参与人类咀嚼运动的翼肌包括翼内肌和翼外肌，颞肌由前、中、后三束肌肉组成，咬肌由深层咬肌和浅层咬肌构成，因此本专利利用力学原理的平行四边形法则对主要参与咀嚼运动的翼肌、颞肌和咬肌进行力的合成，确定合成后翼肌、颞肌和咬肌在咀嚼运动过程中咀嚼力的方向、咀嚼肌附着点及咀嚼运动中力的大小，建立参与咀嚼运动中咀嚼肌的三维模型作为控制系统的基础。

[0041] 对于咀嚼机器人来说，为了控制机器人的关节空间运动量，并使关节运动轨迹平滑，关节运动平稳，需要对机器人的关节运动进行规划，主要是包括关节运动轨迹的选择和关节运动位置的插值，一般采用3次多项式插值的方法，但是所需力矩大，力矩大对电机造成影响，使机器不稳定，在同样起始点位置和同样的目标位置的情况下，过路径点的3次多项式插值比点到点的3次多项式插值所需力矩小，所以过路径点的3次多项式插值法更适用于咀嚼机器人系统。运动控制的目的就是使之能精确并且快速地实现点到点以及任意转角运动，目前，较常用的控制方式主要是PID控制法和模糊逻辑控制法。由于控制机器人六根驱动杆的电机的不完全对称性以及场上的各种干扰因素等,机器人的运动具有时变、非线性、干扰大和不确定等特性,用传统的PID控制器已不能达到较好的控制效果。而模糊控制无需建立精确的数学模型,具有较强的鲁棒性,可用于非线性、时变及时滞系统的控制,是一种对模型要求不高但又有良好控制效果的控制新策略。可是,任何一种纯模糊控制器本质上是一种非线性PD控制,不具备积分作用,控制过程有时会出现不平滑现象,稳态误差也较难减至理想程度。因此,模糊PID控制方法无疑可以取得比传统PID控制或单一的模糊逻辑控制更好的控制效果

[0042] 由于机器人机械加工精度问题,每个咀嚼机器人都存在个体差异,很难得到一致的PID控制参数。本文提出了一种基于模糊控制理论的咀嚼机器人控制系统,针对传统PID控制器的特点,将摸糊控制技术引入到PID参数自整定过程中,根据不同情况在线自整定PID参数。通过和传统的PID控制器比较,模糊自适应PID控制器具有具有灵活性好,控制适应性强,动态、静态性能好等优点。

[0043] 控制系统通过控制电机驱动滚珠丝杠的进行旋转，利用滚珠丝杠将旋转运动转化为直线运动，并且经过减速机构将转矩输出。控制电机的转速和旋转方向来改变滚珠丝杠伸缩量的变化，从而完成驱动杆的直线伸缩，达到来模仿咀嚼肌在咀嚼运动时的收缩运动。该咀嚼机器人使用THK公司的滚珠丝杠，丝杠直径4mm，导程为1mm。因此咀嚼机器人给定下颌运动轨迹方程即给定电机转速与时间的函数关系式，通过对六根驱动器的电机进行控制以达到分别控制六根杆伸缩量，从而驱动下颌骨模拟人的咀嚼运动。

[0044] 控制系统通过RS-485总线与上位机进行通信，以STM32F103RB芯片作为主控制器，主频为72MHz，芯片内部集成USART控制器，具有2个12位模数转换器，5个多功能定时器，满足多样和定时的要求，拥有电机控制的各个接口。采用PWM方式控制三相电桥控制电机转速，采用霍尔元件检测转子位置和电机转速，三相电桥的低端增加一个检流电阻测定子电流。主控芯片不断采集实际转速，修正上位机给出的控制转速，实现对电机的转速闭环控制。基于Cortex-M3内核的STM32F103RB芯片是新型的32位嵌入式微处理器,它是不需操作系统的ARM,其性能远高于51系列单片机,但开发过程与51系列单片机一样简便,因而在很多应用场合可替代51系列单片机。Cortex-M3采用了哈佛结构，拥有独立的指令总线和数据总线，可以让取指与数据访问并行不悖；Cortex-M3内部含有好几条总线接口，每条都为自己的应用场合优化过，并且他们可以平行工作。基于STM2F103RB主控芯片控制6台无刷直流电机带动相对应的驱动杆分别模拟咬肌。其中各电机的运动方程由上位机发送给主控芯片以达到实现不同咀嚼动作的目的。通过安装在磨牙等处的传感器对咀嚼力进行采集并进行AD转换反馈给上位机以待分析食物咀嚼特性之用。

[0045] 步骤1：咀嚼机器人的运动轨迹规划：通过切牙点的位置和位姿,求出各驱动杆的伸缩量,过路径点的3次多项式插值方法求出各驱动杆期望位置表达式和速度表达式)为了仿真人类的咀嚼动作，定坐标系OS-XSYSZS固定在颅骨上，动坐标系OM-XMYMZM固定在下颌骨上.使用三维空间测量仪对切牙进行测量,得到下颌骨切牙点的位置参数(x,y,z)T和位姿参数(α,β,γ)T,通过坐标变换和姿态矩阵求出各分支杆的伸缩量，使用过路径点的3次多项式插值方法对各分支杆的运动轨迹规划。考虑某驱动杆从t0时刻的位置q0运动到tf时刻的位置qf的情况。假设在t0时刻的运动速度为 在tf时刻的运动速度为 于是可以得到机器人驱动杆运动的边界条件：

[0046] q(0)＝q0，q(tf)＝qf  (1)

[0047]

[0048] 令驱动杆的位置为：

[0049] q(t)＝a0+a1t+a2t2+a3t3  (3)

[0050] 对其求一阶导数得到驱动杆速度：

[0051]

[0052] 将式(1)和式(2)代入式(3)和式(4)，可以求解出系数a0～a3

[0053]

[0054] 得到过路径点的三次多项式插值的期望位置表达式(5)和速度表达式(6)：

[0055]

[0056]

[0057] 在同样起始点位置和同样的目标位置的情况下，过路径点的3次多项式插值比点到点的3次多项式插值所需力矩小。

[0058] 步骤2：对硬件进行选择和系统图,如图1所示，本发明的控制系统硬件包括机器人控制上位机，以及基于Cortex-M3内核的32位单片机的下位机，其中上下位机利用RS485总线进行通信、主控单片机、最小单片机系统、JTAG调试模块、压力传感器、模拟信号放大电路、SPI控制总线、三相电桥驱动电路、无刷直流电机。

[0059] 图2是系统框图，无刷直流电机的加速度与电动机的转矩成正比，而转矩又与电动机的电流成正比，要实现电机的高精度高动态性能控制，仅仅对转子位置和速度进行检测是不够的，还需要对电机的电流检测和控制。采用控制器核心采用STM32F103RB芯片，驱动部分采用MOSFET构成的三相电桥实现电机所需的三相逆变驱动电路；光电隔离是由FOD3181集成隔离驱动的；采用PWM脉宽调制控制三相电桥调节电机转速，通过电机自带的霍尔传感器检测电机转速及电机位置。实时对电机转速进行检测修正转速表中电机速度，实现对电机的闭环控制。无刷直流电机通过霍尔传感器将三路位置信号与stm32的3个GPIO口相连，对3个接口的电平边沿进行检测，当电平发生变化即产生一个中断从而完成对定子转速计算及换相的功能。电流检测是把检流电阻上的电压信号送入信号调理电路，调理电路分为两部分，第一个是把信号送入一个比较器，作为故障电流报警；第二个是把信号送入一个差分放大电路中，然后直接送入STM32的A/D端口，转化成数字量进行过流判断以及参与电流环运算。

[0060] 将运动参数输入上位机控制界面如图(3)所示。通过测量肌电图可以推出不同时间驱动杆驱动力大小，在选定Faulhaber公司2038无刷直流伺服电机的条件下，将各驱动杆的速度曲线离散为若干个时间节点，发送给下位机存入调速表。且可以通过上位机控制面板设置咀嚼次数n发送给主控芯片。

[0061] 压力传感器采用MD-PS002型号，安装在下颌不同牙齿处采集咀嚼时牙齿的受力情况。由于传感器输出的电压信号较为微弱，因此使用运算放大器对传感器信号进行放大处理。在该系统中使用TL431作为ADC的基准电压。利用STM32中的通用定时器的定时中断功能，当定时器溢出时，处理器根据中断子程序内容，对压力传感器处的信号进行AD转换如果大于0则发送给上位机。

[0062] 主控芯片负责将的启动停止标志位存入数组START[6]及STOP[6]，开启基本定时器后分别检测各电机对应的启动停止标志，达到控制各步进电机的启动停止顺序的任务。

[0063] 步骤3：机器人的位置控制：不同阶段采用不同的控制方法，张开闭合阶段采用模糊控制；咬合阶段采用模糊PID控制系统)咀嚼运动是周期性的咬合运动，一个正常的咬合过程可以被分为三个阶段：张开、闭合和咬合。通过对不同阶段进行不同控制，在张开和闭合两个阶段，使用模糊控制，根据预先在STM32中保存这两种状态关键位置的速度、位置等值，通过对无刷直流电机定子转速及转子位置进行检测，进行模糊控制，达到预设位置后，执行张开和闭合动作。图(4)若a、b位置为张开和闭合位置，因模糊控制具有不依赖数学模型，控制速度快，精度高，在一个干扰环境中，具有较强的鲁棒性，张开闭合的动作瞬间完成，一个咬合过程时间从0-1变为a-b，时间更短。

[0064] 如图5为模糊PID控制系统原理图，E、EC为模糊变量，输入变量有输出偏差E与输出偏差变化率EC，以偏差E和偏差的变化率Ec作为控制器的二维输入量,经过模糊控制器按模糊推理规则处理,输出△Kp,△Ki和△Kd用于整定PID参数。机器人调节的初期,偏差E最大,为加快系统的响应速度,并避免开始时误差瞬间变大可能引起的微分过饱和,而使控制作用超出许可范围,应取较大的△Kp和较小的△Kd,同时为避免系统响应出现较大的超调,应对积分作用加以限制。在调节过程中期,为使系统响应具有较小的超调,并保持机器人的稳定,△Kp、△Ki和△Kd都不应取得过大,在这种情况下,△Kd的取值对系统响应的影响较大。在调节过程后期,为提高控制精度减少静差,使系统具有较好的稳态性能,△Ki和△Kp均应取大些；为避免系统在设定值附近出现振荡,并考虑系统的抗干扰性能,当误差变化较大时,△Kd可取大些；当误差变化较小时,△Kd应取小些。

[0065] 如图6表示位置环变参数模糊PID控制器框图，位置的实际测量值作为模糊控制器的输入，按照图5模糊控制规则导出PID控制器参数Kp、Ki和Kd的修正值。PID控制器的参数以设定值为主分量，以模糊控制器产生的Kp、Ki和Kd的修正值为次要分量，两者相加构成PID控制器的参数当前值。PID控制器以给定位置与实际位置的偏差作为输入，利用PID控制器的参数当前值，经过运算产生位置环的速度输出。速度前馈通道中的滤波器，用于对位置给定信号滤波，滤波后再经过微分并乘以一个比例系数，作为速度前馈通道的速度输出。位置环的速度输出和前馈通道的速度输出，经过叠加后作为总的输出，用作驱动器的速度给定。校正环用于改善系统的动态品质，需要根据对象和驱动器的模型进行设计。

[0066] (第4步速度控制)STM32通用定时器是一个预分频器(PSC)驱动的16位自动装载计数器(CNT)构成。通过TIM1和TIM8两个高级定时器可以实现输出3路互补PWM波且每个端口都可以通过三相电桥对无刷直流电机进行脉宽调制。通过修改高级定时器的捕获/比较模式寄存器(TIMx_CCER1/2)、捕获比较使能寄存器(TIMx_CCER)、捕获比较寄存器(TIM_CCR)、以及刹车死区寄存器(TIM_BDTR)通过调用STM32官方库函数TIM_setcompare()，依据丝杠伸缩量—时间函数关系，通过修改TIM—>CCR寄存器的值控制PWM占空比如图4所示达到电机调速的目的。

[0067] 如图8三相无刷直流电机用软件实现全数字双闭环控制，给定速度和速度反馈量形成偏差，经速度调节后产生电流参考量，它与电流反馈量形成的偏差经电流调节后形成PWM占空比的控制量，实现电动机的速度控制。速度反馈则是通过霍尔传感器输出的位置量，经过计算得到；其位置传感器输出的位置量还用于控制换相。其中电流环的作用是提高系统的快速性，抑制电流环内部干扰，限制最大电流保障系统安全运行，电流环采用变速积分PI算法。速度环的作用是增加系统抗负载扰动的能力，抑制速度波动，保证系统静态精度和动态跟踪的性能，速度环采用积分分离的PI控制算法。

[0068] 电流环通过对电机电流的控制使电机表现出期望的力矩特性，速度环使电机表现出期望的速度特性，位置环是控制系统外环，使电机到达期望的位置，从而使驱动杆达到期望的位置，使咀嚼机器人按规划的轨迹进行运动。