康复步行训练机器人的轨迹跟踪误差约束安全控制方法
【专利摘要】康复步行训练机器人的轨迹跟踪误差约束安全控制方法属于轮式康复机器人的控制领域，尤其涉及一种康复步行训练机器人的轨迹跟踪误差约束安全控制方法。本发明提供一种使康复步行训练机器人在运动过程中，暂态阶段和稳态阶段轨迹跟踪误差同时受约束的康复步行训练机器人的轨迹跟踪误差约束安全控制方法。本发明基于康复步行训练机器人的动力学模型，设计非线性控制器，建立x,y轴和旋转角三个运动方向的轨迹跟踪误差状态方程；基于Lyapunov稳定理论，构建各轨迹跟踪误差系统渐近稳定的约束条件，获得非线性控制器参数矩阵的求解方法，将康复步行训练机器人暂态阶段和稳态阶段的实际运动轨迹控制在指定范围内，保障康复者安全。
【专利说明】
康复步行训练机器人的轨迹跟踪误差约束安全控制方法
技术领域
[0001] 本发明属于轮式康复机器人的控制领域，尤其涉及一种康复步行训练机器人的轨 迹跟踪误差约束安全控制方法。
【背景技术】
[0002] 步行是人类日常自立生活重要的基本活动之一，随着高龄人口增加，老年人腿部 肌肉力量逐渐减弱，如果不及时加强老年人步行训练，会导致步行功能逐渐丧失，无法实现 日常独立生活。由于我国缺少专业护理人员，同时家庭青年人口减少，如何帮助老年人恢复 步行运动功能成为严重的社会问题。因此，发展康复步行训练机器人，帮助老年人安全地进 行步行训练具有重要意义。
[0003] 康复步行训练机器人需要跟踪医生指定的轨迹对患者进行训练，有关康复机器人 轨迹跟踪控制方法已有许多研究成果，然而这些成果都忽略了暂态阶段的跟踪性能。机器 人运行在室内未知环境中，如果暂态阶段轨迹跟踪误差过大，机器人会碰撞周围人或物体， 威胁训练者的安全。已有研究结果仅仅考虑了机器人稳态阶段的跟踪性能，导致这些控制 方法在实际应用中均具有一定的局限性。到目前为止，还没有同时约束暂态阶段和稳态阶 段轨迹跟踪误差的控制方法。本发明研究轨迹跟踪误差约束的安全控制方法，如何约束康 复步行训练机器人在整个运动过程中的轨迹跟踪误差，保障使用者的安全具有重要意义。
【发明内容】
[0004] 本发明就是针对上述问题，提供一种使康复步行训练机器人在运动过程中，暂态 阶段和稳态阶段轨迹跟踪误差同时受约束的康复步行训练机器人的轨迹跟踪误差约束安 全控制方法。
[0005] 为实现上述目的，本发明采用如下技术方案，本发明包括以下步骤：
[0006] 步骤1)建立X，y轴和旋转角三个运动方向的轨迹跟踪误差状态方程，系统的运动 学模型如下：
[0012] X(t)为康复步行训练机器人的实际行走轨迹，u(t)表示控制输入力，Μ表示康复步 行训练机器人的质量，m表示康复者的质量，1〇表示转动惯量，Μ。, Α'(6〇, 为系数矩 阵；Θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，0 = 0:，根据康复步行机器
^表示系统重心到每个轮子中心的距离，r〇表 示中心到重心的距离，Φ i表示V轴和每个轮子对应的li之间的夹角，i = l,2,3,4;fi，f2，f3， f4分别表不四个驱动轮的控制输入力山>2>3,\4分别表不重心到各轮子的距离，Φ 1, Φ 2, Φ 3, Φ4分别表示水平坐标轴和重心到各轮子中心连线的夹角；
[0013] 步骤2)康复步行训练机器人实际行走轨迹X(t)，医生指定训练轨迹Xd(t)，运动轨 迹和运动速度跟踪误分别为
[0014] ei(t)=X(t)-Xd(t) (2)
[0015] ￡%的:=龙^>-岛的： (3)
[0016] 其中X(t) = [x(t) y(t) 0(t)]T表示x，y轴和旋转角方向的实际运动轨迹，Xd(t) = [xd(t) yd(t) 0d(t)]T表示x，y轴和旋转角方向的期望跟踪运动轨迹;ei(t) = [en(t) ei2 (t) ei3(t)]T表示x，y轴和旋转角方向的轨迹跟踪误差，e2(t) = [e2i(t) e22(t) 623(1:)]1? 示x，y轴方向速度和旋转角速度的跟踪误差；
[0017] 步骤3)设置非线性控制器：
[0018] iAj) = Ι^)\Ψ1ΜΘΜΧ{ι)+{ΜΜΘ))\ΧΛΙ)^ Ktlc:(l)^K(/1 )
[0019] 其中Kd = diag{Kdi，Kd2，Kd3} 'Kp = diag{Kpi，Kp2，KP3} ' 6(60 = 51'(沒)(5(沒)5Γ(0))-1表示B (θ)的伪逆矩阵，Kd，心表示控制器参数矩阵；
[0020] 将控制器公式(4)代入系统模型公式(1)，得
[0021] ?). (?). =. 勒. （5 )
[0022] 由式(2)、（3)、（5)可得x，y轴和旋转角方向轨迹跟踪误差系统状态方程为 「m"i ~(') =~⑴ 7 = 123 (6)
[0024] 步骤4)设置Lyapunov函数：
[0028]沿轨迹跟踪误差系统(6)对式(8)求导，在如下约束条件成立时，
[0030] 有
[0031] ν^-ε^??^ ( 1〇)
[0032] 成立，即x，y轴和旋转角三个运动方向的轨迹跟踪误差系统渐近稳定，其中^表示 任意指定的小正数；
[0033]步骤5)对式(10)两端从0到t积分，得
[0035] 进一步整理得
[0036] Vi(t)-Vi(0)<-ei(|eii(t)|-|eii(0)|) (12)
[0037] 当如下条件成立时
[0039] 有下式成立
[0041] 由式(8)、（14)、（12)得
[0042] <￡·; +￡·，!?,,(〇){ ( 1.5)
[0043] 于是有
[0044] en(t)| <ei+|eii(0) (16)
[0045] 康复步行训练机器人的实际轨迹X(t)的安全运动范围为
[0046] x(t)-xd(t)| < ει+|x(0)-xd(0) (17)
[0047] y(t)-yd(t)| < ε2+|y(0)-yd(0) (18)
[0048] 0(t)-9d(t)| < e 2+|0(〇)-0d(〇) (19)。
[0049] 作为一种优选方案，本发明基于MSP430系列单片机将输出PWM信号提供给电机驱 动单元，使康复步行训练机器人的轨迹跟踪误差限制在指定范围内；以MSP430系列单片机 为主控制器，主控制器的输入接电机测速模块、输出接电机驱动模块;电机驱动电路与直流 电机相连;电源系统给各个电气设备供电。
[0050] 作为另一种优选方案，本发明所述主控制器控制方法为:读取电机编码器的反馈 信号与主控制器给定的控制命令信号Xd(t)和W)，计算得出误差信号;根据误差信号，主 控制器按照预定的控制算法计算出电机的控制量，送给电机驱动单元，电机转动带动轮子 维持自身平衡及按指定方式运动。
[0051 ]本发明有益效果。
[0052]本发明基于康复步行训练机器人的动力学模型，根据运动速度和运动位置跟踪误 差，设计非线性控制器，建立X，y轴和旋转角三个运动方向的轨迹跟踪误差状态方程;基于 Lyapunov稳定理论，构建各轨迹跟踪误差系统渐近稳定的约束条件，获得非线性控制器参 数矩阵的求解方法，将康复步行训练机器人暂态阶段和稳态阶段的实际运动轨迹控制在指 定范围内，保障康复者安全。
[0053]本发明结合动力学模型，设计的非线性控制器，能建立x，y轴和旋转角三个运动方 向的轨迹跟踪误差状态方程;分别构建三个轨迹跟踪误差系统的渐近稳定条件，在约束轨 迹跟踪误差的基础上，求取康复步行训练机器人的安全运动位置范围。本发明控制器设计 简单，易于实现，控制器不仅使康复步行训练机器人实现轨迹跟踪，而且能主动约束实际运 动轨迹，在安全运动区域内实现暂态阶段和稳态阶段的跟踪性能，该控制方法能提高训练 者的安全性。
【附图说明】
[0054]下面结合附图和【具体实施方式】对本发明做进一步说明。本发明保护范围不仅局限 于以下内容的表述。
[0055]图1为本发明控制器工作框图；
[0056]图2为本发明系统坐标图；
[0057]图3为本发明MSP430单片机最小系统；
[0058]图4为本发明主控制器外围扩展电路；
[0059]图5为本发明硬件总体原理电路。
[0060]图2中，xOy为固定坐标系，YC/为机器人机构坐标系，G为机器人与康复者构成人 机系统的重心。
【具体实施方式】
[0061 ]如图所示，本发明总体步骤如下：
[0062] 1)根据康复步行训练机器人对医生指定训练轨迹的运动速度和运动位置跟踪误 差，设计非线性控制器，结合动力学模型，建立x，y轴和旋转角三个运动方向的轨迹跟踪误 差状态方程；
[0063] 2)设计Lyapunov函数，构建各轨迹跟踪误差系统渐近稳定的约束条件，获得非线 性控制器参数矩阵的求解方法，使康复步行训练机器人暂态阶段和稳态阶段的实际运动轨 迹约束在指定范围内；
[0064] 3)基于MSP430系列单片机将输出PWM信号提供给电机驱动单元，使康复步行训练 机器人在安全位置范围内跟踪医生指定的训练轨迹。
[0065]本发明具体步骤如下：
[0066] 步骤1)基于康复步行训练机器人的动力学模型，根据运动速度和运动位置跟踪误 差，建立x，y轴和旋转角三个运动方向的轨迹跟踪误差状态方程，系统的运动学模型如下
[0067] Μ ,K W )XU) +Μ l：K {θ.Θ)Χ in = Β(θ)!：α) ( 1)
[0068] 其中
[0072] X(t)为康复步行训练机器人的实际行走轨迹，u(t)表示控制输入力，Μ表示康复步 行训练机器人的质量，m表示康复者的质量，1〇表示转动惯量，]/。1(6^(6^),执6>)为系数矩 阵。Θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，即0 = 0:，由康复步行机器
^表示系统重心到每个轮子中心的距离， ro表示中心到重心的距离，Φ i表示X'轴和每个轮子对应的li之间的夹角，i = l，2,3,4。
[0073] 步骤2)基于康复步行训练机器人的动力学模型，根据运动速度和运动位置跟踪误 差，建立X，y轴和旋转角三个运动方向的轨迹跟踪误差状态方程，康复步行训练机器人实际 行走轨迹X(t)，医生指定训练轨迹Xd(t)，设运动轨迹和运动速度跟踪误差 ei(tWPe2(t)分 别为
[0074] ei(t)=X(t)-Xd(t) (2)
[0075] e2.(〇 = i^(i).-Xd(〇 (3)
[0076] 其中X(t) = [x(t) y(t) 0(t)]T表示x，y轴和旋转角方向的实际运动轨迹，Xd(t) = [xd(t) yd(t) 0d(t)]T表示x，y轴和旋转角方向的期望跟踪运动轨迹。ei(t) = [en(t) ei2 (t) ei3(t)]T表示x，y轴和旋转角方向的轨迹跟踪误差，e2(t) = [e2i(t) e22(t) 623(1:)]1? 示x，y轴方向速度和旋转角速度的跟踪误差。
[0077]设计非线性控制器如下
[0078] 冲）=/狀 K(.K 幻及办义(，) + (M,A冲 >)| 足,(，) +人）:(0 + (.0)1} ( 1 )
[0079] 其中Kd = diag{Kdi ,Kd2 ,Kd3}，Kp = diag{Kpi ,KP2,KP3 }，》(<?) = #(60(5(60,(0))-1 表示B (Θ)的伪逆矩阵。
[0080] 将控制器(4)代入系统模型（1)，得
[0081] e\ (i) = Kde2 (r) + Κ.ρββ) ( 〇 )
[0082] 由式(2)、（3)、（5)可得x，y轴和旋转角方向轨迹跟踪误差系统状态方程为 「00831 / = 1-2.3 (6)
[0084]步骤3)设计Lyapunov函数，构建各轨迹跟踪误差系统渐近稳定的约束条件，获得 非线性控制器参数矩阵的求解方法，使康复步行训练机器人暂态阶段和稳态阶段的实际运 动轨迹控制在指定范围内；设计Lyapunov函数如下
[0088]沿轨迹跟踪误差系统(6)对式(8)求导，在如下约束条件成立时，
[0090] 有
[0091] Ιζ <-ε.\β2ιφ\ (1〇)
[0092] 成立，即x，y轴和旋转角三个运动方向的轨迹跟踪误差系统渐近稳定，其中^表示 任意指定的小正数。
[0093]步骤4)设计Lyapunov函数，构建各轨迹跟踪误差系统渐近稳定的约束条件，获得 非线性控制器参数矩阵的求解方法，使康复步行训练机器人暂态阶段和稳态阶段的实际运 动轨迹约束在指定范围内；其特征在于:对式(10)两端从0到t积分，得
[0095] 进一步整理得
[0096] Vi(t)-Vi(0)<-ei(|eii(t)|-|eii(0)|) (12)
[0097] 当如下条件成立时
[0099] 有下式成立
[0101]由式(8)、（14)、（12)得
[0102] <￡·;+￡·，!<?!,(0)] (1.5)
[0103] 于是有
[0104] en(t)| <ei+|eii(0) (16)
[0105] 因此，康复步行训练机器人的实际轨迹X(t)的安全运动范围为
[0106] x(t)-xd(t)| < ει+|x(0)-xd(0) (17)
[0107] y(t)-yd(t)| < ε2+|y(0)-yd(0) (18)
[0108] 0(t)-9d(t)| <e 2+|0(〇)-0d(〇) (19)
[0109] 步骤5)基于MSP430系列单片机将输出PWM信号提供给电机驱动单元，使康复步行 训练机器人的轨迹跟踪误差限制在指定范围内，以MSP430系列单片机为主控制器，主控制 器的输入接电机测速模块、输出接电机驱动模块;电机驱动电路与直流电机相连;电源系统 给各个电气设备供电。
[0110] 主控制器控制方法为读取电机编码器的反馈信号与主控制器给定的控制命令信 号Xd(t)和Αω，计算得出误差信号。根据误差信号，主控制器按照预定的控制算法计算出 电机的控制量，送给电机驱动单元，电机转动带动轮子维持自身平衡及按指定方式运动。
[0111] 本发明解决了康复步行训练机器人轨迹跟踪误差约束的安全控制问题，基于动力 学模型和非线性控制器，建立x， y轴和旋转角三个运动方向的轨迹跟踪误差状态方程。通过 Lyapunov函数构建各轨迹跟踪误差系统渐近稳定的约束条件，从而求解控制器参数矩阵， 将康复步行训练机器人暂态阶段和稳态阶段的实际运动轨迹控制在指定范围内，有效提高 了训练者的安全性。
[0112] 可以理解的是，以上关于本发明的具体描述，仅用于说明本发明而并非受限于本 发明实施例所描述的技术方案，本领域的普通技术人员应当理解，仍然可以对本发明进行 修改或等同替换，以达到相同的技术效果；只要满足使用需要，都在本发明的保护范围之 内。
【主权项】
1.康复步行训练机器人的轨迹跟踪误差约束安全控制方法，其特征在于包括以下步 骤： 步骤1)建立X，y轴和旋转角三个运动方向的轨迹跟踪误差状态方程，系统的运动学模 型如下：X(t)为康复步行训练机器人的实际行走轨迹，u(t)表示控制输入力，Μ表示康复步行训 练机器人的质量，m表示康复者的质量，1〇表示转动惯量，Μ〇，Κ(θ)，?Γ(6^),Β(θ)为系数矩阵;Θ 表示水平轴和机器人中心与第一个轮子中心连线间的夹角，9 = 0:，根据康复步行机器人结 构，夂^ + f 03 = 0+3?，Α h表示系统重心到每个轮子中心的距离，『〇表示中心 到重心的距离，Φ i表示X'轴和每个轮子对应的li之间的夹角，i = l，2,3,4;fi，f2，f3，f4分别 表不四个驱动轮的控制输入力，λ1，λ2，λ3，λ4分别表不重心到各轮子的距离，Φ?, Φ2, Φ3, Φ4 分别表示水平坐标轴和重心到各轮子中心连线的夹角； 步骤2)康复步行训练机器人实际行走轨迹X(t)，医生指定训练轨迹Xd(t)，运动轨迹和 运动速度跟踪误差ei(tWPe2(t)分别为 ei(t) =X(t)-Xd(t) (2) e.(i) = ^(/)-^(/) (3) 其中X(t) = [x(t) y(t) 0(t)]T表示x，y轴和旋转角方向的实际运动轨迹，Xd(t) = [xd (t) yd(t) 0d⑴]T表示x，y轴和旋转角方向的期望跟踪运动轨迹;ei(t) = [en(t) ei2(t) ei3(t)]T表示x，y轴和旋转角方向的轨迹跟踪误差，e2(t) = [e2i(t) e22(t) e23(t)]T表示x, y轴方向速度和旋转角速度的跟踪误差； 步骤3)设詈非线件棹制器：⑷ 其中Kd = diag{Kdi，Kd2，Kd3}，Kp = diag{Kpi，KP2，Kp3}，月(句= 表示Β(θ)的伪 逆矩阵，Kd，心表示控制器参数矩阵； 将控制器公式(4)代入系统模型公式(1 )，得 efi) =Kde2(i)+Kpel(jt) ( 5 ) 由式(2)、（3)、（5)可得x，y轴和旋转角方向轨迹跟踪误差系统状态方程为(6) 步骤4)设置Lyapunov函数：(7) 其中 - - (8)沿轨迹跟踪误差系统(6)对式(8)求导，在如下约束条件成立时，(9) 有 - (1:0) 成立，即x，y轴和旋转角三个运动方向的轨迹跟踪误差系统渐近稳定，其中￡1表示任意 指定的小正数； 步骤5)对式(10)两端从0到t积分，得 ......(11) 进一步整理得 Vi(t)-Vi(0)<-ei(|eii(t)|-|eii(0)|) (12) 当如下条件成立时 L」 （13) 有下式成立(14) 由式(8)、（14)、（12)得 (15) 于是有en(t) I < ei+|eii(0) (16) 康复步行训练机器人的实际轨迹X(t)的安全运动范围为 x(t)-xd(t)| < ει+Ix(0)-xd(0) (17) y(t)-yd(t)| < e2+|y(0)-yd(0) (18) 9(t)-0a(t)I < ￡2+I0(〇)-0d(〇) (19)〇2. 根据权利要求1所述康复步行训练机器人的轨迹跟踪误差约束安全控制方法，其特 征在于基于MSP430系列单片机将输出PWM信号提供给电机驱动单元，使康复步行训练机器 人的轨迹跟踪误差限制在指定范围内；以MSP430系列单片机为主控制器，主控制器的输入 接电机测速模块、输出接电机驱动模块;电机驱动电路与直流电机相连;电源系统给各个电 气设备供电。3. 根据权利要求2所述康复步行训练机器人的轨迹跟踪误差约束安全控制方法，其特 征在于所述主控制器控制方法为:读取电机编码器的反馈信号与主控制器给定的控制命令 信号Xd(t)和心W，计算得出误差信号;根据误差信号，主控制器按照预定的控制算法计算 出电机的控制量，送给电机驱动单元，电机转动带动轮子维持自身平衡及按指定方式运动。
【文档编号】G05B13/04GK105867130SQ201610239765
【公开日】2016年8月17日
【申请日】2016年4月15日
【发明人】孙平, 孙桐, 李树江, 杨德国, 郑青矾, 曾宏翔
【申请人】沈阳工业大学