CN113485353B - Micro-robot obstacle avoidance method based on combination of RRT algorithm and artificial potential field method - Google Patents

Micro-robot obstacle avoidance method based on combination of RRT algorithm and artificial potential field method Download PDF

Info

Publication number
CN113485353B
CN113485353B CN202110838460.1A CN202110838460A CN113485353B CN 113485353 B CN113485353 B CN 113485353B CN 202110838460 A CN202110838460 A CN 202110838460A CN 113485353 B CN113485353 B CN 113485353B
Authority
CN
China
Prior art keywords
robot
micro
obstacle
potential field
sub
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110838460.1A
Other languages
Chinese (zh)
Other versions
CN113485353A (en
Inventor
樊启高
赵正青
谢林柏
黄文涛
朱一昕
毕恺韬
贾捷
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangnan University
Original Assignee
Jiangnan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangnan University filed Critical Jiangnan University
Priority to CN202110838460.1A priority Critical patent/CN113485353B/en
Publication of CN113485353A publication Critical patent/CN113485353A/en
Application granted granted Critical
Publication of CN113485353B publication Critical patent/CN113485353B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The invention discloses a microcomputer robot obstacle avoidance method based on the combination of an RRT algorithm and an artificial potential field method, which relates to the technical field of micro-nano robots, and comprises the following steps: obtaining an obstacle avoidance experimental scene graph, wherein the graph comprises a simulated blood vessel edge, a micro robot and an obstacle; identifying simulated blood vessel edges from the obstacle avoidance experimental scene graph through template matching, and identifying static obstacles from the obstacle avoidance experimental scene graph through an HSV model; based on the identified simulated vessel edges and static obstacles, performing global path planning by utilizing an improved RRT algorithm, and determining key nodes on a global path; and taking the key node as a sub-target point, and utilizing the improved artificial potential field rule to avoid dynamic barriers to sequentially reach the sub-target point until reaching a path end point. The method can not only avoid static obstacle, but also avoid dynamic obstacle under narrow environment.

Description

Micro-robot obstacle avoidance method based on combination of RRT algorithm and artificial potential field method
Technical Field
The invention relates to the technical field of micro-nano robots, in particular to a micro-robot obstacle avoidance method based on the combination of an RRT algorithm and an artificial potential field method.
Background
At present, the microcomputer robot is widely applied due to the advantages of no damage, strong compatibility, wireless remote control and the like, comprises the medical biological fields of thrombus dredging, targeted drug delivery, brachytherapy and the like, and has revolutionary application prospect. Path planning for micro-robots is an important research hotspot in current micro-robot technology. At present, most micro robots can only avoid static obstacles, but cannot avoid the static obstacles in real time, and the micro robots need to avoid the static obstacles and the dynamic obstacles in complex environments, so that an automatic obstacle avoidance strategy of the micro robots on the dynamic/static obstacles in the complex environments is required to be provided.
Disclosure of Invention
Aiming at the problems and the technical requirements, the inventor provides a micro-robot obstacle avoidance method based on the combination of an RRT algorithm and an artificial potential field method, firstly, a key node is selected and a global optimal path is planned through an improved RRT algorithm, then, the micro-robot is segmented to reach a sub-target point through an improved artificial potential field algorithm, and finally, the whole path planning of the micro-robot is completed.
The technical scheme of the invention is as follows:
the robot obstacle avoidance method based on the combination of the RRT algorithm and the artificial potential field method comprises the following steps:
obtaining an obstacle avoidance experimental scene graph, wherein the graph comprises a simulated blood vessel edge, a micro robot and an obstacle;
identifying simulated blood vessel edges from the obstacle avoidance experimental scene graph through template matching, and identifying static obstacles from the obstacle avoidance experimental scene graph through an HSV model;
based on the identified simulated vessel edges and static obstacles, performing global path planning by utilizing an improved RRT algorithm, and determining key nodes on a global path;
and taking the key node as a sub-target point, and utilizing the improved artificial potential field rule to avoid dynamic barriers to sequentially reach the sub-target point until reaching a path end point.
The further technical scheme is that the simulated blood vessel edge is identified from the obstacle avoidance experimental scene graph through template matching, and the method comprises the following steps:
extracting an ROI region of a simulated blood vessel from the obstacle avoidance experimental scene graph;
performing blurring treatment on the ROI area to remove edge noise points, performing edge extraction and binarization treatment, and performing edge expansion on the obtained binarized edge image to obtain a sampling image simulating the rough edge of the blood vessel;
performing edge extraction and binarization processing on the design drawing of the simulated blood vessel, and performing edge expansion on the obtained binarization edge image to obtain the design drawing of the rough edge of the simulated blood vessel;
and carrying out translation, ROI region selection and scaling operation on the design drawing of the simulated blood vessel rough edge, and matching the processed design drawing of the simulated blood vessel rough edge with each pixel point of the sampling drawing of the simulated blood vessel rough edge by utilizing a differential evolution algorithm, wherein the identification of the simulated blood vessel edge is completed if the matching is successful.
The further technical scheme is that the static obstacle is identified from the obstacle avoidance experimental scene graph through the HSV model, and the method comprises the following steps:
extracting an ROI region of a simulated blood vessel from the obstacle avoidance experimental scene graph;
converting the ROI area from RGB space to HSV space;
and adjusting hue parameters of the HSV space according to the colors of the static obstacles, so as to complete the identification of the static obstacles.
The method further comprises the steps of performing global path planning by utilizing an improved RRT algorithm, and determining key nodes on a global path, wherein the method comprises the following steps:
generating an initial planning path by utilizing a bidirectional RRT algorithm, and acquiring all nodes of the initial planning path to form a node set A { Y } i 1.ltoreq.i.ltoreq.n }, wherein Y i The ith node of the initial planning path is represented, the nodes are ordered in sequence from the starting point to the end point, and n is the number of the nodes of the initial planning path;
in Y form 1 Is sequentially combined with Y as a starting point m (m=2, 3, …, n) and sequentially judging the straight line Y 1 Y m Whether a static obstacle is encountered:
if yes, Y is m-1 As key node, take Y m Is sequentially combined with Y as a starting point m+1 Making straight line connection and successively judging straight line Y m Y m+1 Whether or not a static obstacle is encountered until the end point Y connected to the initially planned path n Adding a starting point, a key node and an ending point to the set B;
otherwise, Y is 2 ,Y 3 …Y m-1 As redundant node, directly eliminating;
and sequentially connecting the nodes in the set B to obtain the optimized global path planning.
The further technical scheme is that the dynamic obstacle is avoided from reaching the sub-target point in sequence by utilizing the improved artificial potential field regulation until reaching the path end point, and the method comprises the following steps:
respectively constructing attractive force potential fields of the sub-target points to the micro-robot and repulsive force potential fields of the dynamic obstacles to the micro-robot;
acquiring initialization parameters of a time step of a sub-target point at the current moment, and respectively inputting the initialization parameters into an attractive force potential field and a repulsive force potential field to obtain virtual resultant force received by the micro-robot, wherein the virtual resultant force is the sum of attractive force and repulsive force;
acquiring the real-time position of the micro-robot, judging whether the real-time position reaches a sub-target point, if so, acquiring the initialization parameter of the time step at the current moment of the next sub-target point until the path end point is reached; otherwise, let time step=time step+1, and re-execute the initialization parameter of the time step at the current time of the sub-target point.
The further technical scheme is that constraint conditions of the bidirectional RRT algorithm comprise:
(1) Nodes on the initial planned path generated by the bi-directional RRT algorithm are all in the identified region D formed by the simulated vessel edges and the static obstacle, expressed as: y is Y i ∈D,i＝1,2,3,...,n
(2) Constraints of individual nodes from simulated vessel edges are expressed as: x > tao line
Wherein X represents Y i Distance from the simulated vessel edge, taoline is the threshold.
The further technical scheme is that the method for constructing the attraction potential field of the sub-target point to the micro-robot comprises the following steps:
the gravitational potential field comprises a position potential field and a speed potential field, and the expression is:
wherein, xi p Is the position potential field proportionality coefficient, ζ v As a velocity potential field proportionality coefficient, p m Is the position of the micro-robot, p g For the position of the sub-target point ρ (p m ,p g ) Is the relative position of the micro-robot and the sub-target point, v m Is the speed of the micro-robot, v g For the velocity of the sub-target point ρ (v m ,v g ) Relative speeds for the micro-robot and the sub-target point;
the expression for deriving attraction from the attraction potential field is:
wherein F is attp (p) is the attractive force component of the relative position of the pointing sub-target point of the micro-robot, F attv (v) Is the gravitational component of the relative velocity of the sub-target point to the micro-robot,is the unit vector of the sub-target point relative to the direction of motion of the micro-robot,/or->Is a unit vector of the micro-robot pointing to the sub-target point.
The further technical scheme is that the method for constructing the repulsive force potential field of the dynamic barrier to the micro-robot comprises the following steps:
dividing an action area of a repulsive force potential field into an absolute safety area, an early warning obstacle avoidance area, an execution obstacle avoidance area and an absolute obstacle avoidance area according to the distance between the micro robot and a dynamic obstacle and the simulated blood vessel field by taking the micro robot as a center;
the absolute safety zone ranges from: beyond the area beyond the circle with the detection obstacle avoidance distance as the radius, the dynamic obstacle positioned in the absolute safety area does not reach the obstacle avoidance condition, and does not generate repulsive force on the micro-robot, and the micro-robot only receives attractive force at the moment;
the range of the early warning obstacle avoidance area is as follows: removing an annular region formed by a circle with the detection obstacle avoidance distance as a radius from the circle with the safety distance between the micro-robot and the sub-target point as the radius;
the range of the execution obstacle avoidance area is as follows: removing an annular region formed by a circle with the movement length of the micro robot as a radius from the circle with the safety distance as the radius;
the range of the absolute obstacle avoidance area is as follows: a circular region having a radius of a movement length of the micro robot;
the repulsive potential field under each region is expressed as:
wherein ρ is 0 Radius, eta, of the influence range of repulsive potential field of dynamic barrier s Is the proportion coefficient of the repulsive potential field of the early warning obstacle avoidance area, eta e Is to execute the proportion coefficient of the repulsive potential field of the obstacle avoidance area, lambda is the proportion coefficient of the repulsive potential field of the absolute obstacle avoidance area, R m Is the radius of the m-th dynamic obstacle, theta is the included angle between the relative position line of the micro-robot and the dynamic obstacle and the relative speed line of the micro-robot and the obstacle, d is the distance between the micro-robot and the dynamic obstacle, d g Is the distance between the micro-robot and the sub-target point, d m Is the safe distance between the micro-robot and the sub-target point, tau is the radius of the motion field of the micro-robot, CD is the set detection obstacle avoidance distance, v mo Is the relative speed of the micro-robot and the dynamic barrier.
The further technical proposal is that the initialization parameters comprise a position matrix P of the micro robot m Position matrix P of sub-target points g Speed matrix V of microcomputer robot m Velocity matrix V of dynamic obstacle o Radius R of mth dynamic obstacle m Expressed as:
P m ＝[p ij ] s×2
P g ＝[p g ij ] k×2
V o ＝[v ij ] m×2
where s is the total number of time steps, i is the ith time step, j=1 represents the abscissa, j=2 represents the ordinate, and m is the number of dynamic obstacles;
the speed and direction of the dynamic obstacle are randomly varied, expressed as:
V o-new ＝V o ×A
wherein,representing the sign and the size of each turn of the micro robot, wherein gamma is the maximum change angle of the dynamic barrier in one time step, A is the change matrix of the micro robot in changing direction, and V o-new Updated velocity matrix for the dynamic obstacle.
The further technical proposal is that the expression of deducing the repulsive force from the repulsive force potential field is as follows:
wherein F is rs1 、F rs2 、F re1 、F re2 、F re3 Respectively representing repulsive force components, the expressions are respectively:
in the method, in the process of the invention,is the unit vector of the micro-robot pointing to the dynamic obstacle,/->Is the unit vector of the pointing sub-target point of the micro-robot;
F rs1 ，F re1 the direction of the robot is from the robot to the dynamic obstacle, and the robot is far away from the dynamic obstacle; f (F) rs2 ,F re3 The direction of the robot is from the robot to the sub-target point, and the robot is enabled to move to the sub-target point; f (F) re2 Direction of (F) re1 Is perpendicular to the direction of (2); when the relative position direction of the micro-robot and the dynamic obstacle is coincident with the relative speed direction, the micro-robot turns left or turns right to avoid the obstacle, and when the relative position direction of the micro-robot and the dynamic obstacle is positioned at the left side of the relative speed direction, the micro-robot turns left to avoid the obstacle; when the relative position direction of the micro robot and the dynamic obstacle is positioned on the right side of the relative speed direction, the micro robot turns right to avoid the obstacle;
the repulsive force applied by the micro robot is the sum of repulsive force vectors of each dynamic obstacle to the micro robot, and is expressed as:
wherein F is repi The repulsive force generated by the microcomputer robot on the ith dynamic obstacle, and m is the number of the dynamic obstacles.
The beneficial technical effects of the invention are as follows:
according to the method, a global path is planned through a bidirectional RRT algorithm, redundant nodes on the global path are removed by utilizing condition constraint and key nodes, and the safety and the length of the path are optimized; then, the global path is segmented according to key nodes, each segment of path is optimized by combining an improved artificial potential field method, a repulsive potential field is constructed by partitioning a repulsive potential field acting area taking the microcomputer robot as a circle center, dynamic obstacles can be effectively avoided, the global path is further optimized, and finally, the microcomputer robot can avoid static obstacles and dynamic obstacles in a narrow environment.
Drawings
Fig. 1 is an overall flowchart of the obstacle avoidance method of the micro robot provided in the present application.
Fig. 2 is a view of obstacle avoidance experimental scenario provided in the present application.
Fig. 3 is a process diagram of image processing on an obstacle avoidance experimental scene graph provided by the application.
Fig. 4 is a process diagram of image processing of a design drawing of an analog blood vessel provided in the present application.
Fig. 5 is a graph of the number of matches versus the number of iterations in the template matching process provided in the present application.
Fig. 6 is a graph of the results of identifying static obstructions provided herein.
Fig. 7 is a schematic diagram of path planning of the bidirectional RRT algorithm provided in the present application.
Fig. 8 is a flow chart of the improved artificial potential field method provided herein.
Fig. 9 is a plot of the region of action of the repulsive potential field provided herein.
Detailed Description
The following describes the embodiments of the present invention further with reference to the drawings.
As shown in fig. 1, a method for avoiding obstacle by a micro-robot based on the combination of RRT algorithm and artificial potential field method comprises the following steps:
step 1: the obstacle avoidance experimental scene graph is obtained through a CCD camera, and as shown in fig. 2, the graph comprises a simulated blood vessel edge 1, a micro robot 2 and a static obstacle 3.
Alternatively, the micro-robot 2 employs magnetic beads with a diameter of 300 μm, which are mainly composed of ferroferric oxide and polystyrene.
Step 2: and identifying the simulated vessel edge 1 from the obstacle avoidance experimental scene graph through template matching, and identifying the static obstacle 3 from the obstacle avoidance experimental scene graph through the HSV model.
The specific method for identifying the simulated vessel edge 1 comprises the following steps:
(2A-1) As shown in FIG. 3 (1), the ROI area of the simulated blood vessel is extracted from the obstacle avoidance experimental scene graph, and the ROI area is subjected to blurring treatment to remove edge noise points.
(2A-2) with reference to FIGS. 3 (2) and 3 (3), performing edge extraction and binarization, and performing edge expansion on the obtained binarized edge image to obtain a sample image simulating the rough edge of the blood vessel.
(2A-3) referring to FIGS. 4 (1) and 4 (2), edge extraction and binarization are performed on the design drawing of the simulated blood vessel, and edge expansion is performed on the obtained binarized edge image to obtain the design drawing of the rough edge of the simulated blood vessel.
(2A-4) carrying out translation, ROI region selection and scaling operation on the design drawing of the simulated blood vessel rough edge, and matching the processed design drawing of the simulated blood vessel rough edge with each pixel point of the sampling drawing of the simulated blood vessel rough edge by utilizing a differential evolution algorithm, wherein if the matching is successful, the matching frequency is increased by one, the matching success frequency is gradually increased along with the increase of the iteration frequency, the final trend is stable, and the identification of the simulated blood vessel edge is completed if the matching is successful.
The specific method for identifying the static obstacle 3 comprises the following steps:
(2B-1) extracting the ROI area of the simulated blood vessel from the obstacle avoidance experimental scene graph.
(2B-2) converting the ROI area from RGB space to HSV space.
(2B-3) adjusting hue parameters of the HSV space according to the colors of the static obstacles, thereby completing the identification of the static obstacles.
Alternatively, the static obstacle injected by the method is green, so that the static obstacle can be identified by setting the hue parameter to 0.7-1, as indicated by the four-star mark in fig. 6.
Step 3: based on the identified simulated vessel edge 1 and static obstacle 3, performing global path planning by utilizing an improved RRT algorithm, and determining key nodes on the global path, wherein the method specifically comprises the following steps:
(3-1) As shown in FIG. 7 (1), an initial planned path is generated using a bi-directional RRT algorithm, and all nodes of the initial planned path are acquired to form a node set A { Y } i 1.ltoreq.i.ltoreq.n }, wherein Y i The i-th node of the initial planning path is represented, and the nodes are ordered from the starting point to the end point, and n is the number of the nodes of the initial planning path.
(3-2) As shown in FIG. 7 (2), in Y 1 Is sequentially combined with Y as a starting point m (m=2, 3, …, n) and sequentially judging the straight line Y 1 Y m Whether a static obstacle is encountered:
if yes, Y is m-1 As key nodes (i.e. delta marks in the graph), in Y m Is sequentially combined with Y as a starting point m+1 Making straight line connection and successively judging straight line Y m Y m+1 Whether or not a static obstacle is encountered until the end point Y connected to the initially planned path n The start point, key node and end point are added to set B.
Otherwise, Y is 2 ,Y 3 …Y m-1 As a redundant node, direct culling is performed.
(3-3) sequentially connecting the nodes in the set B to obtain an optimized global path plan, as shown in fig. 7 (3).
Constraints of the bi-directional RRT algorithm include:
(1) Nodes on the initial planned path generated by the bi-directional RRT algorithm are all in the identified region D formed by the simulated vessel edges and the static obstacle, expressed as: y is Y i ∈D,i＝1,2,3,...,n
(2) Constraints of individual nodes from simulated vessel edges are expressed as: x > tao line
Wherein X represents Y i Distance from the simulated vessel edge, taoline is the threshold.
Step 4: the key node is used as a sub-target point, and the dynamic obstacle is avoided from sequentially reaching the sub-target point by utilizing the improved artificial potential field rule until reaching a path end point, which comprises the following steps:
(4-1) respectively constructing attractive potential fields of the sub-target points to the micro-robot and repulsive potential fields of the dynamic obstacles to the micro-robot, wherein the specific flow is shown in figure 8.
The basic idea of the improved artificial potential field method is to use the combined action of a gravitational potential field and a repulsive potential field to move the micro-robot.
<1> the attraction potential field includes a position potential field and a velocity potential field, and the expression is:
wherein, xi p Is the position potential field proportionality coefficient, ζ v As a velocity potential field proportionality coefficient, p m Is the position of the micro-robot, p g For the position of the sub-target point ρ (p m ,p g ) Is the relative position of the micro-robot and the sub-target point, v m Is the speed of the micro-robot, v g For the velocity of the sub-target point ρ (v m ,v g ) Is the relative speed of the micro-robot and the sub-target point.
The expression for deriving attraction from the attraction potential field is:
wherein F is attp (p) is the attractive force component of the relative position of the pointing sub-target point of the micro-robot, F attv (v) Is the gravitational component of the relative velocity of the sub-target point to the micro-robot,is the unit vector of the sub-target point relative to the direction of motion of the micro-robot,/or->Is a unit vector of the micro-robot pointing to the sub-target point.
<2> construction of an improved repulsive potential field comprising:
firstly, taking a micro-robot as a center, dividing an action area of a repulsive force potential field into an absolute safety area, an early warning obstacle avoidance area, an execution obstacle avoidance area and an absolute obstacle avoidance area according to the distance between the micro-robot and a dynamic obstacle and the simulated blood vessel field, as shown in fig. 9.
a. Absolute safe area: beyond the area beyond the circle with the detection obstacle avoidance distance CD as the radius, the dynamic obstacle in the absolute safety area does not reach the obstacle avoidance condition and does not generate repulsive force on the micro-robot, and the micro-robot only receives attractive force at the moment.
b. Early warning obstacle avoidance area: removing from a circle with a radius of a safety distance CD between the micro-robot and the sub-target point to detect the obstacle avoidance distance d m And the annular area is formed by a radius circle, and dynamic obstacles in the early warning obstacle avoidance area generate corresponding repulsive force potential fields to enable the micro-robot to be far away from the dynamic obstacles.
c. Executing an obstacle avoidance area: from a safe distance d m The annular region formed by the circle with the radius of the motion length tau of the micro-robot is removed from the circle with the radius, and the dynamic obstacle in the obstacle avoidance area generates a corresponding repulsive force potential field to enable the micro-robot to avoid the obstacle in emergency.
d. Absolute obstacle avoidance area: the dynamic obstacle in the absolute obstacle avoiding area generates great repulsive force potential field in the circular area with the motion length tau of the microcomputer robot as the radius, so as to achieve the aim of effective obstacle avoidance.
The repulsive potential field under each region is expressed as:
wherein ρ is 0 Radius, eta, of the influence range of repulsive potential field of dynamic barrier s Is the proportion coefficient of the repulsive potential field of the early warning obstacle avoidance area, eta e Is to execute the proportion coefficient of the repulsive force potential field of the obstacle avoidance area, and lambda is the repulsive force potential field of the absolute obstacle avoidance areaScaling factor, R m Is the radius of the m-th dynamic obstacle, theta is the included angle between the relative position line of the micro-robot and the dynamic obstacle and the relative speed line of the micro-robot and the obstacle, d is the distance between the micro-robot and the dynamic obstacle, d g Is the distance between the micro-robot and the sub-target point, d m Is the safe distance between the micro-robot and the sub-target point, tau is the motion length of the micro-robot, CD is the set detection obstacle avoidance distance, v mo Is the relative speed of the micro-robot and the dynamic barrier.
The expression for deriving repulsive force from repulsive potential field is:
wherein F is rs1 、F rs2 、F re1 、F re2 、F re3 Respectively representing repulsive force components, the expressions are respectively:
in the method, in the process of the invention,is the unit vector of the micro-robot pointing to the dynamic obstacle,/->Is a unit vector of the micro-robot pointing to the sub-target point.
F rs1 ，F re1 The direction of the robot is from the robot to the dynamic obstacle, and the robot is far away from the dynamic obstacle; f (F) rs2 ,F re3 The direction of the robot is from the robot to the sub-target point, and the robot is enabled to move to the sub-target point; f (F) re2 Direction of (F) re1 Is perpendicular to the direction of (2); when the relative position direction of the micro-robot and the dynamic obstacle is coincident with the relative speed direction, the micro-robot turns left or turns right to avoid the obstacle, and when the relative position direction of the micro-robot and the dynamic obstacle is positioned at the left side of the relative speed direction, the micro-robot turns left to avoid the obstacle; when the relative position direction of the micro robot and the dynamic obstacle is positioned at the right side of the relative speed direction, the micro robot turns right to avoid the obstacle.
The repulsive force applied by the micro robot is the sum of repulsive force vectors of each dynamic obstacle to the micro robot, and is expressed as:
wherein F is repi The repulsive force generated by the microcomputer robot on the ith dynamic obstacle, and m is the number of the dynamic obstacles.
And (4-2) acquiring an initialization parameter of the time step at the current moment of the sub-target point, and respectively inputting the initialization parameter into the attraction potential field and the repulsion potential field to obtain a virtual resultant force born by the micro-robot, wherein the virtual resultant force is the sum of the attraction and the repulsion.
The initialization parameters include a position matrix P of the micro-robot m Position matrix P of sub-target points g Speed matrix V of microcomputer robot m Velocity matrix V of dynamic obstacle o Radius R of mth dynamic obstacle m Expressed as:
P m ＝[p ij ] s×2
P g ＝[p g ij ] k×2
V o ＝[v ij ] m×2
where s is the total number of time steps, i is the i-th time step, j=1 represents the abscissa, j=2 represents the ordinate, and m is the number of dynamic obstacles.
The speed and direction of the dynamic obstacle are randomly varied, expressed as:
V o-new ＝V o ×A
wherein,representing the sign and the size of each turn of the micro robot, wherein gamma is the maximum change angle of the dynamic barrier in one time step, A is the change matrix of the micro robot in changing direction, and V o-new Updated velocity matrix for the dynamic obstacle.
Virtual resultant force F v Expressed as: f (F) v ＝F att +F rep
(4-3) acquiring the real-time position of the micro-robot, judging whether the sub-target point is reached, if so, acquiring the initialization parameter of the time step at the current moment of the next sub-target point until the path end point is reached; otherwise, let time step=time step+1, and re-execute the initialization parameter of the time step at the current time of the sub-target point.
What has been described above is only a preferred embodiment of the present application, and the present invention is not limited to the above examples. It is to be understood that other modifications and variations which may be directly derived or contemplated by those skilled in the art without departing from the spirit and concepts of the present invention are deemed to be included within the scope of the present invention.

Claims (8)

1. The method for avoiding the obstacle by the micro-robot based on the combination of the RRT algorithm and the artificial potential field method is characterized by comprising the following steps:
obtaining an obstacle avoidance experimental scene graph, wherein the graph comprises a simulated blood vessel edge, a micro robot and an obstacle;
identifying the simulated vessel edge from the obstacle avoidance experimental scene graph through template matching, and identifying a static obstacle from the obstacle avoidance experimental scene graph through an HSV model;
based on the identified simulated vessel edges and static obstacles, performing global path planning by utilizing an improved RRT algorithm, and determining key nodes on a global path;
taking the key node as a sub-target point, and utilizing the improved artificial potential field rule to avoid dynamic barriers to sequentially reach the sub-target point until reaching a path end point;
the global path planning is performed by using the improved RRT algorithm, and the determination of the key nodes on the global path comprises the following steps:
generating an initial planning path by utilizing a bidirectional RRT algorithm, and acquiring all nodes of the initial planning path to form a node set A { Y } i 1.ltoreq.i.ltoreq.n }, wherein Y i The ith node of the initial planning path is represented, the nodes are ordered in sequence from the starting point to the end point, and n is the number of the nodes of the initial planning path;
in Y form 1 Is sequentially combined with Y as a starting point m (m=2, 3, …, n) and sequentially judging the straight line Y 1 Y m Whether the static obstacle is encountered:
if yes, Y is m-1 As key node, take Y m Is sequentially combined with Y as a starting point m+1 Making straight line connection and successively judging straight line Y m Y m+1 Whether or not the static obstacle is encountered until an end point Y connected to the initial planned path n Adding the start point, the key node and the end point to a set B;
otherwise, Y is 2 ,Y 3 …Y m-1 As redundant node, directly eliminating;
sequentially connecting the nodes in the set B to obtain an optimized global path plan;
the constraint conditions of the bidirectional RRT algorithm comprise:
(1) Nodes on the initial planned path generated by the bi-directional RRT algorithm are all in the identified region D formed by the simulated vessel edges and static obstructions, expressed as: y is Y i ∈D,i＝1,2,3,...,n
(2) Constraints of individual nodes from simulated vessel edges are expressed as: x > tao line
Wherein X represents Y i Distance from the simulated vessel edge, taoline is the threshold.
2. The method for avoiding obstacle by a micro-robot based on the combination of RRT algorithm and artificial potential field method according to claim 1, wherein said identifying the simulated vessel edge from the obstacle avoidance experimental scene graph by template matching comprises:
extracting an ROI (region of interest) of a simulated blood vessel from the obstacle avoidance experimental scene graph;
performing blurring treatment on the ROI area to remove edge noise points, performing edge extraction and binarization treatment, and performing edge expansion on the obtained binarization edge image to obtain a sampling image simulating the rough edge of the blood vessel;
performing edge extraction and binarization processing on the design drawing of the simulated blood vessel, and performing edge expansion on the obtained binarization edge image to obtain the design drawing of the rough edge of the simulated blood vessel;
and carrying out translation, ROI region selection and scaling operation on the design drawing of the simulated blood vessel rough edge, and matching the processed design drawing of the simulated blood vessel rough edge with each pixel point of the sampling drawing of the simulated blood vessel rough edge by utilizing a differential evolution algorithm, wherein the identification of the simulated blood vessel edge is completed if the matching is successful.
3. The method for avoiding obstacle by a micro-robot based on the combination of RRT algorithm and artificial potential field method according to claim 1, wherein the identifying the static obstacle from the obstacle avoidance experimental scene graph by the HSV model comprises:
extracting an ROI (region of interest) of a simulated blood vessel from the obstacle avoidance experimental scene graph;
converting the ROI area from RGB space to HSV space;
and adjusting hue parameters of the HSV space according to the colors of the static obstacles, so as to finish the identification of the static obstacles.
4. The method for avoiding the obstacle by the micro-robot based on the combination of the RRT algorithm and the artificial potential field method according to claim 1, wherein the dynamic obstacle avoidance by the improved artificial potential field rule sequentially reaches the sub-target point until reaching the path end point, comprising:
respectively constructing attractive force potential fields of the sub-target points to the micro-robot and repulsive force potential fields of the dynamic obstacles to the micro-robot;
acquiring initialization parameters of a time step of a sub-target point at the current moment, and respectively inputting the initialization parameters into the attraction potential field and the repulsion potential field to obtain virtual resultant force born by the micro-robot, wherein the virtual resultant force is the sum of attraction and repulsion;
acquiring the real-time position of the micro-robot, judging whether the sub-target point is reached, if so, acquiring the initialization parameter of the time step at the current moment of the next sub-target point until the path end point is reached; otherwise, let time step=time step+1, and re-execute the initialization parameter of the current time step of the acquisition sub-target point.
5. The method for avoiding obstacle for a micro-robot based on the combination of the RRT algorithm and the artificial potential field method as set forth in claim 4, wherein constructing the attractive potential field of the sub-target point to the micro-robot comprises:
the attraction potential field comprises a position potential field and a speed potential field, and the expression is as follows:
wherein, xi p Is the position potential field proportionality coefficient, ζ v As a velocity potential field proportionality coefficient, p m Is a microcomputerPosition of robot, p g For the position of the sub-target point ρ (p m ,p g ) Is the relative position of the micro-robot and the sub-target point, v m Is the speed of the micro-robot, v g For the velocity of the sub-target point ρ (v m ,v g ) Relative speeds for the micro-robot and the sub-target point;
the expression for deriving attraction from the attraction potential field is:
wherein F is attp (p) is the attractive force component of the relative position of the pointing sub-target point of the micro-robot, F attv (v) Is the gravitational component of the relative velocity of the sub-target point to the micro-robot,is a unit vector of the sub-target point with respect to the moving direction of the micro-robot,is a unit vector of the micro-robot pointing to the sub-target point.
6. The method for avoiding obstacle for a micro-robot based on the combination of the RRT algorithm and the artificial potential field method according to claim 4, wherein constructing the repulsive potential field of the dynamic obstacle to the micro-robot comprises:
taking the micro-robot as a center, dividing an action area of a repulsive force potential field into an absolute safety area, an early warning obstacle avoidance area, an execution obstacle avoidance area and an absolute obstacle avoidance area according to the distance between the micro-robot and a dynamic obstacle and the simulated blood vessel field;
the absolute safety zone ranges from: the dynamic obstacle located in the absolute safety area does not reach the obstacle avoidance condition beyond the area with the detection obstacle avoidance distance as the radius, the repulsive force is not generated on the micro-robot, and the micro-robot only receives the attractive force at the moment;
the range of the early warning obstacle avoidance area is as follows: removing an annular area formed by the circle with the radius of the detection obstacle avoidance distance from the circle with the radius of the safety distance between the micro-robot and the sub-target point;
the range of the execution obstacle avoidance area is as follows: removing an annular area formed by a circle with the radius of the motion length of the micro-robot from the circle with the radius of the safety distance;
the range of the absolute obstacle avoidance area is as follows: a circular area having a radius corresponding to a movement length of the micro-robot;
the repulsive potential field under each region is expressed as:
wherein ρ is 0 Radius, eta, of the influence range of repulsive potential field of dynamic barrier s Is the proportion coefficient of the repulsive potential field of the early warning obstacle avoidance area, eta e Is to execute the proportion coefficient of the repulsive potential field of the obstacle avoidance area, lambda is the proportion coefficient of the repulsive potential field of the absolute obstacle avoidance area, R m Is the radius of the m-th dynamic obstacle, theta is the included angle between the relative position line of the micro-robot and the dynamic obstacle and the relative speed line of the micro-robot and the obstacle, d is the distance between the micro-robot and the dynamic obstacle, d g Is the distance between the micro-robot and the sub-target point, d m Is the safe distance between the micro-robot and the sub-target point, tau is the radius of the motion field of the micro-robot, CD is the set detection obstacle avoidance distance, v mo Is the relative speed of the micro-robot and the dynamic barrier.
7. The method for avoiding obstacle for a micro-robot based on a combination of RRT algorithm and artificial potential field method as set forth in claim 4, wherein said initialization parameters comprise a position matrix P of said micro-robot m Position matrix P of sub-target points g Speed matrix V of microcomputer robot m Velocity matrix V of dynamic obstacle o Radius R of mth dynamic obstacle m Expressed as：
P m ＝[p ij ] s×2
P g ＝[p g ij ] k×2
V o ＝[v ij ] m×2
Where s is the total number of time steps, i is the ith time step, j=1 represents the abscissa, j=2 represents the ordinate, and m is the number of dynamic obstacles;
the speed and direction of the dynamic barrier are randomly varied, expressed as:
V o-new ＝V o ×A
wherein,representing the sign and the size of each turn of the micro robot, wherein gamma is the maximum changing angle of the dynamic obstacle in a time step, A is the change matrix of the direction of the micro robot, and V o-new And updating the speed matrix for the dynamic obstacle.
8. The method for avoiding obstacle by micro-robot based on the combination of RRT algorithm and artificial potential field method according to claim 6, wherein the expression for deriving repulsive force from said repulsive potential field is:
wherein F is rs1 、F rs2 、F re1 、F re2 、F re3 Respectively are provided withThe repulsive force component is expressed by the expressions:
in the method, in the process of the invention,is the unit vector of the micro-robot pointing to the dynamic obstacle,/->Is the unit vector of the pointing sub-target point of the micro-robot;
F rs1 ，F re1 the direction of the robot is from the robot to the dynamic obstacle, and the robot is far away from the dynamic obstacle; f (F) rs2 ,F re3 The direction of the robot is from the robot to the sub-target point, and the robot is enabled to move to the sub-target point; f (F) re2 Direction of (F) re1 Is perpendicular to the direction of (2); when the relative position direction of the micro-robot and the dynamic obstacle is coincident with the relative speed direction, the micro-robot turns left or turns right to avoid the obstacle, and when the relative position direction of the micro-robot and the dynamic obstacle is positioned at the left side of the relative speed direction, the micro-robot turns left to avoid the obstacle; when the relative position direction of the micro-robot and the dynamic obstacle is positioned at the right side of the relative speed direction, the micro-robot is rightTransferring obstacle avoidance;
the repulsive force applied to the micro-robot is the sum of repulsive force vectors of the dynamic barriers to the micro-robot, and is expressed as:
wherein F is repi The repulsive force generated by the microcomputer robot on the ith dynamic obstacle is that m is the number of the dynamic obstacles.
CN202110838460.1A 2021-07-23 2021-07-23 Micro-robot obstacle avoidance method based on combination of RRT algorithm and artificial potential field method Active CN113485353B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110838460.1A CN113485353B (en) 2021-07-23 2021-07-23 Micro-robot obstacle avoidance method based on combination of RRT algorithm and artificial potential field method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110838460.1A CN113485353B (en) 2021-07-23 2021-07-23 Micro-robot obstacle avoidance method based on combination of RRT algorithm and artificial potential field method

Publications (2)

Publication Number Publication Date
CN113485353A CN113485353A (en) 2021-10-08
CN113485353B true CN113485353B (en) 2024-03-15

Family

ID=77942362

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110838460.1A Active CN113485353B (en) 2021-07-23 2021-07-23 Micro-robot obstacle avoidance method based on combination of RRT algorithm and artificial potential field method

Country Status (1)

Country Link
CN (1) CN113485353B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113985875B (en) * 2021-10-27 2024-02-27 福州大学 Artificial potential field unmanned vehicle dynamic path planning method based on collision prediction model

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101076008B1 (en) * 2010-07-19 2011-10-21 삼성탈레스 주식회사 Autonomous navigation robot for performing magnetic field based path planning
CN102682292A (en) * 2012-05-10 2012-09-19 清华大学 Method based on monocular vision for detecting and roughly positioning edge of road
CN105167849A (en) * 2015-05-19 2015-12-23 上海大学 Three-dimensional vascular path planning method based on ant colony algorithm
CN112577491A (en) * 2020-12-14 2021-03-30 上海应用技术大学 Robot path planning method based on improved artificial potential field method

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101076008B1 (en) * 2010-07-19 2011-10-21 삼성탈레스 주식회사 Autonomous navigation robot for performing magnetic field based path planning
CN102682292A (en) * 2012-05-10 2012-09-19 清华大学 Method based on monocular vision for detecting and roughly positioning edge of road
CN105167849A (en) * 2015-05-19 2015-12-23 上海大学 Three-dimensional vascular path planning method based on ant colony algorithm
CN112577491A (en) * 2020-12-14 2021-03-30 上海应用技术大学 Robot path planning method based on improved artificial potential field method

Also Published As

Publication number Publication date
CN113485353A (en) 2021-10-08

Similar Documents

Publication Publication Date Title
CN110703747B (en) Robot autonomous exploration method based on simplified generalized Voronoi diagram
WO2020180014A2 (en) Method and system for training autonomous driving agent on basis of deep reinforcement learning
EP2618232A1 (en) Map generation device, map generation method, method for moving mobile body, and robot device
CN113110482B (en) Indoor environment robot exploration method and system based on priori information heuristic method
Derry et al. Automated doorway detection for assistive shared-control wheelchairs
JP2017185578A (en) Object gripping device and gripping control program
CN109108942A (en) The mechanical arm motion control method and system of the real-time teaching of view-based access control model and adaptive DMPS
CN109697458A (en) Control equipment mobile method, apparatus, storage medium and electronic equipment
CN106780542A (en) A kind of machine fish tracking of the Camshift based on embedded Kalman filter
CN113485353B (en) Micro-robot obstacle avoidance method based on combination of RRT algorithm and artificial potential field method
JP2020038660A (en) Learning method and learning device for detecting lane by using cnn, and test method and test device using the same
Gao et al. Multi-mobile robot autonomous navigation system for intelligent logistics
CN113614784A (en) Detecting, tracking and three-dimensional modeling of objects using sparse RGB-D SLAM and interactive perception
Zhang et al. Human-robot shared control for surgical robot based on context-aware sim-to-real adaptation
CN107363834B (en) Mechanical arm grabbing method based on cognitive map
WO2020069379A1 (en) Training a deep neural network model to generate rich object-centric embeddings of robotic vision data
Bustamante et al. Towards information-based feedback control for binaural active localization
CN110811497A (en) Control method of capsule endoscope and capsule endoscope using control method
Drwięga Features matching based merging of 3D maps in multi-robot systems
Agustianto et al. Development of real-time motion autonomous surface vehicle controlling for coral reef conservation and fisheries
CN115469662A (en) Environment exploration method, device and application
Ye et al. Design of Industrial Robot Teaching System Based on Machine Vision
Huang et al. An autonomous throat swab sampling robot for nucleic acid test
Majer et al. A precise teach and repeat visual navigation system based on the convergence theorem
Du et al. A Feature Reserved Teaching Method for Pick-Place System under Robot Operating System

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant