CN113485353A - 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
CN113485353A
CN113485353A CN202110838460.1A CN202110838460A CN113485353A CN 113485353 A CN113485353 A CN 113485353A CN 202110838460 A CN202110838460 A CN 202110838460A CN 113485353 A CN113485353 A CN 113485353A
Authority
CN
China
Prior art keywords
micro
robot
obstacle
obstacle avoidance
potential field
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.)
Granted
Application number
CN202110838460.1A
Other languages
Chinese (zh)
Other versions
CN113485353B (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

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a micro-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: acquiring an obstacle avoidance experiment scene graph, wherein the graph comprises simulated blood vessel edges, micro robots and obstacles; identifying simulated blood vessel edges from an obstacle avoidance experiment scene graph through template matching, and identifying static obstacles from the obstacle avoidance experiment scene graph through an HSV (hue, saturation and value) model; based on the identified simulated blood vessel edges and static obstacles, carrying out global path planning by using an improved RRT algorithm, and determining key nodes on the global path; and taking the key nodes as child target points, and utilizing an improved artificial potential field rule to prevent dynamic obstacles from sequentially reaching the child target points until a path end point is reached. By using the method, the micro-robot can avoid the obstacle of not only the static obstacle but also the dynamic obstacle in a 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 micro-robot is widely applied due to the advantages of no damage, strong compatibility, wireless remote control and the like, and comprises the medical and biological fields of thrombus dredging, targeted drug delivery, brachytherapy, thermotherapy and the like, thereby representing revolutionary application prospects. Path planning of micro-robots is an important research focus in current micro-robot technology. At present, most of micro-robots can only avoid barriers to static barriers and cannot avoid barriers in real time, and the micro-robots need to consider avoiding the barriers to the static barriers and need to avoid the barriers to dynamic barriers in a complex environment, so that an automatic barrier avoiding strategy for the micro-robots to the dynamic/static barriers in the complex environment needs to be provided.
Disclosure of Invention
The invention provides a micro-robot obstacle avoidance method based on the combination of an RRT algorithm and an artificial potential field method aiming at the problems and technical requirements.
The technical scheme of the invention is as follows:
the micro-robot obstacle avoidance method based on the combination of the RRT algorithm and the artificial potential field method comprises the following steps:
acquiring an obstacle avoidance experiment scene graph, wherein the graph comprises simulated blood vessel edges, micro robots and obstacles;
identifying simulated blood vessel edges from an obstacle avoidance experiment scene graph through template matching, and identifying static obstacles from the obstacle avoidance experiment scene graph through an HSV (hue, saturation and value) model;
based on the identified simulated blood vessel edges and static obstacles, carrying out global path planning by using an improved RRT algorithm, and determining key nodes on the global path;
and taking the key nodes as child target points, and utilizing an improved artificial potential field rule to prevent dynamic obstacles from sequentially reaching the child target points until a path end point is reached.
The further technical scheme is that the method for identifying the simulated blood vessel edge from the obstacle avoidance experiment scene graph through template matching comprises the following steps:
extracting an ROI (region of interest) of a simulated blood vessel from an obstacle avoidance experiment scene graph;
blurring the ROI area to remove edge noise, then performing edge extraction and binarization processing, and performing edge expansion on the obtained binarization edge image to obtain a sampling image simulating the rough edge of the blood vessel;
carrying out edge extraction and binarization processing on the design drawing of the simulated blood vessel, and carrying out edge expansion on the obtained binarization edge image to obtain a design drawing of the simulated blood vessel rough edge;
and carrying out translation, ROI area selection and scaling operation on the design drawing of the simulated blood vessel rough edge, pairing 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 using a differential evolution algorithm, and finishing the identification of the simulated blood vessel edge if the matching is successful.
The further technical scheme is that the method for identifying the static obstacle from the obstacle avoidance experiment scene graph through the HSV model comprises the following steps:
extracting an ROI (region of interest) of a simulated blood vessel from an obstacle avoidance experiment scene graph;
converting the ROI area from an RGB space to an HSV space;
and adjusting hue parameters of the HSV space according to the color of the static obstacle, thereby completing the identification of the static obstacle.
The further technical scheme is that global path planning is carried out by utilizing an improved RRT algorithm, and the key nodes on the global path are determined, and the method comprises the following steps:
generating an initial planned path by using a bidirectional RRT algorithm, and acquiring all nodes of the initial planned path to form a node set A { Y }iI is more than or equal to 1 and less than or equal to n, wherein YiRepresenting the ith node of the initial planned path, sequencing the nodes from a starting point to an end point in sequence, and n is the number of the nodes of the initial planned path;
with Y1As starting point sequentially with Ym(m is 2,3, …, n) are connected by straight line, and the straight line Y is judged in turn1YmWhether a static obstacle is encountered:
if yes, then Y is addedm-1As key node, with YmAs starting point sequentially with Ym+1Making a straight line connection, and sequentially judging straight lines YmYm+1Whether a static obstacle is encountered until the end point Y connected to the initially planned pathnAdding a starting point, a key node and an end point to the set B;
otherwise, will Y2,Y3…Ym-1Directly eliminating the redundant nodes;
and sequentially connecting the nodes in the set B to obtain the optimized global path plan.
The further technical scheme is that the method for preventing dynamic barriers from sequentially reaching sub-target points by utilizing an improved artificial potential field rule until reaching a path terminal point comprises the following steps:
respectively constructing an attraction potential field of the sub-target points to the micro-robot and a repulsion potential field of the dynamic barrier to the micro-robot;
acquiring an initialization parameter of a current time step of a sub-target point, and respectively inputting the initialization parameter into a gravitational potential field and a repulsive force potential field to obtain a virtual resultant force borne by the micro-robot, wherein the virtual resultant force is the sum of the gravitational force and the repulsive force;
acquiring the real-time position of the micro-robot, judging whether the micro-robot reaches a sub-target point, and if so, acquiring the initialization parameter of the current time step of the next sub-target point until the path end point is reached; otherwise, let time step be time step +1, and re-execute the initialization parameter of the current time step of the sub-target point.
The further technical scheme is that the constraint conditions of the bidirectional RRT algorithm comprise:
(1) the nodes on the initial planned path generated by the bi-directional RRT algorithm are all in the region D formed by the identified simulated vessel edges and static obstacles, and are represented as: y isi∈D,i＝1,2,3,...,n
(2) The constraint that each node is from the simulated vessel edge is expressed as: x > taoline
Wherein X represents YiDistance from the simulated vessel edge, tao line, is the threshold.
The further technical scheme is that the method for constructing the attraction potential field of the sub-target points 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 as follows:
Figure BDA0003178020860000031
wherein ξpIs the position potential field scale factor, ξvIs a velocity potential field proportionality coefficient, pmPosition of micro-robot, pgIs the position of the child target point, p (p)m,pg) Is the relative position of the micro-robot and the sub-target point, vmVelocity of the micro-robot, vgVelocity of child target point, ρ (v)m,vg) Relative velocity of the micro-robot and the sub-target point;
the expression for gravity is derived from the gravity potential field as:
Figure BDA0003178020860000032
wherein, Fattp(p) is the gravitational component of the micro-robot pointing to the relative position of the child target points, Fattv(v) Is the gravitational component of the relative velocity of the sub-target points pointing to the micro-robot,
Figure BDA0003178020860000033
is a unit vector of the sub-target points relative to the motion direction of the micro-robot,
Figure BDA0003178020860000034
is the unit vector of the micro-robot pointing to the child target point.
The further technical proposal is that the method for constructing the repulsive force potential field of the dynamic barrier to the micro-robot comprises the following steps:
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 field of simulated blood vessels;
the range of absolute safety zones is: the dynamic barrier in the absolute safety area does not reach the obstacle avoidance condition when exceeding the area outside the circle taking the detection obstacle avoidance distance as the radius, and does not generate the repulsive force action on the micro-robot, and the micro-robot only receives the attraction force action at the moment;
the range of the early warning obstacle avoidance area is as follows: removing an annular area formed by a circle with the radius of the detection obstacle avoidance distance from a circle with the radius of the safety distance between the micro robot and the sub-target points;
the range of the executed obstacle avoidance area is as follows: removing a circular area formed by a circle with the radius of the movement 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 with the movement length of the micro-robot as the radius;
the expression of the repulsive potential field under each region is:
Figure BDA0003178020860000041
where ρ is0Radius, η, of influence range of repulsive potential field of dynamic barriersIs the proportional coefficient of the repulsive force potential field of the early warning obstacle avoidance area etaeIs to execute the proportional coefficient of the repulsive force field of the obstacle avoidance area, lambda is the proportional coefficient of the repulsive force field of the absolute obstacle avoidance area, RmIs the radius of the m-th dynamic obstacle, theta is the angle between the relative position line of the micro-robot and the dynamic obstacle and the relative velocity line of the micro-robot and the obstacle, d is the distance between the micro-robot and the dynamic obstacle, dgIs the distance between the micro-robot and the sub-target point, dmIs the safe distance between the micro-robot and the sub-target points, tau is the radius of the micro-robot in the motion field, CD is the set detection obstacle avoidance distance, vmoIs the relative velocity of the micro-robot and the dynamic obstacle.
The further technical proposal is that the initialization parameters comprise a position matrix P of the micro robotmPosition matrix P of child target pointsgSpeed matrix V of micro-robotmVelocity matrix V of dynamic obstaclesoRadius R of the m-th dynamic obstaclemExpressed as:
Ps＝[pij]s×2
Pg＝[pg ij]k×2
Vo＝[vij]m×2
wherein 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 magnitude and direction of the velocity of the dynamic obstacle is randomly varied, and is represented as:
Figure BDA0003178020860000042
Figure BDA0003178020860000043
Vo-new＝Vo×A
wherein the content of the first and second substances,
Figure BDA0003178020860000044
representing the sign and size of each turn of the micro-robot, gamma being the maximum change angle of the dynamic obstacle in a time step, A being the transformation matrix of the micro-robot changing direction, Vo-newAn updated velocity matrix for the dynamic obstacle.
The further technical scheme is that the expression of the repulsion is deduced by the repulsion potential field as follows:
Figure BDA0003178020860000051
wherein, Frs1、Frs2、Fre1、Fre2、Fre3Respectively, the repulsive force components are expressed as:
Figure BDA0003178020860000052
Figure BDA0003178020860000053
Figure BDA0003178020860000054
Figure BDA0003178020860000055
in the formula (I), the compound is shown in the specification,
Figure BDA0003178020860000056
is a unit vector of the micro-robot pointing to the dynamic obstacle,
Figure BDA0003178020860000057
is a unit vector of the micro-robot pointing to the sub-target point;
Frs1，Fre1the direction of (1) is from the micro-robot to the dynamic barrier, the action is to make the micro-robot far away from the dynamic barrier; frs2,Fre3The direction of (1) is from the micro-robot to the sub-target point, and the function is to make the micro-robot move to the sub-target point; fre2Direction of (1) and Fre1Is vertical; when the relative position direction of the micro-robot and the dynamic barrier coincides with the relative speed direction, the micro-robot turns left or turns right to avoid the barrier, and when the relative position direction of the micro-robot and the dynamic barrier is positioned on the left side of the relative speed direction, the micro-robot turns left to avoid the barrier; when the relative position direction of the micro-robot and the dynamic barrier is positioned on the right side of the relative speed direction, the micro-robot turns right to avoid the barrier;
the repulsion force borne by the micro-robot is the sum of the repulsion force vectors of all the dynamic obstacles to the micro-robot, and is represented as:
Figure BDA0003178020860000058
wherein, FrepiIs the repulsion force generated by the micro-robot to the ith dynamic obstacle, and m is the number of the dynamic obstacles.
The beneficial technical effects of the invention are as follows:
the method comprises the steps of planning a global path through a bidirectional RRT algorithm, eliminating redundant nodes on the global path by using condition constraints and key nodes, and optimizing the safety and the length of the path; and then segmenting the global path according to the key nodes, optimizing each segment of path by combining an improved artificial potential field method, and constructing a repulsive potential field in a region of action of the repulsive potential field with the micro-robot as a circle center, so that the dynamic barrier can be effectively avoided, the global path is further optimized, and finally the micro-robot can not only avoid the barrier to the static barrier but also avoid the barrier to the dynamic barrier in a narrow environment.
Drawings
Fig. 1 is an overall flowchart of an obstacle avoidance method for a micro-robot provided by the present application.
Fig. 2 is an obstacle avoidance experiment scene diagram provided by the present application.
Fig. 3 is a process diagram of image processing performed on an obstacle avoidance experiment scene graph according to the present application.
Fig. 4 is a process diagram of image processing of a plan view of a simulated blood vessel provided by the present application.
Fig. 5 is a graph showing the variation of the number of matching times with the number of iterations in the template matching process provided by the present application.
Fig. 6 is a graph of the results provided herein for identifying static obstacles.
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 an improved artificial potential field method provided by the present application.
Figure 9 is a regional view of the region of action of the repulsive potential field provided by the present application.
Detailed Description
The following further describes the embodiments of the present invention with reference to the drawings.
As shown in fig. 1, a micro-robot obstacle avoidance method based on the combination of the RRT algorithm and the artificial potential field method includes the following steps:
step 1: an obstacle avoidance experiment scene graph is obtained through a CCD camera, as shown in fig. 2, the graph comprises a simulated blood vessel edge 1, a micro-robot 2 and a static obstacle 3.
Optionally, the micro-robot 2 uses magnetic beads with a diameter of 300 μm, and the magnetic beads mainly comprise ferroferric oxide and polystyrene.
Step 2: and identifying a simulated blood vessel edge 1 from the obstacle avoidance experiment scene graph through template matching, and identifying a static obstacle 3 from the obstacle avoidance experiment scene graph through an HSV (hue, saturation, value) model.
The specific method for identifying the simulated blood vessel edge 1 comprises the following steps:
(2A-1) as shown in fig. 3-1, extracting an ROI (region of interest) of a simulated blood vessel from an obstacle avoidance experiment scene graph, and performing fuzzification processing on the ROI to remove edge noise.
(2A-2) performing edge extraction and binarization processing as shown in figures 3-2 and 3-3, and performing edge expansion on the obtained binarized edge image to obtain a sampling image simulating the rough edge of the blood vessel.
(2A-3) with reference to FIGS. 4-1 and 4-2, performing edge extraction and binarization processing on the design drawing of the simulated blood vessel, and performing edge expansion on the obtained binarized edge image to obtain a design drawing of the rough edge of the simulated blood vessel.
(2A-4) translating the design drawing of the simulated blood vessel rough edge, selecting an ROI (region of interest) region and carrying out zooming operation, pairing 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 using a differential evolution algorithm, and adding one to the matching times if the matching is successful as shown in figure 5, wherein the matching success times gradually increase along with the increase of the iteration times and finally tend to be stable, and the identification of the simulated blood vessel edge is completed if the matching is successful.
The specific method of identifying the static obstacle 3 includes:
and (2B-1) extracting an ROI (region of interest) of the simulated blood vessel from the obstacle avoidance experiment scene graph.
(2B-2) converting the ROI area from RGB space to HSV space.
And (2B-3) adjusting hue parameters of the HSV space according to the color of the static obstacle, thereby completing the identification of the static obstacle.
Alternatively, the static obstacle injected by the present application is green, so that the static obstacle can be identified by setting the hue parameter to 0.7-1, as identified by the four-star in fig. 6.
And step 3: based on the identified simulated blood vessel edge 1 and the static obstacle 3, performing global path planning by using an improved RRT algorithm, and determining a key node on a global path, specifically comprising:
(3-1) As shown in FIG. 7-1, generating an initial planned path by using a bidirectional RRT algorithm, and acquiring all nodes of the initial planned path to form a node set A { Y }iI is more than or equal to 1 and less than or equal to n, wherein YiAnd the ith node represents the initial planned path, the nodes are sequentially ordered from the starting point to the end point, and n is the number of the nodes of the initial planned path.
(3-2) as shown in FIG. 7-2, with Y1As starting point sequentially with Ym(m is 2,3, …, n) are connected by straight line, and the straight line Y is judged in turn1YmWhether a static obstacle is encountered:
if yes, then Y is addedm-1As key nodes (i.e. delta marks in the figure), with YmAs starting point sequentially with Ym+1Making a straight line connection, and sequentially judging straight lines YmYm+1Whether a static obstacle is encountered until the end point Y connected to the initially planned pathnThe start point, key node, and end point are added to set B.
Otherwise, will Y2,Y3…Ym-1And directly eliminating the nodes as redundant nodes.
(3-3) sequentially connecting the nodes in the set B to obtain the optimized global path plan, as shown in FIG. 7-3.
The constraints of the bidirectional RRT algorithm include:
(1) the nodes on the initial planned path generated by the bi-directional RRT algorithm are all in the region D formed by the identified simulated vessel edges and static obstacles, and are represented as: y isi∈D,i＝1,2,3,...,n
(2) The constraint that each node is from the simulated vessel edge is expressed as: x > taoline
Wherein X represents YiDistance from the simulated vessel edge, tao line, is the threshold.
And 4, step 4: taking the key node as a child target point, and utilizing an improved artificial potential field rule to avoid dynamic obstacles to sequentially reach the child target point until a path end point is reached, the method specifically comprises the following steps:
(4-1) constructing an attraction potential field of the sub-target points to the micro-robot and a repulsion potential field of the dynamic obstacle to the micro-robot respectively, wherein the specific flow is shown in FIG. 8.
The basic idea of the improved artificial potential field method is to use the combined action of an attractive force potential field and a repulsive force potential field to move the micro-robot.
<1> the gravitational potential field comprises a position potential field and a velocity potential field, and the expression is as follows:
Figure BDA0003178020860000081
wherein ξpIs the position potential field scale factor, ξvIs a velocity potential field proportionality coefficient, pmPosition of micro-robot, pgIs the position of the child target point, p (p)m,pg) Is the relative position of the micro-robot and the sub-target point, vmVelocity of the micro-robot, vgVelocity of child target point, ρ (v)m,vg) The relative velocity of the micro-robot and the sub-target point.
The expression for gravity is derived from the gravity potential field as:
Figure BDA0003178020860000082
wherein, Fattp(p) is the gravitational component of the micro-robot pointing to the relative position of the child target points, Fattv(v) Is the gravitational component of the relative velocity of the sub-target points pointing to the micro-robot,
Figure BDA0003178020860000083
is a unit vector of the sub-target points relative to the motion direction of the micro-robot,
Figure BDA0003178020860000084
is the unit vector of the micro-robot pointing to the child target point.
<2> constructing an improved repulsive force potential field includes:
firstly, with the micro-robot as the center, the action area of the repulsive force potential field is divided 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 the dynamic obstacle and the field of the simulated blood vessel, as shown in fig. 9.
a. Absolute safety zone: and when the obstacle avoidance distance exceeds the area outside 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, the repulsion action is not generated on the micro-robot, and the micro-robot is only under the action of the attraction force.
b. Early warning obstacle avoidance area: removing the circle with the radius of the safety distance CD between the micro-robot and the sub-target point to detect the obstacle avoidance distance dmThe micro robot is an annular area formed by a circle with a radius, and a dynamic obstacle positioned in the early warning obstacle avoidance area generates a corresponding repulsive force potential field to enable the micro robot to be far away from the dynamic obstacle.
c. And (4) executing an obstacle avoidance area: from a safety distance dmThe circle with the radius is removed from an annular area formed by the circle with the radius of the movement length tau of the micro-robot, and a dynamic barrier in the obstacle avoidance execution area generates a corresponding repulsive force potential field to enable the micro-robot to carry out emergency obstacle avoidance.
d. And (3) absolute obstacle avoidance area: the movement length tau of the micro-robot is taken as a radius of the circular area, and a dynamic barrier positioned in the absolute obstacle avoidance area generates a large repulsive force potential field so as to achieve the purpose of effectively avoiding the obstacle.
The expression of the repulsive potential field under each region is:
Figure BDA0003178020860000091
where ρ is0Radius, η, of influence range of repulsive potential field of dynamic barriersIs the proportional coefficient of the repulsive force potential field of the early warning obstacle avoidance area etaeIs to execute the proportional coefficient of the repulsive force field of the obstacle avoidance area, lambda is the proportional coefficient of the repulsive force field of the absolute obstacle avoidance area, RmIs the radius of the m-th dynamic obstacle, theta is the angle between the relative position line of the micro-robot and the dynamic obstacle and the relative velocity line of the micro-robot and the obstacle, d is the distance between the micro-robot and the dynamic obstacle, dgIs the distance between the micro-robot and the sub-target point, dmIs the safe distance between the micro-robot and the sub-target points, tau is the movement length of the micro-robot, CD is the set detection obstacle avoidance distance, vmoIs the relative velocity of the micro-robot and the dynamic obstacle.
The expression for repulsion derived from the repulsive potential field is:
Figure BDA0003178020860000092
wherein, Frs1、Frs2、Fre1、Fre2、Fre3Respectively, the repulsive force components are expressed as:
Figure BDA0003178020860000093
Figure BDA0003178020860000094
Figure BDA0003178020860000095
Figure BDA0003178020860000096
in the formula (I), the compound is shown in the specification,
Figure BDA0003178020860000097
is a unit vector of the micro-robot pointing to the dynamic obstacle,
Figure BDA0003178020860000098
is the unit vector of the micro-robot pointing to the child target point.
Frs1，Fre1The direction of (1) is from the micro-robot to the dynamic barrier, the action is to make the micro-robot far away from the dynamic barrier; frs2,Fre3The direction of (1) is from the micro-robot to the sub-target point, and the function is to make the micro-robot move to the sub-target point; fre2Direction of (1) and Fre1Is vertical; when the relative position direction of the micro-robot and the dynamic barrier coincides with the relative speed direction, the micro-robot turns left or turns right to avoid the barrier, and when the relative position direction of the micro-robot and the dynamic barrier is positioned on the left side of the relative speed direction, the micro-robot turns left to avoid the barrier; when the relative position direction of the micro-robot and the dynamic barrier is positioned on the right side of the relative speed direction, the micro-robot turns right to avoid the barrier.
The repulsion force borne by the micro-robot is the sum of the repulsion force vectors of all the dynamic obstacles to the micro-robot, and is represented as:
Figure BDA0003178020860000101
wherein, FrepiIs the repulsion force generated by the micro-robot to the ith dynamic obstacle, and m is the number of the dynamic obstacles.
And (4-2) acquiring an initialization parameter of the current time step of the sub-target points, and respectively inputting the initialization parameter into the attraction force potential field and the repulsion force potential field to obtain a virtual resultant force applied to the micro-robot, wherein the virtual resultant force is the sum of the attraction force and the repulsion force.
The initialization parameters include a position matrix P of the micro-robotmPosition matrix P of child target pointsgSpeed matrix V of micro-robotmVelocity matrix V of dynamic obstaclesoRadius R of the m-th dynamic obstaclemExpressed as:
Ps＝[pij]s×2
Pg＝[pg ij]k×2
Vo＝[vij]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 magnitude and direction of the velocity of the dynamic obstacle is randomly varied, and is represented as:
Figure BDA0003178020860000102
Figure BDA0003178020860000103
Vo-new＝Vo×A
wherein the content of the first and second substances,
Figure BDA0003178020860000104
representing the sign and size of each turn of the micro-robot, gamma being the maximum change angle of the dynamic obstacle in a time step, A being the transformation matrix of the micro-robot changing direction, Vo-newAn updated velocity matrix for the dynamic obstacle.
Virtual resultant force FvExpressed as: fv＝Fatt+Frep
(4-3) acquiring the real-time position of the micro robot, judging whether the micro robot reaches a sub target point, and if so, acquiring the initialization parameter of the current time step of the next sub target point until the path end point is reached; otherwise, let time step be time step +1, and re-execute the initialization parameter of the current time step 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 embodiment. It is to be understood that other modifications and variations directly derivable or suggested by those skilled in the art without departing from the spirit and concept of the present invention are to be considered as included within the scope of the present invention.

Claims (10)

1. The micro-robot obstacle avoidance method based on the combination of the RRT algorithm and the artificial potential field method is characterized by comprising the following steps:
acquiring an obstacle avoidance experiment scene graph, wherein the graph comprises simulated blood vessel edges, micro robots and obstacles;
identifying the simulated blood vessel edge from the obstacle avoidance experiment scene graph through template matching, and identifying a static obstacle from the obstacle avoidance experiment scene graph through an HSV (hue, saturation and value) model;
based on the identified simulated blood vessel edges and static obstacles, carrying out global path planning by using an improved RRT algorithm, and determining key nodes on the global path;
and taking the key nodes as child target points, and utilizing an improved artificial potential field rule to prevent dynamic obstacles from sequentially reaching the child target points until a path end point is reached.
2. The micro-robot obstacle avoidance method based on the combination of the RRT algorithm and the artificial potential field method according to claim 1, wherein the identifying the simulated blood vessel edges from the obstacle avoidance experiment scene graph through template matching comprises:
extracting an ROI (region of interest) of a simulated blood vessel from the obstacle avoidance experiment scene graph;
blurring the ROI area to remove edge noise, then performing edge extraction and binarization processing, and performing edge expansion on the obtained binarization edge image to obtain a sampling image simulating the rough edge of the blood vessel;
carrying out edge extraction and binarization processing on the design drawing of the simulated blood vessel, and carrying out edge expansion on the obtained binarization edge image to obtain a design drawing of the simulated blood vessel rough edge;
and carrying out translation, ROI area selection and scaling operation on the design drawing of the simulated blood vessel thick edge, pairing the processed design drawing of the simulated blood vessel thick edge with each pixel point of the sampling drawing of the simulated blood vessel thick edge by using a differential evolution algorithm, and finishing the identification of the simulated blood vessel edge if the matching is successful.
3. The micro-robot obstacle avoidance method based on the combination of the RRT algorithm and the artificial potential field method according to claim 1, wherein the identifying static obstacles from the obstacle avoidance experiment scene graph through the HSV model comprises:
extracting an ROI (region of interest) of a simulated blood vessel from the obstacle avoidance experiment scene graph;
converting the ROI area from RGB space to HSV space;
and adjusting hue parameters of the HSV space according to the color of the static obstacle, thereby completing the identification of the static obstacle.
4. The micro-robot obstacle avoidance method based on the combination of the RRT algorithm and the artificial potential field method according to claim 1, wherein the determining the key nodes on the global path by using the improved RRT algorithm for global path planning comprises:
generating an initial planned path by using a bidirectional RRT algorithm, and acquiring all nodes of the initial planned path to form a node set A { Y }iI is more than or equal to 1 and less than or equal to n, wherein YiRepresenting the ith node of the initial planned path, sequencing the nodes from a starting point to an end point in sequence, and n is the number of the nodes of the initial planned path;
with Y1As starting point sequentially with Ym(m is 2,3, …, n) are connected by straight line, and the straight line Y is judged in turn1YmWhether the static obstacle is encountered:
if yes, then Y is addedm-1As key node, with YmAs starting point sequentially with Ym+1Making a straight line connection, and sequentially judging straight lines YmYm+1Whether the static obstacle is encountered until the end point Y connected to the initial planned pathnAdding the starting point, the key node and the end point to a set B;
otherwise, will Y2,Y3…Ym-1Directly eliminating the redundant nodes;
and sequentially connecting the nodes in the set B to obtain the optimized global path plan.
5. The micro-robot obstacle avoidance method based on the combination of the RRT algorithm and the artificial potential field method according to claim 1, wherein the using of the improved artificial potential field law to avoid dynamic obstacles sequentially reaches the sub-target points until reaching the end point of the path comprises:
respectively constructing an attraction potential field of the sub-target points to the micro-robot and a repulsion potential field of the dynamic barrier to the micro-robot;
acquiring an initialization parameter of a current time step of a sub-target point, and respectively inputting the initialization parameter into the attraction force potential field and the repulsion force potential field to obtain a virtual resultant force borne by the micro-robot, wherein the virtual resultant force is the sum of the attraction force and the repulsion force;
acquiring the real-time position of the micro robot, judging whether the micro robot reaches the sub target point, and if so, acquiring the initialization parameter of the current time step of the next sub target point until the path end point is reached; otherwise, making the time step be time step +1, and re-executing the initialization parameter of the current time step of the acquired child target point.
6. The micro-robot obstacle avoidance method based on the combination of the RRT algorithm and the artificial potential field method according to claim 4, wherein the constraint conditions of the bidirectional RRT algorithm comprise:
(1) the nodes on the initial planned path generated by the bi-directional RRT algorithm are all in the region D formed by the identified simulated vessel edges and static obstacles, and are represented as: y isi∈D,i＝1,2,3,...,n
(2) The constraint that each node is from the simulated vessel edge is expressed as: x > tao line
Wherein X represents YiDistance from the simulated vessel edge, tao line, is the threshold.
7. The obstacle avoidance method for the micro-robot based on the combination of the RRT algorithm and the artificial potential field method as claimed in claim 5, wherein constructing the attraction potential field of the sub-targets to the micro-robot comprises:
the gravitational potential field comprises a position potential field and a speed potential field, and the expression is as follows:
Figure FDA0003178020850000031
wherein ξpIs the position potential field scale factor, ξvIs a velocity potential field proportionality coefficient, pmPosition of micro-robot, pgIs the position of the child target point, p (p)m,pg) Is the relative position of the micro-robot and the sub-target point, vmVelocity of the micro-robot, vgVelocity of child target point, ρ (v)m,vg) Relative velocity of the micro-robot and the sub-target point;
deriving from the gravitational potential field an expression for gravitational force as:
Figure FDA0003178020850000032
wherein, Fattp(p) is the gravitational component of the micro-robot pointing to the relative position of the child target points, Fattv(v) Is the gravitational component of the relative velocity of the sub-target points pointing to the micro-robot,
Figure FDA0003178020850000033
is a unit vector of the sub-target points relative to the motion direction of the micro-robot,
Figure FDA0003178020850000034
is a sheet of the micro-robot pointing to a sub-target pointA bit vector.
8. The obstacle avoidance method for the micro-robot based on the combination of the RRT algorithm and the artificial potential field method according to claim 5, wherein the constructing of the repulsive potential field of the dynamic obstacle to the micro-robot comprises:
dividing an action area of a repulsive force field into an absolute safety area, an early warning obstacle avoidance area, an execution obstacle avoidance area and an absolute obstacle avoidance area by taking the micro robot as a center and according to the distance between the micro robot and a dynamic obstacle and the field of a simulated blood vessel;
the range of the absolute safety zone is as follows: exceeding the area outside the circle with the detection obstacle avoidance distance as the radius, and enabling the dynamic obstacle located in the absolute safety area not to reach the obstacle avoidance condition, so that the micro-robot is not subjected to the repulsive force action, and is only subjected to the attractive force action at the moment;
the early warning obstacle avoidance area has the following range: removing an annular area formed by a circle with the radius of the detection obstacle avoidance distance from a circle with the radius of the safety distance between the micro robot and the sub-target points;
the range of the execution obstacle avoidance area is as follows: removing a circular area formed by a circle with the radius of the movement length of the micro-robot from the circle with the radius of the safe distance;
the range of the absolute obstacle avoidance area is as follows: a circular area with the movement length of the micro-robot as a radius;
the expression of the repulsive potential field under each region is:
Figure FDA0003178020850000041
where ρ is0Radius, η, of influence range of repulsive potential field of dynamic barriersIs the proportional coefficient of the repulsive force potential field of the early warning obstacle avoidance area etaeIs to execute the proportional coefficient of the repulsive force field of the obstacle avoidance area, lambda is the proportional coefficient of the repulsive force field of the absolute obstacle avoidance area, RmIs the radius of the mth dynamic obstacle, and theta is the relative position line of the micro-robot and the dynamic obstacleAnd the relative velocity line of the micro-robot and the obstacle, d is the distance between the micro-robot and the dynamic obstacle, dgIs the distance between the micro-robot and the sub-target point, dmIs the safe distance between the micro-robot and the sub-target points, tau is the radius of the micro-robot's field of motion, CD is the set detection obstacle avoidance distance, vmoIs the relative velocity of the micro-robot and the dynamic obstacle.
9. The obstacle avoidance method for micro-robot based on combination of RRT algorithm and artificial potential field method of claim 5, wherein said initialization parameters comprise position matrix P of said micro-robotmPosition matrix P of child target pointsgSpeed matrix V of micro-robotmVelocity matrix V of dynamic obstaclesoRadius R of the m-th dynamic obstaclemExpressed as:
Ps＝[pij]s×2
Pg＝[pg ij]k×2
Vo＝[vij]m×2
wherein 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 magnitude and direction of the velocity of the dynamic obstacle are randomly varied, and are expressed as:
Figure FDA0003178020850000042
Figure FDA0003178020850000043
Vo-new＝Vo×A
wherein the content of the first and second substances,
Figure FDA0003178020850000044
representing the sign and size of each turn of the micro-robot, gamma being the maximum angle of change of the dynamic barrier in a time step, A being the transformation matrix of the micro-robot's direction of change, Vo-newAn updated velocity matrix for the dynamic obstacle.
10. The obstacle avoidance method for the micro-robot based on the combination of the RRT algorithm and the artificial potential field method according to claim 8, wherein the expression of the repulsion force derived from the repulsion force potential field is as follows:
Figure FDA0003178020850000051
wherein, Frs1、Frs2、Fre1、Fre2、Fre3Respectively, the repulsive force components are expressed as:
Figure FDA0003178020850000052
Figure FDA0003178020850000053
Figure FDA0003178020850000054
Figure FDA0003178020850000055
in the formula (I), the compound is shown in the specification,
Figure FDA0003178020850000056
is a unit vector of the micro-robot pointing to the dynamic obstacle,
Figure FDA0003178020850000057
is a unit vector of the micro-robot pointing to the sub-target point;
Frs1，Fre1the direction of (1) is from the micro-robot to the dynamic barrier, the action is to make the micro-robot far away from the dynamic barrier; frs2,Fre3The direction of (1) is from the micro-robot to the sub-target point, and the function is to make the micro-robot move to the sub-target point; fre2Direction of (1) and Fre1Is vertical; when the relative position direction of the micro-robot and the dynamic obstacle coincides 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 on 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 barrier is positioned on the right side of the relative speed direction, the micro-robot turns right to avoid the barrier;
the repulsion force borne by the micro-robot is the vector sum of the repulsion force of each dynamic obstacle to the micro-robot, and is represented as:
Figure FDA0003178020850000058
wherein, FrepiIs the repulsion force generated by the micro-robot to the ith dynamic obstacle, and 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 true CN113485353A (en) 2021-10-08
CN113485353B 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)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113985875A (en) * 2021-10-27 2022-01-28 福州大学 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

Cited By (2)

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

Also Published As

Publication number Publication date
CN113485353B (en) 2024-03-15

Similar Documents

Publication Publication Date Title
CN110703747B (en) Robot autonomous exploration method based on simplified generalized Voronoi diagram
Cheng et al. Topological indoor localization and navigation for autonomous mobile robot
US8649557B2 (en) Method of mobile platform detecting and tracking dynamic objects and computer-readable medium thereof
WO2020062433A1 (en) Neural network model training method and method for detecting universal grounding wire
CN113110482B (en) Indoor environment robot exploration method and system based on priori information heuristic method
WO2018052875A1 (en) Image depth prediction neural networks
Kulhánek et al. Visual navigation in real-world indoor environments using end-to-end deep reinforcement learning
Johns et al. Autonomous dry stone: On-site planning and assembly of stone walls with a robotic excavator
CN109697458A (en) Control equipment mobile method, apparatus, storage medium and electronic equipment
Asadi et al. Building an integrated mobile robotic system for real-time applications in construction
WO2020069379A1 (en) Training a deep neural network model to generate rich object-centric embeddings of robotic vision data
Wang et al. Affordance-based mobile robot navigation among movable obstacles
CN113485353A (en) Micro-robot obstacle avoidance method based on combination of RRT algorithm and artificial potential field method
Chronis et al. Sketch-based navigation for mobile robots
CN115083199B (en) Parking space information determining method and related equipment thereof
CN113671960A (en) Autonomous navigation and control method of magnetic micro-nano robot
Matsuo et al. Outdoor visual localization with a hand-drawn line drawing map using fastslam with pso-based mapping
Song et al. Critical rays self-adaptive particle filtering SLAM
Hong et al. Real-time visual-based localization for mobile robot using structured-view deep learning
CN115309164B (en) Man-machine co-fusion mobile robot path planning method based on generation of countermeasure network
Hoshino et al. End-to-end motion planners through multi-task learning for mobile robots with 2D LiDAR
CN114859940A (en) Robot movement control method, device, equipment and storage medium
Vatcha et al. Practical motion planning in unknown and unpredictable environments
Viet et al. Dyna-QUF: Dyna-Q based univector field navigation for autonomous mobile robots in unknown environments
Al-Shanoon Developing a mobile manipulation system to handle unknown and unstructured objects

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