A radar control method for three-dimensional space dynamic obstacle avoidance of a micro-helical robot

Abstract

The application discloses a radar control method for three-dimensional space dynamic obstacle avoidance of a micro screw robot, and comprises the following steps: generating all optional motion directions of the micro screw robot in a three-dimensional space in a motion layer; detecting whether an obstacle enters a specified range around the micro screw robot through a detection layer; if the obstacle is detected to enter the specified range, selecting a desired motion direction of the micro screw robot when avoiding the obstacle from all the optional motion directions based on a radar obstacle avoidance algorithm; and controlling the micro screw robot to move away from the obstacle according to the desired motion direction by using a radar control strategy. The application proposes a radar automatic navigation strategy combining the radar obstacle avoidance algorithm and a global path planning algorithm, and proposes a new obstacle avoidance algorithm in a three-dimensional environment, so that the obstacle avoidance efficiency and accuracy of the micro / nano robot in a three-dimensional dynamic environment are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of micro-nano robots, and particularly relates to a radar control method for three-dimensional space dynamic obstacle avoidance of a micro spiral robot. BACKGROUND

[0002] Micro spiral robots have received extensive attention and research in recent years because such robots can achieve efficient and accurate movement under a low-intensity rotating magnetic field. At the same time, micro spiral robots have the potential to be applied to scenarios such as magnetic control micro operation, transport of extracorporeal DNA, and drug delivery in vivo. Advanced control strategies are crucial for micro spiral robots to achieve precise movement, however, it is still a great challenge to use control strategies to achieve automatic navigation of micro spiral robots in a three-dimensional environment with dynamic obstacles.

[0003] For obstacle avoidance of micro-nano robots in a three-dimensional environment, existing technologies are mainly used to solve the problem of static obstacle avoidance of micro-nano robots in a three-dimensional space. For example, an optimized bidirectional RRT* algorithm is used to generate a safe global path in a three-dimensional environment with static obstacles to guide collision-free movement of a micro spiral robot. For example, a small fish-shaped robot can avoid collision with static obstacles in a three-dimensional environment based on a width learning system. However, the above methods for automatically controlling micro-nano robots to avoid obstacles in a three-dimensional space are difficult to apply to dynamic obstacle avoidance in a three-dimensional space. Because dynamic obstacle avoidance requires high efficiency of the algorithm to ensure safe movement of micro-nano robots in an environment with dynamic obstacles. The existing methods require a large amount of calculation for dynamic obstacle avoidance in a three-dimensional environment, and cannot guarantee safe and efficient dynamic obstacle avoidance of micro-nano robots in a three-dimensional environment.

[0004] For dynamic obstacle avoidance, existing technologies are mainly used to solve the problem of dynamic obstacle avoidance of micro-nano robots in a two-dimensional environment. For example, an artificial potential field algorithm is used, and a spherical micro-nano robot can avoid a single moving obstacle in a two-dimensional environment. For example, based on a machine learning method, a micro-nano robot swarm can avoid dynamic obstacles in a two-dimensional environment. For example, based on a two-dimensional radar obstacle avoidance algorithm, a micro-nano robot swarm can also avoid obstacles in a two-dimensional environment with multiple or various dynamic obstacles. However, the existing technologies for automatically controlling micro-nano robots to avoid dynamic obstacles in a two-dimensional environment are difficult to apply to a three-dimensional dynamic environment, because the calculation amount required for obstacle avoidance in a three-dimensional dynamic environment is higher, and the efficiency of the algorithm is more stringent. The existing algorithms in a two-dimensional dynamic environment cannot handle the high calculation amount in a three-dimensional dynamic environment, thereby greatly increasing the risk of obstacle avoidance failure.

[0005] Therefore, the existing technologies need to be improved. SUMMARY

[0006] The technical problem to be solved by the present invention is that, in response to the defects of the existing technology, the present invention provides a radar control method for dynamic obstacle avoidance of a micro spiral robot in three-dimensional space, so as to solve the problem of low obstacle avoidance accuracy of the existing dynamic obstacle avoidance algorithm in a two-dimensional dynamic environment in a three-dimensional dynamic environment.

[0007] The technical solutions adopted by the present invention to solve the technical problems are as follows:

[0008] In a first aspect, the present invention provides a radar control method for a micro-helical robot to avoid obstacles in three-dimensional space, comprising:

[0009] Generate all alternative motion directions of the micro-helical robot in three-dimensional space in the motion layer;

[0010] Detecting, by a detection layer, whether an obstacle enters a specified range around the micro-helical robot;

[0011] If it is detected that the micro-screw robot enters the specified range, a desired movement direction for the micro-screw robot to avoid obstacles is selected from all alternative movement directions based on a radar obstacle avoidance algorithm;

[0012] According to the desired movement direction, the micro spiral robot is controlled to perform obstacle avoidance movement using a radar control strategy.

[0013] In one implementation, generating all the alternative motion directions of the micro-screw robot in the three-dimensional space in the motion layer includes:

[0014] Obtaining the position of each reference micro-screw robot after it moves a certain distance along an alternative direction;

[0015] generating an alternative motion direction of the corresponding reference micro-screw robot according to the acquired position;

[0016] Based on all generated candidate motion directions, all candidate motion directions of the micro screw robot in the three-dimensional space are obtained.

[0017] In one implementation, selecting the desired movement direction of the micro-screw robot when avoiding obstacles from all alternative movement directions based on the radar obstacle avoidance algorithm includes:

[0018] Determine the tendency of each alternative direction to point to the target point and the tendency of each alternative direction to avoid obstacles;

[0019] Determine the objective function of obstacle avoidance based on the tendency of each alternative direction to point to the target point and the tendency of each alternative direction to avoid obstacles;

[0020] Determining the obstacle avoidance quality of each alternative direction according to the objective function;

[0021] The micro-helical robot is controlled to perform obstacle avoidance motion according to the expected motion direction.

[0022] In an implementation, the determining of the trend of each candidate direction pointing to the target point and the trend of each candidate direction avoiding obstacles comprises:

[0023] calculating a relative distance between the micro-helical robot and the position of the target point, and calculating the trend of the corresponding candidate direction pointing to the target point according to the calculated relative distance and a weight of the corresponding candidate direction pointing to the target point;

[0024] calculating a relative distance between the micro-helical robot and the position of the obstacle, and calculating the trend of the corresponding candidate direction avoiding obstacles according to the calculated relative distance and an obstacle avoidance weight.

[0025] In an implementation, the selecting of the candidate direction with the maximum obstacle avoidance quality as the expected motion direction of the micro-helical robot when avoiding obstacles based on the selection constraint and the coarse-to-fine search rule comprises:

[0026] performing coarse search by using the objective function and the selection constraint to obtain a coarse search expected motion direction;

[0027] performing fine search by using the objective function and the selection constraint based on a constant of the direction range in fine search to obtain the expected motion direction of the micro-helical robot when avoiding obstacles.

[0028] In an implementation, the controlling of the micro-helical robot to perform obstacle avoidance motion according to the expected motion direction in a radar control strategy comprises:

[0029] generating a global reference path by using a global path planning algorithm;

[0030] for a detected static obstacle, controlling the micro-helical robot to perform obstacle avoidance motion according to a node in the global reference path and the expected motion direction;

[0031] for a detected dynamic obstacle, adjusting an obstacle avoidance weight in an objective function corresponding to the expected motion direction according to a distance between the dynamic obstacle and the micro-helical robot, and switching a node in the global reference path according to the adjusted obstacle avoidance weight, and controlling the micro-helical robot to perform obstacle avoidance motion according to the switched node and the expected motion direction.

[0032] In an implementation, the detecting of whether the obstacle enters a specified range around the micro-helical robot by using the detection layer further comprises:

[0033] If no entering into the specified range is detected, the expected motion direction of the micro spiral robot when avoiding obstacles is set as a direction from the center to the target point;

[0034] Based on the direction from the center to the target point, the micro spiral robot is controlled to move by the radar control strategy.

[0035] In a second aspect, the present application provides a radar control device for dynamic obstacle avoidance of a micro spiral robot in three-dimensional space, comprising:

[0036] A direction generation module is configured to generate all candidate motion directions of the micro spiral robot in three-dimensional space in a motion layer;

[0037] An obstacle detection module is configured to detect whether an obstacle enters a specified range around the micro spiral robot through a detection layer;

[0038] A direction selection module is configured to select an expected motion direction of the micro spiral robot when avoiding obstacles from all candidate motion directions based on a radar obstacle avoidance algorithm if an entering into the specified range is detected;

[0039] A motion control module is configured to control the micro spiral robot to move by a radar control strategy according to the expected motion direction.

[0040] In a third aspect, the present application provides a terminal, comprising a processor and a memory, wherein the memory stores a radar control program for dynamic obstacle avoidance of a micro spiral robot in three-dimensional space, and the radar control program for dynamic obstacle avoidance of the micro spiral robot in three-dimensional space is used to implement the operations of the radar control method for dynamic obstacle avoidance of the micro spiral robot in three-dimensional space according to the first aspect when executed by the processor.

[0041] In a fourth aspect, the present application further provides a medium, which is a computer readable storage medium, and the medium stores a radar control program for dynamic obstacle avoidance of a micro spiral robot in three-dimensional space, and the radar control program for dynamic obstacle avoidance of the micro spiral robot in three-dimensional space is used to implement the operations of the radar control method for dynamic obstacle avoidance of the micro spiral robot in three-dimensional space according to the first aspect when executed by a processor.

[0042] The present application has the following effects by adopting the above technical solutions:

[0043] The application generates all alternative motion directions of the micro-helical robot in the three-dimensional space through the motion layer, and detects whether the obstacle enters the specified range around the micro-helical robot through the detection layer, so that the desired motion direction of the micro-helical robot when avoiding the obstacle can be selected from all the alternative motion directions based on the radar obstacle avoidance algorithm, and the micro-helical robot is controlled to move to avoid the obstacle according to the desired motion direction in the radar control strategy. The application proposes a radar automatic navigation strategy combining the radar obstacle avoidance algorithm and the global path planning algorithm, and proposes a new obstacle avoidance algorithm in a three-dimensional environment, which improves the obstacle avoidance efficiency and accuracy of the micro-nano robot in a three-dimensional dynamic environment. BRIEF DESCRIPTION OF DRAWINGS

[0044] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or the prior art description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0045] Figure 1 is a flow chart of a radar control method for three-dimensional space dynamic obstacle avoidance of a micro-helical robot in an implementation manner of the present application.

[0046] Figure 2 is a schematic diagram of a three-dimensional layered radar in an implementation manner of the present application.

[0047] Figure 3 is a schematic diagram of a coarse-to-fine direction search in a three-dimensional space in an implementation manner of the present application.

[0048] Figure 4 is a schematic diagram of a radar automatic navigation strategy combining a radar obstacle avoidance algorithm and a global path planning algorithm in an implementation manner of the present application.

[0049] Figure 5 is a control block diagram of a radar control strategy in an implementation manner of the present application.

[0050] Figure 6 is a functional principle diagram of a terminal in an implementation manner of the present application.

[0051] The purposes, functional features and advantages of the present application will be further described with reference to the drawings in combination with the embodiments. DETAILED DESCRIPTION

[0052] In order to make the purposes, technical solutions and advantages of the present application more clear and explicit, the present application will be further described in detail with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application, and are not used to limit the present application.

[0053] Exemplary method

[0054] For dynamic obstacle avoidance, existing technologies are mainly applied to solve the problem of dynamic obstacle avoidance of micro-nano robots in a two-dimensional environment. For example, using an artificial potential field algorithm, a spherical micro-nano robot can avoid a single moving obstacle in a two-dimensional environment; for example, based on a machine learning method, a micro-nano robot cluster can avoid dynamic obstacles in a two-dimensional environment; for example, based on a two-dimensional radar obstacle avoidance algorithm, a micro-nano robot cluster can also avoid obstacles in a two-dimensional environment with multiple or multiple dynamic obstacles. However, existing technologies for automatically controlling micro-nano robots to avoid obstacles in a two-dimensional environment are difficult to apply to a three-dimensional dynamic environment, because the required calculation amount for obstacle avoidance in a three-dimensional dynamic environment is higher, and the efficiency requirement for the algorithm is more stringent. The existing algorithm in a two-dimensional dynamic environment cannot handle the high calculation amount in a three-dimensional dynamic environment, thereby greatly increasing the risk of obstacle avoidance failure.

[0055] To solve the above technical problems, the embodiment of the present application provides a radar control method for three-dimensional space dynamic obstacle avoidance of micro-helical robots. The method generates all candidate motion directions of the micro-helical robot in the three-dimensional space through the motion layer, detects whether the obstacle enters the specified range around the micro-helical robot through the detection layer, selects the desired motion direction of the micro-helical robot when avoiding obstacles from all candidate motion directions based on the radar obstacle avoidance algorithm, and controls the micro-helical robot to move according to the radar control strategy. Therefore, the embodiment of the present application proposes a radar automatic navigation strategy combining radar obstacle avoidance algorithm and global path planning algorithm, proposes a new obstacle avoidance algorithm in a three-dimensional environment, and improves the obstacle avoidance efficiency and accuracy of micro-nano robots in a three-dimensional dynamic environment.

[0056] As shown in Figure 1 The embodiment of the present application provides a radar control method for three-dimensional space dynamic obstacle avoidance of micro-helical robots, which includes the following steps:

[0057] Step S100, generating all candidate motion directions of the micro-helical robot in the three-dimensional space through the motion layer.

[0058] In this embodiment, a radar control strategy is proposed to automatically avoid dynamic obstacles for micro-helical robots in a three-dimensional space. The core of the control strategy is a three-dimensional layered radar, which includes a motion layer and a detection layer. The motion layer can generate all candidate motion directions of the micro-helical robot in the three-dimensional space, and the detection layer can detect whether the obstacle enters a certain range around the micro-helical robot, and then determine whether the obstacle avoidance algorithm is activated.

[0059] Through the radar obstacle avoidance algorithm, the desired motion direction of the micro-helical robot during obstacle avoidance can be determined in real time. Furthermore, this embodiment proposes a coarse-to-fine search method, which reduces the computational complexity of the obstacle avoidance algorithm in three-dimensional space and improves its efficiency. Subsequently, a radar automatic navigation strategy is proposed that combines the radar obstacle avoidance algorithm with an existing global path planning algorithm. This strategy enables the micro-helical robot to achieve three navigation modes in a three-dimensional dynamic environment. Combining the radar automatic navigation strategy with a motion controller results in a radar control strategy that enables the micro-helical robot to achieve collision-free automatic navigation in a three-dimensional environment with multiple static and dynamic obstacles.

[0060] Specifically, in one implementation of this embodiment, obtaining multi-source heterogeneous data includes the following steps:

[0061] Step S101, obtaining the position of each reference micro-screw robot after moving a certain distance along an alternative direction;

[0062] Step S102, generating a corresponding alternative motion direction of the reference micro-screw robot according to the acquired position;

[0063] Step S103: Based on all generated candidate motion directions, all candidate motion directions of the micro screw robot in the three-dimensional space are obtained.

[0064] In this embodiment, the core of the radar control strategy is the three-dimensional layered radar, such as Figure 2 As shown. The three-dimensional layered radar includes a motion layer ( Figure 2 Small and medium circles) and a detection layer ( Figure 2 The detection layer is used to detect whether an obstacle enters a specific range around the micro-nano spiral robot. If no obstacle enters the detection layer, the desired movement direction of the micro-spiral robot will point from its center to the target point. At this time, the desired movement direction of the micro-spiral robot is It can be expressed as:

[0065]

[0066] It should be noted that the direction of motion in any three-dimensional space can be determined by the direction angle θ and the inclination angle To express, that is The direction angle θ is the angle between the projection of the three-dimensional direction on the xoy plane and the positive half axis of the x-axis. It is the angle between the three-dimensional direction and the positive z-axis.

[0067] In formula (1), the position of the target point is P ga =[x ga ,y ga ,zga ] T , and the position of the micro-helical robot is represented by P h (t) = [x h (t),y h (t),z h (t)] T .

[0068] When an obstacle enters the detection layer, the obstacle avoidance behavior of the micro-helical robot will be activated. At this time, the desired motion direction of the micro-helical robot will be selected from all the alternative directions in the three-dimensional space. All the alternative directions in the three-dimensional space are generated by the motion layer, where the i-th alternative direction is represented as like Figure 2 v in i As shown by the arrows. Each alternative direction is obtained by the position of its corresponding reference spiral robot. The reference spiral robot represents the current spiral robot moving a distance d along an alternative direction. m The position after the i-th reference spiral robot is expressed as P rh (i,t)=[x rh (i,t),y rh (i,t),z rh (i,t)] T In this case, the i-th candidate direction corresponding to the i-th reference robot can be expressed as:

[0069]

[0070] Thus, all the alternative directions in three-dimensional space can be expressed as:

[0071]

[0072] In formula (3), Δθ and is the angle θ and Based on formula (3), each reference spiral robot corresponds to an alternative direction in three-dimensional space.

[0073] like Figure 1 As shown, in one implementation of the embodiment of the present invention, the radar control method for dynamic obstacle avoidance of a micro spiral robot in three-dimensional space further includes the following steps:

[0074] Step S200: detecting whether an obstacle enters a specified range around the micro spiral robot through a detection layer.

[0075] In this embodiment, based on all alternative motion directions in the three-dimensional space generated by the motion layer of the inner circle of the micro spiral robot, the detection layer of the outer circle of the micro spiral robot is used to detect whether an obstacle enters the specified range around the micro spiral robot, thereby determining whether the obstacle avoidance algorithm is activated.

[0076] like Figure 1 As shown, in one implementation of the embodiment of the present invention, the radar control method for dynamic obstacle avoidance of a micro spiral robot in three-dimensional space further includes the following steps:

[0077] Step S300 : selecting a desired movement direction for the micro-screw robot to avoid obstacles from all candidate movement directions based on a radar obstacle avoidance algorithm.

[0078] In this embodiment, when an obstacle enters the detection layer, the obstacle avoidance behavior of the micro-helical robot will be activated. At this time, the desired movement direction of the micro-helical robot will be selected from all the alternative directions in the three-dimensional space.

[0079] Specifically, in one implementation of this embodiment, step S300 includes the following steps:

[0080] Step S301 : determining the tendency of each candidate direction pointing to the target point and the tendency of each candidate direction avoiding obstacles.

[0081] In one implementation of this embodiment, step S301 includes the following steps:

[0082] Step S301a, calculating the relative distance between the micro-screw robot and the target point, and calculating the tendency of the corresponding alternative direction to point to the target point based on the calculated relative distance and the weight of the corresponding alternative direction pointing to the target point;

[0083] Step S301b: calculating the relative distance between the micro-screw robot and the obstacle, and calculating the corresponding obstacle avoidance trend in the alternative direction according to the calculated relative distance and the obstacle avoidance weight.

[0084] In this embodiment, during the obstacle avoidance process, the desired movement direction of the micro-screw robot is The objective function is obtained as follows:

[0085]

[0086] In formula (4), σ i represents the quality of the i-th alternative direction, l h is the length of the micro-helical robot, P o is the position of the obstacle, r o is the radius of the obstacle, such as Figure 2 shown. Indicates the trend of the i-th alternative direction pointing to the target point, Indicates the trend of obstacle avoidance in the i-th alternative direction, k g and k a is the corresponding weight. k g and k a The value of is controlled by the following formula:

[0087]

[0088] In formula (5), k s is a constant used to control the two weights (i.e. k g and k a ), and when there are dynamic obstacles, because the difficulty of obstacle avoidance is higher, the ratio of the two weights changes dynamically according to the position of the obstacle. At this time, this ratio is mainly determined by Control, where K g and K a The formula of is inspired by the artificial potential field method and is set as:

[0089]

[0090] Matrix A d is set to:

[0091]

[0092] In formula (7), r de Represents the radius of the detection layer. Based on formulas (5)-(7), the weight k g and k a Can be automatically controlled. When a dynamic obstacle enters the detection layer, k a The value of will increase dynamically as the distance between the obstacle and the micro-spiral robot decreases, ensuring the safety of obstacle avoidance. At the same time, when a dynamic obstacle exists, the weight k is calculated by formulas (6)-(7). g Controlled greater than k a , which ensures that the micro spiral robot can reach the target point when moving in the alternative direction instead of executing the obstacle avoidance command all the time. When there are only static obstacles, because the environment is not dynamically changing, the weight k g and k a The ratio between them is set to a constant, reducing k a The amount of calculation caused by dynamic changes.

[0093] Specifically, in one implementation of this embodiment, step S300 further includes the following steps:

[0094] Step S302, determining an obstacle avoidance target function according to the trend of each alternative direction pointing to the target point and the trend of each alternative direction avoiding obstacles;

[0095] Step S303, determining the obstacle avoidance quality of each alternative direction according to the target function;

[0096] Step S304, selecting the alternative direction with the maximum obstacle avoidance quality as the expected motion direction of the micro spiral robot when avoiding obstacles based on selection constraints and coarse-to-fine search rules.

[0097] Specifically, in an implementation manner of the embodiment, step S304 includes the following steps:

[0098] Step S304a, performing coarse search using the target function and the selection constraints to obtain the expected motion direction of coarse search;

[0099] Step S304b, performing fine search using the target function and the selection constraints based on a constant of the direction range in fine search to obtain the expected motion direction of the micro spiral robot when avoiding obstacles.

[0100] In the embodiment, in the target function (4), σ i represents the quality of the i-th alternative direction, that is, the trend of the alternative direction pointing to the target point and the trend of the alternative direction avoiding obstacles are added under the weights k g and k a The higher the quality of the alternative direction, the higher the efficiency of the direction, that is, the micro spiral robot has a greater possibility to walk a shorter distance along the direction when avoiding obstacles.

[0101] Among all the alternative directions, the alternative direction with the maximum σ i is the expected motion direction of the micro spiral robot, and the maximum σ i is represented by σ max The position of the reference spiral robot corresponding to this alternative direction is P rh (max,t)=[x rh (max,t),y rh (max,t),z rh (max,t)] T Based on formula (2), the expected motion direction at this time is which is obtained by the following formula:

[0102]

[0103] It should be noted that the selection of the expected motion direction is also affected by the selection constraints, which can be represented by the following formula:

[0104]

[0105] Among them, d s For safe distance, use To represent the distance between the micro spiral robot and the obstacle, when this distance is less than the safety distance d s , the corresponding movement direction will not be selected as the desired movement direction.

[0106] In summary, when there is no obstacle entering the detection layer, the expected movement direction of the micro spiral robot is The robot itself will point to the target point, which is also the direction with the shortest distance to the target point. When an obstacle enters the detection layer, the obstacle avoidance behavior of the micro spiral robot will be activated, and the expected movement direction at this time will be selected based on the objective function (Formula (4)) and the selection constraints (Formula (9)).

[0107] In the direction selection of obstacle avoidance, based on formula (3), Δθ and is the angle θ and If the resolution is too low, such as Δθ and If the angular resolution is 1°, then the number of candidate directions is 180*360. The computational complexity of selecting the desired direction of motion from this order of magnitude is extremely large. Unlike two-dimensional space, when the angular resolution is 1°, there are only 360 candidate directions in two-dimensional space. However, simply increasing the resolution Δθ and For example, if these two values ​​are set to 30°, the number of alternative directions will be too small, and the best desired movement direction cannot be selected.

[0108] Therefore, in order to ensure the best direction and reduce the amount of calculation of the algorithm in three-dimensional space, a coarse-to-fine direction search algorithm is designed in this embodiment. The schematic diagram of this algorithm is as follows: Figure 3 In the coarse search, the range of alternative directions is expressed as:

[0109]

[0110] At this time, the angular resolution is set to a relatively large Δθ1 and For example, 30°, which reduces the computational complexity of the algorithm. Using the objective function and selection constraints, the desired motion direction in the coarse search First, it is selected. Then, a fine search is performed. In the fine search, the range of candidate directions is controlled to be close to the desired motion direction selected by the coarse search, which is expressed by the following formula:

[0111]

[0112] Where Δθ fis a constant used to control the range of direction in the fine search. In the fine search, the angular resolution is set to be smaller As shown in 1°, to ensure the accuracy of the algorithm, the more efficient desired motion direction is selected. Finally, the desired motion direction is obtained in the fine search by the objective function and the selection constraint.

[0113] As shown in Figure 1 In an implementation form of the embodiment, the radar control method for three-dimensional dynamic obstacle avoidance of the micro-helical robot further comprises the following steps:

[0114] In step S400, the micro-helical robot is controlled to perform obstacle avoidance motion according to the desired motion direction by using the radar control strategy.

[0115] In this embodiment, the radar obstacle avoidance algorithm includes an objective function, a selection constraint, and a coarse-to-fine search. However, the radar obstacle avoidance algorithm is a local path planning algorithm, which has the disadvantage of being unable to achieve a global optimal solution. However, using only a global path planning algorithm that can achieve a global optimal solution will result in too much calculation to guide the micro-helical robot to perform dynamic obstacle avoidance in a three-dimensional environment. Therefore, a radar automatic navigation strategy is proposed in this embodiment, which combines the radar obstacle avoidance algorithm and the global path planning algorithm.

[0116] Specifically, in an implementation form of the embodiment, step S400 comprises the following steps:

[0117] In step S401, a global path planning algorithm is used to generate a global reference path.

[0118] In step S402, for the detected static obstacle, the micro-helical robot is controlled to perform obstacle avoidance motion according to the node in the global reference path and the desired motion direction.

[0119] In step S403, for the detected dynamic obstacle, the obstacle avoidance weight in the objective function corresponding to the desired motion direction is adjusted according to the distance between the dynamic obstacle and the micro-helical robot, and the node in the global reference path is switched according to the adjusted obstacle avoidance weight, so that the micro-helical robot is controlled to perform obstacle avoidance motion according to the switched node and the desired motion direction.

[0120] In this embodiment, as shown in Figure 4 By using the radar automatic navigation strategy, three navigation modes of the micro-helical robot in an environment with static obstacles and dynamic obstacles can be realized. First, a global path planning algorithm is used to generate a global reference path (a curve in Figure 4 ) to avoid static obstacles (static obstacles in Figure 4The nodes in the global path are q0, q1,... q6, which are also used as the target points to guide the obstacle avoidance of the micro-helical robot. Then, the radar obstacle avoidance algorithm is used to control the micro-helical robot to avoid collision with the dynamic obstacle (the light gray circle). Figure 4 In the first navigation mode, no obstacle is detected by the detection layer of the radar, in which case the micro-helical robot moves along the global path.

[0121] In the second navigation mode, only static obstacles are detected, in which case the ratio of the two weights in the target function is constant, as shown in equation (5). At this time, the micro-helical robot can avoid collision with static obstacles by relying on the obstacle avoidance algorithm, but the constant ratio of the weights reduces the calculation amount, and the weight of the alternative direction pointing to the target point (i.e., the node in the global path) is larger, because the risk of collision in a static environment is low, and the demand for obstacle avoidance is small. Therefore, the second navigation mode is a global path dominated obstacle avoidance. When the dynamic obstacle enters the detection layer, according to equation (5), the weights in the target function will change dynamically with the change of the distance between the dynamic obstacle and the robot, and the weight of the alternative direction obstacle avoidance is larger, because the risk of collision in a dynamic environment is higher, at this time, the navigation mode is the third radar dominated obstacle avoidance mode. In the whole process, the switching of the nodes in the global path as target points is based on the following formula:

[0122]

[0123] wherein Q(t) is the set of nodes in the global path with a distance less than M q from the robot, M q is a constant controlling the number of nodes in the set, q p (i) is the node selected as the target point in the i-th iteration, and when there is no node in the global path with a distance less than M q from the robot, the node selected in the i-th iteration is the same as the node selected in the (i-1)-th iteration.

[0124] In one implementation of the embodiment of the present application, the radar control method for three-dimensional dynamic obstacle avoidance of the micro-helical robot further comprises the following steps:

[0125] Step S500, if no dynamic obstacle is detected in the specified range, the desired motion direction of the micro-helical robot during obstacle avoidance is set as the direction from the center to the target point;

[0126] Step S600, based on the direction from the center to the target point, the radar control strategy is used to control the micro-helical robot to perform obstacle avoidance.

[0127] In practical applications, the motion control, image processing, driving strategy and radar automatic navigation strategy are combined to form the final radar control strategy for the micro spiral robot to automatically perform dynamic obstacle avoidance in three-dimensional space. The block diagram of the radar control strategy is shown in FIG. 8. Figure 5 The radar control strategy includes four parts-global path planning, radar obstacle avoidance algorithm, motion controller and image module. The global path planning and radar obstacle avoidance algorithm have been described above. The final desired motion direction obtained by the radar obstacle avoidance algorithm is input into the motion controller as a desired control signal to control the actual motion of the micro spiral robot. The motion controller generally includes a high-level controller including a feedforward controller and a feedback controller, and a three-dimensional Helmholtz coil system as an actuator. Through the three-dimensional Helmholtz coil system, the micro spiral robot can perform the desired motion, and its real-time position can be obtained through the industrial camera and image processing (see the image module). The output of the entire control strategy is the actual motion direction of the micro spiral robot

[0128] In this embodiment, the innovative radar control strategy is used to control the micro spiral robot to perform dynamic obstacle avoidance in three-dimensional space. The radar obstacle avoidance algorithm in the radar control strategy, the coarse-to-fine direction search algorithm, and the combination of the radar obstacle avoidance algorithm and the global path planning are all very innovative. In actual experiments, using the control strategy, the micro spiral robot can perform obstacle avoidance in three-dimensional space for a single moving obstacle, and the update frequency of the desired motion direction is 3 Hz, which ensures the safety and efficiency of the motion. At the same time, using the control strategy, the micro spiral robot can perform automatic navigation in a three-dimensional space with multiple static obstacles and dynamic obstacles, and the update frequency of the desired motion direction is 2.7 Hz, which also reflects the efficiency of the algorithm applied to obstacle avoidance in three-dimensional space, especially for dynamic obstacle avoidance in three-dimensional space.

[0129] In the actual application scenario of this embodiment, the three-dimensional Helmholtz coil can be used to execute the above strategy to control the micro spiral robot to automatically navigate in a three-dimensional environment with dynamic obstacles; and the magnetic control mechanical arm system can be used to control the micro spiral robot to automatically navigate in a three-dimensional environment with dynamic obstacles.

[0130] In other implementations of this embodiment, other local path planning algorithms can be used instead of the radar algorithm, but the core is still to use the method of the reference robot to obtain the candidate direction in three-dimensional space, and to use the position of the reference robot to obtain the desired motion direction. Or this method will hide the coarse-to-fine direction search algorithm in its own algorithm. These variations of the embodiment of the application all belong to the protection scope of the embodiment of the application.

[0131] The embodiment achieves the following technical effects through the technical scheme:

[0132] The embodiment generates all candidate motion directions of the micro-helical robot in the three-dimensional space through the motion layer, and detects whether the obstacle enters a specified range around the micro-helical robot through the detection layer, so that the desired motion direction of the micro-helical robot when avoiding the obstacle is selected from all candidate motion directions based on a radar obstacle avoidance algorithm, and the micro-helical robot is controlled to move to avoid the obstacle according to the desired motion direction in a radar control strategy. The embodiment proposes a radar automatic navigation strategy combining the radar obstacle avoidance algorithm and the global path planning algorithm, and proposes a new obstacle avoidance algorithm in a three-dimensional environment, thereby improving the obstacle avoidance efficiency and accuracy of the micro-nano robot in a three-dimensional dynamic environment.

[0133] Exemplary device

[0134] Based on the above embodiment, the application further provides a radar control device for three-dimensional space dynamic obstacle avoidance of a micro-helical robot, comprising:

[0135] A direction generation module is configured to generate all candidate motion directions of the micro-helical robot in the three-dimensional space through the motion layer.

[0136] An obstacle detection module is configured to detect whether the obstacle enters a specified range around the micro-helical robot through the detection layer.

[0137] A direction selection module is configured to select a desired motion direction of the micro-helical robot when avoiding the obstacle from all candidate motion directions based on a radar obstacle avoidance algorithm if it is detected that the obstacle enters the specified range.

[0138] A motion control module is configured to control the micro-helical robot to move to avoid the obstacle according to the desired motion direction in a radar control strategy.

[0139] The embodiment achieves the following technical effects through the technical scheme:

[0140] The embodiment generates all candidate motion directions of the micro-helical robot in the three-dimensional space through the motion layer, and detects whether the obstacle enters a specified range around the micro-helical robot through the detection layer, so that the desired motion direction of the micro-helical robot when avoiding the obstacle is selected from all candidate motion directions based on a radar obstacle avoidance algorithm, and the micro-helical robot is controlled to move to avoid the obstacle according to the desired motion direction in a radar control strategy. The embodiment proposes a radar automatic navigation strategy combining the radar obstacle avoidance algorithm and the global path planning algorithm, and proposes a new obstacle avoidance algorithm in a three-dimensional environment, thereby improving the obstacle avoidance efficiency and accuracy of the micro-nano robot in a three-dimensional dynamic environment.

[0141] Based on the above embodiment, the application further provides a terminal, a principle block diagram of which can be shown in Figure 6

[0142] The terminal comprises a processor, a memory, an interface, a display screen and a communication module connected through a system bus; the processor of the terminal is configured to provide computing and control capabilities; the memory of the terminal comprises a storage medium and an internal memory; the storage medium stores an operating system and a computer program; the internal memory provides an environment for the operating system and the computer program in the storage medium to run; the interface is configured to connect external devices; the display screen is configured to display corresponding information; and the communication module is configured to communicate with a cloud server or other devices.

[0143] The computer program is configured to implement the radar control method for the three-dimensional space dynamic obstacle avoidance of the micro spiral robot when executed by the processor.

[0144] Those skilled in the art can understand that, Figure 6 The principle block diagram shown in the above embodiment is only a block diagram of part of the structure related to the application scheme, and does not constitute a limitation on the terminal to which the application scheme is applied; specifically, the terminal can comprise more or fewer components than those shown in the diagram, or combine certain components, or have a different component arrangement.

[0145] In one embodiment, a terminal is provided, comprising a processor and a memory, the memory storing a radar control program for the three-dimensional space dynamic obstacle avoidance of a micro spiral robot, the radar control program for the three-dimensional space dynamic obstacle avoidance of the micro spiral robot being configured to implement the operations of the radar control method for the three-dimensional space dynamic obstacle avoidance of the micro spiral robot as described above when executed by the processor.

[0146] In one embodiment, a storage medium is provided, the storage medium storing a radar control program for the three-dimensional space dynamic obstacle avoidance of a micro spiral robot, the radar control program for the three-dimensional space dynamic obstacle avoidance of the micro spiral robot being configured to implement the operations of the radar control method for the three-dimensional space dynamic obstacle avoidance of the micro spiral robot as described above when executed by the processor.

[0147] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be completed by a computer program instructing relevant hardware, and the computer program can be stored in a non-volatile storage medium, and when executed, can include the processes of the above-mentioned embodiments. Any reference to the memory, storage, database or other medium used in each embodiment of the application can include non-volatile and volatile memories.

[0148] ​In conclusion, the application provides a radar control method for three-dimensional space dynamic obstacle avoidance of a micro-helical robot, which comprises: generating all candidate movement directions of the micro-helical robot in a three-dimensional space in a movement layer; detecting whether an obstacle enters a specified range around the micro-helical robot through a detection layer; if the obstacle is detected to enter the specified range, selecting a desired movement direction of the micro-helical robot when avoiding the obstacle from all candidate movement directions based on a radar obstacle avoidance algorithm; and controlling the micro-helical robot to move to avoid the obstacle according to the desired movement direction in a radar control strategy. The application proposes a radar automatic navigation strategy combining a radar obstacle avoidance algorithm and a global path planning algorithm, proposes a new obstacle avoidance algorithm in a three-dimensional environment, and improves the obstacle avoidance efficiency and accuracy of the micro-nano robot in a three-dimensional dynamic environment.

[0149] It should be understood that the application is not limited to the above examples, and can be improved or changed according to the above description for those of ordinary skill in the art, and all these improvements and changes shall belong to the protection scope of the appended claims of the application.

Claims

1. A radar control method for three-dimensional space dynamic obstacle avoidance of a micro-helical robot, characterized in that, The method comprises the following steps: generating all candidate motion directions of the micro-helical robot in the three-dimensional space at a motion layer; detecting whether an obstacle enters a specified range around the micro-helical robot through a detection layer; if it is detected that the obstacle enters the specified range, selecting a desired motion direction of the micro-helical robot when avoiding the obstacle from all candidate motion directions based on a radar obstacle avoidance algorithm; controlling the micro-helical robot to move away from the obstacle according to the desired motion direction in a radar control strategy; the step of selecting the desired motion direction of the micro-helical robot when avoiding the obstacle from all candidate motion directions based on the radar obstacle avoidance algorithm comprises the steps of: determining the trend of each candidate direction pointing to a target point and the trend of each candidate direction avoiding the obstacle; determining a target function of avoiding the obstacle according to the trend of each candidate direction pointing to the target point and the trend of each candidate direction avoiding the obstacle; determining the obstacle avoidance quality of each candidate direction according to the target function; selecting the candidate direction with the best obstacle avoidance quality as the desired motion direction of the micro-helical robot when avoiding the obstacle based on selection constraints and a coarse-to-fine search rule.

2. The radar control method for three-dimensional space dynamic obstacle avoidance of micro-helical robot according to claim 1, characterized in that, the step of generating all candidate motion directions of the micro-helical robot in the three-dimensional space at the motion layer comprises the steps of: obtaining the position of each reference micro-helical robot after moving along a candidate direction for a distance; generating the candidate motion direction of the corresponding reference micro-helical robot according to the obtained position; obtaining all candidate motion directions of the micro-helical robot in the three-dimensional space based on all generated candidate motion directions. 3.The radar control method for 3D space dynamic obstacle avoidance of micro-helical robot according to claim 1, wherein, the step of determining the trend of each candidate direction pointing to a target point and the trend of each candidate direction avoiding the obstacle comprises the steps of: calculating the relative distance between the micro-helical robot and the position of the target point, and calculating the trend of the corresponding candidate direction pointing to the target point according to the calculated relative distance and the weight of the corresponding candidate direction pointing to the target point; calculating the relative distance between the micro-helical robot and the position of the obstacle, and calculating the trend of the corresponding candidate direction avoiding the obstacle according to the calculated relative distance and the weight of avoiding the obstacle.

4. The radar control method for three-dimensional space dynamic obstacle avoidance of micro-helical robot according to claim 3, characterized in that, the step of selecting the candidate direction with the best obstacle avoidance quality as the desired motion direction of the micro-helical robot when avoiding the obstacle based on the selection constraints and the coarse-to-fine search rule comprises the steps of: performing coarse search using the target function and the selection constraints to obtain the desired motion direction of coarse search; performing fine search using the target function and the selection constraints based on a constant range of directions in fine search to obtain the desired motion direction of the micro-helical robot when avoiding the obstacle.

5. The radar control method for three-dimensional space dynamic obstacle avoidance of micro-helical robot according to claim 1, characterized in that, the step of controlling the micro-helical robot to move away from the obstacle according to the desired motion direction in the radar control strategy comprises the steps of: generating a global reference path using a global path planning algorithm; for the detected static obstacle, controlling the micro-helical robot to move away from the obstacle according to the nodes in the global reference path and the desired motion direction. For the detected dynamic obstacle, the obstacle avoidance weight in the target function corresponding to the desired motion direction is adjusted according to the distance between the dynamic obstacle and the micro spiral robot, and the node in the global reference path is switched according to the adjusted obstacle avoidance weight, and the micro spiral robot is controlled to move in the switched node and the desired motion direction.

6. The radar control method for three-dimensional space dynamic obstacle avoidance of micro-helical robot according to claim 1, characterized in that, The method further comprises: If no obstacle is detected to enter the specified range, the desired motion direction of the micro spiral robot when avoiding obstacles is set as a direction from the center to the target point; Based on the direction from the center to the target point, the micro spiral robot is controlled to move in the radar control strategy.

7. A radar control device for three-dimensional space dynamic obstacle avoidance of a micro-helical robot, for implementing the radar control method for three-dimensional space dynamic obstacle avoidance of a micro-helical robot according to any one of claims 1-6, characterized in that, The method comprises: A direction generation module for generating all candidate motion directions of the micro spiral robot in a three-dimensional space in a motion layer; An obstacle detection module for detecting whether an obstacle enters a specified range around the micro spiral robot through a detection layer; A direction selection module for selecting the desired motion direction of the micro spiral robot when avoiding obstacles from all candidate motion directions based on a radar obstacle avoidance algorithm if it is detected that the specified range is entered; A motion control module for controlling the micro spiral robot to move in the radar control strategy according to the desired motion direction.

8. A terminal, characterized by comprising: The method comprises: A processor and a memory, the memory stores a radar control program for three-dimensional space dynamic obstacle avoidance of a micro spiral robot, and the radar control program for three-dimensional space dynamic obstacle avoidance of the micro spiral robot is used to implement the operations of the radar control method for three-dimensional space dynamic obstacle avoidance of the micro spiral robot as claimed in any one of claims 1-6 when executed by the processor.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a radar control program for three-dimensional space dynamic obstacle avoidance of a micro spiral robot, and the radar control program for three-dimensional space dynamic obstacle avoidance of the micro spiral robot is used to implement the operations of the radar control method for three-dimensional space dynamic obstacle avoidance of the micro spiral robot as claimed in any one of claims 1-6 when executed by the processor.

Images