康复训练机器人运动速度和运动轨迹同时跟踪的控制方法
【技术领域】
[0001] 本发明属于轮式康复机器人的控制领域，尤其涉及一种康复训练机器人运动速度 和运动轨迹同时跟踪的控制方法。
【背景技术】
[0002] 随着老龄化社会的到来，由于疾病和交通事故等原因，使步行功能障碍患者逐年 增多。然而，我国医护资源不足和医疗费用昂贵等问题，使许多患者错过了最佳恢复期，从 而导致患者步行功能逐渐丧失，给家庭和社会带来沉重的负担。因此，发展康复机器人及时 有效对患者进行步行功能训练具有重要意义。
[0003] 康复步行训练机器人需要跟踪医生指定的训练轨迹对患者进行训练，有关康复机 器人轨迹跟踪控制方法已有许多研究成果，然而这些成果在考虑轨迹跟踪时都忽视了速度 跟踪。机器人运行在未知环境中，如果运动速度过快超过患者承受能力，会使患者再次受伤 威胁其安全；如果运动速度过慢，会使患者达不到运动强度而影响康复效果，因此探索康复 机器人运动速度和运动轨迹同时跟踪的控制方法尤为重要。本发明所研究的康复步行训练 机器人是冗余机器人，到目前为止，还没有将运动学模型和动力学模型相结合使冗余康复 机器人运动速度和运动轨迹同时跟踪的控制方法。因此研究如何使康复步行训练机器人同 时实现速度跟踪和轨迹跟踪具有重要意义。
【发明内容】
[0004] 本发明就是针对上述问题，提供一种可有效提高训练者的安全性和康复效果的康 复训练机器人运动速度和运动轨迹同时跟踪的控制方法。
[0005] 为实现上述目的，本发明采用如下技术方案，本发明包括以下步骤：
[0006] 步骤1)建立各驱动轮转速与驱动力之间的解耦状态方程，系统的运动学模型为
[0010] V表示机器人四个驱动轮的运动速度，X为机器人的实际行走轨迹，K。表示系数矩 阵，Θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，L表示机器人中心到各 个轮子的距离，X (t)为机器人横轴方向的实际行走轨迹、y(t)为机器人纵轴方向的实际行 走轨迹，Θ (t)为机器人旋转方向的实际行走轨迹，t为机器人的实际行走时间；
[0011] 基于冗余自由度特征有心+^2= v3+v4成立，得到如下运动学模型
[0013] 其中
[0015] 步骤2)系统的动力学模型为
[0019] M。为包含机器人质量M、康复者质量m和转动惯量IO的系数矩阵，Β( Θ )为机器人 旋转角度构成的系数矩阵，u(t)表示机器人的控制输入力，r。为机器人中心到康复者重心 偏移的距离；
[0020] 基于冗余机器人特征，令两个输入力f2= f4,得到如下动力学模型
[0024] 印)表示机器人转化为非冗余系统后的控制输入力亙(的表示系数矩阵；
[0025] 步骤3)由运动学模型（2)得如下表达形式：
[0027] 其中
[0029] 对模型（5)两边同时微分并结合动力学模型（4)，得
[0031] 其中
[0033] 步骤4)定义系统状态变量
[0035] 系统模型（6)化为如下仿射非线性系统
[0037] 针对系统模型（7)设计非线性反馈控制律
[0039] 系统（7)化为如下形式
[0043] I表示具有恰当维数的单位矩阵；
[0044] 定义系统输出向量y为
[0046] 对输出向量y进行微分，得
[0048] 得各驱动轮转速与驱动力之间的解耦状态方程如下
[0050] 步骤5)设计驱动力控制器，康复机器人实际行走轨迹X，医生指定训练轨迹1;对 X和心分别微分，得机器人实际运动速度呛)=I和医生指定运动速度为
设运动 速度跟踪误差为
[0052] 驱动力控制器为
[0054] 在控制器（13)作用下，得到速度跟踪误差模型如下
[0056] 选取适当的控制器增益矩阵Kp，使运动速度跟踪渐近稳定；同时驱动力控制器 (13)代入到非线性反馈控制律⑶中，基于康复步行训练机器人动力学模型（4)使运动轨 迹跟踪渐近稳定。
[0057] 作为一种优选方案，本发明所述步骤4) I表示具有三维数的单位矩阵。
[0058] 作为另一种优选方案，本发明所述步骤5)选取3X3控制器增益矩阵Kp。
[0059] 其次，本发明基于MSP430系列单片机将输出PffM信号提供给电机驱动单元，使机 器人同时对参考轨迹信号的运动速度和运动轨迹跟踪，以MSP430系列单片机为主控制器， 主控制器的输入接电机测速模块、输出接电机驱动模块；电机驱动电路与直流电机相连；
[0060] 另外，本发明所述主控制器控制方法为读取电机编码器的反馈信号与主控制器给 定的控制命令信号&和，计算得出误差信号；根据误差信号，主控制器按照预定的控制 算法计算出电机的控制量，送给电机驱动单元，电机转动带动轮子维持自身平衡及按指定 方式运动。
[0061] 本发明有益效果。
[0062] 本发明基于冗余康复步行训练机器人的特征，结合运动学和动力学模型，应用非 线性输入-输出线性化理论，建立各驱动轮转速与驱动力之间的解耦状态方程。
[0063] 本发明设计驱动力控制器，基于解耦状态方程，使康复步行训练机器人的运动速 度实现渐近跟踪；驱动力控制器结合非线性反馈控制律，基于康复步行训练机器人动力学 模型，使运动轨迹实现渐近跟踪。
[0064] 本发明解决了康复步行训练机器人运动速度和运动轨迹同时跟踪问题，基于冗余 自由度特征和非线性输入-输出反馈线性化理论建立各驱动轮转速与驱动力之间的解耦 状态方程，易于实现速度跟踪，在实现运动速度跟踪的基础上再实现运动轨迹跟踪，有效的 提高了训练者的安全性和康复效果。
【附图说明】
[0065] 下面结合附图和【具体实施方式】对本发明做进一步说明。本发明保护范围不仅局限 于以下内容的表述。
[0066] 图1为本发明控制器工作框图。
[0067] 图2为本发明的MSP430单片机最小系统。
[0068] 图3为本发明的主控制器外围扩展电路。
[0069] 图4为本发明硬件总体原理电路。
[0070] 图5为本发明机器人结构坐标图。
[0071] 图5中，xOy为固定坐标系，X' Cy'为机器人机构坐标系，G为机器人与康复者构 成人机系统的重心。
【具体实施方式】
[0072] 如图所示，本发明包括以下步骤：
[0073] 步骤1)基于冗余康复步行训练机器人的特征，结合运动学和动力学模型，应用非 线性输入-输出线性化理论，建立各驱动轮转速与驱动力之间的解耦状态方程，系统的运 动学模型为
[0077] V表示机器人四个驱动轮的运动速度，X为机器人的实际行走轨迹，K。表示系数矩 阵，Θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，L表示机器人中心到各 个轮子的距离，X (t)为机器人横轴方向的实际行走轨迹、y(t)为机器人纵轴方向的实际行 走轨迹，Θ (t)为机器人旋转方向的实际行走轨迹，t为机器人的实际行走时间；
[0078] 基于冗余自由度特征WVl+v2= v3+v4成立，得到如下运