CN117207197A - Mechanical arm safety boundary control method and system - Google Patents
Mechanical arm safety boundary control method and system Download PDFInfo
- Publication number
- CN117207197A CN117207197A CN202311399660.7A CN202311399660A CN117207197A CN 117207197 A CN117207197 A CN 117207197A CN 202311399660 A CN202311399660 A CN 202311399660A CN 117207197 A CN117207197 A CN 117207197A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- safety
- motion
- motion signal
- signal
- 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.)
- Pending
Links
Landscapes
- Manipulator (AREA)
Abstract
The invention provides a method and a system for controlling a safety boundary of a mechanical arm, wherein the control method comprises the following steps: the IMU sensor acquires a first motion signal of the mechanical arm, the encoder acquires a second motion signal of the mechanical arm, and the vision positioning tracking system acquires a third motion signal of the mechanical arm; acquiring a safety boundary of the mechanical arm; planning the track of the mechanical arm according to the first motion signal, the second motion signal, the third motion signal and the safety boundary of the mechanical arm to obtain a safety warning area and a critical motion state T1 of the corresponding mechanical arm; acquiring a real-time motion state Ti of the mechanical arm at the moment T, and judging that the mechanical arm exceeds a safety warning area when Ti is more than or equal to T1; and acquiring a feedforward instruction according to a second real-time motion signal at the t moment acquired by the IMU sensor, and controlling the mechanical arm according to the feedforward instruction. The safety boundary control method provided by the invention can solve the problem that the mechanical arm easily exceeds the safety boundary in the control process in the prior art.
Description
Technical Field
The invention relates to the field of mechanical arms, in particular to a mechanical arm safety boundary control method and system.
Background
With the development of artificial intelligence technology, various robots are increasingly used in the fields of medical treatment, industrial manufacturing and the like, and mechanical arms are core components of the robots, so that people can complete various complicated and fine operations of the robots through the operation of the mechanical arms, such as operations of surgical operations, intelligent manufacturing and the like. Among them, how to realize the precise control of the mechanical arm is one of the most critical core technologies in the development of the robot field.
In the field of surgical robots, the surgical robots generally comprise a control trolley, a visual positioning tracking system and a mechanical arm, wherein the visual positioning tracking system can track the position of the mechanical arm, in addition, an encoder is further arranged at a joint of the mechanical arm and can feed back the position and the motion state of the mechanical arm, and the control trolley can control the position and the motion state of the mechanical arm according to pose signals transmitted by the visual positioning tracking system and position and motion state signals fed back by the mechanical arm encoder, so that the control of the mechanical arm is realized.
When the mechanical arm exceeds the safety area, the mechanical arm possibly collides with a patient or other medical equipment to cause injury of personnel or damage of the medical equipment, and the like. Therefore, safety margin control for the robot is an important part of the robot control process. In the aspect of the control of the safety boundary of the mechanical arm, the control method can cause the problem that the mechanical arm exceeds a safety zone due to control delay, pose signal deviation and the like. Firstly, the visual positioning tracking system is mainly used for tracking the position of the mechanical arm based on optical tracking, when shielding exists between the visual positioning tracking system and the mechanical arm, the visual positioning tracking system cannot track the position of the mechanical arm in real time, so that the pose signal of the mechanical arm fed back by the visual positioning tracking system has deviation due to the problem of shielding in visual positioning, the control of the mechanical arm is finally deviated, and the mechanical arm possibly exceeds a safety boundary area in the control process. In addition, as the encoder of the mechanical arm is arranged at the joint of the mechanical arm, the wear at the joint is unavoidable along with the longer and longer service time of the mechanical arm, and the encoder is also worn, so that errors exist in the position and motion state signals fed back by the encoder at the joint of the mechanical arm along with the time, and finally, the deviation can also occur to the control of other machines. Moreover, as the bandwidth of the mechanical arm encoder is not high, certain delay exists in the feedback position and motion state signals, so that the control of the control trolley on the mechanical arm is delayed to a certain extent, and the mechanical arm is caused to exceed a safety area.
It should be noted that the information disclosed in this background section is only for enhancement of understanding of the general background of the invention and should not be taken as an acknowledgement or any form of suggestion that this information forms the prior art already known to a person skilled in the art.
Disclosure of Invention
The invention aims to provide a method and a system for controlling a safety boundary of a mechanical arm, which are used for solving the problem that the mechanical arm easily exceeds the safety boundary in the control process in the prior art.
In order to solve the above technical problems, the present invention provides a method for controlling a safety boundary of a mechanical arm, including:
s1: the method comprises the steps that an IMU sensor arranged at a joint of the mechanical arm collects first motion signals of the mechanical arm, an encoder arranged at the joint of the mechanical arm collects second motion signals of the mechanical arm, and a visual positioning tracking system collects third motion signals of the mechanical arm;
s2: acquiring a safety boundary of a mechanical table;
s3: planning the track of the mechanical arm according to the first motion signal, the second motion signal, the third motion signal and the safety boundary of the mechanical arm to obtain a safety warning area and a critical motion state T1 of the corresponding mechanical arm;
s4: acquiring a real-time motion state Ti of the mechanical arm at the moment T, and judging that the mechanical arm exceeds a safety warning area when Ti is more than or equal to T1;
s5: when the mechanical arm exceeds the safety warning area, a feedforward instruction is acquired according to a second real-time motion signal at the t moment acquired by the IMU sensor, and the mechanical arm is controlled according to the feedforward instruction.
Further, in the step S2, the first motion signal includes an acceleration signal and/or an angle signal of the mechanical arm, the second motion signal includes a second pose signal of the mechanical arm, and the third motion signal includes a third pose signal of the mechanical arm.
Further, the S1 specifically includes:
establishing an IMU coordinate system, a mechanical arm coordinate system and a visual coordinate system, and normalizing the three coordinate systems to obtain a coordinate transformation matrix;
and acquiring a safety boundary of the mechanical arm under a visual coordinate system according to the set movement region of the mechanical arm and the coordinate transformation matrix.
Further, the step S3 includes:
s31: acquiring a motion state T0 of the mechanical arm under a visual coordinate system according to the first motion signal, the second motion signal, the third motion signal and the coordinate conversion matrix;
s32: and planning the track of the mechanical arm according to the motion state T0 of the mechanical arm under the visual coordinate system and the safety boundary to obtain a safety warning area and a corresponding critical motion state T1 of the mechanical arm.
Further, in S32, a fifth order polynomial interpolation method is used to plan the trajectory of the mechanical arm.
Further, the critical motion state T1 of the mechanical arm includes at least one critical position in the safety guard area, and a critical speed and/or a critical acceleration value corresponding to the critical position of the mechanical arm.
Further, when Ti is more than or equal to T1, the real-time speed or the real-time acceleration value corresponding to the mechanical arm moving to the critical position at the moment T is more than or equal to the critical speed or the critical acceleration value corresponding to the critical position of the mechanical arm.
Further, the second real-time motion signal includes an acceleration signal of the mechanical arm, and in S32, the feedforward command is obtained according to a current acceleration of the mechanical arm.
Further, the second real-time motion signal includes an angle signal of the mechanical arm, and in S32, a current speed of the mechanical arm is obtained according to a current angle of the mechanical arm, and the feedforward command is obtained according to the current speed.
The invention also provides a mechanical arm safety boundary control system, which comprises:
the IMU sensor is arranged at a joint of the mechanical arm and is configured to acquire a first motion signal of the mechanical arm;
the encoder is arranged at a joint of the mechanical arm and is configured to acquire a second motion signal of the mechanical arm;
a visual positioning tracking system configured to acquire a third motion signal of the robotic arm;
a driver in communication with the IMU sensor and the encoder, configured to read the first motion signal and the second motion signal;
the control module is in communication connection with the IMU sensor, the encoder, the mechanical arm, the visual positioning tracking system and the driver;
the control module is further configured to: planning the track of the mechanical arm according to the first motion signal, the second motion signal, the third motion signal and the safety boundary region of the mechanical arm, acquiring a critical motion state T1 of the mechanical arm corresponding to the safety guard region, acquiring a feedforward instruction according to a second real-time motion signal at the time T acquired by an IMU sensor when the mechanical arm moves beyond the safety guard region, and controlling the mechanical arm according to the feedforward instruction.
In summary, compared with the prior art, the mechanical arm safety boundary control method and system provided by the invention have the following advantages:
according to the invention, the IMU sensor is arranged at the joint of the mechanical arm to acquire the motion parameters of the mechanical arm in real time, and the bandwidth of the IMU sensor is higher than that of the encoder, so that the motion parameters of the mechanical arm can be fed back to the control module more quickly, and the control delay is effectively reduced.
And the safety warning area planning is carried out by utilizing the data collected by the IMU sensor, the encoder and the visual positioning tracking system, and when the mechanical arm exceeds the safety warning area, an early warning signal is sent out, so that the control module can regulate and control the mechanical arm in advance, and the overshoot of the movement caused by the overshoot can be effectively restrained, the mechanical arm is prevented from unexpected entering an unsafe area, and the safety and the reliability of the system are improved.
In addition, in the scheme of the invention, when the mechanical arm exceeds a safety warning area, the low-delay data acquired by the IMU sensor is utilized for feedforward control, so that the response speed of the system can be further improved, the control delay of the mechanical arm is reduced, and the motion overshoot caused by the overshoot of the mechanical arm is further restrained. Moreover, the data collected by the IMU sensor is not blocked by vision, the driving error is not influenced, the condition that the data is inaccurate due to abrasion of an encoder is avoided, and the regulation and control of the mechanical arm can be more accurate.
Furthermore, in the mechanical arm safety boundary control method, the acceleration signal acquired by the IMU is directly used as the input of feedforward control, so that the mechanical arm safety boundary control method has two obvious advantages, namely, the acceleration signal is directly acquired, the calculation time of a system is shortened, and the response speed of the system is improved; secondly, the acceleration signal is introduced into the control (current loop) of the inner ring of the mechanical arm control system, so that the response speed of the mechanical arm system can be improved, the mechanical arm is controlled to be away from a safety boundary, and the operation safety is ensured.
Drawings
FIG. 1 is a schematic diagram of a method for controlling a safety margin of a robot according to an embodiment of the present invention;
FIG. 2 is a flow chart of a method for controlling a safety margin of a robot according to an embodiment of the present invention;
FIG. 3 is a diagram showing a correspondence between a visual positioning tracking system and a robot in a robot safety boundary control system according to an embodiment of the present invention;
FIG. 4 is a schematic diagram of an IMU sensor disposed on a mechanical arm according to an embodiment of the present invention;
FIG. 5 is a schematic view of a positioning target disposed on a mechanical arm according to an embodiment of the present invention;
FIG. 6 is a schematic diagram of a safety boundary of a robot in a method for controlling a safety boundary of a robot according to an embodiment of the present invention;
FIG. 7 is a control scheme diagram of an embodiment of the present invention employing acceleration as a feed forward command;
fig. 8 is a control pattern diagram of an embodiment of the present invention using a speed signal as an input.
Wherein, the system is a 10-vision positioning and tracking system; 20-a mechanical arm; a 21-IMU sensor; 22-localization of the target.
Detailed Description
The following describes the method and system for controlling the safety boundary of the mechanical arm according to the present invention in further detail with reference to the accompanying drawings and detailed description. The advantages and features of the present invention will become more apparent from the following description.
It should be noted that the drawings are in a very simplified form and are all to a non-precise scale, merely for the purpose of facilitating and clearly aiding in the description of embodiments of the invention. For a better understanding of the invention with objects, features and advantages, refer to the drawings. It should be understood that the structures, proportions, sizes, etc. shown in the drawings are for illustration purposes only and should not be construed as limiting the invention to the extent that any modifications, changes in the proportions, or adjustments of the sizes of structures, proportions, or otherwise, used in the practice of the invention, are included in the spirit and scope of the invention which is otherwise, without departing from the spirit or essential characteristics thereof.
The invention aims to provide a method and a system for controlling a safety boundary of a mechanical arm, which are used for solving the problem that the mechanical arm easily exceeds the safety boundary in the control process in the prior art.
Fig. 1 is a schematic diagram of a method for controlling a safety boundary of a mechanical arm according to the present invention, and fig. 2 is a flow diagram of a method for controlling a safety boundary of a mechanical arm, where the method for controlling a safety boundary of a mechanical arm according to the present invention specifically includes the following steps:
s1: as shown in fig. 3, the IMU sensor 21 mounted at the joint of the mechanical arm 20 collects a first motion signal of the mechanical arm, and an encoder (not shown) mounted at the joint of the mechanical arm 20 collects a second motion signal of the mechanical arm 20, and the visual positioning tracking system 10 collects a third motion signal of the mechanical arm 20. An encoder is generally installed at a joint of the mechanical arm 20, where the encoder may collect a pose signal of the mechanical arm 20, for convenience of description, the pose signal collected by the encoder is defined herein as a second motion signal, where the second motion signal includes a second pose signal of the mechanical arm 20 (for convenience of mapping the motion signal with the pose signal to avoid confusion in a later description, where a number of the pose signal corresponding to the second motion signal is directly defined as the second pose signal, and in the scheme of the present invention, the first pose signal is not present), where the second pose signal reflects pose information of the mechanical arm 20 recorded by the encoder of the mechanical arm 20 relative to an initial state, and the encoder may feed back the second motion signal to the control module to monitor a motion state of the mechanical arm 20. In addition, the visual positioning and tracking system 10 also tracks the mechanical arm 20 in real time, acquires a third motion signal of the mechanical arm 20, where the third motion signal includes a third pose signal of the mechanical arm 20, reflects a pose state of the mechanical arm 20 under a visual coordinate system, and includes a position distance between the mechanical arm 20 and a surrounding object, for example, a pose state of the mechanical arm 20 relative to a patient or other targets, and information such as a velocity signal of the mechanical arm obtained according to image information acquired by the visual positioning and tracking system 10, and the visual positioning and tracking system 10 may feed back the third pose signal to the control module. In the solution of the present invention, the motion signal of the mechanical arm 20 is also acquired in real time by the IMU sensor 21 disposed at the joint of the mechanical arm 20, where the motion signal may be defined as a first motion signal, and the first motion signal may include an acceleration signal and/or an angle signal of the mechanical arm 20, for example, the acceleration signal of the mechanical arm 20 may be acquired by using an acceleration sensor, or the angle signal of the mechanical arm 20 may be acquired by using an angular velocity sensor or the like. The first motion signal acquired by the IMU sensor 21 may be fed back to the control module.
S2: acquiring a safety boundary of the mechanical arm; when the mechanical arm works, the normal operation range of the mechanical arm is defined according to the actual operation flow, and the range is the safety boundary of the mechanical arm. The safety margin of the manipulator is composed of two parts, including a static safety margin and a dynamic safety margin, the static safety margin is generally given according to a pre-operation plan of a CT image, as shown in fig. 3 to 6, taking a surgical robot as an example, a surgical requirement space is set according to clinical surgical requirements, for example, the set surgical requirement space is a cube space of 300 x 300mm centered on a patient, the end of the manipulator 20 of the surgical robot drives a surgical instrument to move in the cube space to perform a surgical operation, that is, the spatial coordinate range of the end of the manipulator 20 under the visual coordinate system of the visual positioning tracking system 10 is (x, y, z) = (±150mm ), and the position shown by the dashed manipulator in fig. 6 is the static safety margin of the manipulator. When the end of the robotic arm 20 is beyond this range, it may collide with other medical equipment or medical operators, resulting in equipment damage or personnel injury. Therefore, the mechanical arm must be ensured to move within the static safety boundary so as to ensure the normal operation. Generally, the static safety boundary of the mechanical arm is defined according to the actual situation before working, but the static safety boundary is not completely unchanged after setting, but can be updated according to the actual situation, for example, during the operation, when the patient moves,the static safety margin of the robot 20 may also be reprogrammed. In addition to the static safety boundary, the safety boundary of the mechanical arm also comprises a dynamic safety boundary part, wherein the dynamic safety boundary part mainly comprises the real-time motion state of the mechanical arm, the dynamic safety boundary reflects the current inertia of the mechanical arm, the faster the current mechanical arm moves, the longer the time required for representing the mechanical arm to go from the motion state to the static state is, the higher the possibility that the mechanical arm exceeds the limit is, and the part is an important reference index for subsequently setting a warning area. Specifically, the motion state of the mechanical arm corresponding to the dynamic safety boundary may include, for example, a current real-time speed and acceleration of the mechanical arm, and the portion may be calculated according to the first motion signal, the second motion signal, and/or the third motion signal acquired in the step S1. Thus, the safety boundary P of the robotic arm can be characterized as: p=k 1 *P s +k 2 *V i 。
Wherein P represents the safety boundary of the mechanical arm, ps represents the static safety boundary of the mechanical arm planned before operation, vi is the current real-time speed signal of the mechanical arm (the current real-time speed of the mechanical arm can be the current real-time speed of the mechanical arm or the current real-time acceleration of the mechanical arm can be included), and k1 and k2 are constant boundary coefficients. Specifically, V i ＝C 1 *V coder +C 2 *V Vision sense +C 3 *V imu The method comprises the steps of carrying out a first treatment on the surface of the Wherein C1, C2 and C3 are sensor coefficients, V coder ，V Vision sense ，V imu The speed signals of the mechanical arm obtained by the encoder, the speed signals of the mechanical arm obtained by the visual positioning tracking system and the speed signals of the mechanical arm obtained by the IMU sensor are represented respectively. One innovation of the invention is that the speed signal of the mechanical arm is acquired by the IMU sensor, the performance is obviously superior to that of the encoder due to low bandwidth delay of the IMU sensor, and when the visual positioning tracking system is shielded or the encoder is out of alignment due to abrasion, the dynamic safety boundary part can be accurately acquired by the signal acquired by the IMU sensor.
It should be understood by those skilled in the art that the manipulator of the present invention is not limited to the manipulator of the surgical robot, but may be a manipulator of other fields, for example, a manipulator in an intelligent manufacturing factory, etc., i.e., the safety margin control method of the present invention may be applied to any field where the safety margin control of the manipulator is required.
S3: planning the track of the mechanical arm 20 according to the first motion signal, the second motion signal, the third motion signal and the safety boundary of the mechanical arm 20, and obtaining a safety guard region of the mechanical arm to obtain a critical motion state T1 of the mechanical arm corresponding to the safety guard region of the mechanical arm. After the three motion signals are collected, the current motion state of the mechanical arm 20 can be determined, the track of the mechanical arm 20 is planned by the safety boundary, the safety guard area of the mechanical arm 20 is set, and the safety guard area of the mechanical arm 20 comprises a static position area and a dynamic critical speed part of the corresponding mechanical arm 20. The safety alert area of the robotic arm 20 is represented by: when the mechanical arm 20 reaches the position of the safety guard area, if the movement speed of the mechanical arm 20 is greater than the critical speed set by the safety guard area, if no additional intervention is performed, the mechanical arm 20 continues to move according to the original trend, and the mechanical arm 20 eventually exceeds the safety boundary, so that the safety guard area is set in advance to know whether the mechanical arm 20 has a risk exceeding the safety boundary in advance, thereby facilitating the subsequent intervention on the movement of the mechanical arm 20 and preventing the movement from exceeding the safety boundary. The setting of the safety guard area of the mechanical arm 20 is related to the current motion state of the mechanical arm 20, the mass of the mechanical arm 20 and the signal response speed of the control system, when the mechanical arm 20 moves faster, the mass of the mechanical arm 20 is larger, the signal response speed of the control system is slower, which means that the mechanical arm 20 is less prone to reduce the speed from the current motion state and is easier to exceed the safety area, the static position area of the safety guard area is set smaller, and the position of the safety guard area is far away from the safety boundary, so that a large enough response space is reserved for slowing down the mechanical arm. It should be noted that the safety guard area of the arm 20 is set according to the current real-time motion state and safety boundary of the arm 20, and thus is not completely fixed, and may also change when the motion state of the arm 20 changes. In the solution of the present invention, the motion state of the mechanical arm 20 generally includes the current position of the mechanical arm 20 and the current speed and/or acceleration value of the mechanical arm 20, and the critical motion state T1 of the mechanical arm 20 corresponding to the safety guard area may be the critical speed of the mechanical arm 20, or the acceleration value, or both the speed value and the acceleration value. It should be noted that the safety guard area of the mechanical arm 20 may include a plurality of critical positions and the critical movement states of the mechanical arm 20 corresponding to the critical positions at the same time, that is, a plurality of critical positions may be selected in the safety guard area of the mechanical arm 20, each critical position corresponds to a critical speed of the mechanical arm 20, so that a set including the critical positions and the corresponding critical speeds is formed, when the movement states of the mechanical arm 20 are monitored later, whether the mechanical arm 20 exceeds the safety guard area may be determined in advance according to the need, that is, as long as the mechanical arm 20 moves to a corresponding critical position later, it is found that the real movement speed is greater than the critical speed corresponding to the critical position, and then the mechanical arm 20 is determined to have a risk exceeding the safety guard area.
S4: and acquiring a real-time motion state Ti of the mechanical arm at the moment T, comparing the motion state Ti with a threshold state T2, and judging that the mechanical arm 20 exceeds a safety guard area when Ti is more than or equal to T2, wherein T2 is less than or equal to T1. When the safety guard area is determined, the subsequent movement state of the mechanical arm 20 can be monitored, and when the mechanical arm is found to exceed the safety guard area, the mechanical arm is found to be required to interfere with the movement of the mechanical arm 20, so that the mechanical arm is prevented from exceeding the safety boundary. However, since there may be a certain error in the real-time speed of the mechanical arm acquired by the sensor and a certain delay in the control of the mechanical arm 20 by the control system, if the critical speed corresponding to the safety guard area is completely used as the standard for intervention and regulation, the situation of control delay may occur, which leads to the mechanical arm 20 exceeding the safety boundary, and therefore, a threshold state T2 may be set, where the speed or acceleration value of the mechanical arm 20 is less than or equal to the critical speed or acceleration value of the mechanical arm 20 corresponding to the safety guard area in the threshold state T2. The real-time motion state Ti of the mechanical arm 20 can be compared with the threshold state T2, when Ti is greater than or equal to T2, the corresponding speed or acceleration value of the mechanical arm 20 at the position of the moment T is equal to or greater than the corresponding speed or acceleration value of the mechanical arm 20 at the position of the threshold state, and at this moment, it is determined that the mechanical arm 20 exceeds the safety guard area at the moment T, and the motion of the mechanical arm 20 needs to be regulated and controlled to prevent the mechanical arm 20 from exceeding the safety boundary.
S5: when the mechanical arm 20 exceeds the safety guard area, a feedforward instruction is acquired according to a second real-time motion signal at the time t acquired by the IMU sensor 21, and the mechanical arm 20 is controlled according to the feedforward instruction. In the scheme of the invention, when the mechanical arm 20 is detected to exceed the safety guard area, the motion state of the mechanical arm 20 is regulated to prevent the mechanical arm 20 from exceeding the safety boundary in the subsequent motion process, specifically, a feedforward command is generated according to the second real-time motion signal acquired by the IMU sensor 21, the control module controls the mechanical arm 20 by using the feedforward command, for example, the IMU sensor 21 acquires the current motion parameter of the mechanical arm 20 in real time, when the mechanical arm 20 moves beyond the safety guard area, the current motion parameter is indicated to cause the mechanical arm 20 to move too fast, at the moment, the control module generates a feedforward command according to the current motion parameter of the mechanical arm 20, and controls the motion speed of the mechanical arm 20 to slow down by the feedforward command, so that the mechanical arm 20 is prevented from exceeding the safety boundary in advance.
In the scheme of the invention, the IMU sensor 21 is arranged at the joint of the mechanical arm 20 to acquire the motion parameters of the mechanical arm in real time, and the bandwidth of the IMU sensor 21 is higher than that of the encoder, so that the motion parameters of the mechanical arm 20 can be fed back to the control module more quickly, and the control delay is effectively reduced. And, utilize IMU sensor 21, encoder and vision positioning tracking system 10 data that gathers to carry out the planning of safety warning area, when arm 20 surpassed the safety warning area, send early warning signal promptly, let control module regulate and control arm 20 in advance, can effectively restrain the motion overshoot that the overshoot caused like this, avoid arm 20 unexpected entering unsafe area, improved the security and the reliability of system. In addition, in the scheme of the invention, when the mechanical arm 20 exceeds the safety guard region, the low-delay data acquired by the IMU sensor 21 is utilized for feedforward control, so that the response speed of the system can be further improved, the control delay of the mechanical arm 20 is reduced, and the motion overshoot caused by the overshoot of the mechanical arm 20 is further restrained. Moreover, the data collected by the IMU sensor 21 is not blocked by vision, the driving error is not influenced, the condition that the data is inaccurate due to abrasion of an encoder is avoided, and the control of the mechanical arm 20 can be more accurate.
Further, since the IMU sensor 21, the encoder of the mechanical arm, and the data collected by the visual positioning tracking system 10 are not in the same coordinate system, in order to facilitate calculation, reduce the calculation time of the mechanical arm control system, shorten the response time of the system, normalize the collected data of three motion signals, specifically, in step S1, an IMU coordinate system, a mechanical arm coordinate system, and a visual coordinate system may be established first, and the three coordinate systems may be normalized to obtain a coordinate transformation matrix; and then, acquiring the safety boundary of the mechanical arm under the visual coordinate system according to the set movement region of the mechanical arm and the coordinate transformation matrix. And, the motion parameters collected by the subsequent IMU sensor and the encoder can be converted into a visual coordinate system according to the coordinate conversion matrix. For example, as shown in fig. 5, the positioning target 22 may be disposed on a positioning feature point of the robot arm 20, may be an end of the robot arm 20, or may be another position of the robot arm 20. Then, the visual positioning tracking system 10 is used for tracking the positioning target 22, so that the position of the mechanical arm 20 under the visual coordinate system can be obtained, and the position relationship between the IMU sensor 21 and the mechanical arm 20 and the position relationship between the IMU sensor 21 and the positioning target 22 are determined because the IMU sensor 21 is arranged at the joint of the mechanical arm 20, so that the position of the IMU sensor 21 under the visual coordinate system can be obtained.
In this embodiment, during assembly, the axis of the IMU sensor 21 may be coincident with the axis of rotation of the mechanical arm 20, so that the Z-axis of the IMU coordinate system and the mechanical arm coordinate system may be coincident. When the coordinate normalization processing is carried out, multiple groups of gesture data of the mechanical arm under the visual coordinate system can be acquiredMultiple sets of pose data +.>Then, solving an equation by using a least square method Solving IMU coordinate system->In the visual coordinate system->Is provided.
Further, the step S3 includes:
s31: acquiring a motion state T0 of the mechanical arm 20 under a visual coordinate system according to the first motion signal, the second motion signal, the third motion signal and the coordinate conversion matrix; the motion signals collected by the IMU sensor 21 and the encoder can be converted into motion signals of the tail end of the mechanical arm according to a dynamic model of the mechanical arm 20, a jacobian matrix and the like, and then converted into real-time pose and speed of the mechanical arm 20 under a visual coordinate system through a coordinate conversion matrix.
S32: and planning the track of the mechanical arm 20 according to the motion state T0 of the mechanical arm 20 under the visual coordinate system and the safety boundary, and obtaining a safety warning area and a corresponding critical motion state T1 of the mechanical arm 20. Specifically, the position of the tail end of the mechanical arm can be determined according to a set safety boundary, then the inverse operation is performed through a homogeneous transformation matrix, and the angles alpha= [ alpha 1, alpha 2, alpha 3, alpha 4, ], alpha i, of rotation of each joint of the mechanical arm in the safety boundary range under a visual coordinate system are solved]Then, the current rotation angle of the mechanical arm 20 is obtained according to the real-time pose, the real-time speed and the like of the mechanical arm 20 under the visual coordinate systemAnd then the mechanical arm is converted into a mechanical arm coordinate system through matrix transformation, and then the mechanical arm track planning is carried out, for example, the track of the mechanical arm can be planned by adopting a five-time polynomial interpolation method. At this time, the safety guard region can be obtained by trajectory planning of the mechanical arm 20 in combination with parameters (such as quality) of the mechanical arm 20 and response time of a control system, and speed information of the mechanical arm in a critical motion state is corresponding to a critical position corresponding to the safety guard region, including positions θ (t) = [ θ 1, θ 2, θ 3, θ 4, …, θi of the mechanical arm]And corresponding speedAnd/or acceleration-> When the current real-time speed or acceleration of the mechanical arm 20 corresponding to the corresponding critical position exceeds the threshold range during the subsequent actual movement of the mechanical arm 20, it represents that the mechanical arm 20 exceeds the safety guard region, and at this time, the movement state of the mechanical arm 20 needs to be adjusted in an intervening manner, otherwise, the mechanical arm 20 may exceed the safety boundary.
As an implementation manner of the present invention, the second real-time motion signal includes an acceleration signal of the mechanical arm 20, and in S32, the feedforward command is acquired according to a current acceleration of the mechanical arm 20. Specifically, after the mechanical arm 20 exceeds the safety guard area, the current acceleration signal of the mechanical arm 20 acquired by the IMU sensor 21 can be directly used as a feedforward command, and the control module generates a control command according to the acceleration value corresponding to the current acceleration and the threshold state T2 and transmits the control command to the mechanical arm, so that the acceleration of the mechanical arm is reduced below the acceleration value corresponding to the threshold state T2, the mechanical arm 20 is decelerated in advance, the mechanical arm 20 is far away from the safety guard area, the mechanical arm 20 is prevented from exceeding the safety boundary, and smooth operation is ensured. The control mode is shown in fig. 7, and comprises inner ring control and outer ring control, wherein the mechanical arm basic hardware system is formed by the motor electrical links and the motor mechanical links, the inner ring control (namely current ring control) of the control mode is formed by the current calibration and the motor electrical links and the current sensor, the output current of each phase of the motor is detected by the current sensor (such as an electromagnetic current sensor, an electronic current sensor and the like) in the mechanical arm movement process through the ring, and the PID adjustment is performed by the current correction link responsible for feedback, so that the output current is as close as possible to the set current. The speed calibration, the motor mechanical link, the speed sensor and the inner ring form the outer ring control (namely the speed ring control) of the control mode together, the feedback PID regulation is carried out by the detected signal of the servo motor encoder, and the inner ring PID output of the feedback PID regulation is the setting of the current ring directly. The whole control purpose ensures that the mechanical arm accurately moves the required position. When the mechanical arm moves into the safety guard area, the IMU acceleration signal is used as the input of the inner ring control of the mechanical arm to form a new outer ring control mode of the mechanical arm, and the mechanical arm is controlled to be far away from the safety guard area or stopped moving to the safety guard area. The acceleration signal acquired by the IMU is directly used as the input of feedforward control, so that the method has two obvious advantages, namely, the acceleration signal is directly acquired, the calculation time of the system is shortened, and the response speed of the system is improved; secondly, the acceleration signal is introduced into the control (current loop) of the inner ring of the mechanical arm control system, so that the response speed of the mechanical arm system can be improved, the mechanical arm is controlled to be away from a safety boundary, and the operation safety is ensured.
In another embodiment of the present invention, the second motion signal includes an angle signal of the mechanical arm, and in S32, a speed of the mechanical arm is obtained according to a current angle of the mechanical arm, and the feedforward command is obtained according to the speed. Specifically, when the mechanical arm exceeds the safety guard area, the driver reads an angle signal of the mechanical arm collected by the IMU and transmits the angle signal to the control module, the control module can obtain a speed value of the mechanical arm, which is generally an angular speed, by utilizing the angle signal, and then the speed signal is led into the control system, and the control module generates a feedforward instruction based on the speed signal to control the mechanical arm. The control mode is shown in fig. 8, and the control mode also comprises an inner loop control and an outer loop control, the inner loop control is basically the same as the above description, the current calibration and motor electrical links and the current sensor are the inner loop control (namely the current loop control) of the control mode, the output current of the driver to each phase of the motor in the movement process of the mechanical arm is detected by the current sensor (such as an electromagnetic current sensor, an electronic current sensor and the like), and the PID adjustment is carried out on the current correction link responsible for feedback, so that the output current is as close to the set current as possible. The speed calibration, the motor mechanical link, the speed sensor and the inner ring form the outer ring control (namely the speed ring control) of the control mode together, the feedback PID adjustment is carried out by the detected signal of the servo motor encoder, and the inner ring PID output of the feedback PID adjustment is the setting of the current ring. The whole control purpose ensures that the mechanical arm accurately moves the required position. When the mechanical arm moves beyond the safety guard area, a difference signal between the speed signal and a speed value corresponding to a threshold value state T2 is input as an outer ring of the mechanical arm control mode, and a new mechanical arm outer ring control mode is formed, so that the running speed of the mechanical arm is reduced, the closer to the boundary, the faster the speed is reduced until the speed is reduced to 0, and the robot is ensured to always run in the safety area.
The invention also provides a mechanical arm safety boundary control system, which comprises:
an IMU sensor 21, the IMU sensor 21 being disposed at a joint of the mechanical arm 20 and configured to acquire a first motion signal of the mechanical arm 20 in real time;
an encoder disposed at a joint of the mechanical arm 20 and configured to acquire a second motion signal of the mechanical arm 20 in real time;
a visual positioning tracking system 10 configured to acquire a third motion signal of the robotic arm 20 in real time;
a driver communicatively coupled to the IMU sensor 21 and the encoder, configured to read the first motion signal and the second motion signal; the data collected by the IMU sensor 21 is directly read by the driver and transmitted to the control module through Ethcart (or can be transmitted through can, etc.), so that the delay of data transmission can be greatly reduced.
A control module in communication with the IMU sensor 21, the encoder, the robotic arm 20, the visual positioning tracking system 10, and the driver;
the control module is further configured to: planning the track of the mechanical arm according to the first motion signal, the second motion signal, the third motion signal and the safety boundary area of the mechanical arm, acquiring a safety warning area in real time, acquiring a feedforward instruction according to the second motion signal acquired in real time when the mechanical arm moves beyond the safety warning area, and controlling the mechanical arm according to the feedforward instruction.
Based on the same inventive concept, an embodiment of the present invention further provides an electronic device, where the electronic device includes a processor and a memory, and the memory stores a computer program, and when the computer program is executed by the processor, the method for controlling a safety boundary of a mechanical arm is implemented. Because the electronic device provided by the invention and the mechanical arm safety boundary control method provided by the invention belong to the same inventive concept, the electronic device provided by the invention has at least all the beneficial effects of the mechanical arm safety boundary control method provided by the invention, and the description of the beneficial effects of the mechanical arm safety boundary control method provided by the invention can be referred to, so that the beneficial effects of the electronic device provided by the invention are not repeated here.
The present invention also provides a readable storage medium having stored therein a computer program which, when executed by a processor, can implement the robot arm safety boundary control method described above. Since the readable storage medium provided by the present invention and the mechanical arm safety boundary control method provided by the present invention belong to the same inventive concept, the readable storage medium provided by the present invention has all the beneficial effects of the mechanical arm safety boundary control method provided by the present invention, and the description of the beneficial effects of the mechanical arm safety boundary control method provided by the present invention can be referred to above, so that the beneficial effects of the readable storage medium provided by the present invention will not be repeated herein.
In summary, compared with the prior art, the mechanical arm safety boundary control method and system provided by the invention have the following advantages:
according to the invention, the IMU sensor is arranged at the joint of the mechanical arm to acquire the motion parameters of the mechanical arm in real time, and the bandwidth of the IMU sensor is higher than that of the encoder, so that the motion parameters of the mechanical arm can be fed back to the control module more quickly, and the control delay is effectively reduced.
And the safety warning area planning is carried out by utilizing the data collected by the IMU sensor, the encoder and the visual positioning tracking system, and when the mechanical arm exceeds the safety warning area, an early warning signal is sent out, so that the control module can regulate and control the mechanical arm in advance, and the overshoot of the movement caused by the overshoot can be effectively restrained, the mechanical arm is prevented from unexpected entering an unsafe area, and the safety and the reliability of the system are improved.
In addition, in the scheme of the invention, when the mechanical arm exceeds a safety warning area, the low-delay data acquired by the IMU sensor is utilized for feedforward control, so that the response speed of the system can be further improved, the control delay of the mechanical arm is reduced, and the motion overshoot caused by the overshoot of the mechanical arm is further restrained. Moreover, the data collected by the IMU sensor is not blocked by vision, the driving error is not influenced, the condition that the data is inaccurate due to abrasion of an encoder is avoided, and the regulation and control of the mechanical arm can be more accurate.
Furthermore, in the mechanical arm safety boundary control method, the acceleration signal acquired by the IMU is directly used as the input of feedforward control, so that the mechanical arm safety boundary control method has two obvious advantages, namely, the acceleration signal is directly acquired, the calculation time of a system is shortened, and the response speed of the system is improved; secondly, the acceleration signal is introduced into the control (current loop) of the inner ring of the mechanical arm control system, so that the response speed of the mechanical arm system can be improved, the mechanical arm is controlled to be away from a safety boundary, and the operation safety is ensured.
The above description is only illustrative of the preferred embodiments of the present invention and is not intended to limit the scope of the present invention, and any changes and modifications made by those skilled in the art in light of the above disclosure are intended to fall within the scope of the appended claims. It will be apparent to those skilled in the art that various modifications and variations can be made to the present invention without departing from the spirit or scope of the invention. Thus, it is intended that the present invention also include such modifications and alterations insofar as they come within the scope of the appended claims or the equivalents thereof.
Claims (10)
1. The mechanical arm safety boundary control method is characterized by comprising the following steps of:
s1: the method comprises the steps that an IMU sensor arranged at a joint of the mechanical arm collects first motion signals of the mechanical arm, an encoder arranged at the joint of the mechanical arm collects second motion signals of the mechanical arm, and a visual positioning tracking system collects third motion signals of the mechanical arm;
s2: acquiring a safety boundary of the mechanical arm;
s3: planning the track of the mechanical arm according to the first motion signal, the second motion signal, the third motion signal and the safety boundary of the mechanical arm to obtain a safety warning area and a critical motion state T1 of the corresponding mechanical arm;
s4: acquiring a real-time motion state Ti of the mechanical arm at the moment T, and judging that the mechanical arm exceeds a safety warning area when Ti is more than or equal to T1;
s5: when the mechanical arm exceeds the safety warning area, a feedforward instruction is acquired according to a second real-time motion signal at the t moment acquired by the IMU sensor, and the mechanical arm is controlled according to the feedforward instruction.
2. The method according to claim 1, wherein in S1, the first motion signal includes an acceleration signal and/or an angle signal of the mechanical arm, the second motion signal includes a second pose signal of the mechanical arm, and the third motion signal includes a third pose signal of the mechanical arm.
3. The method for controlling a safety margin of a mechanical arm according to claim 1, wherein the step S2 specifically includes:
establishing an IMU coordinate system, a mechanical arm coordinate system and a visual coordinate system, and normalizing the three coordinate systems to obtain a coordinate transformation matrix;
and acquiring a safety boundary of the mechanical arm under a visual coordinate system according to the set movement region of the mechanical arm and the coordinate transformation matrix.
4. The mechanical arm safety boundary control method according to claim 3, wherein the S3 includes:
s31: acquiring a motion state T0 of the mechanical arm under a visual coordinate system according to the first motion signal, the second motion signal, the third motion signal and the coordinate conversion matrix;
s32: and planning the track of the mechanical arm according to the motion state T0 of the mechanical arm under the visual coordinate system and the safety boundary to obtain a safety warning area and a corresponding critical motion state T1 of the mechanical arm.
5. The method according to claim 4, wherein in S32, the trajectory of the robot is planned by using a fifth order polynomial interpolation method.
6. The method according to claim 4, wherein the critical motion state T1 of the mechanical arm includes at least one critical position in the safety guard area, and a critical velocity and/or a critical acceleration value corresponding to the critical position of the mechanical arm.
7. The method for controlling the safety boundary of the mechanical arm according to claim 6, wherein when Ti is greater than or equal to T1, the real-time speed or the real-time acceleration value corresponding to the mechanical arm moving to the critical position at the moment T is greater than or equal to the critical speed or the critical acceleration value corresponding to the mechanical arm at the critical position.
8. The method according to claim 1, wherein the second real-time motion signal includes an acceleration signal of the mechanical arm, and the feedforward command is obtained according to a current acceleration of the mechanical arm in S32.
9. The method according to claim 1, wherein the second real-time motion signal includes an angle signal of the mechanical arm, and the step S32 obtains a current speed of the mechanical arm according to a current angle of the mechanical arm, and obtains the feedforward command according to the current speed.
10. A robot arm safety margin control system, comprising:
the IMU sensor is arranged at a joint of the mechanical arm and is configured to acquire a first motion signal of the mechanical arm;
the encoder is arranged at a joint of the mechanical arm and is configured to acquire a second motion signal of the mechanical arm;
a visual positioning tracking system configured to acquire a third motion signal of the robotic arm;
a driver in communication with the IMU sensor and the encoder, configured to read the first motion signal and the second motion signal;
the control module is in communication connection with the IMU sensor, the encoder, the mechanical arm, the visual positioning tracking system and the driver;
the control module is further configured to: planning the track of the mechanical arm according to the first motion signal, the second motion signal, the third motion signal and the safety boundary region of the mechanical arm, acquiring a critical motion state T1 of the mechanical arm corresponding to the safety guard region, acquiring a feedforward instruction according to a second real-time motion signal at the time T acquired by an IMU sensor when the mechanical arm moves beyond the safety guard region, and controlling the mechanical arm according to the feedforward instruction.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311399660.7A CN117207197A (en) | 2023-10-26 | 2023-10-26 | Mechanical arm safety boundary control method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311399660.7A CN117207197A (en) | 2023-10-26 | 2023-10-26 | Mechanical arm safety boundary control method and system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117207197A true CN117207197A (en) | 2023-12-12 |
Family
ID=89035530
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311399660.7A Pending CN117207197A (en) | 2023-10-26 | 2023-10-26 | Mechanical arm safety boundary control method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117207197A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117842713A (en) * | 2023-12-28 | 2024-04-09 | 遨博（江苏）机器人有限公司 | Control method of collaborative robot in palletizing process |
-
2023
- 2023-10-26 CN CN202311399660.7A patent/CN117207197A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117842713A (en) * | 2023-12-28 | 2024-04-09 | 遨博（江苏）机器人有限公司 | Control method of collaborative robot in palletizing process |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11033343B2 (en) | Techniques for detecting errors or loss of accuracy in a surgical robotic system | |
| CN105082161B (en) | Binocular stereo camera Robot Visual Servoing control device and its application method | |
| US20110118872A1 (en) | Medical Robot And Method For Meeting The Performance Requirements Of A Medical Robot | |
| CN117207197A (en) | Mechanical arm safety boundary control method and system | |
| CN103568008A (en) | Robotic system and robot control device | |
| US11577384B2 (en) | Brake path monitoring of a kinematic | |
| TW202231428A (en) | Safety systems and methods employed in robot operations | |
| CN111283688B (en) | Obstacle avoidance method of robot and robot equipment | |
| US20220134567A1 (en) | Robot system, robot control device, and robot control program | |
| CN111300491A (en) | Human-robot cooperation safety system based on cooperative driving and controlling integrated robot | |
| CN117601134A (en) | Visual control method and device for measuring and correcting fusion of rope-driven agile arm | |
| CN118809605B (en) | A robotic arm for a six-degree-of-freedom visual grasping system and a self-calibration method thereof | |
| WO2020161910A1 (en) | Control device, control method, and recording medium | |
| CN117506937B (en) | Weldment autonomous placement method based on multi-stage visual servo control | |
| US11872704B2 (en) | Dynamic motion planning system | |
| Hu et al. | A flexible velocity planning algorithm for high-speed mounter with both efficiency and precision | |
| CA2909347C (en) | Safety monitoring for a serial kinematic structure | |
| CN115366115A (en) | Mechanical arm control method and control system thereof and computer readable storage medium | |
| Sugar et al. | Mobile robot interception using human navigational principles: Comparison of active versus passive tracking algorithms | |
| Meng et al. | An occlusion-free intraoperative active navigation system for orthopedic surgery | |
| CN115157251A (en) | Singularity protection method of robot and robot system | |
| CN115429432A (en) | Readable storage medium, surgical robot system and adjustment system | |
| CN119632688A (en) | Redundant joint follow-up obstacle avoidance method, controller and minimally invasive robot | |
| CN112720481A (en) | Mechanical arm minimum motion planning and control method based on abrupt degree | |
| WO2024050729A1 (en) | Robot teleoperation system and method |
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 |