CN111413698A - A target localization method for underwater robot search and exploration - Google Patents

Abstract

The invention discloses a target positioning method for underwater robot search and exploration, comprising the following steps: extracting and identifying A-KAZE feature points of underwater target sonar images; measuring the two-dimensional orientation of the underwater target relative to the underwater robot ; Measure the elevation angle of the underwater robot; measure the three-dimensional orientation of the underwater target relative to the underwater robot; correct the elevation angle of the underwater robot, and correct the three-dimensional orientation of the underwater target point relative to the underwater robot. The present invention is oriented to the target positioning method of underwater robot search and exploration. Based on forward-looking sonar data, the deep convolutional neural network is used to automatically extract and identify the feature points of the underwater target, and combined with the posture of the underwater robot, the underwater target is realized. Precise positioning is convenient for searchers to finely detect the position of underwater targets, and realizes reliable, efficient and intelligent underwater search and exploration operations. The invention is used in the technical field of underwater target search and exploration.

Description

A target localization method for underwater robot search and exploration

technical field

The invention relates to the technical field of underwater target search and exploration, in particular to a target positioning method for underwater robot search and exploration.

Background technique

When conducting scientific research on the ocean, underwater robots are the most important research tools, which are used to replace human beings for long-term underwater operations or work in harsh underwater environments. In complex underwater environments, the most reliable and effective detection method is underwater acoustic detection, and it is also the most widely used underwater detection method for underwater robots. Comprehensively use modern sonar detection technology to conduct underwater search and exploration in the sea area of the accident, and obtain the key feature points of the underwater search target. At the same time, combine the target feature points and the attitude information of the exploration robot to achieve accurate positioning of the underwater target.

The precision and accuracy of the underwater target position obtained by the existing underwater target search and detection methods are not high. The study of the underwater target search and detection method is the focus of scientific research now and for a long time in the future.

SUMMARY OF THE INVENTION

The purpose of the present invention is to solve at least one of the technical problems existing in the prior art, and to provide a target positioning method for underwater robot search and exploration, which can realize accurate positioning of underwater targets.

According to an embodiment of the present invention, there is provided a target positioning method for underwater robot search and exploration, comprising the following steps:

S1. Extract the A-KAZE feature points of the underwater target through the sonar image collected by the forward-looking sonar of the underwater robot;

S2. Input the sonar image with A-KAZE feature into the convolutional neural network method to identify the A-KAZE feature point of the target in the sonar image;

S3. Use the geometric relationship between the target feature point and the front sonar to measure the two-dimensional orientation of the underwater target relative to the underwater robot;

S4. Combine the two-dimensional orientation of the feature point of the underwater target and the attitude of the underwater robot to measure the elevation angle θ of the underwater robot, and measure the three-dimensional orientation of the underwater target relative to the underwater robot through the obtained elevation angle θ;

S5. Use the feature points with insufficient or sufficient constraints to correct the elevation angle θ of the underwater robot, and correct the three-dimensional orientation of the underwater target point relative to the underwater robot.

According to the target positioning method for underwater robot search and exploration according to the embodiment of the present invention, the extraction of the A-KAZE feature points of the underwater target in the step S1 includes the following steps:

S101. Define a set of evolution times to construct a nonlinear scale space;

S102. Convert discrete sets in pixel units into time units;

S103. Given an input image and a contrast factor, use a fast explicit diffusion method;

S104. Embed the fast explicit diffusion method into the coarse-to-fine pyramid method;

S105. Calculate the Hessian determinant for each sonar image;

S106. Calculate the second derivative using a cascaded Schall filter.

According to the target positioning method for underwater robot search and exploration according to the embodiment of the present invention, the specific implementation of step S2 includes the following sub-steps:

S201. Train a convolutional neural network on the sonar image dataset using the GoogLeNet architecture.

According to the target positioning method for underwater robot search and exploration according to the embodiment of the present invention, the GoogLeNet architecture includes five layers, the first layer and the second layer are convolution layers and maximum pooling layers, and the third layer is inception layer, the fourth layer is the feature layer, which is a fully connected layer, the fourth layer maps the previous output to a Dim × 1 vector, the fifth layer is a fully connected layer, and the fifth layer maps the previous feature layer to a 3 × 1 vector and compare the feature layer mapped as a 3×1 vector with the location labels using Euclidean loss.

According to the target positioning method for underwater robot search and exploration according to the embodiment of the present invention, the specific implementation of step S3 includes the following sub-steps:

S301. The local Cartesian sonar coordinate system and the spherical parameter coordinate system are transformed into each other.

According to the target positioning method for underwater robot search and exploration according to the embodiment of the present invention, the specific implementation of step S4 includes the following sub-steps:

S401. Formulate the underwater target feature points and the attitude of the underwater robot into nonlinear least square factor graph optimization. For each attitude X _t , it contains the following 6 parameters (x, y, z, yaw, pitch, roll) , for each feature point, contains the following 3 parameters (x, y, z);

S402. Solve the factor graph as nonlinear least squares optimization;

S403. Convert the feature point l _j =(x, y, z) into a sonar frame, and obtain the azimuth and distance of the local coordinates (x _s , y _s , z _s );

S404. Use the monotonicity of the logarithmic function to find the initial estimate of the feature point through the back projection of the sonar measurement value;

S405. Set the unknown elevation angle θ to 0, and then use the underwater robot attitude X _t to convert the point from the sonar Cartesian coordinates (x _s , y _s , z _s ) to the world Cartesian coordinates (x, y, z), which are used as Initially guess the three-dimensional orientation of the feature point;

S406. Convert the predicted three-dimensional feature point position to the sonar coordinate system of the attitude X _t .

According to the target positioning method for underwater robot search and exploration according to the embodiment of the present invention, the specific implementation of step S5 includes the following sub-steps:

S501. Observe the elevation angle of the target feature point through different attitudes;

S502. Classify the observed feature points as under-constrained or sufficiently-constrained elements;

S503. In order to determine whether the point feature points are sufficiently constrained, use three-degree-of-freedom spherical parameterization;

S504. Take the initial estimation of the feature point l ⁰ as the linearization point, and use the Taylor series expansion of the measurement function;

S505. Reduce optimization to linear least squares problem;

S506. Determine whether the optimization is constrained by measurement;

S507. Completely delete feature points with insufficient constraints from the state vector;

S508. Only completely remove the elevation angles of the under-constrained feature points from the state vector, and then model the under-constrained feature points as two-dimensional azimuth-distance points in the factor graph.

Beneficial effects: This target positioning method for underwater robot search and exploration, based on forward-looking sonar data, uses deep convolutional neural network to automatically extract and recognize underwater target feature points, and combines the posture of the underwater robot to realize the underwater robot posture. The precise positioning of the target facilitates the search personnel to perform precise detection of the position of the underwater target, and realizes the reliability, efficiency and intelligence of the underwater search and exploration operation. The invention is used in the technical field of underwater target search and exploration.

Description of drawings

The present invention will be further described below in conjunction with the accompanying drawings:

Fig. 1 is the step block diagram of the embodiment of the present invention;

Fig. 2 is the geometrical relationship diagram of the target feature point of the embodiment of the present invention and the front sonar;

Fig. 3 is the factor graph model of the embodiment of the present invention;

4 is a three-dimensional position map of an underwater target according to an embodiment of the present invention;

5 is a schematic diagram of an underwater robot rotating around the z-axis according to an embodiment of the present invention;

6 is a factor graph correction model for deleting feature points with insufficient constraints from a state vector according to an embodiment of the present invention;

FIG. 7 is a factor graph correction model for only deleting the elevation angles of feature points with insufficient constraints from the state vector according to an embodiment of the present invention.

Detailed ways

This part will describe the specific embodiments of the present invention in detail, and the preferred embodiments of the present invention are shown in the accompanying drawings. Each technical feature and overall technical solution of the invention should not be construed as limiting the protection scope of the invention.

In the description of the present invention, it should be understood that the azimuth description, such as the azimuth or position relationship indicated by up, down, front, rear, left, right, etc., is based on the azimuth or position relationship shown in the drawings, only In order to facilitate the description of the present invention and simplify the description, it is not indicated or implied that the indicated device or element must have a particular orientation, be constructed and operated in a particular orientation, and therefore should not be construed as limiting the present invention.

In the description of the present invention, the meaning of several is one or more, the meaning of multiple is two or more, greater than, less than, exceeding, etc. are understood as not including this number, above, below, within, etc. are understood as including this number. If it is described that the first and the second are only for the purpose of distinguishing technical features, it cannot be understood as indicating or implying relative importance, or indicating the number of the indicated technical features or the order of the indicated technical features. relation.

In the description of the present invention, unless otherwise clearly defined, words such as setting, installation, connection should be understood in a broad sense, and those skilled in the art can reasonably determine the specific meanings of the above words in the present invention in combination with the specific content of the technical solution.

Referring to FIG. 1, an embodiment of the present invention provides a target positioning method for underwater robot search and exploration, including the following steps:

S1. Extract the A-KAZE feature points of the underwater target through the sonar image collected by the forward-looking sonar of the underwater robot, and the specific implementation of extracting the A-KAZE feature points of the underwater target includes the following sub-steps:

S101. Define a set of evolution times to construct a nonlinear scale space,

σ _i (o,s)=2 ^o+s/S , o∈[0…O-1], s∈[0…S-1], i∈[0…M],

where O is the set of images blurred by different Gaussian kernels, S is the discretized layer, σ is the pixel M, and is the total number of sonar images.

S102. Convert the discrete set in the pixel unit σ _i to a time unit,

S103: Given an input image and a contrast factor, use the fast explicit diffusion method, use M-1 outer fast explicit diffusion cycles, and calculate the minimum number of inner steps n for each cycle.

S104. In order to speed up the computation of the nonlinear scale space, the fast explicit diffusion method is embedded in the coarse-to-fine pyramid method.

对图像进行2倍降频采样，并将该降采样后的图像用作下一个集合中下一个快速显式扩散周期的起始图像。The fast explicit diffusion method is embedded in the coarse-to-fine pyramid decomposition. To reach steady state as quickly as possible, cascading fast explicit diffusion will resolve propagation from coarse to fine levels. Use smoothing matrix

The image is downsampled by a factor of 2, and this downsampled image is used as the starting image for the next fast explicit diffusion cycle in the next set.

^S105 . Calculate the Hessian determinant for each sonar image Li,

A normalized pixel factor that takes into account the set of each particular image in the nonlinear scale space is used, ie σ _i,norm =σi/2.

S106. To calculate the second derivative, use a cascaded Schall filter with a step size of σ _i,norm .

At each evolution level i, it is checked whether the detector response is above a predetermined threshold and it is the maximum value in a 3x3 pixel window. Then for each potential maxima, in a window of size σ _i ×σ _i pixels, respectively, check whether the response is directly above and below the maxima relative to other keypoints in levels i+1 and i-1, respectively. Finally, by fitting a 2D quadratic function to the determinants of the Hessian response in a 3 × 3 pixel neighborhood and finding its maximum, the 2D location of keypoints can be estimated with subpixel accuracy.

S2. Input the sonar image with the A-KAZE feature into the convolutional neural network method to identify the A-KAZE feature point of the target in the sonar image. The specific implementation includes the following sub-steps:

S201. Use the GoogLeNet architecture to train a convolutional neural network (CNN) on the sonar image dataset;

The original GoogLeNet architecture is divided into five layers, the first and second layers are convolutional layers and maximum pooling layers, and the third, fourth, and fifth layers are inception layers.

Two improvements were made to adapt the original network to the present invention:

(1) The penultimate layer (i.e., the fourth layer) is a fully connected layer that maps the previous output to a Dim × 1 vector, called a feature layer.

(2) The last layer (i.e., the fifth layer) is a fully connected layer that maps the previous feature layer into a 3 × 1 vector and compares it with the location labels using Euclidean loss.

S3. Use the geometric relationship between the target feature point and the front sonar to measure the two-dimensional orientation of the underwater target relative to the underwater robot. The specific implementation includes the following sub-steps:

S301. Point C=[xyz] ^T parameterized in the local Cartesian sonar coordinate system. The point can also be represented as Q using a spherical parameterization, and the conversion between the two is,

是方位角，r是距离，θ是仰角。in

is the azimuth, r is the distance, and θ is the elevation.

计算为准确度<1°以内。同时这些测量没有提供关于仰角θ的任何信息。r is determined by the time of launch and the speed of sound in the water. The transceiver array allows the azimuth of the received reflections to be

Calculated to be accurate within <1°. At the same time these measurements do not provide any information about the elevation angle θ.

Detected sonar reflections reflected from curved patches lying on the same elevation arc will be projected to the same pixel in the final imaged sonar image, as shown in Figure 2.

Compiling all measurements within the sensor's field of view results in a grayscale polar image, where the columns correspond to the discrete azimuth space and the rows correspond to the discrete range space.

表示从像素空间到方位范围空间的转换。像素的强度对应于在指定的方位角和范围内从仰角反射的声音的强度。For unit pixel σ, let

Represents a transformation from pixel space to azimuth extent space. The intensity of the pixel corresponds to the intensity of the sound reflected from the elevation angle within the specified azimuth and range.

S4. Combine the two-dimensional orientation of the feature point of the underwater target and the attitude of the underwater robot to measure the elevation angle θ of the underwater robot, and calculate the three-dimensional orientation of the underwater target relative to the underwater robot through the obtained elevation angle θ. The specific implementation includes the following sub-steps :

S401. Formulate the underwater target feature points and the attitude of the underwater robot as nonlinear least square factor graph optimization. For each attitude X _t , there are the following 6 parameters (x, y, z, yaw, pitch, roll ), for each feature point, there are the following 3 parameters (x, y, z).

A factor graph is a bipartite graph in which the variable nodes of the unknown variable to be optimized are connected to the factor nodes of the measured values, as shown in Figure 3.

测量值m_k添加到图形中，从而将特征点l_j连接到观察其的姿态。使用“基本姿态X_b”(观察到的特征点的第一个姿态)的框架的球面坐标，首先假设0°仰角生成特征点三维位置的初始估计。At each time t, the attitude _Xt is added to the factor graph as a new node along with the odometer measurement position value _ut-1 , which provides an estimate of the motion between _Xt-1 and _Xt . The bearing and distance of the jth feature point

The measurements m _k are added to the graph, connecting the feature points l _j to the pose from which it was observed. Using the spherical coordinates of the frame of the "base pose X _b " (the first pose of the observed feature point), an initial estimate of the three-dimensional position of the feature point is generated first assuming a 0° elevation angle.

S402. Solve the factor graph as nonlinear least squares optimization,

Wherein the state vector X=[X ₀ , X ₁ ,...,L ₀ ,L ₁ ,...] ^T contains all unknown variables: pose and feature points.

The _i -th factor specifies the prediction function hi (X), the measured value Z _i ={u _i , m _k } and the measurement uncertainty Σi.

的情况下，预测函数：Measure the bearing and distance of the feature point j from the attitude X _t

In the case of , the prediction function:

h _i (X)=π(X _t ,l _j ).

和距离r，由以下公式获得； _S403.hi (X) firstly convert the feature point l _j = (x, y, z) into a sonar frame according to the attitude X _t , and obtain the azimuth angle of the local coordinates (x _s , y _s , z _s )

and distance r, obtained by the following formula;

测量值，S404. Using the monotonicity of the logarithmic function, find the initial estimates of the feature points by back-projecting the sonar measurement values. Use the first observation for each feature, including distance r and azimuth

Measurements,

S405. Set the unknown elevation angle θ to 0, and then use the underwater robot attitude X _t to convert the point from the sonar Cartesian coordinates (x _s , y _s , z _s ) to the world Cartesian coordinates (x, y, z), which are used as Initial guess of the 3D orientation of the feature points.

S406. Convert the predicted three-dimensional feature point position to the sonar coordinate system of the attitude X _t , the corresponding back-projection function π ^-1 (X _b , m _b , θ) according to the basic attitude X _b , the corresponding azimuth and distance measurement value m _b and the provided elevation to calculate the target three-dimensional feature point position angle θ (underwater robot elevation angle θ), as shown in Figure 4;

S5. Use the feature points with insufficient or sufficient constraints to correct the elevation angle θ of the underwater robot, and correct the three-dimensional orientation of the underwater target point relative to the underwater robot. The specific implementation includes the following sub-steps:

S501. Observe the elevation angle of the target feature point through different attitudes.

Control the underwater robot to perform pure yaw rotation around the z-axis, the rotation angle is yaw, and the azimuth angle of the reflection received by the forward-looking sonar

Feature point parameter (x, y, z) parameter conversion:

As shown in Figure 5, the elevation arcs have minimal overlap when the poses are separated by pure yaw rotation.

This feature point is measured from multiple poses, and the elevation angle of this point needs to be corrected through the following steps.

S502. Classify the observed feature points as under-constrained or sufficiently-constrained features. see if the measurement is sufficient to constrain its elevation angle, and if so, add it to the factor map as a well-constrained feature point using standard parameterization;

S503. In order to determine whether the point feature points are sufficiently constrained, use the three-degree-of-freedom spherical parameterization, where the state consists only of the feature points lj,

Since the poses of the sensors are not state variables, they are treated as constants, and the prediction function h _i (l _j ) uses the latest estimates available from the population factor graph state estimates.

S504. Take the initial estimation of the feature point l ⁰ as the linearization point, use the Taylor series expansion of the measurement function,

S505. Reduce optimization to linear least squares problem,

in:

where A and b:

A _i =∑i ^-1/2 H _i ,

被视为在零仰角处反投影的第一个方位和距离测量值。Linearization point

The first azimuth and distance measurement taken as a backprojection at zero elevation.

S506. Determine whether the optimization is constrained by measurement, wherein checking A ^T A is the key to determining whether the optimization is constrained by measurement.

才能被认为具有充足的约束力，其中ρ是用户定义的可调阈值。如果不符合标准，则将特征点分类为约束不足。If the elevation angle is completely unconstrained, the rank of the 3×3 matrix A ^T A will be insufficient. As the elevation angle becomes more and more constrained, the magnitude of the smallest eigenvalue λ ₃ of A ^T A will be relative to the first two eigenvalues λ ₁ and _λ2 increases. Therefore, the feature points must satisfy the criteria

can be considered sufficiently binding, where ρ is a user-defined adjustable threshold. If the criteria are not met, the feature point is classified as insufficiently constrained.

S507. Remove under-constrained feature points from the state vector so that their positions are not explicitly modeled in the optimization. As shown in Figure 6, the measurements corresponding to feature points _lj are collected into a nonparametric factor _fj . This factor treats the first azimuth distance measurement m _b of the feature point from its base pose X _b as a constant, thereby determining the two spherical coordinates of the feature point;

In each iteration of the optimization, this factor searches the range of feasible elevation angles by sampling the elevation angles in uniform increments, and selects the elevation angle with the lowest total reprojection error as the current predicted value:

where Θ={θ _min ,θ _min +Δθ,…,θ _max -Δθ,θ _max },

Using the measurement uncertainty ∑k, the reprojection error is calculated as a function of the distance between the projection of the feature point to the pose _xk and the measurement _mk .

The cost function for this factor is then the total reprojection error evaluated at the optimal elevation angle:

S508. Only the elevation angles of the under-constrained feature points are removed from the state vector, and then the under-constrained feature points are modeled as two-dimensional azimuth-distance points in the factor graph.

As shown in Figure 7, all measurements of the under-constrained feature points l _j are combined into a single joint measurement factor s _j . This joint measure factor is similar to the nonparametric factor _fj , except that it uses azimuth and distance estimates of feature points instead of measurements of the base pose when computing the reprojection error.

It should be understood that the parts not described in detail in this specification belong to the prior art.

The embodiments of the present invention have been described in detail above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned embodiments. Within the scope of knowledge possessed by those of ordinary skill in the technical field, various modifications can be made without departing from the purpose of the present invention. kind of change.

Claims (7)

1. a target positioning method for underwater robot search and exploration, is characterized in that, comprises the following steps: S1. Extract the A-KAZE feature points of the underwater target through the sonar image collected by the forward-looking sonar of the underwater robot; S2. Input the sonar image with A-KAZE feature into the convolutional neural network method to identify the A-KAZE feature point of the target in the sonar image; S3. Use the geometric relationship between the target feature point and the front sonar to measure the two-dimensional orientation of the underwater target relative to the underwater robot; S4. Combine the two-dimensional orientation of the feature point of the underwater target and the attitude of the underwater robot to measure the elevation angle θ of the underwater robot, and measure the three-dimensional orientation of the underwater target relative to the underwater robot through the obtained elevation angle θ; S5. Use the feature points with insufficient or sufficient constraints to correct the elevation angle θ of the underwater robot, and correct the three-dimensional orientation of the underwater target point relative to the underwater robot. 2. the target location method that faces underwater robot search and probe according to claim 1, is characterized in that, in described step S1, extracts the A-KAZE feature point of underwater target comprises the following steps: S101. Define a set of evolution times to construct a nonlinear scale space; S102. Convert discrete sets in pixel units into time units; S103. Given an input image and a contrast factor, use a fast explicit diffusion method; S104. Embed the fast explicit diffusion method into the coarse-to-fine pyramid method; S105. Calculate the Hessian determinant for each sonar image; S106. Calculate the second derivative using a cascaded Schall filter. 3. the target positioning method facing underwater robot search and probe according to claim 1, is characterized in that, the concrete realization of described step S2 comprises following sub-steps: S201. Train a convolutional neural network on the sonar image dataset using the GoogLeNet architecture. 4. the target positioning method for underwater robot search according to claim 3, is characterized in that: described GoogLeNet architecture comprises five layers, and the first layer and the second layer are convolution layer and maximum pooling layer, The third layer is the inception layer, the fourth layer is the feature layer, which is a fully connected layer, the fourth layer maps the previous output to a Dim × 1 vector, the fifth layer is a fully connected layer, and the fifth layer maps the previous feature The layers are mapped as 3×1 vectors, and the feature layers mapped as 3×1 vectors are compared with location labels using Euclidean loss. 5. the target positioning method facing underwater robot search and probe according to claim 1, is characterized in that, the concrete realization of described step S3 comprises following sub-steps: S301. The local Cartesian sonar coordinate system and the spherical parameter coordinate system are transformed into each other. 6. The target positioning method for underwater robot search and exploration according to claim 1, is characterized in that, the concrete realization of described step S4 comprises following sub-steps: S401. Formulate the underwater target feature points and the attitude of the underwater robot into nonlinear least square factor graph optimization. For each attitude X _t , it contains the following 6 parameters (x, y, z, yaw, pitch, roll) , for each feature point, contains the following 3 parameters (x, y, z); S402. Solve the factor graph as nonlinear least squares optimization; S403. Convert the feature point l _j =(x, y, z) into a sonar frame, and obtain the azimuth and distance of the local coordinates (x _s , y _s , z _s ); S404. Use the monotonicity of the logarithmic function to find the initial estimate of the feature point through the back projection of the sonar measurement value; S405. Set the unknown elevation angle θ to 0, and then use the underwater robot attitude X _t to convert the point from the sonar Cartesian coordinates (x _s , y _s , z _s ) to the world Cartesian coordinates (x, y, z), which are used as Initially guess the three-dimensional orientation of the feature point; S406. Convert the predicted three-dimensional feature point position to the sonar coordinate system of the attitude X _t . 7. The target positioning method for underwater robot search according to claim 1, wherein the concrete realization of the step S5 comprises the following sub-steps: S501. Observe the elevation angle of the target feature point through different attitudes; S502. Classify the observed feature points as under-constrained or sufficiently-constrained elements; S503. In order to determine whether the point feature points are sufficiently constrained, use three-degree-of-freedom spherical parameterization; S504. Take the initial estimation of the feature point l ⁰ as the linearization point, and use the Taylor series expansion of the measurement function; S505. Reduce optimization to linear least squares problem; S506. Determine whether the optimization is constrained by measurement; S507. Completely delete feature points with insufficient constraints from the state vector; S508. Only completely remove the elevation angles of the under-constrained feature points from the state vector, and then model the under-constrained feature points as two-dimensional azimuth-distance points in the factor graph.

Images