Underwater robot track monitoring method based on computer image analysis
By using computer image analysis and laser calibration, the problem of monitoring the motion trajectory of underwater robots has been solved, achieving low-cost and efficient monitoring of motion status and trajectory.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- SHANDONG JIANZHU UNIV
- Filing Date
- 2026-01-21
- Publication Date
- 2026-05-01
AI Technical Summary
In fields such as marine life harvesting, underwater robots face challenges in monitoring their own movement trajectories, which is both difficult and costly. Existing high-cost solutions are not suitable for civilian robots.
A computer image analysis-based approach was adopted, using monocular vision and two lasers, and employing the SIFT feature matching algorithm and laser calibration to monitor the motion state and trajectory of an underwater robot.
It achieves low-cost and accurate underwater robot motion trajectory monitoring by using an underwater monocular camera and laser to match feature points and calibrate distances in images, generating detailed motion state and trajectory data.
Smart Images
Figure CN121962200A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to a method for monitoring the trajectory of an underwater robot based on computer image analysis, belonging to the field of underwater robot trajectory monitoring. Background Technology
[0002] Underwater robots can be mainly divided into three categories: remotely operated vehicles (ROVs), autonomous underwater vehicles (AUVs), and human-occupied vehicles (HOVs). Among them, ROVs rely on cables for continuous power supply and have outstanding advantages such as high efficiency in real-time data transmission, excellent underwater operation performance, and long endurance. They are currently widely used in various fields such as safety inspection of inland river dams, underwater target search and rescue, marine life detection, and fishing.
[0003] With technological advancements, underwater robots have seen rapid development and application in fields such as autonomous aquaculture and harvesting of marine life. Autonomous grasping of marine organisms like sea cucumbers, sea urchins, and scallops is a significant application area for underwater robots. However, because the underwater environment cannot transmit positioning signals like GPS, monitoring the movement trajectory of underwater robots during underwater operations has always been a challenging problem. While some underwater robots now utilize sonar for autonomous navigation, most robots used in marine aquaculture and harvesting are civilian applications, making costly trajectory monitoring solutions unsuitable. Summary of the Invention
[0004] To address the challenges and high costs associated with monitoring the movement trajectory of underwater marine life harvesting robots in underwater environments, this invention proposes a method for monitoring the trajectory of underwater robots based on computer image analysis.
[0005] This invention is achieved through the following technical solution: A method for monitoring the trajectory of an underwater robot based on computer image analysis, implemented using monocular vision, includes: S1: Track monitoring sensor module assembly: It consists of a module mounting bracket, sensor bracket, underwater monocular camera, left optical axis bracket, right optical axis bracket, left laser, right laser, STM32 control board and breadboard. The underwater monocular camera is fixedly mounted on the sensor bracket using nylon cable ties. The left and right optical axis brackets are fixedly mounted on the left and right sides of the sensor bracket with bolts and nuts respectively. The left laser is mounted on the left optical axis bracket and the right laser is mounted on the right optical axis bracket. The distance between the centers of the two lasers is d. The breadboard is fixedly mounted on the sensor bracket with tape. The sensor bracket is fixedly mounted on the module mounting bracket with bolts and nuts. S2: Data Acquisition: After entering the work area, the computer controls the underwater monocular camera to take the initial image, and then takes an image every 1 second during the robot's movement until the underwater robot's work process at the bottom of the water ends. S3: Image feature extraction and matching: Use the SIFT feature matching algorithm to extract features from the n captured images, and match the similar feature points in the (n-1)th image and the nth image one by one; S4: Motion state analysis: By matching feature points in the (n-1)th image and the nth image, the movement direction and rotation of the underwater robot can be analyzed based on the movement direction or rotation direction and angle of the feature points in the image. S5: Laser calibration and movement distance measurement: Adjust the position of the two lasers relative to the underwater monocular camera, and use the two lasers to emit two laser points on the left and right sides of the underwater monocular camera. In the captured image, the two laser points are on the same straight line as the origin of the image pixel coordinates; the distance moved by the underwater robot can be obtained by calibrating the two laser points. S6: Generate motion trajectory: Through n-1 image analysis, obtain the underwater robot's movement direction, movement distance, rotation direction and angle motion data during the movement process. Accumulate and process the motion data analysis to obtain the detailed motion state and motion trajectory of the underwater robot during the operation.
[0006] Furthermore, in step S1, in order to avoid the laser point being too diffuse in the image captured by the underwater camera, an STM32 control board is used for the laser, and the laser power supply voltage is 3~3.3V to maintain a dim laser point. The underwater monocular camera is connected to the computer via USB port.
[0007] Furthermore, step S4 includes the following steps: S4.1: Setting the pixel coordinates of key feature points: The SIFT feature matching algorithm extracts 'a' feature points from the (n-1)th image and the nth image, and sets the pixel coordinates of the feature points in the (n-1)th image to p. i.n-1 = (x i.n-1 ,y i.n-1 (i=1,2,3,…,a), the pixel coordinates of the feature point in the nth image are p i.n = (x i.n ,y i.n (i=1,2,3,…,a); S4.2: Motion state monitoring: based on the change in pixel coordinates of key feature points (∆x) i.n-1 ,∆y i.n-1 To determine whether the underwater robot's motion before taking the nth image is translational or rotational, the following formula is used:
[0008]
[0009] Where, x i.n Let y be the x-coordinate of the i-th feature point in the n-th image. i.n Let x be the ordinate of the pixel of the i-th feature point in the n-th image. i.n-1 Let y be the x-coordinate of the i-th feature point in the (n-1)-th image. i.n-1 Let Δx be the pixel ordinate of the i-th feature point in the (n-1)-th image. i.n-1 ∆y represents the change in the x-coordinate of the i-th feature point pixel in the (n-1)-th image comparison analysis. i.n-1 The change in the ordinate of the pixel of the ith feature point is analyzed in the (n-1)th image comparison. If the result satisfies the equation (1) where the changes in horizontal and vertical pixel coordinates are equal, then the underwater robot's motion is determined to be translational motion; if the result satisfies the equation (2) where the changes in horizontal and vertical pixel coordinates are partially unequal and the changes in horizontal and vertical pixel coordinates of a certain feature point are both 0, then the underwater robot's motion is determined to be rotational motion; if the result satisfies the equation (3) where the changes in horizontal and vertical coordinates are not equal or the changes in horizontal and vertical pixel coordinates are partially unequal and one of the changes in horizontal and vertical pixel coordinates of a certain feature point is 0 and the other is not 0, then it is impossible to determine and step S3 needs to be repeated. S4.3: Translational Movement Direction Measurement: If the underwater robot's motion is determined to be translational, the feature points of the (n-1)th image and the nth image are extracted to the same pixel coordinate system. Then, the feature point pixel coordinates are measured from the pixel coordinate p of the feature point within that same pixel coordinate system. i.n-1 Pointing to the pixel coordinates p of the feature point i.n The direction of the ray is the direction of movement of the feature point. The direction of movement of the feature point is then rotated around p. i.n-1 Rotating 180° will give you the direction of the underwater robot's translational movement. S4.4: Rotation Direction and Angle Measurement: If the underwater robot's motion is determined to be rotational, the feature points of the (n-1)th and nth images are extracted to the same pixel coordinate system. It is known that there exists a feature point I whose horizontal and vertical coordinates do not change in the pixel coordinate system of the (n-1)th and nth images. I =p I.n-1 =p I.n Then it can be determined that the remaining feature points are rotated around feature point I, and feature point p is placed in the same pixel coordinate system. I Connecting to the remaining m feature points, the rotation angle and direction are calculated using the following formula:
[0010]
[0011] in, Let x be the angle of rotation of the m-th feature point (excluding the rotation center feature point I). m.n Let y be the x-coordinate of the m-th feature point in the n-th image. m.n Let x be the ordinate of the m-th feature point in the n-th image. m.n-1 Let y be the x-coordinate of the m-th feature point in the (n-1)-th image. m.n-1 Let x be the ordinate of the m-th feature point in the (n-1)-th image. I Let y be the pixel x-coordinate of the rotation center feature point I. I Let I be the pixel ordinate of the rotation center feature point I; like Then, the feature point rotates clockwise around feature point I in the pixel coordinate system by an angle of _____. The underwater robot rotates to the right by an angle of... ;like Then, the feature point rotates counterclockwise around feature point I in the pixel coordinate system by an angle of _____. The underwater robot rotates to the left by an angle of... .
[0012] Furthermore, step S5 includes the following steps: S5.1: Laser Calibration: Identify the center point of the laser aperture in the image and obtain the corresponding pixel points c1(x1, y1) and c2(x2, y2) in the pixel coordinate system. The distance between pixel points c1 and c2 is... Then the distance of each pixel in the actual environment is ; S5.2: Movement Distance Measurement: Under the condition of translational motion, the pixel distance moved by any feature point is... Then, the actual distance traveled by the underwater robot in the (n-1)th image analysis is .
[0013] Technical effects of the present invention: This invention discloses an underwater robot trajectory monitoring method based on computer image analysis. It uses two lasers to calibrate pixels and actual distances in an image, enabling the underwater robot to measure its own movement distance using an underwater monocular camera. The method employs computer image analysis to implement a motion state judgment strategy for the underwater robot, accurately monitoring its trajectory after confirming its specific motion state. It fully utilizes the characteristic that underwater fishing robots need to remain suspended a certain distance above the seabed during operation, achieving low-cost trajectory monitoring of the underwater robot based on seabed feature extraction and matching. Attached Figure Description
[0014] Figure 1This is a flowchart of an embodiment of the present invention; Figure 2 This is a schematic diagram of the trajectory monitoring sensor module used in this invention.
[0015] In the diagram, 01 is the module mounting bracket, 02 is the sensor bracket, 03 is the right optical axis bracket, 04 is the right laser, 05 is the underwater monocular camera, 06 is the left laser, 07 is the left optical axis bracket, 08 is the breadboard, and 09 is the STM32 control board. Detailed Implementation
[0016] The present invention will be further described below with reference to the embodiments: A method for monitoring the trajectory of an underwater robot based on computer image analysis, implemented using monocular vision, includes six steps: assembling a trajectory monitoring sensor module, data acquisition, image feature extraction and matching, motion state analysis, laser calibration and distance measurement, and generating a motion trajectory.
[0017] S1: Track monitoring sensor module assembly: It consists of module mounting bracket 01, sensor bracket 02, underwater monocular camera 05, left optical axis bracket 07, right optical axis bracket 03, left laser 06, right laser 04, STM32 control board 09, and breadboard 08. The underwater monocular camera 05 is fixedly mounted on the sensor bracket 02 using nylon cable ties. The left optical axis bracket 07 and right optical axis bracket 03 are fixedly mounted on the left and right sides of the sensor bracket 02 respectively with bolts and nuts. The left laser 06 is mounted on the left optical axis bracket 07, and the right laser 04 is mounted on the right optical axis bracket 03. The distance between the centers of the two lasers is d. The breadboard 08 is fixedly mounted on the sensor bracket 02 with tape. The sensor bracket 02 is fixedly mounted on the module mounting bracket 01 with bolts and nuts. To avoid the laser spot being too diffuse in the images captured by the underwater camera, an STM32 control board 09 is used as the laser, with a laser power supply voltage of 3~3.3V to maintain a dim laser spot. The underwater monocular camera 05 is connected to the computer via USB.
[0018] S2: Data Acquisition: After entering the work area, the computer controls the underwater monocular camera to take the initial image, and then takes an image every 1 second during the robot's movement until the underwater robot's work process at the bottom of the water ends. During data collection, the underwater robot is equipped with an end-effector at its bottom, and a certain height must be maintained between the underwater robot and the seabed.
[0019] S3: Image feature extraction and matching: Use the SIFT feature matching algorithm to extract features from the n captured images, and match the similar feature points in the (n-1)th image and the nth image one by one; S4: Motion state analysis: By matching feature points in the (n-1)th image and the nth image, the movement direction and rotation of the underwater robot can be analyzed based on the movement direction or rotation direction and angle of the feature points in the image. Step S4 includes the following steps: S4.1: Setting the pixel coordinates of key feature points: The SIFT feature matching algorithm extracts 'a' feature points from the (n-1)th image and the nth image, and sets the pixel coordinates of the feature points in the (n-1)th image to p. i.n-1 = (x i.n-1 ,y i.n-1 (i=1,2,3,…,a), the pixel coordinates of the feature point in the nth image are p i.n = (x i.n ,y i.n (i=1,2,3,…,a); S4.2: Motion state monitoring: based on the change in pixel coordinates of key feature points (∆x) i.n-1 ,∆y i.n-1 To determine whether the underwater robot's motion before taking the nth image is translational or rotational, the following formula is used:
[0020]
[0021] Where, x i.n Let y be the x-coordinate of the i-th feature point in the n-th image. i.n Let x be the ordinate of the pixel of the i-th feature point in the n-th image. i.n-1 Let y be the x-coordinate of the i-th feature point in the (n-1)-th image. i.n-1 Let Δx be the pixel ordinate of the i-th feature point in the (n-1)-th image. i.n-1 ∆y represents the change in the x-coordinate of the i-th feature point pixel in the (n-1)-th image comparison analysis. i.n-1 The change in the ordinate of the pixel of the ith feature point is analyzed in the (n-1)th image comparison. If the result satisfies the equation (1) where the changes in horizontal and vertical pixel coordinates are equal, then the underwater robot's motion is determined to be translational motion; if the result satisfies the equation (2) where the changes in horizontal and vertical pixel coordinates are partially unequal and the changes in horizontal and vertical pixel coordinates of a certain feature point are both 0, then the underwater robot's motion is determined to be rotational motion; if the result satisfies the equation (3) where the changes in horizontal and vertical coordinates are not equal or the changes in horizontal and vertical pixel coordinates are partially unequal and one of the changes in horizontal and vertical pixel coordinates of a certain feature point is 0 and the other is not 0, then it is impossible to determine and step S3 needs to be repeated. S4.3: Translational Movement Direction Measurement: If the underwater robot's motion is determined to be translational, the feature points of the (n-1)th image and the nth image are extracted to the same pixel coordinate system. Then, the feature point pixel coordinates are measured from the pixel coordinate p of the feature point within that same pixel coordinate system. i.n-1 Pointing to the pixel coordinates p of the feature point i.n The direction of the ray is the direction of movement of the feature point. The direction of movement of the feature point is then rotated around p. i.n-1 Rotating 180° will give you the direction of the underwater robot's translational movement. S4.4: Rotation Direction and Angle Measurement: If the underwater robot's motion is determined to be rotational, the feature points of the (n-1)th and nth images are extracted to the same pixel coordinate system. It is known that there exists a feature point I whose horizontal and vertical coordinates do not change in the pixel coordinate system of the (n-1)th and nth images. I =p I.n-1 =p I.n Then it can be determined that the remaining feature points are rotated around feature point I, and feature point p is placed in the same pixel coordinate system. I Connecting to the remaining m feature points, the rotation angle and direction are calculated using the following formula:
[0022]
[0023] in, Let x be the angle of rotation of the m-th feature point (excluding the rotation center feature point I). m.n Let y be the x-coordinate of the m-th feature point in the n-th image. m.n Let x be the ordinate of the m-th feature point in the n-th image. m.n-1 Let y be the x-coordinate of the m-th feature point in the (n-1)-th image. m.n-1 Let x be the ordinate of the m-th feature point in the (n-1)-th image. I Let y be the pixel x-coordinate of the rotation center feature point I. I Let I be the pixel ordinate of the rotation center feature point I; like Then, the feature point rotates clockwise around feature point I in the pixel coordinate system by an angle of _____. The underwater robot rotates to the right by an angle of... ;like Then, the feature point rotates counterclockwise around feature point I in the pixel coordinate system by an angle of _____. The underwater robot rotates to the left by an angle of... .
[0024] S5: Laser calibration and movement distance measurement: Adjust the position of the two lasers relative to the underwater monocular camera, and use the two lasers to emit two laser points on the left and right sides of the underwater monocular camera. In the captured image, the two laser points are on the same straight line as the origin of the image pixel coordinates; the distance moved by the underwater robot can be obtained by calibrating the two laser points. Step S5 includes the following steps: S5.1: Laser Calibration: Identify the center point of the laser aperture in the image and obtain the corresponding pixel points c1(x1, y1) and c2(x2, y2) in the pixel coordinate system. The distance between pixel points c1 and c2 is... Then the distance of each pixel in the actual environment is ; S5.2: Movement Distance Measurement: Under the condition of translational motion, the pixel distance moved by any feature point is... Then, the actual distance traveled by the underwater robot in the (n-1)th image analysis is .
[0025] S6: Generate motion trajectory: Through n-1 image analysis, obtain the underwater robot's movement direction, movement distance, rotation direction and angle motion data during the movement process. Accumulate and process the motion data analysis to obtain the detailed motion state and motion trajectory of the underwater robot during the operation.
[0026] For those skilled in the art, any changes, modifications, substitutions, and variations made to the embodiments without departing from the principles and spirit of the present invention, based on the teachings of the present invention, still fall within the protection scope of the present invention.
Claims
1. A method for monitoring the trajectory of an underwater robot based on computer image analysis, characterized in that, Based on monocular vision, including: S1: Track monitoring sensor module assembly: It consists of a module mounting bracket, sensor bracket, underwater monocular camera, left optical axis bracket, right optical axis bracket, left laser, right laser, STM32 control board and breadboard. The underwater monocular camera is fixedly mounted on the sensor bracket using nylon cable ties. The left and right optical axis brackets are fixedly mounted on the left and right sides of the sensor bracket with bolts and nuts respectively. The left laser is mounted on the left optical axis bracket and the right laser is mounted on the right optical axis bracket. The distance between the centers of the two lasers is d. The breadboard is fixedly mounted on the sensor bracket with tape. The sensor bracket is fixedly mounted on the module mounting bracket with bolts and nuts. S2: Data Acquisition: After entering the work area, the computer controls the underwater monocular camera to take the initial image, and then takes an image every 1 second during the robot's movement until the underwater robot's work process at the bottom of the water ends. S3: Image feature extraction and matching: Use the SIFT feature matching algorithm to extract features from the n captured images, and match the similar feature points in the (n-1)th image and the nth image one by one; S4: Motion state analysis: By matching feature points in the (n-1)th image and the nth image, the movement direction and rotation of the underwater robot can be analyzed based on the movement direction or rotation direction and angle of the feature points in the image. S5: Laser calibration and movement distance measurement: Adjust the position of the two lasers relative to the underwater monocular camera, and use the two lasers to emit two laser points on the left and right sides of the underwater monocular camera. In the captured image, the two laser points are on the same straight line as the origin of the image pixel coordinates; the distance moved by the underwater robot can be obtained by calibrating the two laser points. S6: Generate motion trajectory: Through n-1 image analysis, obtain the underwater robot's movement direction, movement distance, rotation direction and angle motion data during the movement process. Accumulate and process the motion data analysis to obtain the detailed motion state and motion trajectory of the underwater robot during the operation.
2. The underwater robot trajectory monitoring method based on computer image analysis as described in claim 1, characterized in that, In step S1, to avoid the laser spot being too diffuse in the image captured by the underwater camera, an STM32 control board is used as the laser, and the laser power supply voltage is 3~3.3V to maintain a dim laser spot. The underwater monocular camera is connected to the computer via USB port.
3. The underwater robot trajectory monitoring method based on computer image analysis as described in claim 1, characterized in that, Step S4 includes the following steps: S4.1: Setting the pixel coordinates of key feature points: The SIFT feature matching algorithm extracts 'a' feature points from the (n-1)th image and the nth image, and sets the pixel coordinates of the feature points in the (n-1)th image to p. i.n-1 = (x i.n-1 ,y i.n-1 (i=1,2,3,…,a), the pixel coordinates of the feature point in the nth image are p i.n = (x i.n ,y i.n (i=1,2,3,…,a); S4.2: Motion state monitoring: based on the change in pixel coordinates of key feature points (∆x) i.n-1 ,∆y i.n-1 To determine whether the underwater robot's motion before taking the nth image is translational or rotational, the following formula is used: Where, x i.n Let y be the x-coordinate of the i-th feature point in the n-th image. i.n Let x be the ordinate of the pixel of the i-th feature point in the n-th image. i.n-1 Let y be the x-coordinate of the i-th feature point in the (n-1)-th image. i.n-1 Let Δx be the pixel ordinate of the i-th feature point in the (n-1)-th image. i.n-1 ∆y represents the change in the x-coordinate of the i-th feature point pixel in the (n-1)-th image comparison analysis. i.n-1 The change in the ordinate of the pixel of the ith feature point is analyzed in the (n-1)th image comparison. If the result satisfies the equation (1) where the changes in horizontal and vertical pixel coordinates are equal, then the underwater robot's motion is determined to be translational motion; if the result satisfies the equation (2) where the changes in horizontal and vertical pixel coordinates are partially unequal and the changes in horizontal and vertical pixel coordinates of a certain feature point are both 0, then the underwater robot's motion is determined to be rotational motion; if the result satisfies the equation (3) where the changes in horizontal and vertical coordinates are not equal or the changes in horizontal and vertical pixel coordinates are partially unequal and one of the changes in horizontal and vertical pixel coordinates of a certain feature point is 0 and the other is not 0, then it is impossible to determine and step S3 needs to be repeated. S4.3: Translational Movement Direction Measurement: If the underwater robot's motion is determined to be translational, the feature points of the (n-1)th image and the nth image are extracted to the same pixel coordinate system. Then, the feature point pixel coordinates are measured from the pixel coordinate p of the feature point within that same pixel coordinate system. i.n-1 Pointing to the pixel coordinates p of the feature point i.n The direction of the ray is the direction of movement of the feature point. The direction of movement of the feature point is then rotated around p. i.n-1 Rotating 180° will give you the direction of the underwater robot's translational movement. S4.4: Rotation Direction and Angle Measurement: If the underwater robot's motion is determined to be rotational, the feature points of the (n-1)th and nth images are extracted to the same pixel coordinate system. It is known that there exists a feature point I whose horizontal and vertical coordinates do not change in the pixel coordinate system of the (n-1)th and nth images. I =p I.n-1 =p I.n Then it can be determined that the remaining feature points are rotated around feature point I, and feature point p is placed in the same pixel coordinate system. I Connecting to the remaining m feature points, the rotation angle and direction are calculated using the following formula: in, Let x be the angle of rotation of the m-th feature point (excluding the rotation center feature point I). m.n Let y be the x-coordinate of the m-th feature point in the n-th image. m.n Let x be the ordinate of the m-th feature point in the n-th image. m.n-1 Let y be the x-coordinate of the m-th feature point in the (n-1)-th image. m.n-1 Let x be the ordinate of the m-th feature point in the (n-1)-th image. I Let y be the pixel x-coordinate of feature point I at the center of rotation. I Let I be the pixel ordinate of the rotation center feature point I; like If ϵ(0, 90°], then the feature point rotates clockwise around feature point I in the pixel coordinate system by the following angle: The underwater robot rotates to the right by an angle of... ,like If ϵ[-90°, 0), then the feature point rotates counterclockwise around feature point I in the pixel coordinate system by the following angle: , If the underwater robot rotates to the left by an angle of... .
4. The underwater robot trajectory monitoring method based on computer image analysis as described in claim 1, characterized in that, Step S5 includes the following steps: S5.1: Laser Calibration: Identify the center point of the laser aperture in the image and obtain the corresponding pixel points c1(x1, y1) and c2(x2, y2) in the pixel coordinate system. The distance between pixel points c1 and c2 is... Then the distance of each pixel in the actual environment is ; S5.2: Movement Distance Measurement: Under the condition of translational motion, the pixel distance moved by any feature point is... Then, the actual distance traveled by the underwater robot in the (n-1)th image analysis is .