CN115389077B - Collision detection method, collision detection device, control apparatus, and readable storage medium - Google Patents
Collision detection method, collision detection device, control apparatus, and readable storage medium Download PDFInfo
- Publication number
- CN115389077B CN115389077B CN202211030065.1A CN202211030065A CN115389077B CN 115389077 B CN115389077 B CN 115389077B CN 202211030065 A CN202211030065 A CN 202211030065A CN 115389077 B CN115389077 B CN 115389077B
- Authority
- CN
- China
- Prior art keywords
- joint
- speed
- robot
- current
- motor
- 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
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 45
- 238000003860 storage Methods 0.000 title claims abstract description 13
- 238000000034 method Methods 0.000 claims abstract description 35
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 12
- 230000001133 acceleration Effects 0.000 claims description 21
- 230000002441 reversible effect Effects 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000004590 computer program Methods 0.000 claims description 5
- 238000005520 cutting process Methods 0.000 claims description 2
- 230000008569 process Effects 0.000 abstract description 16
- 230000033001 locomotion Effects 0.000 abstract description 10
- 230000002829 reductive effect Effects 0.000 abstract description 10
- 238000010586 diagram Methods 0.000 description 13
- 238000004891 communication Methods 0.000 description 11
- 230000006870 function Effects 0.000 description 7
- 239000011159 matrix material Substances 0.000 description 6
- 230000009471 action Effects 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 4
- 230000003993 interaction Effects 0.000 description 4
- 238000005457 optimization Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 238000005096 rolling process Methods 0.000 description 4
- 230000009467 reduction Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000005856 abnormality Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000001066 destructive effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000003449 preventive effect Effects 0.000 description 1
- 230000036962 time dependent Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
- G01L5/00—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes
- G01L5/0052—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes measuring forces due to impact
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/06—Safety devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
- G01L5/00—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes
- G01L5/0061—Force sensors associated with industrial machines or actuators
- G01L5/0076—Force sensors associated with manufacturing machines
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Chemical & Material Sciences (AREA)
- Analytical Chemistry (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The embodiment of the application provides a collision detection method, a collision detection device, control equipment and a readable storage medium, and relates to the technical field of robots. The method comprises the following steps: obtaining current information of each joint of the robot, wherein the current information comprises current state information and motor torque serving as current control quantity; based on a model predictive control algorithm, obtaining the motor torque at the next moment according to the current information; judging whether the robot collides currently or not according to the motor torque at the next moment and the preset torque. Therefore, collision can be detected in time, the whole motion process of the robot is stable, and false detection is reduced.
Description
Technical Field
The present application relates to the field of robot technologies, and in particular, to a collision detection method, a collision detection device, a control device, and a readable storage medium.
Background
In order to adapt to more complex working environments by using robots to meet human service requirements, safety problems in human-computer interaction processes need to be solved. The mechanical arm of the robot can generate strong force in the movement process, and is extremely dangerous and even destructive when accidentally contacting with a human body or other objects. The safety problem of the mechanical arm is a problem which needs to be solved in the practical application process, particularly in the human-computer interaction process.
The human-computer interaction safety has two directions, namely active safety and passive safety. Active safety is a preventive measure such as safety action of the mechanical arm before danger occurs. Passive safety refers to the fact that when the mechanical arm is in direct contact with a person or other objects, some safety measures are passively taken by the mechanical arm to avoid danger. The main study endpoint is the latter-passive safety. Therefore, how to provide an effective detection means to detect collision in time is a technical problem that needs to be solved by those skilled in the art.
Disclosure of Invention
The embodiment of the application provides a collision detection method, a device, control equipment and a readable storage medium, which can timely detect collision, enable the whole motion process of a robot to be stable and reduce false detection.
Embodiments of the present application may be implemented as follows:
in a first aspect, an embodiment of the present application provides a collision detection method, including:
obtaining current information of each joint of the robot, wherein the current information comprises current state information and motor torque serving as current control quantity;
based on a model predictive control algorithm, obtaining the motor torque at the next moment according to the current information;
judging whether the robot collides currently or not according to the motor torque at the next moment and the preset torque.
In a second aspect, an embodiment of the present application provides a collision detection apparatus, the apparatus including:
the acquisition module is used for acquiring current information of each joint of the robot, wherein the current information comprises current state information and motor torque serving as current control quantity;
the calculation module is used for obtaining the motor torque at the next moment according to the current information based on a model predictive control algorithm;
and the judging module is used for judging whether the robot is in collision currently or not according to the motor torque at the next moment and the preset torque.
In a third aspect, an embodiment of the present application provides a control apparatus, including a processor and a memory, where the memory stores machine executable instructions executable by the processor, and the processor may execute the machine executable instructions to implement the collision detection method described in the foregoing embodiment.
In a fourth aspect, embodiments of the present application provide a readable storage medium having stored thereon a computer program which, when executed by a processor, implements a collision detection method as described in the foregoing embodiments.
The collision detection method, the collision detection device, the control equipment and the readable storage medium provided by the embodiment of the application firstly acquire current information of each joint of the robot, wherein the current information comprises current state information and motor torque serving as current control quantity; then, calculating to obtain the motor torque at the next moment according to the current information by using a model predictive control algorithm; and finally, judging whether the robot collides currently or not according to the preset torque and the motor torque at the next moment. Therefore, the driving moment of the joint is obtained through rolling circulation optimization, the driving moment of the joint changes with the state of the robot at any time and any place, the driving moment of the joint at each moment is the optimal solution in the prediction period, and the collision detection is carried out on the motor torque at the next moment obtained based on the model prediction control algorithm, so that the whole motion process of the robot is stable, and the false detection is reduced.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the embodiments will be briefly described below, it being understood that the following drawings only illustrate some embodiments of the present invention and therefore should not be considered as limiting the scope, and other related drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a schematic block diagram of a control device according to an embodiment of the present application;
fig. 2 is a schematic flow chart of a collision detection method according to an embodiment of the present application;
FIG. 3 is a second flow chart of a collision detection method according to the embodiment of the present disclosure;
FIG. 4 is one of the flow charts of the sub-steps included in step S150 of FIG. 3;
FIG. 5 is a schematic diagram of a control device in communication with a robot within the provision of an embodiment of the present application;
FIG. 6 is a second flowchart illustrating the sub-steps included in the step S150 of FIG. 3;
FIG. 7 is a block schematic diagram of a collision detection apparatus according to an embodiment of the present disclosure;
fig. 8 is a second schematic block diagram of a collision detection apparatus according to an embodiment of the present disclosure.
Icon: 100-control device; 110-memory; a 120-processor; 130-a communication unit; 200-collision detection means; 210-an acquisition module; 220-a calculation module; 230-a judging module; 240-shutdown control module.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention. The components of the embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations.
Thus, the following detailed description of the embodiments of the present application, as provided in the accompanying drawings, is not intended to limit the scope of the application, as claimed, but is merely representative of selected embodiments of the application. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present application without making any inventive effort, are intended to be within the scope of the present application.
It is noted that relational terms such as "first" and "second", and the like, are 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. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
The following collision detection means are generally known in the art.
Means one: the force sensor is added at the tail end of the mechanical arm, so that the collision force of the tail end of the hand grip can be accurately detected, but other parts of the mechanical arm cannot be detected, so that the detection range is limited.
Means II: the collision is detected by sensing skin, and in the method, sensing is covered on the whole body of the robot, for example, a small stress sensor is distributed on the whole body of the robot, so that collision at any position can be detected. But has the disadvantages that: the wiring is complex, the anti-interference capability is poor, the operation amount of a processor is greatly increased, and the system cost is very high. The poor anti-interference capability means that the sensing covers the whole body of the robot, is sensitive, and is easy to recognize disturbance as collision.
Means III: and obtaining the current or the feedback moment of the motor, performing inverse kinematics calculation based on the current or the moment, and comparing the obtained calculated current or moment with the current or the moment obtained previously, and if abrupt change occurs, considering collision. The collision detection mode is poor in accuracy, so that the robot shake phenomenon can occur in the actual operation process, and the operation is unstable.
Aiming at the problems, the embodiment of the application provides a collision detection method, a device, control equipment and a readable storage medium, wherein motor torque and state information of each joint of a mechanical arm are collected, collision detection is judged in a model prediction control mode, an external sensor is not required to be additionally added, the cost of an additional detection device is not increased, and meanwhile, the detection range is large. The joint driving moment used in collision detection is obtained through rolling circulation optimization, the joint driving moment changes with the state of the robot at any time and any place, the joint driving moment at each moment is an optimal solution in a prediction period, the whole motion process of the robot is stable, and false detection can be reduced.
Some embodiments of the present application are described in detail below with reference to the accompanying drawings. The following embodiments and features of the embodiments may be combined with each other without conflict.
Referring to fig. 1, fig. 1 is a block schematic diagram of a control apparatus 100 according to an embodiment of the present application. The control device 100 may be, but is not limited to, a computer, a control box, or the like. The control apparatus 100 may analyze whether a collision occurs to the robot and control the robot to stop when it is determined that the collision occurs. The control device 100 and the robot may be two independent devices but are connected in communication, or may be one device integrated together, for example, the control device 100 is a control unit in the robot. The control device 100 may include a memory 110, a processor 120, and a communication unit 130. The memory 110, the processor 120, and the communication unit 130 are electrically connected directly or indirectly to each other to realize data transmission or interaction. For example, the components may be electrically connected to each other via one or more communication buses or signal lines.
Wherein the memory 110 is used for storing programs or data. The Memory 110 may be, but is not limited to, random access Memory (Random Access Memory, RAM), read Only Memory (ROM), programmable Read Only Memory (Programmable Read-Only Memory, PROM), erasable Read Only Memory (Erasable Programmable Read-Only Memory, EPROM), electrically erasable Read Only Memory (Electric Erasable Programmable Read-Only Memory, EEPROM), etc.
The processor 120 is used to read/write data or programs stored in the memory 110 and perform corresponding functions. For example, the memory 110 stores the collision detecting device 200, and the collision detecting device 200 includes at least one software functional module that may be stored in the memory 110 in the form of software or firmware (firmware). The processor 120 executes various functional applications and data processing, i.e., implements the collision detection method in the embodiments of the present application, by running software programs and modules stored in the memory 110, such as the collision detection apparatus 200 in the embodiments of the present application.
The communication unit 130 is used for establishing a communication connection between the control device 100 and other communication terminals through a network, and for transceiving data through the network.
It should be understood that the structure shown in fig. 1 is merely a schematic structural diagram of the control apparatus 100, and that the control apparatus 100 may further include more or fewer components than those shown in fig. 1, or have a different configuration from that shown in fig. 1. The components shown in fig. 1 may be implemented in hardware, software, or a combination thereof.
Referring to fig. 2, fig. 2 is a flow chart of a collision detection method according to an embodiment of the present disclosure. The method may be applied to the control apparatus 100 described above. The specific flow of the collision detection method is explained in detail below. In this embodiment, the method may further include steps S110 to S130.
Step S110, current information of each joint of the robot is obtained.
In this embodiment, current information of each joint of the mechanical arm of the robot may be collected. The robotic arm may be a lightweight robotic arm. For example, if the arm of the robot includes 6 joints, current information of each of the 6 joints may be acquired. The current information may include current state information and motor torque as a current control amount. The current state information may include speed information indicating a current speed of a motor of the joint and position information of the joint. During acquisition, the speed of the joint or the rotation speed of the motor of the joint can be used as the speed information, and the current speed of the motor of the joint can be determined subsequently based on the speed information. The position information of the joint indicates the angular position of the motor of the joint, i.e. the current motor rotation angle.
It will be appreciated that the motor torque that is the current control amount is collected as the control amount at the previous time.
And step S120, based on a model predictive control algorithm, obtaining the motor torque at the next moment according to the current information.
And step S130, judging whether the robot is in collision currently or not according to the motor torque at the next moment and the preset torque.
In the present embodiment, collision detection of the robot is determined by means of model predictive control (Model Predictive Control, MPC). MPC control is actually a time-dependent control that uses the current state of the system and the current control quantity to implement predictive control over the future state of the system. The control quantity can be known in real time in the process of controlling the robot by the control quantity, so that certain collision safety performance is ensured. According to the working environment of the robot, a threshold value can be set as a comparison between the preset torque and the control quantity so as to judge whether the robot collides with the surrounding environment.
The control amount in the MPC is obtained in the following manner.
Firstly, establishing a basic dynamics model of the robot:
wherein τ represents the moment of the rotating part, M (q) represents the inertial amount of the rotating part,g (q) represents the gravity term compensation of the rotation part, q represents the rotation angle position of the rotation part, & lt/EN & gt>Indicating the rotational angular velocity of the rotational part, +.>The rotational angular acceleration of the rotational part is indicated.
The basic dynamics model is nonlinear, and a prediction model which can be used by the MPC is built based on the basic dynamics model in a linearization and discretization mode:
Y＝Fξ(k)+ΦΔU
where Y is the output matrix (i.e., the position matrix), F and Φ are coefficient matrices,
Δu is a control amount matrix, Δu= [ Δu (k) Δu (k+1) & Δu (k+np-1)] T 。
And xi is the state quantity matrix,in the above formula, k represents the current time, < +.>Representing a measurement matrix->Representing a system matrix->Representing the input matrix, np representing the prediction horizon (i.e. the number of prediction steps), and +.>Representing the current position and speed of the input, +.>Indicating the amount of control input.
And constructing a cost function aiming at the prediction model, solving the control quantity in a quadratic programming mode, solving according to the state of the current moment and the past moment of the robot each time, solving a series of future control quantity data, and executing (namely performing collision judgment) by taking the first term of the solution as the control quantity of the next moment to obtain the actual control quantity of the joint at each running moment, wherein the control quantity prediction value of the whole preset track can be obtained through circular rolling optimization. That is, the control amount at the next time is predicted continuously by using the current information of each joint throughout the movement phase of the robot, and then collision judgment is performed based on the control amount at the next time. The cost function may be constructed in conjunction with actual demands, for example, with a minimum rate of change of the control quantity.
And taking the first term in the control quantity matrix obtained by using the prediction model as the motor torque at the next moment. Whether the motor torque at the next moment is larger than a preset torque or not can be judged, and if so, the current collision of the robot can be determined; if the collision is not greater than the preset threshold, the fact that the robot does not collide at present can be determined, and normal operation can be continued.
It should be noted that, if the robot includes a plurality of joints, a motor torque at a next moment of each of the plurality of joints is obtained, and when the motor torque at the next moment of at least one joint of the plurality of joints is greater than the preset torque, it may be determined that the robot is currently crashed. And when the motor torque of each joint at the next moment is not larger than the preset torque, determining that the robot is not collided currently.
Therefore, by introducing the MPC control theory, the whole motion process of the robot is stable, compared with PID algorithm control, the shake phenomenon of the robot is obviously reduced, and collision is not detected by mistake.
In case of collision, the robot should be controlled to stop to ensure safety. However, when the prior art detects that the collision is stopped, there is few reliable methods for controlling the stopping, generally, the stopping is directly performed, and in this case, the whole machine may be impacted, so that the whole machine is damaged. In the embodiment, whether deceleration control is performed first and then shutdown is performed is determined according to actual conditions, so that impact is reduced, reliable shutdown is realized,
referring to fig. 3, fig. 3 is a second flowchart of a collision detection method according to an embodiment of the present disclosure. In this embodiment, the current state information includes speed information for indicating a current speed of a motor of a joint, and in case it is determined that the robot collides, the method may further include steps S140 to S160.
Step S140, for each joint, judging whether the joint needs to be decelerated and stopped according to the current speed and the first preset speed of the joint.
In this embodiment, the first preset speed may be set according to the load inertia, or may be set based on other factors, and may specifically be set in combination with an actual requirement. And judging whether the current speed of the motor of each joint is greater than a first preset speed or not according to each joint, and if so, determining that the joint needs to be decelerated and stopped. If not, it can be determined that the joint does not need to be decelerated and shut down. For example, the robot arm of the robot has 6 joints, and whether the joints need to be decelerated and stopped or not can be judged according to the current speed and the first preset speed of each joint.
It should be noted that, in the case that the robot includes a plurality of joints, if the speeds of the plurality of joints are different, it is possible that some joints need to be stopped at a reduced speed, and some joints do not need to be stopped at a reduced speed.
If the joint needs to be stopped at a reduced speed, step S150 is executed. In the case where the joint does not need to be decelerated and stopped, step S160 is performed.
Step S150, controlling the joint to decelerate, and controlling the joint to stop after decelerating.
Step S160, controlling the joint to stop.
The specific processing manners for controlling the joint shutdown in step S150 and step S150 may be the same.
As a possible implementation manner, the same deceleration acceleration can be directly adopted, so that each joint needing to be decelerated is controlled to decelerate, and whether deceleration is completed is continuously judged in the deceleration process. If it is determined that deceleration is complete, deceleration may be stopped and the joint controlled to stop.
The faster the joint speed, the greater the impact on the whole machine during shutdown, and the slower the speed reduction, the lower the speed can not be shut down at a low speed in time. Therefore, the shutdown level can be selected according to the actual demand trade-off. Different stopping levels correspond to different deceleration accelerations, which correspond to different speed ranges. The higher the speed range, the greater the corresponding deceleration acceleration, i.e., the faster the speed range the greater the corresponding deceleration acceleration of the slower the speed range.
As another possible implementation, the deceleration may be performed by the method shown in fig. 4. Referring to fig. 4, fig. 4 is a schematic flow chart of the sub-steps included in step S150 in fig. 3. In this embodiment, step S150 may include sub-steps S151 to S152.
And step S151, determining the target deceleration acceleration corresponding to the current speed of the joint according to the corresponding relation between different speed ranges and different deceleration accelerations.
And step S152, decelerating according to the target deceleration acceleration until the decelerated speed reaches a second preset speed.
In this embodiment, for a joint that needs to be decelerated and stopped, a speed range in which the current speed of the joint is located may be determined according to the current speed indicated by the speed information of the joint and different speed ranges. And determining the deceleration acceleration corresponding to the speed range in which the current speed of the joint is positioned according to the corresponding relation between the different speed ranges and the different deceleration accelerations, and taking the determined deceleration acceleration as the target deceleration acceleration corresponding to the joint.
The motor deceleration of the joint may then be controlled in accordance with the target deceleration acceleration. And continuously judging whether the speed reaches a second preset speed or not in the deceleration process. If not, continuing to control the motor of the joint to decelerate according to the target deceleration acceleration; if so, it can be determined that the deceleration is completed and the deceleration can be stopped.
When the speed is lower than a certain value, the direct stop can not impact the whole machine, but the speed reduction time can be shortened, so that the minimum speed (namely the speed at which the machine can be directly stopped after the speed reduction is completed) can be reduced according to the load inertia. It will be understood, of course, that the second preset speed may be set based on other requirements, and the specific setting manner of the second preset speed is not specifically limited herein.
Alternatively, the control device and the robot may be two independent devices as shown in fig. 5, where the safety circuit board shown in fig. 5 is the control device. The robot comprises 6 drivers, each driver comprises a main control CPU, a motor and the like, and the main control CPU can control the working state of the motor. The safety loop board comprises a control CPU1, and the control CPU1 is connected with a main control CPU in each driver through an EtherCAT bus. The control CPU1 can perform collision analysis, can perform deceleration analysis under the condition of determining collision, and can send a deceleration instruction to a corresponding main control CPU of the motor through an EtherCAT bus for each motor which needs to be decelerated and stopped so as to execute the deceleration instruction through the main control CPU to realize deceleration of the motor.
Alternatively, the control CPU2 or the control CPU3 in the safety circuit board may perform collision analysis, and the result of the collision analysis may be transmitted to the control CPU1, and the control CPU1 may perform stop control when it is determined that a collision has occurred.
For example, if the control CPU1 determines that the motor M1 of the driver 1 needs to be decelerated and stopped, and the corresponding target deceleration acceleration is a, the control CPU1 may send a deceleration instruction to the main control CPU of the driver 1 through the EtherCAT bus, where the deceleration instruction may include the target deceleration acceleration a; the main control CPU of the driver 1 can control the motor to decelerate according to the target deceleration acceleration a. Alternatively, the control CPU1 may determine whether the deceleration is completed, or the main control CPU of the driver 1 may determine whether the deceleration is completed, which may be specifically set in combination with the actual requirement.
Alternatively, as a possible implementation, the joint shutdown may be directly controlled after the motor has completed decelerating.
Alternatively, as another possible implementation, the shutdown may also be performed in the manner shown in fig. 6. Referring to fig. 6, fig. 6 is a second schematic flow chart of the sub-steps included in step S150 in fig. 3. In this embodiment, step S150 may further include sub-steps S153 to S155.
Substep S153, after stopping the deceleration, determines whether the joint needs to be operated in reverse.
In case it is determined that reverse run is required, sub-steps S154 to S155 may be performed. In case it is determined that no reverse run is required, sub-step S155 may be performed.
Substep S154, controlling the joint to reversely run a preset distance.
Substep S155, controlling the joint to stop.
In the present embodiment, after the deceleration is completed, the following processing is performed for each joint requiring deceleration stop: judging whether the joint needs to rebound, controlling the rebound of the joint by a preset distance when the rebound is determined to be needed, and stopping the machine; and directly stopping when the rebound is not required. The continuous pressure on the impacted object can be released by reverse rebound after impact, and the specific value of the preset rebound distance can be set according to actual requirements. The starting point of calculating the rebound distance is the angle position when the motor starts to rebound.
Wherein, optionally, whether the reverse motion is needed can be judged according to the actual requirement. For example, deformation information of the collision part of the robot can be obtained, and whether the robot needs to run reversely is judged according to the deformation information; alternatively, it may be determined directly that reverse operation is required for securing safety. The corresponding preset distances can be the same or different when different joints rebound, and the actual requirements can be set after the joints are particularly saved.
Further damage to the object or other items can be avoided by performing a controlled shut down (quick shut down or reverse rebound, etc.) when a dangerous situation occurs (collision or contact with the object surface).
The joint may be controlled to stop in the event that reverse motion is completed or reverse operation is not required. Optionally, each joint corresponds to a driving unit, and the motor of each joint can be disabled by sending a shutdown control instruction to the driving unit corresponding to each joint, so that the robot is in a safe state. The stop control instruction is used for cutting off power output and/or locking the motor by the band-type brake.
As shown in fig. 5, in the case that the joint shutdown needs to be controlled, the control CPU1 may send a shutdown control instruction to the main control CPU of the driver 1-6 through the EtherCAT bus, so as to cut off the power output and lock the motor by the band-type brake. The driver of fig. 5 is a driving unit.
In order to further ensure the shutdown reliability, each driving unit may further include a switch, the number of which may be 1 or more, through which the power supply supplies power to the motor. When a plurality of switches are included in the driving unit, the plurality of switches may be arranged in series. It is also possible to disconnect the switch in the drive unit after disabling the motor to stop the power supply to the motor.
As a possible implementation, each driving unit includes a plurality of switches connected in series, and the power supply supplies power to the motors in the driving units through a plurality of joints connected in series. The specific number of the plurality of switches may be determined in connection with the actual demand, for example, 2. After the motor is disabled by sending the shutdown control command, it may also be determined whether to shut down the power supply, i.e., whether to shut down the total power. The specific judging mode of whether to cut off the power supply can be set according to the actual requirement, for example, the power supply can be directly determined to be cut off, or the power supply can be determined to be cut off in the occasion with higher requirements on safety requirements. In the case where it is necessary to determine the power supply, a plurality of switches in each driving unit may be controlled to be turned off for each driving unit so that the power supply stops supplying power to the motor. Thus, the robot is controlled to be switched to a safe state through a plurality of paths.
As shown in fig. 5, the security circuit board further includes a control CPU2 and a control CPU3, and the control CPU1 is communicatively connected to the control CPU2 and the control CPU 3. The main control CPU, the digital switch H1, the digital switch H2, the 6 MOS tubes and the motor M1 are sequentially connected in a communication mode in each driver, wherein the digital switch H1 is connected with the digital switch H2 in series. Hard-wire control is adopted between the control CPU2 and the digital switch H2 of each driver, hard-wire control is adopted between the control CPU3 and the digital switch H2 of each driver, and two hard-wire control are independent channels. The control CPU1 can send a power-off instruction to the control CPU2 and the control CPU3, the control CPU2 can control the digital switch H2 in the control driver 1-6 to be disconnected through hard connection, and the control CPU3 can control the digital switch H1 in the control driver 1-6 to be disconnected through hard connection, so that the safety control of the power source is realized through two independent channels, and the safety protection is enhanced.
In this embodiment, when an abnormality is detected, the EtherCAT bus communication can be used to ensure that each main control CPU is controlled to start a safety stop flow, and the safety control of the power source is realized through different hard-wired control channels, so that the robot is ensured to switch to a safety state through a plurality of channels. Therefore, through the redundant design of the control instruction, 1-path control failure can be avoided, and safety stop cannot be performed. Compared with a common mode of controlling the shutdown through a single loop, the method and the device adopt reliable transmission of the multipath control instructions to shutdown, and can reliably transmit the control instructions related to the shutdown to the execution mechanism to perform controlled shutdown, so that reliable shutdown is ensured.
It should be noted that the specific processing manners of the above substep S155 and the step S160 may be the same, that is, the power output is cut off and/or the band-type brake locks the motor, and the power is cut off when the power is required to be cut off.
In order to perform the corresponding steps in the above embodiments and the various possible ways, an implementation of the collision detecting apparatus 200 is given below, and alternatively, the collision detecting apparatus 200 may employ the device structure of the control device 100 shown in fig. 1 and described above. Further, referring to fig. 7, fig. 7 is a block diagram of a collision detection apparatus 200 according to an embodiment of the present disclosure. It should be noted that, the basic principle and the technical effects of the collision detecting device 200 provided in this embodiment are the same as those of the above embodiment, and for brevity, reference should be made to the corresponding contents of the above embodiment. The collision detecting device 200 may include: the device comprises an acquisition module 210, a calculation module 220 and a judgment module 230.
The acquisition module 210 is configured to obtain current information of each joint of the robot. Wherein the current information includes current state information and motor torque as a current control amount.
The calculation module 220 is configured to obtain the motor torque at the next moment according to the current information based on a model predictive control algorithm.
The judging module 230 is configured to judge whether the robot collides currently according to the motor torque at the next moment and the preset torque.
Referring to fig. 8, fig. 8 is a second schematic block diagram of a collision detection apparatus 200 according to an embodiment of the present disclosure. In this embodiment, the current state information includes speed information for indicating a current speed of the motor of the joint, and the collision detecting apparatus 200 may further include a shutdown control module 240. In the event that a collision is determined, the shutdown control module 240 is configured to: judging whether each joint needs to be decelerated and stopped according to the current speed and the first preset speed of the joint; under the condition that the joint is required to be decelerated and stopped, controlling the joint to be decelerated and then stopping the joint; and controlling the joint to stop under the condition that the deceleration stop is not needed.
Alternatively, the above modules may be stored in the memory 110 shown in fig. 1 or solidified in an Operating System (OS) of the control device 100 in the form of software or Firmware (Firmware), and may be executed by the processor 120 in fig. 1. Meanwhile, data, codes of programs, and the like, which are required to execute the above-described modules, may be stored in the memory 110.
The embodiment of the application also provides a readable storage medium, on which a computer program is stored, which when executed by a processor, implements the collision detection method.
In summary, the embodiments of the present application provide a collision detection method, apparatus, control device, and readable storage medium, where current information of each joint of a robot is collected first, where the current information includes current state information and motor torque as a current control amount; then, calculating to obtain the motor torque at the next moment according to the current information by using a model predictive control algorithm; and finally, judging whether the robot collides currently or not according to the preset torque and the motor torque at the next moment. Therefore, the driving moment of the joint is obtained through rolling circulation optimization, the driving moment of the joint changes with the state of the robot at any time and any place, the driving moment of the joint at each moment is the optimal solution in the prediction period, and the collision detection is carried out on the motor torque at the next moment obtained based on the model prediction control algorithm, so that the whole motion process of the robot is stable, and the false detection is reduced.
In the several embodiments provided in this application, it should be understood that the disclosed apparatus and method may be implemented in other manners as well. The apparatus embodiments described above are merely illustrative, for example, flow diagrams and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present application. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
In addition, the functional modules in the embodiments of the present application may be integrated together to form a single part, or each module may exist alone, or two or more modules may be integrated to form a single part.
The functions, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored in a computer-readable storage medium. Based on such understanding, the technical solution of the present application may be embodied essentially or in a part contributing to the prior art or in a part of the technical solution, in the form of a software product stored in a storage medium, including several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
The foregoing is merely an alternative embodiment of the present application and is not intended to limit the present application, and various modifications and variations may be made to the present application by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application should be included in the protection scope of the present application.
Claims (8)
1. A collision detection method, the method comprising:
obtaining current information of each joint of the robot, wherein the current information comprises current state information and motor torque serving as current control quantity;
based on a model predictive control algorithm, obtaining the motor torque of each joint at the next moment according to the current information of each joint;
judging whether the robot collides currently according to the motor torque and the preset torque at the next moment of each joint, wherein the preset torque is a value set according to the working environment of the robot, and judging whether the robot collides comprises the following steps: determining that the robot generates collision when the motor torque at the next moment of at least one joint is larger than the preset torque, and determining that the robot does not collide when the motor torque at the next moment of each joint is smaller than or equal to the preset torque;
the current state information comprises speed information and position information of the joint, the speed information is used for indicating the current speed of a motor of the joint, and under the condition that collision is determined, the method further comprises:
judging whether the joint needs to be decelerated and stopped according to the current speed and the first preset speed of each joint, wherein the deceleration and stopping are determined to be needed when the current speed is greater than the first preset speed, and the deceleration and stopping are determined to be not needed when the current speed is not greater than the first preset speed;
under the condition that the joint is required to be decelerated and stopped, controlling the joint to be decelerated and then stopping the joint;
and controlling the joint to stop under the condition that the deceleration stop is not needed.
2. The method of claim 1, wherein controlling the joint deceleration in the event that a deceleration shutdown is required comprises:
according to the corresponding relation between different speed ranges and different deceleration accelerations, determining a target deceleration acceleration corresponding to the current speed of the joint, wherein the higher the speed range is, the larger the corresponding deceleration acceleration is;
and decelerating according to the target deceleration acceleration until the decelerated speed reaches a second preset speed.
3. The method of claim 2, wherein controlling the joint shutdown after deceleration comprises:
after stopping decelerating, judging whether the joint needs to run reversely;
controlling the joint to reversely run for a preset distance under the condition that the joint is determined to reversely run;
after the reverse run is completed, the joint is controlled to stop.
4. A method according to any one of claims 1-3, wherein each joint corresponds to a drive unit, said controlling the joint to stop comprising:
and sending a shutdown control instruction to a driving unit corresponding to each joint so as to enable the motor of the joint, wherein the shutdown control instruction is used for cutting off power output and/or locking the motor of the band-type brake.
5. The method of claim 4, wherein each of the drive units includes a plurality of switches in series, a power source supplying power to a motor in the drive unit through the plurality of switches in series, the controlling the joint shutdown further comprising:
judging whether to cut off the power supply;
in the case where it is determined that the power supply is to be cut off, the plurality of switches in the driving unit are controlled to be turned off for each driving unit so that the power supply stops supplying power to the motor.
6. A collision detection apparatus, characterized in that the apparatus comprises:
the acquisition module is used for acquiring current information of each joint of the robot, wherein the current information comprises current state information and motor torque serving as current control quantity;
the calculation module is used for obtaining the motor torque of each joint at the next moment according to the current information of each joint based on a model predictive control algorithm;
the judging module is used for judging whether the robot is crashed currently according to the motor torque and the preset torque at the next moment of each joint, wherein the preset torque is a value set according to the working environment of the robot, and the judging mode of judging whether the robot crashes comprises the following steps: determining that the robot generates collision when the motor torque at the next moment of at least one joint is larger than the preset torque, and determining that the robot does not collide when the motor torque at the next moment of each joint is smaller than or equal to the preset torque;
the device comprises a motor, a motor control module, a control module and a control module, wherein the current state information comprises speed information and joint position information, the speed information is used for indicating the current speed of the motor of the joint, and the control module is used for controlling the motor to stop when collision is determined to happen:
judging whether the joint needs to be decelerated and stopped according to the current speed and the first preset speed of each joint, wherein the deceleration and stopping are determined to be needed when the current speed is greater than the first preset speed, and the deceleration and stopping are determined to be not needed when the current speed is not greater than the first preset speed;
under the condition that the joint is required to be decelerated and stopped, controlling the joint to be decelerated and then stopping the joint;
and controlling the joint to stop under the condition that the deceleration stop is not needed.
7. A control apparatus comprising a processor and a memory, the memory storing machine executable instructions executable by the processor to implement the collision detection method of any one of claims 1-5.
8. A readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, implements the collision detection method according to any one of claims 1-5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211030065.1A CN115389077B (en) | 2022-08-26 | 2022-08-26 | Collision detection method, collision detection device, control apparatus, and readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211030065.1A CN115389077B (en) | 2022-08-26 | 2022-08-26 | Collision detection method, collision detection device, control apparatus, and readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115389077A CN115389077A (en) | 2022-11-25 |
| CN115389077B true CN115389077B (en) | 2024-04-12 |
Family
ID=84121732
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211030065.1A Active CN115389077B (en) | 2022-08-26 | 2022-08-26 | Collision detection method, collision detection device, control apparatus, and readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115389077B (en) |
Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1046470A2 (en) * | 1999-03-26 | 2000-10-25 | Fanuc Ltd | Industrial robot with means for detecting collision and preventing re-collision |
| JP2009090462A (en) * | 2003-07-29 | 2009-04-30 | Panasonic Corp | Robot control method and control device |
| CN103192413A (en) * | 2012-01-06 | 2013-07-10 | 沈阳新松机器人自动化股份有限公司 | Sensor-free robot crash detecting and preventing device and method |
| CN105082160A (en) * | 2015-09-07 | 2015-11-25 | 四川大学 | Mobile robot operation system having anti-collision function |
| WO2017141577A1 (en) * | 2016-02-15 | 2017-08-24 | オムロン株式会社 | Impact prediction device, impact prediction system, control device, impact prediction method, and impact prediction program |
| CN108000521A (en) * | 2017-12-06 | 2018-05-08 | 天津大学 | One kind is without sensor type cooperation robot collision checking method |
| EP3351356A1 (en) * | 2015-09-16 | 2018-07-25 | Panasonic Intellectual Property Management Co., Ltd. | Robot collision detection method |
| CN110303521A (en) * | 2018-03-27 | 2019-10-08 | 清华大学 | Joint of robot torque signals acquisition system and method |
| CN110509274A (en) * | 2019-08-26 | 2019-11-29 | 中科新松有限公司 | A kind of robot security's control system |
| CN110587665A (en) * | 2019-09-02 | 2019-12-20 | 埃夫特智能装备股份有限公司 | Industrial robot joint collision protection method |
| WO2021008969A1 (en) * | 2019-07-12 | 2021-01-21 | Franka Emika Gmbh | Collision detection for a robot manipulator |
| JP2021036390A (en) * | 2019-08-30 | 2021-03-04 | 国立大学法人京都大学 | Nonlinear model prediction control device |
| CN113021340A (en) * | 2021-03-17 | 2021-06-25 | 华中科技大学鄂州工业技术研究院 | Robot control method, device, equipment and computer readable storage medium |
| CN114193454A (en) * | 2021-12-31 | 2022-03-18 | 佛山智能装备技术研究院 | Collision response control method, equipment and medium |
| CN114310895A (en) * | 2021-12-31 | 2022-04-12 | 达闼科技（北京）有限公司 | Robot collision detection method, device, electronic device and storage medium |
| CN114474076A (en) * | 2022-03-28 | 2022-05-13 | 法奥意威(苏州)机器人系统有限公司 | Robot collision detection method, device, detection equipment and readable storage medium |
| CN114603599A (en) * | 2020-12-08 | 2022-06-10 | 山东新松工业软件研究院股份有限公司 | Robot collision detection method and device, computer equipment and storage medium |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102015200355B3 (en) * | 2015-01-02 | 2016-01-28 | Siemens Aktiengesellschaft | A medical robotic device with collision detection and method for collision detection of a medical robotic device |
| KR102370879B1 (en) * | 2017-09-12 | 2022-03-07 | 주식회사 한화 | Method and Apparatus for controlling a collaborativve robot |
| CN109940622B (en) * | 2019-04-28 | 2020-10-27 | 西安交通大学 | Non-sensing collision detection method for robot mechanical arm based on motor current |
-
2022
- 2022-08-26 CN CN202211030065.1A patent/CN115389077B/en active Active
Patent Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1046470A2 (en) * | 1999-03-26 | 2000-10-25 | Fanuc Ltd | Industrial robot with means for detecting collision and preventing re-collision |
| JP2009090462A (en) * | 2003-07-29 | 2009-04-30 | Panasonic Corp | Robot control method and control device |
| CN103192413A (en) * | 2012-01-06 | 2013-07-10 | 沈阳新松机器人自动化股份有限公司 | Sensor-free robot crash detecting and preventing device and method |
| CN105082160A (en) * | 2015-09-07 | 2015-11-25 | 四川大学 | Mobile robot operation system having anti-collision function |
| EP3351356A1 (en) * | 2015-09-16 | 2018-07-25 | Panasonic Intellectual Property Management Co., Ltd. | Robot collision detection method |
| WO2017141577A1 (en) * | 2016-02-15 | 2017-08-24 | オムロン株式会社 | Impact prediction device, impact prediction system, control device, impact prediction method, and impact prediction program |
| CN108000521A (en) * | 2017-12-06 | 2018-05-08 | 天津大学 | One kind is without sensor type cooperation robot collision checking method |
| CN110303521A (en) * | 2018-03-27 | 2019-10-08 | 清华大学 | Joint of robot torque signals acquisition system and method |
| WO2021008969A1 (en) * | 2019-07-12 | 2021-01-21 | Franka Emika Gmbh | Collision detection for a robot manipulator |
| CN110509274A (en) * | 2019-08-26 | 2019-11-29 | 中科新松有限公司 | A kind of robot security's control system |
| JP2021036390A (en) * | 2019-08-30 | 2021-03-04 | 国立大学法人京都大学 | Nonlinear model prediction control device |
| CN110587665A (en) * | 2019-09-02 | 2019-12-20 | 埃夫特智能装备股份有限公司 | Industrial robot joint collision protection method |
| CN114603599A (en) * | 2020-12-08 | 2022-06-10 | 山东新松工业软件研究院股份有限公司 | Robot collision detection method and device, computer equipment and storage medium |
| CN113021340A (en) * | 2021-03-17 | 2021-06-25 | 华中科技大学鄂州工业技术研究院 | Robot control method, device, equipment and computer readable storage medium |
| CN114193454A (en) * | 2021-12-31 | 2022-03-18 | 佛山智能装备技术研究院 | Collision response control method, equipment and medium |
| CN114310895A (en) * | 2021-12-31 | 2022-04-12 | 达闼科技（北京）有限公司 | Robot collision detection method, device, electronic device and storage medium |
| CN114474076A (en) * | 2022-03-28 | 2022-05-13 | 法奥意威(苏州)机器人系统有限公司 | Robot collision detection method, device, detection equipment and readable storage medium |
Non-Patent Citations (4)
| Title |
|---|
| earning-Based Real-Time Detection of Robot Collisions Without Joint Torque Sensors;Kyu Min Park 等;IEEE Robotics and Automation Letters;20210131;第第6卷卷(第第1期期);第103 - 110页 * |
| 基于动量偏差观测器的机器人碰撞检测算法;吴海彬 等;电机与控制学报;20150531;第19卷(第05期);第101-108页 * |
| 基于模型预测控制的磨削机器人末端力跟踪控制算法;刘哲 等;山东大学学报(工学版);20181231(第01期);第46-53页 * |
| 无外部传感器的机器人碰撞检测;马志举 等;测试技术学报;20130630;第27卷(第03期);第76-81页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115389077A (en) | 2022-11-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9724827B2 (en) | Monitoring a kinematically redundant robot | |
| US11247702B2 (en) | Vehicle control device and electronic control system | |
| US11609567B2 (en) | Apparatus and method for controlling vehicle based on redundant architecture | |
| EP1477284B1 (en) | Drive control method and drive controller | |
| US20090200978A1 (en) | Robot controller having component protecting function and robot control method | |
| US10599104B2 (en) | Control device and motor control system | |
| CN111665849B (en) | Automatic driving system | |
| CN113799123B (en) | Manipulator anti-collision control method and system, intelligent terminal and storage medium | |
| CN115389077B (en) | Collision detection method, collision detection device, control apparatus, and readable storage medium | |
| CN111891274A (en) | Balance car control method and device and storage medium | |
| CN114589694A (en) | Robot collision detection threshold updating method and device and storage medium | |
| CN110194212A (en) | Steering controller | |
| CN114467245A (en) | Programmable electronic power regulator | |
| US9381967B2 (en) | Inverted vehicle and its control method | |
| Buchholz et al. | Towards adaptive worker assistance in monitoring tasks | |
| US20220266452A1 (en) | Robot collision detection device and method thereof | |
| RU2432297C1 (en) | Method of fault-tolerant automatic control of navigation | |
| CN117297791A (en) | Control method of mechanical arm, medical system and computer equipment | |
| JP4969662B2 (en) | Vehicle control device | |
| JPH06329042A (en) | Rear wheel steering system | |
| JP4577607B2 (en) | Robot control device and robot system | |
| KR101194563B1 (en) | System and method for motion control in network | |
| JPH11245191A (en) | Method and device for controlling driving shaft of industrial robot | |
| CN111638731A (en) | Steering engine, control method thereof and readable storage medium | |
| CN116276972A (en) | Safety protection method and system for robot and computer readable storage medium |
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 |