Mobile robot and sensor network node cooperative positioning method in storage scene

Abstract

The invention discloses a mobile robot and sensor network node cooperative positioning method in a storage scene, and the method comprises the steps: S1, arranging sensor network nodes, starting a robot, collecting the motion information of the robot, the distance measurement information between the robot and the nodes, and the distance measurement information between the nodes, and carrying out the filtering processing of the related distance measurement information; s2, establishing a ranging correction model, and fitting by using a least square method to obtain ranging correction parameters and position coordinate initial values of unknown nodes in the sensor network; s3, establishing a state equation and a measurement equation of the robot-sensor network system; and S4, according to the deduced system state equation and measurement equation, overall estimation is carried out on the system state based on an improved extended Kalman filtering algorithm, and simultaneous positioning of the mobile robot and the sensor network node in the storage scene is realized. According to the method, the distance measurement precision in a complex environment and the positioning precision of the robot and the sensor network node can be improved.

Description

Technical Field

[0001] The present invention relates to the technical fields of sensor networks and mobile robot positioning, and in particular to a collaborative positioning method for mobile robots and sensor network nodes in a warehousing scenario. Background Art

[0002] Mobile robots are widely used in operations such as picking and transporting goods in warehouses. And wireless sensor networks (referred to as sensor networks for short) are used for real-time monitoring of sensitive environmental parameters such as temperature, humidity, and gas concentration in warehouses. The spatial positioning of robots is the basis for their autonomous navigation, and the data information of sensor network nodes usually requires position identification. Therefore, the research on the positioning methods of the two is crucial.

[0003] The current main robot positioning technologies mainly include GPS positioning, laser SLAM (Simultaneous Localization and Mapping), visual SLAM, wheel odometers, and various perception information fusion positioning means. In large-scale warehousing scenarios, due to the wide movement range of robots, the existing methods are prone to cumulative errors; and due to reasons such as similar warehouse environment characteristics and serious occlusion, mis-matching is prone to occur, which affects the positioning effect.

[0004] The essence of the wireless sensor network positioning method is that the node to be positioned determines its own position by relatively measuring the distance to the anchor nodes with known positions. Therefore, its positioning accuracy largely depends on the ranging accuracy between nodes. However, considering the complexity of the warehouse environment, the ranging values between sensor network nodes are often inaccurate, and it is difficult to measure the positions of all nodes. For example, when using Ultra Wide Band (UWB) technology for ranging between nodes, to obtain high ranging accuracy, it is usually required that the nodes be deployed at a height of more than 1.8 m and the nodes be in line-of-sight conditions, which limits its application.

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

[0006] 1) In a large-scale warehousing scenario with similar environmental characteristics and without external global observations, the cumulative positioning error of mobile robots is relatively large.

[0007] 2) In a complex warehouse environment, the ranging values between sensor network nodes usually have large errors, resulting in poor positioning effects.

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

[0009] In view of the above problems in the prior art, the purpose of the present invention is to provide a collaborative positioning method for mobile robots and sensor network nodes in a warehousing scenario, so as to achieve the simultaneous precise positioning of robots and sensor network nodes under the conditions of no prior information and large system measurement errors.

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

[0011] S1: Data acquisition and preprocessing. Arrange sensor network nodes, start the robot, collect the information of the robot's wheel odometer, the ranging information between the robot and the nodes, and the ranging information between the nodes, and perform filtering processing on the relevant ranging information.

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

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

[0014] S4: According to the derived system state equation and measurement equation, based on the improved extended Kalman filter algorithm, perform an overall estimation of the system state to achieve the simultaneous positioning of the mobile robot and the sensor network nodes in the warehousing scenario.

[0015] Among them, the specific process of the step S1 is as follows:

[0016] Step S11: Start the mobile robot, and the robot moves at a constant speed for a certain distance, and collect the odometer information of the robot, the ranging information between the robot and the nodes in the sensor network, and the ranging information between the nodes in the sensor network.

[0017] Step S12: Perform limit 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] Among them, the "filtering out outliers" is to filter out the abnormal data with serious jumps between adjacent sampling points in the ranging information by using limit filtering; by detecting the difference between two consecutive measurement values, if it exceeds the preset threshold, the measurement value is regarded as invalid. Since the measurement frequency of the nodes is known, the approximate error between adjacent ranging data can be deduced in combination with the actual running speed of the robot, and thus the threshold of limit filtering can be obtained, and then the outliers can be filtered out:

[0020]

[0021] Where d kis the k-th measurement value of the distance measurement between a certain node and the mobile robot, d k-1 is the (k - 1)-th measurement value, 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.

[0022] Among them, the specific process of step S2 is as follows:

[0023] Step S21: Establish a ranging correction model.

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

[0025]

[0026] Where is the corrected ranging value of the i-th node in the sensor network, d i is the ranging value before correction,

[0027] α 0 , α 1 , …, α m are correction parameters.

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

[0029] Suppose that at time k, the pose of the mobile robot is X r (k) = (x r (k), y r (k), z r (k), θ r (k)) T , and the positions of n sensor nodes are X s (k) = (x 1 (k), y 1 (k), z 1 (k), x 2 (k), y 2 (k), z 2 (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 the i-th node of the sensor network and the mobile robot is:

[0031]

[0032] The Euclidean distance between nodes is as follows:

[0033]

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

[0035]

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

[0037] At time k, the actual measurement values between the robot and other nodes in the sensor network, as well as between nodes in the sensor network, during the movement of the robot can be expressed as:

[0038]

[0039] Among them, represents the distance measurement value between the mobile robot and sensor network node i, and then represents the distance measurement value between sensor network nodes i and j.

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

[0041] Design the following objective function:

[0042]

[0043] Among them is the m-th Hadamard power of the vector g(k).

[0044] Step S24: Solve the non-linear 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] Among them, 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 is the control vector, X(k - 1) is the optimal estimated value of the system state vector at the previous time, ω k is the process noise. According to the motion model of the mobile robot's wheel odometer, the corresponding control vector can be obtained:

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

[0051] Where Δd k is the distance the robot moves from time k - 1 to time k, and Δθ k is the angle the robot rotates from time k - 1 to time k.

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

[0053]

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

[0055] S33: The measurement equation of the system is:

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

[0057] Where, h(X p (k)) is the predicted measurement value between the robot and each node in the sensor network and between the nodes in the sensor network. Z(k) is the corrected measurement value between the robot and the nodes in the sensor network and between the nodes at time k:

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

[0059] Where, the specific process of step S4 is as follows:

[0060] Step S41: According to the improved state equation and based on the initial system state value obtained in the ranging correction stage, calculate the state prediction vector of the robot - sensor network system at time k. ​

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

[0062] Predict the covariance matrix P of the prior estimate of the robot-sensor network system state vector kp as:

[0063]

[0064] where P k-1 is the covariance matrix of the system state vector at time k-1, and Q k is the covariance matrix of the process noise. Since the system is non-linear, it needs to be linearized. F k is the Jacobian matrix of 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 the coordinates of the sensor network nodes.

[0067] Step S45: Perform the correction of the covariance matrix.

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

[0069] 1) Improvement in ranging accuracy in complex environments. In a warehousing scenario, due to the complex environment, the ranging error between nodes is relatively large. The present invention corrects the system ranging value based on the motion information of the mobile robot in the initial stage, improving the ranging accuracy.

[0070] 2) Simultaneous high-precision positioning of the robot and the sensor network nodes. Based on the system-corrected ranging information, the present invention uses an improved Kalman filtering method to achieve the simultaneous positioning of the robot and the sensor network nodes when the node positions are unknown, and improve their respective positioning accuracies.

[0071] To make the above and other objects, features, and advantages of the present invention more obvious and understandable, the following specifically provides preferred embodiments and, in conjunction with the accompanying drawings, makes a detailed description as follows. Brief Description of the Drawings

[0072] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0073] Figure 1The figure shows the flowchart of data acquisition and preprocessing.

[0074] Figure 2 The figure shows the schematic diagram of amplitude-limiting filtering.

[0075] Figure 3 The figure shows the flowchart of ranging correction.

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

[0077] Figure 5 The figure shows the overall flowchart of a collaborative localization method for a mobile robot and sensor network nodes in a warehouse scenario according to the present invention. Detailed implementation manners

[0078] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0079] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned accompanying drawings are used to distinguish similar objects, and do not necessarily need to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, device, product, or equipment including a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products, or equipment.

[0080] A collaborative localization method for a mobile robot and sensor network nodes in a warehouse scenario, the method includes the following steps (as Figure 5 shown):

[0081] S1: Data acquisition and preprocessing. The system applied in this embodiment includes a mobile robot and several sensor network nodes. The sensor network nodes are configured with ranging modules. By configuring a sensor network node on the robot, real-time communication and ranging with other sensor network nodes can be achieved. Arrange the sensor network nodes, start the robot, collect the motion information of the robot, the ranging information between the robot and the nodes and between the nodes, and perform filtering processing on the relevant ranging information. The specific process is as Figure 1 shown.

[0082] S11: Start the mobile robot. The robot moves at a constant speed for a certain distance, and collect the odometer information of the robot, the ranging information between the robot and the nodes in the sensor network, and the ranging information between the nodes in the sensor network.

[0083] S12: Use the limit filtering to filter out the abnormal data with severe jumps between adjacent sampling points from the ranging information. By detecting the difference between two consecutive measurement values, if it exceeds the preset threshold, the measurement value is regarded as invalid. Since the measurement frequency of the node is known, the approximate error between adjacent ranging data can be deduced in combination with the actual running speed of the robot, and thus the threshold of the limit filtering can be obtained, and then the outliers can be filtered out:

[0084] Assume d k is the k-th measurement value of the ranging between a certain node and the mobile robot, and d k-1 is the (k - 1)-th measurement value. According to the principle that the absolute value of the difference between two sides of a triangle is less than the third side, the limit filtering 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. As Figure 2 shown.

[0087] S13: Use the Kalman filter algorithm to perform data smoothing to obtain relatively stable ranging values.

[0088] The Kalman filter algorithm takes the ranging value between nodes as a one-dimensional state vector to obtain the following state equation:

[0089]

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

[0091]

[0092] where Z(k) is the actual ranging value between node i and node j at time k, H is the measurement matrix, and V k is the measurement noise at time k. After obtaining the state equation and the measurement equation, the data can be filtered according to the Kalman filter method.

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

[0094] S21: Considering the generality of the fitting model, use an m-degree polynomial as the ranging correction model:

[0095]

[0096] where is the ranging value of the i-th node in the sensor network after correction, d i is the ranging value before correction, and α 0 , α 1 , …, α m are the correction parameters.

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

[0098] Suppose that at time k, the pose of the mobile robot is X r (k) = (x r (k), y r (k), z r (k), θ r (k)) T , and the positions of n sensor nodes are X s (k) = (x 1 (k), y 1 (k), z 1 (k), x 2 (k), y 2 (k), z 2 (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 the i-th node in the sensor network and the mobile robot is:

[0099]

[0100] The Euclidean distance between the nodes is:

[0101]

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

[0103]

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

[0105] At time k, the actual measurement values between the robot and other nodes in the sensor network, as well as between the nodes in the sensor network during the movement of the robot, can be expressed as:

[0106]

[0107] Among them, represents the distance measurement value between the mobile robot and sensor network node i, then represents the distance measurement value between sensor network nodes i and j.

[0108] S23: To make the corrected distance measurement value as close as possible to the calculated Euclidean distance value between nodes, the least squares method is used to fit the parameters, and the following objective function is designed:

[0109]

[0110] where g(k) οm is the m-th Hadamard power of the vector g(k).

[0111] S24: Using the Levenberg-Marquardt method to solve this non-linear least squares problem, the correction parameters α 0 ,..., α m can be obtained, and the positions X s (k) of the sensor network nodes can be preliminarily solved.

[0112] S25: Use the ranging correction model for real-time ranging correction.

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

[0114] S3: Establish the state equation and measurement equation of 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), uk ) + ω k

[0117] where X p (k) is the predicted value of the system state vector at time k, u k is the control vector, X(k - 1) is the optimal estimated value of the system state vector at the previous time, ω k is the process noise. According to the motion model of the mobile robot's wheel odometer, the corresponding control vector can be obtained:

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

[0119] where Δd k is the distance the robot moves from time k - 1 to time k, and Δθ k is the angle the robot rotates from time k - 1 to time k.

[0120] S32: Considering that there are differences in the ranging frequency of the sensor network nodes and the publishing frequency of the robot odometer data in practice, and the ranging information may be interrupted and missing, to ensure the matching and alignment of the ranging data and the odometer data, the improved state equation is:

[0121]

[0122] where α and β are weighting coefficients, x odom (k), y odom (k) are the actual x and y coordinates of the robot obtained according to the odometer information respectively.

[0123] S33: The measurement equation of the system is:

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

[0125] where h(X p (k)) is the predicted measurement value between the robot and each node in the sensor network and between the nodes in the sensor network. Z(k) is the corrected measurement value between the robot and the nodes in the sensor network and between the nodes at time k:

[0126]

[0127] S4: According to the derived system state equation and measurement equation, based on the improved extended Kalman filter algorithm, the overall estimation of the system state is carried out to realize the simultaneous localization of the mobile robot and the sensor network nodes in the warehousing scenario, as Figure 4 shown.

[0128] ​S41: According to the improved state equation and based on the initial system state value obtained in the ranging correction stage, the state prediction vector X of the robot-sensor network system at time k can be calculated. p (k).

[0129] S42: Predict the covariance matrix P of the prior estimate value of the state vector of the robot-sensor network system. kp It is:

[0130]

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

[0132] S43: Calculate the Kalman gain:

[0133]

[0134] Where H k is the Jacobian matrix of h(X p (k)) with respect to the state vector at time k, and R k is the covariance matrix of the measurement noise.

[0135] S44: Obtain the updated robot state and sensor network node state. Through the Kalman gain, predict the correction of the state vector to obtain a 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: Correct the covariance matrix P k :

[0138] P k = (I - K k H k )P kp .

[0139] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods. Among them, any reference to a memory, database, or other medium used in the embodiments provided in this application can include at least one of non-volatile and volatile memories. Non-volatile memories can include read-only memory (ROM), magnetic tapes, floppy disks, flash memories, optical memories, high-density embedded non-volatile memories, resistive random access memories (ReRAM), magnetoresistive random access memories (MRAM), ferroelectric random access memories (FRAM), phase change memories (PCM), graphene memories, etc. Volatile memories can include random access memory (RAM) or external cache memories, etc. By way of illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc. The databases involved in the embodiments provided in this application can include at least one of relational databases and non-relational databases. Non-relational databases can include distributed databases based on blockchain, etc., without limitation. The processors involved in the embodiments provided in this application can be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logics, data processing logics based on quantum computing, etc., without limitation.

[0140] It should also be understood that in the embodiments of the present invention, the term "and / or" is only a description of the association relationship of associated objects, indicating that three relationships can exist. For example, A and / or B can represent three situations: A exists alone, A and B exist simultaneously, and B exists alone. In addition, in the present invention, the character " / " generally represents an "or" relationship between the associated objects before and after.

[0141] Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed in the present invention can be implemented by electronic hardware, computer software, or a combination of the two. To clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described according to functions in the above description. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Professional technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of the present invention.

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

[0143] In several embodiments provided by the present invention, it should be understood that the disclosed systems, devices, and methods can be implemented in other ways. For example, the device embodiments described above are only illustrative. For example, the division of the units is only a logical function division, and there can be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed couplings or direct couplings or communication connections to each other can be indirect couplings or communication connections through some interfaces, devices, or units, and can also be electrical, mechanical, or other forms of connection.

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

[0145] Specific embodiments of the present invention are used to elaborate on the principles and implementation manners of the present invention. The descriptions of the above embodiments are only used to help understand the method of the present invention and its core idea; at the same time, for those of ordinary skill in the art, based on the idea of the present invention, there will be changes in the specific implementation manners and application scopes. In summary, the content of this specification should not be construed as a limitation to the present invention.

Claims

1. A collaborative positioning method for a mobile robot and a sensor network node in a warehousing scenario, characterized in that: The specific process of this method is as follows: S1: Data collection and preprocessing: Arrange sensor network nodes, start the robot, collect robot motion information, distance measurement information between the robot and the node and between the nodes, and filter the relevant distance measurement information; 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 the unknown nodes in the sensor network; S3: Establish the state equation and measurement equation of the robot-sensor network system; S4: According to the derived system state equation and measurement equation, the system state is estimated as a whole based on the improved extended Kalman filter algorithm to achieve simultaneous positioning of mobile robots and sensor network nodes in warehousing scenarios.

2. The method according to claim 1, characterized in that The specific process of step S1 is as follows: Step S11: start the mobile robot, move the robot at a constant speed for a certain distance, and collect 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; Step S12: performing amplitude limiting filtering on the ranging information to filter out abnormal values; Step S13: Use the Kalman filter algorithm to further smooth the distance measurement information to obtain a relatively stable distance measurement value.

3. The method according to claim 1, characterized in that: The specific process of step S2 is as follows: Step S21: Establishing a distance measurement correction model; Step S22: Calculate the Euclidean distance between the robot and the node, and between the nodes; Step S23: designing the objective function so that the corrected distance measurement value and the Euclidean distance value between nodes are as close as possible, and fitting the parameters using the least square method; Step S24: solving the nonlinear least squares problem to obtain correction parameters and initial values ​​of sensor network node positions; Step S25: Perform real-time distance measurement correction using the distance measurement correction model.

4. The method according to claim 3, characterized in that: The specific process of step S21 is as follows: Take the m-order polynomial as the ranging correction model: in is the corrected distance measurement value of the i-th node in the sensor network, d i is the distance measurement value before correction, α0,α1,...,α m is the calibration parameter.

5. The method according to claim 3, characterized in that: The specific process of step S22 is as follows: Assume that at time k, the position of the mobile robot is X r (k)=(x r (k),y r (k),z r (k),θ r (k)) T , the location of n sensor nodes is 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 for: Euclidean distance between nodes for: Then the Euclidean distance between the mobile robot and n sensor network nodes at time k, as well as the Euclidean distance vector between sensor network nodes can be expressed as: In the initial stage, the mobile robot has a smaller cumulative error in the dead reckoning result based on the odometer within a shorter distance, and this result is taken as its actual coordinate value; At time k, the actual measurement values ​​between the robot and other nodes in the sensor network, and between nodes in the sensor network during movement are expressed as: in, represents the distance measurement between the mobile robot and the sensor network node i, It represents the distance measurement value between sensor network nodes i and j.

6. The method according to claim 3, characterized in that: In step S23, the following objective function is designed: where g(k)° m is the mth Hadamard power of the vector g(k).

7. The method according to claim 1, characterized in that: The specific process of step S3 is as follows: S31: The state equation of the robot-sensor network system is: X p (k)=f(X(k-1),u k )+ω k Where X p (k) is the predicted value of the system state vector at time k, u k is the control vector, X(k-1) is the optimal estimate of the state vector of the system at the previous moment, ω k is the process noise; according to the motion model of the mobile robot wheel odometer, the corresponding control vector can be obtained: you k =[Δd k Dth k ] where Δd k is the distance the robot moves from time k-1 to time k, Δθ k is the angle at which the robot rotates from time k-1 to time k; S32: To ensure the matching alignment of the distance measurement data and the robot odometer data, the improved state equation is: Among them, α, β are weighting coefficients, x odom (k), y odom (k) are the x and y coordinates of the robot actually obtained based on the odometer information; S33: The measurement equation of the system is: Z(k)=h(X p (k))+v k Among them, h(X p (k)) is the predicted measurement value between the robot and each node in the sensor network and between nodes in the sensor network; Z(k) is the corrected measurement value between the robot and the nodes in the sensor network and between nodes at time k: Z(k)=α m ·g(k) m +a m-1 ·g(k) (m-1) + … +α1·g(k)+α0I 8. The method according to claim 1, characterized in that: The specific process of step S4 is as follows: Step S41, according to the improved state equation and based on the initial value of the system state obtained in the ranging correction stage, calculate the state prediction vector of the robot-sensor network system at time k; Step S42, predicting the covariance matrix of the prior estimate value of the robot-sensor network system state vector at time k; Step S43, calculating the Kalman gain; Step S44, correcting the predicted state vector to obtain more accurate robot posture and sensor network node coordinates; Step S45: modify the covariance matrix.

Images