A method for estimating the operating boundary position and constraining the posture of a physiotherapy robot

Abstract

This invention belongs to the field of physical therapy robots and discloses a method for estimating the operating boundary position and constraining the posture of a physical therapy robot. By establishing a coordinate system, the movement range of a physical therapy head is determined. Based on the physical therapy head's position and user movement instructions, the method determines whether the physical therapy head can move and how to move it. The robot's operating range is defined to meet the needs of physical therapy application scenarios. By calculating the appropriate position and posture of the physical therapy head at the desired physical therapy area, the robot's flexibility when operating at that area is optimized, further improving its operability and stability, making it more convenient for users.

Description

Technical Field

[0001] The present invention relates to the field of physiotherapy robots, and in particular to a method for estimating the operation boundary position and constraining the posture of a physiotherapy robot. Background Art

[0002] Currently, physical therapy is typically performed manually through repetitive mechanical manipulation. This repetitive operation consumes a significant amount of the operator's time and energy, resulting in low clinical efficiency. Therefore, physical therapy robots have emerged on the market to replace human operators in performing repetitive physical therapy tasks.

[0003] However, the existing physiotherapy robots on the market have not yet involved the limitation of the physiotherapy work area, and are unable to limit and constrain the safe operating boundaries of the physiotherapy robots performing physiotherapy operations, which may easily cause the robot's activities to exceed the expected range, collide with other objects or people, and affect the physiotherapy work; there are also a few physiotherapy robots that have strategies for limiting the range of activities, but due to different purposes of the limitation strategies, or poor limitation strategies, the physiotherapy robot's range of activities is too small, or collisions with external objects and people cannot be avoided during physiotherapy operations.

[0004] Therefore, the problem that this solution solves is how to ensure that the therapy head moves within the specified range of movement when the therapy robot is working, to avoid unconstrained movement that affects the therapy work and collides with external objects or people, and then provide a simple and efficient algorithm strategy to solve the above problem, and perform vector constraints on the movement according to the position of the therapy head. Summary of the Invention

[0005] The purpose of the present invention is to provide a method for estimating the operating boundary position and constraining the posture of a physiotherapy robot, so as to determine whether the physiotherapy head can continue to move when the physiotherapy robot is working, and ensure that the physiotherapy head moves within a specified range of movement, thereby avoiding unconstrained movement that affects the physiotherapy work and collides with external objects or people.

[0006] The technical solution provided by the present invention is: a method for estimating the operating boundary position of a physiotherapy robot, comprising the following steps:

[0007] S1 determines the base coordinate system. The base coordinate system takes the end where the six-axis robot is connected to the support mobile module as the origin, which is recorded as point O;

[0008] S2 determines the movement range of the physiotherapy head, and the boundaries of the movement range are distributed in a circle with the base coordinate origin as the center;

[0009] S3 identifies the location of the therapy head, which is recorded as point a;

[0010] The S31 control vector is recorded as Let the moving direction vector and The absolute value of the angle is θ∈[0,π]; let the movable distance Set the radius of the range circle for the physiotherapy robot to move, and let the angle between the two sides of the triangle formed by Oa and aD be

[0011] S32 determines whether the therapy head is within the moving range;

[0012] S4 user operates the button to input the direction vector to calculate the moving distance and the posture constraint of the physiotherapy head.

[0013] In the above-mentioned method for estimating the operation boundary position of the physiotherapy robot, when it is determined in step S32 that the physiotherapy head is within the moving range, the calculation method of step S4 is as follows:

[0014] S41 inputs direction vector through user operation button calculate and The angle θ:

[0015]

[0016] S42 solves the quadratic equation to obtain the distance X between the origin of the tool coordinate system and the boundary range:

[0017]

[0018] Since the physiotherapy head is in the movable circle, that is, A 2 <=B 2 , there is always a solution, find the value X>0, according to the moving direction unit vector and the moving distance X to obtain the moving target position D.

[0019] In the above-mentioned method for estimating the operation boundary position of the physiotherapy robot, when it is determined in step S32 that the physiotherapy head is not within the movement range, the angle between the movement direction vector and the physiotherapy head is calculated, and then a determination is made as to whether the movement is to be made. The determination process is as follows: Calculate the angle ψ between the tangent line passing through the physiotherapy head and Oa: because and Ψ is positive;

[0020] If ψ+θ≥π, the moving direction and the range intersect, and the physiotherapy head can be moved into the working range;

[0021] If ψ+θ<π, the moving direction has no intersection with the range, and the physiotherapy head cannot move into the working range. At this time, the movement of the physiotherapy head is restricted.

[0022] In the above-mentioned method for estimating the operating boundary position of the physiotherapy robot, when the moving direction vector is outside the angle between the two tangents of the physiotherapy head and the boundary of the moving range, that is, ψ+θ<π, and there is no intersection between the moving direction and the moving range, the physiotherapy head cannot move into the working range, and the movement of the physiotherapy head is restricted at this time.

[0023] In the above-mentioned method for estimating the operating boundary position of the physiotherapy robot, when the moving direction vector is within the angle between the two tangents of the physiotherapy head and the movement range boundary or is exactly on the tangent, that is, ψ+θ≥π, and the moving direction and the movement range have an intersection, the physiotherapy head can move. In this case, the calculation method of step S4 is as follows:

[0024] Direction vector is passed in through user operation button calculate and The angle θ;

[0025] Calculate the angle between the two tangent lines of the therapy head and the boundary of the movement range as Ψ:

[0026]

[0027] The value range of ψ calculated by inverse sine is ψ>0 when and The absolute value of the angle is θ:

[0028] π≤θ+ψ

[0029] The therapy head can move, the moving distance is;

[0030]

[0031] The moving distance X is calculated.

[0032] In the above-mentioned method for estimating the operation boundary position of the physiotherapy robot, when it is determined in step S32 that the physiotherapy head is on the boundary of the movement range, the angle between the movement direction vector of the physiotherapy head and the position vector of the physiotherapy head is calculated, and it is determined whether it moves.

[0033] In the above-mentioned method for estimating the operation boundary position of the physiotherapy robot, when the angle between the movement direction vector of the physiotherapy head and the position vector of the physiotherapy head is less than or equal to 90°, that is, cosθ≥0, the physiotherapy head cannot move into the working range, and the movement of the physiotherapy head is restricted at this time.

[0034] In the above-mentioned method for estimating the operation boundary position of the physiotherapy robot, when the angle between the movement direction vector of the physiotherapy head and the position vector of the physiotherapy head is greater than 90°, that is, cosθ<0, the physiotherapy head can move. In this case, the calculation method of step S4 is as follows:

[0035] S41 inputs direction vector through user operation button calculate and The angle θ;

[0036] S42 is calculated in The intersection with the moving range circle is used to calculate the moving distance X according to the law of cosines.

[0037]

[0038] The calculated moving distance is X. In order to meet the equipment movement operation requirements, the position coordinates of the desired point D are limited to (x, y), where x ≥ 330.

[0039] The present invention also provides a posture constraint method for estimating the operating boundary position of the above-mentioned physiotherapy robot. The technical solution is: obtain the current position of the physiotherapy robot and the six joint angle information when the robot determines the posture of the physiotherapy part, substitute the six joint angle information into the Cartesian coordinates of the end position of the physiotherapy head, calculate the moving angle q1, and further obtain the end posture of the physiotherapy robot when determining the physiotherapy part.

[0040] In the above-mentioned physiotherapy robot posture constraint method, the calculation method of the moving angle q1 is as follows:

[0041] According to the position of the physiotherapy head before movement (x1, y1), the position after movement (x2, y2) and the distance A from the physiotherapy head to the base coordinate origin:

[0042]

[0043] The terminal posture calculation method of the physiotherapy robot is as follows:

[0044] Substitute into the robotic arm posture matrix:

[0045]

[0046] Get the end posture when moving to this point.

[0047] After adopting the above technical solution, the present invention has the following beneficial effects:

[0048] This solution sets a coordinate system and determines that the movement of the physiotherapy head is within a certain range, thereby ensuring that the physiotherapy head works normally and avoids colliding with foreign objects or people; before moving, the position of the physiotherapy head is first identified, and it is determined whether it is within the moving range, so that subsequent operations are performed, avoiding the situation where the physiotherapy head "goes further and further away" when it is outside the moving range; at the same time, when the physiotherapy head is on the boundary of the moving range or within the moving range, the moving direction vector constraint is calculated based on the control vector and the position of the physiotherapy head, and it is determined in which cases the physiotherapy head will exceed the moving range and therefore its movement needs to be restricted, and in which cases it will not exceed the moving range so that it can move normally; in addition, this application also determines the end posture of the robot when the physiotherapy part is treated by calculating the joint angle and the position of the physiotherapy head, so that the robot can further optimize the working range. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 It is a structural schematic diagram of the physiotherapy robot of the present invention;

[0050] Figure 2 Schematic diagram of the movement direction of the physiotherapy head of the present invention within the movement range;

[0051] Figure 3 This is a schematic diagram of the physiotherapy head of the present invention being out of the moving range and unable to move;

[0052] Figure 4 This is a schematic diagram of the physiotherapy head of the present invention being out of the moving range but movable;

[0053] Figure 5 Schematic diagram of the movement direction of the physiotherapy head of the present invention at the boundary of the movement range.

[0054] Figure numerals: 1. Support and moving module; 2. Six-axis robot; 3. Physiotherapy head. DETAILED DESCRIPTION

[0055] The technical solution of the present invention will be further described in detail below in conjunction with specific implementation methods, but this does not constitute any limitation to the present invention.

[0056] like Figure 1 As shown, the physiotherapy robot includes a support and movement module, a six-axis robot and a physiotherapy head. The support and movement module acts as a movable base, and one end of the six-axis robot is connected to the support and movement module, and the other end is connected to the physiotherapy head.

[0057] The method for estimating the operating boundary position of the physiotherapy robot in this application is as follows:

[0058] Step S1: Establish a base coordinate system with one end of the six-axis robot connected to the supporting mobile module as the origin O;

[0059] Step S2: setting the movement range of the physiotherapy head, where the boundaries of the movement range are distributed in a circle with the base coordinate origin as the center;

[0060] Step S3: record the position of the physiotherapy head as point a;

[0061] The S31 control vector is recorded as Let the moving direction vector and The absolute value of the angle is θ∈[0,π]; let the movable distance Set the radius of the range circle for the physiotherapy robot to move, and let the angle between the two sides of the triangle formed by Oa and aD be

[0062] S32 determines whether the therapy head is within the moving range;

[0063] S4 user operates the button to input the direction vector to calculate the moving distance and the posture constraint of the physiotherapy head.

[0064] In the above technical solution, since the physiotherapy robot has a limited strategy for movement in height, this application will not make additional introductions here, but will focus on the movement strategy in the plane, that is, the base coordinate system of this application is a plane coordinate system when projected to a certain horizontal height parallel to the ground in actual operation. Before the six-axis robot drives the physiotherapy head to move, the system first identifies the current position of the physiotherapy head and determines whether the physiotherapy head is within the preset movement range. According to the result of the above judgment, different calculation methods are selected to calculate the movement distance and direction vector constraints to avoid the physiotherapy head from moving freely without constraints and affecting the physiotherapy work, so that the physiotherapy head moves within the preset movement range, ensuring that it can work normally and will not touch external objects or people, thereby improving safety and reliability. In addition, it should also be noted that in this application, The radius of the range circle set for the physiotherapy robot's movement is a fixed value that is specified in advance.

[0065] Different embodiments are introduced below according to different operating conditions.

[0066] Example 1:

[0067] In actual operation, when it is determined in step S32 that the physiotherapy head is within the preset movement range, the reference Figure 2 , the calculation method of step S4 is as follows:

[0068] Step S41: Direction vector is input through user operation button calculate and The angle θ:

[0069]

[0070] Step S42: Solve the quadratic equation to obtain the distance X between the origin of the tool coordinate system and the boundary range:

[0071]

[0072] Since the physiotherapy head is in the movable circle, that is, A 2 <=B 2 , there is always a solution, find the value X>0, according to the moving direction unit vector and the moving distance X to obtain the moving target position D.

[0073] In the above technical solution, the current position of the physiotherapy head is first identified and determined. Since the physiotherapy head is within the pre-set movement range at this time, there are relatively few constraints on the physiotherapy head. It is only necessary to ensure that it remains within the movement range after moving and does not exceed the boundary of the movement range. Therefore, the physiotherapy head can move in any direction at this time. The difference is that the movement distance in different directions is different. By using the cosine theorem, the movement direction can be obtained. and The variable relationship between the angle θ and the movable distance X can be used to obtain the vector constraint of the moving direction based on the relationship between the two, thereby ensuring that the next movement of the physiotherapy head is still within the preset moving range, can perform normal physiotherapy operations, and avoid collision with external objects or people, which is safe and reliable.

[0074] Example 2:

[0075] In actual operation, when it is determined in step S32 that the physiotherapy head is not within the preset movement range, the angle between the movement vector direction and the physiotherapy head is first calculated, and then it is determined whether to move.

[0076] In the above technical solution, the physiotherapy head is not within the preset moving range. Since there are two situations at this time, one is that the moving direction vector is facing a direction away from the set moving range, and the other is that the moving direction vector is facing a direction close to the set moving direction. The two situations require different strategies. Therefore, the angle between the moving direction vector and the physiotherapy head is first calculated, and then a subsequent judgment is made based on the angle to decide whether to restrict the movement of the physiotherapy head or impose corresponding constraints.

[0077] It should be emphasized that since the boundary of the moving range is a circle, and the position of the physiotherapy head is a point outside the circle, two tangents passing through the position of the physiotherapy head can be obtained by drawing tangents along the circumference. The judgment of the angle between the moving direction vector and the physiotherapy head in the above technical solution is based on this. Furthermore, the basis for distinguishing the inside and outside of the angle between the two tangents of the physiotherapy head and the moving range boundary is that the side where the two tangents intersect with the moving range boundary is inside the angle, and vice versa. A more detailed judgment process is as follows: Calculate the angle ψ between the tangent passing through the physiotherapy head and Oa: because and Ψ is positive;

[0078] If ψ+θ≥π, the moving direction and the range have an intersection, and the physiotherapy head can be moved into the working range;

[0079] If ψ+θ<π, then the moving direction and the range have no intersection, and the physiotherapy head cannot move into the working range. In this case, the movement of the physiotherapy head is restricted. The following technical solutions are similar and will not be repeated here.

[0080] like Figure 3 As shown, when the moving direction vector is outside the angle between the two tangents of the physiotherapy head and the boundary of the moving range (it should be noted that in order to maintain consistency in expression, the moving vector is still written as But in Figure 3 The picture in the middle is The same below). Obviously, in this case, if the movement of the therapy head is not restricted, the therapy head will be further away from the preset movement range and cannot be moved into the working range. Therefore, the movement of the therapy head is restricted at this time.

[0081] like Figure 4 As shown, when the moving direction vector is within the angle between the therapy head and the boundary of the moving range or just on the tangent line (it should be noted that in order to maintain consistency in expression, the moving vector is still written as But in Figure 4 The picture in the middle is The same below), the movement of the physiotherapy head will approach the movement range until it reaches the movement range, so the physiotherapy head can move. At this time, the calculation method of step S4 is as follows:

[0082] Direction vector is passed in through user operation button calculate and The angle θ:

[0083] S42 calculates the angle between the two tangent lines of the therapy head and the boundary of the movement range as Ψ:

[0084]

[0085] The value range of ψ calculated by inverse sine is ψ>0 when and The absolute value of the angle is θ:

[0086] π≤θ+ψ

[0087] The therapy head can move, the moving distance is;

[0088]

[0089] The moving distance X is calculated.

[0090] In the above technical solution, there are two situations when the position of the therapy head is outside the pre-set movement range. The therapy head can only be smoothly moved into the pre-set movement range when the movement direction vector intersects the movement range. Therefore, in this embodiment, after identifying the position of the therapy head and determining that the therapy head is outside the movement range, it is necessary to further determine the direction of the movement direction vector, thereby adopting a strategy of restricting or allowing the movement of the therapy head. After determining that the movement direction is toward the movement range, the movement vector constraint is determined based on the variable relationship between the angle θ and the movement distance X using the law of cosines.

[0091] Example 3:

[0092] In actual work, Figure 5 As shown, when it is determined in step S32 that the physiotherapy head is on the boundary of the preset movement range (it should be noted that in order to maintain consistency in expression, the movement vector is still written as But in Figure 5 The picture in the middle is The same below) calculates the angle between the moving direction vector of the physiotherapy head and the position vector of the physiotherapy head, and determines whether to move.

[0093] When the angle between the moving direction vector of the physiotherapy head and the position vector of the physiotherapy head is less than or equal to 90°, that is, cosθ≥0, the physiotherapy head cannot move into the working range, and the movement of the physiotherapy head is restricted.

[0094] When the angle between the moving direction vector of the physiotherapy head and the position vector of the physiotherapy head is greater than 90°, that is, cosθ<0, the physiotherapy head can move. At this time, the calculation method of step S4 is as follows:

[0095] S41 inputs direction vector through user operation button calculate and The angle θ:

[0096] S42 is calculated in The intersection with the moving range circle is used to calculate the moving distance X according to the law of cosines.

[0097]

[0098] The calculated moving distance is X. In order to meet the equipment movement operation requirements, the position coordinates of the desired point D are limited to (x, y) where x≥330.

[0099] In the above technical solution, when the therapy head is exactly on the boundary of the pre-set movement range, there are two situations that require different execution strategies. Therefore, the angle between the movement direction vector of the therapy head and the position vector of the therapy head is calculated first, and then a judgment is made to decide whether to move and execute different strategies.

[0100] Specifically, if the angle between the movement direction vector of the therapy head and its position vector is less than or equal to 90°, the therapy head will leave the boundary of the movement range and move in a direction away from the movement range, resulting in it being unable to move within the pre-set movement range. Therefore, it is necessary to restrict the movement of the therapy head to avoid this situation. However, if the angle between the movement direction vector of the therapy head and its position vector is greater than or equal to 90°, the therapy head can move from the boundary of the movement range to within the movement range, thus allowing movement. In this case, the movement vector constraint is determined based on the variable relationship between the angle θ and the movement distance X using the law of cosines.

[0101] Example 4:

[0102] In actual operation, since the physiotherapy head moves under the drive of the six-axis robot, when the physiotherapy head completes the determination of the physiotherapy part, the six joints of the robot will have corresponding rotation angle information. Therefore, in order to realize the posture constraint of the physiotherapy robot, this embodiment obtains the current position of the physiotherapy robot when the physiotherapy head completes the determination of the physiotherapy part, and simultaneously obtains the six joint rotation angle information of the six-axis robot in this state, and substitutes the six joint rotation angle information into the Cartesian coordinates of the end position of the physiotherapy head, calculates the moving angle q1, and further obtains the end posture of the physiotherapy robot when the physiotherapy part is determined.

[0103] Specifically, the calculation method of the moving angle q1 is as follows:

[0104] First, obtain the position (x1, y1) of the physiotherapy head before movement, the position (x2, y2) after movement, and the distance A from the physiotherapy head to the base coordinate origin. Substitute the above data into the following formula:

[0105]

[0106] Based on the obtained moving angle, the terminal posture of the physiotherapy robot is calculated as follows:

[0107] Substitute into the robotic arm posture matrix:

[0108]

[0109] Get the end posture when moving to this point.

[0110] The above embodiments are preferred implementations of the present invention, but the implementations of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be considered as equivalent replacement methods and are included in the scope of protection of the present invention.

Claims

1. A method for estimating the operating boundary position of a physiotherapy robot, characterized in that: The following steps are involved: S1 determines the base coordinate system. The base coordinate system takes the end where the six-axis robot is connected to the support mobile module as the origin, which is recorded as point 0; S2 determines the movement range of the physiotherapy head, and the boundaries of the movement range are distributed in a circle with the base coordinate origin as the center; S3 identifies the location of the therapy head, which is recorded as point a; The S31 control vector is recorded as Let the moving direction vector and The absolute value of the angle is θ∈[0,π]; let the movable distance Set the radius of the range circle for the physiotherapy robot to move, and let the angle between the two sides of the triangle formed by Oa and aD be S32 determines whether the therapy head is within the moving range; S4 user operates the button to input the direction vector to calculate the moving distance and the posture constraint of the physiotherapy head; When it is determined in step S32 that the physiotherapy head is within the moving range, the calculation method in step S4 is as follows: S41 inputs direction vector through user operation button calculate and The angle θ: S42 solves the quadratic equation to obtain the distance X between the origin of the tool coordinate system and the boundary range: Since the physiotherapy head is in the movable circle, that is, A 2 <=B 2 , there is always a solution, find the value X>0, according to the moving direction unit vector and the moving distance X to obtain the moving target position D.

2. The method for estimating the operation boundary position of a physiotherapy robot according to claim 1, characterized in that: When it is determined in step S32 that the physiotherapy head is not within the moving range, the angle between the moving direction vector and the physiotherapy head is calculated to determine whether to move.

3. The method for estimating the operation boundary position of a physiotherapy robot according to claim 2, characterized in that: When the moving direction vector is outside the angle between the two tangents of the physiotherapy head and the boundary of the moving range, the physiotherapy head cannot move into the working range, and the movement of the physiotherapy head is restricted.

4. The method for estimating the operation boundary position of a physiotherapy robot according to claim 2, characterized in that: When the moving direction vector is within the angle between the two tangent lines between the physiotherapy head and the boundary of the moving range or is exactly on the tangent line, the physiotherapy head can move. In this case, the calculation method of step S4 is as follows: S41 inputs direction vector through user operation button calculate and The angle θ: S42 calculates the angle between the two tangent lines of the physiotherapy head and the boundary of the moving range as Ψ The value range of ψ calculated by arcsine is ψ>0. and The absolute value of the angle is θ: π≤θ+ψ The therapy head can move, the moving distance is; The moving distance X is calculated.

5. The method for estimating the operation boundary position of a physiotherapy robot according to claim 1, characterized in that: When it is determined in step S32 that the physiotherapy head is on the boundary of the movement range, the angle between the movement direction vector of the physiotherapy head and the position vector of the physiotherapy head is calculated, and it is determined whether the physiotherapy head is moving.

6. The method for estimating the operation boundary position of a physiotherapy robot according to claim 5, characterized in that: When the angle between the moving direction vector of the physiotherapy head and the position vector of the physiotherapy head That is, cosθ≥0, the therapy head cannot move into the working range, and the movement of the therapy head is restricted at this time.

7. The method for estimating the operation boundary position of a physiotherapy robot according to claim 5, characterized in that: When cosθ<0, the therapy head can move. At this time, the calculation method of step S4 is as follows: S41 inputs direction vector through user operation button calculate and The angle θ: S42 is calculated in The intersection point with the moving range circle and the moving distance X is calculated according to the cosine theorem:

8. The posture constraint method of the method for estimating the operation boundary position of a physiotherapy robot according to any one of claims 1 to 7, characterized in that: Obtain the current position of the therapy robot and the six joint angle information when the robot determines the posture of the therapy part, substitute the six joint angle information into the rotation matrix of the end position of the therapy head, and calculate the moving angle q1 based on the known expected movement position D. Then, substitute it into the end tool coordinate system matrix to obtain the end posture of the therapy robot when determining the therapy part.

9. The posture constraint method according to claim 8, characterized in that: The calculation method of the moving angle q1 is as follows: According to the position of the physiotherapy head before movement (x1, y1), the position after movement (x2, y2) and the distance A from the physiotherapy head to the base coordinate origin:

Images