Underwater robot positioning method and system

Abstract

The invention provides an underwater robot positioning method and system. The method comprises the following steps: acquiring a camera image and an event data stream acquired by an underwater robot; performing event motion compensation on event frames in the event data stream to obtain event frames after motion compensation; acquiring a plurality of first candidate features and a plurality of second candidate features, performing multi-frame tracking on the plurality of first candidate features and the plurality of second candidate features, and determining a first permanent landmark and a second permanent landmark; and constructing a joint optimization function of the underwater robot according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler velocity instrument velocity error, and obtaining positioning state information of the underwater robot according to a solving result of the joint optimization function. According to the invention, accurate and robust autonomous positioning of the underwater robot in a weak-light underwater environment is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater robots, and in particular to an underwater robot positioning method and system. Background Art

[0002] Accurate positioning of underwater robots is a prerequisite for their autonomous underwater operations. Currently, the widely used underwater robot positioning methods include positioning methods based on hydroacoustic equipment and dead reckoning methods based on high-precision inertial measurement units (IMUs) and Doppler velocity logs (DVLs).

[0003] However, acoustic positioning technologies such as Ultra-Short Baseline (USBL) and Long Baseline (LBL) require the installation of acoustic beacons outside the underwater robot, such as on a mother ship. This installation process is quite complicated and, to a certain extent, weakens the autonomous positioning performance of the underwater robot. On the other hand, the dead reckoning method that relies on IMU and DVL is difficult to achieve high-precision positioning over long distances due to the problem of error accumulation.

[0004] Therefore, there is an urgent need for an underwater robot positioning method and system to solve the above problems. Summary of the invention

[0005] In view of the problems existing in the prior art, the present invention provides an underwater robot positioning method and system.

[0006] The present invention provides an underwater robot positioning method, comprising: Acquire a camera image and an event data stream collected by the underwater robot, wherein the timestamp of the standard frame in the camera image and the timestamp of the event frame in the event data stream are synchronized; Performing event motion compensation on the event frames in the event data stream to obtain motion-compensated event frames; Acquire multiple first candidate features and multiple second candidate features, and perform multi-frame tracking on the multiple first candidate features and the multiple second candidate features respectively to determine a first permanent landmark and a second permanent landmark, wherein the first candidate features are feature points extracted from the standard frame, and the second candidate features are feature points extracted from the event frame after motion compensation; the first permanent landmark is the first candidate feature whose result of multi-frame tracking meets a preset condition, and the second permanent landmark is the second candidate feature whose result of multi-frame tracking meets the preset condition; A joint optimization function of the underwater robot is constructed according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler velocity error, and the positioning state information of the underwater robot is obtained according to the solution result of the joint optimization function, wherein the standard frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the first permanent landmark, and the event frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the second permanent landmark.

[0007] According to a method for positioning an underwater robot provided by the present invention, the method further comprises: Generate an event frame of the event data stream at the current moment according to the event information triggered within the time window at the current moment; Among them, the time window at the current moment is constructed according to the time interval between the standard frame at the current moment and the standard frame at the previous moment in the camera image, and the event information includes event pixel coordinate information corresponding to the target pixel, and the target pixel is the pixel that triggers the event.

[0008] According to an underwater robot positioning method provided by the present invention, performing event motion compensation on the event frame in the event data stream to obtain the motion-compensated event frame includes: According to a camera attitude conversion matrix and a depth value of the target pixel, motion correction is performed on the event pixel coordinate information of the target pixel to obtain the event pixel coordinate information after motion correction, wherein the camera attitude conversion matrix is ​​obtained by pre-integrating a camera motion measurement value of an inertial measurement unit within a preset time interval; The motion compensated event frame is generated according to the motion corrected event pixel coordinate information.

[0009] According to an underwater robot positioning method provided by the present invention, the method of obtaining a plurality of first candidate features and a plurality of second candidate features, and performing multi-frame tracking on the plurality of first candidate features and the plurality of second candidate features respectively based on a pyramid optical flow method to determine a first permanent landmark and a second permanent landmark includes: Based on the FAST corner detector, feature extraction is performed on the standard frame and the event frame after motion compensation to obtain the first candidate feature and the second candidate feature; Based on the pyramid LK optical flow algorithm, multiple first candidate features are used as candidate standard frame landmarks for multi-frame tracking in the standard frame, and the candidate standard frame landmarks whose multi-frame tracking results satisfy the triangulation principle are determined as the first permanent landmarks and added to the local map; Based on the pyramid LK optical flow algorithm, multiple second candidate features are used as candidate event frame landmarks for multi-frame tracking in the event frame, and the candidate event frame landmarks whose multi-frame tracking results satisfy the triangulation principle are determined as the second permanent landmarks and added to the local map; The local map is used to record the three-dimensional coordinate information of the permanent landmark.

[0010] According to an underwater robot positioning method provided by the present invention, the joint optimization function of the underwater robot is constructed according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler velocity error, including: constructing a standard frame reprojection error term based on the image coordinate information and the three-dimensional coordinate information of the first permanent landmark; constructing an event frame reprojection error term according to the image coordinate information and the three-dimensional coordinate information of the second permanent landmark; According to the deviation between the state prediction value and the state variable value of the underwater robot, an inertial measurement unit error term is constructed, wherein the state prediction value is obtained by pre-integrating all inertial measurement unit measurement data between the current frame and the previous frame; the state variable value includes the world coordinate system velocity value, gyroscope deviation and acceleration deviation of the underwater robot; Constructing a Doppler velocity error term according to a difference between a velocity estimate of the underwater robot and a velocity value of the world coordinate system, wherein the velocity estimate is determined based on a velocity measurement value measured by the Doppler velocity meter; The joint optimization function is constructed with the goal of minimizing the standard frame reprojection error term, the event frame reprojection error term, the inertial measurement unit error term and the Doppler velocity error term.

[0011] According to an underwater robot positioning method provided by the present invention, the joint optimization function of the underwater robot is specifically: ; in, represents the index of the camera image frame, Indicates the current permanent landmark index. Indicates A collection of persistent landmark indices in the frame event frame, Indicates A collection of permanent landmark indices in the frame standard frame, Indicates Frame event frame The information matrix of the event frame error of a permanent landmark, Indicates Frame Standard Frame The information matrix of the standard frame error of permanent landmarks, Indicates The information matrix of the inertial measurement unit error in the frame, Indicates Information matrix of Doppler velocimeter errors in the frame; , , and They respectively represent the event frame reprojection error term, the standard frame reprojection error term, the inertial measurement unit error term and the Doppler velocity error term.

[0012] The present invention also provides an underwater robot positioning system, comprising: An acquisition module, used to acquire camera images and event data streams acquired by the underwater robot, wherein the timestamp of the standard frame in the camera image and the timestamp of the event frame in the event data stream are synchronized; A motion compensated event frame generating module, used for performing event motion compensation on the event frame in the event data stream to obtain a motion compensated event frame; A feature detection and tracking module, used to obtain a plurality of first candidate features and a plurality of second candidate features, and perform multi-frame tracking on the plurality of first candidate features and the plurality of second candidate features respectively, to determine a first permanent landmark and a second permanent landmark, wherein the first candidate features are feature points extracted from the standard frame, and the second candidate features are feature points extracted from the event frame after motion compensation; the first permanent landmark is the first candidate feature whose result of multi-frame tracking meets a preset condition, and the second permanent landmark is the second candidate feature whose result of multi-frame tracking meets the preset condition; A multi-source information joint optimization module is used to construct a joint optimization function of the underwater robot based on a standard frame reprojection error, an event frame reprojection error, an inertial measurement unit error, and a Doppler velocity meter speed error, and obtain the positioning state information of the underwater robot based on a solution result of the joint optimization function, wherein the standard frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the first permanent landmark, and the event frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the second permanent landmark.

[0013] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, any of the above-described underwater robot positioning methods is implemented.

[0014] The present invention also provides a non-transitory computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the underwater robot positioning method as described in any one of the above is implemented.

[0015] The present invention also provides a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the underwater robot positioning method as described above is implemented.

[0016] The underwater robot positioning method and system provided by the present invention generate motion correction event frames, then extract and track feature points of event frames and standard frames respectively, and then use the reprojection error terms, IMU inertia error terms and DVL velocity error terms of motion compensation event frames and standard frames to establish a joint optimization problem of the cost function and solve it directly, avoiding explicit switching between event frames and standard frames, and realizing accurate and robust autonomous positioning of the underwater robot in a weak-light underwater environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0018] Figure 1 A schematic diagram of the flow of the underwater robot positioning method provided by the present invention; Figure 2 A schematic diagram of the structure of the underwater robot provided by the present invention; Figure 3 A schematic diagram of the division of the event time window provided by the present invention; Figure 4 A schematic diagram of event frame motion compensation provided by the present invention; Figure 5 A schematic diagram of the reason for the multi-sensor fusion underwater positioning provided by the present invention; Figure 6 A schematic diagram of the structure of the underwater robot positioning system provided by the present invention; Figure 7 This is a schematic structural diagram of an electronic device provided by the present invention. DETAILED DESCRIPTION

[0019] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0020] Due to the low cost and high accuracy of visual sensors in long-distance positioning, many autonomous positioning technologies based on standard cameras have been developed. These technologies are widely used in autonomous navigation of drones and ground unmanned vehicles. In recent years, researchers have begun to pay attention to the possibility of applying such technologies to underwater robot positioning. However, due to the particularity of the underwater environment, including the absorption of light by water and the scattering of light by particles in the water, the quality of underwater images is reduced and the feature information is scarce, which seriously affects the positioning accuracy of underwater robots based on standard cameras.

[0021] Different from standard cameras, event cameras, as a new type of visual sensor inspired by biology, show significant advantages in high-speed motion, high brightness and low light conditions. In view of the fact that real underwater environments are often insufficiently lit, this paper innovatively introduces event cameras for autonomous positioning of underwater robots, and proposes a comprehensive positioning method for underwater robots that integrates event data, image information, inertial navigation and acoustic data. This method can achieve accurate and stable autonomous positioning of underwater robots in complex environments.

[0022] Figure 1 A schematic diagram of the process flow of the underwater robot positioning method provided by the present invention is shown in FIG. Figure 1 As shown, the present invention provides an underwater robot positioning method, comprising: Step 101 : acquiring a camera image and an event data stream collected by an underwater robot, wherein the timestamp of a standard frame in the camera image and the timestamp of an event frame in the event data stream are synchronized.

[0023] In the present invention, the positioning device of the underwater robot includes an event camera and a Doppler speedometer, wherein the event camera integrates the functions of an event camera and a standard camera, and has an IMU data measurement function, that is, it can output event data streams, standard camera images, and six-axis IMU inertial data; the Doppler speedometer can output speed data.

[0024] Specifically, Figure 2 The schematic diagram of the underwater robot provided by the present invention can be referred to Figure 2As shown, the underwater robot provided by the present invention realizes underwater positioning based on fusion data such as events, images, inertia and acoustics. The underwater robot mainly includes an event camera, a Doppler velocity meter, an onboard controller and an underwater robot. The model of the event camera is DAVIS-346-Color, which can output event streams and standard images at the same time. The spatial resolution of the event frame and the standard frame is 346×260, and the dynamic range of the event frame and the standard frame is 120dB and 55dB respectively; in addition, the event camera is equipped with a six-axis IMU and hardware time synchronization to provide high-frequency linear acceleration information and angular velocity information. The model of the Doppler velocity meter is waterlinked A50, and the maximum measurement distance is 50m. The onboard controller includes a NUC11 industrial computer and a Raspberry Pi. The NUC11 industrial computer realizes the positioning of the underwater robot by fusing event frames, standard frames, inertia and acoustic information, and the Raspberry Pi realizes the motion control of the underwater robot. The model of the underwater robot is BlueROV, which is connected to the NUC11 computer via Ethernet.

[0025] In the present invention, after acquiring the camera image and event data stream, it is necessary to synchronize the spatiotemporal window of the event based on the standard frame timestamp so that the timestamp of the standard frame in the camera image and the timestamp of the event frame in the event data stream are synchronized.

[0026] Based on the above embodiment, the process of generating an event frame is as follows: Generate an event frame of the event data stream at the current moment according to the event information triggered within the time window at the current moment; Among them, the time window at the current moment is constructed according to the time interval between the standard frame at the current moment and the standard frame at the previous moment in the camera image, and the event information includes event pixel coordinate information corresponding to the target pixel, and the target pixel is the pixel that triggers the event.

[0027] Figure 3 For a schematic diagram of the division of the event time window provided by the present invention, please refer to Figure 3 As shown, in the present invention, After receiving a standard frame image at the time (i.e. the current time), Before the moment (i.e. Time has come Triggered by A period of time for recording event information is called a time window , and then according to this time window The event information in generates a new event window set, namely the first k The event set of an event time window is ,in, Indicated in Time has come The sequence number of the first event generated between the times in the event sequence, Represents the total number of events contained in this event time window. It can be seen that the duration of each event time window is inversely proportional to the rate at which events are generated.

[0028] After obtaining the time window of the event, based on the pixel The intensity value of the event frame In pixels The idea of ​​the number of trigger events is to generate event frames as follows: ; in, Represents an event set Middle The sequence number of the event, Indicates events, Indicates The pixel coordinates of the event, represents the generated event frame, To generate event frames at pixel coordinates The intensity value at Represents the Kronecker Delta function.

[0029] Step 102, performing event motion compensation on the event frame in the event data stream to obtain a motion-compensated event frame. Based on the above embodiment, the specific process of step 102 is as follows: According to a camera attitude conversion matrix and a depth value of the target pixel, motion correction is performed on the event pixel coordinate information of the target pixel to obtain the event pixel coordinate information after motion correction, wherein the camera attitude conversion matrix is ​​obtained by pre-integrating a camera motion measurement value of an inertial measurement unit within a preset time interval; The motion compensated event frame is generated according to the motion corrected event pixel coordinate information.

[0030] In the process of generating event frames, when the number of events used to synthesize event frames is small, the generated event frame information is sparse, which makes it difficult to obtain sufficient information for subsequent feature point extraction and tracking. When a large number of events are selected to be directly used for synthesizing event frames, if these event information are only directly accumulated, motion blur may be caused. Therefore, the present invention corrects each event according to the time when the event is generated and the motion information of the camera, so as to generate a motion compensated event frame using the corrected event for subsequent feature point detection and tracking.

[0031] Figure 4For a schematic diagram of event frame motion compensation provided by the present invention, please refer to Figure 4 As shown, in the present invention, in the event time window After obtaining the event information, the influence of motion distortion is eliminated by fusing the IMU high-frequency data to generate a motion-compensated event frame. That is, each event information is corrected and then synthesized into an event frame. The formula is: in, The event frame after motion compensation in pixel coordinates The intensity value at . It represents the event pixel coordinate information after motion correction. The motion correction calculation formula of the event pixel coordinate is as follows: ; in, is the projection model of the camera, obtained by calibrating the camera parameters; For Time has come The camera pose transformation matrix at the moment; Indicates that in the event window, The time difference of the most recent event. In the present invention, the camera attitude transformation matrix Through Perform linear interpolation to obtain, Then, the IMU can be The measured values ​​within the interval (i.e. the preset time interval) are pre-integrated. It's pixels The depth value can be estimated by two-dimensional linear interpolation of the landmarks (i.e., permanent landmarks, for details, please refer to the subsequent content about permanent landmarks in the present invention) re-projected on the current camera frame image plane. Specifically, the depth is estimated for the position of each event in the image plane based on the pixel coordinates of these landmarks contained in the current camera plane and the depth information of the landmarks. The present invention uses the median depth of the current landmark instead of the linear interpolation depth, which can reduce the calculation cost while obtaining satisfactory results. It can be seen from the motion correction calculation formula of the above event pixel coordinates that the quality of motion compensation is closely related to the quality of the available landmark three-dimensional coordinate information. Therefore, in the present invention, the three-dimensional coordinate information of the landmark obtained in the standard frame can be used to improve the quality of the event frame.

[0032] The present invention estimates the depth of each event in the image plane based on the pixel coordinates of the landmarks contained in the current camera plane and the depth information of the landmarks, and then combines the camera imaging model and IMU pre-integration to obtain the pixel coordinates of each event after motion compensation, thereby improving the quality of the event frame.

[0033] Step 103, obtaining multiple first candidate features and multiple second candidate features, and performing multi-frame tracking on the multiple first candidate features and the multiple second candidate features respectively, to determine a first permanent landmark and a second permanent landmark, wherein the first candidate features are feature points extracted from the standard frame, and the second candidate features are feature points extracted from the motion compensated event frame; the first permanent landmark is the first candidate feature whose result of multi-frame tracking meets a preset condition, and the second permanent landmark is the second candidate feature whose result of multi-frame tracking meets the preset condition.

[0034] Based on the above embodiment, the specific process of step 103 is as follows: Based on the FAST corner detector, feature extraction is performed on the standard frame and the event frame after motion compensation to obtain the first candidate feature and the second candidate feature; Based on the pyramid LK optical flow algorithm, multiple first candidate features are used as candidate standard frame landmarks for multi-frame tracking in the standard frame, and the candidate standard frame landmarks whose multi-frame tracking results satisfy the triangulation principle are determined as the first permanent landmarks and added to the local map; Based on the pyramid LK optical flow algorithm, multiple second candidate features are used as candidate event frame landmarks for multi-frame tracking in the event frame, and the candidate event frame landmarks whose multi-frame tracking results satisfy the triangulation principle are determined as the second permanent landmarks and added to the local map; The local map is used to record the three-dimensional coordinate information of the permanent landmark.

[0035] In the present invention, the features on the event frame and the standard frame are extracted as candidate features by using the FAST corner detector, and these candidate features on the event frame and the standard frame are tracked independently by using the pyramid Lukas-Kanade (LK) optical flow method, and the candidate features that meet the triangulation principle are used as permanent landmarks, that is, when a candidate feature can be reliably triangulated, the candidate feature is recorded as a permanent landmark and added to the local map. When the number of tracked candidate feature points is lower than a certain threshold, the key frame is reselected and feature detection is performed.

[0036] Specifically, in the present invention, the standard frame candidate features (i.e., the first candidate features) are extracted from the standard frame using the FAST (Features from Accelerated Segment Test) corner detector. FAST is an efficient corner detection algorithm that quickly identifies corners by comparing the brightness difference between a pixel and its surrounding pixels. In the present invention, the threshold of FAST is set to 30, that is, only when the brightness difference between the candidate point and the surrounding pixels is greater than 30, it will be identified as a corner point (i.e., a candidate feature). The motion-compensated event frame candidate features (i.e., the second candidate features) are also obtained by extracting features from the video frame using the FAST corner detector.

[0037] Furthermore, the pyramid LK optical flow algorithm is used to perform multi-frame tracking on the extracted first candidate feature and the second candidate feature. The pyramid LK optical flow algorithm is a method for estimating pixel motion in an image sequence. It handles motion of different scales by constructing an image pyramid, thereby improving the robustness and accuracy of optical flow estimation. In this process, each candidate feature (whether from a standard frame or an event frame) is tracked across multiple frames to form candidate landmarks (candidate standard frame landmarks and candidate event frame landmarks).

[0038] The results of multi-frame tracking are recorded as permanent landmarks only when their 3D positions can be reliably determined through the triangulation principle. The triangulation principle is based on the observation of feature points from multiple perspectives, and the 3D spatial position of feature points is determined through geometric calculations, requiring observation data from at least two different perspectives. Once a candidate landmark meets the triangulation conditions, the candidate landmark is considered reliable and is added to a local map, which is used to record the coordinate information of all determined 3D landmarks (i.e. permanent landmarks).

[0039] In the present invention, if the number of feature points drops below a preset threshold during the tracking process, a new keyframe can be selected and the above feature extraction and tracking process can be repeated to ensure that there are a sufficient number of landmarks. In addition, in order to ensure that the feature points are evenly distributed across the entire image plane, the present invention uses a barrel grid, each grid size is 32×32 pixels, to avoid excessive concentration of feature points in some areas and too few in other areas, thereby improving the efficiency and accuracy of feature extraction and tracking.

[0040] Step 104, constructing a joint optimization function of the underwater robot according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler velocity error, and obtaining the positioning state information of the underwater robot according to the solution result of the joint optimization function, wherein the standard frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the first permanent landmark, and the event frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the second permanent landmark.

[0041] Figure 5 This is a schematic diagram of the reason for the multi-sensor fusion underwater positioning provided by the present invention, and a schematic diagram of the overall process of the underwater robot positioning method provided by the present invention, which can be referred to Figure 5 As shown, in the present invention, by minimizing the reprojection error term, IMU inertia error term and DVL velocity error term of the event frame and the standard frame, a joint optimization problem of the cost function is constructed for solution, thereby obtaining the state information of the underwater robot, including position, posture and velocity, etc. By jointly optimizing the cost function, explicit switching between the event frame and the standard frame can be avoided.

[0042] The underwater robot positioning method provided by the present invention generates a motion correction event frame, then extracts and tracks feature points of the event frame and the standard frame respectively, and then uses the reprojection error term of the motion compensation event frame and the standard frame, the IMU inertia error term and the DVL velocity error term to establish a joint optimization problem of the cost function and directly solve it, thereby avoiding explicit switching between the event frame and the standard frame, and realizing accurate and robust autonomous positioning of the underwater robot in a weak-light underwater environment.

[0043] On the basis of the above embodiment, the joint optimization function of the underwater robot is constructed according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler velocity error, including: constructing a standard frame reprojection error term based on the image coordinate information and the three-dimensional coordinate information of the first permanent landmark; constructing an event frame reprojection error term according to the image coordinate information and the three-dimensional coordinate information of the second permanent landmark; According to the deviation between the state prediction value and the state variable value of the underwater robot, an inertial measurement unit error term is constructed, wherein the state prediction value is obtained by pre-integrating all inertial measurement unit measurement data between the current frame and the previous frame; the state variable value includes the world coordinate system velocity value, gyroscope deviation and acceleration deviation of the underwater robot; Constructing a Doppler velocity error term according to a difference between a velocity estimate of the underwater robot and a velocity value of the world coordinate system, wherein the velocity estimate is determined based on a velocity measurement value measured by the Doppler velocity meter; The joint optimization function is constructed with the goal of minimizing the standard frame reprojection error term, the event frame reprojection error term, the inertial measurement unit error term and the Doppler velocity error term.

[0044] In the present invention, firstly, based on the local map in the above embodiment, permanent landmarks corresponding to the standard frame and the event frame are obtained.

[0045] Then, according to the image coordinate information and three-dimensional coordinate information of the permanent landmark (i.e., the first permanent landmark) in the standard frame, the standard frame reprojection error term is obtained: : ; in, For the k Frame Standard Frame The image coordinate information of the first permanent landmark, For the The coordinates of the first permanent landmark in the world coordinate system, Indicated in k The transformation matrix from the world coordinate system to the IMU coordinate system at the moment, For the k The transformation matrix from the IMU coordinate system to the camera coordinate system at this moment.

[0046] According to the image coordinate information and three-dimensional coordinate information of the permanent landmark (i.e., the second permanent landmark) in the event frame, the event frame reprojection error term is obtained. : ; in, For the k The event frame The image coordinate information of the second permanent landmark, For the The coordinates of the second permanent landmark in the world coordinate system.

[0047] Since the measurement frequency of the IMU is much higher than that of the camera, there will be multiple IMU measurement values ​​between two key frames. The present invention can obtain the initial values ​​of the position, attitude and velocity increment between two key frames through IMU pre-integration. By defining the IMU coordinate system as the underwater robot coordinate system, all IMU measurement data between two key frames can be obtained. Integrate to get the predicted value of the current state based on the previous state , get the deviation between the state prediction value and the state variable value of the underwater robot, and construct the inertial measurement unit error term Specifically: ; in, represents the position of the underwater robot in the world coordinate system, represents the prediction of the position of the underwater robot in the world coordinate system by the inertial sensor data, represents the posture of the underwater robot, It indicates the prediction of the underwater robot's posture based on IMU data. , is the speed of the underwater robot in the world coordinate system, is the gyro bias, is the acceleration deviation.

[0048] Furthermore, according to the motion relationship between DVL and IMU in the world coordinate system, the velocity of DVL in the robot coordinate system is expressed as: ; in, is the speed of DVL in the robot coordinate system, is the rotation matrix from the world coordinate system to the robot coordinate system, is the position information of DVL in the robot coordinate system, is the speed information obtained by DVL measurement, is the rotation matrix from the DVL coordinate system to the robot coordinate system, It can be expressed as: ; in, , and are the three angular velocities of the IMU. Then, the DVL velocity error term is constructed based on the deviation between the velocity estimate of the current state of the underwater robot and the velocity in the world coordinate system in the underwater robot state variable: ; Based on the above embodiment, the joint optimization function of the underwater robot is specifically: ; in, represents the index of the camera image frame, Indicates the current permanent landmark index. Indicates A collection of persistent landmark indices in the frame event frame, Indicates A collection of permanent landmark indices in the frame standard frame, Indicates Frame event frame The information matrix of the event frame error of a permanent landmark, Indicates Frame Standard Frame The information matrix of the standard frame errors of permanent landmarks; Indicates The information matrix of the inertial measurement unit error in the frame, Indicates The information matrix of the Doppler velocity meter error in the frame, where the information matrix is ​​the weight of the sensor error in the cost function; , , and They respectively represent the event frame reprojection error term, the standard frame reprojection error term, the inertial measurement unit error term and the Doppler velocity error term.

[0049] The underwater robot positioning system provided by the present invention is described below. The underwater robot positioning system described below and the underwater robot positioning method described above can be referred to each other.

[0050] Figure 6 The schematic diagram of the underwater robot positioning system provided by the present invention is as follows: Figure 6As shown, the present invention provides an underwater robot positioning system, including an acquisition module 601, a motion compensation event frame generation module 602, a feature detection and tracking module 603 and a multi-source information joint optimization module 604, wherein the acquisition module 601 is used to obtain the camera image and event data stream collected by the underwater robot, wherein the timestamp of the standard frame in the camera image and the timestamp of the event frame in the event data stream are synchronized; the motion compensation event frame generation module 602 is used to perform event motion compensation on the event frame in the event data stream to obtain the motion compensated event frame; the feature detection and tracking module 603 is used to obtain multiple first candidate features and multiple second candidate features, and perform multi-frame tracking on the multiple first candidate features and the multiple second candidate features respectively to determine a first permanent landmark and a second permanent landmark, wherein the first candidate feature is obtained from the The feature points are extracted from the standard frame, and the second candidate features are feature points extracted from the event frame after motion compensation; the first permanent landmark is the first candidate feature whose result of multi-frame tracking meets the preset conditions, and the second permanent landmark is the second candidate feature whose result of multi-frame tracking meets the preset conditions; the multi-source information joint optimization module 604 is used to construct a joint optimization function of the underwater robot according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler speedometer speed error, and obtain the positioning state information of the underwater robot according to the solution result of the joint optimization function, wherein the standard frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the first permanent landmark, and the event frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the second permanent landmark.

[0051] The underwater robot positioning system provided by the present invention generates a motion correction event frame, then extracts and tracks feature points of the event frame and the standard frame respectively, and then uses the reprojection error term of the motion compensation event frame and the standard frame, the IMU inertia error term and the DVL velocity error term to establish a joint optimization problem of the cost function and directly solve it, thereby avoiding explicit switching between the event frame and the standard frame, and realizing accurate and robust autonomous positioning of the underwater robot in a weak-light underwater environment.

[0052] The system provided in the embodiment of the present invention is used to execute the above-mentioned method embodiments. Please refer to the above-mentioned embodiments for the specific process and detailed contents, which will not be repeated here.

[0053] Figure 7 A schematic diagram of the structure of an electronic device provided by the present invention, such as Figure 7As shown, the electronic device may include: a processor (Processor) 701, a communication interface (Communications Interface) 702, a memory (Memory) 703 and a communication bus 704, wherein the processor 701, the communication interface 702, and the memory 703 communicate with each other through the communication bus 704. The processor 701 can call the logic instructions in the memory 703 to execute the underwater robot positioning method, which includes: obtaining the camera image and event data stream collected by the underwater robot, wherein the timestamp of the standard frame in the camera image and the timestamp of the event frame in the event data stream are synchronized; performing event motion compensation on the event frame in the event data stream to obtain the motion-compensated event frame; obtaining multiple first candidate features and multiple second candidate features, and performing multi-frame tracking on the multiple first candidate features and the multiple second candidate features respectively, to determine the first permanent landmark and the second permanent landmark, wherein the first candidate feature is a feature point extracted from the standard frame, and the second candidate feature is a feature point extracted from the motion compensated frame. The first permanent landmark is the first candidate feature whose result of multi-frame tracking meets the preset condition, and the second permanent landmark is the second candidate feature whose result of multi-frame tracking meets the preset condition; according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler speedometer speed error, a joint optimization function of the underwater robot is constructed, and according to the solution result of the joint optimization function, the positioning state information of the underwater robot is obtained, wherein the standard frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the first permanent landmark, and the event frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the second permanent landmark.

[0054] In addition, the logic instructions in the above-mentioned memory 703 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, 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, and 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 method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a disk or an optical disk.

[0055] On the other hand, the present invention also provides a computer program product, which includes a computer program stored on a non-transitory computer-readable storage medium, and the computer program includes program instructions. When the program instructions are executed by a computer, the computer can execute the underwater robot positioning method provided by the above methods, and the method includes: acquiring a camera image and an event data stream collected by the underwater robot, wherein the timestamp of the standard frame in the camera image and the timestamp of the event frame in the event data stream are synchronized; performing event motion compensation on the event frame in the event data stream to obtain a motion-compensated event frame; acquiring a plurality of first candidate features and a plurality of second candidate features, and performing multi-frame tracking on the plurality of the first candidate features and the plurality of the second candidate features, respectively, to determine a first permanent landmark and a second permanent landmark, wherein the first The candidate features are feature points extracted from the standard frame, and the second candidate features are feature points extracted from the event frame after motion compensation; the first permanent landmark is the first candidate feature whose result of multi-frame tracking meets the preset conditions, and the second permanent landmark is the second candidate feature whose result of multi-frame tracking meets the preset conditions; according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler velocity error, a joint optimization function of the underwater robot is constructed, and according to the solution result of the joint optimization function, the positioning state information of the underwater robot is obtained, wherein the standard frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the first permanent landmark, and the event frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the second permanent landmark.

[0056] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which is implemented when the processor executes the underwater robot positioning method provided in the above embodiments, the method comprising: acquiring a camera image and an event data stream collected by the underwater robot, wherein the timestamp of the standard frame in the camera image and the timestamp of the event frame in the event data stream are synchronized; performing event motion compensation on the event frame in the event data stream to obtain a motion-compensated event frame; acquiring a plurality of first candidate features and a plurality of second candidate features, and performing multi-frame tracking on the plurality of the first candidate features and the plurality of the second candidate features, respectively, to determine a first permanent landmark and a second permanent landmark, wherein the first candidate feature is a feature extracted from the standard frame; The first permanent landmark is a feature point extracted from the event frame after motion compensation; the first permanent landmark is the first candidate feature whose result of multi-frame tracking meets the preset conditions, and the second permanent landmark is the second candidate feature whose result of multi-frame tracking meets the preset conditions; according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler velocity error, a joint optimization function of the underwater robot is constructed, and according to the solution result of the joint optimization function, the positioning state information of the underwater robot is obtained, wherein the standard frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the first permanent landmark, and the event frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the second permanent landmark.

[0057] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0058] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0059] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for positioning an underwater robot, characterized in that: include: Acquire a camera image and an event data stream collected by the underwater robot, wherein the timestamp of the standard frame in the camera image and the timestamp of the event frame in the event data stream are synchronized; Performing event motion compensation on the event frames in the event data stream to obtain motion-compensated event frames; Acquire multiple first candidate features and multiple second candidate features, and perform multi-frame tracking on the multiple first candidate features and the multiple second candidate features respectively to determine a first permanent landmark and a second permanent landmark, wherein the first candidate features are feature points extracted from the standard frame, and the second candidate features are feature points extracted from the event frame after motion compensation; the first permanent landmark is the first candidate feature whose result of multi-frame tracking meets a preset condition, and the second permanent landmark is the second candidate feature whose result of multi-frame tracking meets the preset condition; A joint optimization function of the underwater robot is constructed according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler velocity error, and the positioning state information of the underwater robot is obtained according to the solution result of the joint optimization function, wherein the standard frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the first permanent landmark, and the event frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the second permanent landmark.

2. The underwater robot positioning method according to claim 1, characterized in that: The method further comprises: Generate an event frame of the event data stream at the current moment according to the event information triggered within the time window at the current moment; Among them, the time window at the current moment is constructed according to the time interval between the standard frame at the current moment and the standard frame at the previous moment in the camera image, and the event information includes event pixel coordinate information corresponding to the target pixel, and the target pixel is the pixel that triggers the event.

3. The underwater robot positioning method according to claim 2, characterized in that: The performing event motion compensation on the event frame in the event data stream to obtain the motion-compensated event frame comprises: According to a camera attitude conversion matrix and a depth value of the target pixel, motion correction is performed on the event pixel coordinate information of the target pixel to obtain the event pixel coordinate information after motion correction, wherein the camera attitude conversion matrix is ​​obtained by pre-integrating a camera motion measurement value of an inertial measurement unit within a preset time interval; The motion compensated event frame is generated according to the motion corrected event pixel coordinate information.

4. The underwater robot positioning method according to claim 3, characterized in that: The step of acquiring a plurality of first candidate features and a plurality of second candidate features, and performing multi-frame tracking on the plurality of first candidate features and the plurality of second candidate features respectively based on a pyramid optical flow method to determine a first permanent landmark and a second permanent landmark includes: Based on the FAST corner detector, feature extraction is performed on the standard frame and the event frame after motion compensation to obtain the first candidate feature and the second candidate feature; Based on the pyramid LK optical flow algorithm, multiple first candidate features are used as candidate standard frame landmarks for multi-frame tracking in the standard frame, and the candidate standard frame landmarks whose multi-frame tracking results satisfy the triangulation principle are determined as the first permanent landmarks and added to the local map; Based on the pyramid LK optical flow algorithm, multiple second candidate features are used as candidate event frame landmarks for multi-frame tracking in the event frame, and the candidate event frame landmarks whose multi-frame tracking results satisfy the triangulation principle are determined as the second permanent landmarks and added to the local map; The local map is used to record the three-dimensional coordinate information of the permanent landmark.

5. The underwater robot positioning method according to claim 4, characterized in that: The method of constructing a joint optimization function of the underwater robot according to the standard frame reprojection error, the event frame reprojection error, the inertial measurement unit error and the Doppler velocity error comprises: constructing a standard frame reprojection error term based on the image coordinate information and the three-dimensional coordinate information of the first permanent landmark; constructing an event frame reprojection error term according to the image coordinate information and the three-dimensional coordinate information of the second permanent landmark; According to the deviation between the state prediction value and the state variable value of the underwater robot, an inertial measurement unit error term is constructed, wherein the state prediction value is obtained by pre-integrating all inertial measurement unit measurement data between the current frame and the previous frame; the state variable value includes the world coordinate system velocity value, gyroscope deviation and acceleration deviation of the underwater robot; Constructing a Doppler velocity error term according to a difference between a velocity estimate of the underwater robot and a velocity value of the world coordinate system, wherein the velocity estimate is determined based on a velocity measurement value measured by the Doppler velocity meter; The joint optimization function is constructed with the goal of minimizing the standard frame reprojection error term, the event frame reprojection error term, the inertial measurement unit error term and the Doppler velocity error term.

6. The underwater robot positioning method according to claim 5, characterized in that: The joint optimization function of the underwater robot is specifically: ； in, represents the index of the camera image frame, Indicates the current permanent landmark index. Indicates A collection of persistent landmark indices in the frame event frame, Indicates A collection of permanent landmark indices in the frame standard frame, Indicates Frame event frame The information matrix of the event frame error of a permanent landmark, Indicates Frame Standard Frame The information matrix of the standard frame error of permanent landmarks, Indicates The information matrix of the inertial measurement unit error in the frame, Indicates Information matrix of Doppler velocimeter errors in the frame; , , and They respectively represent the event frame reprojection error term, the standard frame reprojection error term, the inertial measurement unit error term and the Doppler velocity error term.

7. An underwater robot positioning system, characterized in that: include: An acquisition module, used to acquire camera images and event data streams acquired by the underwater robot, wherein the timestamp of the standard frame in the camera image and the timestamp of the event frame in the event data stream are synchronized; A motion compensated event frame generating module, used for performing event motion compensation on the event frames in the event data stream to obtain motion compensated event frames; A feature detection and tracking module, used to obtain a plurality of first candidate features and a plurality of second candidate features, and perform multi-frame tracking on the plurality of first candidate features and the plurality of second candidate features respectively, to determine a first permanent landmark and a second permanent landmark, wherein the first candidate features are feature points extracted from the standard frame, and the second candidate features are feature points extracted from the event frame after motion compensation; the first permanent landmark is the first candidate feature whose result of multi-frame tracking meets a preset condition, and the second permanent landmark is the second candidate feature whose result of multi-frame tracking meets the preset condition; A multi-source information joint optimization module is used to construct a joint optimization function of the underwater robot based on a standard frame reprojection error, an event frame reprojection error, an inertial measurement unit error, and a Doppler velocity meter speed error, and obtain the positioning state information of the underwater robot based on a solution result of the joint optimization function, wherein the standard frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the first permanent landmark, and the event frame reprojection error is constructed based on the image coordinates and three-dimensional coordinates of the second permanent landmark.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the underwater robot positioning method according to any one of claims 1 to 6 is implemented.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the underwater robot positioning method according to any one of claims 1 to 6 is implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the underwater robot positioning method according to any one of claims 1 to 6 is implemented.

Images