Radio orientation method and system with obstacle avoidance function

Abstract

The invention discloses a radio orientation method and system with an obstacle avoidance function, and belongs to the technical field of radio orientation. The method comprises the steps that echo signals are transmitted and received through radio, and the direction and distance of an obstacle are calculated; formulating a simulation schematic diagram based on the position relationship between the target object and the obstacle, performing plane division on the simulation schematic diagram based on the Delaunay triangulation network principle, and connecting circle centers of circumcircles of adjacent triangles to generate a plurality of obstacle avoidance routes; the obstacle avoidance route length is calculated by establishing a rectangular plane coordinate system, the obstacle density is quantified in a unit evaluation area, the priority score of each obstacle avoidance route is calculated by combining the distance and the density weight, and the optimal obstacle avoidance route is selected according to the score. According to the invention, through accurate detection, intelligent route planning and quantitative risk assessment, a safe and short-distance obstacle avoidance scheme is provided for the target object, and the method is suitable for navigation requirements of unmanned aerial vehicles, intelligent robots and the like in complex environments.

Description

Technical Field

[0001] The present invention relates to the field of radio orientation technology, and in particular to a radio orientation method and system with obstacle avoidance function. Background Art

[0002] During the radio direction-finding process, the position of obstacles in the direction of travel can be obtained in real time through the transmitted radio waves, and their direction and distance can be located, so that a corresponding route can be formulated to achieve reasonable avoidance.

[0003] In scenarios involving large-scale obstacle clusters, simply avoiding them by circumventing them often significantly increases travel time. In reality, many obstacles within large obstacle clusters are often spaced far apart, fully satisfying the target's passage requirements. Accurately identifying the distribution of obstacles within a cluster and selecting suitable avoidance routes from within them not only achieves obstacle avoidance but also significantly shortens the distance traveled and reduces the target's passage time. Multiple factors must be considered when selecting an avoidance route: first, the length of the route. A shorter route directly reduces travel time, but feasibility must be determined in light of the internal structure of the obstacle cluster. Second, potential risks. The longer the time spent within the obstacle cluster, the greater the risk of encountering unexpected obstacles or environmental changes, necessitating route planning to minimize delays. Third, visibility. The density of obstacles directly affects visibility. The denser the obstacle avoidance route, the poorer the corresponding visibility. Obstructed vision in dense areas can easily lead to misjudgments. Therefore, routes with sparse obstacles and clear visibility should be prioritized.

[0004] In radio direction-finding technology, which is widely used in fields such as intelligent robots and drone navigation, existing obstacle avoidance solutions still face the following technical bottlenecks: on the one hand, path planning faces the "efficiency-safety" trade-off problem. The perimeter bypass strategy will significantly increase the journey length, while blindly penetrating the interior of the obstacle group will increase the risk of collision due to the lack of local density prediction ability; on the other hand, the obstacle risk assessment dimension is single, relying only on distance parameters and lacking quantitative analysis of spatial distribution density, which makes the system unable to effectively distinguish the differences in travel risks in different obstacle areas. The above problems restrict the travel efficiency and safety of smart devices in complex environments. Summary of the Invention

[0005] The purpose of the present invention is to provide a radio direction-finding method and system with obstacle avoidance function, which can achieve safe and short-distance obstacle avoidance of target objects by detecting the direction and distance of obstacles, planning multiple obstacle avoidance routes and evaluating route priorities based on distance and obstacle density.

[0006] To achieve the above object, the present invention adopts a technical solution: a radio direction finding method with obstacle avoidance function, comprising the following steps: S1, using radio to transmit radio waves in real time to detect obstacles and receive echo signals fed back by obstacles; S2. Use the direction measurement algorithm to calculate the direction of each obstacle, and use the distance measurement algorithm to obtain the distance between the obstacle and the target object; S3. Construct an obstacle simulation diagram based on the target object position and the measured obstacle direction and distance, and annotate the simulation diagram with direction and distance; S4. Divide the obstacle simulation diagram into two planes based on the Delaunay triangulation principle, calculate the center of each triangle's circumscribed circle, connect the centers of adjacent triangles' circumscribed circles, and generate multiple obstacle avoidance routes that pass through the obstacle group. S5. Calculate the path length and obstacle density of each obstacle avoidance route, and calculate the priority score of each obstacle avoidance route by combining the path length weight and the obstacle density weight; S6. Select the optimal obstacle avoidance route according to the priority score.

[0007] Furthermore, the obstacle avoidance route generation step in S4 includes: S4.1. Construct a Delaunay triangulation network using the discrete obstacle points in the obstacle simulation diagram; S4.2. Label the discrete points and the formed triangles, record the vertices of each triangle, and calculate the center of the circumscribed circle of each triangle; S4.3. Traverse the triangle list and search for an adjacent triangle that shares a side with the current triangle. If found, connect the centers of the circumcircles of the current triangle and the adjacent triangle. If not found, store the perpendicular ray of the side in the triangle list as part of the obstacle avoidance route.

[0008] Furthermore, the step of calculating the path length of each obstacle avoidance route in S5 includes the following steps: S5.1. Using the target object as the center of the circle, establish a plane rectangular coordinate system; S5.2. Obtain the coordinates of each obstacle based on the measured direction and distance of each obstacle; S5.3. Use the circumscribed circle center formula to calculate the coordinates of the center of each triangle's circumscribed circle, and calculate the distance between the centers of adjacent circumscribed circles in the obstacle avoidance route to obtain the length of each branch line, and then obtain the path length of the obstacle avoidance route.

[0009] Furthermore, the calculation of the obstacle density in S5 includes the following steps: S5.4. Based on the connection order of each obstacle avoidance route, obtain the connecting line segments between the centers of adjacent circumscribed circles and mark them as branches; S5.5. Select each branch line as a unit and construct a square unit evaluation area with the branch line length as the side; S5.6. Calculate the unit obstacle density of each unit assessment area ,in, is the number of obstacles in the unit evaluation area, is the area of the unit assessment region; S5.7. Calculate the average obstacle density of all branches on the same obstacle avoidance route to obtain the obstacle density. .

[0010] Furthermore, the calculation of the priority score in S5 includes the following steps: S5.8. Set the unit distance reference value and its unit weight , get the path length weight of each obstacle avoidance route ,in is the path length of the obstacle avoidance route; S5.9. Setting the unit obstacle density benchmark value and its unit weight , get the obstacle density weight of each obstacle avoidance route ; S5.10. Calculate the priority score of each obstacle avoidance route .

[0011] Furthermore, the implementation process of S4 includes: using dynamic Delaunay triangulation to divide the plane, when the predicted displacement of the obstacle exceeds the threshold, marking the pre-update area, deleting the old triangles in the area and inserting new discrete points, rebuilding the triangulation, calculating the center of the circumscribed circle and generating a connection path between the circumcenters of adjacent triangles.

[0012] Furthermore, the Kalman filter algorithm is used to predict the obstacle motion. The next position of the obstacle is predicted based on the historical coordinates. The state transition equation is: , in, for The estimated value of the obstacle motion state at time for The predicted value of the obstacle motion state at the moment, is the state transition matrix, is the control input matrix, is the control input; for The covariance matrix of the state estimate at the moment, for The covariance matrix of the prior estimate of the state at the moment, is the process noise covariance.

[0013] Furthermore, the calculation of the priority score in S5 includes: (1) Calculate the path length of the obstacle avoidance route , get the path length weight ; Calculate the density of unit obstacles , average the unit density of all branches on the same obstacle avoidance route to obtain the obstacle density on the obstacle avoidance route , get the obstacle density weight ;in, is the path length of the obstacle avoidance route, 、 are the unit distance benchmark value and its unit weight respectively, 、 are the unit obstacle density benchmark value and its unit weight respectively; (2) Detect echo signal strength , calculate the signal attenuation factor ,in, is the initial signal strength reference value; (3) Get the remaining time of the task Total task time , calculate the time window factor ; (4) Calculate the priority score: , through the fuzzy logic controller, the weight coefficient is dynamically adjusted according to the mean value of obstacle movement speed and density variance 、 、 、 ; In S6, the optimal obstacle avoidance route is selected according to the priority score. If the obstacle position is updated, the triangulation network reconstruction and priority score recalculation are triggered.

[0014] Furthermore, the direction measurement algorithm in S2 includes: S2.1. Arrange the echo signals into a signal array to obtain an array signal; S2.2, using linear mixed model to model array signals; S2.3. Perform covariance matrix analysis on the model to obtain eigenvalues; S2.4. Construct a spatial spectrum function using the eigenvalues to calculate the obstacle direction.

[0015] Furthermore, the distance measurement algorithm in S2 includes: S2.5. Establish a distance signal model to obtain a linear frequency modulation signal transmitted by a radio wave; S2.6. Receive the echo signal and obtain a radio wave echo signal model; S2.7. Extract the difference frequency signal and calculate the distance between each obstacle and the current target object based on the difference frequency signal.

[0016] Furthermore, the method for formulating the obstacle simulation schematic diagram in S3 includes: S3.1. Combine the distance and direction measured for each obstacle to obtain its positional relationship with the target object; S3.2. Use an image to simulate the positional relationship between the target object and each obstacle in equal proportion, obtain a schematic diagram of the obstacle simulation, and annotate the simulated diagram with directions and distances.

[0017] The present invention also provides a radio direction finding system with obstacle avoidance function, which is used to implement the above-mentioned radio direction finding method with obstacle avoidance function, including detection equipment, measurement value simulation module, obstacle avoidance route division module, priority evaluation module and obstacle avoidance route selection module; The detection device is used to transmit radio waves, receive echo signals, and calculate the direction and distance of obstacles; The measurement numerical simulation module is used to construct an obstacle simulation schematic diagram based on the target object position and the obstacle direction and distance; The obstacle avoidance route division module uses the Delaunay triangulation principle to perform plane division on the obstacle simulation diagram to generate multiple obstacle avoidance routes; The priority evaluation module is used to calculate the path length and obstacle density of each obstacle avoidance route and calculate the priority score; The obstacle avoidance route selection module is used to select the optimal obstacle avoidance route according to the priority score.

[0018] The beneficial effects of the above scheme are: (1) This invention can efficiently plan obstacle avoidance routes. Based on the principle of Delaunay triangulation, the obstacle simulation diagram is divided into two planes. By constructing a Delaunay triangulation and connecting the centers of the triangle circumcircles, multiple obstacle avoidance routes are generated. This breaks through the traditional "circumferential navigation" mode and selects a penetrating path within the obstacle group, significantly shortening the obstacle avoidance distance and travel time. Furthermore, a Kalman filter algorithm is introduced to predict the movement trajectory of obstacles. Combined with a pre-update mechanism, the Delaunay triangulation network is locally dynamically reconstructed, avoiding the computational redundancy of traditional full updates.

[0019] (2) The present invention uses a multi-factor dynamic weighting approach to assess route risk, comprehensively calculating the path length and obstacle density of each obstacle avoidance route. By setting weights for unit distance and unit obstacle density, the present invention quantitatively generates a route priority score, prioritizing routes with short distances and sparse obstacles. Based on specific scenarios, the present invention sets a signal attenuation factor to quantify the impact of signal quality on obstacle avoidance reliability, and adds a time window factor to balance path safety and mission timeliness, thereby improving route adaptability and safety.

[0020] (3) The present invention uses direction measurement algorithms and distance measurement algorithms to perform array arrangement, modeling analysis and difference frequency signal processing on radio echo signals, accurately calculate the direction of obstacles and the distance to the target object, and provide reliable data support for obstacle avoidance route planning; through system modular design, the whole process from obstacle detection, simulation modeling, route generation to priority evaluation is automated, improving the intelligence level and real-time response capability of the obstacle avoidance system, and is suitable for target navigation needs in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 is a flow chart of the overall method of the present invention; Figure 2 This is a flow chart of the present invention for calculating the direction of each obstacle using a direction measurement algorithm; Figure 3 This is a flow chart of the structure of the present invention using a distance measurement algorithm to obtain the distance between an obstacle and a target object; Figure 4 A flow chart of formulating an obstacle simulation schematic diagram of the present invention; Figure 5 It is a schematic diagram of obstacle simulation of the present invention; Figure 6 It is a simulation flow chart of obstacle avoidance route division of the present invention; Figure 7 This is one of the obstacle simulation coordinate diagrams of the present invention; Figure 8 This is the second obstacle simulation coordinate diagram of the present invention; Figure 9 The third obstacle simulation coordinate diagram of the present invention; Figure 10 This is a flow chart of performing plane division on the obstacle simulation schematic diagram of the present invention; Figure 11 A flow chart of calculating the path length of each obstacle avoidance route according to the present invention; Figure 12 A flowchart of calculating the obstacle density of each obstacle avoidance route of the present invention; Figure 13 A flowchart of obtaining the route priority of each obstacle avoidance route of the present invention; Figure 14 It is a block diagram of the overall system structure of the present invention. DETAILED DESCRIPTION

[0022] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0023] It should be noted that unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

[0024] Example 1 See also Figure 1 As shown, a radio direction-finding method capable of performing obstacle avoidance route planning includes the following steps: S1, using radio to transmit radio waves in real time to detect obstacles and receive echo signals fed back by obstacles; S2. Use the direction measurement algorithm to calculate the direction of each obstacle, and use the distance measurement algorithm to obtain the distance between the obstacle and the target object; S3. Construct an obstacle simulation diagram based on the target object position and the measured obstacle direction and distance, and annotate the simulation diagram with direction and distance; S4. Divide the obstacle simulation diagram into two planes based on the Delaunay triangulation principle, calculate the center of each triangle's circumscribed circle, connect the centers of adjacent triangles' circumscribed circles, and generate multiple obstacle avoidance routes that pass through the obstacle group. S5. Calculate the path length and obstacle density of each obstacle avoidance route, and calculate the priority score of each obstacle avoidance route by combining the path length weight and the obstacle density weight; S6. Select the optimal obstacle avoidance route according to the priority score.

[0025] The implementation process of the present invention is described in detail below: In order to lock the position of obstacles in advance, radio waves are transmitted in real time to detect obstacles, and echo signals fed back after contact with obstacles are received. The direction of each detected obstacle is calculated using a direction measurement algorithm. The specific measurement method is as follows: Figure 2 shown.

[0026] S2.1. Arrange the echo signals into a signal array to obtain an array signal.

[0027] S2.2. Use a linear mixed model to model the array signal. The specific algorithm is as follows: ; in, is the observed signal, the dimension is usually , represents the mixed signal received by M sensors (or channels) in time t, is the mixing matrix, with latitude , describing the signal mixing method, and the angle parameter (such as direction of arrival and modulation parameters, etc.), is the source signal, and the latitude is , represents K independent source signals, For noise.

[0028] S2.3. After completing the modeling process, perform covariance matrix analysis on the entire model. The specific algorithm is as follows: ; in, To observe the signal The autocovariance matrix of is the matrix quantity, represents the expectation operator, which represents the statistical average of the matrix quantity. is the mixing matrix, and the angle parameter Related, is the autocovariance matrix of the source signal, is the conjugate transpose of the mixing matrix, is the noise power, is the identity matrix.

[0029] Perform eigenvalue analysis on the covariance matrix. The specific algorithm is as follows: ; in, is a matrix composed of eigenvectors corresponding to large eigenvalues, with latitude , is a diagonal matrix containing K large eigenvalues, reflecting the signal energy, for The conjugate transpose of is a matrix consisting of eigenvectors corresponding to small eigenvalues, To include the remaining The theoretical value of a diagonal matrix with small eigenvalues is , for The conjugate transpose of .

[0030] S2.4. Construct a spatial spectrum function based on the eigenvalues and calculate the obstacle direction. The specific algorithm is as follows: ; in, is the orientation vector, and the latitude is , indicating that the signal is from the direction The phase response of the incident light to the array, for The conjugate transpose of , where the denominator is used to represent the steering vector The projected energy on the noise subspace is When is the true direction of arrival, Located in the signal subspace, orthogonal to the noise subspace, the denominator approaches zero, resulting in A peak appears, and the peak corresponds to The direction of the obstacle.

[0031] After completing the obstacle direction positioning work, in order to further accurately locate the obstacle position and provide a reference for the subsequent division of obstacle avoidance routes, this solution uses a distance measurement algorithm to obtain the distance between the obstacle and the target object, such as Figure 3 The specific contents are as follows: S2.5. Establish a distance signal model to obtain the linear frequency modulation signal transmitted by the radio wave. The specific algorithm is as follows: ; in, is the model for transmitting linear frequency modulation signal, is the transmitted wave amplitude, is the starting frequency, is the sweep bandwidth, is the frequency sweep period.

[0032] S2.6. When the radio wave hits an obstacle, it will be reflected and the echo signal will be received. The radio wave echo signal model is obtained. The specific algorithm is as follows: ; in, is the reflected linear frequency modulation signal model, is the reflected wave amplitude, is the delay, and , where R is the distance to the obstacle and c is the speed of light.

[0033] S2.7. Next, extract the difference frequency signal and combine it to calculate the distance between each obstacle and the current target. The specific algorithm is as follows: ; in, is the difference frequency, and the final distance formula of the obstacle is obtained from the above formula: , get the distance between each obstacle and the current target.

[0034] In order to plan a route, we must not only consider the distribution of obstacles, but also the impact of route length and route spacing on the passage of the target. Therefore, in the specific obstacle avoidance route planning process, the obstacles and targets with completed direction and distance measurements are simulated to obtain an obstacle simulation diagram, such as Figure 4 As shown: The method for developing an obstacle simulation diagram includes the following steps: S3.1. Combine the distance and direction measured for each obstacle to obtain its positional relationship with the target object; S3.2. Use an image to simulate the positional relationship between the target object and each obstacle in equal proportion, obtain a schematic diagram of the obstacle simulation, and annotate the simulated diagram with directions and distances.

[0035] Obstacle simulation diagram Figure 5 As shown, For the target, as well as The obstacles are located at different positions and form an obstacle group. The dotted line represents the distance between the target and each measured obstacle (scaled proportionally). The angle formed by the dotted line and the horizontal plane is the direction of the obstacle relative to the target. After the simulation diagram is constructed, the Delaunay triangulation principle is used to divide the obstacle simulation diagram into two planes. The specific implementation process is shown in Figure 10: S4.1. Construct a Delaunay triangulation network using the discrete obstacle points in the obstacle simulation diagram; S4.2. Label the discrete points and the triangles they form, record the three discrete points that make up each triangle, and calculate and record the center of the circumscribed circle of each triangle. S4.3. Traverse the formed triangle linked list and search for an adjacent triangle that shares three sides with the current triangle. If found, connect the centers of the circumcircles of the current triangle and the adjacent triangle. If not found, it indicates that the side of the triangle is the outermost side of the Delaunay triangulation network, and store the perpendicular ray of the side in the triangle linked list as part of the obstacle avoidance route.

[0036] Depend on Figure 6 Obstacle segmentation simulation in Figure I As shown in the figure, the obstacle division simulation is formed by forming a Delaunay triangulation network with each discrete point in the obstacle simulation diagram. Figure II ,Right now Figure 5 middle as well as The figure is an example. The number of obstacles in reality is much higher than the obstacle points marked in the figure. Then the discrete points and the triangles are numbered, and the vertices and the center of the circumscribed circle of each triangle are recorded. Figure 6 Simulation of obstacle avoidance route division in Figure II The intersection of each dotted line is the center of the circumcircle of the corresponding triangle. Then traverse the triangle list formed, find the adjacent triangles that share three sides with the current triangle, and form multiple obstacle avoidance routes through the obstacle group, that is, Figure 6 Simulation of obstacle avoidance route division Figure III Obstacle avoidance routes ① and ② are shown.

[0037] Since there are multiple obstacle avoidance routes after classification, this embodiment introduces the path length and obstacle density to determine the optimal choice. Path length directly determines the time it takes for the target to traverse the obstacle group. The shorter the traversal time, the lower the corresponding risk. The obstacle density determines the efficiency of each route. The higher the density, the greater the probability of the target encountering obstruction and path obstruction. Therefore, each obstacle avoidance route is evaluated based on these two data points to select the optimal obstacle avoidance route.

[0038] For the calculation of the path length of the obstacle avoidance route, this solution is calculated by establishing a coordinate system, such as Figure 7 、 8 and Figure 11 The specific contents are as follows: S5.1. Using the target object as the center of the circle, establish a plane rectangular coordinate system.

[0039] S5.2. Obtain the coordinates of each obstacle based on the previously measured direction and distance of each obstacle; i.e. Figure 7 in as well as , and the corresponding coordinates are as well as .

[0040] S5.3. Use the circumscribed circle center formula to obtain the coordinates of the connection points of each circumscribed circle center, and calculate the distance between the centers of adjacent circumscribed circles in the obstacle avoidance route using the distance summation formula. Calculate the length of each branch line in turn to obtain the path length of the obstacle avoidance route.

[0041] Since each obstacle avoidance route is composed of the circumscribed circles of triangles connected by their centers, Figure 8 As shown, the first obstacle avoidance route is The second obstacle avoidance route is composed of composition, as well as are the centers of the circumscribed circles of different triangles respectively. The corresponding coordinates are obtained by the circumscribed circle center formula. Then, the distance between the centers of adjacent circumscribed circles in the obstacle avoidance route is calculated according to the distance summation formula. For example The distance is ,but , and finally calculate the length of each branch line in turn to obtain the length of the final obstacle avoidance route.

[0042] For another evaluation basis, namely the density of obstacles, the range density calculation algorithm is used, such as Figure 9 and Figure 12 The specific contents are as follows: S5.4. Based on the connection order of each obstacle avoidance route, obtain the connecting line segments between the centers of adjacent circumscribed circles and mark them as branches; S5.5. Select each branch line by unit box and construct a square unit evaluation area with the branch line length as the side; for example Figure 9 As shown in A branch line in the obstacle avoidance route is formed into a square with the length of the branch line as the side, and the area enclosed by the square is marked as the unit evaluation area. Figure 9 As shown in as well as Evaluate the areas for different units of the same obstacle avoidance route separately.

[0043] S5.6. Obtain the obstacle quantity of each unit assessment area in turn and the area of the unit assessment area Calculate the unit obstacle density of each unit assessment area ; S5.7. Obtain the unit obstacle density of each branch line on the same obstacle avoidance route , calculate the average obstacle density ,in to is the unit obstacle density of different branches on the same obstacle avoidance route, n represents the number of branches on the same obstacle avoidance route, and the final calculated average obstacle density is Marked as the density of obstacles.

[0044] Finally, by weighting the two evaluation criteria, the route priority of each obstacle avoidance route is obtained, such as Figure 13 The specific contents are as follows: S5.8. Set the unit distance reference value , for the unit distance reference value Unit weighting Match and obtain the path length weight of each obstacle avoidance route ,in is the path length of the obstacle avoidance route; S5.9. Establish a benchmark value for the density of obstacles per unit , the unit obstacle density benchmark value Unit weighting Match and obtain the obstacle density weight of each obstacle avoidance route ; S5.10. Calculate the priority score of each obstacle avoidance route , which is the priority score The priority score is inversely proportional to the length of the obstacle avoidance route and the density of obstacles. The higher the value, the more suitable the obstacle avoidance route is for the current target.

[0045] The present invention detects the direction and distance of obstacles, identifies the position pattern of obstacles on the current target object's route in advance, divides the obstacle group using the Delaunay triangulation principle, obtains the corresponding route, and thus avoids obstacles. At the same time, for the selection of multiple obstacle avoidance routes, the weights of each obstacle avoidance route are divided according to the path length and obstacle density of each obstacle avoidance route. The final priority score is calculated as the basis for evaluating the obstacle avoidance route, providing the target object with a safe and short-distance obstacle avoidance route, improving the adaptation effect of the obstacle avoidance route, and avoiding potential risks in the route.

[0046] Example 2 In order to further improve the adaptability of the present invention in scenarios with strong electromagnetic interference sources, time-sensitive tasks and dynamic obstacles, this embodiment provides a radio orientation method with obstacle avoidance function, and makes the following optimization and improvements to the technical solution of Example 1: (1) Dynamic obstacle modeling: A pre-update mechanism is used to perform local dynamic reconstruction of the Delaunay triangulation network. Based on the obstacle motion prediction results, only the triangulation network areas around obstacles with predicted displacements exceeding the threshold are targeted for deletion and addition of triangles. (2) Priority score calculation: The signal attenuation factor and the time window factor are introduced to participate in the priority score calculation. The signal attenuation factor quantifies the degree of degradation of the radio echo signal quality, reflecting the impact of the signal misjudgment risk on the reliability of obstacle avoidance; the time window factor associates the remaining time of the task with the total duration, balancing the path safety and task timeliness requirements. The specific implementation process is as follows: First, radio detection equipment is used to transmit radio waves at a certain frequency and receive echo signals. A direction measurement algorithm is used to calculate the direction of each obstacle, and a distance measurement algorithm is used to obtain the distance between the obstacle and the target object. The signal strength is collected synchronously, and the obstacle position, estimated speed and signal strength measured at each moment are combined into a state vector.

[0047] Then, the Kalman filter algorithm is used to predict the obstacle motion. The obstacle motion state prediction formula is: ,in, for The estimated value of the obstacle motion state at time for The predicted value of the obstacle motion state at each moment; is the state transfer matrix, which reflects the change of state variables over time; is the control input matrix, is the control input; if there is no external control input, then is a zero matrix, is the zero vector.

[0048] The covariance prediction formula is: .in, for The covariance matrix of the state estimate at the moment, for The covariance matrix of the prior estimate of the state at the moment, Q is the process noise covariance.

[0049] Using the state transition matrix , the obstacle motion state vector at the current moment , derive the predicted state at the next moment , reflecting the relationship between position and velocity over time. According to the covariance prediction formula, the covariance matrix of the predicted state is updated to reflect the uncertainty of the predicted state, and then the confidence in the predicted state is adjusted to make the final state estimate more accurate.

[0050] After that, the Delaunay triangulation is pre-updated. The displacement Δd between the predicted position of the obstacle and the actual position at the current moment is calculated, and the displacement threshold δ is set. If Δd>δ, the Delaunay triangulation within a certain range around the obstacle is marked as a pre-update area; only the old triangles in this area are deleted, and the discrete points (new obstacle coordinates) are reinserted based on the predicted position to generate new triangulation segments, thereby achieving local dynamic updates and avoiding the time-consuming global reconstruction. Next, the method of Example 1 is used to calculate the center of the circumscribed circle, generate a path connecting the circumcenters of adjacent triangles, and obtain multiple obstacle avoidance routes that pass through the obstacle group.

[0051] Finally, the priority score is calculated as follows: (1) Calculate the path length of the obstacle avoidance route , get the path length weight ; Calculate the density of unit obstacles , average the unit density of all branches on the same obstacle avoidance route to obtain the obstacle density on the obstacle avoidance route , get the obstacle density weight ;in, is the path length of the obstacle avoidance route, 、 are the unit distance benchmark value and its unit weight respectively, 、 are the unit obstacle density benchmark value and its unit weight respectively; 、 The calculation process is the same as that in Example 1.

[0052] (2) Detect echo signal strength , calculate the signal attenuation factor ,in, is the initial signal strength reference value; (3) Get the remaining time of the task Total task time , calculate the time window factor ; (4) Dynamically adjust the weight coefficient through the fuzzy logic controller to calculate the average moving speed of obstacles around the obstacle avoidance route and obstacle density variance ,Will and As the input of the fuzzy logic controller, the dynamically adjusted weight coefficient is output 、 、 、 ; Then calculate the priority score of each obstacle avoidance route by the following formula: ,select the one with the highest score as the optimal obstacle avoidance route.,Whenever the obstacle position is updated, the triangulation reconstruction and priority score,recalculation are triggered.

[0053] In specific implementation, the weight coefficient 、 、 、 The initial values of are set to: 、 、 、 ;Signal strength baseline value , as the reference signal strength when there is no significant interference; unit distance benchmark value , unit obstacle density benchmark value The fuzzy logic controller converts the mean obstacle speed and density variance into a fuzzy set (e.g., low speed, medium speed, high speed, low variance, medium variance, high variance), outputs the fuzzy inference rule according to preset fuzzy rules, and then converts the fuzzy result into an accurate weight coefficient through defuzzification, thereby dynamically adjusting the weight coefficients in the priority score formula. Fuzzy rules include: reducing the path length weight and increasing the density weight when obstacles are moving quickly or unevenly distributed; and adjusting the corresponding factor weight coefficients when the signal is unstable or the task time is tight.

[0054] The present invention realizes local dynamic reconstruction of the Delaunay triangulation through a pre-update mechanism, combines it with Kalman filtering to predict obstacle motion, and improves the real-time performance of dynamic scene modeling; introduces signal attenuation factor and time window factor to calculate priority score, quantifies the impact of signal quality and balances task timeliness, enhances adaptability to complex environments, and realizes an upgrade from passive response to active prediction.

[0055] Example 3 This embodiment provides a system for implementing a radio orientation method with an obstacle avoidance function, which is used to implement the radio orientation method with an obstacle avoidance function described in embodiments 1 and 2. Figure 14 As shown, the system includes detection equipment, a measurement numerical simulation module, an obstacle avoidance route division module, a priority evaluation module and an obstacle avoidance route selection module.

[0056] The detection equipment is used to transmit radio waves in real time to detect obstacles, receive echo signals fed back after contact with obstacles, and calculate the direction of each obstacle and the distance between the obstacle and the target object based on the feedback echo signals; The measurement numerical simulation module is used to construct an obstacle simulation diagram based on the target object position and the measured obstacle direction and distance; The obstacle avoidance route division module uses the Delaunay triangulation principle to perform plane division on the obstacle simulation diagram and obtain multiple obstacle avoidance routes for the target object location; The priority evaluation module is used to calculate the path length and obstacle density of each obstacle avoidance route and obtain the priority score of each obstacle avoidance route; The obstacle avoidance route selection module is used to select the optimal obstacle avoidance route based on the priority score.

[0057] Finally, it should be noted that the parts of the present invention that are not described in detail are all prior art. Those skilled in the art will understand that the above description is only a preferred embodiment of the invention and is not intended to limit the invention. Although the invention has been described in detail with reference to the above examples, those skilled in the art can still modify the technical solutions described in the above examples or replace some of the technical features therein with equivalents. Any modifications, equivalent replacements, etc. made within the spirit and principles of the invention should be included in the scope of protection of the invention.

Claims

1. A radio direction finding method with obstacle avoidance function, characterized in that: The following steps are involved: S1, using radio to transmit radio waves in real time to detect obstacles and receive echo signals fed back by obstacles; S2. Use the direction measurement algorithm to calculate the direction of each obstacle, and use the distance measurement algorithm to obtain the distance between the obstacle and the target object; S3. Construct an obstacle simulation diagram based on the target object position and the measured obstacle direction and distance, and mark the simulation diagram with direction and distance; S4. Divide the obstacle simulation diagram into two planes based on the Delaunay triangulation principle, calculate the center of each triangle's circumscribed circle, connect the centers of adjacent triangles' circumscribed circles, and generate multiple obstacle avoidance routes that pass through the obstacle group. S5. Calculate the path length and obstacle density of each obstacle avoidance route, and calculate the priority score of each obstacle avoidance route by combining the path length weight and the obstacle density weight; S6. Select the optimal obstacle avoidance route according to the priority score.

2. The radio orientation method with obstacle avoidance function according to claim 1, characterized in that: The step of generating the obstacle avoidance route in S4 includes: S4.

1. Construct a Delaunay triangulation network using the discrete obstacle points in the obstacle simulation diagram; S4.

2. Label the discrete points and the formed triangles, record the vertices of each triangle, and calculate the center of the circumscribed circle of each triangle; S4.

3. Traverse the triangle list and search for an adjacent triangle that shares a side with the current triangle. If found, connect the centers of the circumcircles of the current triangle and the adjacent triangle. If not found, store the perpendicular ray of the side in the triangle list as part of the obstacle avoidance route.

3. The radio orientation method with obstacle avoidance function according to claim 1, characterized in that: The step of calculating the path length of each obstacle avoidance route in S5 includes the following steps: S5.

1. Using the target object as the center of the circle, establish a plane rectangular coordinate system; S5.

2. Obtain the coordinates of each obstacle based on the measured direction and distance of each obstacle; S5.

3. Use the circumscribed circle center formula to calculate the coordinates of the center of each triangle's circumscribed circle, and calculate the distance between the centers of adjacent circumscribed circles in the obstacle avoidance route to obtain the length of each branch line, and then obtain the path length of the obstacle avoidance route.

4. The radio direction finding method with obstacle avoidance function according to claim 2, characterized in that: The calculation of the obstacle density in S5 includes the following steps: S5.

4. Based on the connection order of each obstacle avoidance route, obtain the connecting line segments between the centers of adjacent circumscribed circles and mark them as branches; S5.

5. Select each branch line as a unit and construct a square unit evaluation area with the branch line length as the side; S5.

6. Calculate the unit obstacle density of each unit assessment area ,in, is the number of obstacles in the unit evaluation area, is the area of the unit assessment region; S5.

7. Calculate the average obstacle density of all branches on the same obstacle avoidance route to obtain the obstacle density. .

5. The radio direction finding method with obstacle avoidance function according to claim 4, characterized in that: The calculation of the priority score in S5 includes the following steps: S5.

8. Set the unit distance reference value and its unit weight , get the path length weight of each obstacle avoidance route ,in is the path length of the obstacle avoidance route; S5.

9. Setting the unit obstacle density benchmark value and its unit weight , get the obstacle density weight of each obstacle avoidance route ; S5.

10. Calculate the priority score of each obstacle avoidance route .

6. The radio orientation method with obstacle avoidance function according to claim 2, characterized in that: The implementation process of S4 includes: using dynamic Delaunay triangulation to divide the plane, when the predicted displacement of the obstacle exceeds the threshold, marking the pre-update area, deleting the old triangles in the area and inserting new discrete points, rebuilding the triangulation, calculating the center of the circumscribed circle and generating a connection path between the circumcenters of adjacent triangles.

7. The radio orientation method with obstacle avoidance function according to claim 6, characterized in that: Obstacle motion prediction uses the Kalman filter algorithm to predict the next position of the obstacle based on historical coordinates. The state transfer equation is: ， in, for The estimated value of the obstacle motion state at time for The predicted value of the obstacle motion state at the moment, is the state transition matrix, is the control input matrix, is the control input; for The covariance matrix of the state estimate at the moment, for The covariance matrix of the prior estimate of the state at the moment, is the process noise covariance.

8. The radio orientation method with obstacle avoidance function according to claim 4, characterized in that: The calculation of the priority score in S5 includes: (1) Calculate the path length of the obstacle avoidance route , get the path length weight ; Calculate the density of unit obstacles , average the unit density of all branches on the same obstacle avoidance route to obtain the obstacle density on the obstacle avoidance route , get the obstacle density weight ;in, is the path length of the obstacle avoidance route, 、 are the unit distance reference value and its unit weight respectively, 、 are the unit obstacle density benchmark value and its unit weight respectively; (2) Detect echo signal strength , calculate the signal attenuation factor ,in, is the initial signal strength reference value; (3) Get the remaining time of the task Total task time , calculate the time window factor ; (4) Calculate the priority score: , through the fuzzy logic controller, the weight coefficient is dynamically adjusted according to the mean value of obstacle movement speed and density variance 、 、 、 ; In S6, the optimal obstacle avoidance route is selected according to the priority score. If the obstacle position is updated, the triangulation network reconstruction and priority score recalculation are triggered.

9. The radio orientation method with obstacle avoidance function according to claim 1, characterized in that: The direction measurement algorithm in S2 includes: S2.

1. Arrange the echo signals into a signal array to obtain an array signal; S2.2, using linear mixed model to model array signals; S2.

3. Perform covariance matrix analysis on the model to obtain eigenvalues; S2.

4. Construct a spatial spectrum function using the eigenvalues to calculate the obstacle direction.

10. A radio direction finding system with obstacle avoidance function, used to implement the radio direction finding method with obstacle avoidance function according to any one of claims 1 to 9, characterized in that: include: Detection equipment, used to transmit radio waves, receive echo signals, and calculate the direction and distance of obstacles; The measurement numerical simulation module is used to construct an obstacle simulation diagram based on the target object position and the obstacle direction and distance; The obstacle avoidance route division module uses the Delaunay triangulation principle to perform plane division on the obstacle simulation diagram and generate multiple obstacle avoidance routes; The priority evaluation module is used to calculate the path length and obstacle density of each obstacle avoidance route and calculate the priority score; The obstacle avoidance route selection module is used to select the optimal obstacle avoidance route based on the priority score.

Images