Vision and GPS based target ship fusion positioning method

Abstract

The application belongs to the technical field of marine surveying and mapping, and particularly relates to a target ship fusion positioning method based on vision and GPS, which comprises the following steps: step 1, target ship identification and tracking based on YOLOv5 and Deep Sort algorithms; step 2, camera imaging model and coordinate conversion; step 3, construction of a relative coordinate measurement model; step 4, camera and GPS space frequency synchronization; step 5, matching of visual trajectories and GPS trajectories by using a DTW algorithm; and step 6, visual perception and GPS fusion positioning based on an EKF algorithm. The visual positioning method has the advantages of being passive and high-precision measurement.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of marine surveying and mapping, and particularly relates to a target ship fusion positioning method based on vision and GPS. BACKGROUND

[0002] At present, the detection and positioning method of the water surface target ship is relatively single, often relies on a single sensor, and cannot realize accurate positioning, such as visual positioning, radar, GPS and other global navigation positioning systems, the GPS has the advantage of wide coverage, but the positioning accuracy is not high, and there is a large accidental error and system error; the radar has a certain blind area, and the detection accuracy is easily affected by adverse sea conditions, sea clutter and the like; and the visual positioning based on the camera is limited by the environmental light intensity and the camera imaging quality. As can be seen from the above, it is difficult to guarantee the stability and accuracy of the target ship detection by using a single sensor, and the fusion positioning by using multiple sensors has become a development trend of target detection technology.

[0003] The technical scheme based on camera and GPS information fusion well combines the advantages of the two sensors, the camera can obtain more color and texture information of the target, which is convenient for identification and tracking of the target, but the positioning error of the small target at a long distance is large; and for GPS positioning, the positioning accuracy is high, but it is easily affected by weather and position, such as poor weather, fewer satellites, and poor satellite distribution area, and the GPS positioning accuracy also deteriorates. SUMMARY

[0004] The purpose of the present application is to provide a target ship fusion positioning method based on vision and GPS to solve the problems in the background art.

[0005] To achieve the above purpose, the present application provides the following technical scheme: a target ship fusion positioning method based on vision and GPS, comprising the following steps:

[0006] Step 1: target ship identification and tracking based on YOLOv5 and Deep Sort algorithm;

[0007] Step 2: camera imaging model and coordinate conversion;

[0008] Step 3: constructing a measurement model of relative coordinates;

[0009] Step 4: camera and GPS space frequency synchronization;

[0010] Step 5: matching the visual trajectory and the GPS trajectory by using the DTW algorithm;

[0011] Step 6: visual perception and GPS fusion positioning based on EKF algorithm.

[0012] Preferably, in step 1, the input video is detected using the trained YOLOv5 network model. The YOLOv5 algorithm analyzes an input frame of the input video, identifies the target information in the frame image, and outputs the category (ID) of the target and the normalized bounding box information (U, V, W, H). Then, the for loop reads each frame of the input video in sequence, outputs the detection results in sequence, and uses the detection results as the input of the Deep Sort algorithm; the Deep Sort algorithm predicts the trajectory information of the previous frame using the Kalman filter algorithm to obtain uncertain trajectory information, and uses the Mahalanobis distance between the predicted trajectory information of the previous frame and the detection box information input by the current frame and the cosine distance based on the appearance feature vector to obtain joint matching information using a linear weighted method. Finally, the Hungarian algorithm is used for matching, and the cascade matching algorithm is used to update the tracking result.

[0013] Preferably, in step 2, the imaging model of the camera is ideally a linear model of central projection, and target positioning is based on the pinhole imaging model, so it is important to establish the relationship between the pixel coordinate system and the world coordinate system, and the conversion between the two is performed using the camera coordinate system and the image coordinate system.

[0014] Preferably, step 2 includes four coordinate systems, namely, a world coordinate system, a camera coordinate system, an image coordinate system, and a pixel coordinate system.

[0015] Preferably, in step 2, three conversions are required to convert from the pixel coordinate system to the world coordinate system.

[0016] 1) Convert from world coordinate system to camera coordinate system

[0017] From the point p in the world coordinate system w (X w ,Y w ,Z w ) is converted into point p in the camera coordinate system c (X c ,Y c ,Z c ), which is essentially the rotation and translation of the coordinate axis, can be represented by a 3×3 rotation matrix R and a 3×1 translation vector T;

[0018]

[0019] 2) Convert from camera coordinate system to image coordinate system

[0020] The point p in the camera coordinate system c (X c ,Y c ,Z c ) is projected onto the image plane to obtain point p in the image coordinate systemi (x i ,y i ), the conversion method is as follows:

[0021]

[0022] Converting the above formula into matrix form, we can get:

[0023]

[0024] In the above formula, f is the focal length of the camera;

[0025] 3) Convert from image coordinate system to pixel coordinate system

[0026] The point p in the image coordinate system i (x i ,y i ) is converted to the pixel coordinate system point p(u p ,v p )

[0027]

[0028] Converting the above formula into matrix form, we can get:

[0029]

[0030] In the above formula, d x and d y For each pixel at x i Axis and y i Physical dimensions in the axial direction, (u i ,v i ) is the origin o in the image coordinate system i The pixel coordinates after transformation into the pixel coordinate system are the principal point coordinates of the image, which can also be written as (c x ,c y ).

[0031] Combining (1), (3), and (5), we can obtain the following matrix form of the world coordinate system converted into the pixel coordinate system:

[0032]

[0033] in, is the intrinsic parameter matrix of the camera, which can be obtained by calibrating the camera.

[0034] Preferably, in step 2, the distortion model generated by the camera is divided into perspective distortion, radial distortion and tangential distortion. The mathematical model of radial distortion can be expressed by a polynomial:

[0035]

[0036] In the above formula, (x t ,y t ) is the actual position of the distortion point on the imaging plane, (x,y) is the ideal position of the distortion point on the imaging plane, k1, k2, k3 are radial distortion coefficients, and generally only the first two terms k1, k2 are used, and for lenses with large distortion, the third term can be added to the image for de-distortion.

[0037] Tangential distortion is mainly due to the deviation of the convex lens during assembly, so that it is not completely parallel to the sensor plane of the camera. The tangential distortion model expression is as follows, p1 and p2 are the tangential distortion coefficients of the camera:

[0038]

[0039] Integrating the above formula (7) and (8) can obtain:

[0040]

[0041] Preferably, in step 3, the origin of the world coordinate system coincides with the origin of the camera coordinate system, i.e. the origins are both the optical centers of the camera, and the conversion from the camera coordinate system to the world coordinate system needs to rotate the camera coordinate system X c axis counterclockwise by an angle α = 90°. According to the definition of Euler angles, the conversion relationship of the coordinate system is shown in formula (11):

[0042]

[0043]

[0044] The translation vector is shown in formula (12):

[0045]

[0046] Substituting formula (11) and formula (12) into formula (6) obtains:

[0047]

[0048] From the above formula, Z c =Y w , substituting this equation into formula (13) can further obtain:

[0049]

[0050] In formula (14), only one variable u is obtained, only the proportional relationship of X w and Y w can be obtained, and the numerical value cannot be further solved, and then the distance formula of the target relative to the camera optical center is introduced to solve X w and Yw The distance formula is as follows:

[0051]

[0052] where h = v t -v b is the height of the target in the pixel coordinate system, H w is the actual height of itself, f is the focal length, Z c is the distance from the target to the optical center of the camera,

[0053]

[0054] In this world coordinate system, the target ship is on the horizontal plane, and the height of the center of mass is constant. By measurement, the target ship is in the world coordinate system Z w ,

[0055] By combining equations (14), (15), and (16), we get:

[0056]

[0057] (X w ,Y w ) is the relative coordinate of the target ship relative to the world coordinate system with the camera optical center as the origin. The YOLOv5 and Deep Sort algorithms are used to detect and track the ship, and the pixel coordinates of the ship in the pixel coordinate system are obtained. According to the distortion model, the ideal position of the imaging point on the imaging plane is obtained, which is substituted into equation (17) to obtain the relative coordinate of the target ship relative to the camera.

[0058] Preferably, in step 4, the camera is synchronized with the GPS space and the camera is synchronized with the GPS frequency, wherein in the camera synchronization with the GPS space, in order to make the GPS absolute coordinates and the camera relative coordinates in the same coordinate system, the WGS-84 coordinate system is converted into a plane rectangular coordinate system with the equator as the horizontal coordinate axis and the prime meridian as the vertical coordinate axis, and its intersection point as the origin, i.e. Web Mercator projection, the Web Mercator projection formula is as follows:

[0059]

[0060] where (mctX, mctY) is the Web Mercator coordinate, (lon, lat) is the longitude and latitude coordinate in the WGS-84 coordinate system, and 20037508.34 is half of the equatorial circumference;

[0061] After obtaining the Web Mercator coordinate of the target ship, the Web Mercator coordinate of the camera position converted in combination can obtain the relative coordinate of the target ship relative to the camera, and the formula is as follows:

[0062]

[0063] (X R ,Y R ) is the relative coordinate of the camera's original center converted from the GPS absolute coordinate, (mctX S ,mctY S ) and (mctX C ,mctY C ) are the Web Mercator coordinates of the target ship and the camera, respectively;

[0064] In the frequency synchronization between the camera and the GPS, the relative coordinates converted by the camera are systematically sampled based on the time of the GPS sensor with a lower frequency, so as to ensure the synchronization of the relative coordinates converted by the camera and the relative coordinates converted by the GPS.

[0065] Preferably, in step 5, data preprocessing and ship trajectory matching are included, wherein the data processing utilizes mean filtering to preprocess the calculated relative coordinates;

[0066] The ship trajectory matching utilizes the DTW algorithm based on the cosine distance to match the relative coordinates converted by the camera with the relative coordinates converted by the GPS, so as to realize the alignment of the data fusion in time.

[0067] The DTW algorithm is a full name of Dynamic Time Warping, which is a trajectory similarity measurement method. The similarity is measured through the matching points in the trajectory. The relative coordinate sequence converted by the camera is n (n = 1, …, N), and the relative coordinate sequence converted by the GPS is m (m = 1, …, M). The distance d[n,m] between the element n and the element m of the two sequences is calculated as follows:

[0068]

[0069]

[0070] In the formula, (x n-1 ,y n-1 ) and (x n ,y n ) are two adjacent elements in the sequence n, and (x m-1 ,y m-1 ) and (x m ,y m ) are two adjacent elements in the sequence m. In the calculation of the sequence distance, max is used instead of min to obtain the maximum cosine distance of the two sequences. The distance calculation formula of the two sequences is as follows:

[0071] DTW(n,m) = d(n,m) + max{DTW(n-1,m), DTW(n,m-1), DTW(n-1,m-1)} (22)

[0072] DTW(n,m) is the sum of the cosine distance between the current elements and the cumulative distance of the minimum adjacent elements that can reach this point, and DTW(N,M) is the distance between the two sequences. Sequence m is divided into M-N+1 sequences x of length N i The distance between sequence n and sequence x can be calculated using the DTW algorithm as shown in equation (23). The greater the cosine distance between the two sequences, the more similar the trajectories are. The relative coordinates calculated by vision and the relative coordinates converted by GPS are associated with the schematic diagram, which facilitates the fusion of the two in the next stage. i

[0073] x i = m[i,i+1,...,i+N-2,i+N-1], i = 1,2,...,M-N+1 (23).

[0074] Preferably, in step 6, the steps of the extended Kalman filter algorithm can be divided into two stages of prediction and update, and the specific steps are as follows:

[0075] (1) Calculate the prior state estimation at time k according to the state transition matrix F and the optimal estimation at time k-1

[0076]

[0077] where B is the control matrix, u is the control matrix of the system at time k-1,

[0078] (2) Calculate the prior estimation covariance at time k

[0079]

[0080] where P k-1 is the posterior estimation covariance at time k-1, Q is the variance matrix of the process noise,

[0081] (3) Calculate the Kalman filter gain K at time k k

[0082]

[0083] where H is the measurement matrix, R is the variance matrix of the measurement noise,

[0084] (4) Update the optimal state estimation at time k

[0085] ​

[0086] wherein z k is the measurement vector at time k,

[0087] (5) updating the posteriori estimation covariance P of time k k

[0088]

[0089] wherein I is the unit matrix,

[0090] The matched relative coordinates converted by the camera and the relative coordinates converted by the GPS are fused by the EKF to obtain the optimal estimation of the target ship position information.

[0091] The present application has the following advantages:

[0092] (1) The visual positioning method has the advantages of passive and high-precision measurement, compared with the active detection method such as radar, since the radar is active to emit radio waves, it is easy to expose the position and be locked or interfered by the opponent, and has the defect of unable to guarantee the detection concealment. The visual positioning based on the camera is passive to receive the light reflected from the target, and will not expose the position of the detection system, and shows its high use value in many fields.

[0093] (2) The present application calculates the relative coordinates of the target ship based on the identification and tracking results of the target ship and the camera imaging model, and fuses the visual positioning information of the target ship and the GPS information by the EKF algorithm. This method combines the advantages of the two sensors well, overcomes the problem of poor reliability of single sensor perception, and improves the stability of target detection and positioning. BRIEF DESCRIPTION OF DRAWINGS

[0094] Figure 1 is the schematic diagram for establishing four coordinate systems in the present application;

[0095] Figure 2 is the relative position relationship diagram of the image coordinate system and the pixel coordinate system in the present application;

[0096] Figure 3 is the schematic diagram of radial distortion and tangential distortion in the present application;

[0097] Figure 4 is the ranging model diagram in the present application;

[0098] Figure 5 is the camera and GPS time data fusion schematic diagram in the present application;

[0099] Figure 6 is the schematic diagram of the relative coordinate mean filtering in the present application;

[0100] Figure 7 The schematic diagram for associating different sensor trajectories in the present application;

[0101] Figure 8 The rendering diagram for EKF trajectory data fusion effect in the present application;

[0102] Figure 9 The overall flowchart of the present application. DETAILED DESCRIPTION

[0103] The specific embodiments of the present application will be described in detail below with reference to the accompanying drawings and preferred embodiments.

[0104] In the description of the present application, it should be understood that the terms "center", "longitudinal", "transverse", "upper", "lower", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer" and the like indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the device or element referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as a limitation of the present application. In addition, the terms "first", "second" and the like are only for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined with "first", "second" and the like can explicitly or implicitly include one or more of the features. In the description of the present application, unless otherwise specified, the meaning of "a plurality of" is two or more.

[0105] In the description of the present application, it should be noted that unless otherwise specified and limited, the terms "mounting", "connecting", "connecting", "connecting", "connecting", "connecting" should be understood broadly, for example, it can be fixedly connected, or it can be detachably connected, or integrally connected; it can be mechanically connected, or it can be electrically connected; it can be directly connected, or it can be indirectly connected through an intermediate medium, or it can be connected inside two elements. For those skilled in the art, the specific meaning of the above terms in the present application can be understood through specific circumstances.

[0106] The present application will be further described below with reference to the accompanying drawings.

[0107] The overall algorithm includes the following steps:

[0108] 1. Target ship identification and tracking based on YOLOv5 and Deep Sort algorithm

[0109] First, the trained YOLOv5 network model is used to detect the input video. The YOLOv5 algorithm analyzes an input frame of the input video, identifies the target information in the frame image, outputs the class (ID) of the target, and the normalized boundary box information (U, V, W, H). Then, a for loop is used to read each frame of the input video in turn, and the detection results are output in turn, which are used as the input of the Deep Sort algorithm.

[0110] The Deep Sort algorithm predicts the trajectory information of the previous frame through the Kalman filter algorithm to obtain uncertain trajectory information, which can also be called initialized trajectory information. The Mahalanobis distance between the predicted trajectory information of the previous frame and the detection box information input in the current frame and the cosine distance based on the appearance feature vector are obtained in a linear weighted manner to obtain joint matching information. Finally, the Hungarian algorithm is used for matching, and the cascade matching algorithm is used to update the tracking result. After the target ship is successfully tracked, the coordinate information of the center of each frame tracking target in the image is derived.

[0111] 2. Camera imaging model and coordinate conversion

[0112] The imaging model of the camera is a central projection linear model in an ideal state, and the target positioning is based on the pinhole imaging model. Therefore, it is important to establish the relationship between the pixel coordinate system and the world coordinate system, and the conversion between the two is carried out using the camera coordinate system and the image coordinate system. The relationship between the four coordinate systems is shown in Figure 1 .

[0113] (1) Four coordinate systems

[0114] 1) World coordinate system: Since the position of the camera in space is arbitrary, a reference coordinate system can be arbitrarily selected to represent the position of the camera and the position of any object in three-dimensional coordinates. This coordinate system is called the world coordinate system. That is, Figure 1 O W -X w Y w Z w space coordinate system.

[0115] 2) Camera coordinate system: The camera coordinate system is Figure 1 O C -X C Y C Z C space coordinate system. The camera coordinate system takes the optical center O C of the camera as the origin, the Z C axis along the optical axis of the camera, and the intersection point of the imaging plane in the image coordinate system can be represented as (0, 0). The X C axis and Y C axis of the camera coordinate system are parallel to the xi Axis, y i Axis parallel.

[0116] 3) Image coordinate system: Figure 1 The image coordinate system in is o i -x i y i The intersection of the imaging plane and the camera optical axis is the coordinate origin o i , the unit is the actual physical distance, the x of the image coordinate system i o i y i The uov plane of the plane and pixel coordinate system coincides with the camera plane.

[0117] 4) Pixel coordinate system: Figure 1 The pixel coordinate system in the o-uv two-dimensional plane, the origin of the pixel coordinate system is the first pixel in the upper left corner, and the u axis and v axis are parallel to the x axis respectively. i axis and y i Axis, the u-axis and v-axis in the image represent the number of rows and columns in the image array respectively.

[0118] (2) Coordinate transformation

[0119] Three transformations are required to convert from pixel coordinates to world coordinates.

[0120] 1) Convert from world coordinate system to camera coordinate system

[0121] From the point p in the world coordinate system w (X w ,Y w ,Z w ) is converted into point p in the camera coordinate system c (X c ,Y c ,Z c ), which is essentially the rotation and translation of the coordinate axis, can be represented by a 3×3 rotation matrix R and a 3×1 translation vector T.

[0122]

[0123] 2) Convert from camera coordinate system to image coordinate system

[0124] The point p in the camera coordinate system c (X c ,Y c ,Z c ) is projected onto the image plane to obtain point p in the image coordinate system i (x i ,y i ), the conversion method is as follows:

[0125]

[0126] Converting the above formula into matrix form, we can get:

[0127]

[0128] In the above formula, f is the focal length of the camera.

[0129] 3) Convert from image coordinate system to pixel coordinate system (such as Figure 2 shown)

[0130] The point p in the image coordinate system i (x i ,y i ) is converted to the pixel coordinate system point p(u p ,v p )

[0131]

[0132] Converting the above formula into matrix form, we can get:

[0133]

[0134] In the above formula, d x and d y For each pixel at x i Axis and y i Physical dimensions in the axial direction, (u i ,v i ) is the origin o in the image coordinate system i The pixel coordinates after transformation into the pixel coordinate system are the principal point coordinates of the image, which can also be written as (c x ,c y ).

[0135] Combining (1), (3), and (5), we can obtain the following matrix form of the world coordinate system converted into the pixel coordinate system:

[0136]

[0137] in, is the intrinsic parameter matrix of the camera, which can be obtained by calibrating the camera.

[0138] (3) Camera distortion model

[0139] The imaging model in the last section is a linear model assuming that the camera is in an ideal state without distortion. In fact, due to the errors in machining and assembly of the lens of the camera, the shape of the object in the real world will be curved when projected through the camera, resulting in a phenomenon that the theoretical imaging position does not coincide with the actual imaging position, which is called image distortion. Image distortion mainly includes perspective distortion, radial distortion and tangential distortion. Among them, radial distortion and tangential distortion have a greater impact, and other types of distortion have little effect on camera imaging, so they are ignored, Figure 3 For radial distortion d r and tangential distortion d t The schematic diagram of the influence of the imaging point distortion.

[0140] Radial distortion is mainly caused by the inherent characteristics of the convex lens of the camera lens itself. When the light is projected away from the center of the convex lens, it will produce greater bending. The actual performance is that the farther the distance from the center of the convex lens, the more serious the distortion, the greater the error between the theoretical imaging position and the actual imaging position, and the mathematical model of radial distortion can be expressed by a polynomial:

[0141]

[0142] In the above formula, (x t ,y t ) is the actual position of the distorted point on the imaging plane, (x, y) is the ideal position of the distorted point on the imaging plane, k1, k2, k3 are radial distortion coefficients, and generally only the first two terms k1, k2 are used. For lenses with large distortion, the third term can be added to the image for de-distortion.

[0143] Tangential distortion is mainly caused by the deviation of the convex lens during assembly, so that the sensor plane of the camera is not completely parallel to the sensor plane. The tangential distortion model is expressed as follows, and p1 and p2 are the tangential distortion coefficients of the camera:

[0144]

[0145] Combining formulas (7) and (8) can be obtained:

[0146]

[0147] 3. Measurement principle of relative coordinates

[0148] It is assumed that the origin of the world coordinate system coincides with the origin of the camera coordinate system, i.e. the origin is the optical center of the camera. The conversion from the camera coordinate system to the world coordinate system needs to rotate the camera coordinate system X c axis counterclockwise by an angle α = 90° (from the X c axis positive to negative, counterclockwise rotation), and the conversion relationship of the coordinate system is shown in formula (11) according to the definition of Euler angle:

[0149]

[0150]

[0151] The translation vector is shown as formula (12):

[0152]

[0153] Substitute formula (11) and formula (12) into formula (6), and obtain:

[0154]

[0155] From the above formula, Z c = Y w Substitute the equation into formula (13) to further obtain:

[0156]

[0157] In formula (14), only one variable u is obtained, and only the proportional relationship of X w and Y w can be obtained, and the numerical value cannot be further solved. Therefore, the distance formula of the target relative to the camera optical center is introduced to solve X w and Y w , and the distance formula and the mapping model are shown as formula (15) and formula (16): Figure 4

[0158]

[0159] Wherein, h = v t -v b is the height of the target in the pixel coordinate system, H w is the actual height, f is the focal length, and Z c is the distance of the target from the camera optical center.

[0160]

[0161] In the world coordinate system, the target ship is on the horizontal plane, and the mass center height is constant. By measurement, the target ship is in the world coordinate system Z w .

[0162] By combining formula (14), (15) and (16), the following formula can be obtained:

[0163]

[0164] (X w , Y w ​) is the relative coordinate of the target ship relative to the world coordinate system with the camera optical center as the origin. The YOLOv5 and Deep Sort algorithms are used to detect and track the ship, obtaining its pixel coordinates in the pixel coordinate system. According to the distortion model, the ideal position of the imaging point on the imaging plane is obtained, which is substituted into equation (17) to obtain the relative coordinates of the target ship relative to the camera.

[0165] 4. Camera and GPS spatial frequency synchronization

[0166] (1) Camera and GPS spatial synchronization

[0167] For the coordinate system of the target ship, different sensors use different coordinate systems. In order to realize the fusion between different types of sensors, the space of the two needs to be converted to the same coordinate system. When positioning the target ship using the camera, the first thing obtained is the pixel coordinate system. In order to facilitate the fusion with the GPS coordinate, the above-mentioned camera calibration and matrix transformation successfully convert it into the relative coordinate with the camera as the origin. The GPS uses the WGS-84 coordinate system, whose origin is located at the center of the Earth, the Z-axis points to the polar direction, the X-axis points to the intersection of the zero meridian and the equator, and the Y-axis is determined by the right-hand rule.

[0168] In order to make the GPS absolute coordinate and the camera relative coordinate in the same coordinate system, the WGS-84 coordinate system is converted into a plane rectangular coordinate system with the equator as the horizontal coordinate axis and the prime meridian as the vertical coordinate axis, whose intersection is the origin, that is, the Web Mercator projection. The Web Mercator projection formula is as follows:

[0169]

[0170] Where (mctX, mctY) is the Web Mercator coordinate, (lon, lat) is the longitude and latitude coordinate in the WGS-84 coordinate system, and 20037508.34 is half of the equatorial circumference.

[0171] After obtaining the Web Mercator coordinate of the target ship, the Web Mercator coordinate of the camera location conversion is combined to obtain the relative coordinate of the target ship relative to the camera, and the formula is as follows:

[0172]

[0173] (X R ,Y R ) is the relative coordinate of the camera original center converted from the GPS absolute coordinate, (mctX S ,mctY S ) and (mctX C ,mctY C) are the web mercator coordinates of the target ship and camera respectively.

[0174] (2) Camera and GPS frequency synchronization

[0175] When fusing the visual perception and GPS information, it is necessary to ensure that the data output by the camera and GPS are obtained at the same time, otherwise it will lead to a large error in the fusion result. Generally speaking, the time of each sensor is independent, and the sampling frequency is also different. For example, the GPS used by the target ship works at a frequency of 5HZ, that is, the interval of each data is 200ms. In order to facilitate the subsequent data time synchronization of the experiment, the working frequency of the camera is set to 25fps, that is, 25 frames of images are collected per second, and the interval of each frame of data is 40ms. Here we take the time of the GPS sensor with lower frequency as the reference, and sample the relative coordinates converted by the camera to ensure the synchronization of the relative coordinates converted by the camera and the relative coordinates converted by GPS. The time synchronization process is shown in Figure 5

[0176] 5. Ship trajectory matching based on DTW algorithm

[0177] (1) Data preprocessing

[0178] Due to camera jitter, size error of frame selection during manual annotation, and interference of horizontal shadow, there may be a small positional deviation between the target detection frame size and the target ship profile, which will lead to an error estimate of the image coordinates of the tracked target ship, and further reduce the calculation accuracy of the relative coordinates. We use mean filtering to preprocess the calculated relative coordinates to make the trajectory smoother, so as to reduce the influence of external factors on the calculation accuracy of the relative coordinates. The trajectory after mean filtering is shown in Figure 6

[0179] (2) Ship trajectory matching

[0180] The GPS data used in this paper comes from the tracking ship, and the image data is obtained by a fixed camera installed on the shore. They have their own independent time system, so for the same absolute position of the ship, there is a certain time difference between the corresponding time of the camera and GPS. In addition, there is a certain delay in the transmission of the data of the two to the shore-based processing system, and the time of the incoming data transmission is different. In this paper, we use the DTW algorithm based on cosine distance to match the relative coordinates converted by the camera and the relative coordinates converted by GPS, and then realize the time alignment of data fusion.

[0181] ​​The DTW algorithm, short for Dynamic Time Warping, is a trajectory similarity measurement method that measures similarity based on matching points in the trajectory. Let the relative coordinate sequence converted by the camera be n (n = 1, ..., N), and the relative coordinate sequence converted by GPS be m (m = 1, ..., M). The distance d[n, m] between sequence elements n and m is calculated as follows:

[0182]

[0183]

[0184] Where (x n-1 ,y n-1 )(x n ,y n ) are two adjacent elements in the sequence n, (x m-1 ,y m-1 )(x m ,y m ) are two adjacent elements in sequence m. Because this paper uses cosine distance to calculate the distance between elements, the larger the cosine distance, the higher the similarity, which is the opposite of the Euclidean distance method for determining similarity. In Euclidean distance, the smaller the distance, the higher the similarity. Therefore, in the calculation of sequence distance, max is used instead of min to obtain the maximum cosine distance between the two sequences. The distance calculation formula for the two sequences is as follows:

[0185] DTW(n,m)=d(n,m)+max{DTW(n-1,m),DTW(n,m-1),DTW(n-1,m-1)} (22)

[0186] DTW(n,m) is the sum of the cosine distance between the current element and the cumulative distance of the smallest neighboring element that can reach that point. DTW(N,M) is the distance between the two sequences. In the experiment, we divide the sequence m into M-N+1 sequences of length N x i , which can be expressed as formula (23), using the DTW algorithm to calculate the sequence n and sequence x in turn i The larger the cosine distance between two sequences, the more similar the trajectories are. Figure 7 In order to calculate the trajectory similarity results based on the DTW algorithm, a schematic diagram is drawn to associate the relative coordinates solved by vision with the relative coordinates converted by GPS, which facilitates the fusion of the two in the next stage.

[0187] x i =m[i,i+1,...,i+N-2,i+N-1],i=1,2,...,M-N+1 (23)

[0188] 6. Visual perception and GPS fusion positioning based on EKF algorithm

[0189] Extended Kalman Filter (EKF) is an extended form of traditional Kalman filter under nonlinear system model. The EKF algorithm performs Taylor expansion on the nonlinear function, and discards the second and higher order terms, and only retains the first order term of the expansion term, so as to realize the linearization of the nonlinear function. Finally, the state estimation value and variance estimation value of the system are calculated by the Kalman filter algorithm to filter the signal.

[0190] The steps of the extended Kalman filter algorithm can be divided into two stages of prediction and update, and the specific steps are as follows:

[0191] (1) Calculate the prior state estimation at time k according to the state transition matrix F and the optimal estimation at time k-1

[0192]

[0193] In the formula, B is the control matrix, and u is the control matrix of the system at time k-1.

[0194] (2) Calculate the prior estimation covariance at time k

[0195]

[0196] In the formula, P k-1 is the posterior estimation covariance at time k-1, and Q is the variance matrix of process noise.

[0197] (3) Calculate the Kalman filter gain K at time k k

[0198]

[0199] In the formula, H is the measurement matrix, and R is the variance matrix of measurement noise.

[0200] (4) Update the optimal state estimation at time k

[0201]

[0202] In the formula, z k is the measurement vector at time k.

[0203] (5) Update the posterior estimation covariance P at time k k

[0204]

[0205] In the formula, I is the unit matrix.

[0206] Extended Kalman filter is widely used in multi-sensor fusion of IMU, GPS and vision, and the fused data has better performance and accuracy than before. Here we use EKF to fuse the relative coordinates converted by the matched camera and the relative coordinates converted by GPS to get the optimal estimation of the target ship position information, and the result is shown in Figure 8 .

[0207] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, but not limited to them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement to part or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A visual and GPS based target ship fusion positioning method, characterized in that : comprising the following steps: Step 1: target ship identification and tracking based on YOLOv5 and Deep Sort algorithms; Step 2: camera imaging model and coordinate conversion; Step 3: constructing a measurement model of relative coordinates; Step 4: camera and GPS space frequency synchronization; Step 5: matching visual and GPS trajectories using the DTW algorithm; Step 6: visual perception and GPS fusion positioning based on the EKF algorithm; In step 1, the trained YOLOv5 network model is used to detect the input video, the YOLOv5 algorithm analyzes an input frame of the input video, identifies the target information in the frame image, outputs the class ID of the target, and the normalized bounding box information U, V, W, H, then the for loop reads each frame of the input video in turn, and outputs the detection results in turn, which are used as the input of the Deep Sort algorithm; the Deep Sort algorithm predicts the trajectory information of the previous frame using the Kalman filter algorithm to obtain uncertain trajectory information, calculates the Mahalanobis distance between the predicted trajectory information of the previous frame and the detection box information input in the current frame, and the cosine distance based on the appearance feature vector, uses linear weighting to obtain joint matching information, and finally uses the Hungarian algorithm for matching to update the tracking results; In step 2, the imaging model of the camera is a linear model of central projection in an ideal state, and target positioning is based on the pinhole imaging model, so it is important to establish the relationship between the pixel coordinate system and the world coordinate system, and the conversion between the two is carried out using the camera coordinate system and the image coordinate system; In step 2, four coordinate systems are included, namely the world coordinate system, the camera coordinate system, the image coordinate system and the pixel coordinate system; In step 2, three conversions are required from the pixel coordinate system to the world coordinate system, 1) conversion from the world coordinate system to the camera coordinate system From a point p in the world coordinate system w (X w ,Y w ,Z w ) to a point p c (X c ,Y c ,Z c ) in the camera coordinate system, the essence is the rotation and translation of the coordinate axis, which can be represented by a 3x3 rotation matrix R and a 3x1 translation vector T. 2) conversion from the camera coordinate system to the image coordinate system Projecting a point p in the camera coordinate system c (X c ,Y c ,Z c ) onto the image plane gives a point p i (x i ,y i ) in the image coordinate system, the conversion method is as follows: The above formula is converted into matrix form as follows: In the above formula, f is the focal length of the camera; 3) conversion from the image coordinate system to the pixel coordinate system The point p in the image coordinate system i (x i ,y i ) is converted to the pixel coordinate system point p(u p ,v p ) The above formula is converted into matrix form as follows: In the above formula, d x and d y are the physical dimensions of each pixel in the x i and y i directions, respectively, and (u i , v i ) is the pixel coordinate of the origin o i in the image coordinate system after the transformation to the pixel coordinate system, i.e., the principal point coordinate of the image, which can also be written as (c x , c y ). Combining (1), (3) and (5), the matrix form of the conversion from the world coordinate system to the pixel coordinate system is obtained as follows: wherein, is an intrinsic matrix of the camera, which can be obtained by calibrating the camera; In step 2, the distortion model generated by the camera includes perspective distortion, radial distortion and tangential distortion, the mathematical model of radial distortion can be expressed by a polynomial: In the above formula, (x t ,y t ) is the actual position of the distortion point on the imaging plane, (x, y) is the ideal position of the distortion point on the imaging plane, k1, k2, k3 are radial distortion coefficients, and generally only the first two terms k1 and k2 are used. For lenses with large distortion, the third term can be added to remove distortion from the image. Tangential distortion is mainly due to the deviation of the convex lens during assembly, so that the sensor plane of the camera is not completely parallel, and the tangential distortion model expression is as follows, p1 and p2 are the tangential distortion coefficients of the camera: Combining the above formulas (7) and (8), we get: In step 3, it is assumed that the origin of the world coordinate system coincides with the origin of the camera coordinate system, i.e., the origins are both the optical center of the camera. The conversion from the camera coordinate system to the world coordinate system needs to rotate around the X c axis counterclockwise by an angle a = 90°. According to the definition of Euler angles, the conversion relationship of the coordinate systems is shown in equation (11): The translation vector is shown in formula (12): Substitute formula (11) and formula (12) into formula (6) to get: From the above equation, we have Z c = Y w Substituting this equation into equation (13) gives us further Only one variable u in formula (14) can only get the ratio of X w and Y w , and cannot further solve the numerical value, and then introduce the distance formula of the target relative to the camera optical center to solve X w and Y w , the distance formula is as follows: where h = v t -v b H is the height of the target in the pixel coordinate system, H w f is the focal length, Z c Z is the distance of the target from the optical center of the camera, In the world coordinate system, the target ship is in the horizontal plane, the height of the center of mass is constant, and the target ship is measured in the world coordinate system Z w , Combine formulas (14), (15) and (16) to get: (X w ,Y w ) is the relative coordinate of the target ship relative to the world coordinate system with the camera optical center as the origin; the ship is detected and tracked using the YOLOv5 and Deep Sort algorithms to obtain its pixel coordinates in the pixel coordinate system, and the ideal position of the imaging point on the imaging surface is obtained according to the distortion model, which is substituted into formula (17) to obtain the relative coordinates of the target ship relative to the camera.

2. The vision and GPS based target ship fusion positioning method according to claim 1, characterized in that: In step 4, camera and GPS space synchronization and camera and GPS frequency synchronization are included. In camera and GPS space synchronization, in order to make GPS absolute coordinates and camera relative coordinates in the same coordinate system, the WGS-84 coordinate system is converted into a plane rectangular coordinate system with the equator as the horizontal coordinate axis and the prime meridian as the vertical coordinate axis, and the intersection point as the origin, that is, the Web Mercator projection. The Web Mercator projection formula is as follows: Where (mctX, mctY) is the Web Mercator coordinate, (lon, lat) is the longitude and latitude coordinate in the WGS-84 coordinate system, and 20037508.34 is half of the equatorial circumference. After obtaining the Web Mercator coordinate of the target ship, the Web Mercator coordinate converted from the GPS position of the camera can be obtained to obtain the relative coordinate of the target ship relative to the camera, and the formula is as follows: (X R ,Y R ) are relative coordinates of the camera's original center transformed from GPS absolute coordinates, (mctX S ,mctY S ) and (mctX C ,mctY C ) are Web Mercator coordinates of the target ship and the camera, respectively. In camera and GPS frequency synchronization, the relative coordinates converted by the camera are system sampled based on the time of the GPS sensor with lower frequency to ensure the synchronization of the relative coordinates converted by the camera and the relative coordinates converted by the GPS. 3.The visual and GPS based target ship fusion positioning method according to claim 1, characterized in that: In step 5, data preprocessing and ship trajectory matching are included. In data processing, the relative coordinates obtained by solving are preprocessed by mean filtering; Ship trajectory matching uses the DTW algorithm based on cosine distance to match the relative coordinates converted by the camera and the relative coordinates converted by the GPS, and then realizes the alignment of data fusion in time The DTW algorithm is a trajectory similarity measurement method, which measures similarity through matching points in the trajectory. Let the relative coordinate sequence converted by the camera be n (n = 1, …, N), and the relative coordinate sequence converted by the GPS be m (m = 1, …, M). The distance d[n, m] between elements n and m is calculated as follows: Where (x n-1 ,y n-1 )(x n ,y n ) are two adjacent elements in the sequence n, (x m-1 ,y m-1 )(x m ,y m ) are two adjacent elements in sequence m. In the calculation of sequence distance, max is used instead of min to obtain the maximum cosine distance between the two sequences. The distance calculation formula between the two sequences is as follows: DTW(n, m) = d(n, m) + max{DTW(n-1, m), DTW(n, m-1), DTW(n-1, m-1)}(22) DTW(n,m) is the sum of the cosine distance between the current elements and the cumulative distance of the minimum adjacent elements that can reach the point, and DTW(N,M) is the distance between the two sequences, sequence m is divided into M-N+1 sequences x of length N i The distance between sequence n and sequence x can be represented by formula (23) and calculated in sequence by using the DTW algorithm i The greater the cosine distance between the two sequences, the more similar the trajectories are. The relative coordinates calculated by vision and the relative coordinates converted by GPS are associated with the schematic diagram, and then the fusion of the two in the next stage is facilitated. x i = m[i,i+1,...,i+N-2,i+N-1], i = 1,2,...,M-N+1 (23).

4. The target ship fusion positioning method based on vision and GPS according to claim 1 is characterized in that: In step 6, the steps of the extended Kalman filter algorithm can be divided into prediction and update stages, and the specific steps are as follows: (1) Calculate the prior state estimation at time k according to the state transition matrix F and the optimal estimation at time k-1 Where B is the control matrix, u is the control matrix of the system at time k-1, (2) Calculate the prior estimation covariance at time k where P is the covariance of the posterior estimate at time k - 1, Q is the variance matrix of the process noise, and R is the variance of the measurement noise. k-1 where P is the covariance of the posterior estimate at time k - 1, Q is the variance matrix of the process noise, and R is the variance of the measurement noise (3) Calculate the Kalman filter gain K at time k k Where H is the measurement matrix, R is the variance matrix of the measurement noise, (4) update the optimal state estimate at time k where z k is the measurement vector at time k, (5) update the posteriori estimation covariance P at time k k Where I is the unit matrix, The matched relative coordinates converted by the camera and the relative coordinates converted by the GPS are fused by EKF to obtain the optimal estimation of the target ship position information.

Images