[发明专利]一种六自由度咀嚼机器人的控制系统

权利要求书

1.一种六自由度咀嚼机器人的控制系统，包括步骤：步骤1：规划咀嚼机器人的各分支杆的运动轨迹，通过切牙点的位置和位姿求出各驱动杆的伸缩，过路径点的3次多项式插值方法求出各驱动杆期望位置和速度，其特征在于，还包括以下步骤：

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

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

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

2.根据权利要求1所述的一种六自由度咀嚼机器人的控制系统，其特征在于，所述过路径点的3次多项式插值方法如下：

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

q(0)＝q₀，q(t_f)＝q_f(1)

令驱动杆的位置为：

q(t)＝a₀+a₁t+a₂t²+a₃t³(3)

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

将式(1)和式(2)代入式(3)和式(4)，可以求解出系数a₀～a₃a₀＝q₀；

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

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