CN115816470A - Four-foot robot motion protection method and system - Google Patents
Four-foot robot motion protection method and system Download PDFInfo
- Publication number
- CN115816470A CN115816470A CN202310151128.7A CN202310151128A CN115816470A CN 115816470 A CN115816470 A CN 115816470A CN 202310151128 A CN202310151128 A CN 202310151128A CN 115816470 A CN115816470 A CN 115816470A
- Authority
- CN
- China
- Prior art keywords
- joint
- state
- quadruped robot
- robot
- preset
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 70
- 230000002159 abnormal effect Effects 0.000 claims abstract description 139
- 210000004394 hip joint Anatomy 0.000 claims description 24
- 210000000629 knee joint Anatomy 0.000 claims description 24
- 210000000323 shoulder joint Anatomy 0.000 claims description 24
- 238000005259 measurement Methods 0.000 claims description 17
- 239000011159 matrix material Substances 0.000 claims description 12
- 210000001503 joint Anatomy 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 11
- 238000013016 damping Methods 0.000 claims description 3
- 238000001514 detection method Methods 0.000 description 15
- 208000012661 Dyskinesia Diseases 0.000 description 10
- 238000007689 inspection Methods 0.000 description 7
- 230000009471 action Effects 0.000 description 6
- 230000003068 static effect Effects 0.000 description 6
- 230000002349 favourable effect Effects 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000003247 decreasing effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- JXASPPWQHFOWPL-UHFFFAOYSA-N Tamarixin Natural products C1=C(O)C(OC)=CC=C1C1=C(OC2C(C(O)C(O)C(CO)O2)O)C(=O)C2=C(O)C=C(O)C=C2O1 JXASPPWQHFOWPL-UHFFFAOYSA-N 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 230000005021 gait Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 210000002414 leg Anatomy 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/60—Other road transportation technologies with climate change mitigation effect
- Y02T10/72—Electric energy management in electromobility
Landscapes
- Manipulator (AREA)
Abstract
The application relates to the technical field of quadruped robots, and particularly provides a quadruped robot motion protection method and system, wherein the method comprises the following steps: s1, acquiring a real position of a joint motor based on an encoder of the joint motor, and generating a real torque according to a real current of the joint motor; s2, detecting whether the quadruped robot is in an abnormal motion state or not according to the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the expected position and the expected moment, and acquiring the type of the abnormal motion state when the quadruped robot is in the abnormal motion state, wherein the abnormal motion state comprises an unbalance state, a foot end blocked state and an overturning state; s3, when the quadruped robot is detected to be in an abnormal motion state, controlling a joint motor to execute a corresponding preset instruction according to the type of the abnormal motion state, wherein the preset instruction is used for eliminating the abnormal motion state of the corresponding type; the method can automatically detect and eliminate the abnormal motion state of the quadruped robot.
Description
Technical Field
The application relates to the technical field of quadruped robots, in particular to a method and a system for motion protection of quadruped robots.
Background
The quadruped robot is widely applied to scenes such as security inspection, transportation task execution and the like, because the body motor of the quadruped robot, an electronic device, sensors such as a carried radar and the like are expensive, if the quadruped robot overturns in the operation process or the foot end is clamped on the ground and the like, danger is extremely easy to occur, the damage of the quadruped robot is caused, economic loss is caused, and under the scene of man-machine interaction or industrial inspection, if the quadruped robot falls down or is clamped and stumbled, the damage of personnel and equipment is easy to be caused, and therefore the requirement on the safety protection performance of the quadruped robot is gradually improved.
The current common safety protection methods include: 1. on the mechanical level, an anti-collision or protection device is added to a structure which is easy to damage, for example, an anti-collision support is arranged below the quadruped robot, an anti-collision strip is arranged on a leg motor, and a protection bracket is arranged around a laser radar; 2. in the aspect of an algorithm, one method is to perform power-off protection on a body motor when the quadruped robot has an accident (namely, the quadruped robot is in an abnormal motion state), and the robot in motion directly smashes the ground due to sudden power-off, so that the method has large damage to the quadruped robot and surrounding equipment, namely, the two methods belong to passive protection methods of the quadruped robot. In addition, the two methods are not beneficial to constructing an unmanned scene because the abnormal motion state of the quadruped robot needs to be eliminated manually after the quadruped robot is in an accident, for example, the quadruped robot needs to be manually straightened after the quadruped robot falls down.
In view of the above problems, no effective technical solution exists at present.
Disclosure of Invention
The present application is directed to a method and system for protecting a quadruped robot from movement, which can automatically detect whether the quadruped robot is in an abnormal movement state and eliminate the abnormal movement state by a joint motor when the quadruped robot is in the abnormal movement state.
In a first aspect, the present application provides a quadruped robot motion protection method, applied in a quadruped robot motion protection system, the quadruped robot motion protection system includes a quadruped robot, the quadruped robot includes an inertia measurement unit, four motion feet, four foot end force sensors and a plurality of joint motors, the joint motors are used for driving joints of the motion feet to rotate, the inertia measurement unit is used for acquiring roll angle information and pitch angle information of the quadruped robot, the four foot end force sensors are respectively arranged at foot ends of the four motion feet, the foot end force sensors are used for acquiring acting force information of the foot ends, the quadruped robot motion protection method includes the following steps:
s1, acquiring a real position of a joint motor based on an encoder of the joint motor, and generating a real torque according to a real current of the joint motor;
s2, detecting whether the four-legged robot is in an abnormal motion state or not according to the rolling angle information, the pitch angle information, the acting force information, the real position, the real moment, the expected position and the expected moment, and acquiring the type of the abnormal motion state when the four-legged robot is in the abnormal motion state, wherein the abnormal motion state comprises an unbalanced state, a foot end blocked state and an overturning state;
and S3, when the quadruped robot is detected to be in the abnormal motion state, controlling the joint motor to execute a corresponding preset instruction according to the type of the abnormal motion state, wherein the preset instruction is used for eliminating the abnormal motion state of the corresponding type.
The application provides a four-footed robot motion protection method, whether automated inspection four-footed robot is in the abnormal motion state and eliminate the abnormal motion state through joint motor when four-footed robot is in the abnormal motion state, therefore the method can carry out the initiative protection to four-footed robot, thereby avoid effectively appearing because four-footed robot topples and cause the condition of harm to four-footed robot body and surrounding environment, and because the method can eliminate the abnormal motion state through four-footed robot self, and only need adopt artificial mode to eliminate the abnormal motion state when four-footed robot self can't eliminate the abnormal motion state, consequently compare in prior art, the method can reduce the artifical degree of participation after four-footed robot takes place the abnormal motion state effectively, thereby be favorable to constructing the scene of not humanizing.
Optionally, the imbalance condition satisfies a first condition, the first condition being:
the foot end blocked state meets a second condition, and the second condition is as follows:
the overturning state meets a third condition, which is:
and the normal motion state meets a fourth condition, wherein the fourth condition is as follows:
；
wherein, | | is an or operator,&&for AND operator, roll represents roll angle information, roll max,1 Indicating a preset first roll angle threshold, roll max,2 Representing a preset second roll angle threshold, pitch representing pitch angle information, pitch max,1 Representing a preset first pitch threshold, pitch max,2 Representing a preset second pitch angle threshold, Δ p representing the absolute value of the difference between the desired position and the true position, Δ p max,1 Representing a preset first joint position deviation threshold, Δ p max,2 Representing a preset second joint position deviation threshold,
representing the absolute value of the difference between the desired moment and the true moment,
represents a preset first joint moment deviation threshold value,
representing a preset second joint moment deviation threshold, F x Representing the component of the force information in the x-direction, F y Representing the component of the force information in the y-direction, F z Representing the component of force information in the z direction, μ|F z And | represents a friction cone.
Optionally, step S2 comprises the steps of:
s21, detecting whether the quadruped robot is in an unbalanced state or not according to a first condition, if so, executing a step S3, and if not, executing a step S22;
s22, detecting whether the quadruped robot is in an overturning state or not according to a third condition, if so, executing a step S3, and if not, executing a step S23;
and S23, detecting whether the quadruped robot is in a foot end blocked state or not according to a second condition, if so, executing the step S3, and if not, returning to the step S1.
Optionally, the abnormal motion state is an unbalanced state, and the preset instruction includes the following steps:
the rotating speed of the joint motor is reduced to reduce the moving speed of the quadruped robot, and the duty ratio of the joint motor is improved.
Optionally, the quadruped robot motion protection method further comprises the steps of:
and S4, after the movement speed is reduced and the duty ratio is increased, periodically detecting whether the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the preset expected position and the preset expected moment in the corresponding interval time meet a fourth condition or not according to the preset interval time, if so, controlling the four-footed robot to switch to a normal movement state and return to the step S1, and if not, controlling the four-footed robot to keep an unbalanced state and generating first alarm information.
Optionally, the abnormal motion state is a blocked state of the foot end, and the preset instruction includes the following steps:
respectively calculating a Jacobian matrix of each motion foot based on inverse kinematics, and respectively acquiring the applied moment of each joint motor according to the Jacobian matrix and the preset foot end acting force;
and adjusting the output torque of the corresponding joint motor according to the applied torque so as to stop the quadruped robot.
Optionally, the motion foot comprises a shoulder joint, a hip joint and a knee joint which are connected in sequence, the shoulder joint, the hip joint and the knee joint are respectively driven by three joint motors, and the four-foot robot motion protection method further comprises the following steps:
s4', when the time for the quadruped robot to stop moving is greater than or equal to a first preset time threshold, controlling the corresponding joint motors to rotate forwards or backwards according to the sequence of the shoulder joint, the hip joint and the knee joint;
s5, when the joint motor rotates forwards or backwards, if the absolute value of the difference value between the expected torque and the real torque and/or the absolute value of the difference value between the expected position and the real position meet a second condition, the joint motor is controlled to stop rotating forwards or backwards to generate second alarm information, if the roll angle information and/or the pitch angle information meet a third condition, the joint motor is controlled to stop rotating forwards or backwards and update the type of the abnormal motion state to the roll state, the step S3 is returned, and in the process of rotating forwards or backwards of all the joint motors, if the roll angle information, the pitch angle information, the acting force information, the real position, the real torque, the preset expected position and the preset expected torque all meet a fourth condition, the quadruped robot is controlled to be switched to the normal motion state.
Optionally, the abnormal motion state is an overturning state, and the preset instruction includes the following steps:
the position commands, the speed commands and the rigidity coefficients of all joint motors are set to be 0, and the damping of all joint motors is set to be a non-zero constant, so that the quadruped robot slowly falls over.
Optionally, the motion foot comprises a shoulder joint, a hip joint and a knee joint which are connected in sequence, the shoulder joint, the hip joint and the knee joint are respectively driven by three joint motors, and the four-foot robot motion protection method further comprises the following steps:
s4', when the groveling time of the quadruped robot is larger than or equal to a second preset time threshold, controlling the corresponding joint motors to rotate forwards or backwards according to the sequence of the shoulder joint, the hip joint and the knee joint;
s5', detecting whether roll angle information, pitch angle information, acting force information, real position, real moment, a preset expected position and a preset expected moment all meet a fourth condition in the process of forward rotation or reverse rotation of all joint motors, if so, controlling the quadruped robot to be switched to a normal motion state, and if not, controlling the quadruped robot to keep a lying state and generating second alarm information.
In a second aspect, the present application also provides a quadruped robot motion protection system, comprising:
the quadruped robot comprises an inertia measurement unit, four moving feet, four foot end force sensors and a plurality of joint motors, wherein the joint motors are used for driving joints of the moving feet to rotate;
the controller is electrically connected with the inertia measuring unit, the foot end force sensor and the joint motor;
the controller is used for acquiring the real position of the joint motor based on the encoder of the joint motor and generating real torque according to the real current of the joint motor;
the controller is also used for detecting whether the four-foot robot is in an abnormal motion state or not according to the rolling angle information, the pitch angle information, the acting force information, the real position, the real moment, the expected position and the expected moment, and acquiring the type of the abnormal motion state when the four-foot robot is in the abnormal motion state, wherein the abnormal motion state comprises an unbalance state, a foot end blocked state and an overturning state;
the controller is further used for controlling the joint motor to execute a corresponding preset instruction according to the type of the abnormal motion state when the quadruped robot is detected to be in the abnormal motion state, and the preset instruction is used for eliminating the abnormal motion state of the corresponding type.
The application provides a pair of four-footed robot motion protection system, whether automated inspection four-footed robot is in the abnormal motion state and eliminates the abnormal motion state through the joint motor when four-footed robot is in the abnormal motion state, therefore the system can initiatively protect four-footed robot, thereby avoid appearing effectively because four-footed robot topples and cause the condition of harm to four-footed robot body and surrounding environment, and because the system can eliminate the abnormal motion state through four-footed robot self, and only just need adopt artificial mode to eliminate the abnormal motion state when four-footed robot self can't eliminate the abnormal motion state, consequently, compare in prior art, the system can reduce four-footed robot effectively and take place the artifical degree of participation behind the abnormal motion state, thereby be favorable to constructing the scene of unmanned.
Therefore, the method and the system for motion protection of the quadruped robot can automatically detect whether the quadruped robot is in the abnormal motion state and eliminate the abnormal motion state through the joint motor when the quadruped robot is in the abnormal motion state, so that the quadruped robot can be actively protected, the situation that the quadruped robot topples to damage the quadruped robot body and the surrounding environment is effectively avoided, the abnormal motion state can be eliminated through the quadruped robot, and the abnormal motion state can be eliminated only by adopting an artificial mode when the quadruped robot cannot eliminate the abnormal motion state, so that compared with the prior art, the method can effectively reduce the degree of artificial participation of the quadruped robot after the abnormal motion state occurs, and the establishment of an unmanned scene is facilitated.
Drawings
Fig. 1 is a flowchart of a quadruped robot motion protection method according to an embodiment of the present application.
Fig. 2 is a schematic structural diagram of a quadruped robot provided in an embodiment of the present application.
Fig. 3 is a schematic control structure diagram of a quadruped robot motion protection system according to an embodiment of the present application.
Reference numerals: 1. a quadruped robot; 11. a sport foot; 111. a shoulder joint; 112. a hip joint; 113. a knee joint; 114. a foot end; 12. an inertial measurement unit; 13. a foot end force sensor; 14. a joint motor; 2. and a controller.
Detailed Description
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. The components of the embodiments of the present application, generally described and illustrated in the figures herein, can be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present application, presented in the accompanying drawings, is not intended to limit the scope of the claimed application, but is merely representative of selected embodiments of the application. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present application without making any creative effort, shall fall within the protection scope of the present application.
It should be noted that: like reference numbers and letters refer to like items in the following figures, and thus, once an item is defined in one figure, it need not be further defined or explained in subsequent figures. Meanwhile, in the description of the present application, the terms "first", "second", and the like are used only for distinguishing the description, and are not to be construed as indicating or implying relative importance.
In a first aspect, as shown in fig. 1 to 3, the present application provides a quadruped robot motion protection method, which is applied in a quadruped robot motion protection system, the quadruped robot motion protection system includes a quadruped robot 1, the quadruped robot 1 includes an inertia measurement unit 12, four motion feet 11, four foot end force sensors 13 and a plurality of joint motors 14, the joint motors 14 are used for driving joints of the motion feet 11 to rotate, the inertia measurement unit 12 is used for acquiring roll angle information and pitch angle information of the quadruped robot 1, the four foot end force sensors 13 are respectively disposed at foot ends 114 of the four motion feet 11, the foot end force sensors 13 are used for acquiring acting force information of the foot ends 114, and the quadruped robot motion protection method includes the following steps:
s1, acquiring a real position of a joint motor 14 based on an encoder of the joint motor 14, and generating a real torque according to a real current of the joint motor 14;
s2, detecting whether the quadruped robot 1 is in an abnormal motion state or not according to the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the expected position and the expected moment, and acquiring the type of the abnormal motion state when the quadruped robot 1 is in the abnormal motion state, wherein the abnormal motion state comprises an unbalance state, a foot end blocked state and an overturning state;
and S3, when the quadruped robot 1 is detected to be in the abnormal motion state, controlling the joint motor 14 to execute a corresponding preset instruction according to the type of the abnormal motion state, wherein the preset instruction is used for eliminating the abnormal motion state of the corresponding type.
The inertia measurement unit 12 is in the prior art, the inertia measurement unit 12 is disposed on the quadruped robot 1, the inertia measurement unit 12 is preferably disposed in the middle of the body of the quadruped robot 1, the inertia measurement unit 12 is configured to obtain roll angle information and pitch angle information of the quadruped robot 1, the roll angle information is a roll angle of the quadruped robot 1, and the pitch angle information is a pitch angle of the quadruped robot 1. Four moving feet 11 are arranged on the quadruped robot 1, each moving foot 11 comprises a foot end 114 and a plurality of joints, the number of the joint motors 14 is multiple, the joint motors 14 are used for driving the joints of the moving foot 11 to rotate, in order to enable each joint to rotate independently, each joint preferably corresponds to one joint motor 14, namely the number of the joints of the moving foot 11 is the same as the number of the joint motors 14, and each joint motor has a corresponding expected position and a corresponding expected moment. The four foot end force sensors 13 are respectively arranged on the foot ends 114 of the four moving feet 11, and the foot end force sensors 13 are used for acquiring the acting force information of the foot ends 114, wherein the acting force information is the acting force applied to the foot ends 114 by the ground when the foot ends 114 are in contact with the ground. It should be understood that since different foot end force sensors 13 are respectively used to acquire the force information of different motion feet 11, and the number of foot end force sensors 13 is four, the number of force information is also four.
In step S1, the actual position is a position to which the joint motor 14 actually rotates, in step S1, the actual position of each joint motor 14 is directly obtained based on an encoder of the joint motor 14, the actual current is a current generated in a rotation process of the joint motor 14, the actual torque is an output torque actually generated by the joint motor 14, and it is the prior art that the step S1 generates the actual torque according to the actual current, and detailed discussion is not repeated here.
In step S2, the abnormal motion state is a state corresponding to the situation that the quadruped robot 1 cannot normally move, the abnormal motion state includes three types, namely an unbalanced state, a blocked state of the foot end and an overturning state, the unbalanced state corresponds to the situation that the stability of the quadruped robot 1 is reduced due to the fact that the moving speed of the quadruped robot 1 is too high or the foot end 114 of the moving foot 11 slips, the blocked state of the foot end corresponds to the situation that the foot end 114 of the moving foot 11 is blocked on the ground or an obstacle to prevent the foot end 114 of the moving foot 11 from normally swinging, and the overturning state corresponds to the situation that the quadruped robot 1 has an overturning risk due to external force impact or obstacle obstruction. The expected position and the expected torque are data generated based on the existing balance algorithm of the quadruped robot 1 according to the motion control command of the quadruped robot 1, the motion control command can reflect the target action of the quadruped robot 1, the expected position is the position to which the joint motor 14 rotates when the quadruped robot 1 normally moves (i.e. completes the target action), and the expected torque is the output torque generated by the joint motor 14 when the quadruped robot 1 normally moves (i.e. completes the target action). The working principle of the step S2 is as follows: whether the quadruped robot 1 has the risk of overturning or not can be analyzed according to the roll angle information and the pitch angle information, whether the quadruped robot 1 slips or not can be analyzed according to the acting force information, whether the joint motor 14 drives the joint to normally rotate or not can be analyzed according to the real position, the real moment, the expected position and the expected moment, therefore, step S2 can detect whether the quadruped robot 1 is in the abnormal motion state or not according to the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the preset expected position and the preset expected moment, and acquire the type of the abnormal motion state when the quadruped robot 1 is in the abnormal motion state. The step S2 is equivalent to detecting whether the motion state of the quadruped robot 1 is abnormal or not in real time according to various parameters of the quadruped robot 1, and determining the specific abnormal motion state of the quadruped robot 1 according to various parameters when the motion state is detected to be abnormal.
The preset instruction in step S3 is an instruction for eliminating the abnormal motion state under the condition of avoiding the quadruped robot 1 from overturning, and since different types of abnormal motion states correspond to different abnormal conditions and the instructions to be executed for eliminating different abnormal conditions are different, each type of abnormal motion state corresponds to one preset instruction. The step S3 corresponds to elimination of the abnormal motion state by the quadruped robot 1 itself when the quadruped robot 1 is detected to be in the abnormal motion state. It should be understood that after the preset command is executed by the joint motor 14, if it is detected that the quadruped robot 1 is still in the abnormal motion state (i.e. the quadruped robot 1 cannot eliminate the abnormal motion state by itself), the abnormal motion state needs to be eliminated manually.
The working principle of the embodiment is as follows: the method can automatically detect whether the quadruped robot 1 is in an abnormal motion state according to the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the preset expected position and the preset expected moment, automatically acquire the type of the abnormal motion state when detecting that the quadruped robot 1 is in the abnormal motion state and control the joint motor 14 to execute the preset instruction corresponding to the type so as to eliminate the abnormal motion state, namely, the method can automatically detect whether the quadruped robot 1 is in the abnormal motion state and eliminate the abnormal motion state through the joint motor 14 when the quadruped robot 1 is in the abnormal motion state, so that the method can actively protect the quadruped robot 1, thereby effectively avoiding the situation that the quadruped robot 1 body and the surrounding environment are damaged due to the overturning of the quadruped robot 1, and eliminating the abnormal motion state through the quadruped robot 1, and eliminating the abnormal motion state only when the quadruped robot 1 cannot eliminate the abnormal motion state through the self needs to be manually adopted, therefore, compared with the prior art, the method can effectively reduce the degree of artificial robot after the quadruped robot 1 is in the abnormal motion state, thereby being beneficial to the construction of the unmanned robot.
The application provides a quadruped robot motion protection method, whether automated inspection quadruped robot 1 is in the abnormal motion state and eliminate the abnormal motion state through joint motor 14 when quadruped robot 1 is in the abnormal motion state, therefore the method can carry out initiative protection to quadruped robot 1, thereby avoid effectively appearing because quadruped robot 1 topples and cause the condition of harm to quadruped robot 1 body and surrounding environment, and because the method can eliminate the abnormal motion state through quadruped robot 1 self, and only need adopt artificial mode to eliminate the abnormal motion state when quadruped robot 1 self can't eliminate the abnormal motion state, consequently compare in prior art, the method can reduce the artifical participation degree after quadruped robot 1 takes place the abnormal motion state effectively, thereby be favorable to constructing the unmanned scene.
In some embodiments, the imbalance condition satisfies a first condition that is:
the foot end blocked state meets a second condition, and the second condition is as follows:
the overturning state meets a third condition, which is:
and the normal motion state meets a fourth condition, wherein the fourth condition is as follows:
；
wherein, | | is an or operator,&&for AND operator, roll represents roll angle information, roll max,1 Indicating a preset first roll angle threshold, roll max,2 Representing a preset second roll angle threshold, pitch representing pitch angle information, pitch max,1 Representing a preset first pitch threshold, pitch max,2 Representing a preset second pitch angle threshold, Δ p representing the absolute value of the difference between the desired position and the real position, Δ p max,1 Representing a preset first joint position deviation threshold, Δ p max,2 Representing a preset second joint position deviation threshold,
representing the absolute value of the difference between the desired moment and the true moment,
represents a preset first joint moment deviation threshold value,
representing a preset second joint moment deviation threshold, F x Representing the component of the force information in the x-direction (see fig. 2), F y Representing the component of the force information in the y-direction (see fig. 2), F z Representing the component of the force information in the z-direction (see fig. 2), μ | F z And | represents a friction cone. In this embodiment, the first roll angle threshold is the maximum roll angle allowed when the quadruped robot 1 normally moves, the second roll angle threshold is the minimum roll angle when the quadruped robot 1 is at risk of overturning, the first pitch angle threshold is the maximum pitch angle allowed when the quadruped robot 1 normally moves, the second pitch angle threshold is the minimum pitch angle when the quadruped robot 1 is at risk of overturning, the first joint position deviation threshold is the maximum deviation allowed between the expected position and the real position when the quadruped robot 1 normally moves, the second joint position deviation threshold is the minimum deviation between the expected position and the real position when the foot end 114 of the quadruped robot 1 is blocked, the first joint moment deviation threshold is the maximum deviation allowed between the expected moment and the real moment when the quadruped robot 1 normally moves, and the second joint moment deviation threshold is the minimum deviation between the expected moment and the real moment when the foot end 114 of the quadruped robot 1 is blocked.
The working principle of the embodiment is as follows: the unbalanced state corresponds to the situation that the stability of the quadruped robot 1 is reduced due to the fact that the moving speed of the quadruped robot 1 is too high or the foot end 114 of the moving foot 11 slips, and the like, because the too high moving speed of the quadruped robot 1 may be caused by the too high rotating speed of the joint motor 14, the deviation of the real position from the expected position is slightly larger than the deviation threshold of the first joint position or the deviation of the real moment from the expected moment is slightly larger than the deviation threshold of the first joint moment (i.e. the unbalance state is the moment of the four-legged robot 1)
Or
) Whereas a foot end 114 slip corresponds to
And when the stability of the quadruped robot 1 is reduced, the quadruped robot 1 may tilt, and the roll angle information is slightly larger than the first roll angle threshold value or the pitch angle information is slightly larger than the first pitch angle threshold value (i.e., the pitch angle information is slightly larger than the first pitch angle threshold value)
Or
) Therefore, when each parameter of the quadruped robot 1 satisfies any one of the first conditions, the quadruped robot 1 can be considered to be in an unbalanced state; the blocked state of the foot end corresponds to the situation that the foot end 114 cannot swing normally, and the abnormal swinging of the foot end 114 can cause the joint motor 14 to not rotate normally, and at this time, the deviation between the real position and the expected position is much larger than the deviation threshold of the first joint position or the deviation between the real moment and the expected moment is much larger than the deviation threshold of the first joint position (i.e. the deviation between the real moment and the expected moment is much larger than the deviation threshold of the first joint position) (i.e. the deviation between the real moment and the expected moment is
Or
) Therefore, when the deviation between the actual position and the expected position or the deviation between the actual moment and the expected moment satisfies any one of the second conditions, the quadruped robot 1 can be considered to be in the blocked state of the foot end; the overturning state corresponds to the situation that the quadruped robot 1 has the overturning risk, and at the moment, the roll angle information is far larger than the first roll angle threshold value or the pitch angle information is far larger than the first pitch angle threshold value (namely, the overturning state corresponds to the situation that the quadruped robot 1 has the overturning risk)
Or
) Therefore, when the roll angle information or the pitch angle information satisfies any one of the third conditions, the quadruped robot 1 can be considered to be in the overturning state. It should be understood that since the number of the force information is 4, and the number of the real moment and the real position are both plural, the first condition or any condition is satisfied in any one of the force informationWhen the deviation of an expected moment and a real moment meets a first condition or the deviation of any expected position and a real position meets the first condition, the quadruped robot 1 is considered to be in an unbalanced state; when the deviation of any one expected moment and the real moment meets a second condition or the deviation of any one expected position and the real position meets the second condition, the quadruped robot 1 is considered to be in a foot end blocked state; and when the roll angle information, the pitch angle information, all the acting force information, the deviations of all the expected moments and the real moments and the deviations of all the expected moments and the real moments all satisfy the fourth condition, the quadruped robot 1 is considered to be in a normal motion state.
In some embodiments, step S2 comprises the steps of:
s21, detecting whether the quadruped robot 1 is in an unbalanced state or not according to a first condition, if so, executing a step S3, and if not, executing a step S22;
s22, detecting whether the quadruped robot 1 is in an overturning state or not according to a third condition, if so, executing the step S3, and if not, executing the step S23;
and S23, detecting whether the quadruped robot 1 is in a foot end blocked state or not according to a second condition, if so, executing the step S3, and if not, returning to the step S1.
In some embodiments, the abnormal motion state is an unbalanced state, and the preset instruction comprises the following steps:
the rotation speed of the joint motor 14 is reduced to reduce the movement speed of the four-legged robot 1 and increase the duty ratio of the joint motor 14.
Since the unbalanced state corresponds to a case where the stability of the quadruped robot 1 is degraded, the embodiment reduces the moving speed of the quadruped robot 1 by reducing the rotating speed of the joint motor 14 to eliminate the unbalanced state due to the excessively fast moving speed of the quadruped robot 1, and increases the foot end supporting time in each gait cycle (the proceeding process from the ground to the landing again of the same foot end 114) by increasing the duty ratio of the joint motor 14 to improve the stability of the quadruped robot 1, that is, the embodiment eliminates the unbalanced state by reducing the moving time of the quadruped robot 1 and improving the stability of the quadruped robot 1.
In some embodiments, the quadruped robot motion protection method further comprises the steps of:
and S4, after the movement speed is reduced and the duty ratio is increased, periodically detecting whether the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the preset expected position and the preset expected moment in the corresponding interval time meet a fourth condition or not according to the preset interval time, if so, controlling the four-foot robot 1 to switch to a normal movement state and returning to the step S1, and if not, controlling the four-foot robot 1 to keep an unbalance state and generating first alarm information.
The interval time in this embodiment is a preset value, and those skilled in the art can change the size of the interval time according to actual needs. The working principle of the embodiment is as follows: if the parameters of the quadruped robot 1 meet the fourth condition in the interval time, the unbalance state is eliminated, so that the quadruped robot 1 can be controlled to be switched to the normal motion state and the parameters of the quadruped robot 1 are continuously acquired (namely, the step S1 is returned); if the fourth condition is not satisfied to each item parameter of four-footed robot 1 in the interval time, then look the unbalance state and can't be eliminated, four-footed robot 1 can't normally move under current motion environment, consequently need control four-footed robot 1 and keep the unbalance state and generate first alarm information, first alarm information is used for reminding the user to reset the parameter of four-footed robot 1 to make four-footed robot 1 normally move under current motion environment. It should be understood that, since the unbalanced state corresponds to a case where the stability of the four-footed robot 1 is decreased, after the stability of the four-footed robot 1 is increased by decreasing the movement speed and increasing the duty ratio, even if the unbalanced state cannot be eliminated, the four-footed robot 1 can continue to perform tasks (e.g., inspection tasks and transportation tasks), this embodiment is equivalent to periodically detecting whether the unbalanced state is eliminated after the four-footed robot 1 switches to the unbalanced state, if so, controlling the four-footed robot 1 to perform the tasks in the normal movement state, and if not, controlling the four-footed robot 1 to perform the tasks at a low speed.
In some embodiments, the quadruped robot 1 stores actual detection cycle information, and the step S4 includes the steps of:
s41, after the movement speed is reduced and the duty ratio is increased, periodically detecting whether the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the preset expected position and the preset expected moment in the corresponding interval time meet a fourth condition according to the preset interval time, if the actual detection period information is smaller than a preset detection period threshold value, controlling the quadruped robot 1 to switch to a normal movement state and return to the step S1, and if the actual detection period information is not larger than or equal to the detection period threshold value, controlling the quadruped robot 1 to keep an unbalance state and generating first alarm information.
The actual detection period information is the number of periods for detecting whether each parameter of the quadruped robot 1 meets the fourth condition within the interval time, the actual detection period information can reflect the number of times for detecting whether each parameter of the quadruped robot 1 meets the fourth condition within the interval time, the initial value of the actual detection period information is 0, and 1 is added to the actual detection period information if each parameter of the quadruped robot 1 meets the fourth condition within the interval time. The detection period threshold is a preset value, and the detection period threshold is the maximum number of times for detecting whether each parameter of the quadruped robot 1 meets the fourth condition within the interval time. Taking the interval time as 15S and the detection period threshold as 4 as an example, the embodiment detects whether each parameter of the quadruped robot 1 in the current period satisfies the fourth condition every 15S, and if yes, and the actual detection period information is less than 4, the quadruped robot 1 is controlled to be switched to the normal motion state and returns to the step S1; if not, and the actual detection cycle information is equal to or greater than 4, the quadruped robot 1 is controlled to maintain the unbalance state and first alarm information is generated, and this embodiment corresponds to detecting whether the unbalance state is eliminated every 15s within 60s (the product of the detection cycle threshold and the interval time). As can be seen, this embodiment corresponds to periodically detecting whether the unbalanced state is removed or not within a certain period of time after the quadruped robot 1 switches to the unbalanced state, and if so, controlling the quadruped robot 1 to execute a task in a normal motion state, and if not, controlling the quadruped robot 1 to execute a task at a low speed.
In some embodiments, the abnormal motion state is a blocked foot state, and the preset command comprises the following steps:
respectively calculating a Jacobian matrix of each moving foot 11 based on inverse kinematics, and respectively acquiring the applied moment of each joint motor 14 according to the Jacobian matrix and the preset foot end acting force;
the output torque of the corresponding joint motor 14 is adjusted in accordance with the applied torque to stop the quadruped robot 1.
Calculating the jacobian matrices of the moving feet 11 based on inverse kinematics is prior art and will not be discussed in detail here, and each moving foot 11 corresponds to one jacobian matrix, so the number of jacobian matrices is the same as the number of moving feet 11. Since each of the moving feet 11 includes a plurality of joints each corresponding to one joint motor 14, and this embodiment requires different moments to be applied to different joints to stop the movement of the quadruped robot 1, it is necessary to obtain the applied moment of each joint motor 14, which is the output moment of the joint motor 14 required for stopping the movement and stably standing the quadruped robot 1, based on the jacobian matrix and the preset foot end acting force, respectively. After the application torque of each joint motor 14 is acquired, the corresponding joint motor 14 is adjusted according to the application torque, so that the joint motor 14 generates an output torque corresponding to the application torque, and the quadruped robot 1 stops moving and stands stably. Specifically, the moving foot 11 includes a shoulder joint 111, a hip joint 112 and a knee joint 113 connected in sequence, the shoulder joint 111, the hip joint 112 and the knee joint 113 are driven by three joint motors 14 respectively, and the applied torques of the three joint motors 14 on one of the moving feet 11 are calculated according to the jacobian matrix and the preset foot end acting force, which is represented by the following formula:
wherein,
indicating the applied torque of the joint motor 14 corresponding to the shoulder joint 111,
representing the applied torque of the joint motor 14 for the hip joint 112,
represents the applied torque of the joint motor 14 corresponding to the knee joint 113, J represents the Jacobian matrix, J T The method comprises the steps of representing a transposed matrix of a Jacobian matrix, f representing a preset foot end acting force, m representing the whole machine mass of the quadruped robot 1, and g representing the gravity acceleration.
In some embodiments, the motion foot 11 comprises a shoulder joint 111, a hip joint 112 and a knee joint 113 which are connected in sequence, the shoulder joint 111, the hip joint 112 and the knee joint 113 are respectively driven by three joint motors 14, and the four-foot robot motion protection method further comprises the following steps:
s4', when the time for the quadruped robot 1 to stop moving is greater than or equal to a first preset time threshold value, controlling the corresponding joint motors 14 to rotate forwards or backwards according to the sequence of the shoulder joint 111, the hip joint 112 and the knee joint 113;
s5, when the joint motor 14 rotates forwards or backwards, if the absolute value of the difference between the expected torque and the real torque and/or the absolute value of the difference between the expected position and the real position meet a second condition, the joint motor 14 is controlled to stop rotating forwards or backwards to generate second alarm information, if the roll angle information and/or the pitch angle information meet a third condition, the joint motor 14 is controlled to stop rotating forwards or backwards and the type of the abnormal motion state is updated to the overturning state, the operation returns to the step S3, and in the process of rotating forwards or backwards of all the joint motors 14, if the roll angle information, the pitch angle information, the acting force information, the real position, the real torque, the preset expected position and the preset expected torque all meet a fourth condition, the quadruped robot 1 is controlled to be switched to the normal motion state.
The first preset time threshold of the step S4' is a preset value, and a person skilled in the art can change the size of the first preset time threshold according to actual needs, and if the time for the quadruped robot 1 to stop moving is greater than or equal to the first preset time threshold, the quadruped robot 1 is considered to be in a stationary state; and if the time for the quadruped robot 1 to stop moving is less than a first preset time threshold, the quadruped robot 1 is considered to be in a non-static state. Preferably, the embodiment can also detect whether the quadruped robot 1 is in a stationary state according to the speed information returned by the joint motors 14, a preset speed threshold and a first preset time threshold, specifically, the speed threshold is the maximum speed of each joint motor 14 when the quadruped robot 1 is in the stationary state, and if the timing time after all the speed information is less than the speed threshold is greater than or equal to the first preset time threshold, the quadruped robot 1 is considered to be in the stationary state; and if the timing time after all the speed information is smaller than the speed threshold is smaller than a first preset time threshold, the quadruped robot 1 is considered to be in a non-static state.
The working principle of the embodiment is as follows: the foot end blocked state corresponds to the situation that the foot end 114 can not swing normally, after the quadruped robot 1 is in a static state, the embodiment respectively controls the corresponding joint motor 14 to rotate forward or backward according to the sequence of the shoulder joint 111, the hip joint 112 and the knee joint 113 for each motion foot 11 so as to detect whether each joint can move normally, thereby detecting whether the foot end 114 of each motion foot 11 can swing normally (namely whether the foot end blocked state can be eliminated), because the quadruped robot 1 is in a standing state, and the situation that the quadruped robot 1 topples due to the posture change of the quadruped robot 1 can occur when the joint motor 14 rotates forward or backward, therefore, in the forward or backward rotation process of the joint motor 14, whether each parameter of the quadruped robot 1 meets the second condition or the third condition needs to be continuously detected, if the second condition is met, the foot end blocked state can not be eliminated according to the situation, the joint motor 14 is controlled to stop or reversely rotate, and generate a second alarm message, wherein the second alarm message is used for reminding the user that the abnormal motion state needs to be eliminated in a manual mode; if the third condition is met, controlling the joint motor 14 to stop forward rotation or reverse rotation and updating the type of the abnormal motion state to the overturning state according to the existence of the overturning risk of the quadruped robot 1, and executing a preset instruction corresponding to the overturning state (namely returning to the step S3); in the forward rotation or reverse rotation process of all the joint motors 14, if each parameter of the quadruped robot 1 meets the fourth condition, the blocked state of the foot end is eliminated, and at this time, the quadruped robot 1 needs to be controlled to be switched to the normal motion state, so that the quadruped robot 1 continues to execute tasks. It should be understood that in controlling the joint motor 14 on one of the moving feet 11 to rotate in the forward or reverse direction, the embodiment utilizes the existing balance control algorithm to adjust the other three moving feet 11 to keep the quadruped robot 1 balanced.
In some embodiments, the abnormal motion state is an overturning state, and the preset command includes the following steps:
the position commands, the speed commands and the stiffness coefficients of all the joint motors 14 are set to 0, and the damping of all the joint motors 14 is set to a non-zero constant, so that the quadruped robot 1 slowly falls down.
In some embodiments, the motion foot 11 comprises a shoulder joint 111, a hip joint 112 and a knee joint 113 which are connected in sequence, the shoulder joint 111, the hip joint 112 and the knee joint 113 are respectively driven by three joint motors 14, and the four-foot robot motion protection method further comprises the following steps:
s4'', when the groveling time of the quadruped robot 1 is larger than or equal to a second preset time threshold, controlling the corresponding joint motor 14 to rotate forwards or backwards according to the sequence of the shoulder joint 111, the hip joint 112 and the knee joint 113;
s5', detecting whether roll angle information, pitch angle information, acting force information, a real position, a real moment, a preset expected position and a preset expected moment all meet a fourth condition in the process of forward rotation or reverse rotation of all the joint motors 14, if so, controlling the quadruped robot 1 to be switched to a normal motion state, and if not, controlling the quadruped robot 1 to keep a state of lying prone and generating second alarm information.
The second preset time threshold of the step S4 ″ is a preset value, and a person skilled in the art can change the size of the second preset time threshold according to actual needs, and if the time for the quadruped robot 1 to stop moving is greater than or equal to the second preset time threshold, the quadruped robot 1 is considered to be in a stationary state; and if the time for stopping the movement of the quadruped robot 1 is less than a second preset time threshold, the quadruped robot 1 is considered to be in a non-static state. Preferably, the embodiment can also detect whether the quadruped robot 1 is in a stationary state according to the speed information returned by the joint motors 14, a preset speed threshold and a second preset time threshold, specifically, the speed threshold is the maximum speed of each joint motor 14 when the quadruped robot 1 is in the stationary state, and if the timing time after all the speed information is less than the speed threshold is greater than or equal to the second preset time threshold, the quadruped robot 1 is considered to be in the stationary state; and if the timing time after all the speed information is smaller than the speed threshold is smaller than a second preset time threshold, the quadruped robot 1 is considered to be in a non-static state.
The working principle of the embodiment is as follows: after the quadruped robot 1 is in a static state, the embodiment controls the corresponding joint motors 14 to rotate forwards or backwards according to the sequence of the shoulder joint 111, the hip joint 112 and the knee joint 113 so as to detect whether each joint can move normally, and in the process of rotating forwards or backwards of all the joint motors 14, if each parameter of the quadruped robot 1 meets the fourth condition, the visual overturning state is eliminated, and at this time, the quadruped robot 1 needs to be restored to a standing state through the joint motors 14 and the quadruped robot 1 needs to be controlled to be switched to a normal movement state so that the quadruped robot 1 can continue to execute tasks; if the parameters of the quadruped robot 1 do not satisfy the fourth condition, the visual overturning state cannot be eliminated, the joint motor 14 needs to be controlled to stop forward rotation or reverse rotation at the moment, second alarm information is generated, and the second alarm information is used for reminding a user that the abnormal motion state needs to be eliminated in a manual mode.
From the above, according to the method for protecting the movement of the quadruped robot, whether the quadruped robot 1 is in the abnormal movement state or not is automatically detected, and the abnormal movement state is eliminated through the joint motor 14 when the quadruped robot 1 is in the abnormal movement state, so that the quadruped robot 1 can be actively protected, the situation that the quadruped robot 1 body and the surrounding environment are damaged due to overturning of the quadruped robot 1 is effectively avoided, and the abnormal movement state can be eliminated through the quadruped robot 1, and the abnormal movement state is eliminated only by adopting an artificial mode when the quadruped robot 1 cannot eliminate the abnormal movement state, so that compared with the prior art, the method can effectively reduce the artificial participation degree of the quadruped robot 1 after the abnormal movement state occurs, and is favorable for constructing an unmanned scene.
In a second aspect, as shown in fig. 3, the present application further provides a quadruped robotic motion protection system comprising:
the quadruped robot 1 comprises an inertia measurement unit 12, four moving feet 11, four foot end force sensors 13 and a plurality of joint motors 14, wherein the joint motors 14 are used for driving joints of the moving feet 11 to rotate, the inertia measurement unit 12 is used for acquiring roll angle information and pitch angle information of the quadruped robot 1, the four foot end force sensors 13 are respectively arranged at foot ends 114 of the four moving feet 11, and the foot end force sensors 13 are used for acquiring acting force information of the foot ends 114;
the controller 2 is electrically connected with the inertia measurement unit 12, the foot end force sensor 13 and the joint motor 14;
the controller 2 is used for acquiring the real position of the joint motor 14 based on an encoder of the joint motor 14 and generating real torque according to the real current of the joint motor 14;
the controller 2 is further configured to detect whether the quadruped robot 1 is in an abnormal motion state according to the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the expected position and the expected moment, and acquire the type of the abnormal motion state when the quadruped robot 1 is in the abnormal motion state, wherein the abnormal motion state includes an unbalanced state, a foot end blocked state and an overturning state;
the controller 2 is further configured to control the joint motor 14 to execute a corresponding preset instruction according to the type of the abnormal motion state when the quadruped robot 1 is detected to be in the abnormal motion state, and the preset instruction is used for eliminating the corresponding type of the abnormal motion state.
The embodiment of the present application provides a quadruped robot motion protection system, the working principle of which is the same as that of the quadruped robot motion protection method provided by the first aspect, and the detailed discussion is omitted here.
The application provides a pair of four-footed robot motion protection system, whether automated inspection four-footed robot 1 is in the abnormal motion state and eliminates the abnormal motion state through joint motor 14 when four-footed robot 1 is in the abnormal motion state, therefore the system can initiatively protect four-footed robot 1, thereby avoid appearing effectively because four-footed robot 1 topples and cause the condition of harm to four-footed robot 1 body and surrounding environment, and because the system can eliminate the abnormal motion state through four-footed robot 1 self, and only just need adopt artificial mode to eliminate the abnormal motion state when four-footed robot 1 self can't eliminate the abnormal motion state, consequently compare in prior art, the system can reduce four-footed robot 1 effectively and take place the artifical participation degree behind the abnormal motion state, thereby be favorable to constructing the scene of unmanned.
As can be seen from the above, the method and system for motion protection of a quadruped robot provided by the present application automatically detect whether the quadruped robot 1 is in an abnormal motion state and eliminate the abnormal motion state through the joint motor 14 when the quadruped robot 1 is in the abnormal motion state, so the method can actively protect the quadruped robot 1, thereby effectively avoiding the situation that the quadruped robot 1 body and the surrounding environment are damaged due to the overturning of the quadruped robot 1, and because the abnormal motion state can be eliminated through the quadruped robot 1 itself, and the abnormal motion state needs to be eliminated manually only when the quadruped robot 1 itself cannot eliminate the abnormal motion state, compared with the prior art, the method can effectively reduce the human participation degree after the quadruped robot 1 is in the abnormal motion state, thereby facilitating the construction of an unmanned scene.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method may be implemented in other ways. The above-described embodiments of the apparatus are merely illustrative, and for example, the above-described division of the units is only one logical functional division, and there may be other divisions when actually implemented, and for example, a plurality of units or components may be combined or integrated into another robot, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection of devices or units through some communication interfaces, and may be in an electrical, mechanical or other form.
In addition, units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may ascend to one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
Furthermore, the functional modules in the embodiments of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.
In this document, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions.
The above description is only an example of the present application and is not intended to limit the scope of the present application, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, improvement and the like made within the spirit and principle of the present application shall be included in the protection scope of the present application.
Claims (10)
1. A four-foot robot motion protection method is characterized by being applied to a four-foot robot motion protection system, the four-foot robot motion protection system comprises a four-foot robot, the four-foot robot comprises an inertia measurement unit, four motion feet, four foot end force sensors and a plurality of joint motors, the joint motors are used for driving joints of the motion feet to rotate, the inertia measurement unit is used for acquiring roll angle information and pitch angle information of the four-foot robot, the four foot end force sensors are respectively arranged at foot ends of the four motion feet, the foot end force sensors are used for acquiring acting force information of the foot ends, and the four-foot robot motion protection method comprises the following steps:
s1, acquiring a real position of a joint motor based on an encoder of the joint motor, and generating a real torque according to a real current of the joint motor;
s2, detecting whether the quadruped robot is in an abnormal motion state or not according to the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the expected position and the expected moment, and acquiring the type of the abnormal motion state when the quadruped robot is in the abnormal motion state, wherein the abnormal motion state comprises an unbalance state, a foot end blocked state and an overturning state;
and S3, when the quadruped robot is detected to be in an abnormal motion state, controlling the joint motor to execute a corresponding preset instruction according to the type of the abnormal motion state, wherein the preset instruction is used for eliminating the abnormal motion state of the corresponding type.
2. The method for motion protection of a quadruped robot according to claim 1, wherein the unbalanced state satisfies a first condition that:
the foot end blocked state satisfies a second condition, which is:
the overturning state satisfies a third condition, which is:
and the normal motion state meets a fourth condition, wherein the fourth condition is as follows:
；
wherein, | | is an or operator,&&for AND operator, roll represents roll angle information, roll max,1 Indicating a preset first roll angle threshold, roll max,2 Representing a preset second roll angle threshold, pitch representing pitch angle information, pitch max,1 Representing a preset first pitch threshold, pitch max,2 Representing a preset second pitch angle threshold, Δ p representing the absolute value of the difference between the desired position and the real position, Δ p max,1 Representing a preset first joint position deviation threshold, Δ p max,2 Representing a preset second joint position deviation threshold,
representing the absolute value of the difference between the desired moment and the true moment,
represents a preset first joint moment deviation threshold value,
representing a preset second joint moment deviation threshold, F x Representing the component of the force information in the x-direction, F y Representing the component of the force information in the y-direction, F z Representing the component of the force information in the z-direction, μ | F z And | represents a friction cone.
3. The quadruped robot movement protection method according to claim 2, wherein the step S2 comprises the steps of:
s21, detecting whether the quadruped robot is in an unbalanced state or not according to the first condition, if so, executing the step S3, and if not, executing the step S22;
s22, detecting whether the quadruped robot is in an overturning state or not according to the third condition, if so, executing the step S3, and if not, executing the step S23;
and S23, detecting whether the quadruped robot is in a foot end blocked state or not according to the second condition, if so, executing the step S3, and if not, returning to the step S1.
4. The method for motion protection of a quadruped robot according to claim 2, wherein the abnormal motion state is an unbalanced state, and the preset instruction comprises the steps of:
and reducing the rotating speed of the joint motor to reduce the movement speed of the quadruped robot and improving the duty ratio of the joint motor.
5. The quadruped robot motion protection method according to claim 4, further comprising the steps of:
and S4, after the movement speed is reduced and the duty ratio is increased, periodically detecting whether the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the preset expected position and the preset expected moment in the corresponding interval time meet a fourth condition or not according to the preset interval time, if so, controlling the quadruped robot to be switched to a normal movement state and return to the step S1, and if not, controlling the quadruped robot to keep the unbalance state and generate first alarm information.
6. The method for protecting the motion of a quadruped robot according to claim 2, wherein the abnormal motion state is a blocked state of the foot end, and the preset command comprises the following steps:
respectively calculating a Jacobian matrix of each motion foot based on inverse kinematics, and respectively acquiring the applied moment of each joint motor according to the Jacobian matrix and a preset foot end acting force;
and adjusting the output torque of the corresponding joint motor according to the applied torque so as to stop the quadruped robot.
7. The four-footed robot motion preserving method of claim 6, wherein the motion foot comprises a shoulder joint, a hip joint and a knee joint connected in sequence, the shoulder joint, the hip joint and the knee joint being driven by three joint motors respectively, the four-footed robot motion preserving method further comprising the steps of:
s4', when the time for the quadruped robot to stop moving is greater than or equal to a first preset time threshold, controlling the corresponding joint motors to rotate forwards or backwards according to the sequence of the shoulder joint, the hip joint and the knee joint;
s5, when the joint motor rotates forwards or reversely, if the absolute value of the difference value between the expected torque and the real torque and/or the absolute value of the difference value between the expected position and the real position meet the second condition, the joint motor is controlled to stop rotating forwards or reversely to generate second alarm information, if the roll angle information and/or the pitch angle information meet the third condition, the joint motor is controlled to stop rotating forwards or reversely and the type of the abnormal motion state is updated to the overturning state, the operation returns to the step S3, and in the process of rotating forwards or reversely of all the joint motors, if the roll angle information, the pitch angle information, the acting force information, the real position, the real torque, the preset expected position and the preset expected torque all meet the fourth condition, the quadruped robot is controlled to be switched to the normal motion state.
8. The quadruped robot motion protection method according to claim 2, wherein the abnormal motion state is an overturning state, and the preset instruction comprises the steps of:
and setting the position commands, the speed commands and the rigidity coefficients of all joint motors to be 0, and setting the damping of all joint motors to be a non-zero constant so as to enable the quadruped robot to slowly lie prone.
9. The four-footed robot motion preserving method of claim 8, wherein the motion foot comprises a shoulder joint, a hip joint and a knee joint connected in sequence, the shoulder joint, the hip joint and the knee joint being driven by three joint motors respectively, the four-footed robot motion preserving method further comprising the steps of:
s4'' when the groveling time of the quadruped robot is larger than or equal to a second preset time threshold, controlling the corresponding joint motors to rotate forwards or backwards according to the sequence of the shoulder joint, the hip joint and the knee joint;
s5' detects at all joint motor corotation or the in-process of reversal whether roll angle information, pitch angle information, effort information true position, true moment, predetermined expectation position and predetermined expectation moment all satisfy the fourth condition, if yes, then control four-footed robot and switch to normal motion state, if no, then control four-footed robot keeps the state of lying prone and generates second alarm information.
10. A quadruped robotic motion protection system, comprising:
the quadruped robot comprises an inertia measurement unit, four moving feet, four foot end force sensors and a plurality of joint motors, wherein the joint motors are used for driving joints of the moving feet to rotate;
the controller is electrically connected with the inertial measurement unit, the foot end force sensor and the joint motor;
the controller is used for acquiring the real position of the joint motor based on an encoder of the joint motor and generating real torque according to the real current of the joint motor;
the controller is further used for detecting whether the quadruped robot is in an abnormal motion state or not according to the roll angle information, the pitch angle information, the acting force information, the real position, the real moment, the expected position and the expected moment, and acquiring the type of the abnormal motion state when the quadruped robot is in the abnormal motion state, wherein the abnormal motion state comprises an unbalance state, a foot end blocked state and an overturning state;
the controller is further used for controlling the joint motor to execute a corresponding preset instruction according to the type of the abnormal motion state when the quadruped robot is detected to be in the abnormal motion state, and the preset instruction is used for eliminating the abnormal motion state of the corresponding type.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310151128.7A CN115816470B (en) | 2023-02-22 | 2023-02-22 | Motion protection method and system for quadruped robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310151128.7A CN115816470B (en) | 2023-02-22 | 2023-02-22 | Motion protection method and system for quadruped robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115816470A true CN115816470A (en) | 2023-03-21 |
| CN115816470B CN115816470B (en) | 2023-05-05 |
Family
ID=85522083
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310151128.7A Active CN115816470B (en) | 2023-02-22 | 2023-02-22 | Motion protection method and system for quadruped robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115816470B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040172165A1 (en) * | 2002-12-05 | 2004-09-02 | Sony Corporation | Legged mobile robot |
| US20120143376A1 (en) * | 2010-12-02 | 2012-06-07 | Samsung Electronics Co.. Ltd. | Walking robot and method for controlling posture thereof |
| CN110209183A (en) * | 2019-06-04 | 2019-09-06 | 吉林大学 | A kind of hexapod robot unstability adjusting algorithm based on dual-threshold voltage ZMP |
| CN111891252A (en) * | 2020-08-06 | 2020-11-06 | 齐鲁工业大学 | Body posture slope self-adaptive control method of four-footed bionic robot |
| CN115092283A (en) * | 2022-07-25 | 2022-09-23 | 中山大学 | Robot control method based on gradient CPG with vestibular feedback |
-
2023
- 2023-02-22 CN CN202310151128.7A patent/CN115816470B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040172165A1 (en) * | 2002-12-05 | 2004-09-02 | Sony Corporation | Legged mobile robot |
| US20120143376A1 (en) * | 2010-12-02 | 2012-06-07 | Samsung Electronics Co.. Ltd. | Walking robot and method for controlling posture thereof |
| CN110209183A (en) * | 2019-06-04 | 2019-09-06 | 吉林大学 | A kind of hexapod robot unstability adjusting algorithm based on dual-threshold voltage ZMP |
| CN111891252A (en) * | 2020-08-06 | 2020-11-06 | 齐鲁工业大学 | Body posture slope self-adaptive control method of four-footed bionic robot |
| CN115092283A (en) * | 2022-07-25 | 2022-09-23 | 中山大学 | Robot control method based on gradient CPG with vestibular feedback |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115816470B (en) | 2023-05-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Neubauer | A spider-like robot that climbs vertically in ducts or pipes | |
| KR102370879B1 (en) | Method and Apparatus for controlling a collaborativve robot | |
| US7072740B2 (en) | Legged mobile robot | |
| CN106584489B (en) | The robot system for having the function for the position and direction for calculating sensor | |
| Renner et al. | Instability detection and fall avoidance for a humanoid using attitude sensors and reflexes | |
| CN208614800U (en) | A kind of control system of robot | |
| US10940585B2 (en) | Vibration suppression device | |
| KR101689793B1 (en) | Motor control apparatus and control method the same | |
| EP1103449A1 (en) | Robot failure diagnosing systems | |
| US20100057253A1 (en) | Robot and method of controlling safety thereof | |
| US20130178980A1 (en) | Anti-collision system for moving an object around a congested environment | |
| Dimitrova et al. | Towards low-latency high-bandwidth control of quadrotors using event cameras | |
| CN110000807B (en) | Steering engine protection method and system of machine and readable storage medium | |
| CN101667026A (en) | Robot and method of controlling the same | |
| KR101329853B1 (en) | Collision detection system of manipulator using torque filtering and control system and method of manipulator using the same | |
| CN113858207B (en) | Inverted floor control method and device for humanoid robot, electronic equipment and storage medium | |
| CN115816470A (en) | Four-foot robot motion protection method and system | |
| US9862095B2 (en) | Control device for mobile robot | |
| US11472024B2 (en) | Humanoid robot and its control method and computer readable storage medium | |
| CN113442118B (en) | Collision response control method and system for wearable outer limb robot | |
| Leines et al. | LQR control of an under actuated planar biped robot | |
| Zhang et al. | Contact force estimation and regulation of a position-controlled floating base system without joint torque information | |
| Spenneberg et al. | Control of a bio-inspired four-legged robot for exploration of uneven terrain | |
| JP2008126382A (en) | Biped mobile robot and its control method | |
| CN113788085B (en) | Safety protection method and foot type robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |