A warehouse scene mobile robot and sensor network node cooperative positioning method

Abstract

The application discloses a warehouse scene mobile robot and sensor network node cooperative positioning method, which comprises the following steps: S1, arranging sensor network nodes, starting the robot, collecting robot motion information, robot and node distance measurement information and node distance measurement information, and filtering the relevant distance measurement information; S2, establishing a distance measurement correction model, and using the least square method to fit to obtain distance measurement correction parameters and initial values of position coordinates of unknown nodes in the sensor network; S3, establishing a state equation and a measurement equation of the robot-sensor network system; and S4, based on the derived system state equation and measurement equation, the overall state of the system is estimated based on the improved extended Kalman filter algorithm, and the simultaneous positioning of the mobile robot and the sensor network node in the warehouse scene is realized. The method can improve the distance measurement accuracy in a complex environment and the positioning accuracy of the robot and the sensor network node.

Description

Technical Field

[0001] This invention relates to the field of sensor networks and mobile robot positioning technology, and in particular to a collaborative positioning method for mobile robots and sensor network nodes in a warehouse setting. Background Technology

[0002] Mobile robots are widely used in warehousing for tasks such as picking and handling goods. Wireless sensor networks (SNNs), on the other hand, are used for real-time monitoring of sensitive environmental parameters in warehouses, such as temperature, humidity, and gas concentration. Spatial localization is fundamental to a robot's autonomous navigation, and the data from sensor network nodes typically requires location identification. Therefore, research into localization methods for both is crucial.

[0003] Currently, the main robot localization technologies include GPS localization, laser SLAM (Simultaneous Localization and Mapping), visual SLAM, wheeled odometry, and various sensor information fusion localization methods. In large warehouse scenarios, due to the wide range of robot movement, existing methods are prone to cumulative errors; moreover, due to similar warehouse environment features and severe occlusion, mismatches are likely to occur, affecting the localization effect.

[0004] The essence of wireless sensor network (WSN) localization methods is that the node to be located determines its own position by relative ranging from anchor nodes at known locations. Therefore, its positioning accuracy largely depends on the accuracy of the ranging between nodes. However, considering the complexity of warehouse environments, the ranging values ​​between sensor network nodes are often inaccurate, and it is difficult to determine the position of all nodes. For example, when using Ultra Wide Band (UWB) technology for node ranging, achieving high ranging accuracy typically requires nodes to be deployed at a height exceeding 1.8m and to be within line-of-sight between nodes, which limits its application.

[0005] In summary, the shortcomings and challenges of existing technologies mainly include:

[0006] 1) In large-scale warehouse scenarios with similar environmental characteristics, the cumulative positioning error of mobile robots is relatively large when there is no external global observation.

[0007] 2) In complex warehouse environments, the distance measurement values ​​between sensor network nodes usually have large errors, resulting in poor positioning performance.

[0008] 3) Without any prior information, it is difficult to achieve simultaneous high-precision positioning of both the robot and the sensor network nodes. Summary of the Invention

[0009] To address the aforementioned problems in the prior art, the present invention aims to provide a collaborative positioning method for mobile robots and sensor network nodes in a warehousing scenario, so as to achieve simultaneous and accurate positioning of the robot and sensor network nodes under conditions of no prior information and large system measurement errors.

[0010] To solve the above-mentioned technical problems, the specific technical solution of the present invention is as follows: A method for collaborative localization of mobile robots and sensor network nodes in a warehousing scenario, the specific process of which is as follows:

[0011] S1: Data Acquisition and Preprocessing. Deploy sensor network nodes, start the robot, and collect information from the robot's wheel odometer, as well as distance measurement information between the robot and nodes, and between nodes. Filter the relevant distance measurement information.

[0012] S2: Establish a ranging correction model and use the least squares method to fit the ranging correction parameters and the initial values ​​of the position coordinates of unknown nodes in the sensor network.

[0013] S3: Establish the state equations and measurement equations for the robot-sensor network system.

[0014] S4: Based on the derived system state equation and measurement equation, the system state is estimated as a whole using the improved extended Kalman filter algorithm, enabling simultaneous localization of mobile robots and sensor network nodes in a warehousing scenario.

[0015] The specific process of step S1 is as follows:

[0016] Step S11: Start the mobile robot. The robot moves at a constant speed for a certain distance and collects the robot's odometer information, the distance measurement information between the robot and the nodes in the sensor network, and the distance measurement information between each node in the sensor network.

[0017] Step S12: Perform amplitude limiting filtering on the ranging information to filter out outliers.

[0018] Step S13: Use the Kalman filter algorithm to further smooth the ranging information to obtain a relatively stable ranging value.

[0019] The "filtering outliers" involves using amplitude limiting filtering to remove abrupt changes in distance measurement data between adjacent sampling points. This is achieved by detecting the difference between two consecutive measurements; if the difference exceeds a preset threshold, the measurement is considered invalid. Since the measurement frequency of each node is known, the approximate error between adjacent distance measurement data can be calculated based on the robot's actual operating speed. This allows for the determination of the amplitude limiting filtering threshold, thus filtering out outliers.

[0020]

[0021] Where d kLet d be the k-th measurement value for distance measurement between a node and a mobile robot. k-1 Let f be the (k-1)th measurement value, f be the measurement frequency of the node, v be the speed of the mobile robot, and ε be the measurement error of the node when the robot is stationary.

[0022] The specific process of step S2 is as follows:

[0023] Step S21: Establish a distance measurement correction model.

[0024] Using an m-th degree polynomial as the ranging correction model:

[0025]

[0026] in Let d be the calibrated ranging value of the i-th node in the sensor network. i To correct the previous distance measurement value,

[0027] α0, α1, ..., α m For calibration parameters.

[0028] Step S22: Calculate the Euclidean distance between the robot and the nodes, and between the nodes.

[0029] Let the pose of the mobile robot be X at time k. r (k)=(x r (k),y r (k),z r (k),θ r (k)) T The positions of the n sensor nodes are X s (k)=(x1(k),y1(k),z1(k),x2(k),y2(k),z2(k),...,x n (k),y n (k),z n (k)) T The state of the mobile robot-sensor network system is X(k) = (X... r (k),X s (k)).

[0030] At time k, the Euclidean distance between sensor network node i and the mobile robot is... for:

[0031]

[0032] Euclidean distance between nodes for:

[0033]

[0034] Then the Euclidean distance between the mobile robot and the n sensor network nodes at time k, and the Euclidean distance vector between the sensor network nodes, can be expressed as:

[0035]

[0036] In the initial stage, the cumulative error of the dead reckoning results based on odometry over a short distance is very small, and this result can be used as its actual coordinate value.

[0037] At time k, the actual measurements of the robot's interactions with other nodes in the sensor network, as well as the interactions between nodes in the sensor network, during its movement can be expressed as:

[0038]

[0039] in, This represents the distance measurement between the mobile robot and sensor network node i. This represents the distance measurement between sensor network nodes i and j.

[0040] Step S23: Design an objective function that makes the corrected distance measurement value and the Euclidean distance between nodes as close as possible, and use the least squares method to fit the parameters.

[0041] Design the following objective function:

[0042]

[0043] in Let g(k) be the m-th power of Hadamard.

[0044] Step S24: Solve the nonlinear least squares problem to obtain the correction parameters and the initial values ​​of the sensor network node positions.

[0045] Step S25: Perform real-time ranging correction using the ranging correction model.

[0046] The specific process of step S3 is as follows:

[0047] S31: The state equation of the robot-sensor network system is:

[0048] X p (k)=f(X(k-1),u k )+ω k

[0049] Where X p (k) is the predicted value of the system state vector at time k, u k Let X(k-1) be the control vector, X(k-1) be the optimal estimate of the system's state vector at the previous time step, and ω be the ω value. kThis represents process noise. Based on the motion model of the wheeled odometer of the mobile robot, the corresponding control vector can be obtained:

[0050] u k =[Δd k Δθ k ]

[0051] Where Δd k Let Δθ be the distance the robot moves from time k-1 to time k. k Let be the angle of rotation of the robot from time k-1 to time k.

[0052] S32: To ensure the matching and alignment of ranging data and odometer data, the state equation is improved as follows:

[0053]

[0054] Where α and β are weighting coefficients, x odom (k), y odom (k) represents the actual x and y coordinates of the robot obtained from the odometer information.

[0055] S33: The system's measurement equation is:

[0056] Z(k)=h(X p (k))+v k

[0057] Where h(X) p Z(k) represents the predicted measurements between the robot and each node in the sensor network, as well as between nodes in the sensor network, at time k. Z(k) represents the corrected measurements between the robot and each node in the sensor network, as well as between nodes, at time k.

[0058] Z(k)=α m ·g(k) m +α m-1 ·g(k) (m-1) +…+α1·g(k)+α0I.

[0059] The specific process of step S4 is as follows:

[0060] Step S41: Calculate the state prediction vector of the robot-sensor network system at time k based on the improved state equation and the initial system state values ​​obtained during the ranging correction phase.

[0061] Step S42: Predict the prior estimate covariance matrix of the robot-sensor network system state vector at time k.

[0062] Predicting the prior estimate covariance matrix P of the state vector of a robot-sensor network system kp for:

[0063]

[0064] Where P k-1 Let Q be the covariance matrix of the system state vector at time k-1. k Let F be the covariance matrix of the process noise. Because the system is nonlinear, it needs to be linearized. k for Jacobian matrix for the state vector.

[0065] Step S43: Calculate the Kalman gain.

[0066] Step S44: Correct the predicted state vector to obtain a more accurate robot pose and sensor network node coordinates.

[0067] Step S45: Correct the covariance matrix.

[0068] The advantages of the method of the present invention are as follows:

[0069] 1) Improved ranging accuracy in complex environments. In warehouse scenarios, the complex environment leads to significant ranging errors between nodes. This invention corrects the system's ranging values ​​based on the initial motion information of the mobile robot, thereby improving ranging accuracy.

[0070] 2) Simultaneous high-precision positioning of the robot and sensor network nodes. Based on the ranging information calibrated by the system, this invention utilizes an improved Kalman filtering method to achieve simultaneous positioning of the robot and sensor network nodes even when the node positions are unknown, while improving their respective positioning accuracy.

[0071] To make the above and other objects, features and advantages of the present invention more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0072] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0073] Figure 1 The diagram shows the data acquisition and preprocessing process.

[0074] Figure 2 The diagram shown is a schematic of the amplitude limiting filter principle.

[0075] Figure 3 The diagram shown is a flowchart of the distance measurement correction process.

[0076] Figure 4 The diagram shows a flowchart of simultaneous localization of a robot and sensor network nodes based on an improved extended Kalman filter algorithm.

[0077] Figure 5 The diagram shown is an overall flowchart of a collaborative positioning method for mobile robots and sensor network nodes in a warehousing scenario according to the present invention. Detailed Implementation

[0078] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0079] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, apparatus, product, or device that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or devices.

[0080] A method for collaborative localization of mobile robots and sensor network nodes in a warehousing scenario, the method comprising the following steps (e.g.) Figure 5 As shown):

[0081] S1: Data Acquisition and Preprocessing. The system used in this embodiment includes a mobile robot and several sensor network nodes. Each sensor network node is equipped with a ranging module. By configuring a sensor network node on the robot, real-time communication and ranging with other nodes in the sensor network can be achieved. The sensor network nodes are deployed, the robot is started, and robot motion information, ranging information between the robot and nodes, and between nodes are collected. The relevant ranging information is then filtered. The specific process is as follows: Figure 1 As shown.

[0082] S11: Start the mobile robot. The robot moves at a constant speed for a certain distance and collects the robot's odometer information, the distance measurement information between the robot and the nodes in the sensor network, and the distance measurement information between each node in the sensor network.

[0083] S12: Amplitude limiting filtering is applied to the ranging information to remove abnormal data with severe jumps between adjacent sampling points. The difference between two consecutive measurements is detected; if it exceeds a preset threshold, the measurement is considered invalid. Since the measurement frequency of the nodes is known, the approximate error between adjacent ranging data can be calculated based on the robot's actual running speed. This allows the threshold for amplitude limiting filtering to be obtained, thus filtering out outliers.

[0084] Assume d k Let d be the k-th measurement value for distance measurement between a node and a mobile robot. k-1 For the (k-1)th measurement value, based on the principle that the absolute value of the difference between any two sides of a triangle is less than the value of the third side, the amplitude limiting filter threshold of the node can be obtained:

[0085]

[0086] Where f is the measurement frequency of the node, v is the speed of the mobile robot, and ε is the measurement error of the node when the robot is stationary. Figure 2 As shown.

[0087] S13: The Kalman filter algorithm is used to smooth the data and obtain a relatively stable ranging value.

[0088] The Kalman filter algorithm uses the distance measurements between nodes as a one-dimensional state vector to obtain the following state equation:

[0089]

[0090] Where X i,j (k-1) represents the optimal estimated distance between node i and node j at time k-1. For the predicted distance between node i and node j at time k, ω k Let A be the process noise at time k, and A be the state transition matrix. The measurement equation can be defined as:

[0091]

[0092] Where Z(k) is the actual distance measured between nodes i and j at time k, H is the measurement matrix, and V k Let k be the measurement noise at time k. After obtaining the state equation and measurement equation, the data can be filtered using the Kalman filter method.

[0093] S2: Establish a ranging correction model, and use the least squares method to fit and obtain the ranging correction parameters and the initial values ​​of the position coordinates of unknown nodes in the sensor network. For example... Figure 3 As shown.

[0094] S21: Considering the universality of the fitted model, an m-th degree polynomial is used as the ranging correction model:

[0095]

[0096] in Let d be the calibrated ranging value of the i-th node in the sensor network. i The distance measurement values ​​before correction are α0, α1, ..., α m For calibration parameters.

[0097] S22: Calculate the Euclidean distance between the robot and the nodes, as well as the distance between the nodes.

[0098] Let the pose of the mobile robot be X at time k. r (k)=(x r (k),y r (k),z r (k),θ r (k)) T The positions of the n sensor nodes are X s (k)=(x1(k),y1(k),z1(k),x2(k),y2(k),z2(k),...,x n (k),y n (k),z n (k)) T The state of the mobile robot-sensor network system is X(k) = (X... r (k),X s (k)). At time k, the Euclidean distance between sensor network node i and the mobile robot is (k). for:

[0099]

[0100] Euclidean distance between nodes for:

[0101]

[0102] Then the Euclidean distance between the mobile robot and the n sensor network nodes at time k, and the Euclidean distance vector between the sensor network nodes and the sensor network nodes, can be expressed as:

[0103]

[0104] In the initial stage, the cumulative error of the dead reckoning results based on odometry over a short distance is very small, and this result can be used as its actual coordinate value.

[0105] At time k, the actual measurements of the robot's interactions with other nodes in the sensor network, as well as the interactions between nodes in the sensor network, during its movement can be expressed as:

[0106]

[0107] in, This represents the distance measurement between the mobile robot and sensor network node i. This represents the distance measurement between sensor network nodes i and j.

[0108] S23: To ensure that the corrected distance measurements and the calculated Euclidean distances between nodes are as close as possible, the parameters are fitted using the least squares method, and the following objective function is designed:

[0109]

[0110] Where g(k) οm Let g(k) be the m-th power of Hadamard.

[0111] S24: Solving this nonlinear least squares problem using the Levenberg-Marquardt method yields the correction parameters α0,...,α m And the location X of the sensor network node can be initially solved. s (k).

[0112] S25: Real-time ranging correction is performed using a ranging correction model.

[0113] Substitute the correction parameters obtained in step S24 into the model, and during the robot's operation, substitute the distance measurement information between the robot and nodes, and between nodes, collected in real time into the model for real-time correction.

[0114] S3: Establish the state equations and measurement equations for the robot-sensor network system.

[0115] S31: The state equation of the robot-sensor network system is:

[0116] X p (k)=f(X(k-1),u k )+ω k

[0117] Where X p (k) is the predicted value of the system state vector at time k, u k Let X(k-1) be the control vector, X(k-1) be the optimal estimate of the system's state vector at the previous time step, and ω be the ω value. k This represents process noise. Based on the motion model of the wheeled odometer of the mobile robot, the corresponding control vector can be obtained:

[0118] u k =[Δd k Δθ k ]

[0119] Where Δdk Let Δθ be the distance the robot moves from time k-1 to time k. k Let be the angle of rotation of the robot from time k-1 to time k.

[0120] S32: Considering the actual difference between the ranging frequency of sensor network nodes and the odometry data release frequency, and the potential interruption or loss of ranging information, to ensure the matching and alignment of ranging data and odometry data, the improved state equation is as follows:

[0121]

[0122] Where α and β are weighting coefficients, x odom (k), y odom (k) represents the actual x and y coordinates of the robot obtained from the odometer information.

[0123] S33: The system's measurement equation is:

[0124] Z(k)=h(X p (k))+v k

[0125] Where h(X) p Z(k) represents the predicted measurements between the robot and each node in the sensor network, as well as between nodes in the sensor network, at time k. Z(k) represents the corrected measurements between the robot and each node in the sensor network, as well as between nodes, at time k.

[0126]

[0127] S4: Based on the derived system state equation and measurement equation, an improved extended Kalman filter algorithm is used to estimate the overall system state, enabling simultaneous localization of mobile robots and sensor network nodes in a warehousing scenario. Figure 4 As shown.

[0128] S41: Based on the improved state equation and the initial system state values ​​obtained during the ranging correction phase, the state prediction vector X of the robot-sensor network system at time k can be calculated. p (k).

[0129] S42: Predicting the prior estimate of the covariance matrix P of the state vector of a robot-sensor network system. kp for:

[0130]

[0131] Where P k-1 Let Q be the covariance matrix of the system state vector at time k-1. kLet F be the covariance matrix of the process noise. Because the system is nonlinear, it needs to be linearized. k for Jacobian matrix for the state vector.

[0132] S43: Calculate the Kalman gain:

[0133]

[0134] Where H k h(X) at time k p (k) For the Jacobian matrix of the state vector, R k This is the covariance matrix for measuring noise.

[0135] S44: Obtain the updated robot state and sensor network node states. Use Kalman gain to predict state vector corrections, obtaining more accurate robot pose and sensor network node coordinates.

[0136] X(k)=X p (k)+K k (Z(k)-h(X p (k)))

[0137] S45: Corrected covariance matrix P k :

[0138] P k =(IK k H k )P kp .

[0139] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM). The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0140] It should also be understood that, in the embodiments of the present invention, the term "and / or" is merely a description of the relationship between associated objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Furthermore, in the present invention, the character " / " generally indicates that the preceding and following associated objects have an "or" relationship.

[0141] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this invention can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of each example have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0142] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0143] In the embodiments provided by this invention, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative. For instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. In addition, the mutual coupling or direct coupling or communication connection shown or discussed may be indirect coupling or communication connection through some interfaces, devices, or units, or may be electrical, mechanical, or other forms of connection.

[0144] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of the embodiments of the present invention, depending on actual needs.

[0145] Specific embodiments have been used to illustrate the principles and implementation methods of this invention. The descriptions of the embodiments above are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A method for collaborative localization of mobile robots and sensor network nodes in a warehousing scenario, characterized in that: The specific process of the method is as follows: S1: data acquisition and pretreatment: arranging sensor network nodes, starting the robot, collecting robot motion information, robot and node, and node distance measurement information, and filtering the relevant distance measurement information; S2: establishing a distance measurement correction model, and using the least square method to fit to obtain distance measurement correction parameters and initial values of position coordinates of unknown nodes in the sensor network; S3: establishing a state equation and a measurement equation of the robot-sensor network system; the specific process is as follows: S31: the state equation of the robot-sensor network system is: ； where is k the predicted value of the system state vector at the current time instant, is the control vector, is the optimal estimate of the system state vector at the previous time instant, is the process noise; according to the motion model of the wheeled odometry of the mobile robot, the corresponding control vector is obtained: ； wherein is a robot k - the distance moved by the robot from k time to is k - the angle of rotation of the robot from k time to S32: in order to ensure the matching and alignment of the distance measurement data and the robot odometer data, the improved state equation is: ； wherein , are weighting coefficients, , are respectively the actual coordinates of the robot x , y coordinates; S33: the measurement equation of the system is: ； wherein, are predicted measurements of the robot and the nodes in the sensor network and between the nodes in the sensor network; are k are corrected measurements of the robot and the nodes in the sensor network and between the nodes in the sensor network at the time instant ； S4: based on the improved extended Kalman filter algorithm, the system state is estimated as a whole according to the derived system state equation and measurement equation, and the simultaneous positioning of the mobile robot and the sensor network nodes in the warehouse scene is realized.

2. The method of claim 1, wherein, The specific process of the step S1 is as follows: Step S11: starting the mobile robot, the robot moves at a constant speed for a distance, and the odometer information of the robot, the distance measurement information between the robot and the nodes in the sensor network, and the distance measurement information between the nodes in the sensor network are collected; Step S12: the distance measurement information is subjected to amplitude limiting filtering to filter out abnormal values; Step S13: the distance measurement information is further smoothed by using the Kalman filter algorithm to obtain relatively stable distance measurement values.

3. The method of claim 1, wherein: The specific process of the step S2 is as follows: Step S21: establishing a distance measurement correction model; Step S22: calculating the Euclidean distance values between the robot and the nodes, and between the nodes; Step S23: designing a target function so that the corrected distance measurement values and the Euclidean distance values between the nodes are as close as possible, and using the least square method to fit the parameters; Step S24: solving the nonlinear least square problem to obtain the correction parameters and the initial values of the positions of the sensor network nodes; Step S25: using the distance measurement correction model for real-time distance measurement correction.

4. The method of claim 3, wherein: The specific process of the step S21 is as follows: With m polynomial as the ranging correction model: ； wherein is the corrected ranging value of the i-th node in the sensor network, i is the corrected ranging value of the i-th node in the sensor network, is the corrected ranging value of the i-th node in the sensor network, is the corrected ranging value of the i-th node in the sensor network, 5. The method of claim 3, wherein: The specific process of the step S22 is as follows: The pose of the mobile robot at time k is , n the position of the sensor nodes is ; and the state of the mobile robot-sensor network system is ; At k The Euclidean distance between the sensor network node i and the mobile robot is : ； Euclidean distance between nodes is: ； Then k The mobile robot and n The Euclidean distance between a sensor network node and a mobile robot at time t, and the Euclidean distance vector between sensor network nodes can be expressed as: ； In the initial stage, the mobile robot has a small cumulative error in the dead reckoning result based on the odometer in a short distance, and the result is taken as the actual coordinate value; At The actual measurements between the robot and other nodes in the sensor network, and between nodes in the sensor network, during movement are represented as: ； wherein, represents a distance measurement of the mobile robot to the sensor network node, i represents a distance measurement of the sensor network node to the mobile robot, i and j represents a distance measurement of the sensor network node to the mobile robot.​ 6. The method of claim 3, wherein: The step S23 is designed as follows: ； wherein is a vector of m Hadamard powers.

7. The method of claim 1, wherein: The specific process of the step S4 is as follows: Step S41: According to the improved state equation, and based on the system state initial value obtained in the ranging correction stage, the state prediction vector of the robot-sensor network system at the time moment is calculated k Step S41: According to the improved state equation, and based on the system state initial value obtained in the ranging correction stage, the state prediction vector of the robot-sensor network system at the time moment is calculated Step S42: predicting the prior estimation value covariance matrix of the state vector of the robot-sensor network system at time k; Step S43: calculating the Kalman gain; Step S44: correcting the predicted state vector to obtain more accurate robot pose and sensor network node coordinates; Step S45: correcting the covariance matrix.

Images