A method for obstacle avoidance control based on RCM robot motion

Abstract

The application relates to an obstacle avoidance control method based on RCM robot motion, which comprises the following steps: a first operation area and a second operation area located in the first operation area are provided; an executing mechanism pre-enters the first operation area from a point A on the surface of the first operation area at an entering angle theta 1 and along a first entering straight line; it is judged whether the first entering straight line of the executing mechanism intersects with the second operation area; if yes, the executing mechanism enters the needle along the first entering straight line to a depth of L1, and then RCM rotation is performed around a remote motion center point A by theta 2 to obtain a second entering straight line at a needle entering angle; otherwise, the executing mechanism enters the needle along the first entering straight line to a depth of L2, and then RCM rotation is performed around the remote motion center point A by theta 3 to obtain a second entering straight line at a needle entering angle; the executing mechanism enters the needle along the second entering straight line to a depth of L3 to an executing depth; and the executing mechanism performs RCM rotation around the remote motion center point A by theta 7 to an injection angle. The technical scheme of the application can autonomously avoid the second operation area.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of automation control, and more particularly, to an obstacle avoidance control method based on RCM robot motion. BACKGROUND

[0002] RCM (Remote Center of Motion) is a kind of mechanism constraint mode with remote center of motion as constraint point, which can rotate around the remote center of motion without physical rotation joint at the constraint point. RCM robot can perform operations inside the structure without damaging the structure surface. During the operation, RCM acts as a clamping arm of the execution mechanism, which can freely rotate and enter and exit the opening of the structure surface without collision with the opening.

[0003] The existing RCM robot mainly includes spherical robot arm, parallelogram mechanism and circular arc mechanism. The demand for operation inside the structure only provides a structural solution. In actual operation, in order to solve the problem of multi-interface and obstacle avoidance, the algorithm of the motion path of the execution mechanism of the RCM robot structure needs to be provided.

[0004] Patent CN114452003A discloses a surgical robot control method, comprising: collecting the hand action speed of the operator at the master hand; controlling the motion of the slave hand according to the hand action speed; controlling the motion of the slave hand according to the hand action speed; controlling the motion of the slave hand according to the hand action speed; first controlling the general robot to drive the instrument robot to pass through the target position and enter the target object, and then controlling the general robot to move around the remote center of motion and the instrument robot to stretch and retract, so that the instrument robot performs the set operation. The above-mentioned surgical robot control method controls the surgical robot through the control mode of master-slave hand, has high control precision and is convenient and accurate to operate; in the control mode of slave hand, the general robot and the instrument robot are divided into two parts, which can be used for rotating and stretching around the remote center of motion, and adjusting the position of the remote center of motion.

[0005] The existing technical solution can make the RCM robot rotate and stretch around the remote center of motion inside the structure after entering the structure surface. However, it still cannot realize the function of autonomous obstacle avoidance inside the structure. SUMMARY

[0006] In order to overcome the problem that the existing technology cannot autonomously avoid obstacles, which leads to the destruction of the internal structure during operation inside the structure, the present application provides an obstacle avoidance control method based on RCM robot motion.

[0007] To solve the above technical problems, the technical solution adopted by the present application is: an obstacle avoidance control method based on RCM robot motion, comprising the following steps:

[0008] S1: having a first operation area and a second operation area within the first operation area, the execution mechanism pre-enters the first operation area from a point A on the surface outside the first operation area at an entering angle θ1 and along a first entering straight line;

[0009] S2: judging whether the first entering straight line of the execution mechanism intersects with the second operation area, if intersecting, executing step S3; if not intersecting, executing step S4;

[0010] S3: the execution structure enters the needle depth L1 along the direction of the first entering straight line, then rotates RCM around the remote motion center point A by θ2 to obtain the second entering straight line where the needle entering angle is located, and then executes step S5;

[0011] S4: the execution structure enters the needle depth L2 along the direction of the first entering straight line, then rotates RCM around the remote motion center point A by θ3 to obtain the second entering straight line where the needle entering angle is located, and then executes step S5;

[0012] S5: the execution mechanism enters the needle depth L3 along the second entering straight line to the execution depth;

[0013] S6: the execution mechanism rotates RCM around the remote motion center point A by θ7 to the injection angle.

[0014] Preferably, in the step S1, the first operation area is a circular ring with an outer diameter r2 and an inner diameter r2-d; the second operation area is a circle concentric with the circular ring, the radius of the second operation area is r1, and r1

[0015] Preferably, in the step S2, whether the first entering straight line intersects with the second operation area is judged by the following steps:

[0016] S21: calculating the coordinates (x A ,y A ) of point A: Wherein, h is the distance from point A to the equatorial diameter;

[0017] S22: calculating the absolute value of the angle of the straight line passing through point A and tangent to the boundary of the second operation area by the formula:

[0018]

[0019] calculating the absolute value of the angle of the straight line passing through point A and tangent to the boundary of the second operation area, wherein K1 is the slope of the tangent line with the smallest slope among the tangent lines of the boundary of the second operation area passing through point A, the tangent point is E, and θ6 is the absolute value of the tangent angle;

[0020] S23: If the entering angle θ1 is less than θ6 and greater than 45°, the first entering straight line of the actuating mechanism intersects the second operation region, and if the entering angle θ1 is greater than θ6, the first entering straight line of the actuating mechanism does not intersect the second operation region.

[0021] Preferably, the intersection of the first entering straight line and the inner surface of the first operation region is I, and the coordinates of point I are (x I ,y I ).

[0022] The coordinates (x I ,y I ) of point I are obtained from the left formula.

[0023] Preferably, in the step S4, if the first entering straight line does not intersect the second operation region, the needle insertion depth L2 should satisfy that it passes through the boundary of the first operation region, i.e. is greater than the length of the line segment AI, i.e.

[0024] The coordinates (x I ,y I ) of point I are obtained from the left formula.

[0025]

[0026] In the step S3, if the first entering straight line intersects the second operation region, the intersection of the first entering straight line and the second operation region is C, and the coordinates of point C are (x c ,y c ). The needle insertion depth L1 should satisfy that it passes through the boundary of the first operation region, i.e. is greater than the length of the line segment AI and is less than the length of the line segment AC.

[0027] The coordinates (x C ,y C ) of point C are obtained from the left formula.

[0028]

[0029] Preferably, in the step S4, if the first entering straight line does not intersect the second operation region, the needle insertion depth L2 should satisfy the following formula:

[0030]

[0031] In the step S3, if the first entering straight line intersects the second operation region, the needle insertion depth L1 should satisfy the following formula:

[0032]

[0033] Preferably, in the step S3, the angle θ6 of the needle entry that touches the boundary of the second operation region is obtained by the step S23, and the angle after the RCM rotation should be greater than the angle θ6 to achieve the needle entry to the bottom of the second operation region, and the minimum rotation angle of the RCM rotation is solved first;

[0034] When the angle θ1 of the needle entry is greater than the angle θ6 of the needle entry that touches the lens, the minimum rotation angle of the RCM rotation is zero;

[0035] When the angle θ1 of the needle entry is less than the angle θ6 of the needle entry that touches the lens, the minimum rotation angle of the RCM rotation should be:

[0036] θ5=θ6-θ1

[0037] The maximum rotation angle of the RCM rotation to the needle entry angle is solved again, and the steps are as follows: assuming that point G is the point on the inner surface of the first operation region with the maximum vertical distance from point A, the angle of the straight line passing through AG is the injection angle, the line segment AG is translated to be tangent to the second operation interface and a tangent point F is obtained, the distance between the tangent point F and point A is the minimum depth h1 of the needle entry, and when the line segment AF is rotated by the RCM to touch the point D on the inner surface of the first operation region, the absolute value of the angle of the straight line passing through AD is the maximum needle entry angle;

[0038]

[0039] wherein k2 is the slope of the injection angle, and the slope at this time should be negative according to the coordinate system, and the coordinates (x F ,y F ) of point F are obtained by the following formula;

[0040]

[0041] The coordinates (x D ,y D ) of point D are obtained by the following formulas;

[0042]

[0043]

[0044] The coordinates of the point D on the inner surface of the first operation region are obtained from the above formula;

[0045] Then, the maximum rotation angle of the RCM rotation is solved according to the above calculation:

[0046]

[0047] When the first entering straight line does not intersect the second operation interface, the range of the RCM rotation to the needle entry angle is:

[0048] 0 < θ2 < θ4;

[0049] When the first entering straight line intersects with the second operation interface, the range of the rotation angle of the RCM to the needle insertion angle is:

[0050] θ5 < θ3 < θ4.

[0051] Preferably, in the step S5, to ensure that the reachable depth of the needle insertion does not touch and damage the inner surface of the first operation region, the needle insertion angle intersects with the inner surface of the first operation region at a point K;

[0052] The coordinates of the point K are (x K ,y K )

[0053] When the first entering straight line intersects with the second operation region, the maximum depth of the needle insertion is the distance of the line segment AK minus the needle insertion depth L1, and the needle insertion depth L3 should satisfy the following formula:

[0054]

[0055] When the first entering straight line does not intersect with the second operation region, the maximum depth of the needle insertion is the distance of the line segment AK minus the needle insertion depth L2, and the needle insertion depth L3 should satisfy the following formula:

[0056]

[0057] Preferably, when the first entering straight line intersects with the second operation region, the rotation angle θ7 of the RCM to the injection angle is:

[0058] θ7 = θ1 + θ2 - (|tan -1 k2(-π,π)|

[0059] When the first entering straight line does not intersect with the second operation region, the rotation angle θ7 of the RCM to the injection angle is:

[0060] θ7 = θ1 + θ3 - (|tan -1 k2(-π,π)|

[0061] Preferably, the first operation region is an eyeball of an animal, and the second operation region is a lens located in the eyeball.

[0062] Compared with the prior art, the present application has the following beneficial effects:

[0063] The application utilizes RCM rotary motion, and the RCM is fixedly kept at the point A on the outer surface of the first operation area, so that tearing does not occur at the point A, and damage to the point A during operation is reduced; the path planning in the first operation area is used to realize autonomous avoidance of the second operation area and movement to the execution position, so that the execution mechanism can avoid damaging the second operation area in the case of movement in the first operation area. BRIEF DESCRIPTION OF DRAWINGS

[0064] Figure 1 is a flowchart of the obstacle avoidance control method based on RCM robot motion of the application;

[0065] Figure 2 is a schematic diagram of the execution mechanism entering the needle along the direction of the first entering straight line intersecting the second operation area;

[0066] Figure 3 is a schematic diagram of the execution mechanism entering the needle along the direction of the first entering straight line not intersecting the second operation area;

[0067] Figure 4 is a schematic diagram of the execution mechanism entering the needle along the direction of the first entering straight line intersecting the second operation area, and then performing RCM rotation θ2 at the point A to obtain the second entering straight line of the needle entering angle;

[0068] Figure 5 is a schematic diagram of the execution mechanism entering the needle along the direction of the first entering straight line not intersecting the second operation area, and then performing RCM rotation θ3 at the point A to obtain the second entering straight line of the needle entering angle;

[0069] Figure 6 is a schematic diagram of the execution mechanism entering the needle along the direction of the first entering straight line intersecting the second operation area, rotating, and then entering the needle;

[0070] Figure 7 is a schematic diagram of the execution mechanism entering the needle along the direction of the first entering straight line not intersecting the second operation area, rotating, and then entering the needle;

[0071] Figure 8 is a schematic diagram of the execution mechanism entering the needle along the direction of the first entering straight line intersecting the second operation area, rotating, entering the needle, and then performing RCM rotation θ7 to the injection angle;

[0072] Figure 9 is a schematic diagram of the execution mechanism entering the needle along the direction of the first entering straight line not intersecting the second operation area, rotating, entering the needle, and then performing RCM rotation θ7 to the injection angle;

[0073] Figure 10is a parameter description in a kind of obstacle avoidance control method based on RCM robot movement of the present application Figure 1 .

[0074] Figure 11 is a parameter description in a kind of obstacle avoidance control method based on RCM robot movement of the present application Figure 2 . DETAILED DESCRIPTION

[0075] The accompanying drawings are only used for illustrative description, and cannot be understood as limitation to the present patent; in order to better illustrate the present embodiment, some components in the drawings can be omitted, enlarged or reduced, and do not represent the size of actual product; for those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings can be omitted. The positional relationship described in the drawings is only used for illustrative description, and cannot be understood as limitation to the present patent.

[0076] The same or similar reference numerals in the drawings of the embodiments of the present application correspond to the same or similar components; in the description of the present application, it is understood that if there are terms such as "upper", "lower", "left", "right", "long", "short" and the like indicating the orientation or positional relationship, it is only for the convenience of describing the present application and simplifying the description, and does not indicate or imply that the device or element referred to must have a particular orientation, be constructed and operated in a particular orientation, therefore the terms describing the positional relationship in the drawings are only used for illustrative description, and cannot be understood as limitation to the present patent, and for those skilled in the art, the specific meaning of the above terms can be understood according to the specific situation.

[0077] The technical solutions of the present application will be further described in detail below through specific embodiments, and in conjunction with the drawings:

[0078] Embodiment 1

[0079] As shown in Figure 1 , a kind of obstacle avoidance control method based on RCM robot movement, including the following steps:

[0080] S1: with first operation area and second operation area located in the first operation area, executive mechanism from the point A on the surface of the first operation area outside with entering angle θ1 And along the first entering straight line pre-enter the first operation area;

[0081] S2: judge whether the first entering straight line of the executive mechanism intersects with the second operation area, if intersect, then execute step S3;If not intersect, then execute step S4;

[0082] S3: the executive structure along the direction of first entering straight line enters the depth of L1 (such as Figure 2As shown in FIG. 6, the first entering straight line is obtained by rotating the RCM around the remote center point A by θ1, and then the step S5 is performed. Figure 4 As shown in FIG. 6, the first entering straight line is obtained by rotating the RCM around the remote center point A by θ1, and then the step S5 is performed.

[0083] S4: The execution mechanism enters the needle along the first entering straight line to a depth of L2 (as shown in FIG. 6). Figure 3 As shown in FIG. 6, the second entering straight line is obtained by rotating the RCM around the remote center point A by θ3, and then the step S5 is performed. Figure 5 As shown in FIG. 6, the second entering straight line is obtained by rotating the RCM around the remote center point A by θ3, and then the step S5 is performed.

[0084] S5: The execution mechanism enters the needle along the second entering straight line to a depth of L3 to the execution depth (as shown in FIG. 6). Figure 6 、 Figure 7 As shown in FIG. 6, the second entering straight line is obtained by rotating the RCM around the remote center point A by θ3, and then the step S5 is performed.

[0085] S6: The execution mechanism rotates the RCM around the remote center point A by θ7 to the injection angle (as shown in FIG. 6). Figure 8 、 Figure 9 As shown in FIG. 6, the second entering straight line is obtained by rotating the RCM around the remote center point A by θ3, and then the step S5 is performed.

[0086] As shown in FIG. 6, the first entering straight line is obtained by rotating the RCM around the remote center point A by θ1, and then the step S5 is performed. Figure 2 As shown in FIG. 6, the first entering straight line is obtained by rotating the RCM around the remote center point A by θ1, and then the step S5 is performed.

[0087] In the step S1, the first operation region is a circular ring with an outer diameter of r2 and an inner diameter of r2-d; the second operation region is a circle concentric with the circular ring, and the radius of the second operation region is r1, and r1 < r2-d.

[0088] In the embodiment, two circles are used to simulate the mouse eyeball and the lens, the eyeball circle is simulated according to the size of the equator of the mouse eyeball, the mouse eyeball corresponds to the first operation region with a thickness d, the lens corresponds to the second operation region, since the lens sphere is irregular in the actual situation, the lens position is located below the anterior chamber of the eyeball, the vitreous cavity size constituted by the lens sphere and the eyeball is large from top to bottom, that is, the vitreous cavity depth near the fundus is larger than the vitreous cavity at the equator of the eyeball, in order to ensure the effectiveness of the algorithm in any case, the size of the eyeball circle is simulated according to the minimum equator of the actual six-week-old mouse, the size of the lens circle is constructed according to the minimum vitreous cavity width near the equator of the actual six-week-old mouse, at this time, the width of the vitreous cavity that can be operated in the experiment is the minimum width, if the algorithm meets the needle entering requirement in this state, it can be extended to the general case.

[0089] As shown in Figure 2 , a rectangular coordinate system is established, with point O as the circle point, the horizontal direction as the X-axis, and the vertical direction as the Y-axis, in the step S2, whether the first entering straight line intersects with the second operation region is judged by the following steps:

[0090] S21: calculating the coordinates (x A ,y A ) of point A: Wherein, h is the distance from point A to the equatorial diameter;

[0091] S22: calculating the absolute value of the angle of the straight line passing through point A and tangent to the boundary of the second operation region by the formula:

[0092]

[0093] The absolute value of the angle of the straight line passing through point A and tangent to the boundary of the second operation region is calculated, wherein K1 is the slope of the tangent line with the smallest slope among the tangent lines passing through point A of the boundary of the second operation region, the tangent point is E, and θ6 is the absolute value of the tangent angle;

[0094] S23: if the entering angle θ1 is less than θ6 and greater than 45°, the first entering straight line of the execution mechanism will intersect with the second operation region, if the entering angle θ1 is greater than θ6, the first entering straight line of the execution mechanism will not intersect with the second operation region.

[0095] In the embodiment, it is first judged whether the needle entering along the first straight line will touch the second operation region, that is, the lens, if the needle is not entered according to the recommended best needle entering angle, the needle may be entered along the actual needle entering angle and will not touch the lens. Therefore, the angle range of the needle entering which will touch the second operation region, that is, the lens, should be found out first. As Figure 10 , Figure 11It can be seen that if the angle of the needle is greater than the tangent AE of the lens circle passing through point A, the lens will not be touched; if the angle of the needle is less than the tangent of the lens circle passing through point A, the needle will be inserted along the first straight line where the angle of the needle is located, and the lens will be touched. Since the angle of the needle in the mouse eyeball experiment is not suitable to be too small, if the initial angle of the needle is too small, the needle tip may scratch the lens during the RCM rotation process after the needle is inserted, thereby causing damage to the lens. Therefore, a minimum value of the initial angle of the needle is set, and it can be known that the range of the angle of the needle that will touch the lens circle is: [45°, θ6]

[0096]

[0097] If the angle of the needle is less than θ6 and greater than 45°, it is judged that the second operation region, i.e., the lens, will be touched along the current angle of the needle, and if the angle of the needle is greater than θ6, the second operation region, i.e., the lens, will not be touched along the current angle of the needle.

[0098] In addition, the intersection of the first entering straight line and the inner surface of the first operation region is I, and the coordinates of point I are (x I ,y I );

[0099] The coordinates (x I ,y I ) of point I are obtained from the left formula.

[0100] In addition, according to the above steps, the range of the angle of the needle that will touch the second operation region, i.e., the lens, has been calculated. If the current angle of the needle is not in the range of the angle of the needle that will touch the lens, only the minimum needle insertion depth through the eyeball surface needs to be calculated; if the current angle of the needle is in the range of the angle of the needle that will touch the lens, the maximum needle insertion depth that will touch the lens and the minimum needle insertion depth of the needle tip through the eyeball surface need to be calculated.

[0101] Specifically, in the step S4, if the first entering straight line does not intersect with the second operation region, the needle insertion depth L2 of the execution mechanism should satisfy that it is greater than the length of the line segment AI, i.e.,

[0102] The coordinates (x I ,y I ) of point I are obtained from the left formula.

[0103]

[0104] In order to ensure that the needle tip does not damage the eyeball surface during the subsequent RCM rotation process, the needle insertion depth L2 should be slightly greater than the minimum needle insertion depth, and the needle insertion depth L2 is twice the length of the line segment AI (the thickness of the surface layer of the eyeball), i.e.,

[0105]

[0106] In the step S3, if the first entering straight line intersects with the second operation region, the intersection point of the first entering straight line and the second operation region is C, the coordinates of the point C are (x c ,y c ); the needle entry depth L1 of the actuator should satisfy that it passes through the boundary of the first operation region, i.e. it is greater than the length of the line segment AI and less than the length of the line segment AC;

[0107] The coordinates of the point C are (x C ,y C ) obtained from the left side of the equation.

[0108]

[0109] In addition, the needle entry depth L1 should ensure the safety of the intraocular organs while ensuring a certain margin, and therefore the midpoint length of the needle entry depth L1 is selected as follows:

[0110]

[0111] In order to rotate to the needle entry angle, after the needle entry operation is completed, the RCM rotation operation of the needle tip should be performed, with the point A at the opening as the RCM fixed point, i.e. the RCM rotation of the needle tip in the vitreous cavity is performed without displacement of the needle at the point A at the opening, and the needle entry angle is reached. The rotation angle should ensure that the lens is not damaged during the subsequent needle entry process, and ensure the depth requirement required for the subsequent safe rotation to the injection angle.

[0112] Specifically, as shown in Figure 2 、 Figure 3 In the step S3, the angle θ6 of the needle entry that touches the boundary of the second operation region, i.e. the lens, is obtained from the step S23, and it is known that if the needle entry is to be implemented to the fundus, the angle after the RCM rotation should be greater than the angle θ6, and therefore the minimum rotation angle of the RCM rotation is solved first.

[0113] When the angle θ1 of the needle entry is greater than the angle θ6 of the needle entry that touches the lens, the minimum rotation angle of the RCM rotation is zero;

[0114] When the angle θ1 of the needle entry is less than the angle θ6 of the needle entry that touches the lens, the minimum rotation angle of the RCM rotation should be:

[0115] θ5=θ6-θ1

[0116] The maximum rotation angle of the RCM rotation that rotates to the needle entry angle is solved, as shown in Figure 9 、 Figure 11As shown, the definition of the injection angle θ7 required to reach the optic disc, after the needle insertion is completed, the needle should be rotated to the injection angle to complete the subsequent injection operation. Because the actual lens eye can move and is not afraid of extrusion, the needle can extrude the lens moderately without causing damage to the lens. However, the needle tip should be prevented from scratching the lens, that is, the needle tip should not face the lens after being rotated to the injection angle.

[0117] When the distance from the needle tip to point A at the opening after the needle insertion exceeds the second operation area, that is, the distance from the tangent point on the lens circle with the same tangent angle as the injection angle to point A at the opening, it can be ensured that the needle tip does not face the lens after being rotated to the injection angle. This distance is also the minimum depth that can be reached by the needle insertion. Under this depth limit, in order to ensure that the eye surface is not touched and damaged during needle insertion, the rotation angle should satisfy that the depth after rotation is greater than the minimum distance of the above needle insertion.

[0118] The specific steps are as follows: let point G be the point on the inner surface of the first operation area along the vertical direction with the maximum distance from point A (i.e., the point at the bottom of the eye), the angle of the straight line AG is the injection angle, translate the line segment AG to be tangent to the second operation interface, that is, the lens, and get the tangent point F, the distance from the tangent point F to point A is the minimum depth h1 of the needle insertion, when the line segment AF is rotated to touch the point D on the inner surface D of the first operation area (mouse eye), the absolute value of the angle of the straight line AD is the maximum needle insertion angle;

[0119]

[0120] where k2 is the slope of the injection angle, and the slope at this time according to the coordinate system should be negative

[0121] The coordinates (x F ,y F ) of point F are obtained by the following formulae;

[0122]

[0123] The coordinates (x D ,y D ) of point D are obtained by the following formulae;

[0124]

[0125]

[0126] The coordinates of the first operation area inner surface point D are obtained from the above formulae;

[0127] Then, the maximum rotation angle of the RCM rotation is calculated according to the above calculation:

[0128]

[0129] When the first entering straight line intersects with the second operation interface, the range of the RCM rotating to the needle insertion angle is obtained as:

[0130] 0 < θ2 < θ4;

[0131] To ensure the needle insertion depth and the safety of the intraocular organs, the range of the RCM rotating to the needle insertion angle is selected as:

[0132] θ2 = θ4 / 2

[0133] When the first entering straight line does not intersect with the second operation interface, the range of the RCM rotating to the needle insertion angle is obtained as:

[0134] θ5 < θ3 < θ4.

[0135] To ensure the needle insertion depth and the safety of the intraocular organs, the range of the RCM rotating to the needle insertion angle is selected as:

[0136] θ3 = (θ4 + θ5) / 2.

[0137] In addition, after the RCM is rotated, the needle insertion to the fundus step is performed. As shown in Figure 10 , the needle insertion depth should ensure that the needle tip does not scratch the lens after the RCM is rotated to the injection angle, and the needle tip does not puncture the bottom (inner surface) of the eyeball. The specific conditions are as follows:

[0138] In the step S5, as shown in Figure 11 , to ensure that the needle insertion depth does not touch and damage the inner surface of the first operation region, the needle insertion angle intersects with the inner surface of the first operation region (i.e., the eyeball) at point K;

[0139] The coordinates (x K ,y K ) of point K are obtained from the left side as:

[0140] When the first entering straight line intersects with the second operation region, the maximum needle insertion depth is the distance of line segment AK minus the needle insertion depth L1, and the needle insertion depth L3 should satisfy the following formula:

[0141]

[0142] When the first entering straight line does not intersect with the second operation region, the maximum needle insertion depth is the distance of line segment AK minus the needle insertion depth L2, and the needle insertion depth L3 should satisfy the following formula:

[0143]

[0144] Wherein, the RCM rotates to the injection angle, according to the general medicine injection position, i.e. the vicinity of the optic disc, the slope of the straight line same as the injection angle has been calculated in the previous algorithm steps, thus the rotation angle of the RCM rotating to the injection angle can be obtained. More specifically, when the first entering straight line intersects with the second operation region, the rotation angle θ7 of the RCM rotating to the injection angle is:

[0145] θ7=θ1+θ2-(|tan -1 k2(-π,π)|)

[0146] When the first entering straight line does not intersect with the second operation region, the rotation angle θ7 of the RCM rotating to the injection angle is:

[0147] θ7=θ1+θ3-(|tan -1 k2(-π,π)|).

[0148] Embodiment 2

[0149] The difference from Embodiment 1 is that, when the first entering straight line intersects with the second operation region, the maximum needle insertion depth is the distance of the line segment AK minus the needle insertion depth L1, in order to ensure that the distance between the needle tip and the inner surface of the eyeball has a certain margin after the needle insertion, while ensuring sufficient safety to the intraocular organs, thus the midpoint length of the depth distance that can be inserted is selected, and the needle insertion depth L3 should satisfy the following formula:

[0150]

[0151] Embodiment 3

[0152] The difference from Embodiment 1 is that, when the first entering straight line does not intersect with the second operation region, the maximum needle insertion depth is the distance of the line segment AK minus the needle insertion depth L2, in order to ensure that the distance between the needle tip and the inner surface of the eyeball has a certain margin after the needle insertion, while ensuring sufficient safety to the intraocular organs, thus the midpoint length of the depth distance that can be inserted is selected, and the needle insertion depth L3 should satisfy the following formula:

[0153]

[0154] Obviously, the above embodiments of the present application are merely exemplary for clearly illustrating the present application, but are not intended to limit the implementation modes of the present application. Based on the above description, other different forms of changes or variations can be made by those skilled in the art. Here, all the implementation modes are not required or can not be exhausted. Any modification, equivalent replacement and improvement, etc. made within the spirit and principle of the present application should be included in the protection scope of the claims of the present application.

Claims

1. A method for obstacle avoidance control based on RCM robot motion, characterized in that , comprising the following steps: S1 : having a first operating area and a second operating area located within the first operating area, the effector pre-entering the first operating area from a point A on the surface outside the first operating area at an entry angle and along a first entry straight line; S2: judging whether the first entering straight line of the actuator intersects with the second operation region, if intersecting, executing step S3; if not intersecting, executing step S4; S3: the execution mechanism enters the needle along a first entering straight line direction to a needle depth of , and then performs RCM rotation around a remote motion center point A to obtain a second entering straight line in which the needle entry angle is located, and then step S5 is performed. S4: the execution mechanism enters the needle along a first entering straight line direction to a needle depth of , and then performs RCM rotation around a remote motion center point A to obtain a second entering straight line in which the needle entry angle is located, and then step S5 is performed. S5: the execution mechanism advances the needle along a second entry straight line by a needle depth of to the execution depth; S6: the execution mechanism rotates around the remote center of motion point A by RCM to the angle of injection; In the step S1, the first operation region is a circular ring with an outer diameter of r2 and an inner diameter of r2-d; the second operation region is a circle concentric with the circular ring, the radius of the second operation region is r1, and r1 In the step S2, whether the first entering straight line intersects with the second operation region is judged by the following steps: S21: Calculate the coordinate of point A : where h is the distance from point A to the equatorial diameter. S22: judging whether the first entering straight line intersects with the second operation region by the formula: calculating an absolute value of an angle of a straight line passing through point A and tangent to the second operating region boundary, wherein K1 is a slope of a tangent line of the second operating region boundary passing through point A, and the tangent point is E, is an absolute value of the tangent angle; S23: if the entry angle less than and greater than 45°, then the first entry straight line of the actuator intersects the second operating region, if the entry angle greater than then the first entry straight line of the actuator does not intersect the second operating region. 2.The RCM robot motion-based obstacle avoidance control method of claim 1, wherein, The intersection of the first entering straight line and the inner surface of the first operating area is I, and the coordinate of the point I is ; 。 3.The RCM robot motion based obstacle avoidance control method of claim 2, wherein, In the step S4, if the first entering straight line does not intersect with the second operation region, the execution mechanism entering needle depth The following should be met: passing through the first operation region boundary, i.e. greater than the length of the line segment AI, i.e. ； In the step S3, if the first entering straight line intersects with the second operation region, the intersection point of the first entering straight line and the second operation region is C, and the coordinates of the point C are ; the actuator needle depth should satisfy that it passes through the boundary of the first operation region, i.e. greater than the length of the line segment AI and less than the length of the line segment AC. 。 4. The RCM robot motion based obstacle avoidance control method of claim 3, wherein: In the step S4, if the first entering straight line does not intersect with the second operation region, the needle insertion depth The following equation should be satisfied: In the step S3, if the first entering straight line intersects with the second operation region, the needle insertion depth The following equation should be satisfied:

5. The RCM robot motion based obstacle avoidance control method of claim 3, wherein, In the step S3, the angle of the needle insertion that touches the boundary of the second operation area is obtained from the step S23 In order to realize the needle insertion to the bottom of the second operation area, the angle after the RCM rotation should be greater than the angle The minimum rotation angle of the RCM rotation is solved first When the angle of the needle entering is greater than the angle of the needle entering that will touch the lens , the minimum angle of rotation of the RCM is zero; When the angle of the needle entering is less than the angle of the needle entering that will touch the lens The minimum angle of rotation of the RCM should be: Solving the maximum rotation angle of the RCM rotation to the needle insertion angle, the steps are as follows: assuming that point G is the point with the maximum distance from point A in the vertical direction on the inner surface of the first operation area, the angle of the straight line where AG is located is the injection angle, the line segment AG is translated to be tangent to the second operation interface and the tangent point F is obtained, and the distance between the tangent point F and point A is the minimum depth of needle insertion When the line segment AF is rotated to touch the D point on the inner surface of the first operation area through the RCM, the absolute value of the angle of the straight line where AD is located is the maximum needle insertion angle. by the formula The coordinates of point D are found by the following equations ； Then, the maximum rotation angle of RCM rotation is calculated according to the above calculation When the first entering straight line and the second operation interface do not intersect, the range of the rotation angle of the RCM to the needle insertion angle is obtained as: ; When the first entering straight line does not intersect with the second operation interface, the range of the rotation angle of the RCM to the needle insertion angle is obtained as: . 6.The RCM robot motion-based obstacle avoidance control method of claim 4, wherein, In the step S5, in order to ensure that the reachable depth of the needle does not touch and damage the inner surface of the first operation region, the needle angle intersects with the inner surface of the first operation region at point K. The maximum depth of needle insertion is the distance of line segment AK minus the needle insertion depth when the first entering straight line intersects with the second operation region , the needle insertion depth The following formula should be satisfied: When the first entering straight line does not intersect with the second operation region, the maximum needle insertion depth is the distance of line segment AK minus the needle insertion depth , the needle insertion depth should satisfy the following formula: .

7. The obstacle avoidance control method based on RCM robot motion according to claim 6, characterized in that, the rotational angle of the RCM to the injection angle when the first entry straight line intersects the second operation region is: the first entering straight line does not intersect with the second operation region, the RCM rotates to a rotation angle of an injection angle is: .

8. The RCM robot motion based obstacle avoidance control method according to any one of claims 1 to 7, characterized in that, The first operation region is an eyeball of an animal, and the second operation region is a lens located in the eyeball.

Images