动学模型
[0080] 其中
[0082] 步骤2)系统的动力学模型为
[0086] M。为包含机器人质量M、康复者质量m和转动惯量I。的系数矩阵，Β( Θ )为机器人 旋转角度构成的系数矩阵，u(t)表示机器人的控制输入力，r。为机器人中心到康复者重心 偏移的距离；
[0087] 基于冗余机器人特征，令两个输入力f2= f4,得到如下动力学模型
[0091] ??(〇表示机器人转化为非冗余系统后的控制输入力，互(的表示系数矩阵；
[0092] 步骤3)由运动学模型（2)得如下表达形式：
[0096] 对模型（5)两边同时微分并结合动力学模型（4)，得
[0100] 步骤4)定义系统状态变量
[0102]系统模型（6)化为如下仿射非线性系统
[0104] 针对系统模型（7)设计非线性反馈控制律
[0106] 系统（7)化为如下形式
[0110] I表示具有恰当维数的单位矩阵；
[0111] 定义系统输出向量y为
[0113] 对输出向量y进行微分，得
[0115] 得各驱动轮转速与驱动力之间的解耦状态方程如下
[0117] 步骤5)设计驱动力控制器，基于解耦状态方程，使康复步行训练机器人的运动速 度实现渐近跟踪；驱动力控制器结合非线性反馈控制律，基于康复步行训练机器人动力学 模型，使运动轨迹实现渐近跟踪，康复机器人实际行走轨迹X，医生指定训练轨迹X d;对X和 心分别微分，得机器人实际运动速度
和医生指定运动速度为
设运动速度 跟踪误差为
[0119] 驱动力控制器为
[0121] 在控制器（13)作用下，得到速度跟踪误差模型如下
[0123] 选取适当的控制器增益矩阵Kp，使运动速度跟踪渐近稳定；同时驱动力控制器 (13)代入到非线性反馈控制律⑶中，基于康复步行训练机器人动力学模型（4)使运动轨 迹跟踪渐近稳定。
[0124] 所述步骤4) I表示具有三维数的单位矩阵。
[0125] 所述步骤5)选取3X3控制器增益矩阵Kp。
[0126] 本发明基于MSP430系列单片机将输出PffM信号提供给电机驱动单元，使机器人同 时对参考轨迹信号的运动速度和运动轨迹跟踪，以MSP430系列单片机为主控制器，主控制 器的输入接电机测速模块、输出接电机驱动模块；电机驱动电路与直流电机相连；电源系 统给各个电气设备供电。
[0127] 所述主控制器控制方法为读取电机编码器的反馈信号与主控制器给定的控制命 令信号&和计算得出误差信号；根据误差信号，主控制器按照预定的控制算法计算出 电机的控制量，送给电机驱动单元，电机转动带动轮子维持自身平衡及按指定方式运动。
[0128] 可以理解的是，以上关于本发明的具体描述，仅用于说明本发明而并非受限于本 发明实施例所描述的技术方案，本领域的普通技术人员应当理解，仍然可以对本发明进行 修改或等同替换，以达到相同的技术效果；只要满足使用需要，都在本发明的保护范围之 内。
【主权项】
1.康复训练机器人运动速度和运动轨迹同时跟踪的控制方法，其特征在于包括W下步 骤： 步骤1)建立各驱动轮转速与驱动力之间的解禪状态方程，系统的运动学模型为=(1)： 其中V表示机器人四个驱动轮的运动速度，X为机器人的实际行走轨迹，Κ。表示系数矩阵， Θ表示水平轴和机器人中屯、与第一个轮子中屯、连线间的夹角，L表示机器人中屯、到各个轮 子的距离，x(t)为机器人横轴方向的实际行走轨迹、y(t)为机器人纵轴方向的实际行走轨 迹，Θ(t)为机器人旋转方向的实际行走轨迹，t为机器人的实际行走时间； 基于冗余自由度特征有Vi+V2=V3+V4成立，得到如下运动学模型步骤2)系统的动力学模型为M。为包含机器人质量M、康复者质量m和转动惯量I。的系数矩阵，B(Θ)为机器人旋转 角度构成的系数矩阵，u(t)表示机器人的控制输入力，r。为机器人中屯、到康复者重屯、偏移 的距离； 基于冗余机器人特征，令两个输入力f2=f4,得到如下动力学模型泣汾表示机器人转化为非冗余系统后的控制输入力，忘(巧表示系数矩阵； 步骤3)由运动学模型（2)得如下表达形式： ?V = (y) 其中对模型（5)两边同时微分并结合动力学模型（4)，得I表示具有恰当维数的单位矩阵； 定义系统输出向量y为(10) 对输出向量y进行微分，得I、Π; 得各驱动轮转速与驱动力之间的解禪状态方程如下 託')=打斬 (12) 步骤5)设计驱动力控制器，康复机器人实际行走轨迹X，医生指定训练轨迹Xd;对X和Xd分别微分，得机器人实际运动速度v(〇 =义和医生指定运动速度为=不/，设运动速度 跟踪误差为 aA!) =x,-r 驱动力控制器为(13) 在控制器（13)作用下，得到速度跟踪误差模型如下(14) 选取适当的控制器增益矩阵Κρ，使运动速度跟踪渐近稳定；同时驱动力控制器（13)代 入到非线性反馈控制律（8)中，基于康复步行训练机器人动力学模型（4)使运动轨迹跟踪 渐近稳定。2. 根据权利要求1所述康复训练机器人运动速度和运动轨迹同时跟踪的控制方法，其 特征在于所述步骤4)I表示具有Ξ维数的单位矩阵。3. 根据权利要求1所述康复训练机器人运动速度和运动轨迹同时跟踪的控制方法，其 特征在于所述步骤5)选取3X3控制器增益矩阵Κρ。4. 根据权利要求1所述康复训练机器人运动速度和运动轨迹同时跟踪的控制方法，其 特征在于基于MSP430系列单片机将输出PWM信号提供给电机驱动单元，使机器人同时对参 考轨迹信号的运动速度和运动轨迹跟踪，WMSP430系列单片机为主控制器，主控制器的输 入接电机测速模块、输出接电机驱动模块；电机驱动电路与直流电机相连。5. 根据权利要求4所述康复训练机器人运动速度和运动轨迹同时跟踪的控制方法，其 特征在于所述主控制器控制方法为读取电机编码器的反馈信号与主控制器给定的控制命 令信号Xd和义?，计算得出误差信号；根据误差信号，主控制器按照预定的控制算法计算出 电机的控制量，送给电机驱动单元，电机转动带动轮子维持自身平衡及按指定方式运动。
【专利摘要】康复训练机器人运动速度和运动轨迹同时跟踪的控制方法属于轮式康复机器人的控制领域，尤其涉及一种康复训练机器人运动速度和运动轨迹同时跟踪的控制方法。本发明就是针对上述问题，提供一种可有效提高训练者的安全性和康复效果的康复训练机器人运动速度和运动轨迹同时跟踪的控制方法。本发明基于康复步行训练机器人的运动学模型和动力学模型，冗余自由度特征，非线性输入-输出线性化理论，建立各驱动轮转速与驱动力之间的解耦状态方程；设计驱动力控制器，基于解耦状态方程，使康复步行训练机器人的运动速度实现渐近跟踪；进一步，驱动力控制器结合非线性反馈控制律，基于康复步行训练机器人动力学模型，使运动轨迹实现渐近跟踪。
【IPC分类】G05D1/02, A61H1/00
【公开号】CN105320138
【申请号】CN201510850021
【发明人】孙平, 刘博 , 孙桐, 李树江, 薛伟霖, 常洪彬
【申请人】沈阳工业大学
【公开日】2016年2月10日
【申请日】2015年11月28日