Autonomous docking system based on multi-marker collaborative guidance

Abstract

The invention discloses an autonomous docking system based on multi-marker cooperative guidance, and relates to the field of automatic control of underwater vehicles, and the method comprises the steps: fixing a plurality of markers of a guiding light source and a nested structure identification code on a docking station body of a return structure, an image in the advancing direction is obtained in real time through a carried camera, marker detection is carried out, and the position parameter of a marker in a UUV coordinate system is determined and converted into the position parameter in a docking station coordinate system. And determining y-axis and z-axis constraint quantities at the x coordinate of the position parameter according to the error constraint function. And according to the constraint quantity and the position parameter, the position error of the y-axis and the z-axis is determined, the motion in the azimuth direction and the pitching direction is adjusted through an autonomous docking controller, and the UUV is controlled to advance forwards till the docking position is reached. Collision damage caused by blind punching at the tail end can be effectively avoided through cooperative guidance of multiple markers, and stability and reliability of the whole autonomous docking process of the UUV are ensured.

Description

Technical Field

[0001] The present application relates to the field of automatic control of underwater unmanned vehicles, and in particular to an autonomous docking system based on collaborative guidance of multiple markers. Background Art

[0002] The deep sea brings many resources to human beings, but also brings a lot of challenges, which makes UUV (Unmanned Underwater Vehicle) an indispensable tool in underwater detection, search and energy mining. However, due to the limitation of size and load, UUV cannot perform tasks uninterruptedly for a long time, and must be regularly docked and recovered for energy supply and information exchange.

[0003] At present, the docking guidance systems used at home and abroad are mainly of the return cage type, which guides the UUV into the guide port of the docking station by carrying electromagnetic, acoustic, optical and other sensors or markers on the guide port of the docking station. However, after the UUV enters the guide port of the docking station, it mainly relies on the UUV to blindly rush to the docking position at the end of the docking station according to the preset behavior and the structural constraints of the docking station to complete the docking recovery. However, the blind rushing behavior of the UUV in the terminal area between the guide port and the terminal docking position inside the docking station will undoubtedly cause a collision between it and the docking station, which will not only affect the docking accuracy but also damage the UUV. Summary of the invention

[0004] In response to the above problems and technical requirements, this application proposes an autonomous docking system based on multi-marker collaborative guidance. The technical solution of this application is as follows:

[0005] An autonomous docking system based on multi-marker collaborative guidance includes the following steps:

[0006] The autonomous docking system includes an underwater docking station and a UUV, wherein the UUV is equipped with a camera; the underwater docking station includes a docking station body with a cage-type structure and a marker fixed thereon, the size of the docking station body matches the UUV, the marker includes a guiding light source and a plurality of identification codes constituting a nested structure, the guiding light source is installed at the entrance of the docking station body, and the plurality of identification codes constituting the nested structure are arranged at the end inside the docking station body and facing the entrance of the docking station body; each marker has a corresponding guiding area, and the guiding area of ​​each marker is an imaging area of ​​the marker in the camera of the UUV when the underwater docking station is used as a reference object, two adjacent guiding areas are cross-overlapped, and the total guiding range of the guiding areas of all markers covers the area between the docking position at the end inside the docking station body and the predetermined position outside the entrance of the docking station body; the autonomous docking method performed by the UUV includes:

[0007] The camera on board is used to obtain images in the forward direction in real time and detect markers to determine the position parameters of the markers in the UUV coordinate system. The UUV coordinate system takes the center of gravity of the UUV as the coordinate origin, the UUV sailing direction as the positive direction of the x-axis, the azimuth direction pointing to the port side of the UUV as the positive direction of the y-axis, and the pitch direction perpendicular to the xy plane downward as the positive direction of the z-axis;

[0008] The position parameters of the marker in the UUV coordinate system Convert to the position parameter P(x,y,z) in the docking station coordinate system; determine the y-axis constraint l at the x-coordinate of the position parameter P(x,y,z) according to the error constraint function y (x) and z-axis constraints l z (x); the error constraint function is a continuous piecewise function with respect to the x coordinate within the total guidance range of the guidance regions of all markers, and the constraint amount indicated by the error constraint function converges toward 0 as the x coordinate decreases;

[0009] According to the y-axis constraint l y (x) and the y coordinate of the position parameter P(x,y,z) determine the y-axis position error, and according to the z-axis constraint l z (x) and the z coordinate of the position parameter P(x,y,z) determine the z-axis position error;

[0010] The autonomous docking controller is used to adjust the azimuth movement according to the y-axis position error, and the pitch movement is adjusted according to the z-axis position error, and the forward propulsion is controlled until the docking position at the end of the docking station body is reached.

[0011] Its further technical solution is that the error constraint function is a continuous piecewise function with respect to the x-coordinate within the total guiding range of the guiding areas of all markers, the piecewise function of the error constraint function in each single marker guiding area indicates that the constraint amount of each axis decreases as the x-coordinate decreases, and the piecewise function of the error constraint function in each transition guiding area indicates that the constraint amount of each axis has a maximum and a minimum and the x-coordinate corresponding to the maximum is smaller than the x-coordinate corresponding to the minimum; wherein each single marker guiding area is an area in the guiding area of ​​each marker that does not cross-overlap with the guiding areas of other markers, and the transition guiding area is an overlapping area of ​​the guiding areas of multiple markers.

[0012] Its further technical solution is that the constraint amount of the piecewise function of the error constraint function in each single marker guiding area is equal to that of the piecewise function in the adjacent transition guiding area at the boundary position, and the derivative of the piecewise function of the error constraint function in each single marker guiding area is equal to that of the piecewise function in the adjacent transition guiding area at the boundary position.

[0013] Its further technical solution is that, for any one of the y-axis and the z-axis, the piecewise function of the error constraint function in each single marker guidance area indicates that the constraint amount of the current axis is in the form of a linear equation with the x-coordinate and l(x)=kx+f, wherein the slope k and the intercept f are respectively related to the recognition and positioning index requirements of the corresponding direction of the current axis and the coverage range of the guidance area corresponding to each marker.

[0014] Its further technical solution is that the slope in, is the value range of the guide area closest to the inner end of the docking station body along the x-coordinate, P is the comprehensive positioning stability and positioning error of the guide area of ​​all markers, and P e is the comprehensive positioning error of the guide area of ​​all markers, P o is the preset camera field of view, d e is the allowable error for autonomous docking.

[0015] A further technical solution is that the intercept of the piecewise function in any single marker guide region Among them, [x min ,x max ] is the value range of the current single marker guide area along the x coordinate, p s is the localization stability of the current single marker guided region, p e is the positioning error of the current single marker guided region, P o is the preset camera field of view, d e is the allowable error for autonomous docking.

[0016] A further technical solution is that, for any one of the y-axis and the z-axis, the piecewise function of the error constraint function in each transition guide region indicates that the constraint amount of the current axis and the x-coordinate are in the form of a polynomial equation and l(x)=ax 3 +bx 2 +cx+d, where the constraint coefficients a, b, c, and d are respectively related to the piecewise function within the single marker guidance area in the direction corresponding to the current axis and the coverage of the guidance area corresponding to each marker.

[0017] A further technical solution is that the error constraint function in each single marker guide region indicates that the constraint amount of the current axis is in the form of a straight line equation with the x coordinate and l(x)=kx+f; the constraint coefficients a, b, c, d of the piecewise function in any transition guide region are:

[0018]

[0019] Wherein, k is the slope of the piecewise function within the single-marker guide region, f2 is the intercept of the piecewise function of the guide region close to the inner end of the docking station body in two adjacent single-marker guide regions, and f1 is the intercept of the piecewise function of the guide region far from the inner end of the docking station body in two adjacent single-marker guide regions. is the maximum value of the range of values ​​of the guide area close to the inner end of the docking station body in the two adjacent single-marker guide areas along the x-coordinate, It is the minimum value of the range of values ​​of the guide area far away from the inner end of the docking station body in the two adjacent single-marker guide areas along the x-coordinate.

[0020] Its further technical solution is that the guiding light source includes a main light source and n auxiliary light sources installed around the entrance of the docking station body, the main light source and the auxiliary light source have different light colors, and the integer parameter n≥3 determines the position parameter of the marker in the UUV coordinate system. include:

[0021] Performing region segmentation processing on the acquired image to determine the center coordinates of several candidate main light sources having the light color of the main light source, and determining the center coordinates of several candidate auxiliary light sources having the light color of the auxiliary light source;

[0022] A candidate main light source center coordinate and n candidate auxiliary light source center coordinates satisfying the physical layout position relationship of the light source are selected from the identified plurality of candidate main light source center coordinates and candidate auxiliary light source center coordinates to obtain the pixel coordinates of each guiding light source;

[0023] For the guide area of ​​a single marker, the pixel coordinates are solved using the PnP algorithm to obtain the position parameter P'(x, y, z) of the marker in the camera coordinate system;

[0024] For the overlapping area of ​​the guide areas of two adjacent markers, the position parameters of the single marker in the camera coordinate system are calculated. and Perform pose fusion to obtain the position parameters of the marker in the camera coordinate system

[0025]

[0026] The position parameters P'(x, y, z) of the marker in the camera coordinate system are transformed to determine the position parameters of the marker in the UUV coordinate system.

[0027] A further technical solution is to determine the size of the docking station body according to the docking accuracy requirements and the size of the UUV. l is the length of the docking station body, w is the width of the docking station body, and h is the height of the docking station body; l UUVis the length of the UUV, w UUV is the width of the UUV, h UUV is the height of the UUV; Δx is the docking accuracy in the length direction, Δy is the docking accuracy in the width direction, and Δz is the docking accuracy in the height direction;

[0028] The multiple identification codes constituting the nested structure include a first identification code and a second identification code, the size of the first identification code is larger than the second identification code and the second identification code is nested in the first identification code, each identification code is a square structure, and the size of the nested structure is determined according to the size of the docking station body and the docking accuracy Δx in the length direction l q1 is the side length of the second identification code, l q2 is the side length of the first identification code, θ is the horizontal field of view of the camera, and β is the vertical field of view of the camera.

[0029] The beneficial technical effects of this application are:

[0030] The present application proposes an autonomous docking system based on multi-marker collaborative guidance, which forms an autonomous docking system guided by multiple markers by setting a guiding light source and multiple identification codes of a nested structure on the traditional cage-type docking station body, and can achieve long-distance guidance and terminal guidance inside the docking station body. This enables the UUV to have a stable docking posture throughout the docking process, effectively avoiding blind collision at the terminal, and ensuring the stability and reliability of autonomous docking.

[0031] The guidance areas of all markers in the docking process are divided into single marker guidance areas corresponding to each marker and transition guidance areas between the guidance areas of two adjacent markers, and the coverage range and identification and positioning index requirements of each guidance area are converted into error constraint functions. The control divergence problem caused by the large differences in positioning stability in different marker guidance areas is effectively solved, ensuring the smooth and efficient autonomous docking process.

[0032] By setting a guiding light source with one main light source and multiple auxiliary light sources, the acquired guiding light source image is segmented and the pixel coordinates of each guiding light source that meets the physical layout position relationship of the light source are screened out. This eliminates the false targets of the guiding light source due to the reflection of the water surface, effectively suppresses the interference of the water surface reflection on the positioning stability, improves the recognition accuracy of the guiding light source, and thus effectively improves the docking accuracy of the autonomous docking system. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 It is an autonomous docking system based on the collaborative guidance of multiple markers.

[0034] Figure 2 It is a flow chart of the autonomous docking method performed by the UUV.

[0035] Figure 3 is the obtained guiding light source image.

[0036] Figure 4 It is a flow chart for determining pixel coordinates of the guiding light source.

[0037] Figure 5 is the error constraint function distribution diagram.

[0038] Figure 6 It is the autonomous docking controller of the autonomous docking system.

[0039] Figure 7 is the navigation path of the UUV. DETAILED DESCRIPTION

[0040] The specific implementation of the present application is further described below in conjunction with the accompanying drawings.

[0041] This application proposes an autonomous docking system based on multi-marker collaborative guidance, please refer to Figure 1 The autonomous docking system includes an underwater docking station and a UUV. The UUV is equipped with a camera to obtain images in the forward direction in real time. The underwater docking station includes a docking station body with a cage-type structure and a marker fixed thereon. The size of the docking station body matches the UUV to ensure that the UUV can be accommodated within the docking accuracy range. In one embodiment, the size of the docking station body is determined according to the docking accuracy requirements and the size of the UUV as follows:

[0042]

[0043] Wherein, l is the length of the docking station body, w is the width of the docking station body, and h is the height of the docking station body; l UUV is the length of the UUV, w UUV is the width of the UUV, h UUV is the height of the UUV; Δx is the docking accuracy in the length direction, Δy is the docking accuracy in the width direction, and Δz is the docking accuracy in the height direction; the length direction indicates the direction from the end of the docking station body to the entrance of the docking station body, the width direction indicates the direction from the right side to the left side of the docking station body, and the height direction indicates the direction from the upper surface to the lower surface of the docking station body. The docking accuracy in each direction can be customized according to actual application requirements. This application sets the docking accuracy in the length direction Δx = 90mm, the docking accuracy in the width direction Δy = 60mm, and the docking accuracy in the height direction Δz = 100mm. In order to facilitate the operation of the underwater deployment of the docking station body, the docking station body is built into a rectangular frame structure using portable aluminum materials. According to the size of the UUV l UUV =600mm, w UUV =480mm,h UUV=300mm, it can be determined that the dimensions of the docking station body are l = 780mm, w = 600mm, and h = 500mm.

[0044] Compared with the docking station body of the traditional return cage structure, the improved docking station body proposed in the present application is a marker fixed on the docking station body. It not only includes a guiding light source installed at the entrance of the docking station body, but also includes a plurality of identification codes that are arranged at the end inside the docking station body and face the entrance of the docking station body to form a nested structure. It combines the long-distance recognition characteristics of the guiding light source and the close-range accurate recognition performance of the nested structure identification code, and fully utilizes multiple markers to achieve docking guidance. The guiding light source is composed of multiple light sources. The number, combination and light color of the light sources can be customized according to the actual application. In one embodiment, the guiding light source includes a main light source and n auxiliary light sources installed around the entrance of the docking station body. The main light source and the auxiliary light source have different light colors, and the integer parameter n≥3. The present application selects a blue-green light source with a smaller attenuation in water. A green main light source is set at the lower left corner point around the entrance of the docking station body, which is recorded as light source No. 1, and three blue auxiliary light sources are set at the other three corner points, which are recorded as light sources No. 2, No. 3 and No. 4 respectively. The specific physical layout position relationship is as follows Figure 1 shown.

[0045] The guiding light source is used to guide the UUV to navigate toward the entrance of the docking station body at a long distance, and the multiple identification codes forming a nested structure are used to guide the UUV into the docking station body at a short distance and navigate toward the docking position at the end of the docking station body, ensuring that the UUV is constantly guided by the marker after entering the docking station body to prevent collision. Each marker has its own corresponding guiding area, and the guiding area of ​​each marker is the imaging area of ​​the marker in the UUV camera when the underwater docking station is used as a reference. There is a cross-overlap between two adjacent guiding areas, and the total guiding range of the guiding areas of all markers covers the area between the docking position at the end of the docking station body and the predetermined position outside the entrance of the docking station body. The predetermined position outside the entrance of the docking station body is determined according to the imaging area of ​​the guiding light source in the UUV camera.

[0046] The multiple identification codes that constitute the nested structure are used to ensure the alternating connection of the guide areas inside the docking station body, and the specific size, type and layout can be customized. For example, the multiple identification codes that constitute the nested structure are pasted at the center of the end frame inside the docking station body and facing the entrance of the docking station body. Various forms of identification codes can be used, including QR codes, bar codes, and two-dimensional codes. In one embodiment, the multiple identification codes that constitute the nested structure include a first identification code and a second identification code. In order to ensure that the second identification code can be recognized at a close distance, the size of the first identification code is larger than the second identification code and the second identification code is nested in the first identification code. Each identification code is a square structure. The basis for setting the size of the second identification code is to ensure that it can still appear within the field of view of the camera after the autonomous docking is completed. Specifically, the size of the nested structure is determined according to the size of the docking station body and the docking accuracy Δx in the length direction. l q1 is the side length of the second identification code, l q2 is the side length of the first identification code, θ is the horizontal field of view of the camera, and β is the vertical field of view of the camera.

[0047] In one embodiment, based on the autonomous docking system optimized and designed in this application, the autonomous docking method performed by the UUV can be found in Figure 2 The specific steps are as follows:

[0048] Step 1: Use the onboard camera to obtain images in the forward direction in real time and perform marker detection.

[0049] When there is a guiding light source in the acquired image, due to the reflection on the water surface, the guiding light source marker will appear as a pseudo target in the camera field of view. Figure 3 As shown, the upper part of the image is a pseudo target, and the lower part is a real target. Since the image of the reflection of the guiding light source on the water surface in the camera is very similar to the image of the real guiding light source in the camera, in order to accurately identify the guiding light source, it is necessary to remove the pseudo target of the water surface reflection in the image, filter out the real guiding light source target to obtain the pixel coordinates of the guiding light source. In one embodiment, the specific method for determining the pixel coordinates of the guiding light source can be referred to Figure 4 The specific steps are as follows:

[0050] (1) Performing regional segmentation processing on the acquired image to determine the center coordinates of several candidate main light sources with the color of the main light source. When the main light source is a green light source, regional segmentation processing is used to obtain a segmented image of the green light source. The regional segmentation method of the present application adopts a double-threshold green region segmentation method based on the HSV space, converts the acquired image into the HSV color space, and determines the segmentation threshold according to the distribution range of the green hue H, green saturation S and green brightness V of green in the HSV color space. Pixel points in the image that are within the segmentation threshold range are selected to form a binary image of the segmented green region, and the pixel points in the green region are averaged to obtain the center coordinates (x g ,y g )and is the coordinate of pixel k in the green area, n is the total number of pixels in the green area and W×H is the resolution of the binary image, is the pixel value of pixel k.

[0051] (2) Performing regional segmentation processing on the acquired image to determine the center coordinates of several candidate main light sources with the color of the main light source and several candidate auxiliary light sources with the color of the auxiliary light source. When the auxiliary light source is a blue light source, the same regional segmentation method as in (1) is used to determine the segmentation threshold based on the distribution range of green and blue in the HSV color space, and obtain the binary image after all light sources are segmented. The pixel points of the segmented area corresponding to each light source are averaged to obtain the center coordinates of all candidate light sources. All candidate light sources include several candidate main light sources and several candidate auxiliary light sources.

[0052] (3) Select one candidate main light source center coordinate and n candidate auxiliary light source center coordinates that meet the physical layout position relationship of the light source from the identified candidate main light source center coordinates and the candidate auxiliary light source center coordinates to obtain the pixel coordinates of each guiding light source. For the main light source is a green light source, and the auxiliary light sources are three blue light sources, when the center coordinates (x1, y1), (x2, y2), (x3, y3), (x4, y4) of the four identified light sources located in the segmented image meet Figure 1 When the physical layout position relationship of the light sources is shown, the center coordinates of the four identification light sources are determined as the pixel coordinates of the guiding light source. Among them, the center coordinates (x1, y1) of the identification light source located in the lower left corner and the center coordinates (x g ,y g ) overlap, the expression of the physical layout position relationship is This achieves the elimination of false targets and the screening of real targets.

[0053] When there is a nested structured identification code in the acquired image, the identification code recognition method is used to determine the pixel coordinates of the identification code. In the case where the identification code is a QR code, the pixel coordinates of the QR code are calculated using a QR code recognition algorithm. The specific method can adopt the method of the existing technology, which is not elaborated in this application.

[0054] In one embodiment, for the guide area of ​​a single marker, there is only one marker in the acquired image at this time, and the PnP algorithm is used to solve the position parameter P'(x, y, z) of the marker in the camera coordinate system according to the calculated pixel coordinates of the marker, wherein the PnP algorithm is a prior art and will not be described in detail in this application. For the cross-overlapping area of ​​the guide areas of two adjacent markers, there are two markers in the acquired image at this time, and the position parameter P'(x, y, z) of the single marker in the camera coordinate system is solved. and Perform pose fusion to obtain the position parameters of the marker in the camera coordinate system

[0055] The position parameters P'(x, y, z) of the marker in the camera coordinate system are transformed to determine the position parameters of the marker in the UUV coordinate system. Among them, R ct is the rotation matrix between the camera coordinate system and the marker coordinate system, T ct is the translation matrix between the camera coordinate system and the marker coordinate system, R co is the rotation matrix between the camera coordinate system and the UUV coordinate system, T co is the translation matrix between the camera coordinate system and the UUV coordinate system. The UUV coordinate system takes the center of gravity of the UUV as the coordinate origin, the UUV sailing direction as the positive direction of the x-axis, the azimuth pointing to the UUV port side as the positive direction of the y-axis, and the pitch direction perpendicular to the xy plane downward as the positive direction of the z-axis.

[0056] The position parameters of the marker in the UUV coordinate system Converted to the position parameters in the docking station coordinate system in, is the rotation matrix between the UUV coordinate system and the docking station coordinate system, is the translation matrix between the UUV coordinate system and the docking station coordinate system. Figure 1 The center of the end frame inside the docking station body is taken as the coordinate origin, the positive direction of the x-axis is pointed to the entrance of the docking station along the length direction of the docking station, the positive direction of the y-axis is pointed to the left side of the docking station along the width direction of the docking station, and the positive direction of the z-axis is perpendicular to the xy plane and downward.

[0057] Step 2: Determine the y-axis constraint l at the x-coordinate of the position parameter P(x,y,z) according to the error constraint functiony (x) and z-axis constraints l z (x).

[0058] The error constraint function is a continuous piecewise function with respect to the x-coordinate within the total guidance range of the guidance areas of all markers, and the constraint amount indicated by the error constraint function converges toward 0 as the x-coordinate decreases. In one embodiment, the continuous piecewise function divides the segmented intervals by the guidance areas of all markers, and the guidance areas of all markers are divided into single-marker guidance areas guided by only one marker and transition guidance areas guided by two markers, each single-marker guidance area is an area in the guidance area of ​​each marker that does not cross-overlap with the guidance areas of other markers, and the transition guidance area is an intersection-overlapping area of ​​the guidance areas of multiple markers.

[0059] In order to ensure the continuous convergence of the error constraint function, the piecewise function of the error constraint function in each single marker guide area indicates that the constraint amount of each axis decreases as the x coordinate decreases. The piecewise function in each single marker guide area can have a variety of different forms. In one embodiment, considering that the change trend of the positioning performance of a single marker is consistent and there will be no jump, for any axis of the y-axis and the z-axis, the piecewise function of the error constraint function in each single marker guide area indicates that the constraint amount of the current axis is in the form of a linear equation with the x coordinate and l(x)=kx+f, and setting the slope of the piecewise function of each single marker guide area to be the same can better ensure the consistency of the overall change trend of the piecewise function within the total guide range, so that the piecewise function of each single marker guide area has the same convergence speed, and the axial constraint amount converges toward 0 only by adjusting the intercept of the piecewise function of each single marker guide area.

[0060] The y-axis is the azimuth direction, and the z-axis is the pitch direction. The constraint function in each direction has the same form. For any of the y-axis and the z-axis, the slope k is determined based on the identification and positioning index requirements for the direction corresponding to the current axis and the coverage range of the guide area corresponding to each marker. The identification and positioning index requirements include the positioning stability, positioning error, preset camera field of view, and autonomous docking allowable error of the marker guide area. The units for setting the various parameters required for the identification and positioning index and the coverage range of the guide area corresponding to each marker are all the same. Slope in, is the value range of the guide area closest to the inner end of the docking station body along the x-coordinate, P is the comprehensive positioning stability and positioning error of the guide area of ​​all markers, and P e is the comprehensive positioning error of the guide area of ​​all markers, P o is the preset camera field of view, d eis the allowable error of autonomous docking. In one example, for the case where the guide light source and the second identification code are nested in the first identification code to form three markers, the comprehensive positioning stability and positioning error Comprehensive positioning error is the positioning stability of the second identification code guide area, is the positioning stability of the first identification code guide area, is the positioning stability of the guiding area of ​​the guiding light source; is the positioning error of the second identification code guide area, is the positioning error of the first identification code guide area, is the positioning error of the guiding area of ​​the guiding light source.

[0061] For any axis in the y-axis and z-axis, the intercept f is determined according to the identification and positioning index requirements of the current axis corresponding to the direction and the coverage range of the guide area corresponding to each marker. The intercept of the piecewise function in the guide area of ​​any single marker is Among them, [x min ,x max ] is the value range of the current single marker guide area along the x coordinate, p s is the localization stability of the current single marker guided region, p e is the positioning error of the current single marker guided region, P o is the preset camera field of view, d e is the autonomous docking tolerance. In one example, for the case where the nested structure of the guiding light source and the second identification code embedded in the first identification code constitutes three markers, the intercept of the piecewise function of the guiding area of ​​the guiding light source is is the value range of the guiding area of ​​the guiding light source along the x-coordinate, is the positioning stability of the guiding area of ​​the guiding light source, is the positioning error of the guiding area of ​​the guiding light source. The intercept of the piecewise function of the guiding area of ​​the first identification code is the value range of the first identification code guide area along the x coordinate, is the positioning stability of the first identification code guide area, is the positioning error of the first identification code leading area. The intercept of the piecewise function of the leading area of ​​the second identification code At this time, the guide area of ​​the second identification code is the guide area closest to the inner end of the docking station body. is the value range of the second identification code guide area along the x coordinate, is the positioning stability of the second identification code guide area, is the positioning error of the second identification code guide area.

[0062] Since there is a large difference in the positioning performance of two adjacent markers, the constraint amount of the guide area between the front and rear markers changes inversely from small to large. Therefore, the piecewise function of the error constraint function in each transition guide area indicates that the constraint amount of each axis has a maximum and a minimum, and the x-coordinate corresponding to the maximum is smaller than the x-coordinate corresponding to the minimum. In order to better transition the docking accuracy of the guide area of ​​the previous marker to the guide area of ​​the next marker, and ensure that the UUV has a small posture change and stable navigation in the transition guide area, in one example, for the case where the nested structure of the guiding light source and the second identification code nested in the first identification code constitutes three markers, it is determined that the piecewise function of the error constraint function in the transition guide area satisfies the constraint condition Wherein, l2(x) is the piecewise function of the first transition area between the guiding light source guiding area and the first identification code guiding area, l1(x) is the piecewise function of the second transition area between the first identification code guiding area and the second identification code guiding area; x1 is the x-coordinate corresponding to the minimum value of the piecewise function of the second transition area, x2 is the x-coordinate corresponding to the maximum value of the piecewise function of the second transition area, x3 is the x-coordinate corresponding to the minimum value of the piecewise function of the first transition area, x4 is the x-coordinate corresponding to the maximum value of the piecewise function of the first transition area, and f -1 () represents the inverse function.

[0063] In order to ensure that the UUV can navigate stably in the transition guidance area and avoid large fluctuations in posture, the error constraint function in each single-marker guidance area has the same constraint amount as the piecewise function in the adjacent transition guidance area at the boundary position, and the piecewise function in each single-marker guidance area has the same derivative as the piecewise function in the adjacent transition guidance area at the boundary position. In one example, for the case where the guiding light source and the second identification code are nested in the first identification code to form a nested structure of three markers, it is determined that the piecewise function of the error constraint function in the transition guidance area satisfies the constraint condition Among them, l t (x) is the piecewise function of the guiding area of ​​the guiding light source, l q1 (x) is the piecewise function of the second identification code leading area, l q2 (x) is a piecewise function of the first identification code leading area.

[0064] The function form satisfying the above two constraints can be used as a piecewise function in the transition guidance area. In one embodiment, considering that the UUV posture change in the transition guidance area achieves maximum stability, the piecewise function in each transition guidance area needs to be kept as smooth as possible. For any axis of the y-axis and the z-axis, the piecewise function of the error constraint function in each transition guidance area indicates that the constraint amount of the current axis and the x-coordinate are in the form of a polynomial equation and l(x)=ax3 +bx 2 +cx+d. The constraint coefficients a, b, c, d of the piecewise function in the transition guide area are determined according to the piecewise function in the single marker guide area in the direction corresponding to the current axis and the coverage of the guide area corresponding to each marker. The constraint coefficients a, b, c, d of the piecewise function in any transition guide area are:

[0065]

[0066] Wherein, k is the slope of the piecewise function within the single-marker guide region, f2 is the intercept of the piecewise function of the guide region close to the inner end of the docking station body in two adjacent single-marker guide regions, and f1 is the intercept of the piecewise function of the guide region far from the inner end of the docking station body in two adjacent single-marker guide regions. is the maximum value of the range of values ​​of the guide area close to the inner end of the docking station body in the two adjacent single-marker guide areas along the x-coordinate, It is the minimum value of the range of values ​​of the guide area far away from the inner end of the docking station body in the two adjacent single-marker guide areas along the x-coordinate.

[0067] In one example, for a nested structure in which a guiding light source and a second identification code are nested in a first identification code to form three markers, according to the slope k of the piecewise function in the guiding area of ​​the three single markers, the intercept f of the piecewise function in the guiding area of ​​the first identification code q2 , the intercept f of the piecewise function that guides the light source and guides the area t , the first identification code guide area along the x-coordinate value range and the guiding light source guiding area along the value range of the x coordinate The constraint coefficients a2, b2, c2, and d2 of the first transition region are determined as follows:

[0068]

[0069] According to the slope k of the piecewise function in the three single-marker guide regions, the intercept f of the piecewise function in the first identification code guide region q2 and the intercept f of the piecewise function of the second identification code leading area q1 And the value range of the first identification code guide area along the x coordinate The value range of the second identification code guide area along the x coordinate The constraint coefficients a1, b1, c1, and d1 of the second transition region are determined as follows:

[0070]

[0071] Determine the slope k of the piecewise function within the three single-marker guide regions to satisfy

[0072] The error constraint function is determined based on the piecewise function in the single marker guide region and the piecewise function in the transition guide region. The continuous piecewise function of the error constraint function with respect to the x coordinate within the total guide range of all marker guide regions And according to the fact that the positioning performance of the guide area closer to the end of the docking station body is higher, it can be determined that f q1 >f q2 >f t , l is the length of the docking station body. The distribution form of the error constraint function in the y-axis azimuth is as follows: Figure 5 shown.

[0073] Step 3: According to the y-axis constraint l y The y-axis position error is determined by the y-coordinate of the position parameter P(x,y,z) for P(y) is the y coordinate of the position parameter P(x,y,z); according to the z-axis constraint l z The z-axis position error is determined by the z-coordinate of the position parameter P(x,y,z) for P(z) is the z coordinate of the position parameter P(x,y,z).

[0074] Step 4: Use the autonomous docking controller to adjust the azimuth movement according to the y-axis position error, use the autonomous docking controller to adjust the pitch movement according to the z-axis position error, and control the x-axis to advance forward until it reaches the docking position at the end of the docking station body, for example, using a PID controller.

[0075] Autonomous docking controller such as Figure 6 As shown, the position loop is used to realize the position control of UUV, and the expression of autonomous docking controller is: Among them, K p is the position loop ratio, K i is the position loop integral, K d is the gain coefficient of the differential link; K p ′ is the speed loop ratio, K i ′ is the speed loop integral, K d ' is the gain coefficient of the differential link; d is the docking position, e t is the position error, d r is the actual position of UUV; v re is the expected speed, v e is the speed deviation, v r is the actual speed of UUV, τ T For control.

[0076] The movement of UUV is controlled based on the position error. For any axis of the y-axis or z-axis, when the position parameter P(x, y, z) is less than the position error of the current axis, the current axis of UUV maintains the current position and only controls the x-axis to move forward; when the position parameter is greater than or equal to the position error, the current axis of UUV is controlled to move to the docking position and the x-axis is controlled to move forward.

[0077] The docking effect of the autonomous docking system proposed in this application is further verified through experiments. This experiment uses a nested QR code, where the small QR code is nested in the middle of the large QR code. The size of the large QR code is 240mm×240mm, and the size of the small QR code is 24mm×24mm. The error constraint function used in the experiment is:

[0078]

[0079] The UUV docking process is controlled according to the error constraint function, and the navigation path of the UUV is obtained as follows: Figure 7 As shown in the figure, when the x coordinate is between 5.6m and 4.2m, the UUV is in the single marker guidance area. At this time, the y-axis position of the UUV exceeds the constraint range of the error constraint function, and the autonomous docking controller immediately adjusts the UUV to the constraint range to continue docking navigation. When the x coordinate is between 2.6m and 2.8m and 1.3m and 1.8m, the UUV is in the transition guidance area, and the UUV makes a small position adjustment movement within the constraint range. At this time, the UUV sails stably until it reaches the docking position of x=0 to complete the docking.

[0080] The above is only a preferred embodiment of the present application, and the present application is not limited to the above embodiments. It is understood that other improvements and changes directly derived or associated by those skilled in the art without departing from the spirit and concept of the present application should be considered to be included in the protection scope of the present application.

Claims

1. An autonomous docking system based on multi-marker collaborative guidance, characterized in that: The autonomous docking system comprises an underwater docking station and a UUV, wherein the UUV is equipped with a camera; the underwater docking station comprises a docking station body of a cage-type structure and a marker fixed thereon, the size of the docking station body matches the UUV, the marker comprises a guiding light source and a plurality of identification codes constituting a nested structure, the guiding light source is installed at the entrance of the docking station body, and the plurality of identification codes constituting the nested structure are arranged at the end inside the docking station body and facing the entrance of the docking station body; each marker has a respective corresponding guiding area, and the guiding area of ​​each marker is an imaging area of ​​the marker in the camera of the UUV when the underwater docking station is used as a reference object, two adjacent guiding areas are cross-overlapped, and the total guiding range of the guiding areas of all markers covers the area between the docking position at the end inside the docking station body and the predetermined position outside the entrance of the docking station body; The autonomous docking method performed by the UUV includes: The camera on board is used to obtain images in the forward direction in real time and detect markers to determine the position parameters of the markers in the UUV coordinate system. The UUV coordinate system takes the center of gravity of the UUV as the coordinate origin, the UUV sailing direction as the positive direction of the x-axis, the azimuth direction pointing to the port side of the UUV as the positive direction of the y-axis, and the pitch direction perpendicular to the xy plane downward as the positive direction of the z-axis; The position parameters of the marker in the UUV coordinate system Convert to the position parameter P(x,y,z) in the docking station coordinate system; determine the y-axis constraint l at the x-coordinate of the position parameter P(x,y,z) according to the error constraint function y (x) and z-axis constraints l z (x); the error constraint function is a continuous piecewise function with respect to the x coordinate within the total guidance range of the guidance regions of all markers, and the constraint amount indicated by the error constraint function converges toward 0 as the x coordinate decreases; According to the y-axis constraint l y (x) and the y coordinate of the position parameter P(x,y,z) determine the y-axis position error, and according to the z-axis constraint l z (x) and the z coordinate of the position parameter P(x,y,z) determine the z-axis position error; The autonomous docking controller is used to adjust the azimuth movement according to the y-axis position error, and the pitch movement is adjusted according to the z-axis position error, and the forward propulsion is controlled until the docking position at the end of the docking station body is reached.

2. The autonomous docking system according to claim 1, characterized in that: The error constraint function is a continuous piecewise function with respect to the x-coordinate within the total guidance range of the guidance areas of all markers, the piecewise function of the error constraint function in each single marker guidance area indicates that the constraint amount of each axis decreases as the x-coordinate decreases, and the piecewise function of the error constraint function in each transition guidance area indicates that the constraint amount of each axis has a maximum and a minimum and the x-coordinate corresponding to the maximum is smaller than the x-coordinate corresponding to the minimum; wherein each single marker guidance area is an area in the guidance area of ​​each marker that does not cross-overlap with the guidance areas of other markers, and the transition guidance area is an intersection-overlapping area of ​​the guidance areas of multiple markers.

3. The autonomous docking system according to claim 2, characterized in that: The constraint amount of the piecewise function of the error constraint function in each single-marker guided region and the piecewise function in the adjacent transition guided region at the boundary position is equal, and the derivative of the piecewise function of the error constraint function in each single-marker guided region and the piecewise function in the adjacent transition guided region at the boundary position is equal.

4. The autonomous docking system according to claim 2, characterized in that: For any one of the y-axis and the z-axis, the piecewise function of the error constraint function in each single marker guidance area indicates that the constraint amount of the current axis is in the form of a straight line equation with the x-coordinate and l(x)=kx+f, where the slope k and the intercept f are respectively related to the identification and positioning index requirements of the corresponding direction of the current axis and the coverage range of the guidance area corresponding to each marker.

5. The autonomous docking system according to claim 4, characterized in that: Slope in, is the value range of the guide area closest to the inner end of the docking station body along the x-coordinate, P is the comprehensive positioning stability and positioning error of the guide area of ​​all markers, and P e is the comprehensive positioning error of the guide area of ​​all markers, P o is the preset camera field of view, d e is the allowable error for autonomous docking.

6. The autonomous docking system according to claim 4, characterized in that: The intercept of the piecewise function within the bootstrap region of any single marker Among them, [x min ,x max ] is the value range of the current single marker guide area along the x coordinate, p s is the localization stability of the current single marker guided region, p e is the positioning error of the current single marker guided region, P o is the preset camera field of view, d e is the allowable error for autonomous docking.

7. The autonomous docking system according to claim 3, characterized in that: For any axis in the y-axis and z-axis, the piecewise function of the error constraint function in each transition guide region indicates that the constraint amount of the current axis and the x-coordinate are in the form of a polynomial equation and l(x)=ax 3 +bx 2 +cx+d, where the constraint coefficients a, b, c, and d are respectively related to the piecewise function within the single marker guidance area in the direction corresponding to the current axis and the coverage of the guidance area corresponding to each marker.

8. The autonomous docking system according to claim 7, characterized in that: The error constraint function is a piecewise function in each single marker guide region. The constraint amount of the current axis is in the form of a straight line equation with the x coordinate and l(x)=kx+f. The constraint coefficients a, b, c, d of the piecewise function in any transition guide region are: Wherein, k is the slope of the piecewise function within the single-marker guide region, f2 is the intercept of the piecewise function of the guide region close to the inner end of the docking station body in two adjacent single-marker guide regions, and f1 is the intercept of the piecewise function of the guide region far from the inner end of the docking station body in two adjacent single-marker guide regions. is the maximum value of the range of values ​​of the guide area close to the inner end of the docking station body in the two adjacent single-marker guide areas along the x-coordinate, It is the minimum value of the range of values ​​of the guide area far away from the inner end of the docking station body in the two adjacent single-marker guide areas along the x-coordinate.

9. The autonomous docking system according to claim 1, characterized in that: The guiding light source includes a main light source and n auxiliary light sources installed around the entrance of the docking station body. The main light source and the auxiliary light source have different light colors. The integer parameter n≥3 determines the position parameter of the marker in the UUV coordinate system. include: Performing region segmentation processing on the acquired image to determine the center coordinates of several candidate main light sources having the light color of the main light source, and determining the center coordinates of several candidate auxiliary light sources having the light color of the auxiliary light source; A candidate main light source center coordinate and n candidate auxiliary light source center coordinates satisfying the physical layout position relationship of the light source are selected from the identified plurality of candidate main light source center coordinates and candidate auxiliary light source center coordinates to obtain the pixel coordinates of each guiding light source; For the guide area of ​​a single marker, the pixel coordinates are solved using the PnP algorithm to obtain the position parameter P'(x, y, z) of the marker in the camera coordinate system; For the overlapping area of ​​the guide areas of two adjacent markers, the position parameters of the single marker in the camera coordinate system are calculated. and Perform pose fusion to obtain the position parameters of the marker in the camera coordinate system The position parameters P'(x, y, z) of the marker in the camera coordinate system are transformed to determine the position parameters of the marker in the UUV coordinate system.

10. The autonomous docking system according to claim 1, characterized in that: The size of the docking station body is determined according to the docking accuracy requirements and the size of the UUV. l is the length of the docking station body, w is the width of the docking station body, and h is the height of the docking station body; l UUV is the length of the UUV, w UUV is the width of the UUV, h UUV is the height of the UUV; Δx is the docking accuracy in the length direction, Δy is the docking accuracy in the width direction, and Δz is the docking accuracy in the height direction; The multiple identification codes constituting the nested structure include a first identification code and a second identification code, the size of the first identification code is larger than the second identification code and the second identification code is nested in the first identification code, each identification code is a square structure, and the size of the nested structure is determined according to the size of the docking station body and the docking accuracy Δx in the length direction l q1 is the side length of the second identification code, l q2 is the side length of the first identification code, θ is the horizontal field of view of the camera, and β is the vertical field of view of the camera.

Images