WO2022228031A1 - 状态监测方法、安全控制装置、存储介质和机器人 - Google Patents

状态监测方法、安全控制装置、存储介质和机器人 Download PDF

Info

Publication number
WO2022228031A1
WO2022228031A1 PCT/CN2022/084633 CN2022084633W WO2022228031A1 WO 2022228031 A1 WO2022228031 A1 WO 2022228031A1 CN 2022084633 W CN2022084633 W CN 2022084633W WO 2022228031 A1 WO2022228031 A1 WO 2022228031A1
Authority
WO
WIPO (PCT)
Prior art keywords
joint
real
time
torque
robot
Prior art date
Application number
PCT/CN2022/084633
Other languages
English (en)
French (fr)
Inventor
刘谋云
周铜
王红梅
李志青
Original Assignee
上海神泰医疗科技有限公司
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 上海神泰医疗科技有限公司 filed Critical 上海神泰医疗科技有限公司
Publication of WO2022228031A1 publication Critical patent/WO2022228031A1/zh

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
    • A61H1/02—Stretching or bending or torsioning apparatus for exercising
    • A61H1/0237—Stretching or bending or torsioning apparatus for exercising for the lower limbs
    • A61H1/024—Knee
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
    • A61H1/02—Stretching or bending or torsioning apparatus for exercising
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
    • A61H1/02—Stretching or bending or torsioning apparatus for exercising
    • A61H1/0237—Stretching or bending or torsioning apparatus for exercising for the lower limbs
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
    • A61H1/02—Stretching or bending or torsioning apparatus for exercising
    • A61H1/0237—Stretching or bending or torsioning apparatus for exercising for the lower limbs
    • A61H1/0244—Hip
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
    • A61H1/02—Stretching or bending or torsioning apparatus for exercising
    • A61H1/0237—Stretching or bending or torsioning apparatus for exercising for the lower limbs
    • A61H1/0266—Foot
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
    • A61H1/02—Stretching or bending or torsioning apparatus for exercising
    • A61H1/0274—Stretching or bending or torsioning apparatus for exercising for the upper limbs
    • A61H1/0277—Elbow
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H1/00—Apparatus for passive exercising; Vibrating apparatus; Chiropractic devices, e.g. body impacting devices, external devices for briefly extending or aligning unbroken bones
    • A61H1/02—Stretching or bending or torsioning apparatus for exercising
    • A61H1/0274—Stretching or bending or torsioning apparatus for exercising for the upper limbs
    • A61H1/0281—Shoulder
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/01—Constructive details
    • A61H2201/0173—Means for preventing injuries
    • A61H2201/0176—By stopping operation
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61H—PHYSICAL THERAPY APPARATUS, e.g. DEVICES FOR LOCATING OR STIMULATING REFLEX POINTS IN THE BODY; ARTIFICIAL RESPIRATION; MASSAGE; BATHING DEVICES FOR SPECIAL THERAPEUTIC OR HYGIENIC PURPOSES OR SPECIFIC PARTS OF THE BODY
    • A61H2201/00—Characteristics of apparatus not provided for in the preceding codes
    • A61H2201/50—Control means thereof
    • A61H2201/5058—Sensors or detectors
    • A61H2201/5069—Angle sensors

Definitions

  • the invention relates to the technical field of robots, in particular to a state monitoring method, a safety control device, a storage medium and a robot.
  • the traditional torque monitoring safety design method of rehabilitation training robot generally adopts the maximum torque limit value as the safety judgment threshold, under normal circumstances, when a torque larger than the set safety judgment threshold torque occurs during the rehabilitation operation, the equipment will stop running.
  • the disadvantage of this traditional design method is that the setting of the maximum torque limit value is inconsistent with the real-time actual torque or has a large error, resulting in the inability to accurately and timely determine the torque safety fault of the equipment.
  • the purpose of the present invention is to provide a state monitoring method, a safety control device, a readable storage medium and a robot, which can solve the problem in the prior art that when the maximum torque limit value is used as the safety judgment threshold to monitor the torque of the robot, due to the maximum torque limit value There is a problem of inconsistency or large error with the real-time actual torque, which leads to the inability to accurately and timely judge the torque failure of the equipment.
  • the present invention provides a state monitoring method, which is applied to a robot.
  • the robot includes at least one mechanical arm, and the mechanical arm includes at least two connected joints.
  • the state monitoring method includes:
  • the robot is a lower limb/upper limb rehabilitation training robot
  • the real-time theoretical torque of each joint of each of the mechanical arms is calculated according to a preset torque calculation model, including:
  • T i T 1i +T 2i +T 3i
  • T i is the real-time theoretical torque of the joint i
  • T 1i is the real-time torque applied by the gravity of the mechanical arm received by the joint i
  • T 2i is the real-time friction compensation torque received by the joint i
  • T 3i is The real-time additional torque exerted by the lower/upper extremity of the patient on the joint i.
  • the calculation formula of the real-time torque T 1i exerted by the gravity of the mechanical arm on the joint i is as follows:
  • T Ii a i sin ⁇ i +b i
  • a i is a linear term coefficient related to the joint i
  • ⁇ i is the real-time angle of the joint i
  • b i is a constant term coefficient related to the joint i.
  • the calculation formula of the real-time friction compensation torque T 2i received by the joint i is as follows:
  • T 2i ci v i 2 +d i v i +e i
  • c i is a constant
  • v i is the real-time speed of the joint i
  • d i is a constant
  • e i is a constant
  • the calculation formula of the real-time additional torque T 3i exerted by the lower limb/upper limb of the patient on the joint i is as follows:
  • F i is the real-time force of the patient's lower limb/upper limb on the joint i
  • L Ri is the moment arm of F i .
  • the mechanical arm includes a first joint, a second joint and an end joint that are connected in sequence, wherein the first joint corresponds to the hip joint/shoulder joint of the human body, and the second joint corresponds to the knee joint/shoulder joint of the human body.
  • the elbow joint, the end joint corresponds to the ankle joint/wrist joint of the human body, and the calculation formula of the real-time additional torque T 32 exerted by the lower limb/upper limb of the patient on the second joint is as follows:
  • G1 is the weight of the patient's thigh/big arm
  • G2 is the weight of the patient's calf/forearm
  • L1 is the length of the patient's thigh/big arm
  • L2 is the length of the patient's calf/forearm
  • l 1 is the distance between the center of gravity of the patient's thigh/big arm and the hip/shoulder joint
  • l 2 is the distance between the center of gravity of the patient's calf/forearm and the knee/elbow joint
  • ⁇ is the patient's hip joint /The real-time angle of the shoulder joint
  • ⁇ is the real-time angle of the patient's knee/elbow joint
  • ⁇ 2 is the real-time angle of the second joint
  • ⁇ 1 is the real-time angle of the first joint.
  • the present invention also provides a safety control device, which is applied to a robot, the robot includes at least one mechanical arm, the mechanical arm includes at least two connected joints, and the safety control device includes a monitoring module, Judgment module and execution module;
  • the monitoring module includes a first monitoring unit
  • the first monitoring unit is configured to use the state monitoring method described above to monitor the torque of each joint of each of the robotic arms in real time;
  • the judgment module judges whether the robot is faulty according to the received monitoring result of the monitoring module, and divides the fault level;
  • the execution module executes an operation corresponding to the fault level according to the received fault level output by the judgment module.
  • the monitoring module further includes a second monitoring unit and a third monitoring unit, the second monitoring unit is used to monitor the angle of each joint of each of the robotic arms in real time, and the third monitoring unit It is used to monitor the speed of each joint of each of the robotic arms in real time.
  • the robot is a lower limb/upper limb rehabilitation training robot
  • the monitoring module further includes a fourth monitoring unit, and the fourth monitoring unit is used for real-time monitoring of the muscle state of the lower limb/upper limb of the patient.
  • the robot is a lower limb/upper limb rehabilitation training robot
  • the mechanical arm includes a first joint, a second joint and an end joint that are connected in sequence, wherein the first joint corresponds to the hip joint/shoulder joint of the human body,
  • the second joint corresponds to the knee joint/elbow joint of the human body,
  • the end joint corresponds to the ankle joint/wrist joint of the human body
  • the monitoring module further includes a fifth monitoring unit, and the fifth monitoring unit is used for monitoring the lower limbs of the patient. / The angle of each joint of the upper extremity is monitored in real time.
  • the monitoring module further includes a sixth monitoring unit, and the sixth monitoring unit is configured to monitor the alarm information of the robot in real time.
  • the monitoring module further includes a seventh monitoring unit, and the seventh monitoring unit is used for real-time monitoring of the virtual space under the Cartesian coordinate system of the robot.
  • the monitoring module further includes an eighth monitoring unit, and the eighth monitoring unit is configured to monitor the placement state of the robot in real time.
  • the execution module includes a stop unit, an alarm unit and a power supply control unit;
  • the stopping unit is used to perform a stopping operation when the robot has any level of failure
  • the alarm unit is used to perform an alarm operation when any level of failure occurs in the robot
  • the power control unit is configured to perform a shutdown operation when a failure of a corresponding level occurs in the robot.
  • the security control device further includes a communication module, and the communication module is used for data transmission.
  • the present invention also provides a robot, which includes the above-mentioned safety control device.
  • the present invention also provides a readable storage medium, where a computer program is stored in the readable storage medium, and when the computer program is executed by a processor, the state monitoring method described above is implemented.
  • the state monitoring method, safety control device, storage medium and robot provided by the present invention have the following advantages:
  • the state monitoring method provided by the present invention calculates the real-time theoretical torque of each joint of each mechanical arm of the robot respectively according to the preset torque calculation model, and then calculates the real-time theoretical torque and real-time actual torque of each joint respectively.
  • the real-time torque difference of each joint is finally compared with the preset error range corresponding to the real-time torque difference of each joint, so as to judge whether each joint has a torque safety fault at the current moment. It can be seen that the state monitoring method provided by the present invention calculates the theoretical torque of each joint at the current moment in real time, and compares the theoretical torque of each joint at the current moment with its actual torque at the current moment, so as to determine the current moment of each joint.
  • the state monitoring method provided by the present invention has higher monitoring sensitivity and better real-time performance, and can timely and accurately determine whether the robot is Torque failure occurs, which greatly improves the safety performance of the robot during use.
  • the safety control device provided by the present invention monitors the torque of each joint of each mechanical arm of the robot in real time through the first monitoring unit, judges whether the robot has a fault through the judgment module, and classifies the fault that occurs, Through the execution module, the corresponding operation is performed according to the failure level of the robot. It can be seen that the safety control device provided by the present invention can automatically identify the running state of the equipment, and can take corresponding graded response measures for different fault types, thereby further improving the safety performance of the robot during use.
  • the safety monitoring device provided by the present invention since the first monitoring unit uses the above-mentioned state monitoring method to monitor the torque of each joint of the robot in real time, the safety monitoring device provided by the present invention also has the above-mentioned state monitoring method. All the advantages are not repeated here.
  • the readable storage medium provided by the present invention belongs to the same inventive concept as the state monitoring method described above, it has all the advantages of the state monitoring method described above, so the advantages it has are no longer available. Repeat. Since the robot provided by the present invention belongs to the same inventive concept as the above-mentioned safety control device, it has all the advantages of the above-mentioned robot, so the advantages of the robot will not be repeated.
  • FIG. 1 is a schematic diagram of an application scenario of a lower limb rehabilitation training robot in an embodiment of the present invention
  • FIG. 2 is a schematic flowchart of a state monitoring method in an embodiment of the present invention.
  • FIG. 3 is a schematic diagram of the relationship between the torque applied by the gravity of the mechanical arm and the angle received by the second joint in an embodiment of the present invention
  • FIG. 5 is a schematic diagram of the stretching force generated by the robot end on the lower limb/upper limb of a patient in an embodiment of the present invention
  • FIG. 6 is a schematic diagram of the additional torque exerted by the lower/upper limb of the patient on the second joint according to an embodiment of the present invention
  • FIG. 7 is a schematic block diagram of a safety control device in an embodiment of the present invention.
  • FIG. 8 is a schematic diagram of a work flow of a safety control device in an embodiment of the present invention.
  • the main purpose of the present invention is to provide a state monitoring method, a safety control device, a readable storage medium and a robot, which can solve the problem in the prior art that when the maximum torque limit value is used as the safety judgment threshold to monitor the torque of the robot, due to the maximum torque limit There is a problem that the setting of the value does not match the real-time actual torque or has a large error, which leads to the inability to accurately and timely judge the torque failure of the equipment.
  • the present invention provides a state monitoring method, which is applied to a robot, and the robot can be an upper limb rehabilitation training robot, a lower limb rehabilitation training robot or other robots except the rehabilitation training robot, which is not limited in the present invention .
  • a lower limb rehabilitation robot please refer to FIG. 1 , which schematically shows a schematic diagram of an application scenario of a lower limb rehabilitation training robot provided by an embodiment of the present invention. As shown in FIG. 1
  • the lower limb rehabilitation training robot includes a mechanical arm, and each of the mechanical arms includes a first joint 10 , a second joint 20 and an end joint 30 that are connected in sequence (ie, continuous), wherein the first joint
  • the joint 10 corresponds to the hip joint 40 of the human body
  • the second joint 20 corresponds to the knee joint 50 of the human body
  • the end joint 30 corresponds to the ankle joint of the human body.
  • the end joint 30 includes a third joint for driving the leg splint 70 and a fourth joint for driving the ankle splint 60 , and the third joint is sleeved outside the fourth joint.
  • the lower limb rehabilitation training robot is placed on the side of the patient's leg that needs to be rehabilitated, and then the lower leg of the leg that needs to be rehabilitated is stretched into the leg splint 70, and the sole of the foot is stretched into the Inside the ankle splint 60 .
  • the lower limb rehabilitation training robot may also Two robotic arms can be included. At this time, the lower limb rehabilitation training robot can perform rehabilitation training on both legs of the patient at the same time.
  • the upper limb rehabilitation training robot includes at least one mechanical arm, and the mechanical arm includes a first joint, a second joint and an end joint that are connected in sequence, wherein , the first joint corresponds to the shoulder joint of the human body, the second joint corresponds to the elbow joint of the human body, and the end joint corresponds to the wrist joint of the human body.
  • FIG. 2 schematically shows a flow chart of a state monitoring method provided by an embodiment of the present invention.
  • the state monitoring method includes the following steps:
  • Step S100 Calculate the real-time theoretical torque of each joint of each of the robotic arms according to a preset torque calculation model.
  • Step S200 Calculate the real-time torque difference of each of the joints according to the real-time theoretical torque of each of the joints and the real-time actual torque of each of the joints.
  • Step S300 respectively determining whether the real-time torque difference of each of the joints exceeds its preset error range.
  • step S400 If the judgment result is that the real-time torque difference of one or more of the joints exceeds its preset error range, the following step S400 is performed:
  • Step S400 it is determined that the joint whose real-time torque difference exceeds its preset error range has a torque failure.
  • the real-time actual torque of each of the joints can be calculated by the motor installed on each of the joints through the current loop.
  • the preset error range corresponding to each joint may be specifically set according to the specific situation of each joint. For example, for the second joint 20, the preset error range can be set to ⁇ 6NM. If the difference between the real-time theoretical torque of the second joint 20 and its real-time actual torque is within the range of ⁇ 6NM, it means that the The second joint 20 has no torque failure at the current moment.
  • the state monitoring method provided by the present invention calculates the theoretical torque of each joint at the current moment in real time, and compares the theoretical torque of each joint at the current moment with its actual torque at the current moment, so as to determine the current moment of each joint. Whether there is a torque fault, therefore, compared with the method of using a fixed threshold for torque monitoring in the prior art, the state monitoring method provided by the present invention has higher monitoring sensitivity and better real-time performance, and can timely and accurately determine whether the robot is Torque failure occurs, which greatly improves the safety performance of the robot during use.
  • step S100 calculate the real-time theoretical torque of each joint of each of the mechanical arms, including:
  • T i T 1i +T 2i +T 3i (1)
  • T i is the real-time theoretical torque of the joint i
  • T 1i is the real-time torque applied by the gravity of the mechanical arm received by the joint i
  • T 2i is the real-time friction compensation torque received by the joint i
  • T 3i is The real-time additional torque exerted by the patient's lower/upper extremity to which the joint i is subjected.
  • the present invention comprehensively considers the real-time torque exerted by the gravity of the mechanical arm, the real-time friction compensation torque and the real-time additional torque brought by the lower limb/upper limb of the patient (herein, the lower limb or the upper limb) brought by each joint.
  • the real-time theoretical torque of each joint is calculated, so that the final calculated real-time theoretical torque can be closer to the real torque, so as to further improve the monitoring sensitivity of the state monitoring method provided by the present invention, and then can more timely and accurately judge whether the robot appears Torque failure.
  • T Ii a i sin ⁇ i +b i (2)
  • a i is a linear term coefficient related to the joint i
  • ⁇ i is the real-time angle of the joint i
  • b i is a constant term coefficient related to the joint i.
  • the calculation formula of the real-time torque exerted by the gravity of the manipulator on different joints is different, that is, the value of the primary term coefficient a i for different joints is different, and the value of the constant term coefficient b i is different. They are also different, and can be set according to the specific conditions of each joint. For the same joint, since the direction of force when it moves in the clockwise direction is different from the direction of force when it moves in the counterclockwise direction, therefore, for the same joint, when it runs in the clockwise direction, a The values of i and b i are different from the values of a i and b i when they run counterclockwise. In this way, according to the calculation formula of the real-time torque applied by the gravity of the robot arm received by each joint and its real-time angle information, the real-time torque applied by the gravity of the robot arm received by each joint can be obtained.
  • c i is the quadratic term coefficient related to the joint i
  • vi is the real-time speed of the joint i
  • d i is the first-order term coefficient related to the joint i
  • e i is the joint i.
  • the calculation formulas of the real-time friction compensation torque received by different joints are different, that is, the value of c i may be different for different joints, and the value of d i may also be different .
  • the value of may be different, which can be set according to the specific conditions of each joint. For the same joint, the direction of force when it moves in the clockwise direction is different from the direction of force when it moves in the counterclockwise direction. Therefore, for the same joint, when it runs in the clockwise direction, the c The values of i , d i , and e i are different from the values of ci , d i , and ei when they run counterclockwise.
  • the real-time friction compensation torque received by each joint can be obtained according to the calculation formula of the real-time friction compensation torque received by each joint and its real-time speed information.
  • the values of the linear term coefficient a i and the constant term coefficient bi in the calculation formula of the real-time torque T 1i exerted by the gravity of the mechanical arm that the joint i is subjected to, and the real-time friction force that the joint i is subjected to The quadratic term coefficient c i , the first-order term coefficient d i and the constant term coefficient e i in the calculation formula of the compensation torque T 2i can be obtained through experiments without attaching the lower limbs/upper limbs of the human body.
  • the values of a i , b i , c i , di and e i can be obtained by analyzing the torque values at different angles and at different speeds, and analyzing the obtained torque values, angle values and speed values .
  • FIG. 3 schematically shows the relationship between the torque applied by the gravity of the mechanical arm and the angle received by the second joint provided by an embodiment of the present invention.
  • T12 the real-time torque exerted by the gravity of the mechanical arm received by the second joint 20 during clockwise operation
  • FIG. 4 schematically shows the relationship between the friction compensation torque and the speed of the second joint provided by an embodiment of the present invention.
  • Table 1 the calculation formula of the real-time friction compensation torque T 22 received by the second joint 20 during clockwise operation can be obtained, as shown below. :
  • v 2 is the real-time speed of the second joint 20 .
  • ⁇ 2 is the real-time angle of the second joint 20 .
  • v 2 is the real-time speed of the second joint 20 .
  • the present invention takes a certain type of robot as an example to obtain the calculation formula of the real-time torque T 12 exerted by the gravity of the mechanical arm and the real-time friction compensation torque T 22 received by the second joint.
  • the real-time torque T 12 applied by the gravity of the mechanical arm and the real-time friction compensation torque T 22 received by the second joint are calculated formulas Each coefficient in may be different, and the specific value is determined according to the actual situation.
  • F i is the real-time force of the patient's lower limb/upper limb on the joint i
  • L Ri is the moment arm of F i .
  • the formula for calculating the real-time additional torque T 32 of the second joint 20 by the patient's lower/upper extremities is as follows:
  • F 2 is the real-time force exerted by the lower limb/upper limb of the patient on the second joint 20
  • L R2 is the moment arm of F 2 .
  • FIG. 5 schematically shows a schematic diagram of the stretching force generated by the end of the robot on the lower limb/upper limb of a patient provided by an embodiment of the present invention.
  • the real-time force F 2 of the patient’s lower limb/upper limb on the second joint 20 satisfies the following relationship:
  • G 1 is the weight of the patient's thigh
  • G 2 is the weight of the patient's calf
  • L 1 is the length of the patient's thigh
  • L 2 is the length of the patient's calf
  • l 1 is the distance between the center of gravity of the patient's thigh and the hip joint 40
  • l 2 is the distance between the center of gravity of the patient's calf and the knee joint 50
  • ⁇ is the real-time angle of the patient's hip joint 40
  • ⁇ is the patient's knee joint 50 real-time angles.
  • G 1 is the weight of the patient's forearm
  • G 2 is the weight of the patient's forearm
  • L 1 is the length of the patient's forearm
  • L 2 is the length of the patient's forearm
  • l1 is the distance between the center of gravity of the patient's upper arm and the shoulder joint 40'
  • l2 is the distance between the center of gravity of the patient's forearm and the elbow joint 50'
  • ⁇ is the real-time angle of the patient's shoulder joint 40'
  • ⁇ is the real-time angle of the patient's elbow joint 50'.
  • FIG. 6 schematically shows a schematic diagram of the torque generated by the lower limb/upper limb of the patient to the second joint according to an embodiment of the present invention.
  • the torque generated by the lower limb/upper limb of the patient to the second joint 20 satisfies the following relationship:
  • ⁇ 2 is the real-time angle of the second joint 20
  • ⁇ 1 is the real-time angle of the first joint 10 .
  • the robotic arm further includes a base joint 80 connected to the first joint 10 .
  • the real-time theoretical torque of the base joint 80 can also be calculated by the above formula (1), and the real-time torque exerted by the gravity of the robot arm received by the base joint 80 can be obtained by the above formula (2) , the real-time friction compensation torque it receives can be obtained by the above formula (3), and the additional torque applied to it by the lower limb/upper limb of the patient can be obtained by the above formula (8).
  • the state monitoring method provided by the present invention calculates the theoretical torque of each joint at the current moment in real time, and compares the theoretical torque of each joint at the current moment with its actual torque at the current moment, so as to judge whether each joint is at the current moment. Whether there is a torque fault at any time, compared with the method of using a fixed threshold for torque monitoring in the prior art, the state monitoring method provided by the present invention has higher monitoring sensitivity and better real-time performance, and can timely and accurately judge the robot Whether there is a torque failure greatly improves the safety performance of the robot during use.
  • the present invention also provides a safety control device applied to a robot, wherein the robot includes at least one robotic arm, and the robotic arm includes at least two connected joints.
  • the robot may be an upper limb rehabilitation training robot, a lower limb rehabilitation training robot or other robots except for the rehabilitation training robot, which is not limited in the present invention.
  • FIG. 7 schematically shows a structural block diagram of a safety control device according to an embodiment of the present invention.
  • the safety control device includes a monitoring module 100 , a judgment module 200 and an execution module 300 .
  • the monitoring module 100 includes a first monitoring unit 110, and the first monitoring unit 110 is configured to use the state monitoring method described above to monitor the torque of each joint of each of the robotic arms in real time; the The judgment module 200 is used for judging whether the robot has a fault according to the received monitoring result of the monitoring module 100, and classifying the fault level; the execution module 300 is used for according to the received output of the judgment module 200 The failure level, and the operation corresponding to the failure level is performed.
  • Table 2 schematically shows the corresponding relationship between the fault level and the fault type provided by an embodiment of the present invention. As shown in table 2:
  • the safety control device provided by the present invention can monitor in real time whether the torque difference of each joint (the difference between the theoretical torque and the actual torque) exceeds its preset error by monitoring the torque of each joint of the robot in real time. Therefore, when a torque fault occurs in a joint (divided into a medium-level fault), corresponding measures can be taken in time, for example, stop the machine, eliminate the peripheral abnormality, resume the reset, and if it cannot be eliminated, cut off the power of the robot and carry out Shutdown processing. It can be seen that the safety control device provided by the present invention can automatically identify the running state of the equipment, and can take corresponding graded response measures for different fault types, thereby further improving the safety performance of the robot during use. In addition, since the first monitoring unit 110 uses the state monitoring method described above to monitor the torque of each joint of the robot in real time, the safety monitoring device provided by the present invention also has the state monitoring method described above. All the advantages of , so it will not be repeated here.
  • the monitoring module 100 further includes a second monitoring unit 120 and a third monitoring unit 130; the second monitoring unit 120 is used to monitor the angles of each joint of the robot in real time;
  • the three monitoring units 130 are used to monitor the speed of each joint of the robot in real time.
  • the monitoring module 100 further includes a fourth monitoring unit 140, and the fourth monitoring unit 140 is used for monitoring the lower limb/upper limb of the patient. Real-time monitoring of muscle status.
  • a torque sensor can be installed on each joint of the robotic arm of the robot, and the torque measured by the torque sensor can be used to determine whether the patient's lower limb/upper limb has spasm. When the torque measured by the torque sensor suddenly increases, it means that The patient developed muscle spasms in the lower/upper extremities.
  • the first joint corresponds to the hip joint/shoulder joint of the human body (herein represents the hip joint or the shoulder joint, respectively Corresponding to the lower limb or upper limb, wherein the hip joint corresponds to the lower limb, and the shoulder joint corresponds to the upper limb), and the second joint corresponds to the knee joint/elbow joint of the human body (herein represents the knee joint or the elbow joint, corresponding to the lower limb or the upper limb respectively,
  • the knee joint corresponds to the lower limb, and the elbow joint corresponds to the upper limb)
  • the end joint corresponds to the ankle joint/wrist joint of the human body (here, the ankle joint or the wrist joint corresponds to the lower limb or the upper limb, respectively, wherein the ankle joint corresponds to the lower limb and the wrist joint When corresponding to the upper limb), as shown in FIG.
  • the monitoring module 100 further includes a fifth monitoring unit 150, and the fifth monitoring unit 150 is used to monitor the angle of each joint of the lower/upper limb of the patient in real time. Therefore, by monitoring the angle of each joint of the lower limb/upper limb of the patient in real time, it is possible to monitor in real time whether the angle (that is, the angle) of each joint of the lower limb/upper limb of the patient exceeds the preset angle range (the angle range may be artificially designed by the doctor).
  • the angle of each joint of the patient's upper limb may be calculated from the angle of each joint of the robotic arm and the patient's upper limb information (for example, the length of the upper arm and the forearm), and the angle of each joint of the patient's lower limb The angle can be calculated from the angle of each joint of the robotic arm and the patient's lower limb information (eg, thigh length, calf length).
  • angle sensors may also be installed on each joint of the patient's lower/upper limb to obtain the angle of each joint of the patient's lower/upper limb.
  • the monitoring module 100 further includes a sixth monitoring unit 160, and the sixth monitoring unit 160 is configured to monitor the alarm information of the robot in real time.
  • the alarm information of the robot such as controller alarm, driver alarm, motor alarm, communication alarm and other alarm information in real time
  • it is possible to monitor whether there is an alarm in real time so that when the alarm information is monitored (divided into It is a low-level fault), and take corresponding measures in time. For example, when a communication alarm is detected (that is, when the network connection is blocked), first stop, and then reset to continue.
  • the monitoring module 100 further includes a seventh monitoring unit 170 , and the seventh monitoring unit 170 is used to perform a virtual monitoring of the robot in the Cartesian coordinate system. space for real-time monitoring. Therefore, by monitoring the virtual space under the Cartesian coordinate system of the robot in real time, it is possible to monitor whether the robot exceeds the working range in real time, so that when the robot exceeds its working range (classified as a high-level fault) , and take corresponding measures in time, such as shutting down, putting down the human leg, closing the equipment, and starting over.
  • the virtual space in the Cartesian coordinate system of the robot referred to in this article refers to the three-dimensional position of the robotic arm of the robot in the Cartesian coordinate system.
  • the monitoring module 100 may further include an eighth monitoring unit 180, and the eighth monitoring unit 180 is configured to monitor the placement state of the robot in real time.
  • the placement state of the robot can be monitored through a gyroscope installed on the robot, and thus, by monitoring the placement state of the robot in real time, it is possible to monitor in real time whether the robot rolls over, Inclination, translation and other phenomena, so that when the robot rolls over, tilts or translates (classified as the highest level of faults), corresponding measures can be taken in time, such as cutting off the power supply and shutting down.
  • the execution module 300 includes a stop unit 310, an alarm unit 320 and a power control unit 330; the stop unit 310 is used to perform a stop operation when the robot has any level of failure; The alarm unit 320 is configured to perform an alarm operation when the robot has any level of failure; the power control unit 330 is configured to perform a shutdown operation when the robot has a corresponding level of failure. Therefore, this setting can better ensure that the execution module 300 can timely and accurately execute the corresponding operation according to the fault level output by the judgment module 200 .
  • the safety control device further includes an initialization module 400 and a communication module 500, wherein the initialization module 400 is used for parameter configuration, signal initialization and/or automatic
  • the communication module 500 is used for data transmission. Therefore, hardware parameter configuration, software signal initialization and self-checking functions can be realized through the initialization module 400; various data can be transmitted through the communication module 500 to realize communication with the host computer, safety signal lights, and safety buzzer.
  • the communication module 500 can also realize the mutual communication among the initialization module 400 , the monitoring module 100 , the judgment module 200 , and the execution module 300 .
  • FIG. 8 schematically shows a work flow diagram of a safety control device provided by an embodiment of the present invention.
  • the patient can be rehabilitated for the lower limb/upper limb.
  • the monitoring module 100 can monitor in real time Various data, the judging module 200 judges whether there is a fault in real time according to the data monitored by the monitoring module 100, and if the judgment result is a fault, it outputs the corresponding fault level, and the execution module 300 according to the received If the fault level is detected, perform corresponding operations (such as alarm, shutdown, shutdown); for recoverable faults (faults that can be eliminated by reset operation), they will be repeatedly confirmed within a given time period, and if the fault is eliminated, restart Training; if the fault persists, intelligently intervene to eliminate the fault; if the fault still cannot be eliminated, end the training process, thereby improving the safety performance of the robot during use.
  • corresponding operations such as alarm, shutdown, shutdown
  • recoverable faults faults that can be eliminated by reset operation
  • the present invention further provides a robot comprising the above-mentioned safety control device. Since the robot provided by the present invention and the above-mentioned safety control device belong to the same inventive concept, it has all the advantages of the above-mentioned safety control device, so the advantages of the robot will not be repeated.
  • the present invention also provides a readable storage medium, where a computer program is stored in the readable storage medium, and the computer program can realize the state described above when executed by a processor monitoring method. Since the readable storage medium provided by the present invention belongs to the same inventive concept as the state monitoring method described above, it has all the advantages of the state monitoring method described above, so the advantages of the state monitoring method will not be repeated.
  • the readable storage medium of the embodiments of the present invention may adopt any combination of one or more computer-readable media.
  • the readable medium may be a computer-readable signal medium or a computer-readable storage medium.
  • the computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus or device, or a combination of any of the above.
  • a computer-readable storage medium can be any tangible medium that contains or stores a program that can be used by or in combination with an instruction execution system, apparatus, or device.
  • a computer-readable signal medium may include a propagated data signal in baseband or as part of a carrier wave, with computer-readable program code embodied thereon. Such propagated data signals may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the foregoing.
  • a computer-readable signal medium can also be any computer-readable medium other than a computer-readable storage medium that can transmit, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device .
  • Computer program code for carrying out operations of the present invention may be written in one or more programming languages, including object-oriented programming languages - such as Java, Smalltalk, C++, but also conventional Procedural programming language - such as the "C" language or similar programming language.
  • the program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer, or entirely on the remote computer or server.
  • the remote computer may be connected to the user's computer through any kind of network, including a local area network (LAN) or wide area network (WAN), or may be connected to an external computer (eg, using an Internet service provider to connect through the Internet) ).
  • LAN local area network
  • WAN wide area network
  • Internet service provider to connect through the Internet
  • the state monitoring method, safety control device, storage medium and robot provided by the present invention have the following advantages:
  • the state monitoring method provided by the present invention calculates the real-time theoretical torque of each joint of each mechanical arm of the robot respectively according to the preset torque calculation model, and then calculates the real-time theoretical torque and real-time actual torque of each joint respectively.
  • the real-time torque difference of each joint is finally compared with the preset error range corresponding to the real-time torque difference of each joint, so as to judge whether each joint has a torque safety fault at the current moment. It can be seen that the state monitoring method provided by the present invention calculates the theoretical torque of each joint at the current moment in real time, and compares the theoretical torque of each joint at the current moment with its actual torque at the current moment, so as to determine the current moment of each joint.
  • the state monitoring method provided by the present invention has higher monitoring sensitivity and better real-time performance, and can timely and accurately determine whether the robot is Torque failure occurs, which greatly improves the safety performance of the robot during use.
  • the safety control device provided by the present invention monitors the torque of each joint of each mechanical arm of the robot in real time through the first monitoring unit, judges whether the robot has a fault through the judgment module, and classifies the fault that occurs, Through the execution module, the corresponding operation is performed according to the failure level of the robot. It can be seen that the safety control device provided by the present invention can automatically identify the running state of the equipment, and can take corresponding graded response measures for different fault types, thereby further improving the safety performance of the robot during use.
  • the safety monitoring device provided by the present invention since the first monitoring unit uses the above-mentioned state monitoring method to monitor the torque of each joint of the robot in real time, the safety monitoring device provided by the present invention also has the above-mentioned state monitoring method. All the advantages are not repeated here.
  • the readable storage medium provided by the present invention belongs to the same inventive concept as the state monitoring method described above, it has all the advantages of the state monitoring method described above, so the advantages it has are no longer available. Repeat. Since the robot provided by the present invention belongs to the same inventive concept as the above-mentioned safety control device, it has all the advantages of the above-mentioned robot, so the advantages of the robot will not be repeated.
  • each block in the flowcharts or block diagrams may represent a module, program segment, or portion of code, which contains one or more possible functions for implementing the specified logical function(s) Execution instructions, the module, segment, or portion of code containing one or more executable instructions for implementing specified logical functions.
  • each functional module in the various embodiments herein may be integrated together to form an independent part, or each module may exist alone, or two or more modules may be integrated to form an independent part.

Landscapes

  • Health & Medical Sciences (AREA)
  • Epidemiology (AREA)
  • Pain & Pain Management (AREA)
  • Physical Education & Sports Medicine (AREA)
  • Rehabilitation Therapy (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Manipulator (AREA)

Abstract

一种状态监测方法、安全控制装置、存储介质和机器人，状态监测方法包括：根据预设的扭矩计算模型，分别计算各条机械臂的各个关节的实时理论扭矩（S100）；根据各个关节的实时理论扭矩以及各个关节的实时实际扭矩，分别计算各个关节的实时扭矩差值（S200）；分别判断各个关节的实时扭矩差值是否超出其预设误差范围（S300）；若关节的实时扭矩差值超出其预设误差范围，则判定关节出现扭矩故障（S400）。该状态监测方法的监测灵敏度更高，实时性更好，能够及时、准确地判断出机器人是否出现扭矩故障，大大提高了机器人在使用过程中的安全性能。

Description

状态监测方法、安全控制装置、存储介质和机器人 技术领域
本发明涉及机器人技术领域，特别涉及一种状态监测方法、安全控制装置、存储介质和机器人。
背景技术
随着社会老龄化加剧及脑卒中、脊髓损伤、脑外伤等各种疾病造成的残障人口迅速增长，由此带来的康复器具需求也不断增加。然而，现有康复医疗资源非常紧缺，国内普遍采用的康复治疗方法存在人员消耗大、康复周期长、效果有限等问题。康复机器人正是解决传统康复方法的有效技术手段。
为确保康复训练机器人设备的正常工作，需要提高设备的安全控制性能。尤其是一旦人腿发生痉挛或操作方式错误，需要机器人运行设备做出快速和正确的响应，以此确保患者和医生在使用医疗设备时的安全性。
由于传统的康复训练机器人的扭矩监控安全设计方法一般是采用扭矩最大极限值作为安全判断阈值，通常情况下，当康复运行过程中若出现比设定的安全判断阈值扭矩更大的扭矩时，设备将停止运行。这种传统的设计方法，其缺陷在于最大扭矩极限值的设定存在与实时的实际扭矩不符或误差较大，导致不能准确、及时的判断设备出现的扭矩安全故障。
发明内容
本发明的目的在于提供一种状态监测方法、安全控制装置、可读存储介质和机器人，可以解决现有技术中，采用扭矩最大极限值作为安全判断阈值监控机器人的扭矩时，由于最大扭矩极限值的设定存在与实时的实际扭矩不符或误差较大，导致不能准确、及时的判断设备出现的扭矩故障的问题。
为解决上述技术问题，本发明提供一种状态监测方法，应用于机器人，所述机器人包括至少一条机械臂，所述机械臂包括至少两个相连的关节，所述状态监测方法包括：
根据预设的扭矩计算模型，分别计算各条所述机械臂的各个关节的实时 理论扭矩；
根据各个所述关节的实时理论扭矩以及各个所述关节的实时实际扭矩，分别计算各个所述关节的实时扭矩差值；
分别判断各个所述关节的实时扭矩差值是否超出对应的预设误差范围；
若所述关节的实时扭矩差值超出对应的预设误差范围，则判定所述关节出现扭矩故障。
可选的，所述机器人为下肢/上肢康复训练机器人，所述根据预设的扭矩计算模型，分别计算各条所述机械臂的各个关节的实时理论扭矩，包括：
按照如下扭矩计算模型，分别计算各条所述机械臂的各个关节的实时理论扭矩：
T i＝T 1i+T 2i+T 3i
其中，T i为关节i的实时理论扭矩，T 1i为所述关节i所受到的机械臂重力所施加的实时扭矩，T 2i为所述关节i所受到的实时摩擦力补偿扭矩，T 3i为所述关节i所受到的患者的下肢/上肢所施加的的实时附加扭矩。
可选的，所述关节i所受到的机械臂重力所施加的实时扭矩T 1i的计算公式如下所示：
T Ii＝a isinθ i+b i
其中，a i为与所述关节i相关的一次项系数，θ i为所述关节i的实时角度，b i为与所述关节i相关的常数项系数。
可选的，所述关节i所受到的实时摩擦力补偿扭矩T 2i的计算公式如下所示：
T 2i＝c iv i 2+d iv i+e i
其中，c i为一常数，v i为所述关节i的实时速度，d i为一常数，e i为一常数。
可选的，所述关节i所受到的患者的下肢/上肢所施加的实时附加扭矩T 3i的计算公式如下所示：
T 3i＝F i×L Ri
其中，F i为患者的下肢/上肢对所述关节i的实时作用力，L Ri为F i的力臂。
可选的，所述机械臂包括依次相连的第一关节、第二关节和末端关节，其中，所述第一关节对应人体的髋关节/肩关节，所述第二关节对应人体的膝关节/肘关节，所述末端关节对应人体的踝关节/腕关节，所述第二关节所受到的患者的下肢/上肢所施加的实时附加扭矩T 32的计算公式如下所示：
T 32＝F 2×L R2
Figure PCTCN2022084633-appb-000001
L R2＝L 2×sin(θ 2-θ 1)
其中，G 1为患者的大腿/大臂的重量，G 2为患者的小腿/小臂的重量，L 1为患者的大腿/大臂的长度，L 2为患者的小腿/小臂的长度，l 1为患者的大腿/大臂的重心与髋关节/肩关节之间的距离，l 2为患者的小腿/小臂的重心与膝关节/肘关节之间的距离，α为患者的髋关节/肩关节的实时角度，β为患者的膝关节/肘关节的实时角度，θ 2为所述第二关节的实时角度，θ 1为所述第一关节的实时角度。
为解决上述技术问题，本发明还提供一种安全控制装置，应用于机器人，所述机器人包括至少一条机械臂，所述机械臂包括至少两个相连的关节，所述安全控制装置包括监测模块、判断模块和执行模块；
所述监测模块包括第一监测单元；
所述第一监测单元用于采用上文所述的状态监测方法对各条所述机械臂的各个关节的扭矩进行实时监测；
所述判断模块根据接收到的所述监测模块的监测结果，判断所述机器人是否出现故障，并进行故障等级划分；
所述执行模块根据接收到的所述判断模块输出的故障等级，执行与所述故障等级对应的操作。
可选的，所述监测模块还包括第二监测单元和第三监测单元，所述第二监测单元用于对各条所述机械臂的各个关节的角度进行实时监测，所述第三监测单元用于对各条所述机械臂的各个关节的速度进行实时监测。
可选的，所述机器人为下肢/上肢康复训练机器人，所述监测模块还包括 第四监测单元，所述第四监测单元用于对患者的下肢/上肢的肌肉状态进行实时监测。
可选的，所述机器人为下肢/上肢康复训练机器人，所述机械臂包括依次相连的第一关节、第二关节和末端关节，其中，所述第一关节对应人体的髋关节/肩关节，所述第二关节对应人体的膝关节/肘关节，所述末端关节对应人体的踝关节/腕关节，所述监测模块还包括第五监测单元，所述第五监测单元用于对患者的下肢/上肢的各个关节的角度进行实时监测。
可选的，所述监测模块还包括第六监测单元，所述第六监测单元用于对所述机器人的报警信息进行实时监测。
可选的，所述监测模块还包括第七监测单元，所述第七监测单元用于对所述机器人的笛卡尔坐标系下的虚拟空间进行实时监测。
可选的，所述监测模块还包括第八监测单元，所述第八监测单元用于对所述机器人的安放状态进行实时监测。
可选的，所述执行模块包括停止单元、报警单元和电源控制单元；
所述停止单元用于在所述机器人出现任意等级的故障时，执行停机操作；
所述报警单元用于在所述机器人出现任意等级的故障时，执行报警操作；
所述电源控制单元用于在所述机器人出现对应等级的故障时，执行关机操作。
可选的，所述安全控制装置还包括通信模块，所述通信模块用于进行数据的传输。
为解决上述技术问题，本发明还提供一种机器人，所述机器人包括上文所述的安全控制装置。
为解决上述技术问题，本发明还提供一种可读存储介质，所述可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时，实现上文所述的状态监测方法。
与现有技术相比，本发明提供的状态监测方法、安全控制装置、存储介质和机器人具有以下优点：
(1)本发明提供的状态监测方法通过根据预设的扭矩计算模型，分别计 算机器人的各条机械臂的各个关节的实时理论扭矩，再根据各个关节的实时理论扭矩以及实时实际扭矩，分别计算各个关节的实时扭矩差值，最后将各个关节的实时扭矩差值和与其对应的预设误差范围进行比较，以判断各个关节在当前时刻是否出现扭矩安全故障。由此可见，本发明提供的状态监测方法通过实时计算各个关节在当前时刻的理论扭矩，并将各个关节在当前时刻的理论扭矩与其在当前时刻的实际扭矩进行比较，以判断各个关节在当前时刻是否出现扭矩故障，因此相比于现有技术中采用固定的阈值进行扭矩监测的方法，本发明提供的状态监测方法的监测灵敏度更高，实时性更好，能够及时、准确地判断出机器人是否出现扭矩故障，大大提高了机器人在使用过程中的安全性能。
(2)本发明提供的安全控制装置通过第一监测单元对机器人的各条机械臂的各个关节的扭矩进行实时监测，通过判断模块判断机器人是否出现故障，并对所出现的故障进行等级划分，通过执行模块根据机器人出现的故障等级，执行对应的操作。由此可见，本发明提供的安全控制装置能够自动识别设备的运行状态，并能够针对不同的故障类型采取相应的分级响应措施，从而能够进一步提高机器人在使用过程中的安全性能。此外，由于所述第一监测单元是采用上文所述的状态监测方法对机器人的各个关节的扭矩进行实时监测的，因此本发明提供的安全监控装置还具有上文所述的状态监测方法的所有优点，故对此不再进行赘述。
(3)由于本发明提供的可读存储介质与上文所述的状态监测方法属于同一发明构思，因此其具有上文所述的状态监测方法的所有优点，故对其所具有的优点不再进行赘述。由于本发明提供的机器人与上文所述的安全控制装置属于同一发明构思，因此其具有上文所述的机器人的所有优点，故对其所具有的优点不再进行赘述。
附图说明
图1为本发明一实施方式中的下肢康复训练机器人的应用场景示意图；
图2为本发明一实施方式中的状态监测方法的流程示意图；
图3为本发明一实施方式中的第二关节所受到的机械臂重力所施加的扭矩与角度之间的关系示意图；
图4为本发明一实施方式中的第二关节所受到的摩擦力补偿扭矩与速度之间的关系示意图；
图5为本发明一实施方式中的机器人末端对患者的下肢/上肢产生的拉伸作用力的示意图；
图6为本发明一实施方式中的第二关节所受到的患者的下肢/上肢所施加的附加扭矩的示意图；
图7为本发明一实施方式中的安全控制装置的方框结构示意图；
图8为本发明一实施方式中的安全控制装置的工作流程示意图；
其中，附图标记如下：
第一关节-10；第二关节-20；末端关节-30；髋关节-40；肩关节-40’；膝关节-50；肘关节-50’；踝夹板-60；腿夹板-70；基座关节-80；
监测模块-100；判断模块-200；执行模块-300；初始化模块-400；通信模块-500；第一监测单元-110；第二监测单元-120；第三监测单元-130；第四监测单元-140；第五监测单元-150；第六监测单元-160；第七监测单元-170；第八监测单元-180；停止单元-310；报警单元-320；电源控制单元-330。
具体实施方式
以下结合附图和具体实施方式对本发明提出的状态监测方法、安全控制装置、存储介质和机器人作进一步详细说明。根据下面说明，本发明的优点和特征将更清楚。需要说明的是，附图采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施方式的目的。为了使本发明的目的、特征和优点能够更加明显易懂，请参阅附图。须知，本说明书所附图式所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明实施的限定条件，任何结构的修饰、比例关系的改变或大小的调整，在与本发明所能产生的功效及所能达成的目的相同或近似的情况下，均应仍落在本发明所揭示的技术 内容能涵盖的范围内。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本发明的主要目的在于提供一种状态监测方法、安全控制装置、可读存储介质和机器人，可以解决现有技术中，采用扭矩最大极限值作为安全判断阈值监控机器人的扭矩时，由于最大扭矩极限值的设定存在与实时的实际扭矩不符或误差较大，导致不能准确、及时的判断设备出现的扭矩故障的问题。
为实现上述目的，本发明提供一种状态监测方法，应用于机器人，所述机器人可以为上肢康复训练机器人、下肢康复训练机器人或除康复训练机器人以外的其它机器人，本发明对此并不进行限制。以下肢康复机器人为例，请参考图1，其示意性地给出了本发明一实施方式提供的下肢康复训练机器人的应用场景示意图。如图1所示，所述下肢康复训练机器人包括一条机械臂，所述机械臂均包括依次相连(即连续)的第一关节10、第二关节20和末端关节30，其中，所述第一关节10对应人体的髋关节40，所述第二关节20对应人体的膝关节50，所述末端关节30对应人体的踝关节。其中，所述末端关节30包括用于驱动腿夹板70的第三关节以及用于驱动踝夹板60的第四关节，所述第三关节套设于所述第四关节外部。在具体使用时，将所述下肢康复训练机器人放置于患者的需要进行康复训练的腿侧，然后将需要进行康复训练的腿的小腿部伸入所述腿夹板70内，将脚掌部伸入所述踝夹板60内部。需要说明的是，虽然本实施方式是以只有一条机械臂的下肢康复机器人为例进行说明，但是，如本领域技术人员所能理解的，在其它一些实施方式中，所述下肢康复训练机器人还可以包括两条机械臂。此时，通过所述下肢康复训 练机器人可以同时对患者的两条腿进行康复训练。此外，需要说明的是，当所述机器人为上肢康复训练机器人时，所述上肢康复训练机器人包括至少一条机械臂，所述机械臂包括依次相连的第一关节、第二关节和末端关节，其中，所述第一关节对应人体的肩关节，所述第二关节对应人体的肘关节，所述末端关节对应人体的腕关节。
请继续参考图2，其示意性地给出了本发明一实施方式提供的状态监测方法的流程示意图。如图2所示，所述状态监测方法包括如下步骤：
步骤S100、根据预设的扭矩计算模型，分别计算各条所述机械臂的各个关节的实时理论扭矩。
步骤S200、根据各个所述关节的实时理论扭矩以及各个所述关节的实时实际扭矩，分别计算各个所述关节的实时扭矩差值。
步骤S300、分别判断各个所述关节的实时扭矩差值是否超出其预设误差范围。
若判断结果为其中的一个或几个所述关节的实时扭矩差值超出其预设误差范围，则执行下述的步骤S400：
步骤S400、判定实时扭矩差值超出其预设误差范围的所述关节出现扭矩故障。
需要说明的是，如本领域技术人员所能理解的，各个所述关节的实时实际扭矩可通过安装于各个所述关节上的电机通过电流环计算得到，具体可参考现有技术，本发明对此不再进行赘述。此外，需要说明的是，各个关节所对应的预设误差范围具体可根据各个关节的具体情况进行设定。例如，对于第二关节20，可以将预设误差范围设置为±6NM，若所述第二关节20的实时理论扭矩与其实时实际扭矩之间的差值在±6NM范围内，则说明所述第二关节20在当前时刻并未出现扭矩故障，若所述第二关节20的实时理论扭矩与其实时实际扭矩之间的差值超出±6NM，则说明所述第二关节20在当前时刻出现了扭矩故障。由此可见，本发明提供的状态监测方法通过实时计算各个关节在当前时刻的理论扭矩，并将各个关节在当前时刻的理论扭矩与其在当前时刻的实际扭矩进行比较，以判断各个关节在当前时刻是否出现扭矩故障， 因此相比于现有技术中采用固定的阈值进行扭矩监测的方法，本发明提供的状态监测方法的监测灵敏度更高，实时性更好，能够及时、准确地判断出机器人是否出现扭矩故障，大大提高了机器人在使用过程中的安全性能。
具体地，步骤S100中，根据预设的扭矩计算模型，分别计算各条所述机械臂的各个关节的实时理论扭矩，包括：
按照如下扭矩计算模型，分别计算各条所述机械臂的各个关节的实时理论扭矩：
T i＝T 1i+T 2i+T 3i        (1)
其中，T i为关节i的实时理论扭矩，T 1i为所述关节i所受到的机械臂重力所施加的实时扭矩，T 2i为所述关节i所受到的实时摩擦力补偿扭矩，T 3i为所述关节i所受到的患者的下肢/上肢所施加的实时附加扭矩。
由此，本发明通过综合考虑各个关节所受到的机械臂重力所施加的实时扭矩、实时摩擦力补偿扭矩以及患者的下肢/上肢(在此表示下肢或者上肢)所带来的实时附加扭矩，来计算各个关节的实时理论扭矩，从而可以使得最终计算得到的实时理论扭矩更加接近真实的扭矩，以进一步提高本发明提供的状态监测方法的监测灵敏度，进而能够更加及时、准确地判断出机器人是否出现扭矩故障。
进一步地，所述关节i所受到的机械臂重力所施加的实时扭矩T 1i的计算公式如下所示：
T Ii＝a isinθ i+b i         (2)
其中，a i为与所述关节i相关的一次项系数，θ i为所述关节i的实时角度，b i为与所述关节i相关的常数项系数。
需要说明的是，不同关节所受到的机械臂重力所施加的实时扭矩的计算公式是不同的，即针对不同的关节一次项系数a i的取值各不相同，常数项系数b i的取值也各不相同，具体可以根据各个关节的具体情况进行设定。针对同一个关节，由于其在沿顺时针方向运动时的受力方向与其在沿逆时针方向运动时的受力方向是不同的，因此，针对同一个关节，其在顺时针方向运行时的a i、b i的取值与其在逆时针方向运行时的a i、b i的取值是不同的。由此，根据各个 关节所受到的机械臂重力所施加的实时扭矩的计算公式以及其实时角度信息，即可获取各个关节所受到的机械臂重力所施加的实时扭矩。
进一步地，所述关节i所受到的实时摩擦力补偿扭矩T 2i的计算公式如下所示：
T 2i＝c iv i 2+d iv i+e i        (3)
其中，c i为与所述关节i相关的二次项系数，v i为所述关节i的实时速度，d i为与所述关节i相关的一次项系数，e i为与所述关节i相关的常数项系数。
需要说明的是，不同关节所受到的实时摩擦力补偿扭矩的计算公式是不同的，即针对不同的关节c i的取值可能各不相同，d i的取值也可能各不相同，e i的取值可能各不相同，具体可以根据各个关节的具体情况进行设定。针对同一个关节，由于其在沿顺时针方向运动时的受力方向与其在沿逆时针方向运动时的受力方向是不同的，因此，针对同一个关节，其在顺时针方向运行时的c i、d i、e i的取值与其在逆时针方向运行时的c i、d i、e i的取值是不同的。由此，根据各个关节所受到的实时摩擦力补偿扭矩的计算公式以及其实时速度信息，即可获取各个关节所受到的实时摩擦力补偿扭矩。
具体地，所述关节i所受到的机械臂重力所施加的实时扭矩T 1i的计算公式中的一次项系数a i和常数项系数b i的取值以及所述关节i所受到的实时摩擦力补偿扭矩T 2i的计算公式中的二次项系数c i、一次项系数d i和常数项系数e i可以通过在不附加人体下肢/上肢的情况下进行试验得到，通过获取所述关节i在不同角度、不同速度下的扭矩值，并对所获得的扭矩值、角度值和速度值进行分析，即可获取a i、b i、c i、d i和e i的取值。
以第二关节为例，请参考表1，其示意性地给出了本发明一实施方式提供的机器人的第二关节在不附加人体下肢/上肢的情况下顺时针运动时，在不同角度、不同速度下的扭矩值。
表1 第二关节在不同角度、不同速度下的扭矩值
Figure PCTCN2022084633-appb-000002
Figure PCTCN2022084633-appb-000003
Figure PCTCN2022084633-appb-000004
请参考图3，其示意性地给出了本发明一实施方式提供的第二关节所受到的机械臂重力所施加的扭矩与角度之间的关系示意图。如图3所示，通过对表1中的数据进行分析与拟合，可获得顺时针运行时，所述第二关节20所受到的机械臂重力所施加的实时扭矩T 12的计算公式，具体如下所示：
T 12＝-51.115sinθ 2-33.811      (4)
请参考图4，其示意性地给出了本发明一实施方式提供的第二关节在所受到的摩擦力补偿扭矩与速度之间的关系示意图。如图4所示，通过对表1中 的数据进行分析与拟合，可获得顺时针运行时，所述第二关节20所受到的实时摩擦力补偿扭矩T 22的计算公式，具体如下所示：
T 22＝0.1021v 2 2-2.463v 2+8.3193        (5)
其中，v 2为所述第二关节20的实时速度。
同理，通过获取所述第二关节20在不附加人体下肢/上肢的情况下逆针运动时，其在不同角度、不同速度下的扭矩值，并对所获得的数据进行分析与拟合，可获得逆时针运行时，所述第二关节20所受到的机械臂重力所施加的实时扭矩T 12以及所述第二关节20所受到的实时摩擦力补偿扭矩T 22的计算公式，具体如下所示：
T 12＝-47.16sinθ 2+27.418      (6)
其中，θ 2为所述第二关节20的实时角度。
T 22＝-0.1492v 2 2+3.3635v 2-9.7495        (7)
其中，v 2为所述第二关节20的实时速度。
需要说明的是，本发明是以某一型号的机器人为例进行试验得到的第二关节所受到的机械臂重力所施加的实时扭矩T 12以及所受到的实时摩擦力补偿扭矩T 22的计算公式的，如本领域技术人员所能理解的，针对不同型号的机器人，所述第二关节所受到的机械臂重力所施加的实时扭矩T 12以及所受到的实时摩擦力补偿扭矩T 22的计算公式中的各个系数可能是不同的，具体取值根据实际情况进行确定。
进一步地，患者的下肢/上肢对所述关节i的实时附加扭矩T 3i的计算公式如下所示：
T 3i＝F i×L Ri       (8)
其中，F i为患者的下肢/上肢对所述关节i的实时作用力，L Ri为F i的力臂。
由此，通过获取各个关节所受到的患者的下肢/上肢对其所施加的作用力以及作用力的力臂，即可获取各个关节所受到的实时附加扭矩T 3i。
举例而言，患者的下肢/上肢对所述第二关节20的实时附加扭矩T 32的计算公式如下所示：
T 32＝F 2×L R2        (9)
其中，F 2为患者的下肢/上肢对所述第二关节20的实时作用力，L R2为F 2的力臂。
请参考图5，其示意性地给出了本发明一实施方式提供的机器人末端对患者下肢/上肢产生的拉伸作用力的示意图。如图5所示，通过进行力学分析可知，患者的下肢/上肢对所述第二关节20的实时作用力F 2满足如下关系式：
Figure PCTCN2022084633-appb-000005
其中，当所述机器人为下肢康复训练机器人时，G 1为患者的大腿的重量，G 2为患者的小腿的重量，L 1为患者的大腿的长度，L 2为患者的小腿的长度，l 1为患者的大腿的重心与髋关节40之间的距离，l 2为患者的小腿的重心与膝关节50之间的距离，α为患者的髋关节40的实时角度，β为患者的膝关节50的实时角度。当所述机器人为上肢康复训练机器人时，G 1为患者的大臂的重量，G 2为患者的小臂的重量，L 1为患者的大臂的长度，L 2为患者的小臂的长度，l 1为患者的大臂的重心与肩关节40’之间的距离，l 2为患者的小臂的重心与肘关节50’之间的距离，α为患者的肩关节40’的实时角度，β为患者的肘关节50’的实时角度。
请继续参考图6，其示意性地给出了本发明一实施方式提供的患者的下肢/上肢对第二关节产生的扭矩的示意图。如图6所示，患者的下肢/上肢对所述第二关节20所产生的扭矩满足如下关系式：
T 32＝F 2×L R2＝F 2×L 2×sin(θ 2-θ 1)         (11)
其中，θ 2为所述第二关节20的实时角度，θ 1为所述第一关节10的实时角度。
请继续参考图1，如图1所示，所述机械臂还包括与所述第一关节10相连的基座关节80。所述基座关节80的实时理论扭矩也可通过上述的公式(1)进行计算，且所述基座关节80所受到的机械臂重力所施加的实时扭矩可通过上述的公式(2)求得，其所受到的实时摩擦力补偿扭矩可通过上述的公式(3)求得，其所受到的患者的下肢/上肢对其施加的附加扭矩可通过上述的公式(8)求得。
综上所述，本发明提供的状态监测方法通过实时计算各个关节在当前时刻的理论扭矩，并将各个关节在当前时刻的理论扭矩与其在当前时刻的实际扭矩进行比较，以判断各个关节在当前时刻是否出现扭矩故障，因此相比于现有技术中采用固定的阈值进行扭矩监测的方法，本发明提供的状态监测方法的监测灵敏度更高，实时性更好，能够及时、准确地判断出机器人是否出现扭矩故障，大大提高了机器人在使用过程中的安全性能。
基于同一发明构思，本发明还提供一种安全控制装置，应用于机器人，所述机器人包括至少一条机械臂，所述机械臂包括至少两个相连的关节。需要说明的是，如本领域技术人员所能理解的，所述机器人可以为上肢康复训练机器人、下肢康复训练机器人或除康复训练机器人以外的其它机器人，本发明对此并不进行限制。请参考图7，其示意性地给出了本发明一实施方式的安全控制装置的结构框图，如图7所示，所述安全控制装置包括监测模块100、判断模块200和执行模块300。
其中，所述监测模块100包括第一监测单元110，所述第一监测单元110用于采用上文所述的状态监测方法对各条所述机械臂的各个关节的扭矩进行实时监测；所述判断模块200用于根据接收到的所述监测模块100的监测结果，判断所述机器人是否出现故障，并进行故障等级划分；所述执行模块300用于根据接收到的所述判断模块200输出的故障等级，执行与所述故障等级对应的操作。
请参考表2，其示意性地给出了本发明一实施方式提供的故障等级与故障类型之间的对应关系。如表2所示：
表2 故障类型与故障等级之间的对应关系
Figure PCTCN2022084633-appb-000006
Figure PCTCN2022084633-appb-000007
由此，本发明提供的安全控制装置，通过对机器人的各个关节的扭矩进行实时监测，可以实时监测各个关节的扭矩差值(理论扭矩与实际扭矩之间的差值)是否超出其预设误差范围，从而可以在某个关节出现扭矩故障时(划分为中等级故障))，及时采取相应措施，例如，停机，排除外围异常，复位继续，如果不能排除，则切断所述机器人的电源，进行关机处理。可见，本发明提供的安全控制装置能够自动识别设备的运行状态，并能够针对不同的故障类型采取相应的分级响应措施，从而能够进一步提高机器人在使用过程中的安全性能。此外，由于所述第一监测单元110是采用上文所述的状态监测方法对机器人的各个关节的扭矩进行实时监测的，因此本发明提供的安全监控装置还具有上文所述的状态监测方法的所有优点，故对此不再进行赘述。
如图7所示，所述监测模块100还包括第二监测单元120和第三监测单元130；所述第二监测单元120用于对所述机器人的各个关节的角度进行实时监测；所述第三监测单元130用于对所述机器人的各个关节的速度进行实时监测。由此，通过对机器人的各个关节的角度进行实时监测，可以实时监测所述机器人的各个关节的运行角度有没有超出其各自的角度范围，从而可以在某个关节的角度超出其角度范围时(划分为高等级故障)，及时采取相应措施，例如停机，放下人腿，收拢设备，重新开始。通过对机器人的各个关节的速度进行实时监测，可以实时监测所述机器人的各个关节的运行速度有没有超出其各自的速度范围，从而可以在某个关节的运行速度超出其速度范围时(划分为高等级故障)，及时采取相应措施，例如，停机，放下人腿，收拢设备，重新开始。
进一步地，当所述机器人为下肢/上肢康复训练机器人时，如图7所示，所述监测模块100还包括第四监测单元140，所述第四监测单元140用于对患 者的下肢/上肢的肌肉状态进行实时监测。由此，通过对患者的下肢/上肢的肌肉状态进行实时监测，可以实时监测患者的下肢/上肢有没有出现痉挛，从而可以在患者的下肢/上肢出现痉挛时(划分为低等级故障)，及时采取相应措施，例如，停机，询问患者能否继续进行康复训练，若能够继续进行康复训练，则复位继续进行康复训练，以保护患者安全。具体地，可以在所述机器人的机械臂的各个关节上安装扭矩传感器，通过扭矩传感器测得的扭矩判断患者的下肢/上肢有没有出现痉挛，当扭矩传感器测得的扭矩突然增大时，说明患者的下肢/上肢出现了肌肉痉挛。
更进一步地，当所述机械臂包括依次相连的第一关节、第二关节和末端关节，其中，所述第一关节对应人体的髋关节/肩关节(在此表示髋关节或者肩关节，分别与下肢或者上肢相对应，其中髋关节对应下肢，肩关节对应上肢)，所述第二关节对应人体的膝关节/肘关节(在此表示膝关节或者肘关节，分别与下肢或者上肢相对应，其中膝关节对应下肢，肘关节对应上肢)，所述末端关节对应人体的踝关节/腕关节(在此表示踝关节或者腕关节，分别与下肢或者上肢相对应，其中踝关节对应下肢，腕关节对应上肢)时，如图7所示，所述监测模块100还包括第五监测单元150，所述第五监测单元150用于对患者的下肢/上肢的各个关节的角度进行实时监测。由此，通过对患者的下肢/上肢的各个关节的角度进行实时监测，可以实时监测患者的下肢/上肢的各个关节的角度(即角度)是否超出预设角度范围(该角度范围可为医生人为设定的，也可以为根据人体的下肢/上肢关节形体特性所得到的)，从而可以在患者的下肢/上肢的关节超出预设角度范围时(划分为高等级故障)，及时采取相应措施，例如，停机，放下人腿/手臂，收拢设备，重新开始。其中，在一些实施方式中，患者的上肢的各个关节的角度可以通过机械臂的各个关节的角度以及患者的上肢信息(例如大臂长、小臂长)计算得到，患者的下肢的各个关节的角度可以通过机械臂的各个关节的角度以及患者的下肢信息(例如大腿长、小腿长)计算得到。在另一些实施方式中，也可以在患者的下肢/上肢的各个关节上安装角度传感器，以获得患者的下肢/上肢的各个关节的角度。
如图7所示，在一种示范性的实施方式中，所述监测模块100还包括第六监测单元160，所述第六监测单元160用于对所述机器人的报警信息进行实时监测。由此，通过对所述机器人的报警信息，例如控制器报警、驱动器报警、电机报警、通讯报警等报警信息进行实时监测，可以实时监测有没有出现报警，从而可以在监测到报警信息时(划分为低等级故障)，及时采取相应措施，例如当监测到通讯报警时(即网络连接不通时)，先停机，再复位继续。
如图7所示，在一种示范性的实施方式中，所述监测模块100还包括第七监测单元170，所述第七监测单元170用于对所述机器人的笛卡尔坐标系下的虚拟空间进行实时监测。由此，通过对所述机器人的笛卡尔坐标系下的虚拟空间进行实时监测，可以实时监测所述机器人是否超出工作范围，从而可以在所述机器人超出其工作范围时(划分为高等级故障)，及时采取相应措施，例如停机，放下人腿，收拢设备，重新开始。需要说明的是，本文中所称的机器人的笛卡尔坐标系下的虚拟空间是指机器人的机械臂在笛卡尔坐标系下的三维位置。
如图7所示，在一种示范性的实施方式中，所述监测模块100还可包括第八监测单元180，所述第八监测单元180用于对所述机器人的安放状态进行实时监测。具体地，可以通过安装于所述机器人上的陀螺仪对所述机器人的安放状态进行监测，由此，通过对所述机器人的安放状态进行实时监测，可以实时监测所述机器人是否出现侧翻、倾斜、平移等现象，从而可以在所述机器人出现侧翻、倾斜或平移时(划分为最高等级故障)，及时采取相应措施，例如切断电源，进行关机处理。
进一步地，如图7所示，所述执行模块300包括停止单元310、报警单元320和电源控制单元330；所述停止单元310用于在所述机器人出现任意等级的故障时，执行停机操作；所述报警单元320用于在所述机器人出现任意等级的故障时，执行报警操作；所述电源控制单元330用于在所述机器人出现对应等级的故障时，执行关机操作。由此，此种设置，可以更好地确保所述执行模块300能够根据所述判断模块200输出的故障等级，及时、准确地执行相应的操作。
如图7所示，在一种示范性的实施方式中，所述安全控制装置还包括初始化模块400和通信模块500，其中，所述初始化模块400用于进行参数配置、信号初始化和/或自检；所述通信模块500用于进行数据的传输。由此，通过所述初始化模块400能够实现硬件参数配置、软件信号初始化以及自检功能；通过所述通信模块500能够传输各种数据，实现与上位机、安全信号灯、安全蜂鸣器之间的通信，此外，通过所述通信模块500还可以实现初始化模块400、监测模块100、判断模块200、执行模块300之间的相互通信。
请参考图8，其示意性地给出了本发明一实施方式提供的安全控制装置的工作流程示意图。如图8所示，针对下肢/上肢康复训练机器人，通过所述初始化模块400进行初始化后，即可对患者进行下肢/上肢的康复训练，在训练过程中，通过所述监测模块100可以实时监测各种数据，所述判断模块200根据所述监测模块100所监测到的数据，实时判断是否出现故障，若判断结果为出现故障，则输出对应的故障等级，所述执行模块300根据所接收到的故障等级，执行相应操作(例如报警、停机、关机)；对于可恢复类故障(通过复位操作可以消除的故障)，在给定的时间周期内会进行反复确认，若故障排除，则重新开始训练；若故障依旧，则智能性地进行干预，以消除故障，若仍无法消除故障，则结束训练流程，从而提高机器人在使用过程中的安全性能。
与上述的安全控制装置相对应，本发明还提供一种机器人，所述机器人包括上文所述的安全控制装置。由于本发明提供的机器人与上文所述的安全控制装置属于同一发明构思，因此其具有上文所述的安全控制装置的所有优点，故对其所具有的优点不再进行赘述。
与上述的状态监测方法相对应，本发明还提供了一种可读存储介质，所述可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时可以实现上文所述的状态监测方法。由于本发明提供的可读存储介质与上文所述的状态监测方法属于同一发明构思，因此其具有上文所述的状态监测方法的所有优点，故对其所具有的优点不再进行赘述。
本发明实施方式的可读存储介质，可以采用一个或多个计算机可读的介 质的任意组合。可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是但不限于电、磁、光、电磁、红外线或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机硬盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其组合使用。
计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。
可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言-诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言-诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)连接到用户计算机，或者可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
综上所述，与现有技术相比，本发明提供的状态监测方法、安全控制装置、存储介质和机器人具有以下优点：
(1)本发明提供的状态监测方法通过根据预设的扭矩计算模型，分别计算机器人的各条机械臂的各个关节的实时理论扭矩，再根据各个关节的实时 理论扭矩以及实时实际扭矩，分别计算各个关节的实时扭矩差值，最后将各个关节的实时扭矩差值和与其对应的预设误差范围进行比较，以判断各个关节在当前时刻是否出现扭矩安全故障。由此可见，本发明提供的状态监测方法通过实时计算各个关节在当前时刻的理论扭矩，并将各个关节在当前时刻的理论扭矩与其在当前时刻的实际扭矩进行比较，以判断各个关节在当前时刻是否出现扭矩故障，因此相比于现有技术中采用固定的阈值进行扭矩监测的方法，本发明提供的状态监测方法的监测灵敏度更高，实时性更好，能够及时、准确地判断出机器人是否出现扭矩故障，大大提高了机器人在使用过程中的安全性能。
(2)本发明提供的安全控制装置通过第一监测单元对机器人的各条机械臂的各个关节的扭矩进行实时监测，通过判断模块判断机器人是否出现故障，并对所出现的故障进行等级划分，通过执行模块根据机器人出现的故障等级，执行对应的操作。由此可见，本发明提供的安全控制装置能够自动识别设备的运行状态，并能够针对不同的故障类型采取相应的分级响应措施，从而能够进一步提高机器人在使用过程中的安全性能。此外，由于所述第一监测单元是采用上文所述的状态监测方法对机器人的各个关节的扭矩进行实时监测的，因此本发明提供的安全监控装置还具有上文所述的状态监测方法的所有优点，故对此不再进行赘述。
(3)由于本发明提供的可读存储介质与上文所述的状态监测方法属于同一发明构思，因此其具有上文所述的状态监测方法的所有优点，故对其所具有的优点不再进行赘述。由于本发明提供的机器人与上文所述的安全控制装置属于同一发明构思，因此其具有上文所述的机器人的所有优点，故对其所具有的优点不再进行赘述。
应当注意的是，在本文的实施方式中所揭露的装置和方法，也可以通过其他的方式实现。以上所描述的装置实施方式仅仅是示意性的，例如，附图中的流程图和框图显示了根据本文的多个实施方式的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序或代码的一部分，所述模块、程序段或代 码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用于执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本文各个实施方式中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。
上述描述仅是对本发明较佳实施方式的描述，并非对本发明范围的任何限定，本发明领域的普通技术人员根据上述揭示内容做的任何变更、修饰，均属于本发明的保护范围。显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若这些修改和变型属于本发明及其等同技术的范围之内，则本发明也意图包括这些改动和变型在内。

Claims (17)

  1. 一种状态监测方法，应用于机器人，其特征在于，所述机器人包括至少一条机械臂，所述机械臂包括至少两个相连的关节，所述状态监测方法包括：
    根据预设的扭矩计算模型，分别计算各条所述机械臂的各个关节的实时理论扭矩；
    根据各个所述关节的实时理论扭矩以及各个所述关节的实时实际扭矩，分别计算各个所述关节的实时扭矩差值；
    分别判断各个所述关节的实时扭矩差值是否超出对应的预设误差范围；
    若所述关节的实时扭矩差值超出对应的预设误差范围，则判定所述关节出现扭矩故障。
  2. 根据权利要求1所述的状态监测方法，其特征在于，所述机器人为下肢/上肢康复训练机器人，所述根据预设的扭矩计算模型，分别计算各条所述机械臂的各个关节的实时理论扭矩，包括：
    按照如下扭矩计算模型，分别计算各条所述机械臂的各个关节的实时理论扭矩：
    T i＝T 1i+T 2i+T 3i
    其中，T i为关节i的实时理论扭矩，T 1i为所述关节i所受到的机械臂重力所施加的实时扭矩，T 2i为所述关节i所受到的实时摩擦力补偿扭矩，T 3i为所述关节i所受到的患者的下肢/上肢所施加的实时附加扭矩。
  3. 根据权利要求2所述的状态监测方法，其特征在于，所述关节i所受到的机械臂重力所施加的实时扭矩T 1i的计算公式如下所示：
    T Ii＝a isinθ i+b i
    其中，a i为与所述关节i相关的一次项系数，θ i为所述关节i的实时角度，b i为与所述关节i相关的常数项系数。
  4. 根据权利要求2所述的状态监测方法，其特征在于，所述关节i所受到的实时摩擦力补偿扭矩T 2i的计算公式如下所示：
    T 2i＝c iv i 2+d iv i+e i
    其中，c i为与所述关节i相关的二次项系数，v i为所述关节i的实时速度，d i为与所述关节i相关的一次项系数，e i为与所述关节i相关的常数项系数。
  5. 根据权利要求2所述的状态监测方法，其特征在于，所述关节i所受到的患者的下肢/上肢所施加的实时附加扭矩T 3i的计算公式如下所示：
    T 3i＝F i×L Ri
    其中，F i为患者的下肢/上肢对所述关节i的实时作用力，L Ri为F i的力臂。
  6. 根据权利要求5所述的状态监测方法，其特征在于，所述机械臂包括依次相连的第一关节、第二关节和末端关节，其中，所述第一关节对应人体的髋关节/肩关节，所述第二关节对应人体的膝关节/肘关节，所述末端关节对应人体的踝关节/腕关节，所述第二关节所受到的患者的下肢/上肢所施加的实时附加扭矩T 32的计算公式如下所示：
    T 32＝F 2×L R2
    Figure PCTCN2022084633-appb-100001
    L R2＝L 2×sin(θ 2-θ 1)
    其中，G 1为患者的大腿/大臂的重量，G 2为患者的小腿/小臂的重量，L 1为患者的大腿/大臂的长度，L 2为患者的小腿/小臂的长度，l 1为患者的大腿/大臂的重心与髋关节/肩关节之间的距离，l 2为患者的小腿/小臂的重心与膝关节/肘关节之间的距离，α为患者的髋关节/肩关节的实时角度，β为患者的膝关节/肘关节的实时角度，θ 2为所述第二关节的实时角度，θ 1为所述第一关节的实时角度。
  7. 一种安全控制装置，应用于机器人，其特征在于，所述机器人包括至少一条机械臂，所述机械臂包括至少两个相连的关节，所述安全控制装置包括监测模块、判断模块和执行模块；
    所述监测模块包括第一监控单元，所述第一监控单元用于采用权利要求1至6中任一项所述的状态监测方法对各条所述机械臂的各个关节的扭矩进行实时监测；
    所述判断模块根据接收到的所述监测模块的监测结果，判断所述机器人是否出现故障，并进行故障等级划分；
    所述执行模块根据接收到的所述判断模块输出的故障等级，执行与所述故障等级对应的操作。
  8. 根据权利要求7所述的安全控制装置，其特征在于，所述监测模块还包括第二监测单元和第三监测单元，所述第二监测单元用于对各条所述机械臂的各个关节的角度进行实时监测，所述第三监测单元用于对各条所述机械臂的各个关节的速度进行实时监测。
  9. 根据权利要求7所述的安全控制装置，其特征在于，所述机器人为下肢/上肢康复训练机器人，所述监测模块还包括第四监测单元，所述第四监测单元用于对患者的下肢/上肢的肌肉状态进行实时监测。
  10. 根据权利要求7所述的安全控制装置，其特征在于，所述机器人为下肢/上肢康复训练机器人，所述机械臂包括依次相连的第一关节、第二关节和末端关节，其中，所述第一关节对应人体的髋关节/肩关节，所述第二关节对应人体的膝关节/肘关节，所述末端关节对应人体的踝关节/腕关节，所述监测模块还包括第五监测单元，所述第五监测单元用于对患者的下肢/上肢的各个关节的角度进行实时监测。
  11. 根据权利要求7所述的安全控制装置，其特征在于，所述监测模块还包括第六监测单元，所述第六监测单元用于对所述机器人的报警信息进行实时监测。
  12. 根据权利要求7所述的安全控制装置，其特征在于，所述监测模块还包括第七监测单元，所述第七监测单元用于对所述机器人的笛卡尔坐标系下的虚拟空间进行实时监测。
  13. 根据权利要求7所述的安全控制装置，其特征在于，所述监测模块还包括第八监测单元，所述第八监测单元用于对所述机器人的安放状态进行实时监测。
  14. 根据权利要求7所述的安全控制装置，其特征在于，所述执行模块包括停止单元、报警单元和电源控制单元；
    所述停止单元用于在所述机器人出现任意等级的故障时，执行停机操作；
    所述报警单元用于在所述机器人出现任意等级的故障时，执行报警操作；
    所述电源控制单元用于在所述机器人出现对应等级的故障时，执行关机操作。
  15. 根据权利要求7所述的安全控制装置，其特征在于，所述安全控制装置还包括通信模块，所述通信模块用于进行数据的传输。
  16. 一种机器人，其特征在于，包括权利要求7至15中任一项所述的安全控制装置。
  17. 一种可读存储介质，其特征在于，所述可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时，实现权利要求1至6中任一项所述的状态监测方法。
PCT/CN2022/084633 2021-04-26 2022-03-31 状态监测方法、安全控制装置、存储介质和机器人 WO2022228031A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202110455082.9A CN115245439A (zh) 2021-04-26 2021-04-26 状态监测方法、安全控制装置、存储介质和机器人
CN202110455082.9 2021-04-26

Publications (1)

Publication Number Publication Date
WO2022228031A1 true WO2022228031A1 (zh) 2022-11-03

Family

ID=83696903

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/084633 WO2022228031A1 (zh) 2021-04-26 2022-03-31 状态监测方法、安全控制装置、存储介质和机器人

Country Status (2)

Country Link
CN (1) CN115245439A (zh)
WO (1) WO2022228031A1 (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2017030238A (ja) * 2015-07-31 2017-02-09 東芝機械株式会社 金型保護方法および装置、型締装置
CN106826819A (zh) * 2017-01-15 2017-06-13 上海新时达电气股份有限公司 桁架机器人防碰撞检测方法及装置
CN109093623A (zh) * 2018-08-24 2018-12-28 北京秘塔网络科技有限公司 一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法
CN110652423A (zh) * 2019-10-12 2020-01-07 东南大学 带有精确力控制的可穿戴式上肢康复训练机器人
CN111419644A (zh) * 2020-06-09 2020-07-17 上海神泰医疗科技有限公司 康复机器人的操作方法、康复机器人及可读存储介质
CN112455425A (zh) * 2020-12-10 2021-03-09 上海馨联动力系统有限公司 一种混合动力汽车发动机扭矩故障控制方法
CN113043325A (zh) * 2019-12-27 2021-06-29 沈阳新松机器人自动化股份有限公司 一种机器人关节运动状态的检测方法及装置

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2017030238A (ja) * 2015-07-31 2017-02-09 東芝機械株式会社 金型保護方法および装置、型締装置
CN106826819A (zh) * 2017-01-15 2017-06-13 上海新时达电气股份有限公司 桁架机器人防碰撞检测方法及装置
CN109093623A (zh) * 2018-08-24 2018-12-28 北京秘塔网络科技有限公司 一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法
CN110652423A (zh) * 2019-10-12 2020-01-07 东南大学 带有精确力控制的可穿戴式上肢康复训练机器人
CN113043325A (zh) * 2019-12-27 2021-06-29 沈阳新松机器人自动化股份有限公司 一种机器人关节运动状态的检测方法及装置
CN111419644A (zh) * 2020-06-09 2020-07-17 上海神泰医疗科技有限公司 康复机器人的操作方法、康复机器人及可读存储介质
CN112455425A (zh) * 2020-12-10 2021-03-09 上海馨联动力系统有限公司 一种混合动力汽车发动机扭矩故障控制方法

Also Published As

Publication number Publication date
CN115245439A (zh) 2022-10-28

Similar Documents

Publication Publication Date Title
Serrancolí et al. Subject-exoskeleton contact model calibration leads to accurate interaction force predictions
US20220068458A1 (en) Artificial intelligence-based algorithm for physiotherapy and rehabilitation robots for diagnosis and treatment purposes
Rocon et al. Rehabilitation robotics: a wearable exo-skeleton for tremor assessment and suppression
Gonzalez et al. Development and evaluation of a musculoskeletal model of the elbow joint complex
Wu et al. Development and analysis of a gravity-balanced exoskeleton for active rehabilitation training of upper limb
WO2022247172A1 (zh) 按摩运动控制方法、装置、机器人控制设备及存储介质
CN111390877B (zh) 一种外骨骼装置及外骨骼变导纳控制方法
Torricelli et al. A subject-specific kinematic model to predict human motion in exoskeleton-assisted gait
Moshaii et al. Fuzzy sliding mode control of a wearable rehabilitation robot for wrist and finger
Nasr et al. Optimal design of active-passive shoulder exoskeletons: A computational modeling of human-robot interaction
WO2022228031A1 (zh) 状态监测方法、安全控制装置、存储介质和机器人
CN113558609A (zh) 基于坐卧式下肢康复设备的训练数据处理方法和相关设备
Du et al. Human-robot interaction control of a haptic master manipulator used in laparoscopic minimally invasive surgical robot system
KR20180047499A (ko) 보행 보조 장치 및 그 동작 방법
Stoltze et al. Development and functional testing of an unloading concept for knee osteoarthritis patients: A pilot study
TWI587843B (zh) Detection of lower extremity spasticity
Zhou et al. Lower limb exoskeleton parasitic force modeling and minimizing with an adaptive trajectory controller
Crevecoeur et al. Adaptive control of grip force to compensate for static and dynamic torques during object manipulation
US20170340286A1 (en) Lower limb spasticity measurement method
Singhala et al. Prefatory study of the effects of exploration dynamics on stiffness perception
JP2023548953A (ja) センサ出力データを組み合わせて外骨格の安全でない動作を防止する
Kai et al. Development of a new compact and light velocity-based mechanical safety device for a rehabilitation assist suit
Jackson et al. Development of the iPAM MkII system and description of a randomized control trial with acute stroke patients
Amigo et al. Study of patient-orthosis interaction forces in rehabilitation therapies
Song et al. Development of a new compliant exoskeleton device for elbow joint rehabilitation

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22794503

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 22794503

Country of ref document: EP

Kind code of ref document: A1