CN112417948A - Method for accurately guiding lead-in ring of underwater vehicle based on monocular vision - Google Patents
Method for accurately guiding lead-in ring of underwater vehicle based on monocular vision Download PDFInfo
- Publication number
- CN112417948A CN112417948A CN202010998147.XA CN202010998147A CN112417948A CN 112417948 A CN112417948 A CN 112417948A CN 202010998147 A CN202010998147 A CN 202010998147A CN 112417948 A CN112417948 A CN 112417948A
- Authority
- CN
- China
- Prior art keywords
- marker
- auv
- image
- aruco
- aruco marker
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 79
- 239000003550 marker Substances 0.000 claims abstract description 187
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 78
- 238000012545 processing Methods 0.000 claims abstract description 22
- 238000001514 detection method Methods 0.000 claims abstract description 16
- 230000007246 mechanism Effects 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims description 42
- 239000013598 vector Substances 0.000 claims description 38
- 230000008569 process Effects 0.000 claims description 35
- 210000001503 joint Anatomy 0.000 claims description 34
- 238000005259 measurement Methods 0.000 claims description 27
- 238000001914 filtration Methods 0.000 claims description 25
- 230000003044 adaptive effect Effects 0.000 claims description 8
- 238000012360 testing method Methods 0.000 claims description 7
- 238000013519 translation Methods 0.000 claims description 7
- 238000000605 extraction Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 5
- 238000010606 normalization Methods 0.000 claims description 4
- 238000003032 molecular docking Methods 0.000 abstract description 31
- 230000000007 visual effect Effects 0.000 abstract description 11
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 abstract description 10
- 230000008901 benefit Effects 0.000 abstract description 8
- 230000001133 acceleration Effects 0.000 abstract description 5
- 238000004458 analytical method Methods 0.000 abstract description 4
- 230000031700 light absorption Effects 0.000 abstract description 3
- 230000000694 effects Effects 0.000 description 19
- 238000010586 diagram Methods 0.000 description 17
- 238000002474 experimental method Methods 0.000 description 17
- 238000006073 displacement reaction Methods 0.000 description 15
- 230000006870 function Effects 0.000 description 8
- 230000003068 static effect Effects 0.000 description 6
- 238000012549 training Methods 0.000 description 5
- 230000009471 action Effects 0.000 description 4
- 238000013459 approach Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 4
- 238000011156 evaluation Methods 0.000 description 4
- 241001270131 Agaricus moelleri Species 0.000 description 3
- 230000006872 improvement Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 125000004122 cyclic group Chemical group 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000000284 extract Substances 0.000 description 2
- 239000012535 impurity Substances 0.000 description 2
- 238000012886 linear function Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 210000001015 abdomen Anatomy 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 238000000149 argon plasma sintering Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000013075 data extraction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013104 docking experiment Methods 0.000 description 1
- 210000003128 head Anatomy 0.000 description 1
- 238000012417 linear regression Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/40—Extraction of image or video features
- G06V10/50—Extraction of image or video features by performing operations within image blocks; by using histograms, e.g. histogram of oriented gradients [HoG]; by summing image-intensity values; Projection analysis
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/40—Extraction of image or video features
- G06V10/56—Extraction of image or video features relating to colour
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/40—Scenes; Scene-specific elements in video content
- G06V20/48—Matching video sequences
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
Abstract
The invention provides a vision-based AUV accurate pair access ring method, which is used for acquiring visual information of a beacon, such as an Aruco Marker, on a parent aircraft docking mechanism by utilizing a monocular camera along with an aircraft cooperative system, and further acquiring the relative pose information of the AUV through analysis of a detection algorithm. Aiming at factors such as light absorption of a water body, an image enhancement algorithm is preferably adopted to improve the contrast of a beacon Aruco Marker on the docking mechanism, and the robustness of the beacon Aruco Marker is improved. The KCF acceleration strategy is also employed to reduce the number of image traversal pixels in order to increase the image processing speed. And considering the conditions that the Aruco Marker for the underwater identification beacon has buffeting and is shielded and the like, and eliminating noise by adopting a Kalman filter recursive algorithm. Under the addition of a reasonable control flow and a visual algorithm, the method avoids the condition that the remote monocular identification precision is low and misjudgment is easy, is convenient for recovering multiple underwater vehicles, and can effectively take the advantages of wide effective guide range, high close-range guide precision and the like into consideration.
Description
Technical Field
The invention relates to the technical field of underwater vehicle vision, in particular to a monocular vision-based method for accurately guiding an underwater vehicle into a guide ring.
Background
In recent years, with the demand for improvement in technology and strategic development, development and utilization of resources in countries around the world have been expanding toward a wide ocean. The underwater vehicle (AUV) is widely applied as a tool for assisting deep sea exploration, widens the knowledge of people on unknown underwater environment, and has important and wide application in the fields of ocean research, ecological monitoring, military reconnaissance and the like.
Underwater rendezvous and docking is a quick and effective method for solving the problem that AUV communication and carrying capacity are limited. The AUV identification and autonomous tracking are the most basic steps in the whole rendezvous and docking process. Generally, electromagnetic sensors, acoustic sensors, and visual sensors are commonly used for detection of underwater targets. Among them, the acoustic sensor is widely used because of its wide measurement range, but its accuracy is poor. Although the electromagnetic sensor has high accuracy, the measurement range is small due to the fast decay rate in water. The visual sensor has obvious advantages in the aspect of short-range detection, the recognition precision can reach centimeter level, and the visual sensor is small in size, convenient to carry and install, and more suitable for close-range accurate guidance, butt joint and the like.
At present, the research on the feature point-based vision guidance algorithm is mature. The estimation of the relative pose can be realized by the visual guidance algorithm when not less than 4 coplanar feature points exist according to the guidance realization requirement, and the position and the posture of the AUV relative to the target are solved through a PnP (Passive-n-Piont) algorithm. However, the algorithm has the problems of multiple solutions, poor robustness and the like in the solving process, the arrangement of the characteristic points must meet specific conditions, and limited factors are more in a complex underwater environment, so that the method cannot be widely used for controlling the AUV in actual use.
Disclosure of Invention
The invention aims to provide a method for accurately aligning an AUV (autonomous Underwater vehicle) to an access ring based on vision aiming at the defects in the prior art. The method is characterized in that in a multi-underwater vehicle collaborative system, a following vehicle acquires visual information of a beacon, such as an Aruco Marker, on a docking mechanism of a parent vehicle by using a monocular camera, and then the relative pose information of the AUV is obtained through analysis of a detection algorithm. And further aiming at factors such as light absorption of a water body, an image enhancement algorithm is preferably adopted to improve the contrast of the beacon Aruco Marker on the docking mechanism, and the robustness of the beacon Aruco Marker is improved. Also further, a kcf (kernel Correlation filters) acceleration strategy is employed to reduce the number of pixels traversed by the image in order to increase the image processing speed. And further considering the conditions that the Aruco Marker under the underwater identification beacon has buffeting and is shielded and the like, and eliminating noise by adopting a Kalman Filter (KF) recursive algorithm. Under the addition of a reasonable control flow and a visual algorithm, the method avoids the condition that the remote monocular identification precision is low and misjudgment is easy, is convenient for recovering multiple underwater vehicles, and can effectively take the advantages of wide effective guide range, high close-range guide precision and the like into consideration.
The technical scheme of the invention is as follows:
the underwater vehicle accurate guide lead-in ring method based on monocular vision is used in a multi-underwater vehicle cooperative system, the multi-underwater vehicle cooperative system at least comprises a parent AUV and a following AUV, and the parent AUV and the following AUV are provided with an orientation and attitude measurement system and a Doppler velocimeter, so that the real-time angular speed, angle information and speed information of the AUV can be obtained; a monocular camera is arranged along the front section of the AUV; the butt joint mechanism is fixed on the mother body AUV and comprises a plurality of butt joint rings and an inner side limiting plate, the butt joint rings and the inner side limiting plate are sequentially arranged along the axial direction of the mother body AUV, the internal force of each butt joint ring is greater than the diameter of the butt joint AUV, and when the butt joint AUV reaches a preset position and enters the butt joint rings after being aligned, the inner side limiting plate can axially limit the butt joint AUV; beacon Aruco markers are respectively arranged on the butt joint ring and the inner side limiting plate;
the process of identifying and detecting the beacon Aruco Marker in the collected image along with the AUV is as follows: collecting an RGB image through a monocular camera along with the AUV, and converting the RGB image into a gray scale image; after mean filtering is carried out on the gray level image, the image is binarized through a local self-adaptive threshold value method; extracting edges of connected regions with the same pixels from the binary image by using a Suzuki and Abe algorithm; fitting all edge curves through a broken line by using a Douglas-Peucker algorithm; searching all closed contours formed by edge curves, and discarding polygons which cannot be approximated to 4 vertexes; keeping the outermost layer contour in the rest rectangular contour, analyzing the inner region of the contour according to the black boundary characteristic of the outermost layer of the Aruco Marker, dividing grids, extracting coding information, and acquiring an ID according to a pre-stored Marker dictionary;
and after the ID is obtained, obtaining an intersection point of the solved boundary line, obtaining 4 vertex coordinates, solving the relative pose between the camera and the Aruco Marker by using a P4P method according to the camera parameters, and further guiding the follow-up AUV to enter the ring.
Furthermore, hamming codes are introduced into the Aruco Marker codes, the accuracy and the fault tolerance of information extraction are enhanced, the hamming distance of the Aruco Marker in different rotation directions can be obtained, the Marker hamming distance with the correct direction is zero, and the markers in other different viewing angles are not zero, so that the rotation direction can be determined according to the Aruco Marker codes.
Further, after the RGB image is collected by the monocular camera following the AUV, the image is enhanced by adopting an image enhancement method based on a global adaptive threshold value:
step 1: converting the RGB image into a YUV color space, and extracting the brightness value of a Y channel;
step 2: carrying out normalization processing on the Y-channel brightness value of each pixel:
L(i,j)＝Y(i,j)/255
wherein (i, j) is the pixel coordinate corresponding to the image, and L (i, j) is the brightness value after normalization;
and step 3: calculating a logarithmic average of luminance values of an input image
In the formula: m is the height of the image pixel array, n is the width of the image pixel array, and sigma is a set small value; the global adaptive threshold is calculated according to the logarithmic mean of the brightness as follows:
and 4, step 4: respectively scaling the brightness of the RGB three channels of the original image according to the logarithmic average value of the brightness value of the image:
R′(i,j)＝R(i,j)*Lg(i,j)
G′(i,j)＝G(i,j)*Lg(i,j)
B′(i,j)＝B(i,j)*Lg(i,j)
thereby achieving image enhancement.
Furthermore, an Aruco Marker existing region is extracted by adopting a KCF target tracking algorithm, the number of pixels needing to be traversed in the identification and detection process of the Aruco Marker is reduced, and the identification speed of the Aruco Marker is accelerated.
Further, Kalman filtering is adopted to filter interference in the process of identifying and detecting the Aruco Marker:
after the Aruco Marker is successfully identified and detected for one time, the rotation matrix R is solved according to the output result, and the coordinate of the camera under the reference coordinate system of the Aruco Marker
And the roll angle of the camera relative to the Aruco Marker
Pitch angle
And yaw angle
State x of the k-th cyclekIs defined as:
according to the Kalman filtering model, the system state of the k-th period can be obtained from the system state of the k-1 th period, and the dynamic equation is as follows:
xk＝Axk-1+Buk+ωk
where A is the state transition matrix, B is the control input matrix, ωkIs process noise, obeys a mean of 0 and covariance matrix of QkMultivariate normal distribution; the state transition matrix a is defined as follows:
wherein dT is the sampling time;
for state xkThe measurement at time k is given by the metrology equation:
zk＝Hxk+vk
where H is the observation matrix, vkIs process noise, which obeys a mean of 0 and a covariance matrix of RkWhite gaussian noise of (1); the observation matrix H is defined as follows:
and the velocity measurement vector and the angular velocity measurement vector following the AUV are respectively:
the velocity vector of the following AUV relative to the Aruco Marker can be obtained according to the rotation matrix R
And angular velocity vector
According to the dynamic equation and the measurement equation, iteration is carried out by using Kalman filtering, and the prediction updating process is described as follows:
wherein
The state predicted value of the current moment is obtained according to the optimal estimated value of the previous moment,
as a function of the error covariance P at the last momentk-1And the error of the current moment obtained by predicting the process noise Q; the state update process is described as:
wherein KkIn order to obtain the gain of the kalman filter,
for optimal result of final output, PkTo update the error value.
Further, after the Aruco Marker successfully performs recognition detection once, the output result is that the rotation vector r is equal to (r)1,r2,r3)TAnd translation vectort＝(t1,t2,t3)T(ii) a Obtaining a rotation matrix from the rotation vector
Where I is a unit vector, θ ═ norm (r) is the modulo length of the rotation vector, and r' ═ r/θ is the unit vector of the rotation vector; and then obtaining a rotation and translation matrix of the camera under the reference coordinate system of the Aruco Marker as follows:
and obtaining the coordinates of the monocular camera under the reference coordinate system of the Aruco Marker, and the roll angle, the pitch angle and the yaw angle of the camera relative to the Aruco Marker as follows:
wherein
The coordinate of the camera under a Marker reference coordinate system;
respectively roll angle, pitch angle and yaw angle of the camera relative to the Marker.
Further, Q of the Kalman filter is obtained through experimental testskAnd RkHas a value of
Wherein I3Is a 3-dimensional identity matrix, 03Is a 3-dimensional 0 matrix.
Advantageous effects
Compared with the traditional method based on the optical beacon, the method adopts the Aruco Marker as the accurately guided beacon, and breaks through the limitation that the arrangement of the characteristic points in the geometric method must meet specific conditions. And the method adopts methods such as image enhancement and the like to improve the identification precision of the AUV, and Kalman filtering is added to eliminate the underwater buffeting and shielding. Under the addition of a reasonable control flow and a visual algorithm, the situation that the remote monocular recognition accuracy is low and misjudgment is easy is avoided. And the advantages of wide effective guide range, high close-range guide precision and the like can be effectively considered. Successful docking facilitates subsequent recovery operations.
Additional aspects and advantages of the invention will be set forth in part in the description which follows and, in part, will be obvious from the description, or may be learned by practice of the invention.
Drawings
The above and/or additional aspects and advantages of the present invention will become apparent and readily appreciated from the following description of the embodiments, taken in conjunction with the accompanying drawings of which:
FIG. 1: the invention is a schematic diagram of a docking mechanism.
FIG. 2: the invention is a schematic diagram of success of an access ring.
FIG. 3: two Aruco markers with different IDs are selected in the invention.
FIG. 4: the Aruco Marker under different viewing angles in the invention.
FIG. 5: the pose of the AUV relative to the Aruco Marker in the invention is a schematic diagram for resolving.
FIG. 6: different algorithms enhance the effect contrast map in the invention; (a) a DCP enhancement effect, (b) a Retinex enhancement effect, (c) a global adaptive threshold enhancement effect, (d) a DCP histogram of gray scales, (e) a Retinex histogram of gray scales, and (f) a global adaptive threshold histogram of gray scales.
FIG. 7: the Aruco Marker acceleration strategy block diagram is disclosed.
FIG. 8: in the invention, a schematic diagram of ROI region extraction is provided.
FIG. 9: the invention discloses an Aruco Marker error measurement schematic diagram.
FIG. 10: the graphical diagram of the data error curve of the Aruco Marker in the invention.
FIG. 11: the KCF algorithm in the invention processes Aruco markers with different sizes.
FIG. 12: the invention is a schematic diagram of the occlusion of an Aruco Marker.
FIG. 13: in the invention, the Aruco Marker is shielded under a static condition; (a) displacement in the X-axis direction, (b) displacement in the Y-axis direction, (c) displacement in the Z-axis direction, (d) yaw angle around the Y-axis, (e) pitch angle around the X-axis, (f) roll angle around the Z-axis.
FIG. 14: the method has the advantages that the Aruco Marker is shielded under the dynamic condition to obtain an experimental result; (a) displacement in the X-axis direction, (b) displacement in the Y-axis direction, (c) displacement in the Z-axis direction, (d) yaw angle around the Y-axis, (e) pitch angle around the X-axis, (f) roll angle around the Z-axis.
FIG. 15: the graphical illustration of the jitter of the Aruco Marker in the present invention.
FIG. 16: in the invention, an Aruco Marker generates a random jitter experimental result; (a) displacement in the X-axis direction, (b) displacement in the Y-axis direction, (c) displacement in the Z-axis direction, (d) yaw angle around the Y-axis, (e) pitch angle around the X-axis, (f) roll angle around the Z-axis.
FIG. 17: the invention relates to an experimental hardware platform.
FIG. 18: the invention relates to a water pool experiment-a successful pair access ring physical map.
FIG. 19: in the invention, an Aruco Marker outputs a relative pose; (a) x-axis direction measurement data, (b) Y-axis direction measurement data, (c) Z-axis direction measurement data, and (d) yaw angle measurement data.
FIG. 20: in the invention, the filtering result of the Aruco Marker relative pose is obtained; (a) x-axis direction filter data, (b) Y-axis direction filter data, (c) Z-axis direction filter data, and (d) yaw angle filter data.
FIG. 21: in the invention, the underwater butt joint AUV successfully guides the motion track.
Detailed Description
The method starts from a beacon, such as an Aruco Marker, on a docking mechanism, and information in the beacon is identified through a camera so as to analyze and obtain the relative pose information of the AUV; aiming at the problems of light scattering and the like caused by an underwater complex environment, the identification degree of a beacon Aruco Marker on a docking mechanism is improved by adopting an image enhancement method, and the robustness of the beacon Aruco Marker is improved; and the identification of the effective area is improved through a KCF acceleration strategy, and finally the butt-joint AUV is accurately guided to enter a butt-joint mechanism of the mother AUV, so that the butt-joint of the underwater vehicle is completed.
The hardware requirements for implementing the invention are as follows:
the invention is used in a multi-underwater vehicle cooperative system, at least comprises a parent AUV and a following AUV, at least an industrial camera is arranged at the front section of the following AUV, and the frame rate is generally required to be at least more than 20 frames. An azimuth attitude measurement system is arranged on the AUV, so that the real-time angular speed and angular information of the AUV can be obtained. And a Doppler velocimeter is arranged on the AUV head, so that real-time speed information of the underwater vehicle can be obtained.
A docking mechanism is fixed on the mother AUV, and a horn-shaped docking cage is usually adopted in the traditional active docking mechanism, so that the volume is large, the structure is complex, and the cost is high. As shown in fig. 1, the docking mechanism of the present embodiment includes a plurality of docking rings and an inner limiting plate, which are sequentially arranged along an axial direction, and are firmly fixed to the abdomen of the mother AUV; the docking ring entrance diameter is 200 mm, 50 mm wider than the docking AUV diameter. When the butt joint AUV reaches the preset position and enters the butt joint ring after being aligned, the inner side limiting plate can limit the butt joint AUV, and the butt joint AUV abuts against the limiting plate to complete system combination. 1 beacon Aruco Marker is respectively arranged on the butt joint ring and the inner side limiting plate and used for detecting and calculating the position and posture of the parent AUV and the butt joint device after the light source target exits the visual field under a close distance, so that the positioning accuracy and the reliability in the process of connecting the butt joint ring are ensured. Fig. 2 shows a schematic diagram of the success of the access ring according to the present invention.
The method for designing, identifying and detecting the beacon Aruco Marker on the docking mechanism can be realized by the following steps:
1. design of a beacon Aruco Marker on a docking mechanism: a standard Aruco Marker consists of black and white squares of the same size to represent different coded information. FIG. 3 shows two Aruco markers with different IDs selected in the present invention. Each Marker corresponds to a unique ID number. Each Marker can be seen as a 7 x 7 binary matrix with the outermost circle removed to facilitate detection of black edges, and the inner 5 x 5 matrix is used to represent different IDs. The Aruco Marker has small load for coding information and high positioning precision. The detection algorithm of the Aruco Marker mainly comprises two stages: dictionary generation of the Marker and identification detection of the Marker. Dictionary generation is responsible for generating a library of Marker patterns with different IDs, and recognition detection is used for resolving the coded information carried by the Marker.
2. In the guiding butt joint process, the monocular camera identifies information in the Aruco Marker, detects the rectangle, extracts binary coding information from the rectangle, and analyzes the binary coding information to obtain relative pose information. The basic idea is as follows: the BGR image is converted into a gray scale image, the most prominent contour is extracted from the gray scale image, a rectangle is used for approximating the contour, the contour of a polygon which cannot be approximated to 4 vertexes is omitted, and the coding information is extracted by analyzing the internal area. And solving the transformation relation between the camera coordinate system and the Aruco Marker coordinate system through a P4P algorithm.
The detailed process algorithm flow is shown in table 1 below.
Table 1 detection algorithm of Aruco Marker
3. Furthermore, since data extraction errors are caused by interference in multiple aspects in the process of identifying the Aruco Marker coding information, a Hamming code is introduced, and the accuracy and the fault tolerance of information extraction are enhanced. The hamming code is based on the parity check principle, has the double functions of error detection and error correction, and can locate the position of a certain bit of error code. The specific method comprises the following steps: and inserting an x-bit check code into the n-bit binary data to form a new n + x-bit binary string. The check code has a total of 2xA value case, where a value mode is needed to indicate that the data is all correct, and 2 remainsx-1 value mode indicates the case of 1 bit data error; n represents that 1 bit error is possible in n bit coded data, and n conditions are total; x represents the possibility of 1-bit error in the x-bit check data, and x cases are total, so that n + x possibilities are total when the 1-bit error code exists in the encoded information. Therefore, in order to accurately locate the error code, the number x of the check bits is increased to satisfy:
2x-1≥n+k (1-1)
the minimum value of x when the inequality of the above equation is satisfied is the number of the check code, and k is a non-zero integer.
Considering that the Aruco Marker rotates differently under the camera shooting view angle, the same Marker rotates under different camera view angles, as shown in FIG. 4, the Aruco Marker in the invention under different view angles. The monocular camera has to be capable of finding out the correct orientation from different rotating images to ensure the rotation invariance of the Marker, and 3-bit check bits are added into each 2-bit data bit according to the encoding mode of the Hamming code. The hamming distance of each possible Marker can be obtained by the coding mode. The Marker hamming distance with correct direction is zero, and the markers at other different viewing angles are not zero.
4. And (5) estimating the pose of the Aruco Marker. The method for calculating the relative pose of the Marker in the camera coordinate system in the Aruco Marker algorithm is solved through a direct linear transformation method (DLT) in an n-Point Perspective (Perspective-n-Point) problem. And extracting pixel coordinates of four vertexes of the Marker after the camera collects the image, and calculating the pose of the Marker in the camera coordinate system according to the space coordinates of the four vertexes in the world coordinate system. FIG. 5 is a schematic diagram showing the pose of AUV relative to Aruco Marker in the present invention.
Because the water body and suspended solid lamp impurity in aquatic can produce great influence to the image under water: attenuation due to absorption of light by the water body, and image attenuation due to scattering of light by the suspended matter. Therefore, during an underwater experiment, the problems of fast attenuation of an underwater image and the like need to be overcome, and the Aruco Marker image acquired by the monocular camera is enhanced, so that the underwater butt joint efficiency of the AUV is improved. The invention adopts an Aruco Marker image enhancement method based on global self-adaptive threshold, and adopts logarithmic relation for image processing:
1. converting the RGB image into YUV color space, extracting the brightness value of Y channel, the conversion formula is as follows:
Y＝0.299R+0.587G+0.114B (1-2)
2. the luminance channel of the image can be obtained through the above formula, because the image has low luminance and insufficient contrast, in order to avoid interference caused by uneven distribution of pixel values of the image to subsequent calculation, the luminance value is normalized as shown in the following formula:
L(i,j)＝Y(i,j)/255 (1-3)
wherein (i, j) is the pixel coordinate corresponding to the image, and L (i, j) is the normalized luminance channel.
3. We then find the log mean of the input image luminance values, which is formulated as follows:
in the formula: m is the height of the image pixel array, n is the width, and σ is a very small value which is taken to avoid infinite singular points of the log function caused by the existence of black points with the pixel value of 0 in the image, and we take 0.001 here. The global adaptive threshold is calculated according to the logarithmic mean of the brightness as follows:
the above equation has an adaptive function, and can adapt to different scenes because we divide the global brightness value and the global brightness maximum value of the input image by the logarithmic average value of the brightness respectively. When the obtained logarithm average value is higher, the output curve of the logarithm function tends to a linear function, and the image with low logarithm average brightness is more obviously enhanced than the image with high logarithm average brightness.
4. Finally, the brightness of the three channels of RGB of the original image is respectively scaled according to the logarithmic mean brightness of the image, as follows:
R′(i,j)＝R(i,j)*Lg(i,j) (1-6)
G′(i,j)＝G(i,j)*Lg(i,j) (1-7)
B′(i,j)＝B(i,j)*Lg(i,j) (1-8)
5. here, we test the real-time performance and brightness enhancement effect of the algorithm, and compare the real-time performance and image processing effect of the algorithm used in the present invention with the DCP image enhancement algorithm and the Retinex algorithm, as shown in fig. 6, which is a comparison graph of the enhancement effect of different algorithms in the present invention. Our evaluation index for the enhancement algorithm has the following aspects:
(1) real-time comparison
Because the algorithm needs to be applied to underwater operation tasks, the real-time requirement of the algorithm needs to be considered, and different algorithms are used for processing the same Marker video sequence in the same Intel Core i5-7200U processor and calculating the average processing time of a single frame.
(2) Distortion comparison
In addition to this, it is necessary to consider whether the image is excessively enhanced. In order to measure whether an image is excessively enhanced, a Structural Similarity Index (SSIM) is used as an evaluation Index of the distortion degree. Setting an original image as I and a distorted image as I', defining the degrees of structural distortion, brightness distortion and contrast distortion of the two images as follows:
in the formula, gammaII′Denotes the correlation coefficient of I and I', gammaIAnd gammaI′Represents the standard deviation of the image itself; mu.sI、μI′Respectively mean, σ, of the image itself1、σ2、σ3Is a very small constant value to prevent infinite singularities in the formula. Due to the independence between the above equations, SSIM can be expressed as:
the more the value of the SSIM index approaches 1, the smaller the distortion degree is, and the more approaches 0, the greater the distortion degree is, and we need to ensure that after image enhancement, we have a certain distortion degree with the original image, but it cannot be too large, and at the same time, it cannot be too small, so as to ensure the enhancement effect.
(3) Comparison of degree of enhancement
Since the quality of the enhancement effect cannot be evaluated by naked eyes, in order to quantitatively compare the enhancement degrees of the 3 algorithms, Mean-square Error (MSE) is adopted, and the difference between the original image and the enhanced image before and after enhancement is measured by calculating the MSE of the original image and the enhanced image, wherein the MSE calculation formula is as follows:
where m and n represent the number of rows and columns, respectively, of the image pixel matrix, and (i, j) represents the coordinate position of the pixel. MSE represents the difference degree between two images, the larger the value of MSE is, the larger the difference between the two images is, and the difference degree between the images processed by different algorithms and the original image is compared to quantify the enhancement effect.
Another evaluation index is UCIQE. The UCIQE combines the three image characteristics of chroma, saturation and contrast, so as to comprehensively evaluate the quality of the underwater image and quantify the color cast degree, the fuzzy degree and the contrast of the underwater image. The higher the value is, the better the visual effect of the image is, and the calculation formula is as follows:
UCIQE＝k1σc+k2cl+k3μs (1-14)
in the formula sigmacIs the standard deviation of the color, clFor contrast of brightness, musIs the mean value of saturation, k1、k2、k3The weight values of the three indexes are respectively. Since the UCIQE evaluates the image quality in consideration of 3 indexes, the evaluation effect thereof is similar to that observed by human eyes, so that the method is very widely applied.
The calculation results of the indices are shown in table 2.
Table 2 table for evaluating the enhanced results of the three algorithms
As can be seen from the table, the algorithm used in the present invention has better real-time performance than the DCP and Retinex algorithms because the algorithm of the present invention eliminates the overhead of background color estimation, color correction, etc. The SSIM calculated by the three algorithms approaches to 1, which shows that the difference between the enhanced image and the original image is small, while the SSIM index of the algorithm is between that of the DCP algorithm and that of the Retinex algorithm, in order to compare the enhancement effect of the 3 algorithms, the MSE and the UCIQE indexes are integrated, the UCIQE index is as large as possible under the condition that the SSIM index is not very small, and the distortion degree of the algorithm is over 0.55, the enhanced image has no serious distortion, and the UCIQE index is higher than that of the DCP algorithm and that of the Retinex algorithm, so that the enhancement effect is better.
According to experimental results, the enhancement effect of the DCP algorithm on the underwater image is not obvious, and mainly because the attenuation and scattering of underwater light cause the dark channel value to be very small and approximate to 0, parameters such as underwater transmissivity, background light and the like cannot be restored. The Retinex enhancement effect is obvious, and the Retinex algorithm distributes the pixel values in a balanced manner, so that the contrast of a Marker is not promoted, the color of an image is seriously supersaturated, and the subsequent processing of the image is not facilitated. The global self-adaptive threshold method is characterized in that an image heavy RGB space is converted into a YUV space, then the brightness is improved, and then the image heavy RGB space is converted back into the RGB space, so that only the brightness of the image is improved for processing, the hue and the saturation of the image are not damaged, the image is very bright, but the color is not influenced, and the robustness of Marker binaryzation detection can be improved.
As known by the detection algorithm flow of the Aruco Marker, the candidate Marker can be detected by continuously traversing the whole image in the image processing process, and the process takes a lot of time. Therefore, the invention further selects and extracts a region of interest (ROI) to shorten the traversal time so as to improve the recognition efficiency, and correspondingly provides a method for extracting the Marker existing region by using a target tracking algorithm to reduce the number of pixels in the image traversal process so as to accelerate the image processing speed. The algorithm of target tracking selected by the invention is KCF (Kernelized Correlation filters), and as shown in FIG. 7, the algorithm is an Aruco Marker acceleration strategy based on the KCF algorithm, and has the advantages of good tracking effect robustness, high processing speed and the like. The KCF algorithm and the Aruco Marker algorithm have serial and parallel processes in the whole combined processing process. Fig. 8 is a schematic diagram illustrating ROI region extraction in the present invention.
In the KCF algorithm, if two signals are more similar, the correlation value is higher, and in the tracking problem, it is necessary to design a filter template so that when it acts on the tracked object, the obtained response is maximum, and the position of the maximum response value is the position of the object. In order to realize online learning tracking, firstly, training positive and negative samples are collected around each frame of target area to train a classifier. The KCF algorithm uses a cyclic shift operation of the samples to generate the samples instead of the sampling window, and any cyclic shifted vector can be generated by multiplying the permutation matrix P by a sample vector x, where x is defined as:
x＝[x1,x2,…,xn] (1-15)
Px＝[xn,x1,x2,…,xn-1] (1-17)
the above formula is a one-dimensional case, and the same holds true when being generalized to a two-dimensional case. The training sample X obtained was:
the samples may be generated by cyclically shifting the current frame target region instead of the sampling window.
Assuming that the classifier is f, which is a linear function of x, let the training samples be (x)i,yi) Wherein the sample xiAs column vectors, label yiAs a scalar quantity, the linear regression function is f (x)i)＝WTxiAdding a regularization term λ to prevent overfitting, the loss function is:
further simplification of the partial derivatives for w can lead to a closed solution:
w＝(XTX+λI)-1XTy (1-20)
there are n training samples, where X ═ X1,x2,...,xn]Each column represents a feature vector of a sample, y is a column vector, and each element represents a sample label. The collected positive and negative samples are used for training through a ridge regression algorithm of a nuclear space, so that the tracking target can be detected in real time.
In the target attitude tracking of the Aruco Marker, due to underwater impurities or light, the corner of the Marker is possibly shielded or cannot be identified; in addition, as the two AUVs have flexible motion conditions with 6 degrees of freedom, the situation that the Marker is not in the field of view of the camera can also exist. In addition, target attitude tracking for Marker is very sensitive to noise, and when the docked AUV moves faster, there is a jitter phenomenon with respect to Marker and a large effect on the output attitude value. Although the Marker can immediately restore the pose when it appears in the image again after being lost due to occlusion, the pose information is completely lost when it is occluded. Therefore, the invention further adopts an Aruco Marker anti-interference method based on Kalman filtering to eliminate noise through a Kalman Filter (KF):
1. assuming that the system model is a linear model, the system noise follows Gaussian distribution, and the optimal estimation is provided through the prediction model and the measured value. We captured RGB images as input to the system using a pre-calibrated common USB camera, and then the system continuously searched for an Aruco Marker in the image. And if the Marker is detected, solving the relative pose as a measured value. Based on the established dynamic model, the KF can predict a prior pose. After the most recent measurement is obtained, the filter can use it to update the prediction. The system state becomes the posterior value after one cycle is completed.
2. The output result of the Aruco Marker is that a rotation vector r is equal to (r)1,r2,r3)TAnd the translation vector t ═ t (t)1,t2,t3)TThe form, in which the rotation vector needs to be calculated according to the formula of rodgers, is as follows:
where I is a unit vector, θ ═ norm (r) is the modulo length of the rotation vector, and r' ═ r/θ is the unit vector of the rotation vector.
By the formula (1-21), the rotation and translation matrix of the camera under the reference coordinate system of the Marker can be obtained as follows:
with the reference coordinate system of Marker as the center, we can obtain the position and euler angle of the monocular camera as follows:
wherein
The coordinate of the camera under a Marker reference coordinate system;
respectively roll angle, pitch angle and yaw angle of the camera relative to the Marker.
3. After the Aruco Marker is successfully detected, firstly, the conversion is carried out, and then the pose information is subjected to post-processing by adopting linear Kalman filtering. In addition to the displacement and euler angle, the velocity of the above parameters is also taken into account in the system. Linear and angular velocities of the AUV in various directions can be provided by the IMU sensor, and the AUV is assumed to move at a constant velocity in one cycle. Then the state x of the system in the k-th cyclekIs defined as:
according to the Kalman filtering model, the system state of the k-th period can be obtained from the system state of the k-1 th period, and the dynamic equation is as follows:
xk＝Axk-1+Buk+ωk (1-26)
where A is the state transition matrix, B is the control input matrix, ωkIs process noise, obeys a mean of 0 and covariance matrix of QkAnd (4) multivariate normal distribution. The state transition matrix a is defined as follows:
where dT is the sampling time, 100ms in the present invention.
For state xkThe measured value at time k can be given by the metrology equation:
zk＝Hxk+vk (1-28)
where H is the observation matrix, vkIs process noise, which obeys a mean of 0 and a covariance matrix of RkWhite gaussian noise. The observation matrix H is defined as follows:
we obtained the Q of the Kalman filter from experimental testskAnd RkThe values of (a) are as follows:
wherein I3Is a 3-dimensional identity matrix, 03Is a 3-dimensional 0 matrix.
The velocity measurement and angular velocity measurement vectors obtained by the AUV through the IMU are respectively:
in order to convert these vectors into the Marker's reference frame, they need to be multiplied by a rotation matrix. The rotation matrix is formed by the product of three matrices, and the velocity and angular velocity vectors with respect to Marker are as follows:
in the formula VB、EBVelocity vectors and angular velocity vectors in the AUV body coordinate system are respectively, the system is iterated by using Kalman filtering according to equations (1-26) and (1-28), and the prediction updating process can be described as follows:
wherein
The state predicted value of the current moment is obtained according to the optimal estimated value of the previous moment,
as a function of the error covariance P at the last momentk-1And the error of the current time predicted by the process noise Q.
The state update process can be described as:
wherein KkA kalman filter gain, to weigh the confidence between the prediction and the measurement,
for the optimal result of the final output, (1-38) are the update operations performed for the next iteration, i.e. updating the error valuePk。
In conclusion, if the Marker information is available, the relative pose of the AUV and the Marker is estimated through the data of the Marker and the IMU. Otherwise, the AUV will estimate the relative pose using the IMU sensor data.
The Aruco Marker pose estimation error is measured and analyzed as follows:
fig. 9 is a schematic diagram of the measurement of the error of the Aruco Marker in the invention, and the invention uses a camera on a butted AUV to align the center of the Aruco Marker, and records 20 sets of relative pose data every 50cm from near to far. And taking the average value of 20 groups of data as a pose measurement result of the current position, comparing the pose measurement result with the real pose, and analyzing the error of the pose measurement result. Since the range of Marker used in the task of the access ring is 0.5m to 3m, the present invention measures error data of X-axis, Y-axis, Z-axis and yaw angle within the range, as shown in table 3.
TABLE 3 Aruco Marker pose estimation error
FIG. 10 is a schematic diagram showing an error curve of the Aruco Marker data in the present invention. It can be seen from table 3 and fig. 10 that, in the case of a short distance, the errors of the X axis and the Y axis and the fluctuation range thereof are small, but as the distance increases, the errors and the fluctuation range thereof are increased to a certain extent, the average error can be guaranteed to be ± 3cm within the distance range of 2m, the measured value of the Z axis distance is accurate, and as the distance increases, the errors slightly increase but do not exceed ± 2 cm. The yaw angle error also increases with the distance, and there is a case where the fluctuation increases, and the error thereof can be controlled within ± 4 ° in the range of 2 m.
The real-time performance of the Aruco Marker pose estimation is verified, tested and analyzed as follows:
the real-time performance of the algorithm is always a very critical common problem in engineering application, and the requirement on the real-time performance of underwater AUV guidance is higher. In the experiment, the image processing operation environment is a Matebook D notebook, the CPU model is I5-7200U, and the camera pixel is 720 multiplied by 360. The invention respectively uses six markers with different sizes to prevent the markers from being in the distance of 1 meter from the camera, uses a KCF tracking algorithm to circle the markers, and calculates the image processing time. FIG. 11 shows that the KCF algorithm in the present invention deals with Aruco markers of different sizes.
The average elapsed time test results before and after accelerated processing using the KCF algorithm are shown in table 4 below:
TABLE 4 processing time consumption before and after ROI extraction by KCF algorithm
As can be seen from Table 4, the speed improvement of the algorithm is limited only by changing the size of the Marker, the speed improvement of the algorithm obtained by reducing the size of the Marker is only about 10ms, but after the ROI of the Marker is extracted by the KCF tracker, the algorithm processing time of the Marker is greatly reduced, the processing speed of about 10 frames/s is increased to 100 frames/s, and the speed is increased by about 10 times.
The reason why the treatment time is greatly reduced is analyzed as follows:
(1) the Aruco Marker algorithm needs to traverse image pixel values and fit a rectangular boundary when a Marker is detected, and after an ROI area is extracted, the pixel values needing to be traversed when the Marker is detected are greatly reduced.
(2) All recognized candidate boundary graphs need to be screened in the Aruco Marker algorithm, and after the ROI area is extracted, the time occupied by the step in the algorithm can be ignored.
In addition, the strategy does not influence the physical size and the position of the Marker in the image, so that the pose estimation precision of the Marker is not influenced. In conclusion, after the ROI of the Marker is extracted through the KCF tracker, the time consumed by algorithm processing can be greatly shortened, and meanwhile the pose accuracy of the Marker cannot be influenced.
The pose estimation robustness of the Aruco Marker in the invention is verified, tested and analyzed as follows:
due to the fact that underwater experiment measurement difficulty is high, in order to capture shielding and shaking phenomena of the Aruco Marker, a simulation experiment is conducted on the land, and as shown in FIG. 12, a schematic diagram of the shielded Aruco Marker in the invention is shown. The camera of the butt-joint AUV is directly used, the butt-joint AUV is made to be static, the Marker is pasted on the sliding block which is right opposite to the camera and can move in a translation mode along the X axis, and a small shielding position is arranged in the center of the platform to simulate the underwater shielding condition. The slider is provided with an attitude sensor which can measure the motion state of the current platform. The motion of an Aruco Marker is captured by butting a camera of an AUV, and the data is processed through Kalman filtering in combination with attitude data. In order to test the robustness of the algorithm, the invention performs two experiments.
The first experiment is to verify the effect of the algorithm on pose estimation in case some part of the image sequence obscures the Marker.
The results and analysis of the shielded Aruco Marker experiment under the static condition are as follows:
when the mark is shielded, accurate posture information of the Aruco Marker cannot be obtained, but the state and the position of the Aruco Marker in the period of time can be approximately estimated through Kalman filtering. The invention respectively verifies the prediction output result of the algorithm when the Aruco Marker is temporarily shielded in a static state and a moving state.
FIG. 13 shows the results of the occlusion experiment of the Aruco Marker under static conditions in the present invention. According to experimental results, the horizontal axis of a curve in the graph is an image sequence, a red solid line is original data output by a Marker, a blue dotted line is a result output after Kalman filtering, and therefore it can be seen that the Marker is shielded between 120 frames and 280 frames, no available data is output, and at the moment, the motion state of the Marker can be well predicted through data measured by an attitude sensor, so that the estimation of the relative pose is realized. When the Marker has data output, the prediction curve is relatively flat, and when the Marker has occlusion, the prediction curve has small fluctuation, which is mainly caused by noise in speed information output by the attitude sensor, and the fluctuation is in an acceptable range. The image shows that the output result of Kalman filtering under the static condition can well predict the relative pose of the camera and the Marker.
Experimental result and analysis of shielded Aruco Marker under dynamic condition
FIG. 14 shows the results of the occlusion experiment of the Aruco Marker under dynamic conditions in the present invention. According to experimental results, the Marker is shielded between 40 frames and 100 frames in the graph, the Marker does uniform linear motion along an X axis at the moment, and a prediction curve can well track the relative pose of the Marker. Therefore, the relative pose between the camera and the Marker can be well predicted according to the output result of Kalman filtering under the dynamic condition.
The second experiment is the influence of the algorithm on pose estimation under the condition that random jitter noise is added in the Aruco Marker pose estimation process.
And (3) enabling the Marker to do uniform linear motion from left to right along the X axis, and recording the original data and the processing result which are temporarily shielded in the midway. FIG. 15 is a diagram showing the jitter of the Aruco Marker in the present invention.
FIG. 16 shows the results of the Aruco Marker-generated random jitter test in the present invention. According to experimental results, the Marker swings left and right symmetrically along the X axis, and the original pose data can fluctuate greatly along with the addition of jitter noise under the condition that Kalman filtering is not added. This is disadvantageous for AUV guided docking. As can be seen from the figure, after the data are processed by adopting Kalman filtering, the pose can well follow the true value, and can keep relatively smooth and stable.
According to the two experiments, after the data of the Aruco Marker and the attitude sensor are fused, Kalman filtering can be performed, the positioning precision of the underwater vehicle guiding docking task can be improved, the noise influence of positioning data can be reduced under the condition of interference, and the method has great significance for improving the accuracy of autonomous positioning of the underwater guiding docking task.
The following provides a monocular camera-based underwater vehicle precise guidance leading-in ring strategy and a water pool experiment:
experimental platform A pool experiment is carried out in a big pool of an unmanned underwater carrying technology key laboratory of northwest industrial university to verify the method. The length of the experimental site is 70 meters, the width is 50 meters, and the depth reaches 15 meters, so that the experimental site accords with the environment of the experiment. Fig. 17 shows an experimental hardware platform in the present invention. The experiment is mainly divided into the two stages, namely a search stage and a Marker docking ring stage. The mother AUV is suspended in water at a fixed course, the operation of the butt joint AUV is ensured to start at a random position behind the mother AUV, and the initial distance between the mother AUV and the butt joint AUV is about 3 meters. When the experiment is started, the docking AUV starts to search for a Marker beacon, guidance is started according to the algorithm of the invention after the beacon is detected, and the task is ended after the docking AUV enters the docking device of the parent AUV. 4 pilot docking experiments were performed, all with success. FIG. 18 is a diagram showing a physical diagram of a successful pair of access rings in a water pool experiment according to the present invention.
Fig. 19 shows the relative pose output by the Aruco Marker in the present invention. According to experimental results, an original relative pose curve measured in an Aruco Marker docking stage in the underwater guided docking process of the docked AUV for 4 times only shows displacement and yaw angles, the loss of Aruco Marker data caused by the shielding or smearing phenomenon can be seen, and the data has large noise, so that the precision requirement in guided docking is very unfavorable.
Fig. 20 shows the filtering result of the relative pose of the Aruco Marker in the invention. According to the experimental result, after the Aruco Marker data are processed by Kalman filtering for 4 times, the processed curve is smoother, and the relative pose of the Aruco Marker can be well estimated when the Aruco Marker is lost, so that the target can be found back again according to the predicted pose after the AUV is docked and the tracking target is lost.
The data after Kalman filtering is used as a feedback value, X, Y axis offset is 0, the Z axis distance is 10cm, and the yaw angle is 0 degrees in the control input of a butt-joint AUV in an Aruco Marker guide butt-joint task, so that the butt-joint AUV can successfully enter a butt-joint ring to complete butt-joint, the feedback 4-time butt-joint curve shows that the X axis and the Y axis have larger deviation at first, but the displacement deviation gradually attenuates to 0 along with the control action, and the curve is also gradually reduced and converged to 10cm under the action of forward control of the Z axis distance. The yaw angle has slight jitter due to the adjustment of the butt AUV displacement, but under the control action, the yaw error can be controlled within +/-5 degrees. Finally, the underwater AUV guide loop-entering task is successfully realized by using the method, and the Aruco Marker docking track of the first operation result is taken.
Fig. 21 shows the successfully guided motion trajectory of the underwater docking AUV in the present invention. The experimental result shows that the butted AUV recognizes the Aruco Marker when being 3 meters away from a parent AUV, enters an Aruco Marker guiding butted state, has small control action on an X axis and a Y axis when being far away, has slow convergence of lateral deviation and depth deviation, gradually approaches a butted mechanism along with uniform motion, starts to enter a fixed-distance alignment stage when the Z axis reaches 50 centimeters, keeps the distance between the butted AUV and a butted ring in the stage, adjusts the displacement deviation and course angle deviation of the butted AUV relative to the butted ring, rapidly converges X axis, Y axis and yaw curve to 0 and keeps small-amplitude fluctuation in the stage, always judges the deviation range, judges that the yaw oscillation error is within +/-3 degrees, the displacement deviation is less than 3 centimeters, keeps stable state for 3 seconds, considers that the butted AUV reaches the butted condition, and starts to continue to advance slowly, and (6) finishing the butt joint. And finally, stopping the motion track at the coordinate position of (0, 0, 0.1), wherein the butt joint AUV enters the butt joint ring, and finally completing the underwater accurate guide ring-entering experiment.
Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention, and that variations, modifications, substitutions and alterations can be made in the above embodiments by those of ordinary skill in the art without departing from the principle and spirit of the present invention.
Claims (7)
1. A method for accurately guiding an underwater vehicle into a guide ring based on monocular vision is characterized in that: the method is used in a multi-underwater vehicle cooperative system, the multi-underwater vehicle cooperative system at least comprises a parent AUV and a following AUV, and the parent AUV and the following AUV are provided with an orientation and attitude measurement system and a Doppler velocimeter, so that the real-time angular speed, angle information and speed information of the AUV can be obtained; a monocular camera is arranged along the front section of the AUV; the butt joint mechanism is fixed on the mother body AUV and comprises a plurality of butt joint rings and an inner side limiting plate, the butt joint rings and the inner side limiting plate are sequentially arranged along the axial direction of the mother body AUV, the internal force of each butt joint ring is greater than the diameter of the butt joint AUV, and when the butt joint AUV reaches a preset position and enters the butt joint rings after being aligned, the inner side limiting plate can axially limit the butt joint AUV; beacon Aruco markers are respectively arranged on the butt joint ring and the inner side limiting plate;
the process of identifying and detecting the beacon Aruco Marker in the collected image along with the AUV is as follows: collecting an RGB image through a monocular camera along with the AUV, and converting the RGB image into a gray scale image; after mean filtering is carried out on the gray level image, the image is binarized through a local self-adaptive threshold value method; extracting edges of connected regions with the same pixels from the binary image by using a Suzuki and Abe algorithm; fitting all edge curves through a broken line by using a Douglas-Peucker algorithm; searching all closed contours formed by edge curves, and discarding polygons which cannot be approximated to 4 vertexes; keeping the outermost layer contour in the rest rectangular contour, analyzing the inner region of the contour according to the black boundary characteristic of the outermost layer of the Aruco Marker, dividing grids, extracting coding information, and acquiring an ID according to a pre-stored Marker dictionary;
and after the ID is obtained, obtaining an intersection point of the solved boundary line, obtaining 4 vertex coordinates, solving the relative pose between the camera and the Aruco Marker by using a P4P method according to the camera parameters, and further guiding the follow-up AUV to enter the ring.
2. The method for accurately guiding the lead-in ring of the underwater vehicle based on the monocular vision as recited in claim 1, wherein: the hamming code is introduced into the coding of the Aruco Marker, the accuracy and the fault tolerance of information extraction are enhanced, the hamming distance of the Aruco Marker in different rotation directions can be obtained, the hamming distance of the Marker with the correct direction is zero, and the Marker in other different view angles is not zero, so that the rotation direction can be determined according to the Aruco Marker coding.
3. The method for accurately guiding the lead-in ring of the underwater vehicle based on the monocular vision as recited in claim 1, wherein: after an RGB image is collected by a monocular camera along with an AUV, the image is enhanced by adopting an image enhancement method based on a global adaptive threshold value:
step 1: converting the RGB image into a YUV color space, and extracting the brightness value of a Y channel;
step 2: carrying out normalization processing on the Y-channel brightness value of each pixel:
L(i,j)＝Y(i,j)/255
wherein (i, j) is the pixel coordinate corresponding to the image, and L (i, j) is the brightness value after normalization;
and step 3: calculating a logarithmic average of luminance values of an input image
In the formula: m is the height of the image pixel array, n is the width of the image pixel array, and sigma is a set small value; the global adaptive threshold is calculated according to the logarithmic mean of the brightness as follows:
and 4, step 4: respectively scaling the brightness of the RGB three channels of the original image according to the logarithmic average value of the brightness value of the image:
R′(i,j)＝R(i,j)*Lg(i,j)
G′(i,j)＝G(i,j)*Lg(i,j)
B′(i,j)＝B(i,j)*Lg(i,j)
thereby achieving image enhancement.
4. The method for accurately guiding the lead-in ring of the underwater vehicle based on the monocular vision as recited in claim 1, wherein: and extracting the existence area of the Aruco Marker by adopting a KCF target tracking algorithm, reducing the number of pixels needing to be traversed in the identification and detection process of the Aruco Marker, and accelerating the identification speed of the Aruco Marker.
5. The method for accurately guiding the lead-in ring of the underwater vehicle based on the monocular vision as recited in claim 1, wherein: interference in the process of identifying and detecting the Aruco Marker is filtered by Kalman filtering:
after the Aruco Marker is successfully identified and detected for one time, the rotation matrix R is solved according to the output result, and the coordinate of the camera under the reference coordinate system of the Aruco Marker
And the roll angle of the camera relative to the Aruco Marker
Pitch angle
And yaw angle
State x of the k-th cyclekIs defined as:
according to the Kalman filtering model, the system state of the k-th period can be obtained from the system state of the k-1 th period, and the dynamic equation is as follows:
xk＝Axk-1+Buk+ωk
where A is the state transition matrix, B is the control input matrix, ωkIs process noise, obeys a mean of 0 and covariance matrix of QkMultivariate normal distribution; the state transition matrix a is defined as follows:
wherein dT is the sampling time;
for state xkThe measurement at time k is given by the metrology equation:
zk＝Hxk+vk
where H is the observation matrix, vkIs process noise, which obeys a mean of 0 and a covariance matrix of RkWhite gaussian noise of (1); the observation matrix H is defined as follows:
and the velocity measurement vector and the angular velocity measurement vector following the AUV are respectively:
the velocity vector of the following AUV relative to the Aruco Marker can be obtained according to the rotation matrix R
And angular velocity vector
According to the dynamic equation and the measurement equation, iteration is carried out by using Kalman filtering, and the prediction updating process is described as follows:
wherein
The state predicted value of the current moment is obtained according to the optimal estimated value of the previous moment,
as a function of the error covariance P at the last momentk-1And the error of the current moment obtained by predicting the process noise Q; the state update process is described as:
wherein KkIn order to obtain the gain of the kalman filter,
for optimal result of final output, PkTo update the error value.
6. The method for accurately guiding the lead-in ring of the underwater vehicle based on the monocular vision as recited in claim 5, wherein:
after the Aruco Marker successfully performs primary identification detection, the output result is a rotation vector r ═ r (r)1,r2,r3)TAnd the translation vector t ═ t (t)1,t2,t3)T(ii) a Obtaining a rotation matrix from the rotation vector
Where I is a unit vector, θ ═ norm (r) is the modulo length of the rotation vector, and r' ═ r/θ is the unit vector of the rotation vector; and then obtaining a rotation and translation matrix of the camera under the reference coordinate system of the Aruco Marker as follows:
and obtaining the coordinates of the monocular camera under the reference coordinate system of the Aruco Marker, and the roll angle, the pitch angle and the yaw angle of the camera relative to the Aruco Marker as follows:
wherein
The coordinate of the camera under a Marker reference coordinate system;
respectively roll angle, pitch angle and yaw angle of the camera relative to the Marker.
7. The method for accurately guiding the lead-in ring of the underwater vehicle based on the monocular vision as recited in claim 5, wherein:
obtaining Q of Kalman filter through experimental testkAnd RkHas a value of
Wherein I3Is a 3-dimensional identity matrix, 03Is a 3-dimensional 0 matrix.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010998147.XA CN112417948B (en) | 2020-09-21 | 2020-09-21 | Method for accurately guiding lead-in ring of underwater vehicle based on monocular vision |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010998147.XA CN112417948B (en) | 2020-09-21 | 2020-09-21 | Method for accurately guiding lead-in ring of underwater vehicle based on monocular vision |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112417948A true CN112417948A (en) | 2021-02-26 |
| CN112417948B CN112417948B (en) | 2024-01-12 |
Family
ID=74854955
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010998147.XA Active CN112417948B (en) | 2020-09-21 | 2020-09-21 | Method for accurately guiding lead-in ring of underwater vehicle based on monocular vision |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112417948B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114399586A (en) * | 2021-12-08 | 2022-04-26 | 重庆大学 | Touch fingertip sensor, operating system thereof and data processing method |
| CN115060238A (en) * | 2022-05-18 | 2022-09-16 | 深圳荔石创新科技有限公司 | Relative pose measurement method and device for underwater component |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| RU2444759C1 (en) * | 2010-09-21 | 2012-03-10 | Юрий Николаевич Жуков | Navigation method of underwater object by means of hydroacoustic navigation system |
| CN102538781A (en) * | 2011-12-14 | 2012-07-04 | 浙江大学 | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method |
| CN105629981A (en) * | 2016-02-04 | 2016-06-01 | 青岛市光电工程技术研究院 | Underwater laser guide method |
| CN106482749A (en) * | 2016-12-07 | 2017-03-08 | 西北工业大学 | Alignment methods are combined with tachometer based on the inertial navigation of reverse navigation algorithm |
| CN106546233A (en) * | 2016-10-31 | 2017-03-29 | 西北工业大学 | A kind of monocular visual positioning method towards cooperative target |
| CN106767841A (en) * | 2016-11-25 | 2017-05-31 | 上海航天控制技术研究所 | Vision navigation method based on self adaptation volume Kalman filtering and single-point random sampling |
| CN107300382A (en) * | 2017-06-27 | 2017-10-27 | 西北工业大学 | A kind of monocular visual positioning method for underwater robot |
| CN108444478A (en) * | 2018-03-13 | 2018-08-24 | 西北工业大学 | A kind of mobile target visual position and orientation estimation method for submarine navigation device |
| CN109048893A (en) * | 2018-07-27 | 2018-12-21 | 浙江工业大学 | A kind of mechanical arm localization method based on monocular RGB camera |
| US20190266440A1 (en) * | 2018-02-26 | 2019-08-29 | The United States Of America As Represented By The Secretary Of The Navy | Self-Adapting Guidance Method for Unmanned Underwater Vehicle Docking |
| CN110194255A (en) * | 2019-05-21 | 2019-09-03 | 哈尔滨工程大学 | A kind of optical guidance method that AUV is moved in water-conveyance tunnel |
| CN110261858A (en) * | 2019-06-10 | 2019-09-20 | 哈尔滨工程大学 | A kind of tunnel AUV bootstrap technique based on underwater marker straight-line detection |
-
2020
- 2020-09-21 CN CN202010998147.XA patent/CN112417948B/en active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| RU2444759C1 (en) * | 2010-09-21 | 2012-03-10 | Юрий Николаевич Жуков | Navigation method of underwater object by means of hydroacoustic navigation system |
| CN102538781A (en) * | 2011-12-14 | 2012-07-04 | 浙江大学 | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method |
| CN105629981A (en) * | 2016-02-04 | 2016-06-01 | 青岛市光电工程技术研究院 | Underwater laser guide method |
| CN106546233A (en) * | 2016-10-31 | 2017-03-29 | 西北工业大学 | A kind of monocular visual positioning method towards cooperative target |
| CN106767841A (en) * | 2016-11-25 | 2017-05-31 | 上海航天控制技术研究所 | Vision navigation method based on self adaptation volume Kalman filtering and single-point random sampling |
| CN106482749A (en) * | 2016-12-07 | 2017-03-08 | 西北工业大学 | Alignment methods are combined with tachometer based on the inertial navigation of reverse navigation algorithm |
| CN107300382A (en) * | 2017-06-27 | 2017-10-27 | 西北工业大学 | A kind of monocular visual positioning method for underwater robot |
| US20190266440A1 (en) * | 2018-02-26 | 2019-08-29 | The United States Of America As Represented By The Secretary Of The Navy | Self-Adapting Guidance Method for Unmanned Underwater Vehicle Docking |
| CN108444478A (en) * | 2018-03-13 | 2018-08-24 | 西北工业大学 | A kind of mobile target visual position and orientation estimation method for submarine navigation device |
| CN109048893A (en) * | 2018-07-27 | 2018-12-21 | 浙江工业大学 | A kind of mechanical arm localization method based on monocular RGB camera |
| CN110194255A (en) * | 2019-05-21 | 2019-09-03 | 哈尔滨工程大学 | A kind of optical guidance method that AUV is moved in water-conveyance tunnel |
| CN110261858A (en) * | 2019-06-10 | 2019-09-20 | 哈尔滨工程大学 | A kind of tunnel AUV bootstrap technique based on underwater marker straight-line detection |
Non-Patent Citations (2)
| Title |
|---|
| YICHEN LI 等: "Multiple Autonomous Underwater Vehicle Cooperative Localization in Anchor-Free Environments", 《IEEE JOURNAL OF OCEANIC ENGINEERING》, vol. 44, no. 4, XP011750166, DOI: 10.1109/JOE.2019.2935516 * |
| 张立川 等: "多无人水下航行器协同导航定位研究进展", 《高技术通讯》, vol. 26, no. 5 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114399586A (en) * | 2021-12-08 | 2022-04-26 | 重庆大学 | Touch fingertip sensor, operating system thereof and data processing method |
| CN115060238A (en) * | 2022-05-18 | 2022-09-16 | 深圳荔石创新科技有限公司 | Relative pose measurement method and device for underwater component |
| CN115060238B (en) * | 2022-05-18 | 2023-11-10 | 深圳荔石创新科技有限公司 | Method and device for measuring relative pose of underwater component |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112417948B (en) | 2024-01-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US12072705B2 (en) | Intelligent decision-making method and system for unmanned surface vehicle | |
| Wu et al. | Vision-based real-time aerial object localization and tracking for UAV sensing system | |
| Zhang et al. | Eye in the sky: Drone-based object tracking and 3d localization | |
| CN108596053B (en) | Vehicle detection method and system based on SSD and vehicle posture classification | |
| Wang et al. | Real-time underwater onboard vision sensing system for robotic gripping | |
| CN107038713A (en) | A kind of moving target method for catching for merging optical flow method and neutral net | |
| CN105787962B (en) | A kind of monocular vision tracking recycled under water based on UUV | |
| Ren et al. | Two AUVs guidance method for self-reconfiguration mission based on monocular vision | |
| Chen et al. | A stereo visual-inertial SLAM approach for indoor mobile robots in unknown environments without occlusions | |
| Fan et al. | Dynamicfilter: an online dynamic objects removal framework for highly dynamic environments | |
| CN105913455A (en) | Local image enhancement-based object tracking method | |
| CN112417948B (en) | Method for accurately guiding lead-in ring of underwater vehicle based on monocular vision | |
| CN106780560A (en) | A kind of feature based merges the bionic machine fish visual tracking method of particle filter | |
| Muthu et al. | Motion segmentation of rgb-d sequences: Combining semantic and motion information using statistical inference | |
| CN111680713A (en) | Unmanned aerial vehicle ground target tracking and approaching method based on visual detection | |
| CN108469729B (en) | Human body target identification and following method based on RGB-D information | |
| Alla et al. | Vision-based Deep Learning algorithm for Underwater Object Detection and Tracking | |
| Li et al. | Vision-based target detection and positioning approach for underwater robots | |
| Karampinis et al. | Ensuring UAV Safety: A Vision-only and Real-time Framework for Collision Avoidance Through Object Detection, Tracking, and Distance Estimation | |
| CN113888603A (en) | Loop detection and visual SLAM method based on optical flow tracking and feature matching | |
| CN103996207A (en) | Object tracking method | |
| CN117664124A (en) | Inertial guidance and visual information fusion AGV navigation system and method based on ROS | |
| CN116185049A (en) | Unmanned helicopter autonomous landing method based on visual guidance | |
| CN113239936B (en) | Unmanned aerial vehicle visual navigation method based on deep learning and feature point extraction | |
| Dutrannois et al. | Visual SLAM for autonomous drone landing on a maritime platform |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |