Ultra-wideband positioning navigation and path planning method for unmanned equipment in special space

Abstract

The invention discloses an ultra-wideband positioning navigation and path planning method for unmanned equipment in a special space, and relates to the technical field of ultra-wideband positioning, and the method comprises the steps: obtaining an ultra-wideband positioning base station deployment scheme, and deploying a plurality of ultra-wideband positioning base stations in the special space; an ultra-wideband communication module provided with an ultra-wideband positioning navigation system is additionally arranged in an advancing control system of the unmanned equipment; the ultra-wideband positioning navigation system measures the time from the signal to the ultra-wideband positioning base station by adopting a time difference of arrival method, determines the distance from the unmanned equipment to the ultra-wideband positioning base station, and generates real-time position information of the unmanned equipment; performing path planning and advancing control on the unmanned equipment according to the real-time position information based on a path planning algorithm; the operation data in the operation process are collected in real time and are stored and transmitted, so that the application limitation of a traditional positioning navigation system in a special environment can be solved, accurate positioning navigation and path planning of the unmanned equipment are realized, and the operation capability and efficiency of the unmanned equipment are improved.

Description

Technical Field

[0001] The present invention relates to the field of ultra-wideband positioning technology, and in particular to an ultra-wideband positioning navigation and path planning method for unmanned equipment in a special space. Background Art

[0002] In confined spaces or special environments where GPS signals cannot work properly and remote control signals are blocked, traditional GPS-based positioning and navigation systems cannot achieve accurate and effective positioning and navigation tasks. These special environments include mine tunnels, underwater operation areas of reservoir dams, bridge support columns and port culverts, which often require regular structural damage inspections or detection operations. At present, these tasks usually rely on manual exploration, but with the rise of unmanned equipment, unmanned equipment such as drones, unmanned boats and spider-type self-propelled robots are gradually being used in exploration operations in these scenarios.

[0003] However, there are usually various limitations in these special environments, such as electromagnetic shielding, GPS signal penetration, wave or air flow interference, etc. Therefore, the positioning and navigation system of the unmanned equipment may fail, the self-obstacle avoidance function may be mishandled, and the remote control signal may be blocked. These problems seriously limit the operation capabilities of the unmanned equipment in special environments.

[0004] In view of this, how to achieve accurate positioning, navigation and path planning of unmanned equipment in special environments where GPS signals cannot work properly and remote control signals are blocked has become a problem that needs to be solved urgently. Summary of the invention

[0005] In order to solve the above problems, the present invention proposes an ultra-wideband positioning, navigation and path planning method for unmanned equipment in a special space, which solves the application limitations of traditional positioning and navigation systems in special environments. In special environments where GPS signals cannot work normally and remote control signals are blocked, accurate positioning, navigation and path planning of unmanned equipment can be achieved, thereby improving the operating capability and efficiency of unmanned equipment.

[0006] In order to achieve the above object, the present invention is implemented by the following technical solutions:

[0007] The present invention is an ultra-wideband positioning navigation and path planning method for unmanned equipment in a special space, comprising the following steps:

[0008] Obtain the deployment plan of ultra-wideband positioning base stations and deploy multiple ultra-wideband positioning base stations in a special space;

[0009] An ultra-wideband communication module is added to the travel control system of the unmanned equipment, and the ultra-wideband communication module is correspondingly configured with an ultra-wideband positioning and navigation system;

[0010] The ultra-wideband positioning and navigation system uses the arrival time difference method to measure the time when the signal sent by the unmanned device reaches the ultra-wideband positioning base station, determines the distance from the unmanned device to the ultra-wideband positioning base station, and generates the real-time position information of the unmanned device;

[0011] In the special space, path planning and travel control of the unmanned equipment are performed based on the real-time location information based on a path planning algorithm;

[0012] The operation data of the unmanned equipment during operation is collected in real time and stored and transmitted.

[0013] A further improvement of the present invention is that when acquiring the ultra-wideband positioning base station deployment plan, the signal receiving power from the unmanned device to the ultra-wideband positioning base station is determined based on the signal propagation model:

[0014]

[0015] Where P v represents the signal receiving power of the ultra-wideband positioning base station; P t Indicates the signal transmission power of unmanned equipment; G t Represents the transmitting antenna gain of the unmanned equipment; G v represents the receiving antenna gain of the ultra-wideband positioning base station; λ represents the signal wavelength; d represents the distance between the unmanned device and the ultra-wideband positioning base station;

[0016] Calculate the signal coverage area from the unmanned device to the ultra-wideband positioning base station:

[0017] A=πR 2

[0018] Where A represents the signal coverage area from the unmanned device to the ultra-wideband positioning base station; R represents the effective coverage radius of the signal from the unmanned device to the ultra-wideband positioning base station;

[0019] The signal path loss from the unmanned device to the ultra-wideband positioning base station is calculated based on the logarithmic distance path loss model:

[0020]

[0021] Where d represents the distance between the unmanned device and the ultra-wideband positioning base station; L(d) represents the signal path loss at distance d; L(d0) represents the signal path loss at reference distance d0; β represents the signal path loss coefficient; log 10 () represents the logarithmic function with base 10;

[0022] The deployment plan of the ultra-wideband positioning base station is determined according to the signal receiving power, signal coverage area and signal path loss from the unmanned device to the ultra-wideband positioning base station.

[0023] A further improvement of the present invention is that for unmanned equipment operation areas with conditional access to power, the ultra-wideband positioning base station is deployed in the special space by an active connection method; for unmanned equipment operation areas without conditional access to power, a passive method with built-in batteries or solar panels is selected to deploy the ultra-wideband positioning base station in the special space.

[0024] A further improvement of the present invention is that the ultra-wideband positioning and navigation system uses the arrival time difference method to measure the time from the signal sent by the unmanned device to the ultra-wideband positioning base station, determines the distance from the unmanned device to the ultra-wideband positioning base station, and generates the real-time location information of the unmanned device, specifically including:

[0025] The ultra-wideband positioning and navigation system determines the distance from the unmanned device to multiple ultra-wideband positioning base stations by measuring the time when the signal sent by the unmanned device arrives at the ultra-wideband positioning base station; draws a circle with the ultra-wideband positioning base station as the center and the distance from the unmanned device to the ultra-wideband positioning base station as the radius to determine the position of the signal; determines the corresponding distance difference by comparing the time difference when the signal arrives at multiple ultra-wideband positioning base stations, and draws a hyperbola with the ultra-wideband positioning base station as the focus and the distance difference as the major axis. The intersection of the hyperbola is the position of the unmanned device, and the real-time position information of the unmanned device is generated. The specific calculation formula is as follows:

[0026] In the three-dimensional rectangular coordinate system, the position vector of the i-th ultra-wideband positioning base station is set to 1≤i≤M, where Respectively represent the position vector of the i-th ultra-wideband positioning base station in the x, y, and z directions; M represents the total number of ultra-wideband positioning base stations; [] T Represents a matrix transpose operation;

[0027] The total position vector is obtained by combining the position vectors of all ultra-wideband positioning base stations: in, Respectively represent the position vectors of the 1st, 2nd, ..., Mth ultra-wideband positioning base stations;

[0028] Set the position vector of the unmanned equipment to in, Respectively represent the position vector of the unmanned equipment in the x, y, and z directions;

[0029] The first ultra-wideband positioning base station is used as the reference base station, and the true value of the time difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station is calculated, that is, the arrival time difference:

[0030]

[0031] In the formula, represents the true value of the time difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station, i.e., the arrival time difference; c represents the signal propagation speed; u o A vector representing the position of the unmanned equipment; represents the position vector of the jth ultra-wideband positioning base station; represents the position vector of the first ultra-wideband positioning base station, that is, the position vector of the reference base station; M represents the total number of ultra-wideband positioning base stations; represents the distance between the position vector of the unmanned device and the position vector of the jth ultra-wideband positioning base station; Represents the distance between the position vector of the unmanned device and the position vector of the first ultra-wideband positioning base station.

[0032] A further improvement of the present invention is that the arrival time difference is converted into an arrival distance difference:

[0033]

[0034] In the formula, It represents the true value of the distance difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station, that is, the arrival distance difference; represents the position vector of the jth ultra-wideband positioning base station; represents the position vector of the reference base station; represents the distance between the position vector of the unmanned device and the position vector of the jth ultra-wideband positioning base station; represents the distance between the position vector of the unmanned equipment and the position vector of the first ultra-wideband positioning base station; M represents the total number of ultra-wideband positioning base stations;

[0035] The arrival distance difference is updated in combination with the noise factor of the ultra-wideband positioning base station. The calculation formula of the updated arrival distance difference is:

[0036]

[0037] In the formula, r j1 represents the updated arrival distance difference; It represents the true value of the distance difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station, that is, the arrival distance difference; δ represents the noise coefficient of the ultra-wideband positioning base station;

[0038] The arrival distance difference of all ultra-wideband positioning base stations is combined to obtain the total arrival distance difference:

[0039]

[0040] Where r represents the total arrival distance difference; r o Indicates the true value of the distance difference; n r Indicates the distance difference noise value; represents the true value of the distance difference between the signal reaching the 1st, 2nd, ..., Mth ultra-wideband positioning base station and the reference base station; δ represents the noise coefficient of the ultra-wideband positioning base station;

[0041] The distance difference noise value n r The corresponding covariance matrix is ​​Q r , the total arrival distance difference r is about the position vector u of the unmanned device o The log-likelihood function of is:

[0042]

[0043] Where, ln(p()) represents the log-likelihood function; r represents the total arrival distance difference; r o Indicates the true value of the distance difference; Q r Represents the covariance matrix corresponding to the distance difference noise value; u o represents the position vector of the unmanned equipment; ξ represents the position constant of the unmanned equipment;

[0044] The corresponding calculation formula of the Cromerow lower bound is:

[0045]

[0046] Where CRLB represents the Cromer-Rao lower bound; r o Indicates the true value of the distance difference; u o Represents the position vector of the unmanned equipment; Q r Represents the covariance matrix corresponding to the distance difference noise value.

[0047] A further improvement of the present invention is that: the distance difference true value r o and the position vector u of the unmanned equipment o , total position vector s o There is a nonlinear relationship between:

[0048] r o =f(u o ,s o )

[0049] In the formula, r o Indicates the true value of the distance difference; u o Represents the position vector of the unmanned equipment; s o represents the total position vector; f represents the nonlinear function;

[0050] Set the kth iteration result to Based on the weighted least squares criterion, the following iterative formula is obtained:

[0051]

[0052] In the formula, Indicates the result of the k+1th iteration; Indicates the result of the kth iteration; s o represents the total position vector; Indicates about and o Nonlinear function of express right The partial derivative of Q r represents the covariance matrix corresponding to the distance difference noise value; r represents the total arrival distance difference;

[0053] when The iteration stops when , where λ represents the iteration stopping threshold;

[0054] The calculation formula of the covariance matrix of the position result of the unmanned equipment is:

[0055]

[0056] In the formula, Indicates the location result of unmanned equipment; The covariance matrix representing the position results of the unmanned equipment; Indicates the result of the k+1th iteration; Indicates the result of the kth iteration; express right The partial derivative of Q r Represents the covariance matrix corresponding to the distance difference noise value; s o represents the total position vector;

[0057] At this time we get:

[0058]

[0059] In the formula, express right The partial derivative of Indicates the location result of unmanned equipment; represents the covariance matrix of the position result of the unmanned equipment; CRLB represents the Cromer-Rao lower bound; Indicates the result of the kth iteration; s o represents the total position vector; r represents the total arrival distance difference;

[0060] The corresponding real-time location information is determined according to the location result of the unmanned equipment.

[0061] A further improvement of the present invention is that based on the real-time position information of the unmanned equipment, a path planning algorithm is used to determine the path constraints of the unmanned equipment in the special space, namely the shortest path constraint and the minimum obstacle threat constraint, and the path planning and travel control of the unmanned equipment are performed based on the path constraints.

[0062] A further improvement of the present invention is that the operation data collected in real time during the operation of the unmanned equipment is stored and transmitted in an offline manner.

[0063] The beneficial effects of the present invention are as follows: the present invention proposes an ultra-wideband positioning navigation and path planning method for unmanned equipment in a special space, by obtaining an ultra-wideband positioning base station deployment plan, and deploying multiple ultra-wideband positioning base stations in a special space; an ultra-wideband communication module is added to the travel control system of the unmanned equipment, and the ultra-wideband communication module is correspondingly configured with an ultra-wideband positioning navigation system; the ultra-wideband positioning navigation system uses the arrival time difference method to measure the time from the signal sent by the unmanned equipment to the ultra-wideband positioning base station, determines the distance from the unmanned equipment to the ultra-wideband positioning base station, and generates the real-time location information of the unmanned equipment; in the special space, the path planning algorithm is used to plan the path and control the travel of the unmanned equipment according to the real-time location information; the operation data of the unmanned equipment in the operation process is collected in real time and stored and transmitted. The present invention can realize accurate positioning navigation and path planning of unmanned equipment in special environments where GPS signals cannot work normally and remote control signals are blocked, thereby improving the operation capability and efficiency of unmanned equipment in special environments, and reducing the risk and cost of manual exploration; and through the centimeter-level accuracy provided by the UWB positioning navigation system, the details such as structural damage in the special space can be accurately detected and recorded, and the method of the present invention is applicable to a variety of special space environments, and has a wide range of application prospects and promotion value. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 A flowchart of an ultra-wideband positioning, navigation and path planning method for unmanned equipment in a special space provided by an embodiment of the present invention;

[0065] Figure 2 A schematic diagram of deploying multiple ultra-wideband positioning base stations in a special space provided by an embodiment of the present invention;

[0066] Figure 3 A schematic diagram of drawing a circle in the arrival time difference method provided by an embodiment of the present invention;

[0067] Figure 4A schematic diagram of a hyperbola in the arrival time difference method provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0068] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0069] See also Figure 1 , a method for ultra-wideband positioning, navigation and path planning of unmanned equipment in a special space in this embodiment includes the following steps:

[0070] S1, obtain the ultra-wideband positioning base station deployment plan, and deploy multiple ultra-wideband positioning base stations in a special space;

[0071] S2, adding an ultra-wideband communication module to the travel control system of the unmanned equipment, and the ultra-wideband communication module is correspondingly configured with an ultra-wideband positioning and navigation system;

[0072] S3, the ultra-wideband positioning and navigation system uses the arrival time difference method to measure the time from the signal sent by the unmanned device to the ultra-wideband positioning base station, determines the distance from the unmanned device to the ultra-wideband positioning base station, and generates real-time location information of the unmanned device;

[0073] S4, performing path planning and travel control on the unmanned device in the special space based on the real-time location information based on a path planning algorithm;

[0074] S5, collecting, storing and transmitting the operation data of the unmanned equipment during operation in real time.

[0075] Understandably, see Figure 2 First, we can obtain the deployment plan of ultra-wideband (UWB) positioning base stations, and deploy multiple ultra-wideband positioning base stations in a specific complex space structure. The selection and layout of these base stations need to comprehensively consider the spatial size, obstacle distribution and signal attenuation factors, so as to ensure that there is no dead angle coverage and the positioning accuracy meets the application requirements, and ensure the time synchronization and signal transmission quality between base stations.

[0076] Subsequently, an ultra-wideband communication module can be integrated into the travel control system of the unmanned equipment, and the module is tightly coupled with the supporting ultra-wideband positioning and navigation system. The navigation system uses the time difference of arrival (TDOA) positioning technology to accurately measure the time difference of the signal sent by the unmanned equipment to reach each base station, and combines the base station location information to calculate the distance between the unmanned equipment and each base station using the triangulation positioning principle, thereby generating the real-time three-dimensional position information process of the unmanned equipment, thereby ensuring the accuracy and stability of positioning.

[0077] After deployment in a special space, the optimal path can be dynamically generated based on the path planning algorithm and the real-time location information of the unmanned equipment. These algorithms need to consider the physical limitations, safety rules and mission requirements in the special space to optimize the movement trajectory of the unmanned equipment, improve the operating efficiency of the unmanned equipment and avoid collisions.

[0078] In addition, in order to fully monitor the operating status of unmanned equipment, various data during the operation of unmanned equipment can be collected in real time, including environmental parameters such as equipment speed, acceleration, posture information, ambient temperature and humidity, and key indicators such as task completion. These data are efficiently transmitted through ultra-wideband communication modules or other auxiliary communication means, and stored on remote servers or cloud platforms, which is convenient for subsequent data analysis, fault diagnosis and performance optimization.

[0079] By precisely deploying ultra-wideband positioning base stations, integrating ultra-wideband communication modules with positioning and navigation systems, applying path planning algorithms, and real-time data collection and transmission mechanisms, unmanned equipment can be accurately positioned, intelligently navigated, and efficiently managed in special spaces, providing technical support for industrial automation, warehousing and logistics, and emergency rescue.

[0080] When obtaining the ultra-wideband positioning base station deployment plan, the signal receiving power from the unmanned device to the ultra-wideband positioning base station is determined based on the signal propagation model:

[0081]

[0082] Where P v represents the signal receiving power of the ultra-wideband positioning base station; P t Indicates the signal transmission power of unmanned equipment; G t Represents the transmitting antenna gain of the unmanned equipment; G v represents the receiving antenna gain of the ultra-wideband positioning base station; λ represents the signal wavelength; d represents the distance between the unmanned device and the ultra-wideband positioning base station;

[0083] Calculate the signal coverage area from the unmanned device to the ultra-wideband positioning base station:

[0084] A=πR2

[0085] Where A represents the signal coverage area from the unmanned device to the ultra-wideband positioning base station; R represents the effective coverage radius of the signal from the unmanned device to the ultra-wideband positioning base station;

[0086] The signal path loss from the unmanned device to the ultra-wideband positioning base station is calculated based on the logarithmic distance path loss model:

[0087]

[0088] Where d represents the distance between the unmanned device and the ultra-wideband positioning base station; L(d) represents the signal path loss at distance d; L(d0) represents the signal path loss at reference distance d0; β represents the signal path loss coefficient; log 10 () represents the logarithmic function with base 10;

[0089] The deployment plan of the ultra-wideband positioning base station is determined according to the signal receiving power, signal coverage area and signal path loss from the unmanned device to the ultra-wideband positioning base station.

[0090] For unmanned equipment operation areas with conditional access to power, the ultra-wideband positioning base station is deployed in the special space using an active connection method; for unmanned equipment operation areas without conditional access to power, a passive method with built-in batteries or solar panels is selected to deploy the ultra-wideband positioning base station in the special space.

[0091] The ultra-wideband positioning and navigation system uses the arrival time difference method to measure the time when the signal sent by the unmanned device reaches the ultra-wideband positioning base station, determines the distance from the unmanned device to the ultra-wideband positioning base station, and generates the real-time position information of the unmanned device, specifically including:

[0092] See also Figure 3-4 , R1, R2, and R3 are the positions of the ultra-wideband positioning base station, and S is the position of the unmanned device to be positioned. The ultra-wideband positioning navigation system determines the distance from the unmanned device to multiple ultra-wideband positioning base stations by measuring the time when the signal emitted by the unmanned device reaches the ultra-wideband positioning base station; a circle is drawn with the ultra-wideband positioning base station as the center and the distance from the unmanned device to the ultra-wideband positioning base station as the radius to determine the position of the signal; the corresponding distance difference is determined by comparing the time difference when the signal reaches multiple ultra-wideband positioning base stations, and a hyperbola is drawn with the ultra-wideband positioning base station as the focus and the distance difference as the major axis. The intersection of the hyperbola is the position of the unmanned device, and the real-time position information of the unmanned device is generated. The specific calculation formula is as follows:

[0093] In the three-dimensional rectangular coordinate system, the position vector of the i-th ultra-wideband positioning base station is set to 1≤i≤M, where Respectively represent the position vector of the i-th ultra-wideband positioning base station in the x, y, and z directions; M represents the total number of ultra-wideband positioning base stations; [] T Represents a matrix transpose operation;

[0094] The total position vector is obtained by combining the position vectors of all ultra-wideband positioning base stations: in, Respectively represent the position vectors of the 1st, 2nd, ..., Mth ultra-wideband positioning base stations;

[0095] Set the position vector of the unmanned equipment to in, Respectively represent the position vector of the unmanned equipment in the x, y, and z directions;

[0096] The first ultra-wideband positioning base station is used as the reference base station, and the true value of the time difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station is calculated, that is, the arrival time difference:

[0097]

[0098] In the formula, represents the true value of the time difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station, i.e., the arrival time difference; c represents the signal propagation speed; u o A vector representing the position of the unmanned equipment; represents the position vector of the jth ultra-wideband positioning base station; represents the position vector of the first ultra-wideband positioning base station, that is, the position vector of the reference base station; M represents the total number of ultra-wideband positioning base stations; represents the distance between the position vector of the unmanned device and the position vector of the jth ultra-wideband positioning base station; Represents the distance between the position vector of the unmanned device and the position vector of the first ultra-wideband positioning base station.

[0099] Convert the arrival time difference to arrival distance difference:

[0100]

[0101] In the formula, It represents the true value of the distance difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station, that is, the arrival distance difference; represents the position vector of the jth ultra-wideband positioning base station; represents the position vector of the reference base station; represents the distance between the position vector of the unmanned device and the position vector of the jth ultra-wideband positioning base station; represents the distance between the position vector of the unmanned equipment and the position vector of the first ultra-wideband positioning base station; M represents the total number of ultra-wideband positioning base stations;

[0102] The arrival distance difference is updated in combination with the noise factor of the ultra-wideband positioning base station. The calculation formula of the updated arrival distance difference is:

[0103]

[0104] In the formula, r j1 represents the updated arrival distance difference; It represents the true value of the distance difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station, that is, the arrival distance difference; δ represents the noise coefficient of the ultra-wideband positioning base station;

[0105] The arrival distance difference of all ultra-wideband positioning base stations is combined to obtain the total arrival distance difference:

[0106]

[0107] Where r represents the total arrival distance difference; r o Indicates the true value of the distance difference; n r Indicates the distance difference noise value; represents the true value of the distance difference between the signal reaching the 1st, 2nd, ..., Mth ultra-wideband positioning base station and the reference base station; δ represents the noise coefficient of the ultra-wideband positioning base station;

[0108] The distance difference noise value n r The corresponding covariance matrix is ​​Q r , the total arrival distance difference r is about the position vector u of the unmanned device o The log-likelihood function of is:

[0109]

[0110] Where, ln(p()) represents the log-likelihood function; r represents the total arrival distance difference; r o Indicates the true value of the distance difference; Q r Represents the covariance matrix corresponding to the distance difference noise value; u o represents the position vector of the unmanned equipment; ξ represents the position constant of the unmanned equipment;

[0111] The corresponding calculation formula of the Cromerow lower bound is:

[0112]

[0113] Where CRLB represents the Cromer-Rao lower bound; r o Indicates the true value of the distance difference; u oRepresents the position vector of the unmanned equipment; Q r Represents the covariance matrix corresponding to the distance difference noise value.

[0114] The distance difference true value r o and the position vector u of the unmanned equipment o , total position vector s o There is a nonlinear relationship between:

[0115] r o =f(u o ,s o )

[0116] In the formula, r o Indicates the true value of the distance difference; u o Represents the position vector of the unmanned equipment; s o represents the total position vector; f represents the nonlinear function;

[0117] Set the kth iteration result to Based on the weighted least squares criterion, the following iterative formula is obtained:

[0118]

[0119] In the formula, Indicates the result of the k+1th iteration; Indicates the result of the kth iteration; s o represents the total position vector; Indicates about and o Nonlinear function of express right The partial derivative of Q r represents the covariance matrix corresponding to the distance difference noise value; r represents the total arrival distance difference;

[0120] when The iteration stops when , where λ represents the iteration stopping threshold;

[0121] The calculation formula of the covariance matrix of the position result of the unmanned equipment is:

[0122]

[0123] In the formula, Indicates the location result of unmanned equipment; The covariance matrix representing the position results of the unmanned equipment; Indicates the result of the k+1th iteration; Indicates the result of the kth iteration; express right The partial derivative of Qr Represents the covariance matrix corresponding to the distance difference noise value; s o represents the total position vector;

[0124] At this time we get:

[0125]

[0126] In the formula, express right The partial derivative of Indicates the location result of unmanned equipment; represents the covariance matrix of the position result of the unmanned equipment; CRLB represents the Cromer-Rao lower bound; Indicates the result of the kth iteration; s o represents the total position vector; r represents the total arrival distance difference;

[0127] The corresponding real-time location information is determined according to the location result of the unmanned equipment.

[0128] Based on the real-time location information of the unmanned equipment, a path planning algorithm is used to determine the path constraints of the unmanned equipment in the special space, namely the shortest path constraint and the minimum obstacle threat constraint, and the path planning and travel control of the unmanned equipment are performed based on the path constraints.

[0129] The operation data collected in real time during the operation of the unmanned equipment is stored and transmitted in an offline manner.

[0130] The present invention will be further described below in conjunction with specific embodiments:

[0131] After deploying the ultra-wideband (UWB) positioning base station, unmanned equipment can use the UWB positioning navigation mode in the travel control system to meet the needs of video operation tasks. The following is an example of a damage detection task in a port culvert to regularly inspect whether the load-bearing structure is damaged:

[0132] The port culvert has a bridge-tunnel structure. When the tide is low, the vertical pier-type structure is exposed. The distance between the water surface and the bottom of the beam is generally between 20cm and 3m. In this working environment, the unmanned equipment is under the seawater and above it is a solid cement beam as a load-bearing structure. The cement contains steel bars, which can strengthen the dense distribution of the structure and form a structure with a certain electromagnetic shielding around the cross section of the workspace. In this workspace, it is difficult for the remote control signal to penetrate and perform delicate and precise operations. After actual measurement, it is found that it is difficult for the signal to penetrate 10 meters to operate. At the same time, the seawater below the unmanned equipment may form waves under the influence of environmental factors such as wind, causing uncontrollable behavior of the unmanned equipment. For example, when the drone detects the waves below, it can immediately start automatic obstacle avoidance, but due to insufficient space, it is very easy to cause the drone to collide or be damaged; when the unmanned boat is overturned by the waves, it may lose power or sink. In addition, it is difficult for the GPS system to achieve accurate positioning in this environment, and a large error will be generated, which may cause damage to the unmanned equipment.

[0133] Since the ultra-wideband (UWB) positioning base station is directly deployed in the working space, a corrosion-resistant and high-strength plastic shell is required. And based on the characteristics of UWB communication technology, it will not interfere with commonly used Bluetooth or WiFi communications. Its centimeter-level positioning accuracy solves the accuracy and signal problems of the GPS system, and there is no need to consider the situation where the signal cannot penetrate. Under the guidance of the UWB positioning and navigation system, unmanned equipment can achieve stable operation. In addition to manual remote control intervention operations, path planning can also be set in advance to allow unmanned equipment to operate along a predetermined route.

[0134] The damage detection work mainly includes image acquisition operations. However, due to electromagnetic shielding problems, image transmission is usually not possible. Therefore, this solution can obtain offline image data through video recording and other means, and cross-compare the actual position of the UWB system through time stamps to complete a secondary detailed exploration and form a complete record. In this process, an AI intelligent system can also be added to reduce manpower input such as manual investigation.

[0135] Through the introduction of the above embodiments, the present invention adopts the method of ultra-wideband positioning, navigation and path planning of unmanned equipment in special space, obtains the deployment plan of ultra-wideband positioning base station, and deploys multiple ultra-wideband positioning base stations in special space; adds ultra-wideband communication module in the travel control system of unmanned equipment, and the ultra-wideband communication module is correspondingly configured with ultra-wideband positioning and navigation system; the ultra-wideband positioning and navigation system adopts the arrival time difference method to measure the time from the signal sent by the unmanned equipment to the ultra-wideband positioning base station, determines the distance from the unmanned equipment to the ultra-wideband positioning base station, and generates the real-time location information of the unmanned equipment; in the special space, the path planning algorithm is used to plan the path and control the travel of the unmanned equipment according to the real-time location information; the operation data of the unmanned equipment in the operation process is collected in real time and stored and transmitted. The present invention can realize the accurate positioning, navigation and path planning of unmanned equipment in special environments where GPS signals cannot work normally and remote control signals are blocked, thereby improving the operation ability and efficiency of unmanned equipment in special environments, and reducing the risk and cost of manual exploration; and through the centimeter-level accuracy provided by the UWB positioning and navigation system, the details such as structural damage in the special space can be accurately detected and recorded, and the method of the present invention is applicable to a variety of special space environments, and has a wide range of application prospects and promotion value.

[0136] It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as those generally understood by those skilled in the art in the art to which the present invention belongs. It should also be understood that terms such as those defined in common dictionaries should be understood to have the same meaning as in the context of the prior art, and will not be interpreted with an idealized or overly formal meaning unless defined similarly here.

[0137] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for ultra-wideband positioning, navigation and path planning of unmanned equipment in a special space, characterized in that: The steps include: Obtain the deployment plan of ultra-wideband positioning base stations and deploy multiple ultra-wideband positioning base stations in a special space; An ultra-wideband communication module is added to the travel control system of the unmanned equipment, and the ultra-wideband communication module is correspondingly configured with an ultra-wideband positioning and navigation system; The ultra-wideband positioning and navigation system uses the arrival time difference method to measure the time when the signal sent by the unmanned device reaches the ultra-wideband positioning base station, determines the distance from the unmanned device to the ultra-wideband positioning base station, and generates the real-time position information of the unmanned device; In the special space, path planning and travel control of the unmanned equipment are performed based on the real-time location information based on a path planning algorithm; The operation data of the unmanned equipment during operation is collected in real time and stored and transmitted.

2. According to claim 1, the method for ultra-wideband positioning, navigation and path planning of unmanned equipment in a special space is characterized in that: When obtaining the ultra-wideband positioning base station deployment plan, the signal receiving power from the unmanned device to the ultra-wideband positioning base station is determined based on the signal propagation model: Where P v represents the signal receiving power of the ultra-wideband positioning base station; P t Indicates the signal transmission power of unmanned equipment; G t Represents the transmitting antenna gain of the unmanned equipment; G v represents the receiving antenna gain of the ultra-wideband positioning base station; λ represents the signal wavelength; d represents the distance between the unmanned device and the ultra-wideband positioning base station; Calculate the signal coverage area from the unmanned device to the ultra-wideband positioning base station: A=πR 2 Where A represents the signal coverage area from the unmanned device to the ultra-wideband positioning base station; R represents the effective coverage radius of the signal from the unmanned device to the ultra-wideband positioning base station; The signal path loss from the unmanned device to the ultra-wideband positioning base station is calculated based on the logarithmic distance path loss model: Where d represents the distance between the unmanned device and the ultra-wideband positioning base station; L(d) represents the signal path loss at distance d; L(d0) represents the signal path loss at reference distance d0; β represents the signal path loss coefficient; log 10 () represents the logarithmic function with base 10; The deployment plan of the ultra-wideband positioning base station is determined according to the signal receiving power, signal coverage area and signal path loss from the unmanned device to the ultra-wideband positioning base station.

3. The method for ultra-wideband positioning, navigation and path planning of unmanned equipment in a special space according to claim 1, characterized in that: For unmanned equipment operation areas with conditional access to power, the ultra-wideband positioning base station is deployed in the special space using an active connection method; for unmanned equipment operation areas without conditional access to power, a passive method with built-in batteries or solar panels is selected to deploy the ultra-wideband positioning base station in the special space.

4. The method for ultra-wideband positioning, navigation and path planning of unmanned equipment in a special space according to claim 1, characterized in that: The ultra-wideband positioning and navigation system uses the arrival time difference method to measure the time when the signal sent by the unmanned device reaches the ultra-wideband positioning base station, determines the distance from the unmanned device to the ultra-wideband positioning base station, and generates the real-time position information of the unmanned device, specifically including: The ultra-wideband positioning and navigation system determines the distance from the unmanned device to multiple ultra-wideband positioning base stations by measuring the time when the signal sent by the unmanned device arrives at the ultra-wideband positioning base station; draws a circle with the ultra-wideband positioning base station as the center and the distance from the unmanned device to the ultra-wideband positioning base station as the radius to determine the position of the signal; determines the corresponding distance difference by comparing the time difference when the signal arrives at multiple ultra-wideband positioning base stations, and draws a hyperbola with the ultra-wideband positioning base station as the focus and the distance difference as the major axis. The intersection of the hyperbola is the position of the unmanned device, and the real-time position information of the unmanned device is generated. The specific calculation formula is as follows: In the three-dimensional rectangular coordinate system, the position vector of the i-th ultra-wideband positioning base station is set to in, Respectively represent the position vector of the i-th ultra-wideband positioning base station in the x, y, and z directions; M represents the total number of ultra-wideband positioning base stations; [] T Represents a matrix transpose operation; The total position vector is obtained by combining the position vectors of all ultra-wideband positioning base stations: in, Respectively represent the position vectors of the 1st, 2nd, ..., Mth ultra-wideband positioning base stations; Set the position vector of the unmanned equipment to in, Respectively represent the position vector of the unmanned equipment in the x, y, and z directions; The first ultra-wideband positioning base station is used as the reference base station, and the true value of the time difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station is calculated, that is, the arrival time difference: In the formula, represents the true value of the time difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station, i.e., the arrival time difference; c represents the signal propagation speed; u o A vector representing the position of the unmanned equipment; represents the position vector of the jth ultra-wideband positioning base station; represents the position vector of the first ultra-wideband positioning base station, that is, the position vector of the reference base station; M represents the total number of ultra-wideband positioning base stations; represents the distance between the position vector of the unmanned device and the position vector of the jth ultra-wideband positioning base station; Represents the distance between the position vector of the unmanned device and the position vector of the first ultra-wideband positioning base station.

5. The method for ultra-wideband positioning, navigation and path planning of unmanned equipment in a special space according to claim 4, characterized in that: Convert the arrival time difference to arrival distance difference: In the formula, It represents the true value of the distance difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station, that is, the arrival distance difference; represents the position vector of the jth ultra-wideband positioning base station; represents the position vector of the reference base station; represents the distance between the position vector of the unmanned device and the position vector of the jth ultra-wideband positioning base station; represents the distance between the position vector of the unmanned equipment and the position vector of the first ultra-wideband positioning base station; M represents the total number of ultra-wideband positioning base stations; The arrival distance difference is updated in combination with the noise factor of the ultra-wideband positioning base station. The calculation formula of the updated arrival distance difference is: In the formula, r j1 represents the updated arrival distance difference; It represents the true value of the distance difference between the signal reaching the jth ultra-wideband positioning base station and the reference base station, that is, the arrival distance difference; δ represents the noise coefficient of the ultra-wideband positioning base station; The arrival distance difference of all ultra-wideband positioning base stations is combined to obtain the total arrival distance difference: Where r represents the total arrival distance difference; r o Indicates the true value of the distance difference; n r Indicates the distance difference noise value; represents the true value of the distance difference between the signal reaching the 1st, 2nd, ..., Mth ultra-wideband positioning base station and the reference base station; δ represents the noise coefficient of the ultra-wideband positioning base station; The distance difference noise value n r The corresponding covariance matrix is ​​Q r , the total arrival distance difference r is about the position vector u of the unmanned device o The log-likelihood function of is: Where, ln(p()) represents the log-likelihood function; r represents the total arrival distance difference; r o Indicates the true value of the distance difference; Q r Represents the covariance matrix corresponding to the distance difference noise value; u o represents the position vector of the unmanned equipment; ξ represents the position constant of the unmanned equipment; The corresponding calculation formula of the Cromerow lower bound is: Where CRLB represents the Cromer-Rao lower bound; r o Indicates the true value of the distance difference; u o Represents the position vector of the unmanned equipment; Q r Represents the covariance matrix corresponding to the distance difference noise value.

6. The method for ultra-wideband positioning, navigation and path planning of unmanned equipment in a special space according to claim 5, characterized in that: The distance difference true value r o and the position vector u of the unmanned equipment o , total position vector s o There is a nonlinear relationship between: r o =f(u o ,s o ) In the formula, r o Indicates the true value of the distance difference; u o Represents the position vector of the unmanned equipment; s o represents the total position vector; f represents the nonlinear function; Set the kth iteration result to Based on the weighted least squares criterion, the following iterative formula is obtained: In the formula, Indicates the result of the k+1th iteration; Indicates the result of the kth iteration; s o represents the total position vector; Indicates about and o Nonlinear function of express right The partial derivative of Q r represents the covariance matrix corresponding to the distance difference noise value; r represents the total arrival distance difference; when The iteration stops when , where λ represents the iteration stopping threshold; The calculation formula of the covariance matrix of the position result of the unmanned equipment is: In the formula, Indicates the location result of unmanned equipment; The covariance matrix representing the position results of the unmanned equipment; Indicates the result of the k+1th iteration; Indicates the result of the kth iteration; express right The partial derivative of Q r Represents the covariance matrix corresponding to the distance difference noise value; s o represents the total position vector; At this time we get: In the formula, express right The partial derivative of Indicates the location result of unmanned equipment; represents the covariance matrix of the position result of the unmanned equipment; CRLB represents the Cromer-Rao lower bound; Indicates the result of the kth iteration; s o represents the total position vector; r represents the total arrival distance difference; The corresponding real-time location information is determined according to the location result of the unmanned equipment.

7. The method for ultra-wideband positioning, navigation and path planning of unmanned equipment in a special space according to claim 1, characterized in that: Based on the real-time location information of the unmanned equipment, a path planning algorithm is used to determine the path constraints of the unmanned equipment in the special space, namely the shortest path constraint and the minimum obstacle threat constraint, and the path planning and travel control of the unmanned equipment are performed based on the path constraints.

8. The method for ultra-wideband positioning, navigation and path planning of unmanned equipment in a special space according to claim 1, characterized in that: The operation data collected in real time during the operation of the unmanned equipment is stored and transmitted in an offline manner.

Images