康复步行训练机器人的人机互作用力辨识及控制方法与流程

本发明属于轮式康复机器人的控制领域，尤其涉及一种康复步行训练机器人的人机互作用力辨识及控制方法。

背景技术：

随着全球进入老龄化，高龄人口逐年增加，由于老年人腿部肌肉力量的减弱，导致步行功能逐渐下降，如果不及时加强老年人步行训练，会导致步行功能的丧失，从而无法实现自立生活。因此，发展康复步行训练机器人，使其精确跟踪医生指定的训练轨迹，帮助老年人安全地步行锻炼具有重要意义。

近年来，关于康复步行训练机器人轨迹跟踪控制方法已有许多研究成果，然而这些成果都没有考虑人机之间的互作用力。康复步行训练机器人直接与康复者接触，康复者对支撑身体重量机构的压力、腿部的主动步行力，这些人机之间的互作用力会导致机器人严重偏离医生指定的训练轨迹，不仅使机器人可能碰撞周围物体，而且会使机器人与康复者的运动不协调，从而威胁康复者的安全。因此，不考虑人机互作用力的跟踪控制方法在实际应用中均具有一定的局限性。由于人机互作用力是时变量，在实际应用中很难直接获得，这样给康复步行训练机器人跟踪控制器的设计带来了难度。本发明研究人机互作用力的观测方法和补偿人机互作用力的控制方法，对提高康复步行训练机器人的跟踪精度和安全性具有重要意义。

技术实现要素：

为了解决上述问题，本发明提供了一种具有定常增益和时变增益相结合的人机互作用力观测方法，以及对人机互作用力进行补偿的跟踪控制方法，从而提高康复步行训练机器人的跟踪精度和安全性。

为实现上述目的，本发明采用如下技术方案，本发明包括以下步骤：

步骤1)基于康复步行训练机器人系统动力学模型，将广义输入力分解为跟踪控制力和人机互作用力，得到具有人机互作用力的机器人系统动力学模型；系统动力学模型描述如下

其中

x(t)为康复步行训练机器人的实际行走轨迹，u(t)表示广义控制输入力，m表示康复步行训练机器人的质量，m表示康复者的质量，i0表示转动惯量，为系数矩阵；θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，即θ＝θ1，由康复步行机器人结构可知，θ3＝θ+π，li表示系统重心到每个轮子中心的距离，r0表示中心到重心的距离，φi表示x′轴和每个轮子对应的li之间的夹角，λi表示重心到每个轮子的距离，i＝1,2,3,4；符号将u(t)分解为u0(t)和u1(t)并代入模型(1)，得

其中u0(t)表示待设计的跟踪控制力，用于驱动康复步行训练机器人跟踪医生指定的训练轨迹；u1(t)表示待观测的人机互作用力；令x(t)＝x1(t)表示机器人的运动位置，表示机器人的运动速度，表示系统的扩展状态，于是得到具有人机互作用力的机器人系统动力学模型如下

其中h为有界常数，表示机器人系统扩展状态的变化量；

步骤2)基于人机互作用力的康复步行训练机器人系统动力学模型，利用机器人的实时位置输出，设计定常增益和时变增益相结合的系统观测器，估计人机互作用力；康复步行训练机器人的实时位置输出y(t)＝x(t)＝x1(t)，设表示xj(t)(j＝1,2,3)的观测值，表示观测误差，设计观测器如下：

其中λ0，λ2为待设计的观测器定常增益，λ1(t)为待设计的观测器时变增益；根据模型(3)和观测器(4)，得到观测误差系统为

步骤3)基于状态观测误差以及轨迹跟踪误差和速度跟踪误差设计lyapunov函数，使观测误差系统及跟踪误差系统实现渐近稳定；康复步行训练机器人实际行走轨迹x(t)，医生指定训练轨迹xd(t)，设轨迹跟踪误差e1(t)和速度跟踪误差e2(t)分别为

e1(t)＝x(t)-xd(t)(6)

进一步，由式(6)、(7)及模型(3)得到跟踪误差系统为

根据观测误差和跟踪误差设计设计lyapunov函数如下：

步骤4)使观测误差系统及跟踪误差系统达到渐近稳定时，获得观测器增益和人机互作用力的求解方法，并根据获得的人机互作用力，设计补偿跟踪控制器；沿观测误差系统(5)和跟踪误差系统(8)对式(9)求导，调整观测器增益为

可使观测误差系统(5)渐近稳定，其中εσ(σ＝1,2,3)表示指定的小正数，于是得进一步，由系统扩展状态x3(t)得人机互作用力为

获得人机互作用力后，设计补偿跟踪控制器u0(t)为

可使跟踪误差系统(8)渐近稳定；其中表示b(θ)的伪逆矩阵。

作为一种优选方案，本发明基于armcortex-m4的stm32f411系列单片机将输出pwm信号提供给电机驱动模块，使康复步行训练机器人补偿人机互作用力，并精确跟踪医生指定的训练轨迹；以stm32f411系列单片机为主控制器，主控制器的输入接mpu9250传感器模块、输出接电机驱动模块；电机驱动模块与直流电机相连；电源系统给各单元模块供电；主控制器控制方法为读取传感器模块的反馈信号x(t)与主控制器给定的控制命令信号xd(t)，计算得出误差信号，并利用反馈信号x(t)获得人机互作用力；根据误差信号及人机互作用力，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动模块，电机转动带动轮子维持自身平衡及按指定方式运动。

作为另一种优选方案，本发明所述单片机采用stm32f411ceu6芯片，stm32f411ceu6芯片的5脚接8mhz晶振一端，8mhz晶振另一端接stm32f411ceu6芯片的6脚，stm32f411ceu6芯片的7脚通过电容c1接地，stm32f411ceu6芯片的14脚与mpu9250传感器模块的12脚相连；

stm32f411ceu6芯片的15脚通过电阻r12分别与电阻r16一端、nmos管mos4的栅极相连，nmos管mos4的源极分别与电阻r16另一端、地线相连，nmos管mos4的漏极分别与二极管d4阳极、第四个轮子的驱动电机的电源负极相连，第四个轮子的驱动电机的电源正极分别与电池正极、二极管d4阴极相连；

stm32f411ceu6芯片的20脚通过电阻r4接地；

stm32f411ceu6芯片的21脚通过电阻r11分别与电阻r15一端、nmos管mos3的栅极相连，nmos管mos3的源极分别与电阻r15另一端、地线相连，nmos管mos3的漏极分别与二极管d3阳极、第三个轮子的驱动电机的电源负极相连，第三个轮子的驱动电机的电源正极分别与电池正极、二极管d3阴极相连；

stm32f411ceu6芯片的22脚通过电容c2接地；

stm32f411ceu6芯片的42脚通过电阻r10分别与电阻r14一端、nmos管mos2的栅极相连，nmos管mos2的源极分别与电阻r14另一端、地线相连，nmos管mos2的漏极分别与二极管d2阳极、第二个轮子的驱动电机的电源负极相连，第二个轮子的驱动电机的电源正极分别与电池正极、二极管d2阴极相连；

stm32f411ceu6芯片的43脚通过电阻r9分别与电阻r13一端、nmos管mos1的栅极相连，nmos管mos1的源极分别与电阻r13另一端、地线相连，nmos管mos1的漏极分别与二极管d1阳极、第一个轮子的驱动电机的电源负极相连，第一个轮子的驱动电机的电源正极分别与电池正极、二极管d1阴极相连；

stm32f411ceu6芯片的44脚通过r1接地；

stm32f411ceu6芯片的45脚与mpu9250传感器模块的23脚相连，stm32f411ceu6芯片的46脚与mpu9250传感器模块的24脚相连。

另外，本发明所述电源系统包括tp4059芯片、第一xc6204芯片和第二xc6204芯片，tp4059芯片的3脚分别与电池正极、电阻r8一端、电容c12一端、第二xc6204芯片的1脚相连，电阻r8另一端分别与第一xc6204芯片的1脚和3脚相连，电容c12另一端接地；第二xc6204芯片的5脚通过电感l1与mpu9250传感器模块的1脚相连；

tp4059芯片的4脚分别与电容c8一端、电阻r6一端相连，电阻r6另一端通过电阻r7分别与地线、电容c8另一端相连。

本发明有益效果。

本发明结合动力学模型，将广义控制输入力进行分解，并将人机互作用力作为系统扩展状态，建立具有人机互作用力的系统动力学方程；根据康复步行训练机器人的实时位置输出，设计定常增益和时变增益相结合的人机互作用力观测器，并利用获得的人机互作用力设计补偿跟踪控制器，从而消除人机互作用力对康复步行训练机器人跟踪精度的影响。本发明人机互作用力观测方法新颖，且控制器直接对人机互作用力进行补偿，易于实现，该控制方法能提高康复步行训练机器人的跟踪精度和安全性。

本发明解决了康复步行训练机器人的补偿人机互作用力的跟踪控制问题，通过构建lyapunov函数使观测误差系统和跟踪误差系统渐近稳定，求解观测器增益和人机互作用力。

附图说明

下面结合附图和具体实施方式对本发明做进一步说明。本发明保护范围不仅局限于以下内容的表述。

图1为本发明控制器工作框图；

图2为本发明系统坐标图；

图3为本发明stm32f411单片机最小系统图；

图4为本发明mpu9250外围电路图；

图5为本发明电机驱动模块外围电路图；

图6为本发明电源系统电路原理图。

具体实施方式

本发明是通过以下技术方案来实现的：

1)针对康复步行训练机器人动力学模型中的广义控制输入力，将其分解为跟踪控制力和人机互作用力，得到具有人机互作用力的机器人系统动力学模型；

2)把人机互作用力作为系统的扩展状态，利用机器人的实时位置输出，设计定常增益和时变增益相结合的系统观测器，估计人机互作用力；

3)基于状态观测误差以及轨迹跟踪误差和速度跟踪误差设计lyapunov函数，使观测误差系统及跟踪误差系统实现渐近稳定；同时，获得观测器增益和人机互作用力的求解方法，并根据获得的人机互作用力，设计补偿跟踪控制器，使康复步行训练机器人精确跟踪医生指定的训练轨迹。

如图所示，本发明具体包括以下步骤：

步骤1)基于康复步行训练机器人系统动力学模型，将广义输入力分解为跟踪控制力和人机互作用力，得到具有人机互作用力的机器人系统动力学模型；系统动力学模型描述如下

其中

x(t)为康复步行训练机器人的实际行走轨迹，u(t)表示广义控制输入力，m表示康复步行训练机器人的质量，m表示康复者的质量，i0表示转动惯量，为系数矩阵；θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，即θ＝θ1，由康复步行机器人结构可知，θ3＝θ+π，li表示系统重心到每个轮子中心的距离，r0表示中心到重心的距离，φi表示x′轴和每个轮子对应的li之间的夹角，λi表示重心到每个轮子的距离，i＝1,2,3,4；符号将u(t)分解为u0(t)和u1(t)并代入模型(1)，得

其中u0(t)表示待设计的跟踪控制力，用于驱动康复步行训练机器人跟踪医生指定的训练轨迹；u1(t)表示待观测的人机互作用力；令x(t)＝x1(t)表示机器人的运动位置，表示机器人的运动速度，表示系统的扩展状态，于是得到具有人机互作用力的机器人系统动力学模型如下

其中h为有界常数，表示机器人系统扩展状态的变化量；

步骤2)基于人机互作用力的康复步行训练机器人系统动力学模型，利用机器人的实时位置输出，设计定常增益和时变增益相结合的系统观测器，估计人机互作用力；康复步行训练机器人的实时位置输出y(t)＝x(t)＝x1(t)，设表示xj(t)(j＝1,2,3)的观测值，表示观测误差，设计观测器如下：

其中λ0，λ2为待设计的观测器定常增益，λ1(t)为待设计的观测器时变增益；根据模型(3)和观测器(4)，得到观测误差系统为

步骤3)基于状态观测误差以及轨迹跟踪误差和速度跟踪误差设计lyapunov函数，使观测误差系统及跟踪误差系统实现渐近稳定；康复步行训练机器人实际行走轨迹x(t)，医生指定训练轨迹xd(t)，设轨迹跟踪误差e1(t)和速度跟踪误差e2(t)分别为

e1(t)＝x(t)-xd(t)(6)

进一步，由式(6)、(7)及模型(3)得到跟踪误差系统为

根据观测误差和跟踪误差设计设计lyapunov函数如下：

步骤4)使观测误差系统及跟踪误差系统达到渐近稳定时，获得观测器增益和人机互作用力的求解方法，并根据获得的人机互作用力，设计补偿跟踪控制器；沿观测误差系统(5)和跟踪误差系统(8)对式(9)求导，调整观测器增益为

可使观测误差系统(5)渐近稳定，其中εσ(σ＝1,2,3)表示指定的小正数，于是得进一步，由系统扩展状态x3(t)得人机互作用力为

获得人机互作用力后，设计补偿跟踪控制器u0(t)为

可使跟踪误差系统(8)渐近稳定；其中表示b(θ)的伪逆矩阵。

基于armcortex-m4的stm32f411系列单片机将输出pwm信号提供给电机驱动模块，使康复步行训练机器人补偿人机互作用力，并精确跟踪医生指定的训练轨迹；以stm32f411系列单片机为主控制器，主控制器的输入接mpu9250传感器模块、输出接电机驱动模块；电机驱动模块与直流电机相连；电源系统给各单元模块供电；主控制器控制方法为读取传感器模块的反馈信号x(t)与主控制器给定的控制命令信号xd(t)，计算得出误差信号，并利用反馈信号x(t)获得人机互作用力；根据误差信号及人机互作用力，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动模块，电机转动带动轮子维持自身平衡及按指定方式运动。

所述单片机采用stm32f411ceu6芯片，stm32f411ceu6芯片的5脚接8mhz晶振一端，8mhz晶振另一端接stm32f411ceu6芯片的6脚，stm32f411ceu6芯片的7脚通过电容c1接地，stm32f411ceu6芯片的14脚与mpu9250传感器模块的12脚相连；

stm32f411ceu6芯片的15脚通过电阻r12分别与电阻r16一端、nmos管mos4的栅极相连，nmos管mos4的源极分别与电阻r16另一端、地线相连，nmos管mos4的漏极分别与二极管d4阳极、第四个轮子的驱动电机的电源负极相连，第四个轮子的驱动电机的电源正极分别与电池正极、二极管d4阴极相连；

stm32f411ceu6芯片的20脚通过电阻r4接地；

stm32f411ceu6芯片的21脚通过电阻r11分别与电阻r15一端、nmos管mos3的栅极相连，nmos管mos3的源极分别与电阻r15另一端、地线相连，nmos管mos3的漏极分别与二极管d3阳极、第三个轮子的驱动电机的电源负极相连，第三个轮子的驱动电机的电源正极分别与电池正极、二极管d3阴极相连；

stm32f411ceu6芯片的22脚通过电容c2接地；

stm32f411ceu6芯片的42脚通过电阻r10分别与电阻r14一端、nmos管mos2的栅极相连，nmos管mos2的源极分别与电阻r14另一端、地线相连，nmos管mos2的漏极分别与二极管d2阳极、第二个轮子的驱动电机的电源负极相连，第二个轮子的驱动电机的电源正极分别与电池正极、二极管d2阴极相连；

stm32f411ceu6芯片的43脚通过电阻r9分别与电阻r13一端、nmos管mos1的栅极相连，nmos管mos1的源极分别与电阻r13另一端、地线相连，nmos管mos1的漏极分别与二极管d1阳极、第一个轮子的驱动电机的电源负极相连，第一个轮子的驱动电机的电源正极分别与电池正极、二极管d1阴极相连；

stm32f411ceu6芯片的44脚通过r1接地；

stm32f411ceu6芯片的45脚与mpu9250传感器模块的23脚相连，stm32f411ceu6芯片的46脚与mpu9250传感器模块的24脚相连。

所述电源系统包括tp4059芯片、第一xc6204芯片和第二xc6204芯片，tp4059芯片的3脚分别与电池正极、电阻r8一端、电容c12一端、第二xc6204芯片的1脚相连，电阻r8另一端分别与第一xc6204芯片的1脚和3脚相连，电容c12另一端接地；第二xc6204芯片的5脚通过电感l1与mpu9250传感器模块的1脚相连；

tp4059芯片的4脚分别与电容c8一端、电阻r6一端相连，电阻r6另一端通过电阻r7分别与地线、电容c8另一端相连。

可以理解的是，以上关于本发明的具体描述，仅用于说明本发明而并非受限于本发明实施例所描述的技术方案，本领域的普通技术人员应当理解，仍然可以对本发明进行修改或等同替换，以达到相同的技术效果；只要满足使用需要，都在本发明的保护范围之内。