A positioning and tracking method and device combining optical and inertial fusion

Abstract

The present invention provides a positioning and tracking method and device for optical and inertial fusion, and the method includes: A. setting a fusion positioning module and an inertial positioning module; B. calculating a frequency enhancement parameter K; C. deploying the fusion positioning module and the trained inertial positioning module; D. if there are both optical data and inertial data, proceeding to step E; if there are only inertial data, proceeding to step F; E. taking the optical data and the inertial data as inputs of a first filter, proceeding to step D; F. if there is no optical data at the previous K+1 moments, proceeding to step G, otherwise proceeding to step H; G. taking the inertial data as inputs of the first and second neural networks to obtain a process noise matrix Q t and the device noise matrix R t ; H. Using the inertial data as the input of the second filter, and sequentially inputting the inertial data of the inertial acquisition module in this cycle into the second filter. The present invention can locate and track targets with high precision and high reliability, and has strong adaptability and robustness.

Description

Technical Field

[0001] The invention relates to a positioning and tracking method and a device integrating optics and inertia. Background Art

[0002] In general surgery, doctors understand the location and structure of the lesion based on the images of the affected part obtained before the operation, and perform clinical surgery based on memory and experience. The success of the operation depends on the personal experience and professional skills of the clinician. The various parameters in the operation cannot be quantified, which can easily cause positioning deviations, and the operation results often fail to reach the ideal state.

[0003] The stereotactic positioning system is the key to computer-assisted surgical navigation technology. The three-dimensional positioning system combines medical images, surgical area and surgical instrument calibration to determine the spatial position of the surgical target, thereby obtaining the target's posture and position in its three-dimensional coordinate system in real time. The current methods for achieving surgical navigation positioning include mechanical positioning, ultrasonic positioning, optical positioning and electromagnetic positioning.

[0004] Optical tracking technology based on multiple cameras has been widely used in medical robots, virtual reality and industrial measurement due to its good positioning accuracy. Among them, optical sensing and inertial sensing are basic sensing technologies. The two technologies have unique advantages and inevitable defects respectively. Optical sensing has a large measurement range and the best accuracy, but it is limited by the line of sight problem and cannot collect data when the target is blocked. Inertial sensing is independent of its workspace, but it only has relatively stable measurement output in the attitude direction. In view of the shortcomings of each single mode, the fusion of optical and inertial sensing is widely used due to their complementarity and the low cost and low power consumption of inertial sensing. The automatic heading reference system (AHRS) includes a three-axis gyroscope and a three-axis accelerometer, which has a more stable direction and linear acceleration output. With the great success of MEMS (micro-electromechanical system) technology, the micro MEMS AHRS can provide a supplement to the optical tracking system in attitude measurement with the advantage of direction tracking performance.

[0005] However, although this type of optical and inertial fusion target tracking measurement technology is widely used in industrial measurement and industrial automation, its actual application in the field of medical robots still faces some other challenges, such as low optical measurement frequency, unsatisfactory tracking and positioning continuity, optical measurement requires a large number of optical markers, and interference from inertial device noise and measurement noise. Summary of the invention

[0006] The present invention provides a positioning and tracking method and device integrating optics and inertia, which can locate and track targets with high precision and reliability and have strong adaptability and robustness.

[0007] The present invention is achieved through the following technical solutions:

[0008] A positioning and tracking method combining optics and inertia comprises the following steps:

[0009] A. Set up the fusion positioning module, inertial positioning module, optical acquisition module and inertial acquisition module:

[0010] The fusion positioning module includes a cascaded first filter and a second filter, the inertial positioning module includes a first neural network, a second neural network, and a third filter whose input end is connected to the output ends of the first and second neural networks respectively, the first, second and third filters are all unscented Kalman filters, and the first and second neural networks are used to estimate process noise and equipment noise respectively;

[0011] The acquisition frequency f2 of the inertial acquisition module is an integer multiple of the acquisition frequency f1 of the optical acquisition module;

[0012] B. Calculation of frequency enhancement parameters And K is used as the prediction number of the second filter;

[0013] C. Simulate the situation where the target is blocked, train the first and second neural networks, and deploy the fusion positioning module and the trained inertial positioning module on the server;

[0014] D. If at the current time t, the optical acquisition module and the inertial acquisition module simultaneously acquire optical data and inertial data, then proceed to step E; if at the current time t only the inertial acquisition module acquires inertial data, then proceed to step F;

[0015] E. Using the optical data and inertial data at the current time t as the input of the first filter, obtaining the motion state of the target at the current time t and the corresponding covariance matrix, and taking the time t+1 as the current time, and entering step D;

[0016] F. If there is no optical data collected by the optical collection module at K+1 moments before the current moment t, then proceed to step G; otherwise, proceed to step H;

[0017] G. Use the inertial data collected by the inertial acquisition module as the input of the first and second neural networks to obtain the process noise matrix Q t and the device noise matrix R t , and use these two matrices as the input of the third filter, and finally obtain the motion state of the target at the current time t and the corresponding covariance matrix;

[0018] H. Use the inertial data at the current time t as the input of the second filter to obtain the motion state of the target and the corresponding covariance matrix at the current time t, and use the time t+1 as the current time to enter step D.

[0019] Furthermore, the optical data includes three-dimensional position data M of the target t ; The inertial data includes the three-axis linear acceleration A of the target t .

[0020] Furthermore, the calculation process of forward transmission of data in the first neural network is:

[0021] Y t = SeLU(W m×h SeLU(W h×3 A t +B h×1 )+B m×1 );

[0022] Z 1t =W 9×m Y m×1 +B 9×1 ;

[0023] Z 2t =W 9×m Y m×1 +B 9×1 ;

[0024] Z 3t =W 9×m Y m×1 +B 9×1 ;

[0025] Z t =diag([Z 1t ,Z 2t ,Z 3t ]);

[0026]

[0027] Among them, W a×b is the a×b weight parameter matrix of the fully connected layer in the first neural network, B a×b is the a×b bias matrix of the layer, Z 1t , Z 2t and Z 3t is the noise feature estimation corresponding to the motion state of the three axes respectively, and SeLU is the activation function.

[0028] Furthermore, the calculation process of forward data transfer in the second neural network is:

[0029] Y′ m×1 = SeLU(W′ m×h SeLU(W′ h×3 A t +B′ h×1 )+B′ m×1 );

[0030] [R1,R2,R3] t =W′ 3×m Y′ m×1 +B′ 3×1 ;

[0031] R t =abs(diag([R1,R2,R3] t ));

[0032] Among them, W′ a×b is the a×b weight parameter matrix of the fully connected layer in the second neural network, B′ a×b is the a×b bias matrix of this layer, R1, R2 and R3 are the noise feature estimates corresponding to the motion states of the three axes respectively, and SeLU is the activation function.

[0033] Furthermore, the output end of the third filter is also connected to the input end of the third neural network, and the motion state of the target output by the third filter at the current time t is reconstructed after passing through the third neural network to obtain the displacement S″ at the current time t. t :

[0034] S″ t =W″ 3×m ReLU(W″ m×h ReLU(W″ h×3 S′ t )+B″ m×1 )+B″ 3×1 ,

[0035] Among them, W″ a×b is the a×b weight parameter matrix of the fully connected layer in the third neural network, B″ a×b is the a×b bias matrix of the layer, s′ t The motion state X output by the third filter t Contains three-axis displacement, ReLU is the activation function.

[0036] Furthermore, the step C specifically includes the following steps:

[0037] C1. In the absence of occlusion, the optical acquisition module and the inertial acquisition module respectively collect optical data and inertial data of the randomly moving target to establish a training data set;

[0038] C2, taking the optical data as the true value and the inertial data as the input value of the first and second neural networks of the inertial positioning module, and training the first and second neural networks in the case of simulating the target being blocked;

[0039] C3. Deploy the fusion positioning module and the trained inertial positioning module on the server.

[0040] Furthermore, the optical acquisition module includes an optical camera; the inertial acquisition module includes an optical marker point and an automatic heading reference system.

[0041] Furthermore, before the optical acquisition module and the inertial acquisition module start to acquire data, the two need to be synchronized.

[0042] The present invention is also achieved through the following technical solutions:

[0043] A positioning and tracking device integrating optical and inertial sensing comprises a server, an optical acquisition module and an inertial acquisition module respectively connected to the server, and a fusion positioning module and an inertial positioning module deployed on the server, wherein the optical acquisition module acquires optical data, the inertial acquisition module acquires inertial data, the acquisition frequency of the inertial acquisition module is greater than that of the optical acquisition module, the fusion positioning module comprises a cascaded first filter and a second filter, the inertial positioning module comprises a first neural network, a second neural network, and a third filter whose input end is respectively connected to the output ends of the first and second neural networks, the first, second and third filters are all unscented Kalman filters, the first and second neural networks are respectively used to estimate process noise and equipment noise, when the server determines that a target is not blocked, the optical data and part of the inertial data are input into the first filter, and the remaining inertial data are sequentially input into the second filter, otherwise, the inertial data are respectively input into the first and second neural networks to estimate process noise and equipment noise.

[0044] Furthermore, the optical data includes three-dimensional position data of the target; and the inertial data includes three-axis linear acceleration of the target.

[0045] The present invention has the following beneficial effects:

[0046] 1. When the target is not blocked, the fusion positioning module is selected for positioning and tracking. If the target is blocked, the inertial positioning module is selected for positioning and tracking. The fusion positioning module includes two cascaded unscented Kalman filters. In addition to an unscented Kalman filter, the inertial positioning module is also provided with a first neural network and a second neural network. The first neural network and the second neural network are used to estimate process noise and equipment noise respectively. Because the acquisition frequency of the inertial acquisition module is an integer multiple of that of the optical acquisition module, when the target is not blocked, in one cycle of the optical acquisition module, the inertial data is more than the optical data. The optical data and inertial data collected at the same time are respectively input into the first unscented Kalman filter for data fusion estimation, and then the remaining inertial data are sequentially input into the second unscented Kalman filter for frequency enhancement, so that the continuity of positioning and tracking is improved. In this way, it is still possible to accurately locate the target moving at high speed. Because the second unscented Kalman filter also takes the motion state of the target at the previous moment and the covariance matrix corresponding to the motion state as input, the frequency enhancement process also makes the change of the covariance matrix continuous, thereby effectively improving the effect of positioning and tracking. When the target is blocked, accurate positioning cannot be performed only by inertial data. This is because there is interference and integral error of the noise of the inertial acquisition module, and the noise of the inertial acquisition module is random noise (although its noise is controllable in a stable state, its noise parameters are no longer predictable when in motion, especially when in variable acceleration motion). Therefore, the unscented Kalman filter method using preset noise parameters in the prior art is defective and will cause serious trajectory deviation. In the present invention, when the target is blocked, the first neural network and the second neural network are used to estimate the process noise and the equipment noise respectively, and the real-time dynamics of the noise are estimated, thereby optimizing the performance of the Kalman unscented filter of the inertial acquisition module, effectively avoiding the interference of noise, and realizing that even if the target is blocked, it can be continuously located and tracked. In this way, the present invention can locate and track the target with high precision and high reliability, and has strong adaptability and robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The present invention is further described in detail below with reference to the accompanying drawings.

[0048] Figure 1 Flow chart of the method of the present invention.

[0049] Figure 2 Schematic diagram of the principle of the fusion positioning module.

[0050] Figure 3 This is a schematic diagram of the inertial positioning module.

[0051] Figure 4 Schematic diagram of the network structure of the first neural network.

[0052] Figure 5Schematic diagram of the network structure of the second neural network.

[0053] Figure 6 It is a schematic diagram of the structure of the device of the present invention.

[0054] Among them, 1. Optical acquisition module; 11. Optical camera; 12. Power supply and communication controller; 13. Bracket; 2. Inertial acquisition module; 21. Optical marker point; 22. Automatic heading reference system; 3. Server. DETAILED DESCRIPTION

[0055] like Figure 1 As shown, the positioning and tracking method of optical and inertial fusion includes the following steps:

[0056] A. Set up the fusion positioning module, inertial positioning module, optical acquisition module 1 and inertial acquisition module 2:

[0057] like Figure 2 and Figure 3 As shown, the fusion positioning module includes a cascaded first filter UKF1 and a second filter UKF2, the inertial positioning module includes a first neural network, a second neural network, a third filter UKF3 whose input end is respectively connected to the output ends of the first and second neural networks, and a third neural network whose input end is connected to the output end of the third filter UKF3, the first filter UKF1, the second filter UKF2, and the third filter UKF3 are all unscented Kalman filters, the first and second neural networks are used to estimate process noise and equipment noise, respectively, and the third neural network is used to reconstruct the output displacement;

[0058] like Figure 6 As shown, the optical acquisition module 1 includes a bracket 13, four optical cameras 11 arranged at intervals on the bracket 13, and a power supply and a communication controller 12 arranged on the bracket 13 and respectively connected to the four optical cameras 11. The inertial acquisition module 2 includes an automatic heading reference system 22 and an optical marker 21, and the optical marker 21 is an infrared LED arranged on the automatic heading reference system 22. The communication controller and the automatic heading reference system 22 are wirelessly connected to the server 3 respectively; the communication controller 12 is a prior art; and the setting structure of the optical marker 21 on the automatic heading reference system 22 is a prior art.

[0059] The acquisition frequency f1 of the inertial acquisition module 2 is an integer multiple of the acquisition frequency f1 of the optical acquisition module 1; in this embodiment, f1 = 100 Hz, f2 = 25 Hz;

[0060] B. Calculation of frequency enhancement parameters And K is used as the predicted number of times of the second filter UKF2, which is the number of times the second filter UKF2 inputs data in one acquisition cycle of the optical acquisition module;

[0061] The optical acquisition module 1 and the inertial acquisition module 2 coordinate system calibration calculation is performed to convert the output data of the inertial acquisition module 2 into the optical acquisition module 1 coordinate system. This process is a prior art.

[0062] C. Simulate the situation where the target is blocked, train the first and second neural networks, and deploy the fusion positioning module and the trained inertial positioning module on the server 3; more specifically, the steps are:

[0063] C1, synchronize the optical acquisition module 1 and the inertial acquisition module 2. In the absence of occlusion, the optical acquisition module 1 and the inertial acquisition module 2 respectively collect the optical data and the inertial data of the randomly moving target to establish a training data set;

[0064] C2, taking the optical data as the true value and the inertial data as the input value of the first and second neural networks of the inertial positioning module, and training the first and second neural networks in the case of simulating the target being blocked;

[0065] C3, deploy the fusion positioning module and the trained inertial positioning module on server 3;

[0066] The optical data includes the three-dimensional position data M of the target. t , inertial data includes the three-axis linear acceleration A of the target t , which will be referred to as M in the following text t Refers to optical data, with A t Refers to inertial data;

[0067] D. If at the current time t, the optical acquisition module 1 and the inertial acquisition module 2 simultaneously collect optical data M t and inertial data A t , then go to step E; if at the current time t only the inertial acquisition module 2 collects the inertial data A t , then go to step F;

[0068] E. The optical data M at the current time t t , Inertial data A t , the target's motion state at the previous moment X t-1 and motion state X t-1 The corresponding covariance matrix P t-1 All of them are used as the input of the first filter UKF1 to obtain the motion state X of the target at the current time t t And the corresponding covariance matrix P t , the motion state X t Including the three-axis displacement S of the target t , velocity and acceleration, and take time t+1 as the current time, and proceed to step D;

[0069] F. If there is no optical data M collected by the optical collection module 1 at K+1 moments before the current moment t t , which means the target is blocked, then go to step G, otherwise, go to step H;

[0070] G. The inertial data A collected by the inertial acquisition module 2 t As the input of the first and second neural networks respectively, to obtain the process noise matrix Q t and the device noise matrix R t , and these two matrices, as well as the target's previous motion state X t-1 and motion state X t-1 The corresponding covariance matrix P t-1 Together they serve as the input of the third filter UKF3, and the output of the third filter UKF3 includes the motion state X of the target at the current time t t And the corresponding covariance matrix P t , from the motion state X t The obtained three-axis displacement S t is the output of the inertial positioning module; t The three-axis displacement S is obtained t The process is prior art;

[0071] The first neural network is based on the inertial data A t (i.e., three-axis linear acceleration) is used for feature extraction, a diagonal matrix is ​​constructed, and a semi-positive definite process noise matrix Q is constructed based on the diagonal matrix and its transposed matrix t ;

[0072] like Figure 4 As shown, the calculation process of data forward transmission in the first neural network is:

[0073] Y t = SeLU(W m×h SeLU(W h×3 A t +B h×1 )+B m×1 );

[0074] Z 1t =W 9×m Y m×1 +B 9×1 ;

[0075] Z 2t =W 9×m Y m×1 +B 9×1 ;

[0076] Z 3t =W 9×m Ym×1 +B 9×1 ;

[0077] Z t =diag([Z 1t ,Z 2t ,Z 3t ]);

[0078]

[0079] Among them, Y t is the inter-layer output of the first neural network, W a×b is the a×b weight parameter matrix of the fully connected layer in the first neural network, B a×b is the a×b bias matrix of the layer, Z 1t , Z 2t and Z 3t is the noise feature estimation corresponding to the motion state of the three axes respectively, and SeLU is the activation function;

[0080] The second neural network is used to calculate the inertial data A t (i.e. three-axis linear acceleration) to extract the features directly to estimate the equipment noise and obtain the equipment noise matrix R t , limit the values ​​in the matrix to be greater than 0;

[0081] like Figure 5 As shown, the calculation process of data forward transmission in the second neural network is:

[0082] Y′ m×1 = SeLU(W′ m×h SeLU(W′ h×3 A t +B′ h×1 )+B′ m×1 );

[0083] [R1,R2,R3] t =W′ 3×m Y′ m×1 +B′ 3×1 ;

[0084] R t =abs(diag([R1,R2,R3] t ));

[0085] Among them, Y′ m×1 is the inter-layer output of the second neural network, W′ a×b is the a×b weight parameter matrix of the fully connected layer in the second neural network, B′ a×bis the a×b bias matrix of the layer, R1, R2 and R3 are the noise feature estimates corresponding to the motion states of the three axes respectively, and SeLU is the activation function;

[0086] The motion state of t at the current moment output by the third filter UKF3 is reconstructed after passing through the third neural network to obtain the displacement S at the current moment t t :

[0087] S t =W″ 3×m ReLU(W″ m×h ReLU(W″ h×3 S′ t )+B″ m×1 )+B″ 3×1

[0088] Among them, W″ a×b is the a×b weight parameter matrix of the fully connected layer in the third neural network, B″ a×b is the a×b bias matrix of the layer, S′ t is the motion state X based on the output of the third filter t The three-axis displacement, ReLU is the activation function;

[0089] H. The inertial data A at the current time t t , the target's motion state at the previous moment X t-1 and motion state X t-1 The corresponding covariance matrix P t-1 As the input of the second filter UKF2, the motion state X of the target at the current time t is obtained. t And the corresponding covariance matrix P t , and take time t+1 as the current time and go to step D.

[0090] like Figure 2 As shown, in this embodiment, K=3, the optical data M at the current time t t , Inertial data A t , the motion state X at the previous time t-1 t-1 And the covariance matrix P t-1 As the input of the first filter UKF1, the motion state X at the current time t is obtained. t And the covariance matrix P t , motion state X t Including three-axis displacement S t Next, at time t+1, only the inertial data A t+1 , then the inertial data A t+1 , the motion state X at the moment before time t+1 t And the covariance matrix P tAs the input of the second filter UKF2, the motion state X at time t+1 is obtained. t+1 And the covariance matrix P t+1 , until time t+3, the difference between this time and the previous time t when both optical data and inertial data were available is 3, which is K. The three-axis displacement S corresponding to time t is t This is the output of the fusion positioning module. At time t+4, it enters the next cycle and repeats the above process.

[0091] The positioning and tracking device of optical and inertial fusion includes a server 3, an optical acquisition module 1 and an inertial acquisition module 2 respectively connected to the server 3, and a fusion positioning module and an inertial positioning module deployed on the server 3. The optical acquisition module 1 collects optical data, and the inertial acquisition module 2 collects inertial data. The acquisition frequency of the inertial acquisition module 2 is greater than that of the optical acquisition module 1. The fusion positioning module includes a cascaded first filter UKF1 and a second filter UKF2. The inertial positioning module includes a first neural network, a second neural network, and a third filter UKF3 whose input end is respectively connected to the output ends of the first and second neural networks. The first filter UKF1, the second filter UKF2, and the third filter UKF3 are all unscented Kalman filters. The first and second neural networks are respectively used to estimate process noise and equipment noise. When the server 3 determines that the target is not blocked, the optical data and part of the inertial data are input into the first filter UKF1, and the remaining inertial data are sequentially input into the second filter UKF2. Otherwise, the inertial data are respectively input into the first and second neural networks to estimate process noise and equipment noise.

[0092] The above description is only a preferred embodiment of the present invention, and therefore cannot be used to limit the scope of implementation of the present invention. That is, equivalent changes and modifications made according to the scope of the patent application of the present invention and the contents of the specification should still fall within the scope covered by the patent of the present invention.

Claims

1. A positioning and tracking method combining optical and inertial fusion, characterized in that: The steps include: A. Set up the fusion positioning module, inertial positioning module, optical acquisition module and inertial acquisition module: The fusion positioning module includes a cascaded first filter and a second filter, the inertial positioning module includes a first neural network, a second neural network, and a third filter whose input end is connected to the output ends of the first and second neural networks respectively, the first, second and third filters are all unscented Kalman filters, and the first and second neural networks are used to estimate process noise and equipment noise respectively; The acquisition frequency f2 of the inertial acquisition module is an integer multiple of the acquisition frequency f1 of the optical acquisition module; B. Calculation of frequency enhancement parameters And K is used as the prediction number of the second filter; C. Simulate the situation where the target is blocked, train the first and second neural networks, and deploy the fusion positioning module and the trained inertial positioning module on the server; D. If at the current time t, the optical acquisition module and the inertial acquisition module simultaneously acquire optical data and inertial data, then proceed to step E; if at the current time t only the inertial acquisition module acquires inertial data, then proceed to step F; E. Using the optical data and inertial data at the current time t as the input of the first filter, obtaining the motion state of the target at the current time t and the corresponding covariance matrix, and taking the time t+1 as the current time, and entering step D; F. If there is no optical data collected by the optical collection module at K+1 moments before the current moment t, then proceed to step G; otherwise, proceed to step H; G. Use the inertial data collected by the inertial acquisition module as the input of the first and second neural networks to obtain the process noise matrix Q t and the device noise matrix R t , and use these two matrices as the input of the third filter, and finally obtain the motion state of the target at the current time t and the corresponding covariance matrix; H. Use the inertial data at the current time t as the input of the second filter to obtain the motion state of the target and the corresponding covariance matrix at the current time t, and use the time t+1 as the current time to enter step D.

2. The optical and inertial fusion positioning and tracking method according to claim 1, characterized in that: The optical data includes three-dimensional position data M of the target t ; The inertial data includes the three-axis linear acceleration A of the target t .

3. The optical and inertial fusion positioning and tracking method according to claim 2, characterized in that: The calculation process of forward data transmission in the first neural network is: Y t =SeLU(W m×h SELU (W h×3 A t +B h×1 )+B m×1 ); Z 1t =W 9×m Y m×1 +B 9×1 ; Z 2t =W 9×m Y m×1 +B 9×1 ; Z 3t =W 9×m Y m×1 +B 9×1 ; WITH t =diag([Z 1t ,WITH 2t ,WITH 3t ]); Among them, W a×b is the a×b weight parameter matrix of the fully connected layer in the first neural network, B a×b is the a×b bias matrix of the layer, Z 1t , Z 2t and Z 3t is the noise feature estimation corresponding to the motion state of the three axes respectively, and SeLU is the activation function.

4. The optical and inertial fusion positioning and tracking method according to claim 2 or 3, characterized in that: The calculation process of forward data transmission in the second neural network is: Y′ m×1 =SeLU(W′ m×h SeLU(W′ h×3 A t +B′ h×1 )+B′ m×1 ); [R1,R2,R3] t =W′ 3×m Y′ m×1 +B′ 3×1 ; R t =abs(diag([R1,R2,R3] t )); Among them, W′ a×b is the a×b weight parameter matrix of the fully connected layer in the second neural network, B′ a×b is the a×b bias matrix of this layer, R1, R2 and R3 are the noise feature estimates corresponding to the motion states of the three axes respectively, and SeLU is the activation function.

5. The optical and inertial fusion positioning and tracking method according to claim 2 or 3, characterized in that: The output end of the third filter is also connected to the input end of the third neural network. The motion state of the target output by the third filter at the current time t is reconstructed after passing through the third neural network to obtain the displacement S at the current time t. t ″: S'' t =W'' 3×m ReLU(W'' m×h ReLU(W'' h×3 S t ′)+B′' m×1 )+B'' 3×1 , Among them, W'' a×b is the a×b weight parameter matrix of the fully connected layer in the third neural network, B'' a×b is the a×b bias matrix of the layer, s t ' is the motion state X output by the third filter t Contains three-axis displacement, ReLU is the activation function.

6. The optical and inertial fusion positioning and tracking method according to claim 1, 2 or 3, characterized in that: The step C specifically comprises the following steps: C1. In the absence of occlusion, the optical acquisition module and the inertial acquisition module respectively collect optical data and inertial data of the randomly moving target to establish a training data set; C2, taking the optical data as the true value and the inertial data as the input value of the first and second neural networks of the inertial positioning module, and training the first and second neural networks in the case of simulating the target being blocked; C3. Deploy the fusion positioning module and the trained inertial positioning module on the server.

7. The optical and inertial fusion positioning and tracking method according to claim 1, 2 or 3, characterized in that: The optical acquisition module includes an optical camera; the inertial acquisition module includes an optical marker and an automatic heading reference system.

8. The optical and inertial fusion positioning and tracking method according to claim 1, 2 or 3, characterized in that: Before the optical acquisition module and the inertial acquisition module start collecting, the two need to be synchronized.

9. An optical and inertial fusion positioning and tracking device, characterized in that: The invention comprises a server, an optical acquisition module and an inertial acquisition module respectively connected to the server, and a fusion positioning module and an inertial positioning module deployed on the server, wherein the optical acquisition module acquires optical data, the inertial acquisition module acquires inertial data, the acquisition frequency of the inertial acquisition module is greater than that of the optical acquisition module, the fusion positioning module comprises a cascaded first filter and a second filter, the inertial positioning module comprises a first neural network, a second neural network, and a third filter whose input end is respectively connected to the output ends of the first and second neural networks, the first, second and third filters are all unscented Kalman filters, the first and second neural networks are respectively used to estimate process noise and equipment noise, when the server determines that the target is not blocked, the optical data and part of the inertial data are input into the first filter, and the remaining inertial data are sequentially input into the second filter, otherwise, the inertial data are respectively input into the first and second neural networks to estimate process noise and equipment noise.

10. The optical and inertial fusion positioning and tracking device according to claim 9, characterized in that: The optical data includes three-dimensional position data M of the target t ; The inertial data includes the three-axis linear acceleration A of the target t .

Images