KR20130047455A - Apparatus and method for estimating the position of underwater robot - Google Patents

Abstract

PURPOSE: A device and a method for estimating the position of an underwater robot are provided to accurately measuring a three-dimensional position of the underwater robot in water by solving problems regarding errors, difficulty of installing a sensor, and cost. CONSTITUTION: A device for estimating the position of an underwater robot comprises an inner sensor unit(110), an outer sensor unit(120), a potential position obtaining unit(130), a reliability obtaining unit(140), a potential position rearrangement unit(150), and a positioning unit(160). The reliability obtaining unit obtains the reliability for potential particles by allowing environmental information to a random variable at each sampling time. The potential position rearrangement unit rearranges the potential particles according to reliability. The determining unit calculates an average position of the potential particles rearranged at each sampling time and determines a calculated average position as a current position of the underwater robot. [Reference numerals] (110) Inner sensor unit; (120) Outer sensor unit; (130) Potential position obtaining unit; (131) Potential position generating unit; (132) Potential position update unit; (140) Reliability obtaining unit; (150) Potential position rearrangement unit; (160) Positioning unit; (AA) Speed information; (BB) Potential particle; (CC) Environment information; (DD) Reliability; (EE) Rearranged potential particle; (FF) Current position

Description

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an apparatus for estimating the position of an underwater robot,

The present invention relates to an apparatus and method for estimating the position of an underwater robot, and more particularly, to an apparatus and method for estimating the position of an underwater robot in an underwater robot, which can accurately measure three- And a method for estimating the position of the mobile terminal.

The present invention is derived from research carried out by the Ministry of Education, Science and Technology and the Chosun University Industry-Academy Collaboration Center as part of the revitalization of regional basic research (local innovation manpower training).

[Project number: 1345135934, Research title: Robot autonomic driving technology and commercialization and manpower development project, Research period: 2010-07.01 ~ 2013-04-30 (Study period: 2011-05-01 ~ 2012- 04-30)]

Underwater robots can be used in a variety of ways to address issues facing marine ecosystems, such as underwater research and ocean mapping, climate change, and marine habitat monitoring.

In order for the underwater robot to perform the correct operation, the robot itself must judge and compensate for the obstacle or other external environment that may appear during the movement, the degree to which the given path is deviated to some extent, in which direction, and to what extent. However, position estimation and navigation in an unstructured underwater environment is a difficult task.

The estimation of the position of the underwater robot is indispensable function for autonomous navigation of the underwater robot. Conventionally, there are a speculative navigation using an inertial sensor (INS) and an LBL, USBL, and SBL methods for estimating a position by installing an external sensor . However, the conventional method using the inertial sensor has a problem that the position error is accumulated over time due to the uncertainty of robot operation and sensor information. In addition, this method has a problem in that a robot abortion phenomenon frequently occurs in which the robot's position is lost by moving the robot to another place while the internal sensor of the robot can not detect the environment. In addition, the existing method of locating the robot by installing the sensor in the external environment does not cause the sensor error accumulation problem as compared with the estimated navigation using the inertial sensor, but there is a problem of the difficulty of the sensor installation and the cost.

An object of the present invention is to provide an apparatus for estimating the position of an underwater robot and a method thereof for accurately measuring the three-dimensional position of the underwater robot in the water by solving the error accumulation problem which is a disadvantage of the existing position estimation method There is.

According to an aspect of the present invention, there is provided a robot control method comprising: an internal sensor unit for acquiring velocity information including information on a posture, a moving direction, and a moving velocity of an underwater robot at each sampling time; An external sensor unit that senses an external environment that changes according to the movement of the underwater robot at each sampling time to acquire environment information; A potential position acquiring unit for acquiring potential particles that are likely to be positioned by the underwater robot at each sampling time; A reliability acquiring unit for acquiring reliability of the obtained potential particles by using the environmental information as a random variable at each sampling time; A latent location rearrangement unit for rearranging the obtained potential particles according to the reliability for each sampling time; And a position determiner for calculating an average position of the rearranged potential particles for each sampling time and determining the calculated average position as a current position of the underwater robot at each sampling time .

Preferably, the velocity information may include information about a roll angular velocity, a pitch angular velocity, a yaw angular velocity, a surge linear velocity, a sweep linear velocity, and a Hebrew linear velocity of the underwater robot.

Preferably, the latent position acquiring unit initially generates latent particles from the initial position of the underwater robot and the speed information, and acquires the latent particles rearranged at the previous sampling time, So that the potential particles can be obtained.

According to another aspect of the present invention, there is provided a robot control method for a robot, comprising the steps of: (a) acquiring velocity information including information about a posture, a moving direction, and a moving velocity of an underwater robot at each sampling time; (b) (C) acquiring potential particles that are likely to be located in the underwater robot, (d) acquiring the environment information by using the environment information as a random variable, (E) relocating each of the obtained potential particles according to the reliability, and (f) calculating an average position of the relocated potential particles, and calculating Determining the average position of the underwater robot as the current position of the underwater robot, and repeating the steps (a) to (e) for each sampling time to estimate the position of the underwater robot in real time And a method of estimating the position of the underwater robot.

According to the present invention, it is possible to accurately measure the three-dimensional position of the underwater robot in the water by solving the problems of error accumulation, difficulty of sensor installation, and cost, which are disadvantages of existing underwater robot position estimation methods.

1 is a diagram illustrating a process of estimating a position of an underwater robot.
2 is a block diagram illustrating an apparatus for estimating a position of an underwater robot according to the present invention.
3 and 4 are views for explaining attitude information and velocity information, respectively.
Fig. 5 is a diagram illustrating the attitude rotation about the yaw angle, the pitch angle, and the roll angle, respectively.
6 to 8 are graphs showing simulation results of the present invention.
9 is a flowchart illustrating a method of estimating a position of an underwater robot according to the present invention.

While the invention is susceptible to various modifications and alternative forms, specific embodiments thereof are shown by way of example in the drawings and are herein described in detail. It should be understood, however, that the invention is not intended to be limited to the particular forms disclosed, but includes all modifications, equivalents, and alternatives falling within the spirit and scope of the invention.

The terms first, second, etc. may be used to describe various components, but the components should not be limited by the terms. The terms are used only for the purpose of distinguishing one component from another. For example, without departing from the scope of the present invention, the first component may be referred to as a second component, and similarly, the second component may also be referred to as a first component.

The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. Singular expressions include plural expressions unless the context clearly indicates otherwise. In the present application, the terms "comprising" or "having ", and the like, are intended to specify the presence of stated features, integers, steps, operations, elements, parts, or combinations thereof, But do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, parts, or combinations thereof.

Unless defined otherwise, all terms used herein, including technical or scientific terms, have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs.

Terms such as those defined in commonly used dictionaries are to be interpreted as having a meaning consistent with the meaning in the context of the relevant art and are to be interpreted as ideal or overly formal in meaning unless explicitly defined in the present application Do not.

Underwater robots are used for exploring seabed resources, lifting sinking ships, removing oil, installing submarine cables, and repairing underwater structures. The underwater robot has to move freely in three dimensional water. To this end, the underwater robot has an attitude control device for adjusting the attitude and a movement control device for controlling the movement in the three dimensional direction. The posture control device and the movement control device precisely control the motor connected with the propeller or the like, so that the underwater robot can move freely in the water.

On the other hand, it is essential to understand where the underwater robot is present in the water for autonomous navigation of the underwater robot.

The present invention provides an apparatus and method for estimating the current position of a submersible robot based on various information inside and outside the submersible robot. The underwater robot can perform autonomous travel based on the current position provided by the present invention.

1 is a diagram illustrating a process of estimating a position of an underwater robot.

When an underwater robot enters the water, the robot moves under water according to the instruction of the upper system or autonomously. In Fig. 1, L _t1 denotes an initial position of the underwater robot, and the initial position can be input by the user.

With the passage of time underwater robots there is moved in the water (L -L _{t1 t2 t3} -L), the current position of the robot hand to the position estimating apparatus and method for a robot hand based on the various kinds of information according to the present invention in real time, And provides it to the underwater robot.

Meanwhile, the underwater robot can estimate the current position based on the external information received from the external environment information providing apparatus 20 as well as the information about the position and speed acquired from various sensors built in the underwater robot. The external environment information providing apparatus 20 provides the underwater robot with the position information and the map information of the environment information providing apparatus 20 to help estimate the current position of the underwater robot, , Water flow, and so on.

2 is a block diagram illustrating an apparatus for estimating a position of an underwater robot according to the present invention.

2, the apparatus 100 for estimating the position of an underwater robot according to the present invention includes an internal sensor unit 110, an external sensor unit 120, a latent position acquiring unit 130, a reliability acquiring unit 140, A location rearranging unit 150, and a positioning unit 160. [

The internal sensor unit 110 is connected to the propulsion unit (not shown) of the underwater robot to acquire velocity information related to the posture, the moving direction, and the moving velocity of the underwater robot at each sampling time. The propulsion means both an attitude control device for controlling the attitude of the underwater robot and a motion control device for controlling the moving direction and the moving speed of the underwater robot.

The internal sensor unit 110 is a sensor such as an angular velocity sensor (gyroscope) and an encoder connected to a propulsion unit for controlling the attitude, movement direction, and movement speed of the underwater robot, and acquires angular velocity information related to the posture of the underwater robot And a linear velocity acquisition sensor (not shown) for acquiring linear velocity information associated with the movement direction and the movement velocity. The velocity information acquired at each sampling time in the internal sensor unit 110 includes angular velocity information related to the attitude of the underwater robot and linear velocity information related to the moving direction and the moving velocity of the underwater robot.

The angular velocity information includes information about the roll angular velocity, the pitch angular velocity, and the yaw angular velocity of the underwater robot, which provides information on the posture of the underwater robot as described above. The linear velocity information is information providing information on the moving direction and the moving velocity of the underwater robot. The linear velocity information includes information on the surge linear velocity, sway linear velocity and heave linear velocity of the underwater robot. .

FIGS. 3 and 4 are views for explaining angular velocity information and linear velocity information, respectively.

Fig. 3 (a) shows the roll angular velocity, Fig. 3 (b) shows the pitch angular velocity, and Fig. 3 (c) shows the yaw angular velocity.

4A shows the surge linear velocity, FIG. 4B shows the sway linear velocity, and FIG. 4C shows the heave linear velocity.

Returning to Fig. 2, description will be made again.

The velocity information obtained by the internal sensor unit 110 is provided to a potential position acquisition unit 130 to be described later.

On the other hand, the external sensor unit 120 senses the external environment changing according to the operation of the underwater robot, acquires environment information for each sampling time, and provides the acquired environment information to the potential position acquisition unit 130.

The external sensor unit 120 includes at least one of various sensors such as a distance sensor, a gyroscope, an electronic compass, an inertial sensor, and a position sensor to detect an external environment that changes according to a change and an attitude of the robot in the water And may receive external information from the external environmental information providing device 20. [

The potential position acquisition unit 130 acquires potential particles that are likely to be located in the underwater robot at each sampling time.

Here, the latent particle is information indicating a position where a submersible robot may exist, and it is composed of information (x, y, z, φ, θ, ψ) about the attitude and position of the underwater robot. In other words, the potential particle does not mean the definite position of the underwater robot but the position where the underwater robot may exist and the attitude at the position, and x, y, z denote the position of the underwater robot in the fixed coordinate system And φ, θ, and ψ represent the attitude (roll angle, pitch angle, and yaw angle) of the robot in the fixed coordinate system, respectively.

The potential position acquiring unit 130 includes a potential position generating unit 131 and a potential position updating unit 132. The latent position generating unit 131 generates latent particles from the initial position, which is the initial position of the underwater robot, and the speed information at the start of the movement, and the latent position update unit 132 updates the latent position And moves the potential particles rearranged by the rearrangement unit 150 based on the speed information, thereby updating the potential particles of the previous sampling time with the potential particles of the current sampling time. That is, the potential position update unit 132 moves the potential particles in the past according to the current velocity information, thereby causing the potential particles to continuously update in real time.

Hereinafter, the process of generating and updating the potential particles by the potential position acquisition unit 130 will be described in more detail.

In the present invention, the Euler angle used in the inertial navigation system is used to calculate the potential particles for a submersible robot. The Euler angles are values of the angular velocity in the moving robot coordinate system fixed to the robot, It is a matrix converted into the angular velocity value in the fixed coordinate system. The Euler angular method uses the relationship between the rotational angular velocity between the fixed coordinate system and the robot coordinate system and the derivative of the Euler angles to determine the roll angle (roll, φ), pitch angle (pitch, θ) yaw, ψ) are calculated, and the coordinate transformation matrix is constructed by using these three angles.

The Euler angles used to represent the coordinate system of the underwater robot in the fixed coordinate system of the earth are composed of roll angle, pitch angle and yaw angle.

Fig. 5 is a diagram illustrating the attitude rotation about the yaw angle, the pitch angle, and the roll angle, respectively.

First, the yaw angle is rotated from the z-axis of the reference coordinate system as shown in Fig. 5 (a), and the pitch angle in the rotated y-axis is rotated as shown in Fig. 5 (b). 5 (a) and 5 (b), the roll angle in the new x axis is rotated as shown in Fig. 5 (c) based on the axis rotated.

The posture rotation as shown in FIG. 2 can be defined by the following mathematical expression.

The first matrix of Equation (1) represents a rotation matrix for the yaw angle, the second matrix of Equation (1) represents a rotation matrix for the pitch angle, and the third matrix of Equation (1) Respectively. Here, N denotes a fixed coordinate system, B denotes a robot coordinate system, and Fn _denotes a coordinate system converted by a determinant.

The attitude of the underwater robot with respect to the robot coordinate system can be calculated through the product of the respective coordinate rotation determinants. Therefore, the transformation matrix for transforming from the fixed coordinate system to the robot coordinate system can be defined as the following equation (2).

로 변환하면 다음의 [수학식 3]에서처럼 고정좌표계의 변환행렬인

을 획득할 수 있다. In order to obtain the coordinates of the underwater robot with respect to the fixed coordinate system,

The transformation matrix of the fixed coordinate system as shown in the following equation (3)

Can be obtained.

In Equation (3), c and s mean cos and sin, respectively.

On the other hand, using the linear velocity information of the underwater robot occurring in the coordinate system of the underwater robot, the linear velocity information of the underwater robot in the fixed coordinate system is expressed as follows.

는 고정좌표계에서의 수중로봇의 선속도를 의미한다.In Equation (4), u, v, and w are the linear velocities of the underwater robot in the robot coordinate system,

Means the linear velocity of the underwater robot in the fixed coordinate system.

는 다음의 [수학식 5]처럼 다시 표현될 수 있다,In Equation (4)

Can be re-expressed as: < EMI ID =

In Equation (5), c and s mean cos and sin, respectively.

The relationship between the angular velocity of each axis and the Euler angle over time can be expressed as follows.

[Mathematical Expression 1-1] to [Mathematical Expression 1-3] to [Expression 6], the expression can be rewritten as follows.

Equation (7) expresses the amount of change of the Euler angle as an angular velocity.

The roll angles, pitch angles, and yaw angles are expressed by the following equations.

를 적분하면 고정좌표계에서의 수중로봇의 위치정보가 획득되며, [수학식 8]에서 획득한

를 적분하면 고정좌표계에서의 수중로봇의 자세정보가 획득될 수 있다.In Equation (4)

By integrating, the position information of the underwater robot in the fixed coordinate system is obtained.

The posture information of the underwater robot in the fixed coordinate system can be obtained.

On the other hand, the potential position acquiring unit 130 predicts and acquires positions of potential particles according to the velocity information of the underwater robot using the algorithm shown in [Table 1] below.

The algorithm shown in [Table 1] is an algorithm that models the movement of potential particles corresponding to the velocity information including the information about the angular velocity and the linear velocity of the underwater robot, and adds the uncertainty of each potential particle according to the parameters, Lt; / RTI > That is, the algorithm of [Table 2] mathematically expresses the movement of each potential particle according to the parameters of the underwater robot's velocity information (u, v, w, p, q, r) and α _uu to α _rr .

In Table 1, u _t represents the velocity information of the underwater robot, and x _{t -1} represents the position (x, y, z, φ, θ, ψ) of the potential particle at time t-1. Here, it is assumed that v = w = p = 0.

The surge velocity parameters α _uu , α _uv , α _uw , α _up , α _uq , and α _ur determine the distribution of potential particles in the surge direction. α _uu, α _uv, α _uw, α _uq, α _uq, α _ur of and are potential particles widely distributed in the direction Works higher the parameter value is larger, the pitch angle velocity parameter α _qu, α _qv, α _qw, α _qp, α _qq , α _qr The larger the parameter value, the wider the potential particles are distributed in the pitch direction. The angular velocity parameters α _ru , α _rv , α _rw , α _rp , α _rq and α _rr also show that the larger the parameter value, the wider the potential particles are in the yaw direction.

The sample normal distribution function in [Table 2] is an algorithm that adds the uncertainty of the potential particle motion for the velocity command parameters α _uu to α _rr and λ _xu to λ _{ψ r} .

The potential position acquiring unit 130 acquires the position (x, y, z,?,?,?) Of the potential particles, which are likely to be located in the underwater robot in the fixed coordinate system, based on the velocity information received from the internal sensor unit 110, θ, ψ), and provides the obtained potential particles to the reliability acquiring unit 140.

The reliability acquisition unit 140 extracts the reliability of the potential particles using the environmental information as a random variable.

The process in which the reliability obtaining unit 140 extracts the reliability will be described in more detail.

The potential particles obtained by the potential position acquiring unit 130 include uncertainties as described above. The reliability acquisition unit 140 calculates the reliability of the potential particles based on the environment information received from the external sensor unit 120, i.e., the distance information and the angle detected from the external environment.

Reliability may include a distribution of four states: Measurement Noise, Unexpected Object, Sensing Failure, and Unexplainable Measuring Noise.

Measurement Noise is expressed as a Gaussian probability distribution as an error distribution around the distance and angle between the potential particle and the sensor as follows.

An Unexpected Object can be expressed as an exponential probability distribution as an error distribution in which a dynamic obstacle can exist in a given map environment as follows.

Equation (11) shows a sensing failure when the sensor data is detected or not operated.

Unexplainable Measuring Noise is the probability distribution of sensor values due to wall reflection or crosstalk.

After the above four probability distribution apply the _hit weights z, z _long, z _max, z _rand the following combination is obtained by the reliability of the potential of particles.

The following Table 3 represents an algorithm for extracting the reliability of the potential particles by using the distribution diagram of Equation (13). The reliability acquisition unit 140 extracts the reliability of each potential particle based on the potential particles and the map information, using the environment information received from the external sensor unit 120 as a random variable, as in the algorithm of Table 3. The extracted reliability is provided to the potential position rearrangement unit 150. [

The potential position rearranging unit 150 rearranges the potential particles obtained from the potential position obtaining unit 130 based on the reliability of each potential particle obtained at the reliability acquiring unit 140 at each sampling time. The process of rearranging the latent particles implies the process of replicating and removing each latent particle according to the reliability. The following table 4 shows an algorithm for replicating or removing each latent particle according to the reliability of each latent particle . The rearranged potential particles are provided to the potential position update unit 132 of the potential position acquiring unit 130. The potential position update unit 132 updates the potential particles by moving the rearranged potential particles in accordance with the current velocity information, respectively.

The positioning unit 160 calculates the average position of the rearranged potential particles and determines the calculated average position value to be the current position of the underwater robot. The position determination unit 160 determines the current position of the underwater robot in accordance with the passage of time so that the current position of the underwater robot can be continuously determined according to the flow of time and the motion (movement) of the underwater robot.

[Table 5] is a positioning algorithm for determining the position of a robot in the water.

[Table 6] is an algorithm expressing all the processes described above.

In the algorithm of Table 6, the motion model of the third line is a process of estimating each potential particle at the current sampling time by moving each potential particle rearranged at the previous sampling time according to the current velocity information. The motion model is an algorithm performed by the potential position update unit 132 of the potential position acquisition unit 130.

The sensor model of the 4th line is a part for calculating the reliability. The sensor model of the 4th line is a part for calculating the uncertainty of the sensor information based on the environmental information including the map information at the current position of the robot, And then calculating the reliability of each potential particle. The sensor model is an algorithm performed by the reliability obtaining unit 140.

The resampling model of the 6th to 8th lines is a part to rearrange the potential particles, and it is a process of re-selecting the potential particles based on the reliability calculated in the sensor model. The resampling model is an algorithm performed by the potential position rearrangement unit 150.

And the Decision Robot Pose of the 9th line is the process of determining the position of the underwater robot, and calculates the average position of the potential particles selected by the resampling model every sampling. The Desision Robot Pose is an algorithm that the positioning unit 160 performs.

6 to 8 are graphs showing simulation results of the present invention.

The sensors used in the simulation are two distance sensors, a depth sensor and a yaw sensor. In addition, the commands given to the underwater robot in the simulation were limited to u = 0.5m / s, q = r = 0.05rad / s and v = w = p = 0.

FIGS. 6 to 8 show the result of positional estimation with respect to the X, Y, and Z axes when the above command is given. Robot denotes a simulated underwater robot, and PT Avg denotes an average value of potential particles.

In FIG. 6, when the portion indicated by the dotted line is initially found, the particles are not accurately positioned with respect to the position of the X coordinate, but the position is accurately estimated over time.

Fig. 7 shows the position estimation result of the robot with respect to the Y coordinate. At the beginning of the algorithm execution, it looks like the dotted line part because the potential particles take an average as a result of random scattering. However, it can be seen that the position estimation is possible by selecting the potential particles having a high probability of being located in the data of the received sensor.

Fig. 8 shows the position estimation result of the underwater robot with respect to the Z coordinate. This is because the dotted line portion takes an average of the potential particles randomly distributed at the beginning of the algorithm execution as described in FIG.

According to the present invention, the three-dimensional position of the underwater robot in water can be accurately measured by solving the error accumulation problem which is a disadvantage of the position estimation method using the existing dead reckoning method.

Hereinafter, a method for estimating a position of an underwater robot according to the present invention will be described. The method for estimating the position of the underwater robot according to the present invention is essentially the same as the position estimating apparatus for the underwater robot according to the present invention, and thus a detailed description and a duplicate description will be omitted.

9 is a flowchart illustrating a method of estimating a position of an underwater robot according to the present invention.

First, the potential position acquiring unit 130 acquires velocity information including information on a posture, a moving direction, and a moving velocity of the underwater robot from the internal sensor unit 110 (S10). Here, the velocity information is information indicating the posture, the moving direction, and the moving velocity of the underwater robot 10, and information about the roll angular velocity, pitch angular velocity, yaw angular velocity, surge linear velocity, .

Then, the potential position acquiring unit 130 generates potential particles from the initial position and velocity information of the underwater robot 10 in the initial stage, that is, in the first movement (S30), and after the initial stage, Is moved according to the velocity information of the current sampling time to acquire potential particles (S35).

The reliability acquiring unit 140 acquires environment information from the external sensor unit 120, which senses an external environment that changes according to the movement of the underwater robot (S40).

The reliability acquiring unit 140 acquires the reliability of the potential particles acquired in step S30 or step S35, respectively, using the obtained environment information as a random variable (S40). The obtained reliability of each potential particle is provided to the potential position rearrangement unit 150. [

The potential location rearranging unit 150 rearranges the obtained potential particles in accordance with the reliability acquired in step S50, respectively (S60).

The potential particles rearranged in step S60 are provided to the potential position acquiring unit 130. [

The positioning unit 160 calculates the average position of the rearranged potential particles and determines the calculated average position as the current position of the underwater robot (S70).

Then, the positioning unit 160 transmits the determined current position of the underwater robot to the parent apparatus, that is, the apparatus for controlling the posture and movement of the underwater robot (S80).

Meanwhile, the potential particles rearranged in step S60 are provided to the potential position acquiring unit 130, and the potential position acquiring unit 130 uses the potential particles rearranged in step S60 as potential particles of the previous sampling time, Obtain the potential particle.

By repeating steps S10 to S80 for each sampling time, the underwater robot's position estimation apparatus can continuously estimate the current position of the underwater robot in real time.

The method of the present invention can also be embodied as computer readable code on a computer readable recording medium. A computer-readable recording medium includes all kinds of recording apparatuses in which data that can be read by a computer system is stored. Examples of the computer-readable recording medium include a ROM, a RAM, a CD-ROM, a magnetic tape, a floppy disk, an optical data storage device, and the like, and may be implemented in the form of a carrier wave (for example, transmission via the Internet) . The computer readable recording medium can also be distributed over network coupled computer systems so that the computer readable code is stored and executed in a distributed fashion.

While the present invention has been particularly shown and described with reference to exemplary embodiments thereof, it is to be understood that the invention is not limited to the disclosed embodiments, but, on the contrary, is intended to cover various modifications and equivalent arrangements included within the spirit and scope of the invention. Accordingly, the scope of protection of the present invention should be construed in accordance with the following claims, and all technical ideas within the scope of equivalents and equivalents thereof should be construed as being covered by the scope of the present invention.

Internal sensor unit, 110 external sensor unit, 120
Potential position obtaining section, 130 reliability obtaining section, 14
Potential position rearranging unit, 150 positioning unit, 160

Claims (6)

An internal sensor unit for acquiring velocity information including information on a posture, a moving direction, and a moving velocity of the underwater robot at each sampling time;
An external sensor unit which senses an external environment that changes according to the movement of the underwater robot at each sampling time to obtain environmental information;
A potential position acquisition unit for acquiring potential particles in which the underwater robot is likely to be located at each sampling time;
A reliability acquiring unit for acquiring reliability of the obtained potential particles by using the environmental information as a random variable at each sampling time;
A latent location rearrangement unit for rearranging the obtained potential particles according to the reliability for each sampling time; And
And a positioning unit for calculating an average position of the rearranged latent particles at each sampling time, and determining the calculated average position as a current position of the underwater robot at each sampling time. The method of claim 1, wherein the speed information is
And a roll angular velocity, pitch angular velocity, yaw angular velocity, surge linear velocity, sway linear velocity, and hive linear velocity of the underwater robot. The apparatus of claim 1,
In the initial stage, potential particles are generated and acquired from the initial position of the underwater robot and the velocity information. In the initial stage, potential particles rearranged at the previous sampling time are moved according to the velocity information of the current sampling time to acquire potential particles Of the robot. (a) acquiring velocity information including information on the attitude, movement direction, and movement speed of the underwater robot at each sampling time;
(b) obtaining environmental information sensing an external environment that changes according to the movement of the underwater robot;
(c) obtaining latent particles that are likely to be located in the underwater robot;
(d) acquiring reliability of the obtained potential particles by using the environmental information as a random variable; And
(e) relocating each of the obtained potential particles according to the reliability; And
(f) calculating an average position of the rearranged latent particles, and determining the calculated average position as the current position of the underwater robot,
Wherein the position of the underwater robot is estimated in real time by repeating the steps (a) to (e) for each sampling time. The method of claim 4, wherein the speed information is
And a roll angular velocity, pitch angular velocity, yaw angular velocity, surge linear velocity, sway linear velocity, and hive linear velocity of the underwater robot. The method of claim 4, wherein step (c)
(c1) initially generating and acquiring potential particles from the initial position of the underwater robot and the velocity information; And
and (c2) moving the potential particles rearranged at the previous sampling time according to the speed information of the current sampling time after the initial sampling to obtain the latent particles.

Images