CN-122017732-A - Positioning method and positioning device for moving target, electronic equipment and warehouse

CN122017732ACN 122017732 ACN122017732 ACN 122017732ACN-122017732-A

Abstract

The embodiment of the application provides a positioning method and device for a moving target, electronic equipment and a warehouse. The method comprises the steps of obtaining anchor point distances between a moving target and a plurality of fixed anchor points at the current moment and filtering states of the moving target at the last moment, taking the filtering states at the last moment as prior states when the filtering states at the last moment are effective, determining the filtering initial states through auxiliary positioning information and taking the filtering initial states as prior states when the filtering states at the last moment are ineffective, predicting through a kinematic model based on the prior states to obtain the predicting states at the current moment, and carrying out Kalman filtering updating on the predicting states based on the anchor point distances to obtain the position information of the moving target. The auxiliary positioning method based on space geometry is provided, the initial filtering state can be provided as the prior state under the condition that the filtering state is invalid, and the problem of poor initial positioning robustness is solved.

Inventors

HUANG ZHIHANG

Assignees

深圳市库宝软件有限公司

Dates

Publication Date: 20260512
Application Date: 20260210

Claims (13)

1. A method of locating a moving object, comprising: acquiring the anchor point distance between a moving target and a plurality of fixed anchor points at the current moment and the filtering state of the moving target at the last moment; when the filtering state at the previous moment is effective, taking the filtering state at the previous moment as the prior state of the current Kalman filtering cycle; When the filtering state at the previous moment is invalid, determining a filtering initial state through auxiliary position information, and taking the filtering initial state as the prior state of the current Kalman filtering cycle; based on the prior state, predicting through a kinematic model to obtain a predicted state of the current moment; Based on the anchor point distances, carrying out Kalman filtering updating on the prediction state to obtain the position information of the moving target; The auxiliary position information is position information determined through space geometric positioning based on anchor point distances between the moving target and the plurality of fixed anchor points.
2. The positioning method according to claim 1, wherein the determining the initial state of filtering by the auxiliary positioning information specifically includes: at each moment, determining corresponding auxiliary position information based on anchor point distances between the moving target and a plurality of fixed anchor points; Calculating a first distance between the auxiliary position information at the kth time and the auxiliary position information at the (k+1) th time; calculating a second distance between the auxiliary position information at the k-1 time and the auxiliary position information at the k time; When the first distance and the second distance are both smaller than a first distance threshold, taking the auxiliary position information at the k+1th time as the position information of the filtering initial state, and Calculating the determined speed of the moving target as the speed information of the filtering initial state by using the auxiliary position information at the kth time and the auxiliary position information at the kth+1 time; the first distance threshold is a product of an expected moving speed upper limit of the moving target and a time interval between two adjacent moments, k is a positive integer greater than or equal to 2, and the k+1th moment is the current moment.
3. The positioning method according to claim 2, wherein the determining the corresponding auxiliary position information based on the anchor point distances between the moving target and the plurality of fixed anchor points specifically includes: determining whether a reference anchor point distance smaller than a second distance threshold exists among anchor point distances between the moving target and the plurality of fixed anchor points; When the reference anchor point distance exists, the position information of the fixed anchor point corresponding to the reference anchor point distance is used as the auxiliary position information; when the reference anchor point distance does not exist, calculating the auxiliary position information based on a plurality of anchor point distances.
4. The positioning method according to claim 3, wherein the calculating auxiliary position information of the moving object based on the anchor point distances specifically includes: based on preset screening conditions, determining three candidate fixed anchor points in the plurality of fixed anchor points; Respectively taking the three candidate fixed anchor points as circle centers and the corresponding anchor point distances as radiuses to construct three positioning circles; calculating intersection point sets obtained by intersecting the three positioning circles two by two; selecting at least three intersection points with highest aggregation degree from the intersection point set to form a triangle area; And taking the geometric center of the triangle area as the auxiliary position information.
5. The positioning method according to claim 1, wherein the prior state comprises a first state vector for representing a motion state of the moving object and a first covariance matrix for representing uncertainty of the motion state, and the kinematic model is a uniform motion model; The predicting, based on the prior state, by a kinematic model to obtain the predicted state of the current moment specifically includes: Determining a corresponding state transition matrix based on the uniform motion model; Constructing a process noise covariance matrix, wherein the process noise covariance matrix is used for representing uncertainty of acceleration change possibly occurring under the uniform motion model; Performing state prediction on the first state vector by using the state transition matrix to generate a second state vector at the current moment; And based on the state transition matrix and the process noise covariance matrix, carrying out propagation update on the first covariance matrix to obtain a second covariance matrix at the current moment.
6. The positioning method according to claim 5, wherein the constructing a process noise covariance matrix specifically comprises: configuring noise power parameters; Generating the process noise covariance matrix based on the noise power parameter and a time interval between adjacent moments; The motion state of the moving target comprises position information and speed information, and process noise variance and covariance related to the position information and the speed information are obtained by linear combination of the noise power parameter and a power term of the time interval.
7. The positioning method according to claim 5, wherein the performing a kalman filter update on the prediction state based on the anchor point distances to obtain the position information of the moving object specifically includes: Forming a measurement vector based on a plurality of anchor point distances; calculating a corresponding predictive measurement vector based on the position information in the second state vector; based on the measurement vector and the predicted measurement vector, obtaining a corresponding measurement residual; linearizing the nonlinear measurement function at the second state vector, and calculating to obtain a jacobian matrix; Constructing a ranging noise covariance matrix for representing a ranging error of the anchor point distance; Calculating a kalman gain based on the jacobian matrix, the ranging noise covariance matrix, and the second covariance matrix; Updating the second state vector according to the Kalman gain and the measurement residual error to obtain a third state vector, and updating the second covariance matrix according to the Kalman gain and the jacobian matrix to obtain a third covariance matrix; Extracting position information from the third state vector as position information of the moving object, and taking the third covariance matrix and the third state vector as the filtering state; The nonlinear measurement function takes the position information in the second state vector as input and takes the predicted distance from each fixed anchor point to the moving target as output.
8. The positioning method according to claim 7, wherein the constructing a ranging noise covariance matrix for representing the ranging noise of the anchor point distance specifically comprises: Determining residual errors corresponding to all the fixed anchor points; According to the residual error of each fixed anchor point, scaling and updating diagonal elements corresponding to each anchor point in the measurement noise covariance matrix; wherein the scaling update comprises multiplying the diagonal elements by scaling coefficients, which are determined by corresponding residual calculations.
9. The positioning method according to claim 8, wherein the scaling factor is set to 1 when the corresponding residual is smaller than a preset threshold, and wherein the scaling factor increases as the residual increases as a preset function when the corresponding residual is larger than the preset threshold.
10. A positioning device, comprising: The acquisition module is used for acquiring the anchor point distance between the moving target and a plurality of fixed anchor points at the current moment and the filtering state of the moving target at the last moment; The initialization module is used for taking the filtering state at the last moment as the prior state of the current Kalman filtering cycle when the filtering state at the last moment is effective, determining the filtering initial state through auxiliary position information when the filtering state at the last moment is ineffective, and taking the filtering initial state as the prior state of the current Kalman filtering cycle; the iteration updating module is used for predicting through a kinematic model based on the prior state to obtain a predicted state at the current moment, and carrying out Kalman filtering updating on the predicted state based on a plurality of anchor point distances to obtain the position information of the moving target; The auxiliary position information is position information determined through space geometric positioning based on anchor point distances between the moving target and the plurality of fixed anchor points.
11. An electronic device comprising a processor and a memory, the memory storing computer program instructions that, when invoked by the processor, cause the processor to perform the positioning method of any of claims 1-9.
12. A warehouse, comprising: The system comprises a plurality of fixed devices, a first UWB module and a second UWB module, wherein each fixed device is provided with a first UWB module; A plurality of mobile devices, each mobile device being provided with a second UWB module, moving between the plurality of stationary devices; The electronic device of claim 11, communicatively coupled to each of the mobile devices, configured to determine a measured distance between the mobile device and each of the stationary devices via the first and second UWB modules, and to determine location information for the mobile device based on a plurality of the measured distances.
13. A computer readable storage medium, characterized in that the computer readable storage medium stores a computer program comprising program instructions which, when executed by a processor, cause the processor to perform the positioning method according to claims 1-9.

Description

Positioning method and positioning device for moving target, electronic equipment and warehouse Technical Field The present application relates to the field of logistics warehouse technology, and in particular, to a method and apparatus for positioning a moving object, an electronic device, a warehouse, and a computer readable storage medium. Background With the intelligent upgrade of the logistics storage industry in recent years, ultra Wide Band (UWB) positioning technology is widely applied because of the characteristics of high positioning precision, strong multipath interference resistance and flexible hardware deployment. However, when the UWB positioning technology is applied to indoor scenes such as large logistics warehouse and dense shelf warehouse, the UWB positioning technology is very dependent on the ranging data of the fixed anchor points as a basis for performing the position information calculation, and the position calculation is generally performed using only a single model. Therefore, the method has the defects of weak initial positioning robustness, insufficient noise dynamic adaptation capability, easy jump of positioning results under complex shielding scenes and the like. Particularly, in the storage environment with dense shielding of a goods shelf and high-frequency cross flow of staff and operation equipment, the problem of interruption of position information calculation is very easy to occur, and the high-precision and high-reliability positioning requirements of a modern logistics warehouse on services such as real-time personnel tracking, operation collision early warning and intelligent path optimization are difficult to meet. Disclosure of Invention The embodiment of the application provides a method, a device, electronic equipment, a warehouse and a computer readable storage medium for positioning a moving target, which aim to solve at least part of defects existing in the existing moving target positioning mode. In a first aspect, an embodiment of the present application provides a method for positioning a moving object. The positioning method comprises the steps of obtaining anchor point distances between a moving target and a plurality of fixed anchor points at the current moment and filtering states of the moving target at the last moment, taking the filtering states at the last moment as priori states of a current Kalman filtering cycle when the filtering states at the last moment are effective, determining a filtering initial state through an auxiliary positioning method and taking the filtering initial state as the priori states of the current Kalman filtering cycle when the filtering states at the last moment are invalid, predicting through a kinematic model based on the priori states to obtain prediction states at the current moment, and carrying out Kalman filtering update on the prediction states based on the anchor point distances to obtain position information of the moving target. The auxiliary position information is position information determined through space geometric positioning based on anchor point distances between the moving target and the plurality of fixed anchor points. In some embodiments, the determining the initial state of filtering by the auxiliary positioning information specifically includes: At each moment, auxiliary position information of a corresponding moment is determined based on anchor point distances between the moving target and a plurality of fixed anchor points, a first distance between the auxiliary position information of the kth moment and the auxiliary position information of the kth+1 moment is calculated, a second distance between the auxiliary position information of the kth-1 moment and the auxiliary position information of the kth moment is calculated, when the first distance and the second distance are smaller than a first distance threshold, the auxiliary position information of the kth+1 moment is used as the position information of the filtering initial state, the speed of the moving target determined by calculating the auxiliary position information of the kth moment and the auxiliary position information of the kth+1 moment is used as the speed information of the filtering initial state, wherein the first distance threshold is a product of an expected moving speed upper limit of the moving target and a time interval between two adjacent moments, k is a positive integer larger than or equal to 2, and the kth+1 moment is the current moment. In some embodiments, the determining corresponding auxiliary position information based on the anchor point distances between the moving target and the plurality of fixed anchor points specifically includes determining whether a reference anchor point distance smaller than a second distance threshold exists among the anchor point distances between the moving target and the plurality of fixed anchor points, taking the position information of the fixed anchor point corresponding to