Robot motion control method based on safety and space planning

Abstract

The invention relates to the technical field of physical therapy robot motion control, and discloses a robot motion control method based on safety and space planning, which is applied to a mechanical arm system, and specifically comprises the following steps: S101, receiving a movement value input by a user terminal through a rocker value, and triggering the mechanical arm system to start, the target position of the mechanical arm after movement is calculated; s102, the mechanical arm system obtains the current position of the mechanical arm, and boundary setting is conducted on the physiotherapy area through the current position; and S103, the moving distance of the mechanical arm is obtained according to the moving distance and the offset. According to the method, the failure rate caused by singular points can be reduced to 0.01% or below, compared with the failure rate of 0.1% of a traditional algorithm, the safety is improved by 90%, an alarm is sent to a user in time when the mechanical arm approaches a critical area, the user can know the state of the mechanical arm more visually and timely, and therefore the misoperation risk is reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of motion control of a physiotherapy robot, and in particular to a robot motion control method based on safety and space planning. Background Art

[0002] With the development of modern robotics technology, robots are increasingly used in the medical, industrial and service fields. Especially in the field of rehabilitation therapy, robotic arms are widely used to help patients complete massage, acupressure and other physiotherapy operations. However, existing physiotherapy robots often face the following problems during work:

[0003] Singularity problem: The kinematic singularity of the robot arm means that in certain positions or postures, the robot arm cannot move freely, and may even cause the movement trajectory to be out of control. This will not only affect the physical therapy effect, but also may bring potential safety hazards to patients.

[0004] Joint abnormality: During the movement of the robot arm joint, the joint may be overstretched or collided due to exceeding the range of motion or being in an extreme posture. Such abnormalities may damage the equipment or cause secondary injuries to the patient.

[0005] To address these issues, current physical therapy robots usually use some simple boundary restriction algorithms, or manually set the range of motion to prevent the robotic arm from entering the dangerous area. However, these methods lack intelligence and real-time performance, and cannot be dynamically adjusted according to the current position and posture of the robotic arm, resulting in greater safety risks. Summary of the invention

[0006] The purpose of the present invention is to provide a robot motion control method based on safety and spatial planning, which significantly improves the ability of the physiotherapy robot to avoid singularities and joint abnormalities during work, and ensures the safety and stability of the physiotherapy process. Through the spatial planning algorithm of the present invention, the robotic arm will automatically perform position restrictions and safety prompts before entering the critical position or singularity area, aiming to solve the problem of lack of intelligence and real-time performance in the prior art, and the inability to dynamically adjust according to the current position and posture of the robotic arm, resulting in greater safety risks.

[0007] The present invention is implemented in this way: a robot motion control method based on safety and space planning is applied to a robotic arm system, specifically comprising the following steps:

[0008] S101: receiving a movement value inputted by a user terminal through a joystick value, triggering the start of the robotic arm system, and calculating a target position after the robotic arm moves;

[0009] S102: The robotic arm system obtains the current position of the robotic arm, and uses the current position to set the boundary of the physical therapy area;

[0010] S103: Calculating the moving distance of the robot arm according to the moving distance and the offset;

[0011] S104: Obtain and calculate the target position after the robot arm moves, perform verification based on the moving distance of the robot arm, obtain the final movable distance of the robot arm, and execute the robot arm to complete the movement action.

[0012] Further, in S101, receiving a movement value inputted by a user terminal through a joystick value includes:

[0013] Receiving a connection request sent by a user terminal through a preset network, wherein the connection request is used to request to establish a connection with the robotic arm system;

[0014] Detecting whether the current account of the user terminal is the target account;

[0015] If the current account of the user terminal is the target account, the user terminal is connected according to the connection request, and after the connection is completed, the movement value input by the user terminal through the joystick value is received.

[0016] Furthermore, the preset network includes one or a combination of a 3G network, a 4G network, a 5G network, and a WIFI network.

[0017] Furthermore, in S102, the robot arm system obtains the current position of the robot arm, and uses the current position to set the boundary of the treatment area, including:

[0018] By setting the boundary of the therapy area, the movement of the robot arm is limited to a safe range. The boundary of the therapy area is represented by a four-dimensional space, where the x and y axes are the horizontal and vertical directions respectively, the z axis is the height direction, and θ is the posture angle. The set area boundary can be described as follows:

[0019] space=[x min ,x max ,y min ,y max ]

[0020] The current position of the robot arm (x, y, z, θ) is subject to the following restrictions within the treatment area:

[0021] y min <y<y max ;

[0022] x min <x<x max .

[0023] Further, in S103, the moving distance of the robot arm is obtained according to the moving distance and the offset, including:

[0024] By dynamically calculating the position of each joint of the robot arm, when moving in the y-axis direction, the movable range of the x-axis is dynamically calculated to prevent the robot arm from entering a singular point. The geometric formula is:

[0025]

[0026] In the formula, x max is the maximum x-axis motion range of the robot arm when it is at a certain position on the y-axis, and y is the current position of the robot arm on the y-axis. The qBound function is used to limit the x-axis position of the robot arm to the calculated range.

[0027] Furthermore, in S104, the moving distance of the robot arm is verified to obtain the final movable distance of the robot arm, including:

[0028] When the user terminal passes the joystick movement instruction, the robotic arm system first calculates the target position after the robotic arm moves, and obtains the target position after the robotic arm moves;

[0029] The feasibility of the target position is verified by boundary setting and singular point calculation. If the target position exceeds the allowable range, the system will automatically limit the position and issue a warning signal.

[0030] Furthermore, in S104, the robot arm is executed to complete the motion action, specifically:

[0031] A monitoring module is provided on the robotic arm, and the monitoring module is used to monitor the movement of the robotic arm in real time to ensure that there is no excessive stretching of the joints or collision of the robotic arm at any time.

[0032] Furthermore, the robotic arm system can remind the user through sound and light alarms when the robotic arm approaches or reaches the boundary setting area. If the robotic arm is about to cross the boundary or has an abnormal posture, the robotic arm system will automatically trigger the LED light to flash, remind the user through the voice system, and limit the movement of the robotic arm.

[0033] Furthermore, qBound (function formula is: Qbound = ΣΔx^2 / N or Qbound = ΣΔy^2 / N;

[0034] Among them, Δx and Δy are the average values ​​of a given data set, that is, (x max -x min ) or (y max -y min), N is the number of samples in the data set, the x-axis and y-axis positions of the robot arm are restricted to the calculated range, Δx is the increment in the x-axis direction input by the user, and Δy is the increment in the y-axis direction input by the user.

[0035] Furthermore, the above-mentioned robot motion control method based on safety and space planning is applied to an intelligent physiotherapy robot.

[0036] Compared with the prior art, the robot motion control method based on safety and space planning provided by the present invention has the following beneficial effects:

[0037] 1. The ability of the physiotherapy robot to avoid singularities and joint abnormalities during work is significantly improved, ensuring the safety and stability of the physiotherapy process. Through the spatial planning algorithm of the present invention, the robot arm will automatically perform position restrictions and safety prompts before entering the critical position or singularity area, effectively avoiding the robot arm from entering the singularity or having joint abnormalities, ensuring the safe operation of the robot arm;

[0038] 2. The present invention can reduce the failure rate caused by singular points to below 0.01%, which improves the safety by 90% compared with the 0.1% failure rate of the traditional algorithm. It can also issue a warning to the user in time when the robotic arm approaches the critical area, so that the user can understand the status of the robotic arm more intuitively and timely, thereby reducing the risk of misoperation. This design enables users to use the robotic arm for physical therapy without worrying about unexpected situations. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 A schematic flow chart of a robot motion control method based on safety and space planning proposed by the present invention;

[0040] Figure 2 A schematic diagram of a flow chart of receiving movement values ​​inputted by a user terminal through a joystick value in a robot motion control method based on safety and space planning proposed by the present invention;

[0041] Figure 3 The present invention proposes a robot motion control method based on safety and space planning, which verifies the moving distance of the robot arm and obtains a flow chart of the final movable distance of the robot arm. DETAILED DESCRIPTION

[0042] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0043] The implementation of the present invention is described in detail below in conjunction with specific embodiments.

[0044] The same or similar numbers in the drawings of this embodiment correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right" and the like indicate directions or positional relationships based on the directions or positional relationships shown in the drawings, it is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific direction, be constructed and operated in a specific direction. Therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes and cannot be understood as limitations on the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0045] Reference Figure 1-3 As shown, the robot motion control method based on safety and space planning is applied to the robotic arm system, which specifically includes the following steps:

[0046] S101: receiving a movement value inputted by a user terminal through a joystick value, triggering the start of the robotic arm system, and calculating a target position after the robotic arm moves;

[0047] The step of receiving the movement value inputted by the user terminal through the joystick value includes:

[0048] Receive a connection request sent by a user terminal through a preset network, the connection request being used to request to establish a connection with the robotic arm system;

[0049] Check whether the current account of the user terminal is the target account;

[0050] If the current account of the user terminal is the target account, the user terminal is connected according to the connection request, and after the connection is completed, the movement value input by the user terminal through the joystick value is received;

[0051] S102: The robotic arm system obtains the current position of the robotic arm, and uses the current position to set the boundary of the physical therapy area;

[0052] The robotic arm system obtains the current position of the robotic arm and uses the current position to set the boundary of the treatment area, including:

[0053] By setting the boundary of the therapy area, the movement of the robot arm is limited to a safe range. The boundary of the therapy area is represented by a four-dimensional space, where the x and y axes are the horizontal and vertical directions respectively, the z axis is the height direction, and θ is the posture angle. The set area boundary can be described as follows:

[0054] space=[x min ,x max ,y min ,y max ]

[0055] The current position of the robot arm (x, y, z, θ) is subject to the following restrictions within the treatment area:

[0056] y min <y<y max ;

[0057] x min <x<x max ;

[0058] S103: Calculating the moving distance of the robot arm according to the moving distance and the offset;

[0059] Among them, according to the moving distance and the offset, the moving distance of the robot arm is obtained, including:

[0060] By dynamically calculating the position of each joint of the robot arm, when moving in the y-axis direction, the movable range of the x-axis is dynamically calculated to prevent the robot arm from entering a singular point. The geometric formula is:

[0061]

[0062] In the formula, x max is the maximum x-axis motion range of the robot arm when it is at a certain position on the y-axis, and y is the current position of the robot arm on the y-axis. The x-axis position of the robot arm is limited to the calculated range through the qBound function;

[0063] Specifically, qBound (function formula is: Qbound = ΣΔx^2 / N or Qbound = ΣΔy^2 / N;

[0064] Among them, Δx and Δy are the average values ​​of a given data set, that is, (x max -x min ) or (y max -y min ), N is the number of samples in the data set, the x-axis and y-axis positions of the robot arm are restricted to the calculated range, Δx is the increment in the x-axis direction input by the user, and Δy is the increment in the y-axis direction input by the user;

[0065] For example, in actual application, the user inputs Δx=5 and Δy=3 through the joystick value. At this time, the system first obtains the current position of the robot arm, such as x current =15 and y current , the new target position is calculated by the algorithm.

[0066] According to the formula:

[0067]

[0068] Therefore, the system limits the target position of the x-axis to the range of [-8.7, 8.7]. If the calculated x-value exceeds this range, the system will issue a warning and limit the movement of the robot arm to ensure that the robot arm does not enter the singularity point or joint abnormality area.

[0069] S104: Obtain and calculate the target position after the robot arm moves, verify based on the moving distance of the robot arm, obtain the final movable distance of the robot arm, and execute the robot arm to complete the movement. This technical solution significantly improves the ability of the physiotherapy robot to avoid singularities and joint abnormalities during work, and ensures the safety and stability of the physiotherapy process. Through the spatial planning algorithm of the present invention, the robot arm will automatically limit the position and issue safety prompts before entering the critical position or singularity area, effectively avoiding the situation where the robot arm enters the singularity or has joint abnormalities, thereby ensuring the safe operation of the robot arm.

[0070] In this embodiment, the preset network includes one or a combination of a 3G network, a 4G network, a 5G network, and a WIFI network.

[0071] In S104 of this embodiment, the moving distance of the robot arm is verified to obtain the final movable distance of the robot arm, including:

[0072] When the user terminal passes the joystick movement instruction, the robot arm system first calculates the target position after the robot arm moves and obtains the target position after the robot arm moves;

[0073] The feasibility of the target position is verified by boundary setting and singular point calculation. If the target position exceeds the allowable range, the system will automatically limit the position and issue a warning signal.

[0074] In S104 of this embodiment, the robot arm is executed to complete the motion action, specifically:

[0075] A monitoring module is provided on the robotic arm, which is used to monitor the movement of the robotic arm in real time to ensure that there is no excessive stretching of the joints or collision of the robotic arm at any time.

[0076] In this embodiment, the robotic arm system can remind the user through sound and light alarms when the robotic arm approaches or reaches the boundary setting area. If the robotic arm is about to cross the boundary or has an abnormal posture, the robotic arm system will automatically trigger the LED light to flash, remind the user through the voice system, and limit the movement of the robotic arm.

[0077] In addition, the specific algorithm code for this technical solution is as follows:

[0078]

[0079]

[0080] The above algorithm can reduce the failure rate caused by singular points to below 0.01%, which improves safety by 90% compared to the 0.1% failure rate of traditional algorithms. It also promptly warns users when the robotic arm approaches the critical area, allowing users to understand the status of the robotic arm more intuitively and promptly, thereby reducing the risk of misoperation. This design allows users to use the robotic arm for physical therapy with confidence without worrying about unexpected situations.

[0081] The present invention provides a robot motion control method based on safety and space planning, which significantly improves the ability of the physiotherapy robot to avoid singular points and joint abnormalities during operation, and ensures the safety and stability of the physiotherapy process. Compared with the prior art, the present invention has the following technical advantages and effects:

[0082] Improved physical therapy safety: Through the spatial planning algorithm of the present invention, the robot arm will automatically limit its position and provide safety prompts before entering the critical position or singularity area. The algorithm combines geometric calculation and real-time monitoring to effectively avoid the robot arm entering the singularity or having joint abnormalities, ensuring the safe operation of the robot arm.

[0083] In the prior art, the robot arm is prone to entering singular points during complex movements, resulting in joint jamming or sudden uncontrolled movement. The present invention greatly reduces the occurrence of such problems by calculating the movement range boundary in real time and automatically limiting the movement of the robot arm.

[0084] Experimental data show that the present invention can reduce the failure rate caused by singular points to below 0.01%, which improves the safety by 90% compared with the 0.1% failure rate of traditional algorithms.

[0085] Significant improvement in user experience: The present invention uses integrated sound and light alarms and voice prompts to warn users in a timely manner when the robotic arm approaches a critical area, allowing users to more intuitively and timely understand the state of the robotic arm, thereby reducing the risk of misoperation. This design allows users to safely use the robotic arm for physical therapy without worrying about unexpected situations.

[0086] In terms of user experience, traditional robot control systems usually lack a clear safety feedback mechanism, and users cannot immediately know whether the robot arm is in a dangerous state. The present invention greatly improves the user's sense of security through integrated sound, light and voice prompts.

[0087] According to user surveys, 90% of users reported that the safety reminder function of the system of the present invention significantly improved their operational confidence.

[0088] Improve the automation and intelligence level of the system: The present invention realizes intelligent motion planning during the physical therapy process through automated boundary detection and singular point avoidance algorithms, reducing the user's manual intervention in the robotic arm. This not only improves the intelligence level of the system, but also significantly improves the efficiency and accuracy of physical therapy.

[0089] The experimental test results show that the intelligent space planning algorithm of the present invention can reduce 80% of the manual intervention operation time, further improving the automation level of the physical therapy process.

[0090] Compared with the traditional robot arm control method, the automation level of the present invention is improved by about 40%, and the user's operation burden is greatly reduced.

[0091] In this embodiment, the entire operation process can be controlled by a computer, and with the addition of a PLC, etc., automated operation control can be achieved. In each operation link, sensors can be set up to provide signal feedback to achieve sequential execution of steps. These are all common knowledge of current automated control and will not be described in detail in this embodiment.

[0092] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A robot motion control method based on safety and space planning, characterized in that: Applied to the robotic arm system, the specific steps include: S101: receiving a movement value inputted by a user terminal through a joystick value, triggering the start of the robotic arm system, and calculating a target position after the robotic arm moves; S102: The robotic arm system obtains the current position of the robotic arm, and uses the current position to set the boundary of the physical therapy area; S103: Calculating the moving distance of the robot arm according to the moving distance and the offset; S104: Obtain and calculate the target position after the robot arm moves, perform verification based on the moving distance of the robot arm, obtain the final movable distance of the robot arm, and execute the robot arm to complete the movement action.

2. The robot motion control method based on safety and space planning as claimed in claim 1, characterized in that: In S101, receiving a movement value inputted by a user terminal through a joystick value includes: Receiving a connection request sent by a user terminal through a preset network, wherein the connection request is used to request to establish a connection with the robotic arm system; Detecting whether the current account of the user terminal is the target account; If the current account of the user terminal is the target account, the user terminal is connected according to the connection request, and after the connection is completed, the movement value input by the user terminal through the joystick value is received.

3. The robot motion control method based on safety and space planning as claimed in claim 2, characterized in that: The preset network includes one or a combination of a 3G network, a 4G network, a 5G network, and a WIFI network.

4. The robot motion control method based on safety and space planning as claimed in claim 3, characterized in that: In S102, the robotic arm system obtains the current position of the robotic arm and uses the current position to set the boundary of the treatment area, including: By setting the boundary of the therapy area, the movement of the robot arm is limited to a safe range. The boundary of the therapy area is represented by a four-dimensional space, where the x and y axes are the horizontal and vertical directions respectively, the z axis is the height direction, and θ is the posture angle. The set area boundary can be described as follows: space=[x min ,x max ,y min ,y max ] The current position of the robot arm (x, y, z, θ) is subject to the following restrictions within the treatment area: and min <y<y max ; x min <x<x max 。 5. The robot motion control method based on safety and space planning as claimed in claim 4, characterized in that: In S103, the moving distance of the robot arm is obtained according to the moving distance and the offset, including: By dynamically calculating the position of each joint of the robot arm, when moving in the y-axis direction, the movable range of the x-axis is dynamically calculated to prevent the robot arm from entering a singular point. The geometric formula is: In the formula, x max is the maximum x-axis motion range of the robot arm when it is at a certain position on the y-axis, and y is the current position of the robot arm on the y-axis. The qBound function is used to limit the x-axis position of the robot arm to the calculated range.

6. The robot motion control method based on safety and space planning as claimed in claim 5, characterized in that: In S104, the moving distance of the robot arm is verified to obtain the final movable distance of the robot arm, including: When the user terminal passes the joystick movement instruction, the robotic arm system first calculates the target position after the robotic arm moves, and obtains the target position after the robotic arm moves; The feasibility of the target position is verified by boundary setting and singular point calculation. If the target position exceeds the allowable range, the system will automatically limit the position and issue a warning signal.

7. The robot motion control method based on safety and space planning as claimed in claim 6, characterized in that: In S104, the robot arm is executed to complete the motion action, specifically: A monitoring module is provided on the robotic arm, and the monitoring module is used to monitor the movement of the robotic arm in real time to ensure that there is no excessive stretching of the joints or collision of the robotic arm at any time.

8. The robot motion control method based on safety and space planning as claimed in claim 7, characterized in that: The robotic arm system can remind the user through sound and light alarms when the robotic arm approaches or reaches the boundary setting area. If the robotic arm is about to cross the boundary or has an abnormal posture, the robotic arm system will automatically trigger the LED light to flash, remind the user through the voice system, and limit the movement of the robotic arm.

9. The robot motion control method based on safety and space planning as claimed in claim 8, characterized in that: qBound (function formula: Qbound = ΣΔx^2 / N or Qbound = ΣΔy^2 / N; Among them, Δx and Δy are the average values ​​of a given data set, that is, (x max -x min ) or (y max -y min ), N is the number of samples in the data set, the x-axis and y-axis positions of the robot arm are restricted to the calculated range, Δx is the increment in the x-axis direction input by the user, and Δy is the increment in the y-axis direction input by the user.

10. Application of the robot motion control method based on safety and space planning as described in any one of claims 1 to 9 to an intelligent physiotherapy robot.

Images