KR102469164B1 - Apparatus and method for geophysical navigation of USV(Unmanned Surface Vehicles) - Google Patents

Abstract

An apparatus and method for geophysical navigation of an unmanned surface ship are disclosed. The geophysical navigation method includes generating an underwater terrain map using sensing information obtained by a sonar sensor installed on an unmanned surface ship, and geomagnetic field of the underwater topography using sensing information obtained by a magnetometer installed on the unmanned surface ship. Generating a map and performing geophysical navigation using the generated terrain map and geomagnetic field map, wherein the performing of the geophysical navigation includes updating a pose of the unmanned surface vehicle , updating weights of particles defined as a set of pose hypotheses, and performing resampling on particles and weights.

Description

Apparatus and method for geophysical navigation of USV (Unmanned Surface Vehicles)}

The present invention relates to an apparatus and method for geophysical navigation of an unmanned surface ship.

GNSS (Global Navigation Satellite System) is the most common solution used by surface ships to acquire location and direction information in real time. However, it is necessary to prepare for situations in which GNSS cannot be used. For example, during a military operation, there is a high possibility of GNSS jamming or spoofing, or a malfunction of a receiver antenna due to a physical attack may occur. Therefore, an additional navigation source is required as a backup for the existing GNSS, and approaches for this are as follows.

The first approach is to use the existing maritime wireless infrastructure as a navigation beacon. Unlike GNSS signals, which are weak and vulnerable, maritime radio signals are strong and resistant to interference. Examples of such technologies include enhanced long-range navigation (eLoran) and ranging mode (R-Mode). However, these systems use low-frequency radio ranges and, depending on the environment, can result in very coarse positioning (accuracies of greater than 80m for eLoran and greater than 50m for R-MODE). In addition, such a system requires that the surface ship be equipped with an additional maritime radio signal receiver.

A second approach is to perform dead-reckoning (DR) via onboard proprioceptive sensors such as inertial measurement units (IMUs) and Doppler velocity logs (DVLs). will be. The inertial measuring device can measure linear acceleration and angular velocity in the fixed frame of the main body, and the Doppler velocity logarithm can measure linear velocity. These signals can be integrated to obtain the three-dimensional (3D) position and orientation of the waterline. However, dead reckoning has the problem of accumulating errors due to sensor noise and bias. Moreover, the measurement accuracy of the Doppler velocity log is severely degraded on sloping or irregular underwater bottoms.

A third approach is to use a map of the environment surrounding the surface ship. All types of environmental properties that help locate a surface craft can be utilized and encoded into a map. If the map is initially known or provided, localization or localization becomes an issue. If the map is unknown, localization and mapping issues must be addressed simultaneously.

Therefore, in consideration of this approach, there is a need for a navigation technology for Unmanned Surface Vehicles (USVs) that can be used even in situations where GNSS cannot be used.

Republic of Korea Patent Registration No. 10-1908534 (2018.10.10)

The present invention generates an underwater terrain map and a geomagnetic field map using information measured through a geophysical sensor of Unmanned Surface Vehicles (USV), and generates an underwater terrain profile obtained through a geophysical sensor. It is to provide a geophysical navigation device and method for an unmanned surface ship that calculates navigation information by matching with a map.

According to one aspect of the present invention, a geophysical navigation method performed by a geophysical navigation device of Unmanned Surface Vehicles (USV) is disclosed.

The geophysical navigation method according to an embodiment of the present invention includes generating an underwater terrain map using sensing information obtained by a sonar sensor installed on the unmanned surface ship, and sensing information obtained by a magnetometer installed on the unmanned surface ship. Generating a geomagnetic field map for an underwater topography by using a geomagnetic field map and performing geophysical navigation using the generated geomagnetic field map and the geomagnetic field map, wherein the performing geophysical navigation includes the unmanned Updating the pose of the waterline, updating the weights of particles defined as a set of pose hypotheses, and performing resampling on the particles and the weights. do.

According to another aspect of the present invention, a geophysical navigation device for Unmanned Surface Vehicles (USV) is disclosed.

A geophysical navigation device according to an embodiment of the present invention includes a memory for storing commands and a processor for executing the commands, wherein the commands use sensing information obtained by a sonar sensor installed in the unmanned surface ship Generating a geomagnetic field map of the underwater terrain using sensing information obtained by a magnetometer installed on the unmanned surface ship, and geophysical navigation using the generated geomagnetic field map and the geomagnetic field map Including the step of performing, wherein the step of performing the geophysical navigation includes updating the pose of the unmanned surface vehicle, updating the weight of a particle defined by a set of pose hypotheses and performing resampling on the particles and the weights.

Geophysical navigation device and method for unmanned surface vehicles according to embodiments of the present invention generate underwater terrain maps and geomagnetic field maps using information measured by geophysical sensors of unmanned surface vehicles (USVs) And, navigation information can be calculated by matching the underwater terrain profile obtained through the geophysical sensor to the created map.

1 is a flowchart illustrating a geophysical navigation method performed by a geophysical navigation device of Unmanned Surface Vehicles (USV) according to an embodiment of the present invention.
2 is a diagram showing the measurement concept of a geophysical sensor installed on an unmanned surface ship.
3 is a diagram showing algorithm pseudocode of a geophysical navigation method according to an embodiment of the present invention.
4 and 5 are views for explaining a geophysical navigation method according to an embodiment of the present invention.
6 is a diagram schematically illustrating the configuration of a geophysical navigation device for an unmanned surface ship according to an embodiment of the present invention.

Singular expressions used herein include plural expressions unless the context clearly dictates otherwise. In this specification, terms such as "consisting of" or "comprising" should not be construed as necessarily including all of the various components or steps described in the specification, and some of the components or some of the steps It should be construed that it may not be included, or may further include additional components or steps. In addition, terms such as "...unit" and "module" described in the specification mean a unit that processes at least one function or operation, which may be implemented as hardware or software or a combination of hardware and software. .

Hereinafter, various embodiments of the present invention will be described in detail with reference to the accompanying drawings.

1 is a flowchart illustrating a geophysical navigation method performed by a geophysical navigation device of Unmanned Surface Vehicles (USV) according to an embodiment of the present invention, and FIG. 2 illustrates a measurement concept of a geophysical sensor installed on an unmanned surface vehicle. FIG. 3 is a diagram showing an algorithm pseudo code of a geophysical navigation method according to an embodiment of the present invention, and FIGS. 4 and 5 are diagrams for explaining the geophysical navigation method according to an embodiment of the present invention. . Hereinafter, a geophysical navigation method of an unmanned surface vehicle according to an embodiment of the present invention will be described with reference to FIG. 1, but reference will be made to FIGS. 2 to 5.

The first task in geophysical navigation is to obtain reliable geophysical maps. In this work, two geophysical properties are utilized: underwater terrains and geomagnetic fields.

In step S110, the geophysical navigation device generates an underwater terrain map using sensing information obtained by a sonar sensor installed in the unmanned surface ship 100.

Step S110 is a terrain mapping procedure, which may include raw measurement of bathymetry by a sonar sensor, removal of false junctions from a sonar profile, and 3D point cloud reconstruction of an underwater terrain using pose information of an unmanned surface vehicle.

Referring to FIG. 2, the unmanned surface vessel 100 includes a Global Positioning System (GPS) 110, a Fiber Optic Gyroscope (FOG) 120, Doppler velocity logs (DVL) 130, Sensors such as a multibeam echosounder (MBES) 140 and a magnetometer 150 may be included.

For example, several sonar sensors including a single beam profiler, a Doppler velocity log 130, a multi-beam echo sounder 140, and the like may be used for depth measurement.

In an embodiment of the present invention, the multi-beam echo sounder 140 acquires hundreds of sonar ranges in a single scan, providing much more information than other sonar range sensors, so that the multi-beam echo sounder (140) provides superior efficiency. 140) was used.

In the detection range of the sonar sensor, there is generally multi-path propagation caused by the sonar signal reflected on the water surface or the surface ship itself, and the sonar range affected by this must be modified or removed for consistent mapping. Assuming that the terrain captured in all sonar scans has a smooth, continuous shape and no high-frequency components in the spatial domain, the outlier range can be removed using a threshold calculated as the average terrain height.

는 하기 수학식으로 나타낼 수 있다.Through coordinate transformation, the internal sonar range can be transformed into a three-dimensional point cloud of the underwater topography and reconstructed. Since the unmanned surface ship 100 moves in the 3-dimensional space of the water surface, the pose state vector

can be represented by the following equation.

은 무인 수상선(100)의 3차원 위치이고,

는 무인 수상선(100)의 방향을 나타내는 오일러 각도이다. 둘 다 전역 또는 관성 기준 프레임 {I}에서 나타내어진다.here,

is the 3-dimensional position of the unmanned surface ship 100,

Is an Euler angle representing the direction of the unmanned surface ship 100. Both are represented in the global or inertial frame of reference {I}.

In this specification, a Universal Transverse Mercator (UTM) coordinate system is applied as the {I} frame, but various global coordinate systems may be used depending on applications.

Next, conversion of the sonar range into terrain points can be expressed by the following equation.

where p ^S _k,t is a three-dimensional point on the terrain displayed in the sonar sensor fixed frame {S}, r _k,t is the range measurement of the kth sonar beam at time step t, and s is the sonar scan width , b is the beam spacing.

p ^S _k,t is further transformed using the following equation to be displayed in the {I} frame.

및

는 시간 단계 t에서 무인 수상선(100)의 상태로서, 각각 무인 수상선(100)의 위치 및 방향이고, R^I _B(·)는 {B} 프레임에서 {I} 프레임으로 변환시키는 회전 행렬이고, R^B _S는 {S} 프레임에서 {B} 프레임으로 변환시키는 회전 행렬이고, t^B _S는 무인 수상선(100)의 모션 센터로부터 센서 위치를 나타내는 변환 벡터이다.here,

and

Is the state of the unmanned surface ship 100 at time step t, is the position and direction of the unmanned surface ship 100, respectively, R ^I _B (·) is a rotation matrix for converting {B} frame to {I} frame, R ^B _S is a rotation matrix for converting frame {S} to frame {B}, and t ^B _S is a conversion vector representing a sensor position from the motion center of the unmanned surface vehicle 100.

및 방향

은 각각 GPS(110) 및 관성 측정기(IMU: Inertial Measurement Units)를 이용하여 측정될 수 있으며, {B} 프레임은 GPS(110)의 고정 프레임이다.Here, the position of the unmanned surface ship 100

and direction

may be measured using the GPS 110 and Inertial Measurement Units (IMU), respectively, and the {B} frame is a fixed frame of the GPS 110.

는 하기 수학식과 같이, 재구성된 3차원 지형 지점으로 구성된다.Finally, a terrain map

is composed of reconstructed 3D terrain points, as shown in the following equation.

Here, n _b is the number of sonar beams in a single scan, and t _s is the total time step required for sonar mapping.

In step S120 , the geophysical navigation device generates a geomagnetic field map of the underwater topography using sensing information obtained by the magnetometer 150 installed in the unmanned surface ship 100 .

Here, the magnetometer 150 measures the 3D vector m ^M of the Earth's magnetic field in the magnetometer fixed frame {M}.

However, the mapping result of magnetometer 150 is biased due to the effect of hard iron and soft iron caused by various electromagnetic devices and ferromagnetic objects. Therefore, calibration of the magnetometer 150 installed on the unmanned surface vehicle 100 is an essential step for consistent mapping.

은 센서 고정 프레임에서 구의 호 또는 원을 구성하며, {I} 프레임으로 변환되어 표현된다. 이때, 광섬유 자이로스코프(120)에 의하여 추정된 무인 수상선(100)의 방향이 사용된다.After calibration, the measured value of the calibrated geomagnetic field

constitutes an arc or a circle of a sphere in the sensor fixed frame, and is expressed by being converted into an {I} frame. At this time, the direction of the unmanned surface ship 100 estimated by the fiber optic gyroscope 120 is used.

The conversion of the geomagnetic field value from the {M} frame to the {I} frame is performed using the following equation.

Then, the total intensity of the geomagnetic field F is calculated by the following equation.

Since only a single three-dimensional geomagnetic field vector is obtained along the path of the unmanned surface vehicle 100, the resulting geomagnetic field strength map is very sparse. Additional interpolation is required to generate a dense geomagnetic field strength map. In order to generate a dense geomagnetic field strength map, Gaussian Process Regression (GPR), which is a nonparametric regression method, may be used. Maps of geomagnetic field strength with higher density are more useful for navigation.

In GPR, the probability of a function of two domains is defined as a normal distribution, as shown in the following equation.

및 공분산

은 하기 수학식과 같이 정의된다.where the mean of the function

and covariance

Is defined as in the following equation.

은 함수 관측(Function observations)에서 가우시안 노이즈(Gaussian noise)의 분산이고, K는 문제에 따라 미리 설계된 커널 함수(Kernel function)이다.here,

is the variance of Gaussian noise in function observations, and K is a pre-designed kernel function according to the problem.

As the kernel function, a relatively simple squared exponential kernel may be used as shown in the following equation.

는 크로네커 델타 함수(Kronecker delta function)이고,

는 신호 분산이고, l은 길이이다.here,

is the Kronecker delta function,

is the signal variance and l is the length.

이 0이라고 가정하면, 회귀는 하기 수학식으로 산출된다.beforehand, average

Assuming that is 0, the regression is calculated by the following equation.

및

는 각각 목표 함수 f(x')의 평균 및 공분산이고, y는 f(x)의 노이즈를 포함하는 관측값이다.here,

and

are the mean and covariance of the target function f(x'), respectively, and y is the observation containing the noise of f(x).

Finally, the geomagnetic field intensity map with high density can be defined as in the following equation.

은 자기장이 보간되는 작업 영역에서 사용자 정의된 3차원 위치의 수이다.here,

is the number of user-defined three-dimensional positions in the work area over which the magnetic field is interpolated.

When using the geomagnetic field map for navigation, it can be assumed that the unmanned surface vessel 100 operates at a water level similar to that in the mapping step and the tidal difference is negligible. To address this limitation, downward continuation or upward continuation may be performed using the obtained magnetic field data, and the tidal effect may be compensated for. However, since this approach requires high fidelity for sequencing procedures (especially downward sequencing), it may be excluded in embodiments of the present invention.

In step S130 , the geophysical navigation device performs geophysical navigation using the generated terrain map and geomagnetic field map.

In the geophysical navigation method according to an embodiment of the present invention, a particle filter according to a standard framework of sampling-importance resampling (SIR) is used.

That is, a particle set may be defined as a set of N pose hypotheses as shown in the following equation.

의 사후 확률은 하기 수학식의 가중치 세트를 이용하여 산출될 수 있다.The target state of this particle set

The posterior probability of can be calculated using a set of weights in the following equation.

(파티클 사이즈),

(파티클 사이즈 임계치),

(포즈 초기값),

(제어 입력),

(모션 노이즈 파라미터),

(소나 범위 측정값),

(자기장 측정값),

(소나 범위 노이즈 공분산),

(평활화된 노이즈 공분산),

(자기장 노이즈 공분산),

(지형 거칠기 임계치),

(지형 지도),

(지자기장 지도),

(소나 관심 영역),

(자력계 관심 영역),

(소나 센싱 파라미터)를 포함한다. 그리고, 도 3의 알고리즘의 출력은

(포즈),

(파티클 세트),

(파티클 가중치 세트)를 포함한다.For example, FIG. 3 shows pseudocode of an algorithm according to a geophysical navigation method according to an embodiment of the present invention. The input of the algorithm of Figure 3 is

(particle size),

(particle size threshold),

(Pose initial value),

(control input),

(motion noise parameter),

(sonar range measurements),

(magnetic field measurements),

(sonar range noise covariance),

(Smoothed Noise Covariance),

(magnetic field noise covariance),

(terrain roughness threshold),

(topographic map),

(geomagnetic field map),

(sonar region of interest),

(magnetometer region of interest),

(sonar sensing parameters). And, the output of the algorithm of Figure 3 is

(pose),

(particle set),

(set of particle weights).

The process of this algorithm may consist of three steps: motion update, weight update, and resampling. These steps may be performed based on the motion and sensor observation model of the designed unmanned surface ship 100 .

That is, as shown in FIG. 1, the step of performing geophysical navigation in step S130 includes a step of updating a pose of the unmanned surface vehicle 100 (S131), a step of updating particle weights (S132), and a resampling step (133). do.

In step S131, the geophysical navigation device updates the pose of the unmanned surface craft 100.

및 요 각속도(Yaw angular velocity)

의 두 속도에 의하여 제어되는 것으로 가정된다.In this step, the particles are moved and dispersed according to the motion model of the unmanned surface ship 100. In an embodiment of the present invention, a velocity motion model is used, in which the motion is a forward linear velocity.

and yaw angular velocity

It is assumed to be controlled by the two velocities of

및 요 각속도

를 측정한다.The motion sensor used in the present invention includes Doppler velocity logs (DVL) 130 and Fiber Optic Gyroscope (FOG) 120, each with a forward linear velocity.

and yaw angular velocity

to measure

은 모션 예측의 불확실성을 실현하기 위하여 확률적으로 각 파티클에 적용된다. and control input

is stochastically applied to each particle to realize the uncertainty of motion prediction.

이 샘플링된다.First, a control input and an additional heading perturbation term such as the following equation

This is sampled.

은 평균이 0이고, 표준편차가

인 정규분포에서 샘플링된 값이고, 파라미터 세트

는 모션 노이즈를 나타낸다. 특히,

및

는 각각 현재의 선형 속도 및 각속도에 따라 선형 속도 측정의 불확실성이 증가하는 정도를 나타내고,

및

는 유사하게, 각속도의 불확실성을 설정한다.here,

has a mean of 0 and a standard deviation of

is a value sampled from a normal distribution of , and a set of parameters

represents motion noise. Especially,

and

Represents the degree to which the uncertainty of linear velocity measurement increases according to the current linear velocity and angular velocity, respectively,

and

Similarly, sets the uncertainty of the angular velocity.

The pose of the unmanned surface ship 100 is updated using the following equation.

는 파도로 인한 무인 수상선(100)의 z축 방향 모션의 방해(Disturbance)이다.here,

Is the disturbance of the motion of the unmanned surface ship 100 in the z-axis direction due to waves.

In step S132, the geophysical navigation device updates the particle weight of the unmanned surface vehicle 100.

In this step, the weight of each particle is updated according to the probability of the predicted measurement from each particle, given the actual sensor measurement. Predicted measured values may be calculated from observation models of each sensor. For the multibeam echosounder (MBES) 140, two types of observation models can be applied: projection-based and range-based. In this work, a range-based model was applied because it provides more information in the case of multi-beam observation.

에서 히트 포인트(Hit point)를 찾아서 수행된다. 다중 빔 소나 감지에 사용되는 빔의 개수가 수백 개이기 때문에, 효율적인 광선 투사가 필요하다. 이를 위한 한 가지 기본적인 접근방식은 빔 측정을 다운 샘플링하는 것이다. 그리고, 다른 접근방식에는 지도 해상도를 줄이는 것이 있다. 그러나, 이는 대상 영역의 크기가 상당히 클 때에 한계가 있다.Prediction of the sonar range casts rays in the beam direction and maps the terrain.

It is performed by finding a hit point in Since the number of beams used for multi-beam sonar detection is hundreds, efficient beam projection is required. One basic approach for this is downsampling the beam measurement. And, another approach is to reduce the map resolution. However, this has limitations when the size of the target area is considerably large.

Therefore, in an embodiment of the present invention, as shown in FIG. 4 , a region of interest (ROI) is set to limit a search region for ray projection. This approach sets a bounding box centered on the current pose with a predefined width and height to cover the maximum roll and pitch of the unmanned surface vehicle (100). This approach can reduce the computational load and ensure the scalability of the range-based observation model.

Once the range is predicted, the particle weights for the sonar range can be updated using the equation:

는 i번째 파티클로부터 예측된 소나 범위이고,

는 소나 범위 측정값의 노이즈 공분산이다.where r _t is the array of real sonar ranges with outliers removed in a single scan at time step t,

is the predicted sonar range from the ith particle,

is the noise covariance of the sonar range measurements.

And, the particle weights are updated in a productive way, but additional updates can also be used.

에서 히트 포인트를 찾아 예측될 수 있다.Similarly, the magnetometer measurements are taken by projecting a single beam down from the center of the magnetometer 150, and mapping the geomagnetic field.

It can be predicted by finding the hit point in .

Accordingly, the update of particle weights for the magnetometer can be calculated using the following equation.

는 i번째 파티클로부터 예측된 강도이고,

는 자력계 측정값의 노이즈 공분산이다.where F _t is the actual geomagnetic field strength measured by magnetometer 150 at time step t,

is the intensity predicted from the ith particle,

is the noise covariance of the magnetometer measurements.

In step S133, the geophysical navigation device performs resampling on particles and weights.

Existing sampling-importance resampling (SIR) filters have a particle shortage problem when likelihood values between particles are very similar for a certain period of time. In particular, particle shortages can occur in the vicinity of flat underwater terrain.

In order to solve this particle shortage problem, in the embodiment of the present invention, terrain roughness R _z is used, and the existing particle weight update step (S132) and resampling step (133) based on the R _z value amended

The R _z value of each sonar scan can be calculated using the following equation.

Here, z _peak and z _valley are the ith highest peak value and lowest valley value, respectively, and q is the number of extreme z values used.

This parameter R _z represents the average of the maximum differences in terrain elevations captured in a single multi-beam sonar scan. When R _z becomes smaller than a threshold value, a larger value is optionally assigned to the sensor covariance used for weight update. Therefore, the particle weight update equation in Equation 16 when R _z exceeds the threshold value is modified to the following equation when R _z is below the threshold value.

는 소나 범위 측정값의 평활화된 공분산이다.here,

is the smoothed covariance of the sonar range measurements.

On the other hand, the resampling step 133 is modified by limiting the resampling frequency for R _z below a threshold. That is, resampling is performed only when the size of the particle set is less than the threshold and R _z exceeds the threshold, and conversely, when the size of the particle set is greater than the threshold and R _z is less than the threshold, resampling is not performed.

The particle weight update step (S132) and the resampling step (133) modified as described above appear in lines 8 to 12 and lines 19 to 22 of the algorithm pseudo code of FIG. 3, respectively.

5 shows an example of terrain smoothness (1/R _z ) calculated along the trajectory of the unmanned surface ship 100 using data obtained from a field test. Referring to FIG. 5, in an area where the topographic smoothness (1/R _z ), which is an area indicated by yellow and red, is greater than a specific threshold, a modified particle weight update step (S132) and a resampling step (133) according to an embodiment of the present invention ) can be applied.

6 is a diagram schematically illustrating the configuration of a geophysical navigation device for an unmanned surface ship according to an embodiment of the present invention.

Referring to FIG. 6 , a geophysical navigation device for an unmanned surface vessel according to an embodiment of the present invention includes a processor 10 , a memory 20 , a communication unit 30 and an interface unit 40 .

The processor 10 may be a CPU or a semiconductor device that executes processing instructions stored in the memory 20 .

Memory 20 may include various types of volatile or non-volatile storage media. For example, memory 20 may include ROM, RAM, and the like.

For example, the memory 20 may store instructions for performing a geophysical navigation method of an unmanned surface vehicle according to an embodiment of the present invention.

The communication unit 30 is a means for transmitting and receiving data with other devices through a communication network.

The interface unit 40 may include a network interface and a user interface for accessing a network.

On the other hand, the components of the above-described embodiment can be easily grasped from a process point of view. That is, each component can be identified as each process. In addition, the process of the above-described embodiment can be easily grasped from the viewpoint of components of the device.

In addition, the technical contents described above may be implemented in the form of program instructions that can be executed through various computer means and recorded in a computer readable medium. The computer readable medium may include program instructions, data files, data structures, etc. alone or in combination. Program commands recorded on the medium may be specially designed and configured for the embodiments or may be known and usable to those skilled in computer software. Examples of computer-readable recording media include magnetic media such as hard disks, floppy disks and magnetic tapes, optical media such as CD-ROMs and DVDs, and magnetic media such as floptical disks. - includes hardware devices specially configured to store and execute program instructions, such as magneto-optical media, and ROM, RAM, flash memory, and the like. Examples of program instructions include high-level language codes that can be executed by a computer using an interpreter, as well as machine language codes such as those produced by a compiler. A hardware device may be configured to act as one or more software modules to perform the operations of the embodiments and vice versa.

The embodiments of the present invention described above have been disclosed for illustrative purposes, and those skilled in the art having ordinary knowledge of the present invention will be able to make various modifications, changes, and additions within the spirit and scope of the present invention, and such modifications, changes, and additions will be considered to fall within the scope of the following claims.

100: unmanned surface ship
10: Processor
20: memory
30: Ministry of Communications
40: interface unit

Claims (10)

In the geophysical navigation method performed by the geophysical navigation device of Unmanned Surface Vehicles (USV),
generating an underwater terrain map using sensing information obtained by a sonar sensor installed in the unmanned surface ship;
Generating a geomagnetic field map for underwater topography using sensing information obtained by a magnetometer installed in the unmanned surface ship; and
Including the step of performing geophysical navigation using the generated terrain map and geomagnetic field map,
The step of performing the geophysical navigation,
Updating a pose of the unmanned surface ship;
Updating a weight of a particle defined as a set of pose hypotheses; and
And performing resampling on the particles and the weight,
The updating of the weights and the performing of the resampling may be modified using terrain roughness.
According to claim 1,
The terrain map (

) is a geophysical navigation method, characterized in that composed of three-dimensional terrain points reconstructed using the following equation.

where n _b is the number of sonar beams in a single scan, t _s is the total time step required for sonar mapping,

and

Is the position and direction of the unmanned surface ship at time step t, R ^I _B ( ) is a rotation matrix that converts {B} frame to {I} frame, R ^B _S is {B} in {S} frame A rotation matrix for transforming into a frame, t ^B _S is a conversion vector representing the sensor position from the motion center of the unmanned surface vehicle, p ^S _k,t is a 3D point on the terrain displayed in the sonar sensor fixed frame {S}, r _k,t is the range measure of the kth sonar beam at time step t, s is the sonar scan width, and b is the beam spacing
According to claim 1,
The geomagnetic field map (

) is a geophysical navigation method, characterized in that calculated by the following equation.

here,

Is the average of the target function f(x') calculated using Gaussian Process Regression (GPR),

is the number of user-defined three-dimensional positions in the work area where the magnetic field is interpolated
According to claim 3,
The geophysical navigation method, characterized in that the Gaussian Process Regression (GPR) is performed using the following equation.

here,

is the probability of a function of the two domains,

and

is the mean and covariance,

is the variance of Gaussian noise, K is a pre-designed kernel function,

is the Kronecker delta function,

is the signal variance, l is the length,

and

are the mean and covariance of the target function f(x'), respectively, and y is the observation containing the noise of f(x)
According to claim 1,
The geophysical navigation method, characterized in that the pose of the unmanned surface vehicle is updated using the following equation.

here,

Is the disturbance of the z-axis motion of the unmanned surface vessel due to waves,

has a mean of 0 and a standard deviation of

is a value sampled from a normal distribution of , and a set of parameters

is the motion noise,

is the forward linear velocity,

is the yaw angular velocity
According to claim 1,
Updating the weight of the particle,
A geophysical navigation method characterized in that weights of particles for a sonar range are updated using the following equation.

where r _t is the array of real sonar ranges with outliers removed in a single scan at time step t,

is the predicted sonar range from the ith particle,

is the noise covariance of the sonar range measurements
According to claim 1,
Updating the weight of the particle,
A geophysical navigation method characterized in that a weight of a particle with respect to a magnetometer is updated using the following equation.

where F _t is the actual geomagnetic field strength measured by the magnetometer at time step t,

is the intensity predicted from the ith particle,

is the noise covariance of the magnetometer measurements
According to claim 1,
The step of updating the weight of the particle and the step of performing the resampling are modified using a terrain roughness R _z calculated using the following equation.

where z _peak and z _valley are the ith highest peak and lowest valley values, respectively, q is the number of extreme z values used, and R _z is the average of the maximum differences in terrain elevations captured in a single multi-beam sonar scan. indicate
According to claim 8,
The step of performing the resampling is,
Resampling is not performed when the size of the particle set is greater than or equal to the threshold and the R _z is less than or equal to the threshold,
Updating the weight of the particle,
The geophysical navigation method characterized in that, when the R _z is less than or equal to a threshold value, a weight of a particle for a sonar range is updated using the following equation.

where r _t is the array of real sonar ranges with outliers removed in a single scan at time step t,

is the predicted sonar range from the ith particle,

is the smoothed covariance of the sonar range measurements
In the geophysical navigation device of unmanned surface vehicles (USV),
memory for storing instructions; and
Including a processor that executes the instructions,
The command is
generating an underwater terrain map using sensing information obtained by a sonar sensor installed in the unmanned surface ship;
Generating a geomagnetic field map for underwater topography using sensing information obtained by a magnetometer installed in the unmanned surface ship; and
Including the step of performing geophysical navigation using the generated terrain map and geomagnetic field map,
The step of performing the geophysical navigation,
Updating a pose of the unmanned surface ship;
Updating a weight of a particle defined as a set of pose hypotheses; and
Performing a geophysical navigation method comprising the step of performing resampling on the particles and the weight,
The updating of the weights and the performing of the resampling may be modified using terrain roughness.

Images