Robotic arm safety boundary control method and system

Abstract

The application provides a mechanical arm safety boundary control method and system, the control method comprising: an IMU sensor collecting a first motion signal of the mechanical arm, an encoder collecting a second motion signal of the mechanical arm, and a visual positioning tracking system collecting a third motion signal of the mechanical arm; obtaining a safety boundary of the mechanical arm; planning a trajectory 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, obtaining a safety warning area and a corresponding critical motion state T1 of the mechanical arm; obtaining a real-time motion state Ti of the mechanical arm at time t, and when Ti is greater than or equal to T1, judging that the mechanical arm exceeds the safety warning area; obtaining a feedforward instruction according to the second real-time motion signal collected by the IMU sensor at time t, and controlling the mechanical arm according to the feedforward instruction. The safety boundary control method provided by the application can solve the problem that the mechanical arm is easy to exceed the safety boundary in the control process in the prior art.

Description

Technical Field

[0001] This invention relates to the field of robotic arms, and in particular to a method and system for controlling the safety boundaries of robotic arms. Background Technology

[0002] With the development of artificial intelligence technology, various robots are being used more and more widely in fields such as medical care and industrial manufacturing. The robotic arm is a core component of a robot, and people control it to enable the robot to perform various complex and precise operations, such as surgery and intelligent manufacturing. Among these, achieving precise control of the robotic arm is one of the most critical core technologies in the development of the robotics field.

[0003] In the field of surgical robots, a surgical robot generally includes a control cart, a visual positioning and tracking system, and a robotic arm. The visual positioning and tracking system can track the position of the robotic arm. In addition, encoders are installed at the joints of the robotic arm to provide feedback on the position and motion status of the robotic arm. The control cart can manipulate the posture and motion status of the robotic arm based on the pose signals transmitted by the visual positioning and tracking system and the position and motion status signals fed back by the robotic arm encoders, thereby achieving control of the robotic arm.

[0004] In general, surgical robots operate within a safe zone during surgery. The surgery can only proceed normally if the robotic arm operates within this safe zone. When the robotic arm exceeds this safe zone for various reasons, it may collide with the patient or other medical equipment, causing injury or damage. Once the robotic arm exceeds the safe zone, it must be shut down, manually moved back into the safe zone, and then restarted. This undoubtedly increases the number of interventions and the time required by the surgeon, affecting the smooth progress of the surgery. This is undoubtedly a type of robot malfunction. Therefore, controlling the safety boundary of the robotic arm is a crucial part of the robotic arm control process. However, the control method mentioned above can lead to the robotic arm exceeding the safe zone due to control delays and pose signal deviations. First, the visual positioning and tracking system primarily relies on optical tracking to track the robotic arm's position. When there is occlusion between the visual positioning and tracking system and the robotic arm, the system cannot track the arm's position in real time. This occlusion causes deviations in the pose signal of the robotic arm fed back by the visual positioning and tracking system, leading to errors in the final control of the robotic arm and potentially causing it to exceed the safety boundary during control. Second, since the encoders of the robotic arm are installed at the joints, wear and tear is inevitable over time, causing errors in the position and motion status signals fed back by the encoders at the joints. This also leads to deviations in the final control of the robotic arm. Furthermore, due to the limited bandwidth of the robotic arm encoders, there is a certain delay in the position and motion status signals fed back, causing a delay in the control of the robotic arm by the control vehicle, which can also cause the robotic arm to exceed the safety zone.

[0005] It should be noted that the information disclosed in the background section of this invention is intended only to enhance the understanding of the general background of this invention, and should not be construed as an admission or in any way implying that the information constitutes prior art known to those skilled in the art. Summary of the Invention

[0006] The purpose of this invention is to provide a method and system for controlling the safety boundaries of a robotic arm, so as to solve the problem that robotic arms are prone to exceeding safety boundaries during the control process in the prior art.

[0007] To address the aforementioned technical problems, this invention provides a method for controlling the safety boundary of a robotic arm, comprising:

[0008] S1: The IMU sensor installed at the joint of the robotic arm collects the first motion signal of the robotic arm, the encoder installed at the joint of the robotic arm collects the second motion signal of the robotic arm, and the visual positioning and tracking system collects the third motion signal of the robotic arm.

[0009] S2: Obtain the safety boundaries of the mechanical watch;

[0010] S3: Based on the first motion signal, the second motion signal, the third motion signal and the safety boundary of the robotic arm, the trajectory of the robotic arm is planned to obtain the safety warning area and the corresponding critical motion state T1 of the robotic arm;

[0011] S4: Obtain the real-time motion state Ti of the robotic arm at time t. When Ti≥T1, determine that the robotic arm has exceeded the safety warning area.

[0012] S5: When the robotic arm exceeds the safety warning area, a feedforward command is obtained based on the second real-time motion signal at time t collected by the IMU sensor, and the robotic arm is controlled according to the feedforward command.

[0013] Furthermore, in S2, the first motion signal includes the acceleration signal and / or angle signal of the robotic arm, the second motion signal includes the second pose signal of the robotic arm, and the third motion signal includes the third pose signal of the robotic arm.

[0014] Furthermore, S1 specifically includes:

[0015] Establish the IMU coordinate system, the robotic arm coordinate system, and the vision coordinate system, and normalize the three coordinate systems to obtain the coordinate transformation matrix;

[0016] The safety boundary of the robotic arm in the visual coordinate system is obtained based on the set motion area of ​​the robotic arm and the coordinate transformation matrix.

[0017] Furthermore, S3 includes:

[0018] S31: Obtain the motion state T0 of the robotic arm in the visual coordinate system based on the first motion signal, the second motion signal, the third motion signal, and the coordinate transformation matrix;

[0019] S32: Based on the motion state T0 of the robotic arm in the visual coordinate system and the safety boundary, the trajectory of the robotic arm is planned to obtain the safety warning area and the corresponding critical motion state T1 of the robotic arm.

[0020] Furthermore, in step S32, a fifth-order polynomial interpolation method is used to plan the trajectory of the robotic arm.

[0021] Furthermore, the critical motion state T1 of the robotic arm includes at least one critical position within the safety warning area, and the critical velocity and / or critical acceleration value of the robotic arm at that critical position.

[0022] Furthermore, when Ti≥T1, the real-time velocity or real-time acceleration value corresponding to the robotic arm moving to the critical position at time t is greater than or equal to the critical velocity or critical acceleration value corresponding to the robotic arm at that critical position.

[0023] Furthermore, the second real-time motion signal includes the acceleration signal of the robotic arm, and in S32, the feedforward command is obtained based on the current acceleration of the robotic arm.

[0024] Furthermore, the second real-time motion signal includes the angle signal of the robotic arm. In step S32, the current speed of the robotic arm is obtained based on the current angle of the robotic arm, and the feedforward command is obtained based on the current speed.

[0025] The present invention also provides a robotic arm safety boundary control system, comprising:

[0026] An IMU sensor is disposed at a joint of the robotic arm and is configured to acquire a first motion signal of the robotic arm;

[0027] An encoder, which is disposed at the joint of the robotic arm, is configured to acquire a second motion signal of the robotic arm;

[0028] A visual positioning and tracking system is configured to acquire a third motion signal from the robotic arm;

[0029] A driver, which is communicatively connected to the IMU sensor and the encoder, is configured to read the first motion signal and the second motion signal;

[0030] The control module is communicatively connected to the IMU sensor, the encoder, the robotic arm, the visual positioning and tracking system, and the driver.

[0031] The control module is further configured to: plan the trajectory of the robotic arm based on the first motion signal, the second motion signal, the third motion signal and the safety boundary area of ​​the robotic arm, obtain the critical motion state T1 of the robotic arm corresponding to the safety warning area, and when the robotic arm moves beyond the safety warning area, obtain a feedforward command based on the second real-time motion signal at time t collected by the IMU sensor, and control the robotic arm based on the feedforward command.

[0032] In summary, compared with the prior art, the robotic arm safety boundary control method and system provided by the present invention have the following advantages:

[0033] This invention sets up IMU sensors at the joints of the robotic arm to collect the motion parameters of the robotic arm in real time. Since the bandwidth of the IMU sensor is higher than that of the encoder, the motion parameters of the robotic arm can be fed back to the control module more quickly, effectively reducing the control delay.

[0034] Furthermore, by using data collected from IMU sensors, encoders, and visual positioning and tracking systems to plan safety warning zones, a warning signal is issued when the robotic arm exceeds the safety warning zone, allowing the control module to adjust the robotic arm in advance. This effectively suppresses motion overshoot caused by overshoot, prevents the robotic arm from unexpectedly entering unsafe areas, and improves the safety and reliability of the system.

[0035] Furthermore, in the solution of this invention, when the robotic arm exceeds the safety warning area, feedforward control is performed using low-latency data collected by the IMU sensor. This further improves the system's response speed, reduces the control delay of the robotic arm, and further suppresses motion overshoot caused by overshoot of the robotic arm. Moreover, the data collected by the IMU sensor is not affected by visual obstruction, drive errors, or encoder wear leading to inaccurate data, allowing for more precise control of the robotic arm.

[0036] Furthermore, in the robotic arm safety boundary control method of the present invention, the acceleration signal collected by the IMU is directly used as the input of the feedforward control, which has two obvious advantages. First, the acceleration signal is directly acquired, which reduces the system calculation time and improves the system response speed. Second, the acceleration signal is introduced into the inner loop control (current loop) of the robotic arm control system, which can improve the response speed of the robotic arm system and help control the robotic arm away from the safety boundary, thus ensuring surgical safety. Attached Figure Description

[0037] Figure 1 This is a schematic diagram of a robotic arm safety boundary control method according to an embodiment of the present invention;

[0038] Figure 2 This is a flowchart illustrating a robotic arm safety boundary control method according to an embodiment of the present invention.

[0039] Figure 3 This is a diagram showing the correspondence between the visual positioning and tracking system and the robotic arm in a robotic arm safety boundary control system according to an embodiment of the present invention.

[0040] Figure 4 This is a schematic diagram of the structure of an IMU sensor mounted on a robotic arm according to one embodiment of the present invention;

[0041] Figure 5 This is a schematic diagram of a positioning target mounted on a robotic arm according to one embodiment of the present invention;

[0042] Figure 6 This is a schematic diagram of the safety boundary of a robotic arm in a robotic arm safety boundary control method according to an embodiment of the present invention;

[0043] Figure 7 This is a control mode diagram in one embodiment of the present invention that uses acceleration as a feedforward command;

[0044] Figure 8 This is a control mode diagram using a speed signal as input in one embodiment of the present invention.

[0045] Among them, 10-visual positioning and tracking system; 20-robotic arm; 21-IMU sensor; 22-positioning target. Detailed Implementation

[0046] The following detailed description, in conjunction with the accompanying drawings and specific embodiments, provides a further detailed explanation of the robotic arm safety boundary control method and system proposed in this invention. The advantages and features of this invention will become clearer from the following description.

[0047] It should be noted that the accompanying drawings are in a very simplified form and use non-precise proportions, and are only used to facilitate and clearly illustrate the purpose of the embodiments of the present invention. Please refer to the accompanying drawings to make the objectives, features, and advantages of the present invention more apparent and understandable. It should be understood that the structures, proportions, sizes, etc., depicted in the accompanying drawings are only used to complement the content disclosed in the specification, for those skilled in the art to understand and read, and are not intended to limit the implementation conditions of the present invention. Therefore, they have no substantial technical significance. Any modifications to the structure, changes in proportions, or adjustments to the size, without affecting the effects and objectives achieved by the present invention, should still fall within the scope of the technical content disclosed in the present invention.

[0048] The purpose of this invention is to provide a method and system for controlling the safety boundaries of a robotic arm, so as to solve the problem that robotic arms are prone to exceeding safety boundaries during the control process in the prior art.

[0049] Figure 1 This is a schematic diagram of a robotic arm safety boundary control method provided by the present invention. Figure 2 This is a flowchart illustrating a method for controlling the safety boundary of a robotic arm. Specifically, the method for controlling the safety boundary of a robotic arm provided by this invention includes the following steps:

[0050] S1: As Figure 3As shown, an IMU sensor 21 installed at the joint of the robotic arm 20 collects the first motion signal of the robotic arm, an encoder (not shown) installed at the joint of the robotic arm 20 collects the second motion signal of the robotic arm 20, and a visual positioning and tracking system 10 collects the third motion signal of the robotic arm 20. An encoder is typically installed at the joint of the robotic arm 20. This encoder can collect the pose signal of the robotic arm 20. For ease of description, the pose signal collected by the encoder is defined here as the second motion signal. This second motion signal includes the second pose signal of the robotic arm 20 (to easily correlate the motion signal with the pose signal and avoid confusion in subsequent descriptions, the number of the pose signal corresponding to the second motion signal is directly defined as the second pose signal; in the scheme of this invention, there is no first pose signal). This second pose signal reflects the pose information of the robotic arm 20 relative to its initial state, recorded by the encoder of the robotic arm 20. The encoder can feed back this second motion signal to the control module to monitor the motion state of the robotic arm 20. In addition, the visual positioning and tracking system 10 also tracks the robotic arm 20 in real time, collecting a third motion signal of the robotic arm 20. This third motion signal includes a third pose signal of the robotic arm 20, reflecting the pose state of the robotic arm 20 in the visual coordinate system, including the positional distance between the robotic arm 20 and surrounding objects, such as the pose state of the robotic arm 20 relative to the patient or other targets, and the speed signal of the robotic arm obtained from the image information collected by the visual positioning and tracking system 10. The visual positioning and tracking system 10 can feed back this third pose signal to the control module. In the solution of the present invention, the motion signal of the robotic arm 20 is also collected in real time by an IMU sensor 21 set at the joint of the robotic arm 20. This motion signal can be defined as a first motion signal, which may include the acceleration signal and / or angle signal of the robotic arm 20. For example, the acceleration signal of the robotic arm 20 can be collected by an accelerometer, and the angle signal of the robotic arm 20 can be collected by an angular velocity sensor. The first motion signal collected by the IMU sensor 21 can be fed back to the control module.

[0051] S2: Obtain the safety boundary of the robotic arm; during operation, the robotic arm's normal operating range is defined according to the actual operating procedure, and this range is the robotic arm's safety boundary. The robotic arm's safety boundary consists of two parts: static safety boundary and dynamic safety boundary. The static safety boundary is generally given preoperatively based on CT images, such as... Figures 3 to 6As shown, taking a surgical robot as an example, the surgical procedure space is set according to clinical surgical needs. For example, the set surgical procedure space is a 300*300*300mm cube space centered on the patient. The end effector of the surgical robot's robotic arm 20 moves the surgical instruments within this cube space to perform the surgical operation. That is, the spatial coordinate range of the end effector of the robotic arm 20 in the visual coordinate system of the visual positioning and tracking system 10 is (x,y,z)=(±150mm, ±150mm, ±150mm). Figure 6 The position indicated by the dashed line on the robotic arm represents its static safety boundary. When the end effector of robotic arm 20 exceeds this boundary, it may collide with other medical equipment or personnel, causing equipment damage or injury. Therefore, it is essential to ensure the robotic arm moves within the static safety boundary to guarantee the smooth progress of the surgery. Generally, the static safety boundary of the robotic arm is defined before operation based on actual conditions. However, this static safety boundary is not immutable once set; it can be updated according to actual circumstances. For example, during surgery, when the patient moves, the static safety boundary of robotic arm 20 can be redefined. In addition to the static safety boundary, the robotic arm's safety boundary also includes a dynamic safety boundary. This part mainly includes the real-time motion state of the robotic arm. The dynamic safety boundary reflects the current inertia of the robotic arm. The faster the robotic arm moves, the longer it takes to return to a stationary state, and the higher the probability of the robotic arm exceeding the boundary. This part is an important reference indicator for subsequently setting the warning area. Specifically, the motion state of the robotic arm corresponding to the dynamic safety boundary can include, for example, the current real-time speed and acceleration of the robotic arm. This part can be calculated based on the first motion signal, the second motion signal, and / or the third motion signal acquired in step S1 above. Therefore, the safety boundary P of the robotic arm can be characterized as: P = k1 * P s +k2*V i .

[0052] Where P represents the safety boundary of the robotic arm, Ps represents the static safety boundary of the robotic arm planned before surgery, Vi is the current real-time velocity signal of the robotic arm (which can be the current real-time velocity of the robotic arm, or it can include the current real-time acceleration of the robotic arm), and k1 and k2 are constant boundary coefficients. Specifically, V i =C1*V coder +C2*V 视觉 +C3*V imu Where C1, C2, and C3 are sensor coefficients, and V coder V 视觉 V imuThese represent the speed signals of the robotic arm acquired by the encoder, the visual positioning and tracking system, and the IMU sensor, respectively. One innovation of this invention is the addition of an IMU sensor to acquire the robotic arm's speed signal. Because the IMU sensor has low bandwidth delay and significantly better performance than the encoder, even when the visual positioning and tracking system is obstructed or the encoder's signal is inaccurate due to wear, the dynamic safety boundary portion can still be accurately obtained through the signal acquired by the IMU sensor.

[0053] Those skilled in the art should understand that the robotic arm of the present invention is not limited to the robotic arm of a surgical robot, but can also be a robotic arm in other fields, such as a robotic hand in a smart manufacturing plant. That is, the safety boundary control method of the present invention can be applied to any field that requires safety boundary control of a robotic arm.

[0054] S3: Based on the first motion signal, the second motion signal, the third motion signal, and the safety boundary of the robotic arm 20, the trajectory of the robotic arm 20 is planned to obtain the safety warning area of ​​the robotic arm, and the critical motion state T1 of the robotic arm in the safety warning area is obtained. After collecting three motion signals, the current motion state of the robotic arm 20 can be determined, and the trajectory of the robotic arm 20 is planned according to the safety boundary, setting the safety warning area of ​​the robotic arm 20. The safety warning area of ​​the robotic arm 20 includes a static position area and a corresponding dynamic critical speed part of the robotic arm 20. The safety warning area of ​​the robotic arm 20 represents that when the robotic arm 20 reaches the position of the safety warning area, if the movement speed of the robotic arm 20 is greater than the critical speed set by the safety warning area, if no additional intervention is made and it continues to move according to the original trend, the robotic arm 20 has a high probability of eventually exceeding the safety boundary. Therefore, setting the safety warning area in advance can detect in advance whether the robotic arm 20 is at risk of exceeding the safety boundary, thereby facilitating subsequent intervention on the movement of the robotic arm 20 to prevent it from exceeding the safety boundary. The setting of the safety warning zone for the robotic arm 20 is related to the current motion state of the robotic arm 20, its mass, and the signal response speed of the control system. The faster the robotic arm 20 moves, the greater its mass, and the slower the signal response speed of the control system, the less likely it is to reduce its speed from its current motion state, and the more likely it is to exceed the safety zone. Therefore, the static position area of ​​the safety warning zone should be smaller and farther from the safety boundary to allow sufficient response space for the robotic arm to slow down. It is important to note that the safety warning zone for the robotic arm 20 is set based on its current real-time motion state and safety boundary; therefore, it is not completely fixed. When the motion state of the robotic arm 20 changes, the safety warning zone can also change accordingly. In the present invention, the motion state of the robotic arm 20 generally includes its current position and its current speed and / or acceleration value. The critical motion state T1 of the robotic arm 20 corresponding to the safety warning zone can be its critical speed, its acceleration value, or both.It is important to note that the safety warning zone of the robotic arm 20 can simultaneously include multiple critical positions and their corresponding critical motion states of the robotic arm 20. That is, several critical positions can be selected within the safety warning zone of the robotic arm 20, and each critical position corresponds to a critical speed of the robotic arm 20. This forms a set containing critical positions and their corresponding critical speeds. When monitoring the motion state of the robotic arm 20 in the future, it is possible to determine in advance whether the robotic arm 20 has exceeded the safety warning zone as needed. That is, as long as the robotic arm 20 moves to a corresponding critical position and its real-time movement speed is found to be greater than the critical speed corresponding to that critical position, it can be determined that the robotic arm 20 is at risk of exceeding the safety warning zone.

[0055] S4: Obtain the real-time motion state Ti of the robotic arm at time t and compare it with the threshold state T2. When Ti ≥ T2, it is determined that the robotic arm 20 has exceeded the safety warning zone, where T2 ≤ T1. After determining the safety warning zone, the subsequent motion state of the robotic arm 20 can be monitored. When the robotic arm is found to have exceeded the safety warning zone, it means that the motion of the robotic arm 20 needs to be intervened to prevent it from exceeding the safety boundary. However, since the real-time speed of the robotic arm collected by the sensor may have a certain error, and the control system inevitably has a certain delay in controlling the robotic arm 20, if the critical speed corresponding to the safety warning zone is used as the standard for intervention and control, control delay may occur, causing the robotic arm 20 to exceed the safety boundary. Therefore, a threshold state T2 can be set. Under this threshold state T2, the speed or acceleration value of the robotic arm 20 is less than or equal to the critical speed or acceleration value of the robotic arm 20 corresponding to the safety warning zone. The real-time motion state Ti of the robotic arm 20 can be compared with the threshold state T2. When Ti≥T2, it means that the velocity or acceleration value of the robotic arm 20 at the position at time t is greater than or equal to the velocity or acceleration value of the robotic arm 20 at the position under the threshold state. At this time, it is determined that the robotic arm 20 has exceeded the safety warning area at time t, and the motion of the robotic arm 20 needs to be controlled to prevent it from exceeding the safety boundary.

[0056] S5: When the robotic arm 20 exceeds the safety warning area, a feedforward command is obtained based on the second real-time motion signal at time t collected by the IMU sensor 21, and the robotic arm 20 is controlled according to the feedforward command. In the solution of the present invention, when the robotic arm 20 is detected to have exceeded the safety warning area, the motion state of the robotic arm 20 is adjusted to prevent it from exceeding the safety boundary in subsequent movements. Specifically, a feedforward command is generated based on the second real-time motion signal collected by the IMU sensor 21. The control module controls the robotic arm 20 with this feedforward command. For example, the IMU sensor 21 collects the current motion parameters of the robotic arm 20 in real time. When the robotic arm 20 moves beyond the safety warning area, it indicates that the current motion parameters are causing the robotic arm 20 to move too fast. At this time, the control module generates a feedforward command based on the current motion parameters of the robotic arm 20, and controls the movement speed of the robotic arm 20 to slow down through this feedforward command, so that the robotic arm 20 slows down in advance to prevent the robotic arm 20 from exceeding the safety boundary.

[0057] In this invention, IMU sensors 21 are installed at the joints of the robotic arm 20 to collect the motion parameters of the robotic arm in real time. Since the bandwidth of the IMU sensor 21 is higher than that of the encoder, the motion parameters of the robotic arm 20 can be fed back to the control module more quickly, effectively reducing control latency. Furthermore, the data collected by the IMU sensor 21, encoder, and visual positioning and tracking system 10 is used for safety warning zone planning. When the robotic arm 20 exceeds the safety warning zone, a warning signal is issued, allowing the control module to adjust the robotic arm 20 in advance. This effectively suppresses motion overshoot caused by overshoot, preventing the robotic arm 20 from unexpectedly entering unsafe areas and improving the safety and reliability of the system. In addition, in this invention, when the robotic arm 20 exceeds the safety warning zone, feedforward control is performed using the low-latency data collected by the IMU sensor 21, which further improves the system's response speed, reduces the control latency of the robotic arm 20, and further suppresses motion overshoot caused by overshoot of the robotic arm 20. Moreover, the data collected by the IMU sensor 21 is not obstructed by visual interference, is not affected by drive errors, and is not subject to inaccurate data due to encoder wear, which allows for more precise control of the robotic arm 20.

[0058] Furthermore, since the data collected by the IMU sensor 21, the robotic arm encoder, and the vision positioning and tracking system 10 are not in the same coordinate system, to facilitate calculation, reduce the computation time of the robotic arm control system, and shorten the system response time, the data of the three collected motion signals can be normalized. Specifically, in step S1, the IMU coordinate system, the robotic arm coordinate system, and the vision coordinate system can be established first, and the three coordinate systems can be normalized to obtain a coordinate transformation matrix; then, the safety boundary of the robotic arm in the vision coordinate system can be obtained according to the set motion area of ​​the robotic arm and the coordinate transformation matrix. Furthermore, the motion parameters subsequently collected by the IMU sensor and encoder can also be transformed to the vision coordinate system according to the coordinate transformation matrix. For example, as... Figure 5 As shown, a positioning target 22 can be placed on a positioning feature point of the robotic arm 20. This target can be the end of the robotic arm 20 or another location on the robotic arm 20. Then, the visual positioning and tracking system 10 tracks the positioning target 22 to obtain the position of the robotic arm 20 in the visual coordinate system. Since the IMU sensor 21 is installed at the joint of the robotic arm 20, its positional relationship with the robotic arm 20 and with the positioning target 22 is determined. Therefore, the position of the IMU sensor 21 in the visual coordinate system can be obtained.

[0059] To facilitate calculations and reduce the computation time of the robotic arm control system, in this embodiment, the axis of the IMU sensor 21 can be aligned with the rotation axis of the robotic arm 20 during assembly. This ensures that the Z-axis of the IMU coordinate system and the robotic arm coordinate system are aligned. During coordinate normalization, multiple sets of posture data of the robotic arm in the visual coordinate system can be collected. Multiple sets of attitude data from the IMU in the world coordinate system Then, the least squares method is used to solve the equation. Solving the IMU coordinate system In the visual coordinate system The position in the middle.

[0060] Furthermore, S3 includes:

[0061] S31: Obtain the motion state T0 of the robotic arm 20 in the visual coordinate system based on the first motion signal, the second motion signal, the third motion signal and the coordinate transformation matrix; the motion signals collected by the IMU sensor 21 and the encoder can be transformed into motion signals at the end of the robotic arm according to the dynamic model of the robotic arm 20 and the Jacobian matrix, and then transformed into the real-time pose and speed of the robotic arm 20 in the visual coordinate system through the coordinate transformation matrix.

[0062] S32: Based on the motion state T0 of the robotic arm 20 in the visual coordinate system and the safety boundary, the trajectory of the robotic arm 20 is planned to obtain the safety warning area and the corresponding critical motion state T1 of the robotic arm 20. Specifically, the position of the end effector of the robotic arm can be determined first according to the set safety boundary, and then the inverse operation can be performed through the homogeneous transformation matrix to solve the angles α = [α1, α2, α3, α4, ..., αi] of each joint of the robotic arm within the safety boundary range in the visual coordinate system. Then, the current rotation angle of the robotic arm 20 is obtained according to the real-time pose and speed of the robotic arm 20 in the visual coordinate system, and then transformed to the robotic arm coordinate system through matrix transformation. Then, the trajectory of the robotic arm is planned. For example, the trajectory of the robotic arm can be planned using the fifth-order polynomial interpolation method. At this point, the safety warning zone can be obtained through trajectory planning of the robotic arm 20, combined with its own parameters (such as mass) and the response time of the control system. The critical position corresponding to the safety warning zone corresponds to the velocity information of the robotic arm in its critical motion state, including the position θ(t) = [θ1, θ2, θ3, θ4, ..., θi] of the robotic arm and its corresponding velocity. and / or acceleration When the real-time speed or acceleration of the robotic arm 20 exceeds the threshold range at the corresponding critical position during the subsequent actual movement, it means that the robotic arm 20 has exceeded the safety warning area. At this time, it is necessary to intervene and adjust the movement state of the robotic arm 20, otherwise it may cause the robotic arm 20 to exceed the safety boundary.

[0063] In one implementation of the present invention, the second real-time motion signal includes the acceleration signal of the robotic arm 20. In step S32, the feedforward command is obtained based on the current acceleration of the robotic arm 20. Specifically, when the robotic arm 20 exceeds the safety warning area, the current acceleration signal of the robotic arm 20 collected by the IMU sensor 21 can be directly used as the feedforward command. The control module generates a control command based on the current acceleration and the acceleration value corresponding to the threshold state T2, and transmits it to the robotic arm to reduce the acceleration of the robotic arm to below the acceleration value corresponding to the threshold state T2. This causes the robotic arm 20 to decelerate in advance, move away from the safety warning area, prevent the robotic arm 20 from exceeding the safety boundary, and ensure the smooth progress of the surgery. Its control mode is as follows: Figure 7As shown, the control system includes inner-loop control and outer-loop control. The basic hardware system of the robotic arm consists of the motor electrical and mechanical components. The inner-loop control (i.e., current loop control) is composed of current calibration, motor electrical components, and current sensors. This loop detects the output current of each phase of the motor from the driver during the robotic arm's movement using current sensors (such as electromagnetic current sensors or electronic current sensors). The feedback current correction loop performs PID adjustment to ensure that the output current is as close as possible to the set current. The outer-loop control (i.e., speed loop control) is composed of speed calibration, motor mechanical components, speed sensors, and the inner loop. It uses the signal from the servo motor encoder for feedback PID adjustment, and its PID output is directly the current loop setting. The overall control objective is to ensure that the robotic arm moves accurately to the required position. When the robotic arm enters the safety warning area, the IMU acceleration signal serves as the input to the inner-loop control, forming a new outer-loop control mode that controls the robotic arm to move away from the safety warning area or stop moving into the safety warning area. Using the acceleration signal acquired by the IMU directly as the input for feedforward control has two significant advantages: first, directly acquiring the acceleration signal reduces system calculation time and improves system response speed; second, introducing the acceleration signal into the inner loop control (current loop) of the robotic arm control system can improve the response speed of the robotic arm system, which is beneficial for controlling the robotic arm away from the safety boundary and ensuring surgical safety.

[0064] In another embodiment of the present invention, the second motion signal includes the angle signal of the robotic arm. In step S32, the speed of the robotic arm is obtained based on its current angle, and the feedforward command is obtained based on the speed. Specifically, when the robotic arm exceeds the safety warning area, the driver reads the angle signal of the robotic arm collected by the IMU and transmits it to the control module. The control module uses the angle signal to obtain the speed value of the robotic arm through differential calculation. This speed value is generally angular velocity. Then, the speed signal is imported into the control system. The control module generates a feedforward command based on the speed signal to control the robotic arm. Its control mode is as follows: Figure 8As shown, this control mode also includes inner-loop control and outer-loop control. The inner-loop control is basically the same as described above. Current calibration, motor electrical components, and current sensors constitute the inner-loop control (i.e., current loop control). This loop uses current sensors (such as electromagnetic current sensors, electronic current sensors, etc.) to detect the output current of each phase of the motor from the driver during the robot arm's movement. The current correction component responsible for feedback performs PID adjustment to make the output current as close as possible to the set current. Speed ​​calibration, motor mechanical components, speed sensors, and the inner loop together constitute the outer-loop control (i.e., speed loop control). It uses the signal from the servo motor encoder for feedback PID adjustment, and its PID output is directly the current loop setting. The overall control objective is to ensure that the robot arm moves accurately to the required position. When the robot arm moves beyond the safety warning area, the difference between the speed signal and the speed value corresponding to the threshold state T2 is used as the outer-loop input of the robot arm control mode, forming a new outer-loop control mode for the robot arm, thereby reducing the robot arm's operating speed. The closer to the boundary, the faster the speed decreases, until it drops to 0, thus ensuring that the robot always operates within the safe area.

[0065] The present invention also provides a robotic arm safety boundary control system, comprising:

[0066] IMU sensor 21, which is disposed at the joint of the robotic arm 20, is configured to acquire the first motion signal of the robotic arm 20 in real time;

[0067] An encoder is disposed at the joint of the robotic arm 20 and is configured to acquire the second motion signal of the robotic arm 20 in real time;

[0068] The visual positioning and tracking system 10 is configured to acquire the third motion signal of the robotic arm 20 in real time;

[0069] The driver, which is communicatively connected to the IMU sensor 21 and the encoder, is configured to read the first motion signal and the second motion signal. By directly reading the data collected by the IMU sensor 21 through the driver and transmitting the data to the control module via Ethcart (or CAN, etc.), the latency of data transmission can be greatly reduced.

[0070] The control module is communicatively connected to the IMU sensor 21, the encoder, the robotic arm 20, the visual positioning and tracking system 10, and the driver.

[0071] The control module is further configured to: plan the trajectory of the robotic arm based on the first motion signal, the second motion signal, the third motion signal and the safety boundary area of ​​the robotic arm; acquire the safety warning area in real time; and when the robotic arm moves beyond the safety warning area, acquire a feedforward command based on the real-time collected second motion signal and control the robotic arm based on the feedforward command.

[0072] Based on the same inventive concept, one embodiment of the present invention also provides an electronic device, which includes a processor and a memory. The memory stores a computer program, and when the computer program is executed by the processor, it implements the robotic arm safety boundary control method described above. Since the electronic device provided by the present invention and the robotic arm safety boundary control method provided by the present invention belong to the same inventive concept, the electronic device provided by the present invention possesses at least all the beneficial effects of the robotic arm safety boundary control method provided by the present invention. For details, please refer to the relevant descriptions of the beneficial effects of the robotic arm safety boundary control method provided by the present invention above; therefore, the beneficial effects of the electronic device provided by the present invention will not be repeated here.

[0073] This invention also provides a readable storage medium storing a computer program. When executed by a processor, the computer program can implement the robotic arm safety boundary control method described above. Since the readable storage medium provided by this invention and the robotic arm safety boundary control method provided by this invention belong to the same inventive concept, the readable storage medium provided by this invention has all the beneficial effects of the robotic arm safety boundary control method provided by this invention. For details, please refer to the relevant descriptions of the beneficial effects of the robotic arm safety boundary control method provided by this invention above. Therefore, the beneficial effects of the readable storage medium provided by this invention will not be repeated here.

[0074] In summary, compared with the prior art, the robotic arm safety boundary control method and system provided by the present invention have the following advantages:

[0075] This invention sets up IMU sensors at the joints of the robotic arm to collect the motion parameters of the robotic arm in real time. Since the bandwidth of the IMU sensor is higher than that of the encoder, the motion parameters of the robotic arm can be fed back to the control module more quickly, effectively reducing the control delay.

[0076] Furthermore, by using data collected from IMU sensors, encoders, and visual positioning and tracking systems to plan safety warning zones, a warning signal is issued when the robotic arm exceeds the safety warning zone, allowing the control module to adjust the robotic arm in advance. This effectively suppresses motion overshoot caused by overshoot, prevents the robotic arm from unexpectedly entering unsafe areas, and improves the safety and reliability of the system.

[0077] Furthermore, in the solution of this invention, when the robotic arm exceeds the safety warning area, feedforward control is performed using low-latency data collected by the IMU sensor. This further improves the system's response speed, reduces the control delay of the robotic arm, and further suppresses motion overshoot caused by overshoot of the robotic arm. Moreover, the data collected by the IMU sensor is not affected by visual obstruction, drive errors, or encoder wear leading to inaccurate data, allowing for more precise control of the robotic arm.

[0078] Furthermore, in the robotic arm safety boundary control method of the present invention, the acceleration signal collected by the IMU is directly used as the input of the feedforward control, which has two obvious advantages. First, the acceleration signal is directly acquired, which reduces the system calculation time and improves the system response speed. Second, the acceleration signal is introduced into the inner loop control (current loop) of the robotic arm control system, which can improve the response speed of the robotic arm system and help control the robotic arm away from the safety boundary, thus ensuring surgical safety.

[0079] The above description is merely a description of preferred embodiments of the present invention and is not intended to limit the scope of the invention in any way. Any changes or modifications made by those skilled in the art based on the above disclosure are within the scope of the claims. Obviously, those skilled in the art can make various modifications and variations to the invention without departing from the spirit and scope of the invention. Therefore, if these modifications and variations of the invention fall within the scope of the claims and their equivalents, the invention also intends to include these modifications and variations.

Claims

1. A method for controlling the safety boundary of a robotic arm, characterized in that, include: S1: The IMU sensor installed at the joint of the robotic arm collects the first motion signal of the robotic arm, the encoder installed at the joint of the robotic arm collects the second motion signal of the robotic arm, and the visual positioning and tracking system collects the third motion signal of the robotic arm. S2: Obtain the safety boundaries of the robotic arm; S3: Based on the first motion signal, the second motion signal, the third motion signal and the safety boundary of the robotic arm, the trajectory of the robotic arm is planned to obtain a safety warning area and the corresponding critical motion state T1 of the robotic arm. The safety warning area is located within the safety boundary. The critical motion state T1 of the robotic arm includes at least one critical position within the safety warning area and the critical velocity and / or critical acceleration value of the robotic arm at the critical position. S4: Obtain the real-time motion state Ti of the robotic arm at time t. When Ti≥T1, the real-time velocity or real-time acceleration value corresponding to the robotic arm moving to the critical position at time t is greater than or equal to the critical velocity or critical acceleration value corresponding to the robotic arm at the critical position. At this time, it is determined that the robotic arm has exceeded the safety warning area. S5: When the robotic arm exceeds the safety warning area, a feedforward command is obtained based on the second real-time motion signal at time t collected by the IMU sensor, and the robotic arm is controlled according to the feedforward command.

2. The robotic arm safety boundary control method according to claim 1, characterized in that, In S1, the first motion signal includes the acceleration signal and / or angle signal of the robotic arm, the second motion signal includes the second pose signal of the robotic arm, and the third motion signal includes the third pose signal of the robotic arm.

3. The robotic arm safety boundary control method according to claim 1, characterized in that, S2 specifically includes: Establish the IMU coordinate system, the robotic arm coordinate system, and the vision coordinate system, and normalize the three coordinate systems to obtain the coordinate transformation matrix; The safety boundary of the robotic arm in the visual coordinate system is obtained based on the set motion area of ​​the robotic arm and the coordinate transformation matrix.

4. The robotic arm safety boundary control method according to claim 3, characterized in that, S3 includes: S31: Obtain the motion state T0 of the robotic arm in the visual coordinate system based on the first motion signal, the second motion signal, the third motion signal, and the coordinate transformation matrix; S32: Based on the motion state T0 of the robotic arm in the visual coordinate system and the safety boundary, the trajectory of the robotic arm is planned to obtain the safety warning area and the corresponding critical motion state T1 of the robotic arm.

5. The robotic arm safety boundary control method according to claim 4, characterized in that, In step S32, the trajectory of the robotic arm is planned using a fifth-order polynomial interpolation method.

6. The robotic arm safety boundary control method according to claim 4, characterized in that, The second real-time motion signal includes the acceleration signal of the robotic arm. In step S32, the feedforward command is obtained based on the current acceleration of the robotic arm.

7. The robotic arm safety boundary control method according to claim 4, characterized in that, The second real-time motion signal includes the angle signal of the robotic arm. In step S32, the current speed of the robotic arm is obtained based on the current angle of the robotic arm, and the feedforward command is obtained based on the current speed.

8. A robotic arm safety boundary control system for implementing the robotic arm safety boundary control method as described in any one of claims 1-7, characterized in that, include: An IMU sensor is disposed at a joint of the robotic arm and is configured to acquire a first motion signal of the robotic arm; An encoder, which is disposed at the joint of the robotic arm, is configured to acquire a second motion signal of the robotic arm; A visual positioning and tracking system is configured to acquire a third motion signal from the robotic arm; A driver, which is communicatively connected to the IMU sensor and the encoder, is configured to read the first motion signal and the second motion signal; The control module is communicatively connected to the IMU sensor, the encoder, the robotic arm, the visual positioning and tracking system, and the driver. The control module is further configured to: plan the trajectory of the robotic arm based on the first motion signal, the second motion signal, the third motion signal and the safety boundary area of ​​the robotic arm, obtain the critical motion state T1 of the robotic arm corresponding to the safety warning area, and when the robotic arm moves beyond the safety warning area, obtain a feedforward command based on the second real-time motion signal at time t collected by the IMU sensor, and control the robotic arm based on the feedforward command.

Images