Inertial navigation method and device, electronic equipment and robot

Abstract

The invention provides an inertial navigation method and device, electronic equipment and a robot, and belongs to the technical field of navigation.The method comprises the steps that a current IMU set collected by an IMU sensor and an environment image which is collected by a camera and has a timestamp matching relation with the current IMU set are obtained; based on the time sequence relationship of the IMU data, performing estimation and error correction on the current IMU group to obtain a speed estimation value at the next moment; extracting image features of the environment image, fusing the image feature speed estimation values, and obtaining a prediction track of the target equipment at the next moment according to a fusion result; and controlling the motion of the target equipment according to the predicted trajectory. Therefore, through the time sequence relationship of the IMU data, the current IMU group is estimated and subjected to error correction, and the trajectory prediction is performed in combination with the environment image, the drift error is effectively reduced and even eliminated, and the accuracy of the predicted trajectory is improved, so that the navigation precision and stability of the target equipment in a complex environment are greatly improved.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to an inertial navigation method, device, electronic equipment and robot. Background Art

[0002] In the autonomous navigation technology of lawn mowing robots, most existing methods rely on a single positioning method, such as the Global Positioning System (GPS) and Inertial Measurement Unit (IMU). However, these methods have shown significant limitations in practical applications. Specifically, the positioning accuracy of GPS will drop significantly in areas where satellite signals are blocked, and IMU has the problem of cumulative errors. The monocular vision simultaneous localization and mapping (SLAM) technology performs poorly in scenes with weak textures or sparse feature points.

[0003] Therefore, a navigation method with high accuracy and robustness is urgently needed. Summary of the invention

[0004] In view of this, the object of the present invention is to provide an inertial navigation method, device, electronic device and robot, which correct the drift error of IMU data through time series analysis, and fuse IMU data and camera data for trajectory prediction, so as to realize autonomous navigation of the robot in complex environments with higher accuracy and higher robustness.

[0005] In order to achieve the above purpose, the technical solution adopted by the embodiment of the present invention is as follows:

[0006] In a first aspect, the present invention provides an inertial navigation method, which is applied to a controller of a target device, wherein the controller is communicatively connected with an IMU sensor and a camera disposed on the target device, and the method comprises:

[0007] Acquire a current IMU group collected by the IMU sensor and an environment image collected by the camera that has a timestamp matching relationship with the current IMU group; wherein the current IMU group includes current IMU data at the current moment and at least one group of historical IMU data that is time-continuous with the current IMU data;

[0008] Based on the time series relationship of the IMU data, the current IMU group is estimated and the error is corrected to obtain the velocity estimation value at the next moment;

[0009] Extracting image features of the environment image, fusing the image features with the speed estimation value, and obtaining a predicted trajectory of the target device at the next moment according to the fusion result;

[0010] The movement of the target device is controlled according to the predicted trajectory.

[0011] Optionally, the step of estimating and correcting the error of the current IMU group based on the time series relationship of the IMU data to obtain the velocity estimation value at the next moment includes:

[0012] Based on the current IMU group, using a preset time series model to generate an original estimate value at the next moment; wherein the time series model is a model that has learned the correlation between IMU data with consecutive timestamps;

[0013] The original estimated value is subjected to error correction to obtain a speed estimate value at the next moment.

[0014] Optionally, the time series model includes an angular velocity model and an acceleration model;

[0015] Before the step of acquiring the current IMU group acquired by the IMU sensor and the environment image acquired by the camera and having a timestamp matching relationship with the current IMU group, the method further includes:

[0016] Acquire multiple IMU sample groups; wherein the IMU sample groups include at least time-continuous IMU samples and a target IMU, and both the IMU samples and the target IMU include acceleration and angular velocity;

[0017] Based on the multiple IMU sample groups, a first velocity model and a second velocity model are respectively fitted using a model fitting algorithm; wherein the first velocity model represents the relationship between the acceleration of the IMU sample and the acceleration of the target IMU, and the second velocity model represents the relationship between the angular velocity of the IMU sample and the angular velocity of the target IMU;

[0018] Based on the multiple IMU sample groups, respectively optimizing the first velocity model and the second velocity model to obtain multiple optimized first velocity models and second velocity models;

[0019] Verifying each of the first velocity models and each of the second velocity models respectively to obtain a verification value of each of the first velocity models and a verification value of each of the angular velocity models;

[0020] Based on the verification value, each of the first velocity models is optimized to obtain an acceleration model, and each of the second velocity models is optimized to obtain an angular velocity model.

[0021] Optionally, the step of generating an original estimate at the next moment based on the current IMU group using a preset time series model includes:

[0022] Inputting the acceleration in the current IMU group into the acceleration model, and using the acceleration model to generate an estimated acceleration value at the next moment;

[0023] The angular velocity in the current IMU group is input into an angular velocity model, and the angular velocity model is used to generate an angular velocity estimate at the next moment.

[0024] Optionally, the step of extracting image features of the environment image, fusing the image features with the speed estimation value, and obtaining a predicted trajectory of the target device at the next moment according to the fusion result includes:

[0025] Based on the velocity estimation value and the current IMU group, velocity integration is performed to obtain an IMU estimation value at the next moment;

[0026] Inputting the environment image into a preset feature extraction model, and using the feature extraction model to generate image features of each environment image;

[0027] The image features and the IMU estimated values ​​are input into a preset trajectory prediction model, and the trajectory prediction model is used to generate a predicted trajectory at the next moment.

[0028] Optionally, before the step of acquiring the current IMU group acquired by the IMU sensor and the environment image acquired by the camera and having a timestamp matching relationship with the current IMU group, the method further includes:

[0029] Obtain sample images, IMU data, and RTK data, and obtain the initial convolutional neural network and long short-term memory network;

[0030] According to the timestamp alignment relationship, the sample images, the IMU data and the RTK data are grouped to obtain a plurality of sample groups; wherein the sample group includes a plurality of sample images with consecutive timestamps, the IMU data aligned with the timestamps of the plurality of sample images one by one, and the supervised RTK; the supervised RTK is the RTK data corresponding to the next timestamp of the sample group;

[0031] Based on the plurality of sample groups, the convolutional neural network and the long short-term memory network are jointly trained in a supervised manner to obtain a feature extraction model and a trajectory prediction model.

[0032] Optionally, the step of performing supervised joint training on the convolutional neural network and the long short-term memory network based on the plurality of sample groups to obtain a feature extraction model and a trajectory prediction model comprises:

[0033] Selecting a target group from the plurality of sample groups;

[0034] Extracting image features of sample images in the target group using the convolutional neural network, and generating an estimated IMU sample value at the next moment based on the IMU data in the target group;

[0035] Inputting the IMU sample estimation value and the image feature into the long short-term memory network, and using the long short-term memory network to generate an estimated trajectory at the next moment;

[0036] Calculating a loss value between the estimated trajectory and the supervised RTK, and determining whether the loss value is less than a loss threshold;

[0037] If not, adjusting the model parameters of the convolutional neural network and the long short-term memory network, selecting the next target group from the multiple sample groups, and returning to execute the step of extracting the image features of the sample images in the target group by using the convolutional neural network, and generating the IMU sample estimate at the next moment based on the IMU data in the target group;

[0038] If so, the training is terminated, and the current neural network model is used as the feature extraction model, and the current long short-term memory network is used as the trajectory prediction model.

[0039] In a second aspect, the present invention provides an inertial navigation device, applied to a controller of a target device, wherein the controller is communicatively connected with an IMU sensor and a camera disposed on the target device, and the inertial navigation device comprises a data acquisition module, an estimation correction module, a fusion prediction module and a navigation control module;

[0040] The data acquisition module is used to obtain the current IMU group collected by the IMU sensor and the environment image collected by the camera and having a timestamp matching relationship with the current IMU group; wherein the current IMU group includes the current IMU data at the current moment and at least one group of historical IMU data that is time continuous with the current IMU data;

[0041] The estimation correction module is used to estimate and correct the error of the current IMU group based on the time series relationship of the IMU data to obtain the speed estimation value at the next moment;

[0042] The fusion prediction module is used to extract image features of the environment image, fuse the image features with the speed estimation value, and obtain the predicted trajectory of the target device at the next moment according to the fusion result;

[0043] The navigation control module is used to control the movement of the target device according to the predicted trajectory.

[0044] In a third aspect, the present invention provides a robot, comprising a body, and a controller, an IMU sensor, and a camera arranged on the body, wherein the controller is communicatively connected with the IMU sensor and the camera respectively;

[0045] The IMU sensor is used to collect the current IMU group of the robot in real time and transmit the current IMU group to the controller;

[0046] The camera is used to collect the current environment image of the robot and transmit the environment image to the controller;

[0047] The controller is used to implement the inertial navigation method as described in the first aspect.

[0048] In a fourth aspect, the present invention provides an electronic device, comprising a processor and a memory, wherein the memory stores a computer program executable by the processor, and the processor can execute the computer program to implement the inertial navigation method as described in the first aspect.

[0049] In a fifth aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the inertial navigation method as described in the first aspect.

[0050] The inertial navigation method, device, electronic device and robot provided by the embodiments of the present invention include: obtaining a current IMU group collected by an IMU sensor and an environmental image collected by a camera and having a timestamp matching relationship with the current IMU group, wherein the current IMU group includes current IMU data at a current moment and at least one group of historical IMU data that is time-continuous with the current IMU data; estimating and correcting errors of the current IMU group based on the time series relationship of the IMU data to obtain a speed estimation value at the next moment; extracting image features of the environmental image, and fusing the image feature speed estimation values, and obtaining a predicted trajectory of the target device at the next moment based on the fusion result; and controlling the movement of the target device based on the predicted trajectory.

[0051] In this way, the current IMU group is estimated and error corrected through the time series relationship of IMU data to reduce or even eliminate drift errors, making the obtained speed estimation value more accurate, and then the predicted trajectory is obtained by fusing the IMU estimation with the environmental image to further improve the accuracy of the predicted trajectory, thereby greatly improving the navigation accuracy of the target device in complex environments. Therefore, it helps to significantly improve the navigation accuracy and stability of the lawn mower robot in complex environments.

[0052] In addition, by first correcting the IMU data errors and then fusing them into the predicted trajectory, high-precision navigation is still possible even when the signal is weak or unstable, which greatly improves the navigation stability and robustness of the target device in complex environments.

[0053] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0055] Figure 1 A schematic diagram of the structure of an inertial navigation system provided by an embodiment of the present invention is shown.

[0056] Figure 2 A block diagram of an electronic device provided by an embodiment of the present invention is shown.

[0057] Figure 3 One of the flow charts of the inertial navigation method provided by an embodiment of the present invention is shown.

[0058] Figure 4 Shows Figure 3 Schematic diagram of the process flow of some sub-steps of step 13.

[0059] Figure 5 The second flowchart of the inertial navigation method provided by the embodiment of the present invention is shown.

[0060] Figure 6 Shows Figure 3 Schematic diagram of the process flow of some sub-steps of step 15.

[0061] Figure 7 The third flowchart of the inertial navigation method provided by an embodiment of the present invention is shown.

[0062] Figure 8 Shows Figure 7 Schematic diagram of the process flow of some sub-steps of step 35.

[0063] Fig. 9 A block diagram of an inertial navigation device provided by an embodiment of the present invention is shown.

[0064] Explanation of the reference numerals: 10 - inertial navigation system; 110 - target device; 120 - controller; 130 - IMU sensor; 140 - camera; 20 - electronic device; 210 - memory; 220 - processor; 230 - communication module; 30 - inertial navigation device; 310 - data acquisition module; 320 - estimation and correction module; 330 - fusion prediction module; 340 - navigation control module. DETAILED DESCRIPTION

[0065] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.

[0066] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making creative work are within the scope of protection of the present invention.

[0067] It should be noted that relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0068] The inertial navigation method provided by the embodiment of the present invention can be applied to Figure 1 The inertial navigation system 10 shown includes a target device 110, a controller 120 of the target device 110, and an IMU sensor 130 and a camera 140 installed on the body of the target device 110. The controller 120 can be communicatively connected with the IMU sensor 130 and the camera 140 via an Ethernet cable, a USB cable, serial communication, WiFi, Bluetooth, etc.

[0069] The camera 140 may be a monocular camera 140 , and the target device 110 may be, but is not limited to, a lawn mowing robot, an inspection robot, and the like.

[0070] The IMU sensor 130 is used to continuously collect current IMU data during the movement of the target device 110 and transmit the current IMU data to the controller 120 in real time.

[0071] The camera 140 is used to continuously collect environmental images of the environment in which the target device 110 is located during the movement of the target device 110 , and transmit the environmental images to the controller 120 in real time.

[0072] The controller 120 is used to receive current IMU data and an environmental image, and use the latest current IMU data and at least one group of historical IMU data that is time-continuous with the current IMU data as the current IMU group, and implement the inertial navigation method provided in an embodiment of the present invention.

[0073] It should be noted that the controller 120 can be, but is not limited to, a computer, a programmable logic controller 120 , a single-chip microcomputer, a field programmable gate array, a server, and the like.

[0074] Please refer to Figure 2 , is a block diagram of an electronic device 20, which may be Figure 1 The controller 120 in the inertial navigation system 10 shown. The electronic device 20 includes a memory 210, a processor 220 and a communication module 230. The memory 210, the processor 220 and the communication module 230 are electrically connected to each other directly or indirectly to achieve data transmission or interaction. For example, these components can be electrically connected to each other through one or more communication buses or signal lines.

[0075] The memory 210 is used to store programs or data and can be, but not limited to, a random access memory, a read-only memory, a programmable read-only memory, an erasable read-only memory, an electrically erasable read-only memory, and the like.

[0076] The processor 220 is used to read / write data or programs stored in the memory 210 and execute corresponding functions. For example, Figure 1 In the inertial navigation system 10 shown, the processor 220 of the controller 120 reads the computer program stored in the memory 210 to implement the inertial navigation method provided by the embodiment of the present invention.

[0077] The communication module 230 is used to establish a communication connection between the electronic device 20 and other communication terminals through a wired connection, a wireless connection, etc., and is used to send and receive data. Figure 1 In the inertial navigation system 10 shown, the controller 120 can transmit and receive data with the IMU sensor 130 and the camera 140 respectively through the communication module 230.

[0078] It should be understood that Figure 2 The structure shown is only a schematic diagram of the structure of the electronic device 20. The electronic device 20 may also include Figure 2 More or fewer components as shown, or with Figure 2 Different configurations shown. Figure 2 Each component shown in the figure can be implemented by hardware, software or a combination thereof.

[0079] In order to improve Figure 1 The target device 110 (such as a lawn mowing robot, an inspection robot, etc.) of the inertial navigation system 10 shown in the figure has navigation accuracy, stability and robustness in a complex environment. The embodiment of the present invention provides an inertial navigation method, referring to Figure 3 , including steps 11 to 17. And, Figure 1 In the inertial navigation system 10 shown, the controller 120 may Figure 2 The structure shown implements step 11 to step 17 when the processor 220 executes the computer program executed in the memory 210.

[0080] Step 11, obtain the current IMU group collected by the IMU sensor, and the environment image collected by the camera that has a timestamp matching relationship with the current IMU group.

[0081] The current IMU group includes the current IMU data at the current moment and at least one group of historical IMU data that is time continuous with the current IMU data.

[0082] Step 13, based on the time series relationship of the IMU data, estimate and correct the error of the current IMU group to obtain the velocity estimate value at the next moment.

[0083] Step 15, extracting image features of the environment image, and fusing the image features with the speed estimation value, and obtaining the predicted trajectory of the target device at the next moment according to the fusion result.

[0084] Step 17: Control the movement of the target device according to the predicted trajectory.

[0085] Among them, the current IMU data and the historical IMU data both include the speed (including acceleration and angular velocity), position and posture at the corresponding moment, and the speed estimation value includes the acceleration estimation value and the angular velocity estimation value.

[0086] For example, in combination Figure 1In the inertial navigation system 10 shown, when the target device 110 is powered on and enters the working mode (such as when the mowing robot starts mowing), the IMU sensor 130 continuously collects the current IMU data of the target device 110 at a preset collection period (such as 2ms), and transmits the current IMU data to the controller 120 in real time. At the same time, at the same collection period, the camera 140 on the target device 110 continuously collects the environmental image around the target device 110, and transmits the environmental image to the controller 120 in real time. The controller 120 receives and caches the current IMU data and environmental image, takes the latest current IMU data and at least one set of historical IMU data that is time-continuous with the current IMU data as the current IMU group, and retrieves the environmental image that corresponds one-to-one with the timestamp of the current IMU group.

[0087] Next, the controller 120 estimates and corrects the drift error of the current IMU group based on the time series relationship between the pre-fitted IMU data to obtain the estimated speed value at the next moment. After obtaining the estimated speed value, the controller 120 fuses the estimated speed value with the image features of the environmental image, and obtains the predicted trajectory of the target device 110 at the next moment based on the fusion result. Thus, the controller 120 adjusts the speed, direction, angle, etc. of the target device 110 according to the difference between the predicted trajectory and the target trajectory to adjust the motion trajectory.

[0088] Compared with the traditional autonomous navigation method, the relational navigation method provided by the embodiment of the present invention estimates and corrects the error of the current IMU group through the time series relationship of the IMU data to reduce or even eliminate the drift error, so that the obtained speed estimate is more accurate, and then the predicted trajectory is obtained by fusing the IMU estimation with the environmental image to further improve the accuracy of the predicted trajectory, thereby greatly improving the navigation accuracy of the target device in a complex environment. At the same time, by first correcting the IMU data error and then fusing the predicted trajectory, high-precision navigation can still be performed in the case of weak or unstable signals, thereby greatly improving the navigation stability and robustness of the target device in a complex environment.

[0089] Among them, in step 13, the method of estimating and correcting the error of the current IMU group can be flexibly selected. For example, Kalman filtering can be used to estimate the speed estimate value at the next moment, or a pre-trained filtering model and estimation model can be used for estimation and error correction. The above method is only an example, and its implementation method is not limited.

[0090] In order to make the estimation of the current IMU group more accurate, a time series model is first constructed to fit the time series correlation (i.e., time series relationship) between IMU data with consecutive timestamps, and then the idea of ​​using the time series model for estimation and the Kalman filter for error correction is introduced in step 13. Figure 4 The process of obtaining the speed estimation value at the next moment in step 13 includes steps 131 to 133.

[0091] Step 131, based on the current IMU group, using a preset time series model to generate the original estimated value of the next moment.

[0092] The time series model is a model that has learned the correlation between IMU data with consecutive timestamps. After the current IMU group is input into the time series model, the time series model generates an estimated value of the IMU data at the next moment, that is, the original estimated value, based on the pre-learned correlation between IMU data with consecutive timestamps.

[0093] Step 133, performing error correction on the original estimated value to obtain the speed estimate value at the next moment.

[0094] Here, the time series model is a pre-built model, and its acquisition method can be flexibly set. For example, the time series model can be obtained by performing linear regression fitting on historical IMU data using the least squares method, or the time series model can be obtained by training a neural network model using historical IMU data as a sample. The implementation method of building a time series model is not limited.

[0095] In order to make the acceleration and angular velocity in the original estimated values ​​more accurate, the idea of ​​separately constructing the time series model of acceleration and the time series model of angular velocity is introduced in the process of constructing the time series model. Figure 5 The inertial navigation method provided by the embodiment of the present invention further includes steps 21 to 29, which are performed before step 11, so that the time series model is obtained before the controller adopts the inertial navigation method provided by the embodiment of the present invention.

[0096] Step 21, obtaining multiple IMU sample groups.

[0097] The IMU sample group includes at least time-continuous IMU samples and target IMU, and both the IMU samples and the target IMU include acceleration and angular velocity. The number of IMU samples in the IMU sample group is two or more. For example, an IMU sample group may include t 1 IMU sample at time t 2 IMU samples at time t 3 The target IMU at time t can also be an IMU sample group including t1 IMU sample at time t 2 IMU sample at time t 3 IMU samples at time t 4 The target IMU at that moment.

[0098] Step 23: Based on the multiple IMU sample groups, a model fitting algorithm is used to fit the first velocity model and the second velocity model respectively.

[0099] Here, the model fitting algorithm includes but is not limited to: least squares method, polynomial regression, curve fitting. The first velocity model characterizes the relationship between the acceleration of the IMU sample and the acceleration of the target IMU, that is, the correlation between the accelerations in the time-continuous IMU data. The second velocity model characterizes the relationship between the angular velocity of the IMU sample and the angular velocity of the target IMU, that is, the correlation between the angular velocities in the time-continuous IMU data.

[0100] In order to make both the first velocity model and the second velocity model have the function of time series analysis, the fitting construction method of the autoregressive moving average model can be used to construct the first velocity model and the second velocity model in step 23. At this time, the first velocity model and the second velocity model are both autoregressive moving average models.

[0101] Step 25: Optimize the first velocity model and the second velocity model based on the multiple IMU sample groups to obtain multiple optimized first velocity models and second velocity models.

[0102] For example, the first velocity model and the second velocity model can be iteratively trained respectively, and after each round of iteration, the first velocity model and the second velocity model can be adjusted using optimization methods such as gradient descent, Newton's method, quasi-Newton's method, and heuristic optimization. An optimized model is obtained each time the adjustment is made, and the iterations are repeated until an iteration threshold is reached to obtain multiple optimized first velocity models and second velocity models.

[0103] Step 27, using multiple verification algorithms to verify each first velocity model and each second velocity model respectively, to obtain multiple verification values ​​of each first velocity model and multiple verification values ​​of each angular velocity model.

[0104] The setting of the verification algorithm can be flexibly selected, for example, it can be model residual analysis, autocorrelation function and partial correlation function, or cross-validation and leave-one-out method, and its implementation method is not limited. Part of the data can be selected from the multiple IMU sample groups obtained in step 21 as a test set. Thus, for each first velocity model, the IMU sample of the IMU sample group in the test set is used as input, and the error between the output value of the first velocity model and the target IMU in the IMU sample group is used as a verification value. Similarly, the verification value of each second velocity model is calculated.

[0105] Step 29, based on the verification value, each first velocity model is optimized to obtain an acceleration model, and each second velocity model is optimized to obtain an angular velocity model.

[0106] Here, when each first velocity model has multiple verification values, the final verification value can be calculated by taking the average value, the intermediate value, the high frequency value, the weighted sum, etc., and then the model with the smallest final verification value is selected as the acceleration model. Similarly, when each second velocity model has multiple verification values, the final verification value can be calculated by taking the average value, the intermediate value, the high frequency value, the weighted sum, etc., and then the model with the smallest final verification value is selected as the angular velocity model.

[0107] The acceleration model obtained from steps 21 to 29 above can be as follows: in, and are model parameters, and represents white noise, a t represents the acceleration at time t.

[0108] The acceleration model obtained from steps 21 to 29 above can be as follows: in, and represents the model parameters, and represents white noise, ω t represents the angular velocity at time t.

[0109] Through the above steps 21 to 29, a more accurate acceleration model and angular velocity model are obtained. Therefore, in step 131, the acceleration in the current IMU group is input into the acceleration model, and the acceleration model is used to generate the estimated acceleration value of the next moment based on the correlation between the accelerations in the IMU data with consecutive timestamps learned in advance. Similarly, the angular velocity in the current IMU group is input into the angular velocity model, and the angular velocity model is used to generate the estimated angular velocity value of the next moment based on the correlation between the angular velocities in the IMU data with consecutive timestamps learned in advance.

[0110] Furthermore, in step 133, Kalman filtering, least squares method, particle swarm optimization or any other filtering method may be used to perform error correction on the original estimated value. The implementation method is not limited.

[0111] Taking the error correction using Kalman filtering as an example, the generated angular velocity estimate and acceleration estimate are respectively filtered online using Kalman filtering to correct the drift error and obtain the velocity estimate at the next moment.

[0112] The process of online filtering using Kalman filtering can be represented by an angular velocity filtering formula and an acceleration filtering formula.

[0113] The angular velocity filtering formula includes: z ω，k =H ω x ω，k +v ω，k Among them, A ω The state transfer matrix representing the angular velocity, B ω represents the control matrix of angular velocity, represents the prediction result of the angular velocity model, x ω，k The state vector representing the angular velocity (i.e., the filtered angular velocity estimate), u ω，k-1 represents the control input, w ω，k-1 represents the process noise of angular velocity, H ω The observation transfer matrix representing the angular velocity, v ω，k represents the observation noise of angular velocity, z ω，k Represents the observed quantity of angular velocity.

[0114] The acceleration filtering formula includes: z a，k =H a x a，k +v a，k Among them, A a The state transfer matrix representing the acceleration, B a represents the control matrix of acceleration, represents the prediction result of the acceleration model, x a，k represents the state vector of acceleration (i.e., the filtered acceleration estimate), u a，k-1 Represents the control input, W a,k-1 represents the process noise of acceleration, H a represents the observed transfer matrix of acceleration, v a，k represents the acceleration observation noise, z a，k Represents the observed quantity of acceleration.

[0115] For IMU data, B ω and Ba All are 0 matrices.

[0116] Through the above method, an acceleration model of the correlation between accelerations in the learned IMU data with continuous timestamps and an angular velocity model of the correlation between angular velocities in the learned IMU data with continuous timestamps are first constructed, and based on the time series analysis of the acceleration model and the angular velocity model, the acceleration estimate and angular velocity estimate of the current IMU group at the next moment are obtained, and the Kalman filter is used to perform online filtering on the two estimates to eliminate the integral drift error, so as to obtain a more accurate velocity estimate.

[0117] In step 15, the method of fusing the speed estimation value and the environment image can be flexibly selected. For example, the speed estimation value and the environment image can be used as inputs of a preset model, and the preset model can be used to perform data fusion prediction to generate a predicted trajectory. Alternatively, the fusion prediction can be performed according to preset rules to obtain a predicted trajectory. In this embodiment, there is no limitation on the implementation method.

[0118] In order to make the predicted trajectory more accurate, in step 15, the IMU estimated value is obtained based on the integral of the velocity estimated value, and the image features and the velocity estimated value are fused using a pre-trained model to generate the trajectory prediction for the next moment. Figure 6 , step 15 includes steps 151 to 155.

[0119] Step 151, based on the velocity estimation value and the current IMU group, velocity integration is performed to obtain the IMU estimation value at the next moment.

[0120] Step 153: input the environment image into a preset feature extraction model, and use the feature extraction model to generate image features for each environment image.

[0121] Step 155, input the image features and the IMU estimation value into a preset trajectory prediction model, and use the trajectory prediction model to generate a predicted trajectory at the next moment.

[0122] Among them, the feature extraction model and trajectory prediction model are high-precision true values ​​(i.e., RTK data) provided by real-time kinematic differential positioning technology (Real-Time Kinematic, RTK), which are obtained by jointly supervising the training of the neural network model and the long short-term memory network.

[0123] Reference Figure 7 , the process of constructing the feature extraction model and the trajectory prediction model includes steps 31 to 35.

[0124] Step 31, obtain sample images, IMU data and RTK data, and obtain the initial convolutional neural network and long short-term memory network.

[0125] Here, the collected sample images can be expressed as The IMU data can be expressed as D i ={D v , D p , D q},in Indicates speed data, Represents location data, Represents attitude data. RTK data can be represented as

[0126] Step 33, grouping the sample images, IMU data and RTK data according to the timestamp alignment relationship to obtain multiple sample groups.

[0127] The sample group includes multiple sample images with consecutive timestamps, IMU data aligned with the timestamps of the multiple sample images, and supervised RTK. The supervised RTK is the RTK data corresponding to the next timestamp of the sample group.

[0128] Step 35, based on multiple sample groups, supervised joint training is performed on the convolutional neural network and the long short-term memory network to obtain a feature extraction model and a trajectory prediction model.

[0129] Here, we can adopt the iterative training method, refer to Figure 8 The process of using iteration in step 35 to perform supervised joint training of the convolutional neural network and the long short-term memory network may include steps 351 to 356.

[0130] Step 351, selecting a target group from multiple sample groups.

[0131] Step 352, using a convolutional neural network to extract image features of the sample images in the target group, and generating an estimated IMU sample value at the next moment based on the IMU data in the target group.

[0132] It should be noted that the pre-built time series model is used here to generate the estimated IMU sample value at the next moment. The process of generating the estimated IMU sample value can refer to the description and effect of step 131 and related contents above, which will not be repeated here.

[0133] Step 353, input the IMU sample estimate and image features into the long short-term memory network, and use the long short-term memory network to generate the estimated trajectory at the next moment.

[0134] Step 354, calculate the loss value between the estimated trajectory and the supervised RTK, and determine whether the loss value is less than the loss threshold. If not, execute step 355, and if so, execute step 356.

[0135] Here, the setting of the loss function can be flexibly selected, which can be a mean square error loss function, a cross entropy loss function, or a logarithmic loss function, and its implementation method is not restricted.

[0136] Step 355, adjusting the model parameters of the convolutional neural network and the long short-term memory network, and selecting the next target group from the multiple sample groups. After step 355, the process returns to step 352.

[0137] Step 356, end the training, use the current neural network model as the feature extraction model, and use the current long short-term memory network as the trajectory prediction model.

[0138] The target group includes sample images IMU Data D i ={D v , D p , D q},in, And supervised RTK for For example.

[0139] Use convolutional neural network (CNN network) to take sample images The image features are obtained in Represents a CNN network.

[0140] The image feature sequence and IMU data sequence D i ={D v , D p , D q The estimated value of the IMU sample is sent to the long short-term memory network (LSTM network) to obtain the predicted trajectory at the next moment.

[0141] Then, the mean square error loss function is used to calculate the predicted trajectory With supervised RTK The mean square error loss of each part between them is combined with a certain weight to obtain the final loss, such as MSE = αMSE V +βMSE p +γMSE q , α, β, γ are hyperparameters, α+β+γ=1.

[0142] When the loss function value MSE is less than the set threshold μ, the optimal CNN network and LSTM network are output; otherwise, the parameters of the CNN network and LSTM network are reset, and the target group is reselected, and the process returns to step 352.

[0143] Thus, through the above steps 31 to 35, and steps 351 to 356, the model is supervised and trained using high-precision RTK data to ensure the accuracy of the feature extraction model and trajectory prediction model obtained in the end, thereby making the predicted trajectory obtained in step 15 more accurate.

[0144] Based on the same inventive concept as the above-mentioned inertial navigation method, referring to Fig. 9 The embodiment of the present invention further provides an inertial navigation device 30, which can be applied to Figure 1 The controller 120 in the inertial navigation system 10 shown includes a data acquisition module 310 , an estimation and correction module 320 , a fusion prediction module 330 and a navigation control module 340 .

[0145] The data acquisition module 310 is used to obtain the current IMU group collected by the IMU sensor, and the environment image collected by the camera and having a timestamp matching relationship with the current IMU group.

[0146] The current IMU group includes the current IMU data at the current moment and at least one group of historical IMU data that is time continuous with the current IMU data.

[0147] The estimation and correction module 320 is used to estimate and correct the error of the current IMU group based on the time series relationship of the IMU data to obtain the speed estimation value at the next moment.

[0148] The fusion prediction module 330 is used to extract image features of the environment image, fuse the image features with the speed estimation value, and obtain the predicted trajectory of the target device at the next moment based on the fusion result.

[0149] The navigation control module 340 is used to control the movement of the target device according to the predicted trajectory.

[0150] Furthermore, it may also include a first model building module and a second model building module.

[0151] The first model building module is used to execute steps 21 to 29 above.

[0152] The second model building module is used to execute steps 31 to 35 above.

[0153] The above-mentioned inertial navigation device 30 estimates and corrects the error of the current IMU group through the coordinated action of the data acquisition module 310, the estimation correction module 320, the fusion prediction module 330 and the navigation control module 340, through the time series relationship of the IMU data, to reduce or even eliminate the drift error, so that the obtained speed estimation value is more accurate, and then obtains the predicted trajectory through the fusion of the IMU estimation and the environmental image, so as to further improve the accuracy of the predicted trajectory, thereby greatly improving the navigation accuracy of the target device in a complex environment. Therefore, it helps to significantly improve the navigation accuracy and stability of the lawn mowing robot in a complex environment.

[0154] At the same time, by first correcting the IMU data errors and then fusing the predicted trajectory, high-precision navigation can still be performed even when the signal is weak or unstable, thereby greatly improving the navigation stability and robustness of the target device 110 in complex environments.

[0155] For the specific implementation and effect of the inertial navigation device 30, please refer to the description of the implementation of the inertial navigation method above, such as, for the specific implementation and effect of the data acquisition module 310, please refer to the description of the relevant content of step 11 above, for the specific implementation and effect of the estimation and correction module 320, please refer to the description of the relevant content of step 13 above, for the specific implementation and effect of the fusion prediction module 330, please refer to the description of the relevant content of step 15 above, for the specific implementation and effect of the navigation control module 340, please refer to the description of the relevant content of step 17 above, and no further details will be given here.

[0156] In addition, each module of the above-mentioned inertial navigation device 30 can be implemented in whole or in part by software, hardware and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor 220 in the electronic device 20 in the form of hardware, or can be stored in the memory 210 of the electronic device 20 in the form of software, so that the processor 220 can call and execute the operations corresponding to the above-mentioned modules to implement the inertial navigation method provided above.

[0157] Based on the same concept, referring to the drawings, an embodiment of the present invention further provides a robot, including a main body and a controller 120, an IMU sensor 130 and a camera 140 arranged on the main body, and the controller 120 is communicatively connected with the IMU sensor 130 and the camera 140 respectively.

[0158] The IMU sensor 130 is used to collect the current IMU group of the robot in real time and transmit the current IMU group to the controller 120.

[0159] The camera 140 is used to collect the current environment image of the robot and transmit the environment image to the controller 120 .

[0160] The controller 120 is used to implement the inertial navigation method provided above.

[0161] The robot may be any device that requires autonomous navigation, such as a lawn mowing robot, a sweeping robot, or an inspection robot, and the camera 140 may be a monocular camera 140 .

[0162] In this way, the robot can achieve higher-precision and more robust autonomous navigation, improve the robot's navigation accuracy and stability in areas with weak or unstable satellite signals and in scenarios with few feature points, and improve operational efficiency and intelligence.

[0163] The embodiment of the present invention further provides an electronic device 20, including a processor 220 and a memory 210, wherein the memory 210 stores a computer program executable by the processor 220, and the processor 220 can execute the computer program to implement the inertial navigation method provided above.

[0164] The embodiment of the present invention further provides a storage medium on which a computer program is stored. When the computer program is executed by the processor 220, the inertial navigation method proposed in the embodiment of the present invention is implemented.

[0165] An embodiment of the present invention also provides a computer program product, including a software code portion. When the product is run on a computer, the software code portion is used to execute the steps of the inertial navigation method provided above.

[0166] In summary, the inertial navigation method, device, electronic device and robot provided by the embodiments of the present invention correct the drift error of IMU data through time series analysis, and use a neural network to predict trajectories in combination with environmental images, which has at least the following beneficial effects:

[0167] (1) The robot’s navigation accuracy and stability in complex environments have been significantly improved, especially in areas with weak or unstable satellite signals and in scenes with few feature points;

[0168] (ii) Through time series analysis and neural network fusion of multi-source data, high-precision and high-robustness autonomous navigation is achieved, improving operational efficiency and intelligence level;

[0169] (3) It reduces the need for human intervention, reduces labor costs, and improves the competitiveness and market value of robots in practical applications.

[0170] In several embodiments provided by the present invention, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present invention. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical functions. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0171] In addition, the functional modules in the various embodiments of the present invention may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

[0172] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory 210 (ROM, Read-Only Memory), random access memory 210 (RAM, Random Access Memory), disk or optical disk, and other media that can store program codes.

[0173] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An inertial navigation method, characterized in that: A controller applied to a target device, the controller being communicatively connected to an IMU sensor and a camera disposed on the target device, the method comprising: Acquire a current IMU group collected by the IMU sensor and an environment image collected by the camera that has a timestamp matching relationship with the current IMU group; wherein the current IMU group includes current IMU data at the current moment and at least one group of historical IMU data that is time-continuous with the current IMU data; Based on the time series relationship of the IMU data, the current IMU group is estimated and the error is corrected to obtain the velocity estimation value at the next moment; Extracting image features of the environment image, fusing the image features with the speed estimation value, and obtaining a predicted trajectory of the target device at the next moment according to the fusion result; The movement of the target device is controlled according to the predicted trajectory.

2. The inertial navigation method according to claim 1, characterized in that: The step of estimating and correcting the error of the current IMU group based on the time series relationship of the IMU data to obtain the speed estimation value at the next moment includes: Based on the current IMU group, using a preset time series model to generate an original estimate value at the next moment; wherein the time series model is a model that has learned the correlation between IMU data with consecutive timestamps; The original estimated value is subjected to error correction to obtain a speed estimate value at the next moment.

3. The inertial navigation method according to claim 2, characterized in that: The time series model includes an angular velocity model and an acceleration model; Before the step of acquiring the current IMU group acquired by the IMU sensor and the environment image acquired by the camera and having a timestamp matching relationship with the current IMU group, the method further includes: Acquire multiple IMU sample groups; wherein the IMU sample groups include at least time-continuous IMU samples and a target IMU, and both the IMU samples and the target IMU include acceleration and angular velocity; Based on the multiple IMU sample groups, a first velocity model and a second velocity model are respectively fitted using a model fitting algorithm; wherein the first velocity model represents the relationship between the acceleration of the IMU sample and the acceleration of the target IMU, and the second velocity model represents the relationship between the angular velocity of the IMU sample and the angular velocity of the target IMU; Based on the multiple IMU sample groups, respectively optimizing the first velocity model and the second velocity model to obtain multiple optimized first velocity models and second velocity models; Verifying each of the first velocity models and each of the second velocity models respectively to obtain a verification value of each of the first velocity models and a verification value of each of the angular velocity models; Based on the verification value, each of the first velocity models is optimized to obtain an acceleration model, and each of the second velocity models is optimized to obtain an angular velocity model.

4. The inertial navigation method according to claim 3, characterized in that: The step of generating the original estimated value at the next moment based on the current IMU group using a preset time series model includes: Inputting the acceleration in the current IMU group into the acceleration model, and using the acceleration model to generate an estimated acceleration value at the next moment; The angular velocity in the current IMU group is input into the angular velocity model, and the angular velocity estimation value at the next moment is generated using the angular velocity model.

5. The inertial navigation method according to any one of claims 1 to 3, characterized in that: The step of extracting image features of the environment image, fusing the image features with the speed estimation value, and obtaining a predicted trajectory of the target device at the next moment according to the fusion result includes: Based on the velocity estimation value and the current IMU group, velocity integration is performed to obtain an IMU estimation value at the next moment; Inputting the environment image into a preset feature extraction model, and using the feature extraction model to generate image features of each environment image; The image features and the IMU estimated values ​​are input into a preset trajectory prediction model, and the trajectory prediction model is used to generate a predicted trajectory at the next moment.

6. The inertial navigation method according to claim 5, characterized in that: Before the step of acquiring the current IMU group acquired by the IMU sensor and the environment image acquired by the camera and having a timestamp matching relationship with the current IMU group, the method further includes: Obtain sample images, IMU data, and RTK data, and obtain the initial convolutional neural network and long short-term memory network; According to the timestamp alignment relationship, the sample images, the IMU data and the RTK data are grouped to obtain a plurality of sample groups; wherein the sample group includes a plurality of sample images with consecutive timestamps, the IMU data aligned with the timestamps of the plurality of sample images one by one, and the supervised RTK; the supervised RTK is the RTK data corresponding to the next timestamp of the sample group; Based on the plurality of sample groups, the convolutional neural network and the long short-term memory network are jointly trained in a supervised manner to obtain a feature extraction model and a trajectory prediction model.

7. The inertial navigation method according to claim 6, characterized in that: The step of performing supervised joint training on the convolutional neural network and the long short-term memory network based on the plurality of sample groups to obtain a feature extraction model and a trajectory prediction model comprises: Selecting a target group from the plurality of sample groups; Extracting image features of sample images in the target group using the convolutional neural network, and generating an estimated IMU sample value at the next moment based on the IMU data in the target group; Inputting the IMU sample estimation value and the image feature into the long short-term memory network, and using the long short-term memory network to generate an estimated trajectory at the next moment; Calculating a loss value between the estimated trajectory and the supervised RTK, and determining whether the loss value is less than a loss threshold; If not, adjusting the model parameters of the convolutional neural network and the long short-term memory network, selecting the next target group from the multiple sample groups, and returning to execute the step of extracting the image features of the sample images in the target group by using the convolutional neural network, and generating the IMU sample estimate at the next moment based on the IMU data in the target group; If so, the training is terminated, and the current neural network model is used as the feature extraction model, and the current long short-term memory network is used as the trajectory prediction model.

8. An inertial navigation device, characterized in that: A controller applied to a target device, the controller being communicatively connected to an IMU sensor and a camera disposed on the target device, the inertial navigation device comprising a data acquisition module, an estimation correction module, a fusion prediction module and a navigation control module; The data acquisition module is used to obtain the current IMU group collected by the IMU sensor and the environment image collected by the camera and having a timestamp matching relationship with the current IMU group; wherein the current IMU group includes the current IMU data at the current moment and at least one group of historical IMU data that is time continuous with the current IMU data; The estimation correction module is used to estimate and correct the error of the current IMU group based on the time series relationship of the IMU data to obtain the speed estimation value at the next moment; The fusion prediction module is used to extract image features of the environment image, fuse the image features with the speed estimation value, and obtain the predicted trajectory of the target device at the next moment according to the fusion result; The navigation control module is used to control the movement of the target device according to the predicted trajectory.

9. A robot, characterized in that: It comprises a body, a controller, an IMU sensor and a camera arranged on the body, wherein the controller is respectively connected to the IMU sensor and the camera for communication; The IMU sensor is used to collect the current IMU group of the robot in real time and transmit the current IMU group to the controller; The camera is used to collect the current environment image of the robot and transmit the environment image to the controller; The controller is used to implement the inertial navigation method as described in any one of claims 1 to 7.

10. An electronic device, characterized in that: The invention comprises a processor and a memory, wherein the memory stores a computer program executable by the processor, and the processor can execute the computer program to implement the inertial navigation method according to any one of claims 1 to 7.

Images