KR101784500B1 - Distance determination method for normally distributed obstacle avoidance of mobile robots in stochastic environments - Google Patents

Abstract

According to the present invention, there is provided a distance determining method for avoiding obstacles of a mobile robot, the method comprising: (a) calculating a position and a covariance of the mobile robot, a position and a covariance of the obstacle; (B) calculating a collision probability distribution density by expressing a relative collision probability distribution of the mobile robot and the obstacle as variables of a covariance and converting a coordinate system of the collision probability distribution to an eigenvector; Determining whether a position of the mobile robot exists outside the area of the obstacle using the collision probability distribution density, the distance between the position of the mobile robot and the distance of the Mahalanobis distance, )step; And (d) calculating a distance between the mobile robot and the obstacle using the Lagrangian multiplier if it is determined that the position of the mobile robot is outside the area of the obstacle in step (c) The movement path is calculated in consideration of the uncertainty of the position.

Description

TECHNICAL FIELD [0001] The present invention relates to a distance determining method for avoiding obstacles of a mobile robot in a probabilistic environment. [0002]

The present invention relates to a distance determination method for avoiding obstacles of a mobile robot, and more particularly, to a distance determination method capable of avoiding obstacles of a mobile robot in a stochastic environment in consideration of an uncertainty of an obstacle position.

The mobile robot should be able to move to the target point without collision with the obstacle. Many approaches to avoid obstacles have been proposed for mobile robots. If the position of the obstacle and the position of the robot are accurately determined, the robot can set the movement path without collision. However, it is general that the robot is controlled under an environment where there is noise during measurement. Accordingly, the robot must evaluate the position of the obstacle depending on the stochastic sensor model.

 The distribution of collision probabilities may be associated with a covariance to the location. However, since the collision probability distribution has no bounds, the robot can not maintain the intended distance from the obstacle. Therefore, the robot must possess the ability to determine the boundary conditions of the collision probability distribution, in which case the distance calculation may be calculated as the distance from the boundary. A conventional distance determination approach is as follows.

In the simplest approach, there is a way to calculate the distance without computing the covariance for the location. If it is possible to use high accuracy measurement equipment and sensors, it is the fastest and easiest solution to calculate distances without consideration of covariance in obstacle avoidance of mobile robot. However, the covariance is a parameter that considers the influence on the surrounding environment, and the movement or prediction of the obstacle increases the covariance. Given these environmental factors, it is desirable to consider other approaches that can take into account uncertainties in location.

In another approach, there is a method of calculating the collision probability distribution using an occupancy grid. Obstacles are represented within the occupancy grid, and the distance from the obstacle is expressed as the distance to the nearest occupied lattice unit. Unlike the first method described above, the occupancy grid is an approach considering the uncertainty of the position. However, the path calculation of the mobile robot using the occupancy grid has a disadvantage in that it takes a lot of time because of a large amount of computation.

As another approach, there is an error ellipsoid method. However, the error ellipse method is an approach that depends on the importance of statistical bases. The error ellipse method does not include a physical concept such as a distance or a collision probability, which causes a relatively large error in practical application.

The three representative approaches described above have limitations in determining the distance between the mobile robot and the obstacle. Due to the uncertainty of the position, the computation time and the collision probability must be calculated until the probability of collision can be ignored. The obstacle avoidance method is required to calculate the distance between the mobile robot and the obstacle in consideration of the environment.

Korean Patent Laid-Open No. 10-2011-0047844

Accordingly, the present invention provides a distance determination method for avoiding obstacles of a mobile robot taking into account uncertainty of a position. More specifically, the present invention provides a distance determination method capable of calculating a relative position and a covariance between a mobile robot and an obstacle, and calculating a distance from a relative distribution based on a specific collision probability density using a Lagrangian multiplier method I want to. In this case, it is desired to provide a distance determination method considering the collision probability with a small amount of computation and a high speed by calculating the calculation range and calculating the solution.

According to an aspect of the present invention, there is provided a distance determining method for avoiding an obstacle in a mobile robot, the method comprising: (a) calculating a position and a covariance of the mobile robot, a position and a covariance of the obstacle; (B) calculating a collision probability distribution density by expressing a relative collision probability distribution of the mobile robot and the obstacle as variables of a covariance and converting a coordinate system of the collision probability distribution to an eigenvector; Determining whether a position of the mobile robot exists outside the area of the obstacle using the collision probability distribution density, the distance between the position of the mobile robot and the distance of the Mahalanobis distance, )step; And (d) calculating a distance between the mobile robot and the obstacle using the Lagrangian multiplier if it is determined that the position of the mobile robot is outside the area of the obstacle in step (c) And the uncertainty of the position is taken into consideration.

Preferably, the step (c) may determine a threshold value of the distance of the Mahalanobis distance and the collision probability distribution density according to the following relational expression (1).

[Relation 1]

은 마할라노비스(Mahalanobis distance)의 거리, s는 상기 이동로봇의 상대적 위치, S는 상기 충돌확률 분포 밀도,

는 상기 충돌확률 분포 밀도의 역치를 의미한다.here,

Is the distance of the Mahalanobis distance, s is the relative position of the mobile robot, S is the collision probability distribution density,

Denotes a threshold value of the collision probability distribution density.

In the step (d), the shortest distance may be calculated according to the following equation (1) under the condition that the collision probability distribution density is higher than the threshold value in the step (c).

  [Equation 1]

는 상기 이동로봇과 상기 장애물 사이의 거리, s는 상기 이동로봇의 상대적 위치, S는 상기 충돌확률 분포 밀도,

는 상기 충돌확률 분포 밀도의 역치, e는 E에 포함된 점, E는 상기 장애물의 영역을 의미한다. here,

Is the distance between the mobile robot and the obstacle, s is the relative position of the mobile robot, S is the collision probability distribution density,

Is a threshold value of the collision probability distribution density, e is a point included in E, and E is a region of the obstacle.

Preferably, the step (d) uses a root-finding algorithm to calculate the solution of Equation (1), and the algorithm may be according to the following equation (2) have.

&Quot; (2) "

는 라그랑주 승수,

는 상기 충돌확률 분포 밀도의 역치, g는 제약조건 함수, f는 최적화 대상함수, e는 장애물의 영역에 포함된 점을 의미한다.Where L is the Lagrangian for (1), s is the relative position of the mobile robot, S is the collision probability distribution density,

The Lagrange multiplier,

G is a constraint function, f is an optimization object function, and e is a point included in the obstacle area.

Preferably, in the step (d), the calculation range condition of the expression (2) can be calculated according to the following expression (3).

&Quot; (3) "

는 상기 충돌확률 분포 밀도의 역치,

는 라그랑주 승수를 의미한다.Where g is the constraint function, s is the relative position of the mobile robot, S is the collision probability distribution density,

Is a threshold value of the collision probability distribution density,

Means the Lagrangian multiplier.

According to the present invention, the covariance between the robot and the obstacle is used as an input variable, and the distance considering the uncertainty of the position is calculated by the recursive algorithm by setting the conditions of the Lagrangian index and the calculation range. Accordingly, the present invention provides a method particularly suitable for setting the self-path control of the mobile robot and the self-path of the mobile robot since the distance is calculated in consideration of the probabilistic environment in the mobile robot which can only be controlled under the environment where noise exists. do.

Further, the method according to the present invention is a concept of distance calculation, and there is no need to consider the application separately. In this case, if the distance calculated according to the present invention is used instead of the Euclidean distance, a function considering the location uncertainty can be added to the existing path planning method. The mobile robot will be able to adjust the clearance distance according to the position estimation state of the obstacle and itself.

1 is a flowchart illustrating a method of determining a distance for avoiding an obstacle of a mobile robot according to an embodiment of the present invention.
FIG. 2 is a graph showing the relative positions of the robots relative to the respective regions and obstacle regions of the mobile robot and the obstacle expressed as a result of performing the step (a).
3 shows a simulation result of an algorithm of a distance determination method for obstacle avoidance of a mobile robot according to an embodiment of the present invention.
4 shows a display screen in which a starting point environment of a situation in which obstacles are randomly arranged for an experiment and a mobile robot is mapped on a computer.
FIG. 5 shows experimental results showing the movement path to the mobile robot path point and the target point, which are set to avoid obstacles in the experimental environment of FIG.

Hereinafter, the present invention will be described in detail with reference to the accompanying drawings. However, the present invention is not limited to or limited by the exemplary embodiments. Like reference numerals in the drawings denote members performing substantially the same function.

The objects and effects of the present invention can be understood or clarified naturally by the following description, and the purpose and effect of the present invention are not limited by the following description. In the following description, well-known functions or constructions are not described in detail since they would obscure the invention in unnecessary detail.

1 is a flowchart illustrating a method of determining a distance for avoiding an obstacle of a mobile robot according to an embodiment of the present invention. Referring to FIG. 1, a distance determination method for avoiding obstacles of a mobile robot according to an embodiment of the present invention includes calculating a covariance (step S10), calculating a collision probability distribution density (step S30) (C) determining (S50) a threshold value condition of the collision probability distribution density, and (d) calculating a distance between the mobile robot and the obstacle (S70).

The above steps (a) to (d) according to the embodiment of the present invention may be performed in the form of a program command which is executed by a computer means, which may be recorded in a computer-readable medium. The computer-readable medium includes program instructions, data files, data structures, and the like. The program instructions to be recorded on the medium may be designed for an embodiment or may be available to those skilled in the computer software. Examples of the computer-readable recording medium include a magneto-optical medium such as a hard disk, a floppy disk, a magnetic medium, an optical medium such as a CD-ROM and a DVD, a floppy disk, ), ROM (Read Only Memory), RAM (RAM), and flash memory. An example of a program command includes a high-level language code of a computer using an interpreter or the like. In this embodiment, steps (a) to (d) may be performed by a computer processor and stored in a storage medium on which a distance determination program for obstacle avoidance of the mobile robot is stored. Hereinafter, each step performed in the computer will be described.

(a) Step S10 may calculate the position, covariance, position and covariance of the mobile robot. FIG. 2 is a graph showing the relative positions of the robots with respect to the respective regions and obstacle regions of the mobile robot and the obstacle expressed as a result of performing (a) step (S10).

, 장애물의 위치는

로 나타낼 수 있다. 도 2의 (b)는 상대적인 위치를 나타낸 것으로, Relative position은

로서 푸른색 ‘+’로 표현되었고, 장애물의 영역인 Obstacle region은

로서 붉은색 타원형으로 표현되었다. 본 예시에서 상대적인 위치와 장애물 영역의 가장 가까운 거리는 0.0768[m]로 계산된다. N (r, R) represents the evaluated position of the mobile robot. Where 'N' is the normal distribution of multivariate, 'r' is the mean of the position vector and 'R' is the covariance matrix. Similarly, N (q, Q) represents the estimated location of the obstacle. 2 (a), the position of the mobile robot is

, The position of the obstacle

. 2 (b) shows a relative position, and the relative position is

And the Obstacle region, which is the area of the obstacle,

And was expressed as a red oval. In this example, the closest distance between the relative position and the obstacle area is calculated as 0.0768 [m].

), 허용 거리오차(

)가 될 수 있다. (a) Step S10 can be understood as a step of setting an input variable value required for distance calculation of the mobile robot. The parameters to be set for the distance calculation include the position of the mobile robot, the covariance (r, R), the obstacle position and the covariance (q, Q)

), Allowable distance error (

).

(b) Step (S30) can represent the relative collision probability distribution of the mobile robot and the obstacle as variables of the covariance, and convert the coordinate system of the collision probability distribution to the eigenvector to calculate the collision probability distribution density.

(b) In step S30, it is possible to express a relative collision probability distribution based on the position and the covariance N (r, R) of the mobile robot set in (a) and the obstacle position and the covariance N (q, Q) , Which can be expressed as N (rq, R + Q). Then, N (r-q, R + Q) is analyzed for collision avoidance.

(고유값 분해)로 표현될 수 있다. 여기서

이고, Q+R의 고유벡터 '

'를 구성성분으로 하는

의 스퀘어 행렬이다. 'S'는 충돌확률 분포 밀도이며

이고, 표준 편차

의 대각 행렬이다. 새로운 좌표계에서 'V'를 사용하면, 충돌 확률 분포는

로 표현될 수 있다. 여기서 '0'은 제로 벡터를 의미한다. 또한, 이동 로봇의 상대 위치는

로 표현될 수 있다. 이후, 충돌확률 분포 밀도는 's' 위치에서의 밀도를 의미한다. 실제로, 이동로봇은 항상 충돌확률 분포 밀도가 0이 아닌 값을 갖는다. 그러므로 이동로봇을 위치시킬 타겟 지점을 충돌확률 없이 설정시키기란 불가능하다. 이러한 상황에서, 높은 충돌 확률의 영역을 피하는 것으로 경로를 설정하는 것이 가장 바람직할 것이다. 이러한 영역은 충돌확률 분포 밀도의 역치 ‘

’로 설정된다. 만약 어느 위치이든 역치 ‘

’값보다 충돌확률 분포 밀도가 높다면, 이는 장애물이 존재하는 영역으로 이해될 수 있다. 상기의 비교 과정은 하기의 (c)단계(S50)에서 수행된다.For analysis, a new coordinate system is performed. The eigenvector of the covariance represents the collision probability distribution

(Eigenvalue decomposition). here

, And the eigenvector of Q + R '

&Quot;

. 'S' is the collision probability distribution density

And the standard deviation

Lt; / RTI > Using 'V' in the new coordinate system, the collision probability distribution is

. ≪ / RTI > Here, '0' means a zero vector. Further, the relative position of the mobile robot is

. ≪ / RTI > Hereinafter, the collision probability distribution density means the density at the 's' position. Actually, the mobile robot always has a value of the collision probability distribution density not equal to zero. Therefore, it is impossible to set the target point at which the mobile robot is to be placed without a collision probability. In this situation, it would be most desirable to set the path by avoiding areas of high collision probability. This region is the threshold of collision probability distribution density '

'. If any location is a threshold value,

If the collision probability distribution density is higher than the value, it can be understood as an area where an obstacle exists. The above comparison process is performed in the following step (c) (S50).

(c) Step (S50) can determine whether the position of the mobile robot exists outside the area of the obstacle by using the collision probability distribution density, the distance of the Mahalanobis between the position of the mobile robot and the threshold value of the collision probability distribution density.

(c) Step (S50) can determine the threshold value of the distance and collision probability distribution density of Mahalanobis according to the following relational expression 1.

[Relation 1]

은 마할라노비스(Mahalanobis)의 거리, s는 이동로봇의 상대적 위치, S는 충돌확률 분포 밀도,

는 충돌확률 분포 밀도의 역치를 의미한다. 장애물 영역의 경계는

의 마할라노비스 거리와 동일하므로, 상기의 [관계식]을 통해 이동로봇의 상대적 위치가 장애물 영역의 내외에 있는지 여부를 판단할 수 있다. here,

Mahalanobis distance, s the relative position of the mobile robot, S the collision probability distribution density,

Is the threshold value of the collision probability distribution density. The boundary of the obstacle area

It is possible to determine whether the relative position of the mobile robot is inside or outside the obstacle region through the above [relational expression].

(d) If it is determined in step (c) that the position of the mobile robot exists outside the area of the obstacle in step (c), the distance between the mobile robot and the obstacle can be calculated using the Lagrangian multiplier.

(d) Step (S70) can be performed according to the following equation (1) in the step (c) and the shortest distance under the condition that the collision probability distribution density is higher than the threshold value.

[Equation 1]

는 이동로봇과 장애물 사이의 거리, s는 상기 이동로봇의 상대적 위치, S는 충돌확률 분포 밀도,

는 충돌확률 분포 밀도의 역치, e는 E에 포함된 점, E는 상기 장애물의 영역을 의미한다. here,

S is the relative position of the mobile robot, S is the collision probability distribution density,

Is the threshold value of the collision probability distribution density, e is the point included in E, and E is the area of the obstacle.

’는 하기의 [관계식 2]의 조건을 만족한다.Here,

Satisfy the condition of [Relation 2] below.

[Relation 2]

는 장애물의 경계 영역을 의미한다. 장애물 E는 확률론적인 환경하에 주의가 요구되는 영역상에 부피를 갖고 존재한다. 만약,

값이 알려졌다면 상기의 [관계식 2]에 따라 역치의 범위가 결정된다.

값은 센서 모델 또는 실험을 통해서 획득될 수 있다.here,

Means the boundary region of the obstacle. The obstacle E exists in a volume on a region requiring attention under a stochastic environment. if,

If the value is known, the range of the threshold value is determined according to [Relation 2] above.

The value can be obtained through a sensor model or experiment.

(d) In step S70, a root-finding algorithm is used to calculate the solution of Equation (1), and the algorithm is as shown in Equation (2) below.

&Quot; (2) "

는 라그랑주 승수,

는 상기 충돌확률 분포 밀도의 역치, g는 제약조건 함수, f는 최적화 대상함수, e는 장애물의 영역에 포함된 점을 의미한다.Where L is the Lagrangian for (1), s is the relative position of the mobile robot, S is the collision probability distribution density,

The Lagrange multiplier,

G is a constraint function, f is an optimization object function, and e is a point included in the obstacle area.

(d) Step S70 can calculate the calculation range condition of the formula (2) according to the following formula (3).

&Quot; (3) "

는 상기 충돌확률 분포 밀도의 역치,

는 라그랑주 승수를 의미한다.Where g is the constraint function, s is the relative position of the mobile robot, S is the collision probability distribution density,

Is a threshold value of the collision probability distribution density,

Means the Lagrangian multiplier.

는

(조건 1)에서 연속적이고,

(조건 2)를 만족해야 한다. 따라서 [a, b]의 간격은 조건 1,2를 만족할 수 있도록 설정되어야 한다.That is, the root-finding algorithm for finding the solution of (2) finds a solution satisfying the condition within the calculation range of (3). In Equation (3)

The

(Condition 1)

(Condition 2). Therefore, the interval of [a, b] should be set to satisfy conditions 1 and 2.

An example of the algorithm for initializing the calculation range and calculating the above [a, b] is shown in [Figure 1] below.

[Figure 1]

Figure 2 shows an implementation of a root-finding algorithm that obtains a solution that satisfies conditions within the computation range initialized by the algorithm described above.

[Figure 2]

An embodiment of the algorithm for performing the steps (a) to (d), including the algorithm for calculating [a, b] in [Figure 1] and the root- finding algorithm in [Figure 2] ].

[Figure 3]

이고, 장애물의 위치는

이다. 이들의 확률분포는 도 3의 (a)에서 푸른 영역(이동로봇의 영역) 또는 붉은 영역(장애물의 영역)으로 표현되었다. 도 3의 (b)에서 이동로봇의 상대적 위치는

이고, 푸른색 ‘+’로 표현되었으며, 장애물의 영역은

이고 타원체로 표현되었다. 도 3의 (c)에서 [그림 1]의 [a, b]를 산출하기 위한 알고리즘을 사용하여 연산 범위(searching interval)를 표현하였으며, 도 3의 (d)에서 도 3(c)의 연산 범위 내의 조건으로 [그림 2]의 root-finding algorithm을 사용하여 라그랑주 승수를 산출한 것을 표현하였다. 3 shows simulation results of determining the distance for obstacle avoidance of the mobile robot according to the embodiment of the present invention by executing the above algorithm on a computer processor. In Fig. 3 (a), the position of the mobile robot is

And the position of the obstacle is

to be. These probability distributions are represented by a blue region (a region of a mobile robot) or a red region (an obstacle region) in FIG. 3A. In Fig. 3 (b), the relative position of the mobile robot is

, And is represented by a blue '+'. The area of the obstacle is

And expressed as an ellipsoid. 3 (c), a searching interval is expressed using an algorithm for calculating [a, b] in [Figure 1], and the calculation range of Figure 3 (c) The Lagrangian multiplier was calculated using the root-finding algorithm shown in [Figure 2].

By applying this algorithm, a distance determination method for obstacle avoidance of a mobile robot was experimented. The results are shown in FIGS. 4 and 5. 4 shows a display screen in which a starting point environment of a situation in which obstacles are randomly arranged for an experiment and a mobile robot is mapped on a computer. FIG. 5 shows experimental results showing the movement path to the mobile robot path point and the target point, which are set to avoid obstacles in the experimental environment of FIG. Referring to FIG. 5, the uncertainty of the position in the region of the obstacle is represented by an ellipsoid, and a path for avoiding the obstacle is determined by determining the closest point to the obstacle.

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 exemplary embodiments. will be. Therefore, the scope of the present invention should not be limited to the above-described embodiments, but should be determined by all changes or modifications derived from the scope of the appended claims and equivalents of the following claims.

S10: Step (a) of calculating the covariance
S30: Step (b) of calculating the collision probability distribution density
S50: Step (c) of determining a threshold value condition of the collision probability distribution density
S70: Step (d) of calculating the distance between the mobile robot and the obstacle

Claims (5)

A method for determining a distance for obstacle avoidance of a mobile robot,
(a) calculating a position and a covariance of the mobile robot, a position and a covariance of the obstacle;
(b) calculating a collision probability distribution density by expressing a relative collision probability distribution of the mobile robot and the obstacle as variables of covariance, converting a coordinate system of the collision probability distribution into an eigenvector,
(c) determining whether the position of the mobile robot exists outside the area of the obstacle by using the collision probability distribution density, the distance of the Mahalanobis distance between the position of the mobile robot and the threshold value of the collision probability distribution density ; And
(d) calculating a distance between the mobile robot and the obstacle using the Lagrangian multiplier if it is determined in the step (c) that the position of the mobile robot exists outside the area of the obstacle,
A method for distance determination for avoiding obstacles of a mobile robot characterized by uncertainty of position.
The method according to claim 1,
The step (c)
And determining a threshold value of the distance of the Mahalanobis distance and the threshold value of the collision probability distribution density according to the following relational expression (1).
[Relation 1]

here,

Is the distance of the Mahalanobis distance, s is the relative position of the mobile robot, S is the collision probability distribution density,

Denotes a threshold value of the collision probability distribution density.
The method according to claim 1,
The step (d)
Wherein the shortest distance is calculated according to Equation (1) under the condition that the collision probability distribution density is higher than the threshold value in the step (c).
[Equation 1]

here,

Is the distance between the mobile robot and the obstacle, s is the relative position of the mobile robot, S is the collision probability distribution density,

Is a threshold value of the collision probability distribution density, e is a point included in E, and E is a region of the obstacle.
The method of claim 3,
The step (d)
(1), a root-finding algorithm is used to calculate the solution of Equation (1), and the algorithm is according to Equation (2) below. Distance Determination Method.
&Quot; (2) "

Where L is the Lagrangian for (1), s is the relative position of the mobile robot, S is the collision probability distribution density,

The Lagrange multiplier,

G is a constraint function, f is an optimization object function, and e is a point included in the obstacle area.
5. The method of claim 4,
The step (d)
Wherein the calculation range condition of the equation (2) is calculated according to the following equation (3): " (3) "
&Quot; (3) "

Where g is the constraint function, s is the relative position of the mobile robot, S is the collision probability distribution density,

Is a threshold value of the collision probability distribution density,

Means the Lagrangian multiplier.

Images