[发明专利]康复训练机器人运动速度和运动轨迹同时跟踪的控制方法

权利要求书

1.康复训练机器人运动速度和运动轨迹同时跟踪的控制方法，其特征在于包括以下步骤：

步骤1)建立各驱动轮转速与驱动力之间的解耦状态方程，系统的运动学模型为

其中

V表示机器人四个驱动轮的运动速度，X为机器人的实际行走轨迹，K_c表示系数矩阵，θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，L表示机器人中心到各个轮子的距离，x(t)为机器人横轴方向的实际行走轨迹、y(t)为机器人纵轴方向的实际行走轨迹，θ(t)为机器人旋转方向的实际行走轨迹，t为机器人的实际行走时间；

基于冗余自由度特征有v₁+v₂＝v₃+v₄成立，得到如下运动学模型

其中

步骤2)系统的动力学模型为

其中

M₀为包含机器人质量M、康复者质量m和转动惯量I₀的系数矩阵，B(θ)为机器人旋转角度构成的系数矩阵，u(t)表示机器人的控制输入力，r₀为机器人中心到康复者重心偏移的距离；

基于冗余机器人特征，令两个输入力f₂＝f₄，得到如下动力学模型

其中

表示机器人转化为非冗余系统后的控制输入力，表示系数矩阵；

步骤3)由运动学模型(2)得如下表达形式:

其中

对模型(5)两边同时微分并结合动力学模型(4)，得

其中

步骤4)定义系统状态变量

系统模型(6)化为如下仿射非线性系统

针对系统模型(7)设计非线性反馈控制律

系统(7)化为如下形式

其中

I表示具有恰当维数的单位矩阵；

定义系统输出向量y为

对输出向量y进行微分，得

得各驱动轮转速与驱动力之间的解耦状态方程如下

步骤5)设计驱动力控制器，康复机器人实际行走轨迹X，医生指定训练轨迹X_d；对X和X_d分别微分，得机器人实际运动速度和医生指定运动速度为设运动速度跟踪误差为

驱动力控制器为

在控制器(13)作用下，得到速度跟踪误差模型如下

选取适当的控制器增益矩阵K_p，使运动速度跟踪渐近稳定；同时驱动力控制器(13)代入到非线性反馈控制律(8)中，基于康复步行训练机器人动力学模型(4)使运动轨迹跟踪渐近稳定。

2.根据权利要求1所述康复训练机器人运动速度和运动轨迹同时跟踪的控制方法，其特征在于所述步骤4)I表示具有三维数的单位矩阵。

3.根据权利要求1所述康复训练机器人运动速度和运动轨迹同时跟踪的控制方法，其特征在于所述步骤5)选取3×3控制器增益矩阵K_p。

4.根据权利要求1所述康复训练机器人运动速度和运动轨迹同时跟踪的控制方法，其特征在于基于MSP430系列单片机将输出PWM信号提供给电机驱动单元，使机器人同时对参考轨迹信号的运动速度和运动轨迹跟踪，以MSP430系列单片机为主控制器，主控制器的输入接电机测速模块、输出接电机驱动模块；电机驱动电路与直流电机相连。

5.根据权利要求4所述康复训练机器人运动速度和运动轨迹同时跟踪的控制方法，其特征在于所述主控制器控制方法为读取电机编码器的反馈信号与主控制器给定的控制命令信号X_d和计算得出误差信号；根据误差信号，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动单元，电机转动带动轮子维持自身平衡及按指定方式运动。