Moving vehicle positioning method, device, system and computer program product

Abstract

The invention discloses a moving vehicle positioning method, device and system and a computer program product, and belongs to the technical field of positioning. The method comprises the steps of obtaining motion video information of a vehicle; obtaining a current camera observation value zt of the vehicle according to the motion video information of the vehicle; according to the pre-trained bidirectional long and short time memory network model and the correction value xt-1 at the previous moment, obtaining a prior estimated value # imgabs0 # at the current moment, and obtaining a Kalman gain Kt at the current moment according to a covariance matrix Pt-1 of the correction value xt-1 at the previous moment; based on a Kalman filtering algorithm, obtaining a current moment correction value xt according to a current moment priori estimation value # imgabs1 # current moment camera observation value zt and a current moment Kalman gain Kt; the initial moment correction value x0 is second coordinate information of the vehicle at the initial moment obtained based on the UWB positioning technology. According to the technical scheme, the UWB positioning data can be dynamically corrected, the positioning precision is improved, noise and errors are remarkably reduced, and the motion trail of the vehicle is smoother.

Description

Technical Field

[0001] The present invention belongs to the field of positioning technology, and in particular relates to a moving vehicle positioning method, device, system and computer program product. Background Art

[0002] Experimental testing is a key link in improving the safety and reliability of autonomous driving systems. Compared with non-quantitative typical scenario testing, accurate measurement of vehicle motion parameters is particularly important for quantitative evaluation and technology iteration of autonomous driving systems, and accurate real-time positioning is the basis for ensuring the accuracy of test data. Although the Global Navigation Satellite System (GNSS) can meet the needs of high-precision positioning in open environments, its reliability will be significantly reduced in obstructed environments such as tunnels.

[0003] Ultra-Wideband (UWB) positioning technology has significant advantages in complex and obstructed environments due to its wide-band characteristics, good penetration ability and decimeter-level positioning accuracy. Through flexible base station deployment, positioning signal coverage of specific areas can be achieved, which can effectively solve the test problems in tunnels or complex urban scenes. However, due to the multipath effect, non-line-of-sight (NLOS) and system hardware performance, the application of UWB in high-speed vehicle positioning still faces challenges. Summary of the invention

[0004] In order to solve the above problems, the present invention provides a moving vehicle positioning method, device, system and computer program product.

[0005] In one aspect, the present invention provides a method for positioning a moving vehicle, comprising:

[0006] Obtain the motion video information of the vehicle; obtain the camera observation value z of the vehicle at the current moment according to the motion video information of the vehicle t , the camera observation value z t represents the first coordinate information of the vehicle obtained based on the motion video information; obtains the correction value x at the previous moment t-1 and the correction value x at the previous moment t-1 The covariance matrix P t-1 ; Based on the pre-trained bidirectional long short-term memory network model and the previous moment correction value x t-1 , get the prior estimate at the current time The bidirectional long short-term memory network model is used to correct the value x at the previous moment according to the t-1 Predict the a priori estimate of the current moment According to the correction value x at the previous moment t-1The covariance matrix P t-1 Get the current Kalman gain K t Based on the Kalman filter algorithm, according to the current a priori estimate The current camera observation value z t and the current Kalman gain K t Get the current correction value x t ; Among them, the initial moment correction value x0 is the second coordinate information of the vehicle obtained based on ultra-wideband UWB positioning technology.

[0007] Another aspect of the present invention provides a moving vehicle positioning device, which comprises:

[0008] The first acquisition module is used to acquire the motion video information of the vehicle; the observation value acquisition module is used to obtain the camera observation value z of the vehicle at the current moment according to the motion video information of the vehicle. t , the camera observation value z t represents the first coordinate information of the vehicle obtained based on the motion video information; the second acquisition module is used to obtain the correction value x at the previous moment t-1 and the correction value x at the previous moment t-1 The covariance matrix P t-1 ; A priori estimate value acquisition module is used to obtain the prior estimate value based on the pre-trained bidirectional long short-term memory network model and the previous moment correction value x t-1 , get the prior estimate at the current time The bidirectional long short-term memory network model is used to correct the value x at the previous moment according to the t-1 Predict the current moment prior estimate Kalman gain module is used to correct the value x according to the previous moment t-1 The covariance matrix P t-1 Get the current Kalman gain K t ; A correction value obtaining module for obtaining a priori estimated value at the current moment based on a Kalman filter algorithm The current camera observation value z t and the current Kalman gain K t Get the current correction value x t ; Among them, the initial moment correction value x0 is the second coordinate information of the vehicle obtained based on ultra-wideband UWB positioning technology.

[0009] Yet another aspect of the present invention provides an electronic device, comprising: a processor and a memory for storing executable instructions of the processor; wherein the processor is configured to execute the above-mentioned moving vehicle positioning method.

[0010] On the other hand, the present invention provides a computer-readable storage medium, which stores at least one instruction, at least one program, code set or instruction set, and the at least one instruction, at least one program, code set or instruction set is loaded and executed by a processor to implement the above-mentioned moving vehicle positioning method.

[0011] In yet another aspect, the present invention provides a computer program product comprising instructions, which, when executed on a computer, enables the computer to execute the various steps in the aforementioned moving vehicle positioning method and various possible implementations.

[0012] The technical solution provided by the embodiment of the present invention has the following beneficial effects:

[0013] Obtain the vehicle's motion video information; obtain the vehicle's current camera observation value z based on the vehicle's motion video information t , camera observation value z t Represents the first coordinate information of the vehicle obtained based on the motion video information; obtains the correction value x at the previous moment t-1 and the correction value x at the previous moment t-1 The covariance matrix P t-1 ; Based on the pre-trained bidirectional long short-term memory network model and the previous moment correction value x t-1 , get the prior estimate at the current time The bidirectional long short-term memory network model is used to correct the value x according to the previous moment t-1 Predict the current time prior estimate According to the correction value x at the previous moment t-1 The covariance matrix P t-1 Get the current Kalman gain K t ; Based on the Kalman filter algorithm, according to the current prior estimate The camera observation value z at the current moment t and the current Kalman gain K t Get the current correction value x t; Wherein, the initial moment correction value x0 is the second coordinate information of the vehicle obtained based on the ultra-wideband UWB positioning technology, making the moving vehicle positioning method a UWB positioning vehicle motion trajectory calibration method that integrates imaging technology. This method uses the dynamic position data of the vehicle identified by the high-speed camera (i.e., the camera observation value) as a reference value to correct and predict the UWB positioning data. At the same time, based on the Kalman filter algorithm, the bidirectional long short-term memory network is integrated to learn the state transfer function and related parameters of the moving vehicle. The motion state of the target is learned from the trajectory data of the moving vehicle, the state transfer function of the moving vehicle is modeled as a learnable network, and the Kalman filter algorithm is used to dynamically correct the target state vector. It can effectively improve the positioning accuracy of UWB, reduce positioning noise and errors, and improve the reliability of UWB positioning technology in moving vehicle positioning and trajectory detection. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 A schematic diagram of a flow chart of a moving vehicle positioning method provided by an embodiment of the present invention;

[0015] Figure 2 A side view schematic diagram of a camera space model provided by an embodiment of the present invention;

[0016] Figure 3 A schematic top view of a camera space model provided by an embodiment of the present invention;

[0017] Figure 4 A schematic diagram of measurable line segments in a world coordinate system in a single vanishing point calibration model provided by an embodiment of the present invention;

[0018] Figure 5 A corresponding line segment in an image coordinate system in a single vanishing point calibration model provided by an embodiment of the present invention;

[0019] Figure 6 A schematic diagram of the overall framework of a vehicle motion trajectory calibration model provided by an embodiment of the present invention;

[0020] Figure 7 A schematic diagram of the internal structure of a BILSTM-KF model provided in an embodiment of the present invention;

[0021] Figure 8 A schematic diagram of the internal structure of a BILSTM-KF model provided in an embodiment of the present invention;

[0022] Fig. 9 A schematic diagram of a test site provided by an embodiment of the present invention;

[0023] Fig.10 A diagram of the detection process of the AprilTag algorithm provided in an embodiment of the present invention;

[0024] Fig.11 A schematic diagram of state transfer function training and prediction provided by an embodiment of the present invention;

[0025] Fig.12 Another schematic diagram of state transfer function training and prediction provided by an embodiment of the present invention;

[0026] Fig.13 A comparison diagram of UWB positioning and reference values ​​after correction of a data sequence provided by an embodiment of the present invention;

[0027] Fig.14 Another comparison diagram of UWB positioning after correction and reference value provided by an embodiment of the present invention;

[0028] Fig.15 A comparison diagram of another corrected UWB positioning and a reference value provided by an embodiment of the present invention;

[0029] Fig.16 A comparison diagram of another corrected UWB positioning and a reference value provided in an embodiment of the present invention;

[0030] Fig.17 A schematic structural diagram of a moving vehicle positioning device provided in an embodiment of the present invention. DETAILED DESCRIPTION

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

[0032] See also Figure 1 The embodiment of the present invention provides a moving vehicle positioning method, which includes the following steps:

[0033] Step 101, obtaining motion video information of the vehicle.

[0034] When a vehicle is driving on the road, the vehicle is photographed by a pre-set image acquisition device, and the vehicle's motion video information can be acquired. The image acquisition device can be: a video camera, a high-speed camera, etc. When the parameters of the high-speed camera meet the following requirements, the shooting effect is better: the camera resolution reaches 4096x 2048 (4K resolution), and the maximum FPS (Frames Per Second) at full resolution reaches 500 frames per second. The image acquisition device can be located in the middle of the gantry across the road, with the lens facing downward for shooting.

[0035] In order to better identify, track and locate the vehicle, a visual positioning marker is set and installed on the vehicle, such as the top of the vehicle. When applied, the visual positioning marker is captured in real time using an image acquisition device, that is, the motion video information acquired contains a visual positioning marker, which can monitor the vehicle status and position. The visual positioning marker can be: AprilTag marker, which is a two-dimensional pattern, or: ARTag marker, or ArUco marker, which is not limited in this embodiment, and is preferably an AprilTag marker.

[0036] Step 102: Obtain the camera observation value z of the vehicle at the current moment according to the motion video information of the vehicle t , camera observation value z t Represents the first coordinate information of the vehicle obtained based on the motion video information.

[0037] Since the visual positioning marker is set on the vehicle, the visual positioning marker in the vehicle's motion video information is identified, detected and positioned to obtain the vehicle's current camera measurement value, which is also called the vehicle's pixel coordinate value. Then, the single vanishing point camera calibration method is used to convert the current camera measurement value into the vehicle's current camera observation value z in world coordinates. t , the camera observation value z at the current moment t Represents the first coordinate information of the vehicle at the current time t obtained based on the motion video information.

[0038] The algorithm for identifying the visual positioning mark can adopt the method in the prior art, and this embodiment does not limit this. When the visual positioning mark is an AprilTag mark, the algorithm for identifying the mark can be called the AprilTag detection algorithm, which is a visual marking system used in machine vision tasks, and is commonly used in object recognition, target tracking, visual positioning, simultaneous localization and mapping (SLAM) and posture estimation. The algorithm is suitable for the detection and identification of moving objects due to its high-precision detection capability and robustness under different lighting and partial occlusion. In other words, the algorithm has good robustness to changes in viewing angles and partial occlusions, and can reliably detect and track vehicles even in complex traffic environments, fast-moving vehicles or poor light conditions.

[0039] Before identifying the visual positioning mark, the original video information acquired by the image acquisition device is subjected to image preprocessing. The present embodiment does not limit the specific operation content of the preprocessing operation, which may include: image graying, Gaussian filtering and binarization, etc. The recognition process can be divided into: straight line extraction, quadrilateral detection and decoding of the detected quadrilaterals. First, by calculating the gradient direction and amplitude of the pixel, and fitting the straight lines in the image by weighted least squares; then, the quadrilaterals formed by the straight lines are detected by the search tree method; finally, the detected quadrilaterals are subjected to homography transformation and parameter estimation, and the quadrilaterals are decoded by spatially varying thresholds.

[0040] The current camera measurement value is converted into the current camera observation value z of the vehicle in the world coordinate system. t Before that, the image acquisition device needs to be calibrated. This embodiment does not specifically limit the calibration method, and the method in the prior art can be used.

[0041] Establish the projection relationship between the two-dimensional image coordinates (u, v) and the three-dimensional world coordinates (x, y, z): The schematic diagram of the camera space model is as follows Figure 2-3 As shown, the origin of the world coordinate system is the vertical projection point O of the camera on the road plane. w , X w The axis is perpendicular to the road to the right, Y w Axis parallel to the road, Z w The axis is perpendicular to the ground plane and points upward. The origin of the camera coordinate system is at the camera position, X c Axis and X w Axis parallel, Z c The Y axis points to the ground along the optical axis. c Axis perpendicular to X C OZ c The origin of the image coordinate system is at the image principal point r, i.e. Z c The image coordinate system projection of the intersection of the axis and the ground. Under this condition, the camera calibration parameters can be simplified to the camera focal length f, pitch angle φ, yaw angle θ and camera height h.

[0042] The expanded camera calibration projection relationship can be obtained as follows:

[0043]

[0044] Where λ is the proportional factor and λ≠0.

[0045] See also Figure 4-Figure 5 The vanishing point (VP) contains a lot of information in camera calibration. According to the VP principle, the calibration parameters φ and θ can be solved. The infinite point along the road direction is expressed in three-dimensional homogeneous coordinates as X1 = [-tanθ 1 0 0] T, projected onto the image plane to form the vanishing point (u1, v1), as Figure 5 As shown. Therefore, we can get:

[0046]

[0047] In addition to the VP point, the known physical line segments in the scene can usually be used to construct constraints for solving the camera height. Figure 4-Figure 5 As shown in the figure, let the length of the road dashed line (blue line segment) be l, and the world coordinates of the two end points of the dashed line be (*,y a ,0) and (*,y b ,0), the corresponding image coordinates are (*,v a ) and (*,v b ), where * indicates that the value is irrelevant to the calculation; suppose the road width w( Figure 4 The line segment on the left side of w in the world coordinate system is at X w The projection distance on the axis is Δx, which corresponds to the pixel difference Δu along the u axis on the image. Figure 4 and Figure 5 In the equation, (x3, y3) and (x4, y4) represent any two points with the same y coordinate in the world coordinate system, and u3 and u4 represent the coordinates of the two points on the u axis of the image plane. The physical distance Δx between the two points in the world coordinate system is related to the pixel difference Δu corresponding to the pixel along the u axis on the image, and h can be solved by this relationship. Δx has a geometric relationship with the road width, that is, Δx = w*secθ.

[0048] From the relevant content, we can know that h can be indirectly derived from w and l as:

[0049]

[0050] In the formula, τ=(v a -v1)(v b -v1) / (v a -v b ), δ is the horizontal length along the line v=0 in the image.

[0051] For the same camera, the camera height calculated by the line segment along the road direction or perpendicular to the road direction should be equal, so the equation about f is obtained by combining equations (9) and (10):

[0052]

[0053] In the formula, k v =δτl / wv1.

[0054] When f is uniquely determined, φ and θ can be solved according to equations (2) and (3), and h can be solved according to equations (4) or (5). All unknown calibration parameters have been solved, and the calibration projection relationship between the two-dimensional image coordinates and the three-dimensional world coordinates can be established through equation (1).

[0055] Step 103, obtain the correction value x at the previous moment t-1 and the correction value x at the previous moment t-1 The covariance matrix P t-1 ;

[0056] Among them, the initial moment correction value x0 is the second coordinate information of the vehicle at the initial moment obtained based on the ultra-wideband UWB positioning technology. UWB positioning technology can collect vehicle motion coordinate information. This embodiment does not specifically limit the UWB positioning technology, which can be an existing technology. When applied, it is necessary to deploy UWB positioning base stations and UWB positioning tags.

[0057] For the specific implementation process of this step, please refer to the relevant description content of step 104 to step 106.

[0058] Step 104: Based on the pre-trained bidirectional long short-term memory network model and the previous moment correction value x t-1 , get the prior estimate at the current time The bidirectional long short-term memory network model is used to correct the value x according to the previous moment t-1 Predict the current time prior estimate

[0059] Although the Kalman filter (KF) algorithm is widely used in state estimation of dynamic systems, it has limited performance in nonlinear complex scenarios and is often based on the constant acceleration assumption, which limits the prediction accuracy. In this embodiment, the KF algorithm is combined with the BILSTM (Bidirectional Long Short-Term Memory) neural network to calibrate and predict the vehicle target trajectory. Specifically, according to the optimal result x at the previous moment, t-1 , predict the prior estimate of the current moment At the same time, use the current observation value z t , correct the prior estimate at the current moment So as to obtain the optimal result x at the current moment t .

[0060] The following is an explanation of the bidirectional long short-term memory network (BILSTM) model. This model is used to correct the value x at the previous moment. t-1 Predict the current time prior estimate It can be implemented based on the Keras framework. Specifically, the network model includes: an input layer, a first bidirectional long short-term memory network (LSTM) layer, a first random dropout layer, a second bidirectional long short-term memory network layer, a second random dropout layer, a fully connected layer and an output layer connected in sequence. Among them, two bidirectional LSTM layers can be configured with 120 neurons. In order to balance the fitting ability, computational efficiency and control overfitting of the model, each bidirectional LSTM layer is followed by a Dropout layer with a dropout rate of 0.5. A fully connected layer containing 120 neurons is set before the output layer, and an activation function is used, which can be ReLU.

[0061] In terms of model initialization, all weights can be initialized using the Xavier method, and the deviation value is initialized to 0. The initial motion state of the vehicle is set to the starting point x0, and the covariance matrix P0 of the initial state is set to the unit matrix I. The learning rate of the model is set to 0.001, and the Adam optimizer is used for parameter optimization, and the training batch size is 16. The network model adaptively learns parameters through the gradient descent method on the training set. The training set is based on the continuous trajectory values ​​collected by UWB. The sequence data set of the n+1 step is predicted by the first n steps to learn the process equation of motion. After the model training is completed, it can be put into use after verification by the validation set.

[0062] Get the correction value (or optimal value) x at the previous moment (or last moment) t-1 After that, it is used as the input of the bidirectional long short-term memory network model to predict: the current moment prior estimate

[0063]

[0064] Where: The mapping function f(·) is learned by fitting the BILSTM neural network, and the powerful nonlinear fitting ability of the neural network is used to learn the complex motion trajectory of the vehicle target, thereby replacing the state transfer matrix in the KF algorithm; w t is the process noise.

[0065] Step 105: Correct the value x at the previous moment t-1 The covariance matrix P t-1 Get the current Kalman gain K t .

[0066] Specifically, obtain the correction value x at the previous moment t-1 The covariance matrix P t-1 , according to the correction value x at the previous moment t-1 The covariance matrix P t-1 and the Jacobian matrix F t-1Get the current prior estimate The covariance matrix of According to the covariance matrix and the current camera observation value z t The observation matrix H t Get the current Kalman gain K t .

[0067] Among them, the covariance matrix of the prior estimate at the current moment can be obtained by the following formula:

[0068]

[0069] In the formula, F t-1 is the correction value x at the previous moment t-1 The Jacobian matrix of ; Q is the covariance matrix of process noise; is the prior estimate at the current moment The covariance matrix of t-1 is the correction value x at the previous moment t-1 The covariance matrix of .

[0070]

[0071] Where: k t is the Kalman gain at the current moment; H is the observation matrix at the current moment, which is based on the camera observation value z at the current moment. t Get; R is the observation noise covariance matrix, which can be estimated by the variance of the historical observation data. Q is the covariance matrix of the process noise, which can be manually adjusted according to the size of the observation noise.

[0072] Step 106, based on the Kalman filter algorithm, according to the current a priori estimate The camera observation value z at the current moment t and the current Kalman gain K t Get the current correction value x t .

[0073] Specifically, according to the current Kalman gain K t 、The current camera observation value z t The observation matrix H t and the current prior estimate Get the intermediate value; based on the intermediate value and the prior estimate at the current moment Get the current correction value x t . Get the current correction value x t The formula is as follows:

[0074]

[0075] Where: z t is the observation value of the vehicle target at the current moment;

[0076] In step 103, the correction value x at the previous moment t-1 The method of obtaining the current correction value x can be found in t The methods of obtaining them are not described in detail here.

[0077] According to the unit matrix, the Kalman gain K at the current moment t 、The current camera observation value z t The observation matrix H t , the current prior estimate The covariance matrix of Get the current correction value x t The covariance matrix P t . Get the current correction value x t The covariance matrix P t The formula is as follows:

[0078]

[0079] Where P t is the optimal value x at the current moment t The covariance matrix of ; I is the identity matrix.

[0080] In step 103, the correction value x at the previous moment t-1 The covariance matrix P t-1 The method of obtaining the current correction value x can be found in t The covariance matrix P t The methods of obtaining them are not described in detail here.

[0081] This embodiment combines the KF algorithm with the BILSTM (Bidirectional Long Short-Term Memory) neural network, which is called the BILSTM-KF model. The vehicle motion trajectory calibration model is constructed by the BILSTM-KF model. The overall framework diagram is shown in the figure. Figure 6 As shown, the model is the optimal value x at the previous moment (t-1) t-1 and the observed value z at the current time t t As input, get the optimal value x at the current moment t , and update the target value vector iteratively in turn. The internal structure diagram of the BILSTM-KF model is as follows Figure 7 As shown in Figure 2, the model first uses a BILSTM neural network to learn the state transition function f(·), and then uses the prior estimate x output by f(·) t-1 and the observed value z tInput the KF algorithm to finally solve the optimal correction value at the current moment. Figure 6 In the above equation, x0 represents the second coordinate information of the vehicle at the initial moment obtained based on the ultra-wideband UWB positioning technology, z1 represents the first coordinate information of the vehicle at time 1 obtained based on the motion video information, x1 represents the correction value at time 1, and z t represents the first coordinate information of the vehicle at time t obtained based on the motion video information, x t Indicates the correction value at time t.

[0082] See also Figure 8 From the perspective of algorithm development, in each time step t, the optimal value x at the previous moment (i.e., moment t-1) is t-1 Enter the BILSTM neural network model as input to obtain the prior estimate of the current time t Then use the covariance matrix P at time t-1 t-1 , Q, calculate the prior covariance matrix at the current time t according to formula (8) Then use R, according to formula (9) we get the Kalman gain K t ; Then use the output of the BILSTM neural network Combined with K t and the input value z at the current time t t According to formula (10), the optimal value x at the final time t is obtained t ; while using and K t According to formula (11), we can get the optimal covariance matrix P at the final time t: t , and update iteratively in sequence.

[0083] It should be noted that the process of obtaining the prior estimate at the current moment and the covariance matrix of the prior estimate at the current moment can be called the prediction step; the process of obtaining the Kalman gain at the current moment and the optimal value x at the current moment can be called the prediction step. t , the process step of obtaining the covariance matrix of the optimal value at the current moment is called the update step.

[0084] This embodiment proposes a method for positioning a moving vehicle, and a UWB positioning vehicle motion trajectory calibration model integrating high-speed imaging technology is used in the implementation process of the method. The high-speed camera captures the dynamic position of the vehicle, provides high spatiotemporal resolution and reliable reference position information, which can be used to calibrate and compensate the UWB positioning data to improve the overall positioning accuracy of the UWB positioning device in a high dynamic environment. To implement the above calibration model, the calibration model is a UWB positioning trajectory correction and prediction algorithm (BILSTM-KF) based on Kalman filtering and bidirectional long short-term memory network. By combining the KF algorithm with the BILSTM network, the UWB positioning data can be corrected and the positioning noise and error can be reduced.

[0085] The following is an experiment and result analysis of the positioning method of the moving vehicle provided by this embodiment:

[0086] To build the experimental scene:

[0087] The test site is set up on a 700-meter-long test road in a highway traffic test site. A total of 24 UWB positioning base stations (or UWB base stations) are deployed on both sides of the road. The first 6 positioning base stations are 33 meters apart, and the remaining positioning base stations are 20 meters apart. The total coverage range is approximately 240 meters; a high-speed camera is installed in the middle of the gantry at the starting point. The model can be 5F08-M, with a camera resolution of 4096x2048 (4K resolution), and a maximum of 500 frames per second (FPS) at full resolution. The schematic diagram of the test site is as follows Fig. 9 shown.

[0088] First, a camera calibration model is established. Then, the AprilTag algorithm is used to detect the vehicle position in the camera output video and the UWB vehicle positioning data is collected synchronously. Finally, the two synchronous positioning data are used together with the calibration model and method of this embodiment to complete the correction of the vehicle motion trajectory. The experiment mainly includes the following three aspects of verification: (1) Verification of the accuracy of the camera calibration model; (2) Verification of the AprilTag detection algorithm's recognition success rate for the target vehicle; (3) Verification of the accuracy of the moving vehicle positioning device of this embodiment, that is, the accuracy of the vehicle motion trajectory correction.

[0089] Description of camera calibration results:

[0090] In order to verify the accuracy of the camera calibration method used, multiple verification points were selected in the camera field of view to conduct a comprehensive evaluation of the calibration method. In addition, in order to obtain the accurate true value coordinates of the verification points, the coordinates of the verification points were measured by a total station that has been calibrated, as shown in the following table (x, y). At the same time, the selected verification points were converted from image coordinates to world coordinates using the camera calibration projection relationship established above, and the corresponding calibration results were obtained as shown in Table 1 below.

[0091] Table 1 Camera calibration results and errors

[0092] Verification Points (x, y) / m (X, Y) / m RMSE / m 1 (-4.6877，13.7562) (-4.6252，13.8095) 0.0821 2 (-4.6871，17.7386) (-4.6701，17.7858) 0.0502 3 (-4.6956，21.7294) (-4.7343，21.8129) 0.0920 4 (-4.6931，25.7244) (-4.7514，25.7704) 0.0743 5 (-4.6850，29.7122) (-4.6587，29.8162) 0.1073 6 (-4.6890，33.6920) (-4.6748，33.7443) 0.0542 7 (-4.6816，37.6813) (-4.7070，37.7771) 0.0991 8 (-4.6688，41.6823) (-4.7206，41.6382) 0.0680 9 (3.3971，15.1215) (3.46474，15.0036) 0.1359 10 (3.4010，19.0658) (3.43212，18.9978) 0.0748 11 (3.4025，25.1088) (3.37491，25.0121) 0.1006 12 (1.6378，29.4158) (1.7041，29.3757) 0.0775 13 (1.6285，35.3751) (1.6458，35.3460) 0.0339 14 (1.6416，41.3380) (1.5990，41.2298) 0.1163 15 (3.4027，28.9251) (3.3542，29.0009) 0.0900

[0093] The table above shows the verification point coordinates (x, y) and calibration coordinates (X, Y). In order to evaluate the calibration accuracy, the root mean square error (RMSE) is used as the main evaluation indicator. The calculation formula of RMSE is as follows:

[0094]

[0095] Through formula (12), the RMSE of each verification point can be obtained, as shown in the table above. And the average RMSE of all verification points is 0.0837, which can meet the requirements of the calibration model.

[0096] Description of AprilTag test results:

[0097] The original video data used in the experiment was captured by a 5F08-M high-speed camera. The sampling frequency of the camera was set to 200 Hz, and the image resolution was 4096 × 2048 pixels. Four segments of vehicle motion videos at different speeds along straight lines and curves were used to identify and detect vehicles. The total number of frames in the four segments was 640, 540, 907, and 1272, respectively.

[0098] During the video recognition process, 585 frames were successfully identified in the first video, with a corresponding detection accuracy of 91.40%; 501 frames were successfully identified in the second video, with a detection accuracy of 92.77%; 816 frames were successfully identified in the third video, with a detection accuracy of 89.96%; and 1216 frames were successfully identified in the fourth video, with a detection accuracy of 95.59%. Fig.10 Figure 2 shows the vehicle detection process of the AprilTag algorithm applied to two of the videos.

[0099] Description of vehicle motion trajectory correction result analysis:

[0100] In order to verify the effectiveness of the calibration model proposed in this embodiment in optimizing the positioning accuracy of UWB vehicle motion trajectory, high-speed cameras and UWB positioning were used to collect vehicle positioning data of straight and curved motions at different speeds, and the obtained motion trajectories were analyzed.

[0101] Since the KF algorithm is used for correction and prediction, the state transfer function needs to be set in advance to obtain the prior estimate However, the vehicle motion is affected by the environment, so the motion state function is difficult to obtain or the function is inaccurate, which affects the final result. Therefore, this embodiment uses a bidirectional long short-term memory network to learn the target state transfer function and related parameters of the sequence data, so as to obtain the prior estimation value required for the KF algorithm correction prediction The training and prediction process of the moving target state transfer function is as follows: Figure 11-12 As shown, Fig.11 and Fig.12 They are for different data columns. The purple, gray, and blue balls in the figure represent the prior estimates obtained by training the BILSTM network. ; The green, red and yellow balls represent the In order to show the coordinates more clearly, the x and y coordinates are shown separately.

[0102] In the trajectory correction and prediction model of this embodiment, the BILSTM-KF algorithm is used for data fusion and correction. In the specific operation, the UWB positioning estimation value at time t+1 is converted into As the prior value of the Kalman filter, and based on the camera observation value at the current time t, the UWB positioning data at time t+1 is predicted and corrected. Figure 13-Figure 16 The corrected UWB positioning data, UWB positioning raw data, and the correction reference value observed by the camera are displayed for different data columns. Figure 13-Figure 14 The data point sequence in the figure represents the trajectory of the vehicle traveling along a straight line at different speeds. Figure 15-16 The data point sequence in the figure represents the motion trajectory of the vehicle traveling along the curve at different speeds. The green mark represents the x and y coordinate values ​​in the motion trajectory after UWB positioning correction; the blue mark represents the x and y coordinates of the UWB correction reference value obtained through camera synchronization; the black mark shows the original coordinate data of UWB positioning.

[0103] from Figure 13-Figure 16 It can be clearly seen that the stability of the UWB positioning data after correction has been significantly improved. Compared with before correction, the motion trajectory is smoother and the volatility is significantly reduced. This shows that the method of integrating high-speed camera and UWB positioning using the UWB calibration model and BILSTM-KF algorithm of this embodiment can effectively reduce the noise and error in UWB positioning data, thereby improving the positioning accuracy and the smoothness of the vehicle motion trajectory. Figure 13-Figure 16 The data of the four trajectories are calculated using formula (12). The RMSE between each corrected UWB positioning data and the reference value are 0.0178m, 0.0127m, 0.0110m and 0.0171m respectively. The RMSE results of each trajectory after correction are shown in Table 2.

[0104] Table 2. Error between UWB positioning and camera reference value after correction

[0105] Movement trajectory RMSEx(m) RMSEy(m) RMSE(m) 1 0.0128 0.0227 0.0178 2 0.0100 0.0154 0.0127 3 0.0070 0.0149 0.0110 4 0.0169 0.0173 0.0171

[0106] In addition, further experimental analysis shows that the calibration model proposed in this embodiment shows good robustness and adaptability under different motion modes and environmental conditions. Through repeated experimental verification, the method of this embodiment can maintain high positioning accuracy in complex environments, indicating that it has wide applicability and potential advantages.

[0107] This embodiment proposes a vehicle motion trajectory calibration model for UWB positioning that integrates high-speed imaging technology, introduces an improved Kalman filter algorithm, and integrates a bidirectional long short-term memory network to correct and predict the vehicle motion trajectory. By using the dynamic position data of the vehicle identified by the high-speed camera as a benchmark, this embodiment successfully realizes the dynamic correction of UWB positioning data, improves positioning accuracy, significantly reduces noise and errors, and makes the vehicle motion trajectory smoother. The experimental results verify the effectiveness of the model and algorithm, showing its potential in high-precision positioning.

[0108] This study has important theoretical and practical significance. In theory, it expands the application scope of the traditional Kalman filter algorithm, enabling it to more effectively handle nonlinear motion models, thereby improving the accuracy of UWB positioning. In practice, the calibration model provided by this study can be applied to vehicle positioning systems in occluded environments, and has broad application prospects, such as intelligent transportation systems, autonomous driving, and unmanned driving.

[0109] The UWB positioning vehicle trajectory calibration model and BILSTM-KF algorithm proposed in this embodiment provide new ideas and methods for the development of high-precision positioning technology. Subsequent work can verify the effectiveness of this method in a wider range of application scenarios and explore more efficient calculation methods to meet the real-time requirements in practical applications. At the same time, it is possible to consider combining this technology with other advanced positioning and perception technologies to provide a more comprehensive solution for the precise positioning and trajectory prediction of vehicles.

[0110] See also Fig.17 An embodiment of the present invention provides a moving vehicle positioning device, which is used to execute the moving vehicle positioning method provided by the above embodiment, and includes: a first acquisition module 201, an observation value acquisition module 202, a second acquisition module 203, a priori estimation value acquisition module 204, a Kalman gain acquisition module 205 and a correction value acquisition module 206.

[0111] The first acquisition module 201 is used to acquire the motion video information of the vehicle. The observation value acquisition module 202 is used to obtain the camera observation value z of the vehicle at the current moment according to the motion video information of the vehicle. t , camera observation value z t The second acquisition module 203 is used to obtain the correction value x at the previous moment. t-1and the correction value x at the previous moment t-1 The covariance matrix P t-1 The prior estimation value obtaining module 204 is used to obtain the prior estimation value based on the pre-trained bidirectional long short-term memory network model and the previous moment correction value x t-1 , get the prior estimate at the current time The bidirectional long short-term memory network model is used to correct the value x according to the previous moment t-1 Predict the current time prior estimate The Kalman gain obtaining module 205 is used to obtain the Kalman gain according to the correction value x at the previous moment. t-1 The covariance matrix P t-1 Get the current Kalman gain K t The correction value obtaining module 206 is used to obtain the current prior estimation value based on the Kalman filter algorithm. The camera observation value z at the current moment t and the current Kalman gain K t Get the current correction value x t , where the initial correction value x0 is the coordinate information of the vehicle at the initial moment obtained based on the ultra-wideband UWB positioning technology.

[0112] Optionally, the Kalman gain obtaining module 205 is specifically used for:

[0113] Get the correction value x at the previous moment t-1 The covariance matrix P t-1 ; According to the correction value x at the previous moment t-1 The covariance matrix P t-1 and the Jacobian matrix F t-1 Get the current prior estimate The covariance matrix of According to the covariance matrix and the current camera observation value z t The observation matrix H t Get the current Kalman gain K t .

[0114] Optionally, the correction value obtaining module 206 is specifically used for:

[0115] According to the current Kalman gain K t 、The current camera observation value z t The observation matrix H t and the current prior estimate Get the intermediate value; based on the intermediate value and the prior estimate at the current moment Get the current correction value x t .

[0116] Optionally, the second acquisition module 203 acquires the correction value x at the previous moment t-1 The covariance matrix P t-1 When, specifically used for:

[0117] According to the identity matrix, the Kalman gain K at the previous moment t-1 , the camera observation value z at the previous moment t-1 The observation matrix H t-1 , the prior estimate at the previous moment The covariance matrix of Get the correction value x at the previous moment t-1 The covariance matrix P t-1 .

[0118] Optionally, the vehicle is equipped with a visual positioning marker, and the observation value obtaining module 202 is specifically used for:

[0119] The visual positioning markers in the vehicle's motion video information are identified, detected and positioned to obtain the vehicle's current camera measurement value; the current camera measurement value is converted into the vehicle's current camera observation value z in world coordinates using the single vanishing point camera calibration method. t .

[0120] Optionally, the visual positioning tag is an AprilTag tag.

[0121] Optionally, the bidirectional long short-term memory network model includes: an input layer, a first bidirectional long short-term memory network layer, a first random inactivation layer, a second bidirectional long short-term memory network layer, a second random inactivation layer, a fully connected layer and an output layer connected in sequence.

[0122] It should be noted that: the moving vehicle positioning device provided in the above embodiment only uses the division of the above functional modules as an example to illustrate when positioning the moving vehicle. In actual applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device is divided into different functional modules to complete all or part of the functions described above. In addition, the moving vehicle positioning device provided in the above embodiment and the moving vehicle positioning method embodiment belong to the same concept, and the specific implementation process is detailed in the method embodiment, which will not be repeated here.

[0123] An embodiment of the present invention provides a moving vehicle positioning system, which includes: a vehicle motion measurement subsystem and a moving vehicle positioning device.

[0124] Among them, the vehicle motion measurement subsystem has a visual positioning marker, a camera and a UWB positioning device. The visual positioning marker is installed on the vehicle, the camera is set up above the road on which the vehicle is traveling, and the UWB positioning device is used to locate the vehicle based on the UWB positioning technology to obtain coordinate information. The moving vehicle positioning device is the above-mentioned moving vehicle positioning device.

[0125] That is to say, the system is mainly divided into two parts: vehicle motion measurement system and UWB calibration system, such as Figure 1 As shown in the figure. The measurement system consists of a high-speed camera and a UWB positioning base station, which are used to synchronously collect the vehicle's motion state information. The UWB mainly collects the vehicle's motion coordinate information, and the high-speed camera collects the vehicle's motion video information. The camera resolution is 4096x2048 (4K resolution), and it can reach up to 500 frames per second (FPS) at full resolution. The main function of the UWB correction system is to correct the motion coordinate information output by the UWB using the proposed BILSTM-KF algorithm. At the same time, the collected vehicle motion video information is identified and positioned by the AprilTag detection algorithm, and then converted into vehicle coordinate information in world coordinates using the single vanishing point camera calibration method.

[0126] An embodiment of the present invention provides an electronic device, which includes: a memory and a processor. The processor is connected to the memory and is configured to execute the above-mentioned moving vehicle positioning method based on the instructions stored in the memory. The number of processors can be one or more, and the processor can be single-core or multi-core. The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip. The memory can be an example of the following computer-readable medium.

[0127] An embodiment of the present invention provides a computer-readable storage medium, on which at least one instruction, at least one program, code set or instruction set is stored, and the at least one instruction, at least one program, code set or instruction set is loaded and executed by a processor to implement the above-mentioned moving vehicle positioning method. Computer-readable storage media include: permanent and non-permanent, removable and non-removable media can implement information storage by any method or technology. Information can be a computer-readable instruction, a data structure, a module of a program or other data. Examples of computer storage media include, but are not limited to: phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, read-only compact disk-read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassette, disk storage or other magnetic storage device or any other non-transmission medium, which can be used to store information that can be accessed by a computing device.

[0128] It is known from common technical knowledge that the present invention can be implemented by other embodiments that do not deviate from its spirit or essential features. Therefore, the above disclosed embodiments are only illustrative in all respects and are not exclusive. All changes within the scope of the present invention or within the scope equivalent to the present invention are included in the present invention.

Claims

1. A moving vehicle positioning method, characterized in that: The method comprises: Obtaining motion video information of the vehicle; The camera observation value z of the vehicle at the current moment is obtained according to the motion video information of the vehicle t , the camera observation value z t Indicates first coordinate information of the vehicle obtained based on the motion video information; Get the correction value x at the previous moment t-1 and the correction value x at the previous moment t-1 The covariance matrix P t-1 ; According to the pre-trained bidirectional long short-term memory network model and the previous moment correction value x t-1 , get the prior estimate at the current time The bidirectional long short-term memory network model is used to correct the value x at the previous moment according to the t-1 Predict the a priori estimate of the current moment According to the correction value x at the previous moment t-1 The covariance matrix P t-1 Get the current Kalman gain K t ; Based on the Kalman filter algorithm, according to the current a priori estimate The current camera observation value z t and the current Kalman gain K t Get the current correction value x t ; The initial moment correction value x0 is the second coordinate information of the vehicle at the initial moment obtained based on the ultra-wideband UWB positioning technology.

2. The method according to claim 1, characterized in that The prior estimate based on the previous moment The covariance matrix of the current moment Kalman gain K is obtained t include: Get the correction value x of the previous moment t-1 The covariance matrix P t-1 ; According to the correction value x at the previous moment t-1 The covariance matrix P t-1 and the Jacobian matrix F t-1 Get the current prior estimate The covariance matrix of According to the covariance matrix and the current camera observation value z t The observation matrix H t Get the current Kalman gain K t .

3. The method according to claim 1, characterized in that The Kalman filter algorithm is based on the a priori estimate value at the current moment. The current camera observation value z t and the current Kalman gain K t Get the current correction value x t include: According to the current Kalman gain K t , the camera observation value z at the current moment t The observation matrix H t and the current prior estimate Get the middle value; According to the intermediate value and the prior estimation value at the current moment Get the current correction value x t .

4. The method according to claim 2, characterized in that: The acquisition of the previous moment correction value x t-1 The covariance matrix P t-1 include: According to the identity matrix, the Kalman gain K at the previous moment t-1 , the camera observation value z at the previous moment t-1 The observation matrix H t-1 , the prior estimate at the previous moment The covariance matrix of Get the correction value x at the previous moment t-1 The covariance matrix P t-1 .

5. The method according to claim 1, characterized in that The vehicle is equipped with a visual positioning marker, and the camera observation value z of the vehicle at the current moment is obtained according to the motion video information of the vehicle t include: Identify, detect and locate the visual positioning mark in the motion video information of the vehicle to obtain the camera measurement value of the vehicle at the current moment; The single vanishing point camera calibration method is used to convert the current camera measurement value into the current camera observation value z of the vehicle in world coordinates. t .

6. The method according to claim 5, characterized in that The visual positioning mark is an AprilTag mark.

7. The method according to claim 1, characterized in that The bidirectional long short-term memory network model includes: an input layer, a first bidirectional long short-term memory network layer, a first random inactivation layer, a second bidirectional long short-term memory network layer, a second random inactivation layer, a fully connected layer and an output layer connected in sequence.

8. A moving vehicle positioning device, characterized in that: The device comprises: A first acquisition module is used to acquire motion video information of the vehicle; The observation value obtaining module is used to obtain the camera observation value z of the vehicle at the current moment according to the motion video information of the vehicle. t , the camera observation value z t represents the first coordinate information of the vehicle at an initial moment obtained based on the motion video information; The second acquisition module is used to obtain the correction value x at the previous moment t-1 and the correction value x at the previous moment t-1 The covariance matrix P t-1 ; The module for obtaining the prior estimation value is used to obtain the prior estimation value based on the pre-trained bidirectional long short-term memory network model and the correction value x at the previous moment. t-1 , get the prior estimate at the current time The bidirectional long short-term memory network model is used to correct the value x at the previous moment according to the t-1 Predict the a priori estimate of the current moment Kalman gain module is used to correct the value x according to the previous moment t-1 The covariance matrix P t-1 Get the current Kalman gain K t ; The correction value obtaining module is used to obtain the prior estimation value at the current moment based on the Kalman filter algorithm. The current camera observation value z t and the current Kalman gain K t Get the current correction value x t ; The initial moment correction value x0 is the second coordinate information of the vehicle at the initial moment obtained based on the ultra-wideband UWB positioning technology.

9. A moving vehicle positioning system, characterized in that: The system comprises: A vehicle motion measurement subsystem, the vehicle motion measurement subsystem comprising a visual positioning mark, a camera and a UWB positioning device, the visual positioning mark being installed on the vehicle, the camera being mounted above the road on which the vehicle is traveling, and the UWB positioning device being used to position the vehicle based on the UWB positioning technology to obtain second coordinate information; The moving vehicle positioning device is the moving vehicle positioning device described in claim 8.

10. A computer program product comprising instructions, characterized in that When the computer program product is run on a computer, the method according to any one of claims 1 to 7 is executed by the computer.

Images