[go: up one dir, main page]

CN107703762B - Human-machine interaction force identification and control method for rehabilitation walking training robot - Google Patents

Human-machine interaction force identification and control method for rehabilitation walking training robot Download PDF

Info

Publication number
CN107703762B
CN107703762B CN201711121857.9A CN201711121857A CN107703762B CN 107703762 B CN107703762 B CN 107703762B CN 201711121857 A CN201711121857 A CN 201711121857A CN 107703762 B CN107703762 B CN 107703762B
Authority
CN
China
Prior art keywords
chip
pin
human
interaction force
resistor
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201711121857.9A
Other languages
Chinese (zh)
Other versions
CN107703762A (en
Inventor
孙平
张文娇
孟奇
张帅
刘佳斌
单芮
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Kellymed Co ltd
Original Assignee
Shenyang University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenyang University of Technology filed Critical Shenyang University of Technology
Priority to CN201711121857.9A priority Critical patent/CN107703762B/en
Publication of CN107703762A publication Critical patent/CN107703762A/en
Application granted granted Critical
Publication of CN107703762B publication Critical patent/CN107703762B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Rehabilitation Tools (AREA)
  • Medicines Containing Antibodies Or Antigens For Use As Internal Diagnostic Agents (AREA)

Abstract

本发明公开了一种康复步行训练机器人的补偿人机互作用力的辨识控制方法。针对康复步行训练机器人动力学模型中的广义控制输入力，将其分解为跟踪控制力和人机互作用力，得到具有人机互作用力的系统动力学模型；把人机互作用力作为系统的扩展状态，利用机器人的实时位置输出，设计定常增益和时变增益相结合的系统观测器，估计人机互作用力；基于状态观测误差以及轨迹跟踪误差和速度跟踪误差设计Lyapunov函数，使观测误差系统及跟踪误差系统实现渐近稳定。该控制方法通过设计新颖的系统扩展状态观测器，获得了人机互作用力，并利用补偿控制，消除了人机互作用力对跟踪性能的影响，提高了康复步行训练机器人的跟踪精度和系统的安全性。

Figure 201711121857

The invention discloses an identification control method for compensating human-machine interaction force of a rehabilitation walking training robot. Aiming at the generalized control input force in the dynamic model of the rehabilitation walking training robot, it is decomposed into the tracking control force and the human-machine interaction force, and the system dynamics model with the human-machine interaction force is obtained. Using the real-time position output of the robot, a system observer with a combination of constant gain and time-varying gain is designed to estimate the human-machine interaction force; based on the state observation error, trajectory tracking error and velocity tracking error, the Lyapunov function is designed to make the observation The error system and the tracking error system are asymptotically stable. The control method obtains the human-machine interaction force by designing a novel system to expand the state observer, and uses the compensation control to eliminate the influence of the human-machine interaction force on the tracking performance, and improve the tracking accuracy and system of the rehabilitation walking training robot. security.

Figure 201711121857

Description

康复步行训练机器人的人机互作用力辨识及控制方法Human-machine interaction force identification and control method for rehabilitation walking training robot

技术领域technical field

本发明属于轮式康复机器人的控制领域，尤其涉及一种康复步行训练机器人的人机互作用力辨识及控制方法。The invention belongs to the control field of wheeled rehabilitation robots, in particular to a human-machine interaction force identification and control method of a rehabilitation walking training robot.

背景技术Background technique

随着全球进入老龄化，高龄人口逐年增加，由于老年人腿部肌肉力量的减弱，导致步行功能逐渐下降，如果不及时加强老年人步行训练，会导致步行功能的丧失，从而无法实现自立生活。因此，发展康复步行训练机器人，使其精确跟踪医生指定的训练轨迹，帮助老年人安全地步行锻炼具有重要意义。With the aging of the world, the elderly population is increasing year by year. Due to the weakening of the leg muscle strength of the elderly, the walking function gradually declines. If the elderly walking training is not strengthened in time, it will lead to the loss of walking function, so that self-reliance cannot be achieved. Therefore, it is of great significance to develop a rehabilitation walking training robot so that it can accurately track the training trajectory specified by the doctor and help the elderly to walk and exercise safely.

近年来，关于康复步行训练机器人轨迹跟踪控制方法已有许多研究成果，然而这些成果都没有考虑人机之间的互作用力。康复步行训练机器人直接与康复者接触，康复者对支撑身体重量机构的压力、腿部的主动步行力，这些人机之间的互作用力会导致机器人严重偏离医生指定的训练轨迹，不仅使机器人可能碰撞周围物体，而且会使机器人与康复者的运动不协调，从而威胁康复者的安全。因此，不考虑人机互作用力的跟踪控制方法在实际应用中均具有一定的局限性。由于人机互作用力是时变量，在实际应用中很难直接获得，这样给康复步行训练机器人跟踪控制器的设计带来了难度。本发明研究人机互作用力的观测方法和补偿人机互作用力的控制方法，对提高康复步行训练机器人的跟踪精度和安全性具有重要意义。In recent years, there have been many research results on trajectory tracking control methods for rehabilitation walking training robots, but these results do not consider the interaction between human and machine. The rehabilitation walking training robot is in direct contact with the rehabilitated person. The pressure of the rehabilitated person on the body supporting the body and the active walking force of the legs will cause the robot to seriously deviate from the training trajectory specified by the doctor. It may collide with surrounding objects, and it will make the movement of the robot and the recovered person uncoordinated, thus threatening the safety of the recovered person. Therefore, the tracking control methods that do not consider the human-machine interaction force have certain limitations in practical applications. Since the human-computer interaction force is time-varying, it is difficult to obtain directly in practical applications, which brings difficulties to the design of the tracking controller of the rehabilitation walking training robot. The invention studies the observation method of the human-machine interaction force and the control method for compensating the human-machine interaction force, which has important significance for improving the tracking accuracy and safety of the rehabilitation walking training robot.

发明内容SUMMARY OF THE INVENTION

为了解决上述问题，本发明提供了一种具有定常增益和时变增益相结合的人机互作用力观测方法，以及对人机互作用力进行补偿的跟踪控制方法，从而提高康复步行训练机器人的跟踪精度和安全性。In order to solve the above problems, the present invention provides a human-machine interaction force observation method with a combination of constant gain and time-varying gain, and a tracking control method for compensating the human-machine interaction force, thereby improving the performance of the rehabilitation walking training robot. Tracking accuracy and safety.

为实现上述目的，本发明采用如下技术方案，本发明包括以下步骤：To achieve the above object, the present invention adopts the following technical solutions, and the present invention comprises the following steps:

步骤1)基于康复步行训练机器人系统动力学模型，将广义输入力分解为跟踪控制力和人机互作用力，得到具有人机互作用力的机器人系统动力学模型；系统动力学模型描述如下Step 1) Based on the system dynamics model of the rehabilitation walking training robot, the generalized input force is decomposed into the tracking control force and the human-machine interaction force, and the robot system dynamics model with the human-machine interaction force is obtained; the system dynamics model is described as follows

Figure BDA0001467526340000011
Figure BDA0001467526340000011

其中in

Figure BDA0001467526340000012
Figure BDA0001467526340000012

Figure BDA0001467526340000013
Figure BDA0001467526340000013

X(t)为康复步行训练机器人的实际行走轨迹，u(t)表示广义控制输入力，M表示康复步行训练机器人的质量，m表示康复者的质量，I0表示转动惯量，

Figure BDA0001467526340000014
为系数矩阵；θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，即θ＝θ1，由康复步行机器人结构可知，
Figure BDA0001467526340000021
θ3＝θ+π，
Figure BDA0001467526340000022
li表示系统重心到每个轮子中心的距离，r0表示中心到重心的距离，φi表示x′轴和每个轮子对应的li之间的夹角，λi表示重心到每个轮子的距离，i＝1,2,3,4；符号
Figure BDA0001467526340000023
将u(t)分解为u0(t)和u1(t)并代入模型(1)，得X(t) is the actual walking trajectory of the rehabilitation walking training robot, u(t) represents the generalized control input force, M represents the mass of the rehabilitation walking training robot, m represents the quality of the rehabilitation person, I 0 represents the moment of inertia,
Figure BDA0001467526340000014
is the coefficient matrix; θ represents the angle between the horizontal axis and the line connecting the center of the robot and the center of the first wheel, that is, θ=θ 1 . It can be known from the structure of the rehabilitation walking robot,
Figure BDA0001467526340000021
θ 3 =θ+π,
Figure BDA0001467526340000022
l i represents the distance from the center of gravity of the system to the center of each wheel, r 0 represents the distance from the center to the center of gravity, φ i represents the angle between the x′ axis and the corresponding li of each wheel, λ i represents the center of gravity to each wheel distance, i=1,2,3,4; symbol
Figure BDA0001467526340000023
Decomposing u(t) into u 0 (t) and u 1 (t) and substituting them into model (1), we get

Figure BDA0001467526340000024
Figure BDA0001467526340000024

其中u0(t)表示待设计的跟踪控制力，用于驱动康复步行训练机器人跟踪医生指定的训练轨迹；u1(t)表示待观测的人机互作用力；令X(t)＝x1(t)表示机器人的运动位置，

Figure BDA0001467526340000025
表示机器人的运动速度，
Figure BDA0001467526340000026
表示系统的扩展状态，于是得到具有人机互作用力的机器人系统动力学模型如下where u 0 (t) represents the tracking control force to be designed, which is used to drive the rehabilitation walking training robot to track the training trajectory specified by the doctor; u 1 (t) represents the human-machine interaction force to be observed; let X(t)=x 1 (t) represents the motion position of the robot,
Figure BDA0001467526340000025
represents the movement speed of the robot,
Figure BDA0001467526340000026
Represents the extended state of the system, so the dynamic model of the robot system with human-robot interaction force is obtained as follows

Figure BDA0001467526340000027
Figure BDA0001467526340000027

其中h为有界常数，表示机器人系统扩展状态的变化量；Among them, h is a bounded constant, which represents the change of the expansion state of the robot system;

步骤2)基于人机互作用力的康复步行训练机器人系统动力学模型，利用机器人的实时位置输出，设计定常增益和时变增益相结合的系统观测器，估计人机互作用力；康复步行训练机器人的实时位置输出y(t)＝X(t)＝x1(t)，设

Figure BDA0001467526340000028
表示xj(t)(j＝1,2,3)的观测值，
Figure BDA0001467526340000029
表示观测误差，设计观测器如下：Step 2) The system dynamics model of the rehabilitation walking training robot based on the human-computer interaction force, using the real-time position output of the robot to design a system observer combining constant gain and time-varying gain to estimate the human-computer interaction force; rehabilitation walking training The real-time position output of the robot y(t)=X(t)=x 1 (t), set
Figure BDA0001467526340000028
represents the observed value of x j (t)(j=1,2,3),
Figure BDA0001467526340000029
represents the observation error, and the observer is designed as follows:

Figure BDA00014675263400000210
Figure BDA00014675263400000210

其中λ0，λ2为待设计的观测器定常增益，λ1(t)为待设计的观测器时变增益；根据模型(3)和观测器(4)，得到观测误差系统为where λ 0 , λ 2 are the constant gains of the observer to be designed, and λ 1 (t) is the time-varying gain of the observer to be designed; according to the model (3) and the observer (4), the observation error system is obtained as

Figure BDA00014675263400000211
Figure BDA00014675263400000211

步骤3)基于状态观测误差以及轨迹跟踪误差和速度跟踪误差设计Lyapunov函数，使观测误差系统及跟踪误差系统实现渐近稳定；康复步行训练机器人实际行走轨迹X(t)，医生指定训练轨迹Xd(t)，设轨迹跟踪误差e1(t)和速度跟踪误差e2(t)分别为Step 3) Design the Lyapunov function based on the state observation error, trajectory tracking error and velocity tracking error, so that the observation error system and the tracking error system are asymptotically stable; the actual walking trajectory X(t) of the rehabilitation walking training robot, and the training trajectory X d specified by the doctor (t), let the trajectory tracking error e 1 (t) and velocity tracking error e 2 (t) be respectively

e1(t)＝X(t)-Xd(t) (6)e 1 (t)=X(t)-X d (t) (6)

Figure BDA0001467526340000031
Figure BDA0001467526340000031

进一步，由式(6)、(7)及模型(3)得到跟踪误差系统为Further, the tracking error system obtained from equations (6), (7) and model (3) is

Figure BDA0001467526340000032
Figure BDA0001467526340000032

根据观测误差和跟踪误差设计设计Lyapunov函数如下：The Lyapunov function is designed according to the observation error and tracking error as follows:

Figure BDA0001467526340000033
Figure BDA0001467526340000033

步骤4)使观测误差系统及跟踪误差系统达到渐近稳定时，获得观测器增益和人机互作用力的求解方法，并根据获得的人机互作用力，设计补偿跟踪控制器；沿观测误差系统(5)和跟踪误差系统(8)对式(9)求导，调整观测器增益为Step 4) When the observation error system and the tracking error system are asymptotically stable, obtain the solution method of the observer gain and the human-machine interaction force, and design a compensation tracking controller according to the obtained human-machine interaction force; along the observation error The system (5) and the tracking error system (8) are derived from equation (9), and the observer gain is adjusted as

Figure BDA0001467526340000034
Figure BDA0001467526340000034

Figure BDA0001467526340000035
Figure BDA0001467526340000035

Figure BDA0001467526340000036
Figure BDA0001467526340000036

可使观测误差系统(5)渐近稳定，其中εσ(σ＝1,2,3)表示指定的小正数，于是得

Figure BDA0001467526340000037
进一步，由系统扩展状态x3(t)得人机互作用力为The observation error system (5) can be made asymptotically stable, where ε σ (σ=1, 2, 3) represents a specified small positive number, so we get
Figure BDA0001467526340000037
Further, the human-machine interaction force obtained from the system expansion state x 3 (t) is

Figure BDA0001467526340000038
Figure BDA0001467526340000038

获得人机互作用力后，设计补偿跟踪控制器u0(t)为After obtaining the human-machine interaction force, the design compensation tracking controller u 0 (t) is

Figure BDA0001467526340000039
Figure BDA0001467526340000039

可使跟踪误差系统(8)渐近稳定；其中

Figure BDA00014675263400000310
表示B(θ)的伪逆矩阵。The tracking error system (8) can be made asymptotically stable; wherein
Figure BDA00014675263400000310
Represents the pseudo-inverse of B(θ).

作为一种优选方案，本发明基于ARM Cortex-M4的STM32F411系列单片机将输出PWM信号提供给电机驱动模块，使康复步行训练机器人补偿人机互作用力，并精确跟踪医生指定的训练轨迹；以STM32F411系列单片机为主控制器，主控制器的输入接MPU9250传感器模块、输出接电机驱动模块；电机驱动模块与直流电机相连；电源系统给各单元模块供电；主控制器控制方法为读取传感器模块的反馈信号X(t)与主控制器给定的控制命令信号Xd(t)，计算得出误差信号，并利用反馈信号X(t)获得人机互作用力；根据误差信号及人机互作用力，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动模块，电机转动带动轮子维持自身平衡及按指定方式运动。As a preferred solution, the STM32F411 series single-chip microcomputer based on ARM Cortex-M4 of the present invention provides the output PWM signal to the motor drive module, so that the rehabilitation walking training robot can compensate the human-machine interaction force and accurately track the training trajectory specified by the doctor; the STM32F411 The series of single-chip microcomputer is the main controller, the input of the main controller is connected to the MPU9250 sensor module, and the output is connected to the motor drive module; the motor drive module is connected to the DC motor; the power supply system supplies power to each unit module; the main controller control method is to read the sensor module. The feedback signal X(t) and the control command signal X d (t) given by the main controller can calculate the error signal, and use the feedback signal X(t) to obtain the man-machine interaction force; according to the error signal and the man-machine interaction force The main controller calculates the control amount of the motor according to the predetermined control algorithm and sends it to the motor drive module, and the motor rotates to drive the wheel to maintain its own balance and move in a specified way.

作为另一种优选方案，本发明所述单片机采用STM32F411CEU6芯片，STM32F411CEU6芯片的5脚接8MHz晶振一端，8MHz晶振另一端接STM32F411CEU6芯片的6脚，STM32F411CEU6芯片的7脚通过电容C1接地，STM32F411CEU6芯片的14脚与MPU9250传感器模块的12脚相连；As another preferred solution, the single-chip microcomputer of the present invention adopts the STM32F411CEU6 chip, the 5th pin of the STM32F411CEU6 chip is connected to one end of the 8MHz crystal oscillator, the other end of the 8MHz crystal oscillator is connected to the 6th pin of the STM32F411CEU6 chip, the 7th pin of the STM32F411CEU6 chip is grounded through the capacitor C1, and the STM32F411CEU6 chip is grounded. Pin 14 is connected to pin 12 of the MPU9250 sensor module;

STM32F411CEU6芯片的15脚通过电阻R12分别与电阻R16一端、NMOS管MOS4的栅极相连，NMOS管MOS4的源极分别与电阻R16另一端、地线相连，NMOS管MOS4的漏极分别与二极管D4阳极、第四个轮子的驱动电机的电源负极相连，第四个轮子的驱动电机的电源正极分别与电池正极、二极管D4阴极相连；Pin 15 of the STM32F411CEU6 chip is connected to one end of the resistor R16 and the gate of the NMOS transistor MOS4 through the resistor R12 respectively. The source of the NMOS transistor MOS4 is connected to the other end of the resistor R16 and the ground, respectively. , The negative pole of the power supply of the driving motor of the fourth wheel is connected, and the positive pole of the power supply of the driving motor of the fourth wheel is connected to the positive pole of the battery and the cathode of the diode D4 respectively;

STM32F411CEU6芯片的20脚通过电阻R4接地；Pin 20 of the STM32F411CEU6 chip is grounded through resistor R4;

STM32F411CEU6芯片的21脚通过电阻R11分别与电阻R15一端、NMOS管MOS3的栅极相连，NMOS管MOS3的源极分别与电阻R15另一端、地线相连，NMOS管MOS3的漏极分别与二极管D3阳极、第三个轮子的驱动电机的电源负极相连，第三个轮子的驱动电机的电源正极分别与电池正极、二极管D3阴极相连；Pin 21 of the STM32F411CEU6 chip is connected to one end of the resistor R15 and the gate of the NMOS transistor MOS3 through the resistor R11, respectively. The source of the NMOS transistor MOS3 is connected to the other end of the resistor R15 and the ground, respectively. . The negative pole of the power supply of the driving motor of the third wheel is connected to the negative pole of the power supply of the driving motor of the third wheel, and the positive pole of the power supply of the driving motor of the third wheel is respectively connected to the positive pole of the battery and the cathode of the diode D3;

STM32F411CEU6芯片的22脚通过电容C2接地；Pin 22 of the STM32F411CEU6 chip is grounded through capacitor C2;

STM32F411CEU6芯片的42脚通过电阻R10分别与电阻R14一端、NMOS管MOS2的栅极相连，NMOS管MOS2的源极分别与电阻R14另一端、地线相连，NMOS管MOS2的漏极分别与二极管D2阳极、第二个轮子的驱动电机的电源负极相连，第二个轮子的驱动电机的电源正极分别与电池正极、二极管D2阴极相连；Pin 42 of the STM32F411CEU6 chip is connected to one end of the resistor R14 and the gate of the NMOS transistor MOS2 through the resistor R10, respectively. The source of the NMOS transistor MOS2 is connected to the other end of the resistor R14 and the ground, respectively. , The negative pole of the power supply of the driving motor of the second wheel is connected, and the positive pole of the power supply of the driving motor of the second wheel is connected to the positive pole of the battery and the cathode of the diode D2 respectively;

STM32F411CEU6芯片的43脚通过电阻R9分别与电阻R13一端、NMOS管MOS1的栅极相连，NMOS管MOS1的源极分别与电阻R13另一端、地线相连，NMOS管MOS1的漏极分别与二极管D1阳极、第一个轮子的驱动电机的电源负极相连，第一个轮子的驱动电机的电源正极分别与电池正极、二极管D1阴极相连；Pin 43 of the STM32F411CEU6 chip is connected to one end of the resistor R13 and the gate of the NMOS transistor MOS1 through the resistor R9, respectively. The source of the NMOS transistor MOS1 is connected to the other end of the resistor R13 and the ground, respectively. , The negative pole of the power supply of the driving motor of the first wheel is connected, and the positive pole of the power supply of the driving motor of the first wheel is connected to the positive pole of the battery and the cathode of the diode D1 respectively;

STM32F411CEU6芯片的44脚通过R1接地；Pin 44 of the STM32F411CEU6 chip is grounded through R1;

STM32F411CEU6芯片的45脚与MPU9250传感器模块的23脚相连，STM32F411CEU6芯片的46脚与MPU9250传感器模块的24脚相连。Pin 45 of the STM32F411CEU6 chip is connected to pin 23 of the MPU9250 sensor module, and pin 46 of the STM32F411CEU6 chip is connected to pin 24 of the MPU9250 sensor module.

另外，本发明所述电源系统包括TP4059芯片、第一XC6204芯片和第二XC6204芯片，TP4059芯片的3脚分别与电池正极、电阻R8一端、电容C12一端、第二XC6204芯片的1脚相连，电阻R8另一端分别与第一XC6204芯片的1脚和3脚相连，电容C12另一端接地；第二XC6204芯片的5脚通过电感L1与MPU9250传感器模块的1脚相连；In addition, the power supply system of the present invention includes a TP4059 chip, a first XC6204 chip and a second XC6204 chip. The 3 pins of the TP4059 chip are respectively connected to the positive electrode of the battery, one end of the resistor R8, one end of the capacitor C12, and one pin of the second XC6204 chip. The other end of R8 is connected to pins 1 and 3 of the first XC6204 chip respectively, and the other end of the capacitor C12 is grounded; the 5th pin of the second XC6204 chip is connected to the 1st pin of the MPU9250 sensor module through the inductor L1;

TP4059芯片的4脚分别与电容C8一端、电阻R6一端相连，电阻R6另一端通过电阻R7分别与地线、电容C8另一端相连。The 4 pins of the TP4059 chip are respectively connected to one end of the capacitor C8 and one end of the resistor R6, and the other end of the resistor R6 is respectively connected to the ground wire and the other end of the capacitor C8 through the resistor R7.

本发明有益效果。The present invention has beneficial effects.

本发明结合动力学模型，将广义控制输入力进行分解，并将人机互作用力作为系统扩展状态，建立具有人机互作用力的系统动力学方程；根据康复步行训练机器人的实时位置输出，设计定常增益和时变增益相结合的人机互作用力观测器，并利用获得的人机互作用力设计补偿跟踪控制器，从而消除人机互作用力对康复步行训练机器人跟踪精度的影响。本发明人机互作用力观测方法新颖，且控制器直接对人机互作用力进行补偿，易于实现，该控制方法能提高康复步行训练机器人的跟踪精度和安全性。The invention combines the dynamics model, decomposes the generalized control input force, and takes the human-machine interaction force as the system expansion state to establish a system dynamics equation with the human-machine interaction force; according to the real-time position output of the rehabilitation walking training robot, A human-machine interaction force observer with a combination of constant gain and time-varying gain is designed, and a compensation tracking controller is designed by using the obtained human-machine interaction force, so as to eliminate the influence of the human-machine interaction force on the tracking accuracy of the rehabilitation walking training robot. The human-machine interaction force observation method of the invention is novel, and the controller directly compensates the human-machine interaction force, which is easy to implement, and the control method can improve the tracking accuracy and safety of the rehabilitation walking training robot.

本发明解决了康复步行训练机器人的补偿人机互作用力的跟踪控制问题，通过构建Lyapunov函数使观测误差系统和跟踪误差系统渐近稳定，求解观测器增益和人机互作用力。The invention solves the tracking control problem of compensating the human-machine interaction force of the rehabilitation walking training robot, makes the observation error system and the tracking error system asymptotically stable by constructing the Lyapunov function, and solves the observer gain and the human-machine interaction force.

附图说明Description of drawings

下面结合附图和具体实施方式对本发明做进一步说明。本发明保护范围不仅局限于以下内容的表述。The present invention will be further described below with reference to the accompanying drawings and specific embodiments. The protection scope of the present invention is not limited to the following descriptions.

图1为本发明控制器工作框图；Fig. 1 is the working block diagram of the controller of the present invention;

图2为本发明系统坐标图；Fig. 2 is the system coordinate diagram of the present invention;

图3为本发明STM32F411单片机最小系统图；Fig. 3 is the minimum system diagram of the STM32F411 single-chip microcomputer of the present invention;

图4为本发明MPU9250外围电路图；Fig. 4 is the peripheral circuit diagram of MPU9250 of the present invention;

图5为本发明电机驱动模块外围电路图；Fig. 5 is the peripheral circuit diagram of the motor drive module of the present invention;

图6为本发明电源系统电路原理图。FIG. 6 is a schematic diagram of the power supply system circuit of the present invention.

具体实施方式Detailed ways

本发明是通过以下技术方案来实现的：The present invention is achieved through the following technical solutions:

1)针对康复步行训练机器人动力学模型中的广义控制输入力，将其分解为跟踪控制力和人机互作用力，得到具有人机互作用力的机器人系统动力学模型；1) Aiming at the generalized control input force in the dynamic model of the rehabilitation walking training robot, it is decomposed into the tracking control force and the human-machine interaction force, and the robot system dynamics model with the human-machine interaction force is obtained;

2)把人机互作用力作为系统的扩展状态，利用机器人的实时位置输出，设计定常增益和时变增益相结合的系统观测器，估计人机互作用力；2) Taking the human-machine interaction force as the extended state of the system, and using the real-time position output of the robot, a system observer combining constant gain and time-varying gain is designed to estimate the human-machine interaction force;

3)基于状态观测误差以及轨迹跟踪误差和速度跟踪误差设计Lyapunov函数，使观测误差系统及跟踪误差系统实现渐近稳定；同时，获得观测器增益和人机互作用力的求解方法，并根据获得的人机互作用力，设计补偿跟踪控制器，使康复步行训练机器人精确跟踪医生指定的训练轨迹。3) The Lyapunov function is designed based on the state observation error, trajectory tracking error and velocity tracking error, so that the observation error system and the tracking error system are asymptotically stable; The human-computer interaction force is designed to compensate for the tracking controller, so that the rehabilitation walking training robot can accurately track the training trajectory specified by the doctor.

如图所示，本发明具体包括以下步骤：As shown in the figure, the present invention specifically comprises the following steps:

步骤1)基于康复步行训练机器人系统动力学模型，将广义输入力分解为跟踪控制力和人机互作用力，得到具有人机互作用力的机器人系统动力学模型；系统动力学模型描述如下Step 1) Based on the system dynamics model of the rehabilitation walking training robot, the generalized input force is decomposed into the tracking control force and the human-machine interaction force, and the robot system dynamics model with the human-machine interaction force is obtained; the system dynamics model is described as follows

Figure BDA0001467526340000051
Figure BDA0001467526340000051

其中in

Figure BDA0001467526340000052
Figure BDA0001467526340000052

Figure BDA0001467526340000061
Figure BDA0001467526340000061

X(t)为康复步行训练机器人的实际行走轨迹，u(t)表示广义控制输入力，M表示康复步行训练机器人的质量，m表示康复者的质量，I0表示转动惯量，

Figure BDA0001467526340000062
为系数矩阵；θ表示水平轴和机器人中心与第一个轮子中心连线间的夹角，即θ＝θ1，由康复步行机器人结构可知，
Figure BDA0001467526340000063
θ3＝θ+π，
Figure BDA0001467526340000064
li表示系统重心到每个轮子中心的距离，r0表示中心到重心的距离，φi表示x′轴和每个轮子对应的li之间的夹角，λi表示重心到每个轮子的距离，i＝1,2,3,4；符号
Figure BDA0001467526340000065
将u(t)分解为u0(t)和u1(t)并代入模型(1)，得X(t) is the actual walking trajectory of the rehabilitation walking training robot, u(t) represents the generalized control input force, M represents the mass of the rehabilitation walking training robot, m represents the quality of the rehabilitation person, I 0 represents the moment of inertia,
Figure BDA0001467526340000062
is the coefficient matrix; θ represents the angle between the horizontal axis and the line connecting the center of the robot and the center of the first wheel, that is, θ=θ 1 . It can be known from the structure of the rehabilitation walking robot,
Figure BDA0001467526340000063
θ 3 =θ+π,
Figure BDA0001467526340000064
l i represents the distance from the center of gravity of the system to the center of each wheel, r 0 represents the distance from the center to the center of gravity, φ i represents the angle between the x′ axis and the corresponding li of each wheel, λ i represents the center of gravity to each wheel distance, i=1,2,3,4; symbol
Figure BDA0001467526340000065
Decomposing u(t) into u 0 (t) and u 1 (t) and substituting them into model (1), we get

Figure BDA0001467526340000066
Figure BDA0001467526340000066

其中u0(t)表示待设计的跟踪控制力，用于驱动康复步行训练机器人跟踪医生指定的训练轨迹；u1(t)表示待观测的人机互作用力；令X(t)＝x1(t)表示机器人的运动位置，

Figure BDA0001467526340000067
表示机器人的运动速度，
Figure BDA0001467526340000068
表示系统的扩展状态，于是得到具有人机互作用力的机器人系统动力学模型如下where u 0 (t) represents the tracking control force to be designed, which is used to drive the rehabilitation walking training robot to track the training trajectory specified by the doctor; u 1 (t) represents the human-machine interaction force to be observed; let X(t)=x 1 (t) represents the motion position of the robot,
Figure BDA0001467526340000067
represents the movement speed of the robot,
Figure BDA0001467526340000068
Represents the extended state of the system, so the dynamic model of the robot system with human-robot interaction force is obtained as follows

Figure BDA0001467526340000069
Figure BDA0001467526340000069

其中h为有界常数，表示机器人系统扩展状态的变化量；Among them, h is a bounded constant, which represents the change of the expansion state of the robot system;

步骤2)基于人机互作用力的康复步行训练机器人系统动力学模型，利用机器人的实时位置输出，设计定常增益和时变增益相结合的系统观测器，估计人机互作用力；康复步行训练机器人的实时位置输出y(t)＝X(t)＝x1(t)，设

Figure BDA00014675263400000610
表示xj(t)(j＝1,2,3)的观测值，
Figure BDA00014675263400000611
表示观测误差，设计观测器如下：Step 2) The system dynamics model of the rehabilitation walking training robot based on the human-computer interaction force, using the real-time position output of the robot to design a system observer combining constant gain and time-varying gain to estimate the human-computer interaction force; rehabilitation walking training The real-time position output of the robot y(t)=X(t)=x 1 (t), set
Figure BDA00014675263400000610
represents the observed value of x j (t)(j=1,2,3),
Figure BDA00014675263400000611
represents the observation error, and the observer is designed as follows:

Figure BDA00014675263400000612
Figure BDA00014675263400000612

其中λ0，λ2为待设计的观测器定常增益，λ1(t)为待设计的观测器时变增益；根据模型(3)和观测器(4)，得到观测误差系统为where λ 0 , λ 2 are the constant gains of the observer to be designed, and λ 1 (t) is the time-varying gain of the observer to be designed; according to the model (3) and the observer (4), the observation error system is obtained as

Figure BDA0001467526340000071
Figure BDA0001467526340000071

步骤3)基于状态观测误差以及轨迹跟踪误差和速度跟踪误差设计Lyapunov函数，使观测误差系统及跟踪误差系统实现渐近稳定；康复步行训练机器人实际行走轨迹X(t)，医生指定训练轨迹Xd(t)，设轨迹跟踪误差e1(t)和速度跟踪误差e2(t)分别为Step 3) Design the Lyapunov function based on the state observation error, trajectory tracking error and velocity tracking error, so that the observation error system and the tracking error system are asymptotically stable; the actual walking trajectory X(t) of the rehabilitation walking training robot, and the training trajectory X d specified by the doctor (t), let the trajectory tracking error e 1 (t) and velocity tracking error e 2 (t) be respectively

e1(t)＝X(t)-Xd(t) (6)e 1 (t)=X(t)-X d (t) (6)

Figure BDA0001467526340000072
Figure BDA0001467526340000072

进一步，由式(6)、(7)及模型(3)得到跟踪误差系统为Further, the tracking error system obtained from equations (6), (7) and model (3) is

Figure BDA0001467526340000073
Figure BDA0001467526340000073

根据观测误差和跟踪误差设计设计Lyapunov函数如下：The Lyapunov function is designed according to the observation error and tracking error as follows:

Figure BDA0001467526340000074
Figure BDA0001467526340000074

步骤4)使观测误差系统及跟踪误差系统达到渐近稳定时，获得观测器增益和人机互作用力的求解方法，并根据获得的人机互作用力，设计补偿跟踪控制器；沿观测误差系统(5)和跟踪误差系统(8)对式(9)求导，调整观测器增益为Step 4) When the observation error system and the tracking error system are asymptotically stable, obtain the solution method of the observer gain and the human-machine interaction force, and design a compensation tracking controller according to the obtained human-machine interaction force; along the observation error The system (5) and the tracking error system (8) are derived from equation (9), and the observer gain is adjusted as

Figure BDA0001467526340000075
Figure BDA0001467526340000075

Figure BDA0001467526340000076
Figure BDA0001467526340000076

Figure BDA0001467526340000077
Figure BDA0001467526340000077

可使观测误差系统(5)渐近稳定，其中εσ(σ＝1,2,3)表示指定的小正数，于是得

Figure BDA0001467526340000078
进一步，由系统扩展状态x3(t)得人机互作用力为The observation error system (5) can be made asymptotically stable, where ε σ (σ=1, 2, 3) represents a specified small positive number, so we get
Figure BDA0001467526340000078
Further, the human-machine interaction force obtained from the system expansion state x 3 (t) is

Figure BDA0001467526340000079
Figure BDA0001467526340000079

获得人机互作用力后，设计补偿跟踪控制器u0(t)为After obtaining the human-machine interaction force, the design compensation tracking controller u 0 (t) is

Figure BDA00014675263400000710
Figure BDA00014675263400000710

可使跟踪误差系统(8)渐近稳定；其中

Figure BDA00014675263400000711
表示B(θ)的伪逆矩阵。The tracking error system (8) can be made asymptotically stable; wherein
Figure BDA00014675263400000711
Represents the pseudo-inverse of B(θ).

基于ARM Cortex-M4的STM32F411系列单片机将输出PWM信号提供给电机驱动模块，使康复步行训练机器人补偿人机互作用力，并精确跟踪医生指定的训练轨迹；以STM32F411系列单片机为主控制器，主控制器的输入接MPU9250传感器模块、输出接电机驱动模块；电机驱动模块与直流电机相连；电源系统给各单元模块供电；主控制器控制方法为读取传感器模块的反馈信号X(t)与主控制器给定的控制命令信号Xd(t)，计算得出误差信号，并利用反馈信号X(t)获得人机互作用力；根据误差信号及人机互作用力，主控制器按照预定的控制算法计算出电机的控制量，送给电机驱动模块，电机转动带动轮子维持自身平衡及按指定方式运动。The STM32F411 series MCU based on ARM Cortex-M4 provides the output PWM signal to the motor drive module, so that the rehabilitation walking training robot can compensate the human-machine interaction force and accurately track the training trajectory specified by the doctor; the STM32F411 series MCU is used as the main controller and the main controller. The input of the controller is connected to the MPU9250 sensor module, and the output is connected to the motor drive module; the motor drive module is connected to the DC motor; the power supply system supplies power to each unit module; the control method of the main controller is to read the feedback signal X(t) of the sensor module and the main controller. The controller gives the control command signal X d (t), calculates the error signal, and uses the feedback signal X (t) to obtain the man-machine interaction force; according to the error signal and the man-machine interaction force, the main controller according to the predetermined The control algorithm calculates the control amount of the motor and sends it to the motor drive module. The rotation of the motor drives the wheel to maintain its own balance and move in a specified way.

所述单片机采用STM32F411CEU6芯片，STM32F411CEU6芯片的5脚接8MHz晶振一端，8MHz晶振另一端接STM32F411CEU6芯片的6脚，STM32F411CEU6芯片的7脚通过电容C1接地，STM32F411CEU6芯片的14脚与MPU9250传感器模块的12脚相连；The single-chip microcomputer adopts STM32F411CEU6 chip. Pin 5 of STM32F411CEU6 chip is connected to one end of 8MHz crystal oscillator, and the other end of 8MHz crystal oscillator is connected to pin 6 of STM32F411CEU6 chip. connected;

STM32F411CEU6芯片的15脚通过电阻R12分别与电阻R16一端、NMOS管MOS4的栅极相连，NMOS管MOS4的源极分别与电阻R16另一端、地线相连，NMOS管MOS4的漏极分别与二极管D4阳极、第四个轮子的驱动电机的电源负极相连，第四个轮子的驱动电机的电源正极分别与电池正极、二极管D4阴极相连；Pin 15 of the STM32F411CEU6 chip is connected to one end of the resistor R16 and the gate of the NMOS transistor MOS4 through the resistor R12 respectively. The source of the NMOS transistor MOS4 is connected to the other end of the resistor R16 and the ground, respectively. , The negative pole of the power supply of the driving motor of the fourth wheel is connected, and the positive pole of the power supply of the driving motor of the fourth wheel is connected to the positive pole of the battery and the cathode of the diode D4 respectively;

STM32F411CEU6芯片的20脚通过电阻R4接地；Pin 20 of the STM32F411CEU6 chip is grounded through resistor R4;

STM32F411CEU6芯片的21脚通过电阻R11分别与电阻R15一端、NMOS管MOS3的栅极相连，NMOS管MOS3的源极分别与电阻R15另一端、地线相连，NMOS管MOS3的漏极分别与二极管D3阳极、第三个轮子的驱动电机的电源负极相连，第三个轮子的驱动电机的电源正极分别与电池正极、二极管D3阴极相连；Pin 21 of the STM32F411CEU6 chip is connected to one end of the resistor R15 and the gate of the NMOS transistor MOS3 through the resistor R11, respectively. The source of the NMOS transistor MOS3 is connected to the other end of the resistor R15 and the ground, respectively. . The negative pole of the power supply of the driving motor of the third wheel is connected to the negative pole of the power supply of the driving motor of the third wheel, and the positive pole of the power supply of the driving motor of the third wheel is respectively connected to the positive pole of the battery and the cathode of the diode D3;

STM32F411CEU6芯片的22脚通过电容C2接地；Pin 22 of the STM32F411CEU6 chip is grounded through capacitor C2;

STM32F411CEU6芯片的42脚通过电阻R10分别与电阻R14一端、NMOS管MOS2的栅极相连，NMOS管MOS2的源极分别与电阻R14另一端、地线相连，NMOS管MOS2的漏极分别与二极管D2阳极、第二个轮子的驱动电机的电源负极相连，第二个轮子的驱动电机的电源正极分别与电池正极、二极管D2阴极相连；Pin 42 of the STM32F411CEU6 chip is connected to one end of the resistor R14 and the gate of the NMOS transistor MOS2 through the resistor R10, respectively. The source of the NMOS transistor MOS2 is connected to the other end of the resistor R14 and the ground, respectively. , The negative pole of the power supply of the driving motor of the second wheel is connected, and the positive pole of the power supply of the driving motor of the second wheel is connected to the positive pole of the battery and the cathode of the diode D2 respectively;

STM32F411CEU6芯片的43脚通过电阻R9分别与电阻R13一端、NMOS管MOS1的栅极相连，NMOS管MOS1的源极分别与电阻R13另一端、地线相连，NMOS管MOS1的漏极分别与二极管D1阳极、第一个轮子的驱动电机的电源负极相连，第一个轮子的驱动电机的电源正极分别与电池正极、二极管D1阴极相连；Pin 43 of the STM32F411CEU6 chip is connected to one end of the resistor R13 and the gate of the NMOS transistor MOS1 through the resistor R9, respectively. The source of the NMOS transistor MOS1 is connected to the other end of the resistor R13 and the ground, respectively. , The negative pole of the power supply of the driving motor of the first wheel is connected, and the positive pole of the power supply of the driving motor of the first wheel is connected to the positive pole of the battery and the cathode of the diode D1 respectively;

STM32F411CEU6芯片的44脚通过R1接地；Pin 44 of the STM32F411CEU6 chip is grounded through R1;

STM32F411CEU6芯片的45脚与MPU9250传感器模块的23脚相连，STM32F411CEU6芯片的46脚与MPU9250传感器模块的24脚相连。Pin 45 of the STM32F411CEU6 chip is connected to pin 23 of the MPU9250 sensor module, and pin 46 of the STM32F411CEU6 chip is connected to pin 24 of the MPU9250 sensor module.

所述电源系统包括TP4059芯片、第一XC6204芯片和第二XC6204芯片，TP4059芯片的3脚分别与电池正极、电阻R8一端、电容C12一端、第二XC6204芯片的1脚相连，电阻R8另一端分别与第一XC6204芯片的1脚和3脚相连，电容C12另一端接地；第二XC6204芯片的5脚通过电感L1与MPU9250传感器模块的1脚相连；The power supply system includes a TP4059 chip, a first XC6204 chip and a second XC6204 chip. The 3 pins of the TP4059 chip are respectively connected to the positive electrode of the battery, one end of the resistor R8, one end of the capacitor C12, and one pin of the second XC6204 chip, and the other end of the resistor R8 is respectively connected. It is connected with pins 1 and 3 of the first XC6204 chip, and the other end of the capacitor C12 is grounded; the 5th pin of the second XC6204 chip is connected with the 1 pin of the MPU9250 sensor module through the inductor L1;

TP4059芯片的4脚分别与电容C8一端、电阻R6一端相连，电阻R6另一端通过电阻R7分别与地线、电容C8另一端相连。The 4 pins of the TP4059 chip are respectively connected to one end of the capacitor C8 and one end of the resistor R6, and the other end of the resistor R6 is respectively connected to the ground wire and the other end of the capacitor C8 through the resistor R7.

可以理解的是，以上关于本发明的具体描述，仅用于说明本发明而并非受限于本发明实施例所描述的技术方案，本领域的普通技术人员应当理解，仍然可以对本发明进行修改或等同替换，以达到相同的技术效果；只要满足使用需要，都在本发明的保护范围之内。It can be understood that the above specific description of the present invention is only used to illustrate the present invention and is not limited to the technical solutions described in the embodiments of the present invention. Those of ordinary skill in the art should understand that the present invention can still be modified or It is equivalent to replacement to achieve the same technical effect; as long as the needs of use are met, they are all within the protection scope of the present invention.

Claims (1)

1. The human-computer interaction force identification and control method of the rehabilitation walking training robot is characterized by comprising the following steps of:
step 1) decomposing generalized input force into tracking control force and human-computer interaction force based on a rehabilitation walking training robot system dynamic model to obtain a robot system dynamic model with the human-computer interaction force; the system dynamics model is described below
Figure FDA0002624503460000011
Wherein
Figure FDA0002624503460000012
Figure FDA0002624503460000013
X (t) is the actual walking track of the rehabilitation walking training robot, u (t) represents the generalized control input force, M represents the mass of the rehabilitation walking training robot, M represents the mass of the rehabilitee, I0Representing moment of inertia, M0,K(θ),
Figure FDA0002624503460000014
B (theta) is a coefficient matrix; theta represents the included angle between the horizontal axis and the connecting line between the center of the robot and the center of the first wheel, namely theta-theta1As can be seen from the structure of the rehabilitation walking robot,
Figure FDA0002624503460000015
θ3＝θ+π，
Figure FDA0002624503460000016
lirepresenting the distance, r, of the center of gravity of the system to the center of each wheel0Denotes the distance from the center to the center of gravity, #iDenotes the x' axis and the corresponding l of each wheeliAngle between, λiDenotes the distance of the center of gravity to each wheel, i ═ 1,2,3, 4; symbol
Figure FDA0002624503460000017
Decomposing u (t) into u0(t) and u1(t) and substituting into model (1) to obtain
Figure FDA0002624503460000018
Wherein u is0(t) representing a tracking control force to be designed for driving the rehabilitation walking training robot to track a training track specified by a doctor; u. of1(t) represents a human-computer interaction force to be observed; let X (t) be x1(t) represents a movement position of the robot,
Figure FDA0002624503460000019
which represents the speed of movement of the robot,
Figure FDA0002624503460000021
the expansion state of the system is represented, and then a dynamic model of the robot system with human-computer interaction force is obtained as follows
Figure FDA0002624503460000022
Wherein h is a bounded constant and represents the variation of the expansion state of the robot system;
step 2) a rehabilitation walking training robot system dynamics model based on the human-computer interaction force, a system observer combining a fixed gain and a time-varying gain is designed by utilizing the real-time position output of the robot, and the human-computer interaction force is estimated; real-time position output y (t) x of rehabilitation walking training robot1(t) is provided with
Figure FDA0002624503460000023
Denotes xj(t) (j is 1,2,3),
Figure FDA0002624503460000024
expressing the observation error, the observer was designed as follows:
Figure FDA0002624503460000025
wherein λ0，λ2For the observer to be designed the constant gain, lambda1(t) observer time-varying gain to be designed; according to the model (3) and the observer (4), the system for obtaining the observation error is
Figure FDA0002624503460000026
Step 3) designing a Lyapunov function based on the state observation error, the trajectory tracking error and the speed tracking error, so that the observation error system and the tracking error system are asymptotically stable; the actual walking track X (t) of the rehabilitation walking training robot, the training track X designated by the doctord(t) setting a tracking error e1(t) and velocity tracking error e2(t) are each independently
e1(t)＝X(t)-Xd(t) (6)
Figure FDA0002624503460000027
Further, the tracking error system obtained from equations (6), (7) and model (3) is
Figure FDA0002624503460000031
The Lyapunov function is designed according to the observation error and the tracking error as follows:
Figure FDA0002624503460000032
step 4) obtaining a solving method of observer gain and man-machine interaction force when the observation error system and the tracking error system reach asymptotic stability, and designing a compensation tracking controller according to the obtained man-machine interaction force; the formula (9) is derived along the observation error system (5) and the tracking error system (8), and the gain of the observer is adjusted to
Figure FDA0002624503460000033
Figure FDA0002624503460000034
Figure FDA0002624503460000035
The observation error system (5) can be made asymptotically stable, whereinσ(σ ═ 1,2,3) represents a specified small positive number, and then
Figure FDA0002624503460000036
Further, state x is extended by the system3(t) obtaining a human-machine interaction force of
Figure FDA0002624503460000037
After the human-computer interaction force is obtained, a compensation tracking controller u is designed0(t) is
Figure FDA0002624503460000038
The tracking error system (8) can be enabled to be asymptotically stable; wherein
Figure FDA0002624503460000039
A pseudo-inverse matrix representing B (θ);
an STM32F411 series single chip microcomputer based on ARM Cortex-M4 provides output PWM signals for a motor driving module, so that the rehabilitation walking training robot compensates the human-computer interaction force and accurately tracks a training track appointed by a doctor; an STM32F411 series single chip microcomputer is used as a main controller, and the input end of the main controller is connected with an MPU9250 sensor module and the output end of the main controller is connected with a motor driving module; the motor driving module is connected with the direct current motor; the power supply system supplies power to each unit module; the control method of the main controller is to read the feedback signal X (t) of the sensor module and the control command signal X given by the main controllerd(t) calculating to obtain an error signal, and obtaining a man-machine interaction force by using a feedback signal X (t); according to the error signal and the man-machine interaction force, the main controller calculates the control quantity of the motor according to a preset control algorithm, the control quantity is sent to the motor driving module, and the motor rotates to drive the wheels to maintain self balance and move according to a specified mode;
the single chip microcomputer adopts an STM32F411CEU6 chip, a pin 5 of the STM32F411CEU6 chip is connected with one end of an 8MHz crystal oscillator, the other end of the 8MHz crystal oscillator is connected with a pin 6 of the STM32F411CEU6 chip, a pin 7 of the STM32F411CEU6 chip is grounded through a capacitor C1, and a pin 14 of the STM32F411CEU6 chip is connected with a pin 12 of the MPU9250 sensor module;
a pin 15 of the STM32F411CEU6 chip is respectively connected with one end of a resistor R16 and the grid electrode of an NMOS tube MOS4 through a resistor R12, the source electrode of the NMOS tube MOS4 is respectively connected with the other end of a resistor R16 and the ground wire, the drain electrode of the NMOS tube MOS4 is respectively connected with the anode of a diode D4 and the cathode of a power supply of a driving motor of a fourth wheel, and the anode of the power supply of the driving motor of the fourth wheel is respectively connected with the anode of a battery and the cathode of the diode D4;
the pin 20 of the STM32F411CEU6 chip is grounded through a resistor R4;
a pin 21 of the STM32F411CEU6 chip is respectively connected with one end of a resistor R15 and the grid electrode of an NMOS tube MOS3 through a resistor R11, the source electrode of the NMOS tube MOS3 is respectively connected with the other end of a resistor R15 and the ground wire, the drain electrode of the NMOS tube MOS3 is respectively connected with the anode of a diode D3 and the cathode of a power supply of a driving motor of a third wheel, and the anode of the power supply of the driving motor of the third wheel is respectively connected with the anode of a battery and the cathode of the diode D3;
the 22 pin of the STM32F411CEU6 chip is grounded through a capacitor C2;
a pin 42 of the STM32F411CEU6 chip is respectively connected with one end of a resistor R14 and a grid electrode of an NMOS tube MOS2 through a resistor R10, a source electrode of the NMOS tube MOS2 is respectively connected with the other end of the resistor R14 and a ground wire, a drain electrode of the NMOS tube MOS2 is respectively connected with an anode of a diode D2 and a power supply cathode of a driving motor of the second wheel, and a power supply anode of the driving motor of the second wheel is respectively connected with a battery anode and a cathode of the diode D2;
a pin 43 of the STM32F411CEU6 chip is respectively connected with one end of a resistor R13 and the grid electrode of an NMOS tube MOS1 through a resistor R9, the source electrode of the NMOS tube MOS1 is respectively connected with the other end of the resistor R13 and the ground wire, the drain electrode of the NMOS tube MOS1 is respectively connected with the anode of a diode D1 and the cathode of a power supply of a driving motor of a first wheel, and the anode of the power supply of the driving motor of the first wheel is respectively connected with the anode of a battery and the cathode of the diode D1;
the pin 44 of the STM32F411CEU6 chip is grounded through R1;
the 45 pin of the STM32F411CEU6 chip is connected with the 23 pin of the MPU9250 sensor module, and the 46 pin of the STM32F411CEU6 chip is connected with the 24 pin of the MPU9250 sensor module;
the power supply system comprises a TP4059 chip, a first XC6204 chip and a second XC6204 chip, wherein 3 pins of the TP4059 chip are respectively connected with a battery anode, one end of a resistor R8, one end of a capacitor C12 and 1 pin of the second XC6204 chip, the other end of the resistor R8 is respectively connected with the 1 pin and the 3 pin of the first XC6204 chip, and the other end of the capacitor C12 is grounded; a pin 5 of the second XC6204 chip is connected with a pin 1 of the MPU9250 sensor module through an inductor L1;
the 4 feet of the TP4059 chip are respectively connected with one end of a capacitor C8 and one end of a resistor R6, and the other end of the resistor R6 is respectively connected with the ground wire and the other end of the capacitor C8 through a resistor R7.
CN201711121857.9A 2017-11-14 2017-11-14 Human-machine interaction force identification and control method for rehabilitation walking training robot Active CN107703762B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711121857.9A CN107703762B (en) 2017-11-14 2017-11-14 Human-machine interaction force identification and control method for rehabilitation walking training robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711121857.9A CN107703762B (en) 2017-11-14 2017-11-14 Human-machine interaction force identification and control method for rehabilitation walking training robot

Publications (2)

Publication Number Publication Date
CN107703762A CN107703762A (en) 2018-02-16
CN107703762B true CN107703762B (en) 2020-12-29

Family

ID=61178601

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711121857.9A Active CN107703762B (en) 2017-11-14 2017-11-14 Human-machine interaction force identification and control method for rehabilitation walking training robot

Country Status (1)

Country Link
CN (1) CN107703762B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110666794B (en) * 2019-09-25 2022-03-04 天津大学 A method for compensating load force and moment of robot end tool
CN112068434B (en) * 2020-09-09 2021-09-21 北京航空航天大学 Iterative learning control method for rehabilitation mechanical device based on extended state observer
CN112433475B (en) * 2020-11-27 2022-05-13 沈阳工业大学 SCN system offset identification-based cushion robot time-limited learning control method
CN112506054B (en) * 2020-11-27 2022-06-03 沈阳工业大学 Rehabilitation robot random finite time stable control based on SCN observation active thrust

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4246534B2 (en) * 2003-04-17 2009-04-02 本田技研工業株式会社 Method for estimating point of action of floor reaction force of bipedal mobile body and method of estimating joint moment of bipedal mobile body
JP5288418B2 (en) * 2008-06-04 2013-09-11 国立大学法人 東京大学 Identification of mechanical parameters
CN104375503B (en) * 2013-11-20 2017-05-03 沈阳工业大学 Wheel type rehabilitation robot control method independent from convalescent quality
CN104698848B (en) * 2015-02-11 2017-07-28 电子科技大学 A kind of control method of lower limb exoskeleton rehabilitation robot rehabilitation training
CN105867130B (en) * 2016-04-15 2018-11-13 沈阳工业大学 The track following error constraints method of controlling security of rehabilitation ambulation training robot

Also Published As

Publication number Publication date
CN107703762A (en) 2018-02-16

Similar Documents

Publication Publication Date Title
CN107831667B (en) Tracking control method for compensating human-computer interaction force of rehabilitation walking training robot
CN105963100B (en) By the lower limb rehabilitation robot self-adaptation control method assisted needed for patient motion
CN105867130B (en) The track following error constraints method of controlling security of rehabilitation ambulation training robot
Huang et al. Fuzzy enhanced adaptive admittance control of a wearable walking exoskeleton with step trajectory shaping
CN107703762B (en) Human-machine interaction force identification and control method for rehabilitation walking training robot
CN110215648A (en) Ectoskeleton based on body gait motor coordination characteristic coordinates gait control method
CN113001540B (en) Human-computer interaction intelligent control method and exoskeleton system for load mobile exoskeleton
CN114851171B (en) Gait trajectory tracking control method for lower extremity exoskeleton rehabilitation robot
CN110262510B (en) A humanoid robot mass center trajectory planning method to reduce walking energy consumption
CN104698848A (en) Control method for rehabilitation training of lower extremity exoskeleton rehabilitation robot
CN109330819B (en) Master-slave type upper limb exoskeleton rehabilitation robot control system and control method thereof
CN107479381A (en) Each axle tracking error optimal preventive control method of redundancy rehabilitation ambulation training robot
CN112775937A (en) Self-adaptive robust control method and device for under-actuated hydraulic single-leg power-assisted exoskeleton
CN112433495B (en) Fast finite time control of rehabilitation robot based on SCN human-machine uncertainty model
CN114721258B (en) Backstepping control method for lower extremity exoskeleton based on nonlinear extended state observer
Qiu et al. Exoskeleton active walking assistance control framework based on frequency adaptive dynamics movement primitives
Wang et al. ZMP theory-based gait planning and model-free trajectory tracking control of lower limb carrying exoskeleton system
CN116125817A (en) Lower limb exoskeleton fuzzy self-adaptive control method based on nonlinear disturbance observer
CN117936022A (en) Self-adaptive on-demand auxiliary training control method for rehabilitation training robot
CN112388616B (en) Adaptive robust force control method and device for under-actuated support leg assistance exoskeleton
Qu et al. Research on fuzzy adaptive impedance control of lower extremity exoskeleton
CN114952823A (en) Lower limb exoskeleton sensitivity amplification control method based on adaptive parameter estimation
CN104808657B (en) The full free degree controling power optimal track following method of rehabilitation ambulation training robot
Zhang et al. Fuzzy adaptive sliding mode control of lower limb exoskeleton rehabilitation robot
CN116423517A (en) Three-loop control method for lower extremity exoskeleton based on dynamic motion primitives

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20250813

Address after: 100089 Room 115, Building 4, No. 2 Jia, Tiyuan West Road, Haidian District, Beijing

Patentee after: BEIJING KELLYMED Co.,Ltd.

Country or region after: China

Address before: Shen Liaoning Road 110870 in Liaoning province Shenyang City Economic and Technological Development Zone No. 111

Patentee before: SHENYANG University OF TECHNOLOGY

Country or region before: China