CN111604897B - Anti-collision safety protection method for moxibustion mechanical arm - Google Patents

Anti-collision safety protection method for moxibustion mechanical arm Download PDF

Info

Publication number
CN111604897B
CN111604897B CN202010294900.7A CN202010294900A CN111604897B CN 111604897 B CN111604897 B CN 111604897B CN 202010294900 A CN202010294900 A CN 202010294900A CN 111604897 B CN111604897 B CN 111604897B
Authority
CN
China
Prior art keywords
mechanical arm
radius
patient
moxibustion
moxibustion mechanical
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
CN202010294900.7A
Other languages
Chinese (zh)
Other versions
CN111604897A (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.)
Shanghai Juncon Robot Co ltd
Original Assignee
Shanghai Juncon Robot Co ltd
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 Shanghai Juncon Robot Co ltd filed Critical Shanghai Juncon Robot Co ltd
Priority to CN202010294900.7A priority Critical patent/CN111604897B/en
Publication of CN111604897A publication Critical patent/CN111604897A/en
Application granted granted Critical
Publication of CN111604897B publication Critical patent/CN111604897B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones
    • 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
    • A61H39/00—Devices for locating or stimulating specific reflex points of the body for physical therapy, e.g. acupuncture
    • A61H39/06—Devices for heating or cooling such points within cell-life limits
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • 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/02—Characteristics of apparatus not provided for in the preceding codes heated or cooled
    • A61H2201/0207—Characteristics of apparatus not provided for in the preceding codes heated or cooled heated
    • 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
    • A61H2205/00—Devices for specific parts of the body

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Health & Medical Sciences (AREA)
  • Rehabilitation Therapy (AREA)
  • Epidemiology (AREA)
  • Pain & Pain Management (AREA)
  • Physical Education & Sports Medicine (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Automation & Control Theory (AREA)
  • Measurement Of The Respiration, Hearing Ability, Form, And Blood Characteristics Of Living Organisms (AREA)

Abstract

The invention discloses a collision-prevention safety protection method for a moxibustion mechanical arm, which comprises the following steps: a moxibustion mechanical arm safety protection system is built, and human body data of a patient and moxibustion mechanical arm state information are obtained; constructing a variable-radius human body and moxibustion mechanical arm ball sweeping convex body collision detection model; respectively establishing the relationship between human body data and moxibustion mechanical arm state information and the radius of a human body and moxibustion mechanical arm ball swept convex body collision detection model; and constructing a collision detection pair, and setting and adjusting a safety threshold value of the collision detection of the moxibustion mechanical arm and the patient in real time. According to the moxibustion mechanical arm collision avoidance safety protection method, the variable-radius ball sweep convex body collision detection model is introduced, the relation among the emotion, the motion state of a patient, the state of the moxibustion mechanical arm and the radius of the collision detection model is established, the real-time adjustment of the collision detection safety threshold value of the moxibustion mechanical arm and a human is realized, and therefore the human-computer interaction safety of the moxibustion mechanical arm is enhanced.

Description

Anti-collision safety protection method for moxibustion mechanical arm
Technical Field
The invention belongs to the technical field of a safety protection method and collision detection when a service mechanical arm interacts with a person, and particularly relates to a collision avoidance safety protection method for a moxibustion mechanical arm.
Background
A traditional mechanical arm man-machine interaction safety protection strategy usually adopts a collision detection technology to construct a collision detection model of a mechanical arm and a human body, and the commonly used collision detection model comprises a convex body, an axial bounding box (AAB), a ball bounding box (Sphere), a directional bounding box (OBB) and the like. The shortest distance between the two models in the space is calculated in real time, so that the corresponding protection strategy of the mechanical arm is ensured when the shortest distance is lower than a fixed safety threshold. The practical effect of this strategy is not ideal, and the main problem is that it usually adopts a fixed safety threshold, and when the patient's mood fluctuates or is in a moving state, the mechanical arm needs a certain braking distance, so the safety threshold needs to be changed in real time according to different situations. The invention introduces a variable-radius ball-swept convex body collision detection technology, constructs the relation between the emotion, the motion state of a patient, the state of the mechanical arm and the radius of a collision detection model, and realizes the real-time adjustment of the collision detection safety threshold value of the mechanical arm and a human, thereby enhancing the human-computer interaction safety of the moxibustion mechanical arm.
Disclosure of Invention
The invention aims to provide a collision-prevention safety protection method for a moxibustion mechanical arm, which enhances the human-computer interaction safety of the moxibustion mechanical arm.
The technical scheme adopted by the invention is as follows: a collision avoidance safety protection method for a moxibustion mechanical arm comprises the following steps:
step 1, a moxibustion mechanical arm safety protection system is built, and human body data of a patient and state information of the moxibustion mechanical arm are obtained;
step 2, constructing a variable-radius human body and moxibustion mechanical arm ball swept convex body collision detection model;
step 3, establishing a relation between human body data and the radius of a human body sphere swept convex body collision detection model;
step 4, establishing a relation between the state information of the moxibustion mechanical arm and the radius of a moxibustion mechanical arm ball sweeping convex body collision detection model;
and 5, constructing a collision detection pair, and setting and adjusting a safety threshold value of the collision detection of the moxibustion mechanical arm and the patient in real time.
The present invention is also characterized in that,
the moxibustion mechanical arm safety protection system built in the step 1 comprises a moxibustion mechanical arm, wherein a moxa stick is arranged at one end, close to a patient, of the moxibustion mechanical arm, a somatosensory camera with a view field covering the patient is arranged on one side of the moxibustion mechanical arm, the moxibustion mechanical arm safety protection system also comprises a heart rate measuring bracelet worn on the arm of the patient, and the heart rate measuring bracelet, the somatosensory camera and the moxibustion mechanical arm are jointly connected with a mechanical arm control cabinet for real-time communication; the patient body data acquired in the step 1 comprise patient expressions acquired through the motion sensing camera, patient heart rates acquired through the heart rate measuring bracelet, and patient body skeleton models and key point information acquired through the motion sensing camera; the moxibustion mechanical arm state information acquired in the step 1 comprises control modes and movement speed information of the moxibustion mechanical arm acquired through a joint position sensor of the moxibustion mechanical arm.
The variable-radius human body and moxibustion mechanical arm ball swept convex body collision detection model constructed in the step 2 comprises the steps of constructing a variable-radius human body ball swept convex body collision detection model to obtain the radius of the human body ball swept convex body collision detection model
Figure BDA0002451821930000021
Wherein i is the member number of the human body ball sweeping convex body collision detection model with variable radius, i =1,2,3, …,10; the method also comprises the step of constructing a variable-radius moxibustion mechanical arm ball sweeping convex body collision detection model to obtain the radius of the moxibustion mechanical arm ball sweeping convex body collision detection model
Figure BDA0002451821930000022
Wherein u is the member number of the moxibustion mechanical arm ball sweeping convex body collision detection model with variable radius, and u =1,2,3,4,5.
The step 3 specifically comprises the following steps:
step 3.1: representing the emotion of the patient through the expression and the heart rate of the patient, dividing the acquired expression of the patient into positive expression and negative expression, correspondingly obtaining the emotion of the patient as positive emotion and negative emotion in sequence, and respectively establishing the radius of a collision detection model of the heart rate and the human body ball swept convex body under the positive emotion of the patient
Figure BDA0002451821930000031
In relation to (2)
Figure BDA0002451821930000032
Hr, heart rate and human body sphere swept convex body collision detection model radius under negative emotion of patient
Figure BDA0002451821930000033
In relation to (2)
Figure BDA0002451821930000034
—Hr：
Relationship when the patient is in positive mood
Figure BDA0002451821930000035
-Hr is:
Figure BDA0002451821930000036
in the formula (1), hr is the real-time heart rate of the patient; n is a radical of an alkyl radical 2 Lower limit of heart rate for patient with positive mood; n is 3 The upper limit of the heart rate of the patient when Hr > n 3 The treatment is suspended; a. b is a constant coefficient;
Figure BDA0002451821930000037
sweeping the convex body collision detection model radius for the human body ball under positive emotion;
relationships when patients are in negative mood
Figure BDA0002451821930000038
-Hr is:
Figure BDA0002451821930000039
in the formula (2), hr is the real-time heart rate of the patient; n is 1 Is the patient's standard heart rate; n is a radical of an alkyl radical 3 The upper limit of the heart rate of the patient is defined when Hr is more than n 3 The treatment is suspended; c. d are both Chang Jishu,
Figure BDA00024518219300000310
sweeping the convex body collision detection model radius for the human body ball under negative emotion;
in the formula (1) and the formula (2), n 1 、n 2 、n 3 And Hr are in units of times/min and n 3 ＞n 2 ＞n 1 ；
Step 3.2: calculating the human body speed of the patient through the human body skeleton model of the patient and the key point information, and establishing the radius of the human body speed and human body ball swept convex body collision detection model
Figure BDA00024518219300000311
In relation to (2)
Figure BDA00024518219300000312
—V h The following were used:
Figure BDA0002451821930000041
in the formula (3), the first and second groups of the compound,
Figure BDA0002451821930000042
the radius of a model for detecting bump collision of a human body ball during movement is swept, alpha is the maximum acceleration of the moxibustion mechanical arm during deceleration, and V h Is the instantaneous speed of the patient, obtained by the following equation (4):
Figure BDA0002451821930000043
in formula (4), T is the sampling period of the built-in sensor of the somatosensory camera, and the position information acquired to the initial point is a i (x 1 ,y 1 ,z 1 ) After one sampling period, the position information of the collected end point is B i (x 2 ,y 2 ,z 2 ) Then the distance moved by the time T from the initial point to the end point
Figure BDA0002451821930000044
Figure BDA0002451821930000045
Step 3.3: radius of model for detecting collision between human body data established in step 3.1 and step 3.2 and human body ball swept convex body
Figure BDA0002451821930000046
The relationship is as follows:
when the patient is in a positive mood,
Figure BDA0002451821930000047
when the patient is in a negative mood,
Figure BDA0002451821930000048
the step 4 specifically comprises the following steps:
step 4.1: establishment of moxibustion mechanical arm control mode and moxibustion mechanical arm ball sweeping convex bodyRadius of collision detection model
Figure BDA0002451821930000049
The control mode of the moxibustion mechanical arm is divided into position/speed control and compliance control according to the motion control mode:
when the moxibustion mechanical arm is in a flexible control mode, the radius of the model is detected by the collision of the swept convex body of the moxibustion mechanical arm ball
Figure BDA00024518219300000410
Wherein
Figure BDA00024518219300000411
Detecting the radius of a model for the collision of a scanning convex body of a moxibustion mechanical arm ball in a flexible control mode;
when the moxibustion mechanical arm is in a position/speed control mode, the radius of the model is detected by the collision of the scanning convex body of the moxibustion mechanical arm ball
Figure BDA0002451821930000051
Wherein
Figure BDA0002451821930000052
Detecting the radius of a model for detecting the collision of a convex body swept by a moxibustion mechanical arm ball in a position/speed control mode, wherein f is a constant;
step 4.2: establishing the collision detection model radius of the moxibustion mechanical arm movement speed and the moxibustion mechanical arm ball sweeping convex body
Figure BDA0002451821930000053
The relationship between them is as follows:
Figure BDA0002451821930000054
in the formula (5), the first and second groups,
Figure BDA0002451821930000055
the radius of a model for detecting the collision of a scanning convex body of a moxibustion mechanical arm ball during movement is alpha which is the most important radius when the moxibustion mechanical arm is deceleratedLarge acceleration, V m The real-time movement speed information of the moxibustion mechanical arm is obtained;
step 4.3: the moxibustion mechanical arm state information and moxibustion mechanical arm ball sweeping convex body collision detection model radius established in the step 4.1 and the step 4.2
Figure BDA0002451821930000056
The relationship is as follows:
when the moxibustion mechanical arm is in a flexible control mode,
Figure BDA0002451821930000057
when the moxibustion mechanical arm is in a position/speed control mode,
Figure BDA0002451821930000058
the step 5 specifically comprises the following steps: constructing a collision detection pair, and setting a safety threshold value for detecting the collision of the moxibustion mechanical arm and a patient as
Figure BDA0002451821930000059
When the distance between the human body and the moxibustion mechanical arm basic collision detection model is smaller than a safety threshold value, the system judges the collision in advance, and the mechanical arm control cabinet controls the moxibustion mechanical arm to stop running or switch to a flexible control mode.
The invention has the beneficial effects that: according to the moxibustion mechanical arm collision avoidance safety protection method, the variable-radius ball sweep convex body collision detection model is introduced, the relation among the emotion, the motion state of a patient, the state of the moxibustion mechanical arm and the radius of the collision detection model is established, the real-time adjustment of the collision detection safety threshold value of the moxibustion mechanical arm and a human is realized, and therefore the human-computer interaction safety of the moxibustion mechanical arm is enhanced.
Drawings
FIG. 1 is a flow chart of a moxibustion mechanical arm collision avoidance safety protection method of the invention;
fig. 2 is a schematic diagram of a moxibustion mechanical arm safety protection system built in the moxibustion mechanical arm collision avoidance safety protection method;
FIG. 3 is a schematic diagram of a human body ball scanning convex body collision detection model with variable radius, which is constructed in the moxibustion mechanical arm collision avoidance safety protection method;
FIG. 4 is a schematic diagram of a variable-radius moxibustion mechanical arm ball sweeping convex body collision detection model constructed in the moxibustion mechanical arm collision avoidance safety protection method;
FIG. 5 a) shows the front emotion of a patient in the moxibustion mechanical arm collision avoidance safety protection method
Figure BDA0002451821930000061
-Hr function map;
FIG. 5 b) is a diagram of the negative emotion of a patient in the moxibustion mechanical arm collision avoidance safety protection method
Figure BDA0002451821930000062
-graph of Hr function;
FIG. 6 is a model for detecting the collision of the human body velocity and the human body ball swept convex body with variable radius in the moxibustion mechanical arm collision avoidance safety protection method
Figure BDA0002451821930000063
—V h A function graph.
In the figure, 1 a motion sensing camera, 2 a moxibustion mechanical arm, 3 a distance measuring sensor, 4 a moxa stick, 5a mechanical arm control cabinet, 6 a heart rate measuring bracelet, 7 a moxibustion mechanical arm base, 8 a first joint, 9 a second joint, 10 a third joint and 11 a fourth joint.
Detailed Description
The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.
The invention provides a moxibustion mechanical arm collision avoidance safety protection method, which comprises the following steps of:
step one, information is obtained according to a safety protection system;
step two, constructing a variable-radius human body and moxibustion mechanical arm ball sweeping convex body collision detection model;
step three, people are identifiedVolume data and human body sphere sweep convex body radius
Figure BDA0002451821930000064
Correlating, wherein i is the member number of the human body collision detection model, i =1,2,3, …,10;
step four, sweeping convex body radius of the moxibustion mechanical arm state and the moxibustion mechanical arm ball
Figure BDA0002451821930000071
Associating, wherein u is a component number of the moxibustion mechanical arm collision detection model, and u =1,2,3,4,5;
and step five, adjusting a safety threshold value of collision detection of the moxibustion mechanical arm and a person in real time.
The safety protection system in the first step is shown in fig. 2 and comprises a moxibustion mechanical arm 2, a moxa stick 4, a mechanical arm control cabinet 5, a somatosensory camera 1, a distance measuring sensor 3 and a heart rate measuring bracelet 6, wherein the moxa stick 4 is fixed at the tail end of the moxibustion mechanical arm 2, and the somatosensory camera 1 is arranged at a position where a visual angle can cover the whole bed surface; the distance measuring sensor 3 is fixed at the tail end of the moxibustion mechanical arm 2, the heart rate measuring bracelet 6 is arranged on the wrist of a human body, the motion sensing camera 1, the distance measuring sensor 3 and the heart rate measuring bracelet 6 send monitoring signals to the mechanical arm control cabinet 5, and the mechanical arm control cabinet 5 gives instructions to the moxibustion mechanical arm 2; the acquiring of the information includes: acquiring the expression, the human skeleton model and key point information of a patient through the somatosensory camera 1; acquiring human body thickness information through a distance measuring sensor 3; acquiring the real-time heart rate of the patient through the heart rate measuring bracelet 6; the current pose and speed information of the moxibustion mechanical arm 2 is obtained through a joint position sensor of the moxibustion mechanical arm 2.
And step two, respectively establishing a human body ball sweeping convex body collision detection model with variable radius and a moxibustion mechanical arm ball sweeping convex body collision detection model with variable radius on the basis of the existing basic collision detection model, wherein the radius is related to the emotion, the body state and the mechanical arm state of a patient. The existing basic collision detection model comprises a convex body, an Axial Bounding Box (AABB), a ball bounding box (Sphere), a directional bounding box (OBB) and the like, and a human body three-dimensional model is established by the obtained human body skeleton model and the human body thickness information.
Figure 3 is a method for constructing a human swept convex body collision detection model with variable radius,
Figure BDA0002451821930000072
is the radius of the human head;
Figure BDA0002451821930000073
is the radius of the left upper arm;
Figure BDA0002451821930000074
is the radius of the left lower arm;
Figure BDA0002451821930000075
is the radius of the right upper arm;
Figure BDA0002451821930000076
is the right lower arm radius;
Figure BDA0002451821930000077
is the radius of the torso;
Figure BDA0002451821930000078
is the left thigh radius;
Figure BDA0002451821930000079
is the right thigh radius;
Figure BDA00024518219300000710
is the radius of the left calf;
Figure BDA00024518219300000711
the right calf radius.
As shown in fig. 4, a variable-radius moxibustion mechanical arm ball sweeping convex body collision detection model is constructed:
Figure BDA0002451821930000081
the radius of a moxibustion mechanical arm base 7;
Figure BDA0002451821930000082
is the first joint 8 radius;
Figure BDA0002451821930000083
is the second joint 9 radius;
Figure BDA0002451821930000084
is the third joint 10 radius;
Figure BDA0002451821930000085
the fourth joint 11 radius.
Establishing human body data and human body sphere sweep convex body radius in step three
Figure BDA0002451821930000086
(i is the number of the members of the human body collision detection model, i =1,2,3, …, 10) the specific process of the relationship is:
(1) Establishing human emotion and human sphere swept convex radius
Figure BDA0002451821930000087
The relationship of (1):
the somatosensory camera 1 identifies the expressions of the human body based on a machine learning method and classifies the expressions into positive expressions or negative expressions, and the heart rate measuring bracelet 6 acquires the real-time heart rate of the human body. Representing human emotion by expression and heart rate, establishing human emotion and human sphere sweep convex body radius
Figure BDA0002451821930000088
The relationship (c) in (c).
The patient's normal standard heart rate was 70 times/min, we used 90 times/min as the standard when the patient was positive, and treatment was suspended when the heart rate exceeded 130 times/min; as shown in FIG. 5, when the positive expression and the negative expression are divided into two cases to establish the positive expression of the human body
Figure BDA0002451821930000089
-Hr function fig. 5 a); when the human body has negative expression
Figure BDA00024518219300000810
FIG. 5 b) is the Hr function.
When the facial expression of the human body is a positive expression, the facial expression is the same
Figure BDA00024518219300000811
When Hr is larger than 130, the treatment is suspended, and the above formulas a and b are constant coefficients;
when the facial expression of the human body is negative, the facial expression is negative
Figure BDA00024518219300000812
When Hr is larger than 130, suspending the treatment, wherein the above formulas c and d are constant coefficients;
(2) Establishing human body velocity and human body spherical sweep convex body radius
Figure BDA00024518219300000813
The relationship of (1):
the somatosensory camera 1 scans key points of a human skeleton model; t is the sampling period of a sensor arranged in the somatosensory camera 1, and the position information acquired to the initial point is A i (x 1 ,y 1 ,z 1 ) After one sampling period, the position information of the collected end point is B i (x 2 ,y 2 ,z 2 ) Then the distance that the point moves within T time
Figure BDA0002451821930000091
The instantaneous speed at this point is
Figure BDA0002451821930000092
Get the formula
Figure BDA0002451821930000093
As shown in fig. 6Show, build
Figure BDA0002451821930000094
—V h A function graph.
Wherein the content of the first and second substances,
Figure BDA0002451821930000095
the radius of a human body ball sweeping convex body during motion is represented, and a is the maximum acceleration of the mechanical arm during deceleration;
(3) The convex body radius of the human body feedback and the human body spherical sweep is established by combining the two factors
Figure BDA0002451821930000096
The specific method of the relationship (2) is as follows:
when the facial expression of the human body is a positive expression, the human body is expressed in the positive expression
Figure BDA0002451821930000097
When the facial expression of the human body is negative, the facial expression is negative
Figure BDA0002451821930000098
Step four, establishing the state of the moxibustion mechanical arm 2 and the radius of a convex body swept by the moxibustion mechanical arm ball
Figure BDA0002451821930000099
(u is the member number of the moxibustion mechanical arm collision detection model, and u =1,2,3,4,5) comprises the following specific processes:
(1) Establishing the control mode of the moxibustion mechanical arm 2 and the convex body radius swept by the moxibustion mechanical arm ball
Figure BDA00024518219300000910
The relationship between: the control mode of the moxibustion mechanical arm 2 can be divided into position/speed control and compliance control according to the motion control mode;
the mechanical arm is in a flexible control mode, can control the contact force between the mechanical arm and the external environment, and can well protect the mechanical arm and the external environment when the mechanical arm runs under impedance control and force controlThe environment is not greatly damaged. When the mechanical arm is in a compliant control mode, the convex body radius is swept by the ball of the mechanical arm
Figure BDA0002451821930000101
When the mechanical arm is in a position/speed control mode, the position error of the mechanical arm and the uncertainty of a person in motion can cause the two to collide with each other to cause excessive contact force, and as a result, the mechanical arm and the person can be damaged, so that in the control mode, the mechanical arm ball sweeps the radius of a convex body
Figure BDA0002451821930000102
Wherein f is a constant;
(2) Establishing the motion speed of the mechanical arm and the radius of the spherical swept convex body of the mechanical arm
Figure BDA0002451821930000103
The specific method of the relationship (2) is as follows:
when the running speed of the mechanical arm is too high, the radius of the convex body swept by the mechanical arm ball is increased
Figure BDA0002451821930000104
Obtaining real-time speed information V of mechanical arm through self-sensor of mechanical arm m The convex radius of the human body ball is swept during the exercise
Figure BDA0002451821930000105
(3) The two factors are combined to establish the state of the moxibustion mechanical arm 2 and the convex body radius swept by the mechanical arm ball
Figure BDA0002451821930000106
The specific method of the relationship (2) is as follows:
when the mechanical arm is in a compliant control mode, the mechanical arm is in a compliant control mode
Figure BDA0002451821930000107
When the robot arm is in position/speed control mode, at this time
Figure BDA0002451821930000108
The concrete process of adjusting the safety threshold value of the collision detection between the moxibustion mechanical arm and a person in the fifth step is as follows:
based on the collision detection model, a collision detection pair is constructed, and a safety threshold value is set to
Figure BDA0002451821930000109
The minimum distance between each collision detection pair of moxa-moxibustion arm and patient (minimum distance is the minimum distance between moxa-moxibustion arm and the basic collision detection model of patient) is monitored in real time, and when this minimum distance is less than above-mentioned safe threshold value, the collision is prejudged by the system, with moxa-moxibustion arm 2 stall or arm switch for gentle and agreeable safety control mode this moment to strengthen moxa-moxibustion arm 2's human-computer interaction security.

Claims (1)

1. A collision avoidance safety protection method for a moxibustion mechanical arm is characterized by comprising the following steps:
step 1, a moxibustion mechanical arm safety protection system is built, and the moxibustion mechanical arm safety protection system comprises a moxibustion mechanical arm (2), wherein a moxa stick (4) is arranged at one end, close to a patient, of the moxibustion mechanical arm (2), a somatosensory camera (1) with a view field covering the patient is arranged at one side of the moxibustion mechanical arm (2), the moxibustion mechanical arm safety protection system also comprises a heart rate measuring bracelet (6) worn on the arm of the patient, and the heart rate measuring bracelet (6), the somatosensory camera (1) and the moxibustion mechanical arm (2) are jointly connected with a mechanical arm control cabinet (5) for real-time communication; acquiring patient body data, including patient expressions acquired through the somatosensory camera (1), patient heart rates acquired through the heart rate measuring bracelet (6), and patient body skeleton models and key point information acquired through the somatosensory camera (1); acquiring moxibustion mechanical arm state information which comprises control modes and movement speed information of the moxibustion mechanical arm acquired by a joint position sensor of the moxibustion mechanical arm (2);
step 2, constructing a variable-radius human body and moxibustion mechanical arm ball swept convex body collision detection model, which comprisesConstructing a human body ball swept convex body collision detection model with variable radius to obtain the radius of the human body ball swept convex body collision detection model
Figure FDA0003892925660000011
Wherein i is the member number of the human body ball sweeping convex body collision detection model with variable radius, i =1,2,3, …,10; the method also comprises the step of constructing a variable-radius moxibustion mechanical arm ball scanning convex body collision detection model to obtain the radius of the moxibustion mechanical arm ball scanning convex body collision detection model
Figure FDA0003892925660000012
Wherein u is the member number of the moxibustion mechanical arm ball sweeping convex body collision detection model with variable radius, and u =1,2,3,4,5;
step 3, establishing the relation between the human body data and the radius of the human body ball swept convex body collision detection model, and specifically comprising the following steps:
step 3.1: representing the emotion of the patient through the expression and the heart rate of the patient, dividing the acquired expression of the patient into positive expression and negative expression, correspondingly obtaining the emotion of the patient as positive emotion and negative emotion in sequence, and respectively establishing the radius of a collision detection model of the heart rate and the human body ball swept convex body under the positive emotion of the patient
Figure FDA0003892925660000013
In relation to (2)
Figure FDA0003892925660000021
Heart rate and human body ball swept convex body collision detection model radius under negative emotion of patient
Figure FDA0003892925660000022
In relation to (2)
Figure FDA0003892925660000023
Relationship when the patient is in positive mood
Figure FDA0003892925660000024
Comprises the following steps:
Figure FDA0003892925660000025
in the formula (1), hr is the real-time heart rate of the patient; n is 2 Lower limit of heart rate for patient with positive mood; n is 3 The upper limit of the heart rate of the patient when Hr > n 3 The treatment is suspended; a. b is a constant coefficient;
Figure FDA0003892925660000026
sweeping the convex body collision detection model radius for the human body ball under positive emotion;
relationship when the patient is in negative mood
Figure FDA0003892925660000027
Comprises the following steps:
Figure FDA0003892925660000028
in the formula (2), hr is the real-time heart rate of the patient; n is 1 Is the patient's standard heart rate; n is 3 The upper limit of the heart rate of the patient when Hr > n 3 The treatment is suspended; c. d are all Chang Jishu,
Figure FDA0003892925660000029
sweeping the radius of the convex collision detection model for the human body ball under negative emotion;
in the formula (1) and the formula (2), n 1 、n 2 、n 3 And Hr are in units of times/min and n 3 ＞n 2 ＞n 1 ；
Step 3.2: calculating the human body speed of the patient through the human body skeleton model of the patient and the key point information, and establishing the radius of the human body speed and human body ball swept convex body collision detection model
Figure FDA00038929256600000210
In relation to (2)
Figure FDA00038929256600000211
The following were used:
Figure FDA00038929256600000212
in the formula (3), the first and second groups,
Figure FDA00038929256600000213
the radius of a model for detecting bump collision of a human body ball during movement is swept, alpha is the maximum acceleration of the moxibustion mechanical arm during deceleration, and V h Is the instantaneous speed of the patient, obtained by the following equation (4):
Figure FDA0003892925660000031
in the formula (4), T is the sampling period of a built-in sensor of the somatosensory camera (1), and the position information acquired to the initial point is A i (x 1 ,y 1 ,z 1 ) After one sampling period, the position information of the collected end point is B i (x 2 ,y 2 ,z 2 ) The distance from the initial point to the end point moving within T time
Figure FDA0003892925660000032
Figure FDA0003892925660000033
Step 3.3: radius of model for detecting collision between human body data established in step 3.1 and step 3.2 and human body ball swept convex body
Figure FDA0003892925660000034
The relationship is as follows:
when the patient is in a positive mood,
Figure FDA0003892925660000035
when the patient is in a negative mood,
Figure FDA0003892925660000036
step 4, establishing the relation between the moxibustion mechanical arm state information and the moxibustion mechanical arm ball swept convex body collision detection model radius, and specifically comprising the following steps:
step 4.1: establishing the radius of a moxibustion mechanical arm (2) control mode and a moxibustion mechanical arm ball sweeping convex body collision detection model
Figure FDA0003892925660000037
The control mode of the moxibustion mechanical arm (2) is divided into position/speed control and compliance control according to the motion control mode:
when the moxibustion mechanical arm (2) is in a flexible control mode, the radius of the model is detected by the collision of the swept convex body of the moxibustion mechanical arm ball
Figure FDA0003892925660000038
Wherein
Figure FDA0003892925660000039
Detecting the radius of a model for the collision of a scanning convex body of a moxibustion mechanical arm ball in a flexible control mode;
when the moxibustion mechanical arm (2) is in a position/speed control mode, the radius of the model is detected by the scanning convex body collision of the moxibustion mechanical arm ball
Figure FDA00038929256600000310
Wherein
Figure FDA00038929256600000311
Detecting the radius of a model for the collision of a scanning convex body of a moxibustion mechanical arm ball in a position/speed control mode, wherein f is a constant;
step 4.2: establishment of moxibustion mechanical arm (2) transportationMoving speed and moxibustion mechanical arm ball sweeping convex body collision detection model radius
Figure FDA00038929256600000312
The relationship between them is as follows:
Figure FDA0003892925660000041
in the formula (5), the first and second groups,
Figure FDA0003892925660000042
the radius of a model for detecting the collision of a swept convex body of a moxibustion mechanical arm ball during movement is alpha which is the maximum acceleration of the moxibustion mechanical arm (2) during deceleration and V m The real-time movement speed information of the moxibustion mechanical arm (2);
step 4.3: the moxibustion mechanical arm (2) state information established in the step 4.1 and the step 4.2 and the moxibustion mechanical arm ball swept convex body collision detection model radius
Figure FDA0003892925660000043
The relationship is as follows:
when the moxibustion mechanical arm (2) is in a flexible control mode,
Figure FDA0003892925660000044
when the moxibustion mechanical arm (2) is in a position/speed control mode,
Figure FDA0003892925660000045
step 5, constructing a collision detection pair, setting and adjusting a safety threshold value of the collision detection of the moxibustion mechanical arm and the patient in real time, specifically: constructing a collision detection pair, and setting a safety threshold value of the collision detection between the moxibustion mechanical arm (2) and the patient as
Figure FDA0003892925660000046
When human body and basic collision detection of moxa-moxibustion armWhen the distance between the models is smaller than a safety threshold value, the system judges that collision occurs in advance, and the mechanical arm control cabinet (5) controls the moxibustion mechanical arm (2) to stop running or switch to a flexible control mode.
CN202010294900.7A 2020-04-15 2020-04-15 Anti-collision safety protection method for moxibustion mechanical arm Active CN111604897B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010294900.7A CN111604897B (en) 2020-04-15 2020-04-15 Anti-collision safety protection method for moxibustion mechanical arm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010294900.7A CN111604897B (en) 2020-04-15 2020-04-15 Anti-collision safety protection method for moxibustion mechanical arm

Publications (2)

Publication Number Publication Date
CN111604897A CN111604897A (en) 2020-09-01
CN111604897B true CN111604897B (en) 2022-12-02

Family

ID=72195929

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010294900.7A Active CN111604897B (en) 2020-04-15 2020-04-15 Anti-collision safety protection method for moxibustion mechanical arm

Country Status (1)

Country Link
CN (1) CN111604897B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP4288251A1 (en) * 2021-02-04 2023-12-13 Abb Schweiz Ag Method of controlling mechanical impedance of robot, control system and robot

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1799069A (en) * 2003-05-14 2006-07-05 皮克萨公司 Statistical dynamic collisions method and apparatus
WO2020002922A1 (en) * 2018-06-29 2020-01-02 Cmr Surgical Limited Detecting collisions of robot arms

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6678582B2 (en) * 2002-05-30 2004-01-13 Kuka Roboter Gmbh Method and control device for avoiding collisions between cooperating robots
DE102015224641A1 (en) * 2015-12-08 2017-06-08 Kuka Roboter Gmbh A method for detecting a collision of a robot arm with an object and robot with a robot arm

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1799069A (en) * 2003-05-14 2006-07-05 皮克萨公司 Statistical dynamic collisions method and apparatus
WO2020002922A1 (en) * 2018-06-29 2020-01-02 Cmr Surgical Limited Detecting collisions of robot arms

Also Published As

Publication number Publication date
CN111604897A (en) 2020-09-01

Similar Documents

Publication Publication Date Title
Lu A motion control method of intelligent wheelchair based on hand gesture recognition
CN103750980B (en) Auxiliary rehabilitation training device for hemiplegic finger of patient
Lee et al. Sensory and intrinsic coordination of movement
Moon et al. Wearable EMG-based HCI for electric-powered wheelchair users with motor disabilities
CN101947152B (en) Electroencephalogram-voice control system and working method of humanoid artificial limb
CN111571582B (en) Moxibustion robot man-machine safety monitoring system and monitoring method
CN104524742A (en) Cerebral palsy child rehabilitation training method based on Kinect sensor
CN109529274B (en) Upper limb joint active rehabilitation system based on redundant mechanical arm and training method thereof
CN103961109A (en) Human body posture detection device based on acceleration signals and angular speed signals
JP2015207285A (en) System and method for producing computer control signals from breath attributes
CN113143256B (en) Gait feature extraction method, lower limb evaluation and control method, device and medium
Xu et al. Elders’ fall detection based on biomechanical features using depth camera
CN111604897B (en) Anti-collision safety protection method for moxibustion mechanical arm
Baldi et al. Design of a wearable interface for lightweight robotic arm for people with mobility impairments
CN109498375B (en) Human motion intention recognition control device and control method
Felzer et al. Alternative wheelchair control involving intentional muscle contractions
Khan et al. Automatic recognition of movement patterns in the vojta-therapy using RGB-D data
Gupta MAC-MAN
CN113903052A (en) Indoor human body collision alarm method and device based on image processing and mechanical analysis
CN108062102A (en) A kind of gesture control has the function of the Mobile Robot Teleoperation System Based of obstacle avoidance aiding
Zhang et al. Neural network-based hybrid human-in-the-loop control for meal assistance orthosis
CN101149601A (en) Intelligent interaction device based on action of moving body and action position detection method
CN206614555U (en) Robot
CN115416003A (en) On-demand auxiliary control method for lower limb exoskeleton of old people
Paulo et al. An innovative robotic walker for mobility assistance and lower limbs rehabilitation

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
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20211203

Address after: 1 / F, building 28, 6055 Jinhai highway, Fengxian District, Shanghai, 201403

Applicant after: Shanghai juncon robot Co.,Ltd.

Address before: 710054 No. 58, middle section, Yanta Road, Shaanxi, Xi'an

Applicant before: Xia Jing

GR01 Patent grant
GR01 Patent grant