KR20200072363A - Method for controlling a moving path of a moving vehicle to prevent a collision with an obstacle and apparatus using the same - Google Patents

Abstract

The present invention relates to a method for controlling the movement path of a moving object to prevent a collision with an obstacle. The method includes: (a) a step wherein a moving object control apparatus obtains information on the speed and radius size of the obstacle, obtains a collision risk angle reflecting the radius size of the obstacle and the radius size of the moving object for a reference line, and obtains a relative speed reflecting the speed of the moving object and the speed of the obstacle and a relative angle corresponding to the relative speed for the reference line when it is assumed that the reference line includes the positions of the moving object and the obstacle with the obstacle detected which is positioned within a first predetermined distance from the moving object; and (b) a step wherein the moving object control apparatus calculates the heading angular velocity of the moving object and controls the movement path of the moving object with a first algorithm based on the geometric relationship between the moving object and the obstacle as a first mode for avoiding the collision with the obstacle when (i) the relative angle is less than 90 degrees, (ii) the collision risk angle exceeds the relative angle, and (iii) the distance between the moving object and the obstacle is equal to or less than a second predetermined distance.

Description

METHOD FOR CONTROLLING A MOVING PATH OF A MOVING VEHICLE TO PREVENT A COLLISION WITH AN OBSTACLE AND APPARATUS USING THE SAME}

The present invention relates to a method for controlling a moving path of a moving object to prevent collision with an obstacle, and more specifically, in a state in which the obstacle located at a first predetermined distance from the moving object is detected, and the position of the moving object Assuming that the line including the position of the obstacle is a reference line, the moving object control device obtains the radius size and speed information of the obstacle, and considers the radius size of the moving object and the radius size of the obstacle around the reference line Obtaining a dangerous angle, obtaining a relative speed in consideration of the speed of the moving object and the speed of the obstacle, and a relative angle corresponding to the relative speed based on the reference line; And i) the size of the collision risk angle is less than 90°, ii) the size of the collision risk angle is greater than the size of the relative angle, and iii) when the distance between the moving object and the obstacle is within a second predetermined distance. , The moving object control device, as a first mode for avoiding collision with the obstacle, calculates a heading angular velocity of the moving object through a first algorithm based on a geometrical relationship between the moving object and the obstacle, and the moving path of the moving object It relates to a method comprising the step of controlling.

Vehicles such as drones (multicopters) have been steadily gaining popularity from users as a hobby, and in the future, drones (multicopters) can be utilized even more in the industrial field.

When multiple drones fly in the air at the same time, collisions between drones or collisions between drones and other obstacles may occur frequently. The drone collision is not only the primary damage that the drone is broken, but also the secondary damage can occur as the damaged drone falls from the air, so a technique is needed to prevent this. For reference, the drone's obstacle avoidance technology is an essential part of automatic flight.

Most of the existing obstacle avoidance technology has been achieved through a vision sensor, but the sensor-based product is not only expensive, but also has a visible distance of only 50M. In addition, in the past, only a simple avoidance algorithm was used, and the significance of avoiding obstacles was not considered, and the optimal route was not selected.

Accordingly, the present inventor proposes a method of controlling a moving path of a moving object in order to prevent collision with an obstacle.

The present invention aims to solve all of the above-mentioned problems.

Another object of the present invention is to enable the mobile body to fly to a destination point while avoiding static/dynamic obstacles.

In addition, another object of the present invention is to enable the mobile body to automatically avoid obstacles, so that it can be used in many fields such as hobby/entertainment, disaster disaster monitoring, and agricultural industry.

In addition, another object of the present invention is to deviate from an avoidance algorithm for simply avoiding an obstacle, and to select an effective path to a destination by making a smaller trajectory and avoiding an obstacle through simple and less calculation.

In order to achieve the object of the present invention as described above and to realize the characteristic effects of the present invention described later, the characteristic configuration of the present invention is as follows.

According to an aspect of the present invention, in a method of controlling a moving path of a moving object to prevent collision with an obstacle, in the state in which the obstacle located at a first predetermined distance from the moving object is detected, the position of the moving object and Assuming that the line including the position of the obstacle is a reference line, the moving object control device obtains the radius size and speed information of the obstacle, and considers the radius size of the moving object and the radius size of the obstacle around the reference line Obtaining a risk angle, obtaining a relative speed in consideration of the speed of the moving object and the speed of the obstacle and a relative angle corresponding to the relative speed with respect to the reference line, and i) the magnitude of the collision risk angle is 90° Smaller, ii) the size of the collision risk angle is greater than the size of the relative angle, and iii) when the distance between the movable body and the obstacle is within a second predetermined distance, the movable body control device collides with the obstacle As a first mode for avoiding, a method is provided comprising calculating a heading angular velocity of the moving object and controlling a moving path of the moving object through a first algorithm based on a geometrical relationship between the moving object and the obstacle.

In addition, according to another aspect of the present invention, an apparatus for controlling a movement path of a moving object to prevent collision with an obstacle, the communication unit; And a line including the position of the moving object and the position of the obstacle as a reference line in a state in which the obstacle located within a first predetermined distance from the moving object is sensed by the sensor unit. Obtain radius size and speed information, obtain a collision risk angle considering the radius size of the moving object and the radius size of the obstacle around the reference line, and obtain the relative speed and the reference line considering the speed of the moving object and the speed of the obstacle. Obtaining a relative angle corresponding to the relative speed as a center, i) the size of the relative angle is less than 90°, ii) the size of the collision risk angle is greater than the size of the relative angle, and iii) the moving object and the When the distance between obstacles is within a second predetermined distance, as a first mode for avoiding collision with the obstacle, the heading angular velocity of the mobile body is calculated through a first algorithm based on the geometrical relationship between the mobile body and the obstacle. And a processor for controlling a movement path of the moving object.

According to the present invention, there are the following effects.

The present invention has the effect that the moving object can fly to the destination point while avoiding static/dynamic obstacles.

In addition, the present invention has the effect of enabling the mobile body to automatically avoid obstacles, so that it can be used in many fields such as hobby/entertainment, disaster disaster monitoring, and agricultural industry.

In addition, the present invention has an effect of deviating from an existing avoidance algorithm for simply avoiding obstacles, and selecting an effective route to a destination by drawing a smaller trajectory and avoiding obstacles through simple and less calculation.

1 is a view showing a schematic configuration of a moving object control device according to an embodiment of the present invention.
FIG. 2 is a diagram schematically showing a process in which a moving object moves along an optimal path to a target point while avoiding obstacles according to an embodiment of the present invention.
3 is a view showing a geometric relationship between a moving object and an obstacle according to an embodiment of the present invention.
4 is a view showing a case in which a moving object avoids a collision with an obstacle according to an embodiment of the present invention.
5 is a view showing a trajectory until a moving object avoids an obstacle and reaches a destination according to an embodiment of the present invention.
FIG. 6 is a view showing a change in a collision detection angle and a relative angle related to a moving object according to an embodiment of the present invention.
7 is a view showing a posture angle and a heading angle of a moving object according to an embodiment of the present invention.
8 is a view showing a relative distance between a moving object and an obstacle according to an embodiment of the present invention.
9 is a view showing the angular velocity of the heading of the moving object according to an embodiment of the present invention.
10 is a view showing the speed of the moving object according to an embodiment of the present invention.
11 is a view showing a path of a moving object according to an embodiment of the present invention.

For a detailed description of the present invention, which will be described later, reference is made to the accompanying drawings that illustrate, by way of example, specific embodiments in which the invention may be practiced. These examples are described in detail enough to enable those skilled in the art to practice the present invention. It should be understood that the various embodiments of the invention are different, but need not be mutually exclusive. For example, certain shapes, structures, and characteristics described herein may be implemented in other embodiments without departing from the spirit and scope of the invention in relation to one embodiment. In addition, it should be understood that the location or placement of individual components within each disclosed embodiment can be changed without departing from the spirit and scope of the invention. Therefore, the following detailed description is not intended to be taken in a limiting sense, and the scope of the present invention, if appropriately described, is limited only by the appended claims, along with all ranges equivalent to those claimed. In the drawings, similar reference numerals refer to the same or similar functions across various aspects.

Hereinafter, preferred embodiments of the present invention will be described in detail with reference to the accompanying drawings in order to enable those skilled in the art to easily implement the present invention.

1 is a view showing a schematic configuration of a moving object control device according to an embodiment of the present invention.

As shown in FIG. 1, the moving object control apparatus 100 of the present invention includes a communication unit 110 and a processor 120, and in some cases, unlike FIG. 1, may include a sensor unit 130. In addition, although not shown, the database may also be included in the moving object control device 100.

First, the communication unit 110 of the mobile body control device 100 may be implemented by various communication technologies. That is, Wi-Fi (WIFI), WCDMA (Wideband CDMA), HSDPA (High Speed Downlink Packet Access), HSUPA (High Speed Uplink Packet Access), HSPA (High Speed Packet Access), Mobile WiMAX, WiBro , LTE (Long Term Evolution), Bluetooth (bluetooth), infrared communication (IrDA, infrared data association), NFC (Near Field Communication), Zigbee (Zigbee), WLAN technology, etc. may be applied. In addition, when providing a service connected to the Internet, it is possible to follow TCP/IP, a standard protocol for information transmission on the Internet.

Next, the processor 120 of the present invention includes two control parts, such as a heading angular speed control unit 120-1 and a position (velocity) control unit 120-2. Can be controlled. For reference, although not shown in the drawings, a multiple loop (outer loop, inner loop) may be used in the processor 120, and the angular velocity or movement position control may be performed in the outer loop. Also, a PID controller can be used in the inner loop. The processor 120 will be described in detail through a detailed description later.

In addition, the sensor unit 130 can be accessed by the communication unit 110 of the mobile body control device 100. In principle, the sensor unit 130 in the present invention includes a LIDAR sensor, and does not limit the inclusion of other sensors such as a VISION sensor. The sensor unit 130 may detect the movement of an obstacle located at a certain distance from the moving object, and this will be described later with a description of the processor 120.

Meanwhile, the mobile body described in the present invention may include a drone, a multi-copter remotely controlled mobile body, as well as a drone, a manned aircraft, a hot air balloon, and the like. In addition, the obstacle described in the present invention includes both static objects and dynamic objects, and the object is not particularly limited. The collision avoidance algorithm of the present invention with respect to the moving object and the obstacle is searching for the fastest direction and the shortest path to avoid the obstacle and reach the destination.

In the case of the collision avoidance algorithm, it is not a simple equation, but a combination of geometric constraints and motion equations. That is, the first algorithm is used in the first mode, which is the operation mode for the moving object to avoid the obstacle, and the second algorithm is used in the second mode, the operation mode in which the moving object avoids the obstacle and the angular velocity is changed to reach the target point. It is used, and the third algorithm may be used in the third mode, which is an operation mode in which the moving object completely moves out of the obstacle moving area and reaches an optimal path to a target point.

Hereinafter, conditions for classifying the first, second, and third modes and algorithms in each mode will be described.

FIG. 2 is a diagram schematically showing a process in which a moving object moves along an optimal path to a target point while avoiding obstacles according to an embodiment of the present invention.

First, the processor 120 may sense an obstacle positioned within a first predetermined distance from the moving object through the sensor unit 130. Here, the first predetermined distance may vary depending on the performance of the sensor included in the sensor unit 130. After detecting the obstacle, the processor 120 may acquire geometric information between the obstacle and the moving object through the communication unit 110 and the sensor unit (S210 ). The geometric information will be described in more detail with reference to FIG. 3.

3 is a view showing a geometric relationship between a moving object and an obstacle according to an embodiment of the present invention.

는 이동체의 속도이며,

는

에 대응되는 이동체의 heading 각도를 의미한다. 또한,

는 이동체의 반지름 크기를 나타내고 있다. Looking at the variable related to the moving object shown in Figure 3, M corresponds to the current position of the moving object,

Is the velocity of the moving object,

The

It means the heading angle of the moving object corresponding to. Also,

Indicates the radius of the mobile body.

는 장애물의 속도이며,

는

에 대응되는 장애물의 heading 각도를 의미한다. 또한,

는 장애물의 반지름 크기를 나타내고 있다.Looking at the obstacle-related variables shown in Figure 3, O means the current position of the obstacle,

Is the speed of the obstacle,

The

It means the heading angle of the obstacle corresponding to. Also,

Indicates the radius of the obstacle.

,

의 방향 모두 좌측으로 향하고, 도 3(c)(d)는

,

의 방향 모두 우측으로 향한다. 또한,

의 방향에 따라 도 3(a)와 도 3(b)를 구분하고, 도 3(c)와 도 3(d)를 구분할 수 있다.Meanwhile, it may be assumed that a line including the position of the moving object and the position of the obstacle is a reference line, and in FIG. 3, the straight line MO will correspond to the reference line. At this time, based on the reference line, Figure 3 (a) (b) is

,

All of the directions are directed to the left.

,

All of the directions are towards the right. Also,

3(a) and 3(b), and 3(c) and 3(d).

The processor 120 may acquire each variable as described above through the communication unit 110 and the sensor unit 120. Of course, the speed, radius size, and heading angle of the moving body can also be checked without going through the sensor unit 120. The processor 120 may acquire radius size and speed information of the sensed obstacle, and may use it to obtain a collision risk angle and a relative angle.

Specifically, when the sum of the radius size of the obstacle and the radius size of the moving object is R, a collision occurs when the distance between the center of the moving object and the center of the obstacle is smaller than R. Accordingly, an angle formed by the circle having R as the radius (radius size of the movable body + radius of the obstacle) and the current position M of the movable body may be designated as a collision risk angle.

That is, the processor 120 may acquire a collision risk angle in consideration of the radius size of the moving object and the radius size of the obstacle around the reference line MO. The angle of collision risk in FIG. 3 corresponds to β+ or β-.

)를 생성할 수 있다. 이에 대한 관련 식은

(식 1)와 같다. 또한, 기준선을 중심으로 상기 상대 속도에 대응하는 상대 각도 역시 획득될 수 있는 바, 도 3에서의 상대 각도는 α에 해당한다. In addition, the processor 120 considers the speed of the moving object and the speed of the obstacle in consideration of the relative speed (

). The related expression for this

It is the same as (Equation 1). In addition, a relative angle corresponding to the relative speed around the reference line can also be obtained, and the relative angle in FIG. 3 corresponds to α.

When the relative angle α is greater than or equal to 0, in order to avoid collision with the obstacle, the moving object follows the danger angle β+, and when the relative angle α is less than 0, the moving object is obstructed. In order to avoid collision with, the relative angle α must follow the risk of collision β-. It will be described below in more detail with FIG. 4.

4 is a view showing a moving object after avoiding collision with an obstacle.

또는

(참고로,

와

는 동일)를 이동체의 속도 벡터

에 투영하였을 때, 각각 서로 반대 방향인 경우, 이동체와 장애물의 충돌은 회피된 것으로 볼 수 있다. 구체적으로, 상기 내용을 식으로 나타내면 아래와 같다.As can be seen in Figure 4, the vector from the center of the moving object (M) to the center of the obstacle (O)

or

(Note that,

Wow

Is the same) velocity vector of the moving object

When projected on, it can be considered that collision of the moving object and the obstacle is avoided when they are in opposite directions. Specifically, the above contents are expressed as follows.

(식 2)

(Equation 2)

Here, since σ is a scalar amount, if cos α<0 is satisfied, collision of the moving object and the obstacle will be avoided. Since the algorithm for avoiding collision must be activated when there is a possibility of collision between the moving object and the obstacle, the relative angle α is activated when the following relationship is satisfied.

(식 3)

(Equation 3)

Consequently, the collision avoidance algorithm may be activated only when the size of the relative angle α is less than 90° and the size of the collision risk angles β+ and β- is greater than the size of the relative angle α.

In addition, when the distance between the moving object and the obstacle is long, the moving object does not need to immediately avoid the obstacle. Accordingly, the collision avoidance algorithm may be limited to being activated when the distance between the moving object and the obstacle is within a second predetermined distance. Here, the second predetermined distance will be smaller than the first predetermined distance, which is an obstacle detecting section.

As described above, when i) the size of the relative angle is less than 90°, ii) the size of the collision risk angle is greater than the size of the relative angle, and iii) when the distance between the moving object and the obstacle is within a second predetermined distance, The processor 120 is a first mode for avoiding collision with an obstacle, and calculates a heading angular velocity of the moving object through a first algorithm based on the geometrical relationship between the moving object and the obstacle and controls the moving path of the moving object (S220) )can do.

The process for obtaining the first algorithm for processing the heading angular velocity of the moving object so that the relationship between the relative angle α and the collision risk angle β satisfies α≥β or α≤ β- is as follows.

First, the equations of the moving object and Cartesian coordinates are as follows.

In addition, the equation of the relationship between obstacles and Cartesian coordinates is as follows.

(참고로, 도 3(a)(b)의 경우)일 때의 각도

는 아래와 같이 계산된다. 참고로,

는 이동체의 heading 각도를 의미하고,

는 수평선을 기준으로 상대 속도인

에 대응되는 각도에 해당한다.Also,

(For reference, in the case of Fig. 3 (a) (b)) angle

Is calculated as follows. Note that,

Means the heading angle of the moving object,

Is relative velocity relative to the horizon

Corresponds to the angle corresponding to.

이라 하면

Speaking of

Applying the sign law

에서

.

in

.

는 아래와 같이 계산된다.Distance between moving object and moving obstacle

Is calculated as follows.

를 트래킹 오차라 하면

.

Is the tracking error

.

here

.

.

Taking into account the Lyapunov function

를 얻기 위한 ω는negative

Ω to get

(식 4) 여기서,

는 양수이다.

(Equation 4) Here,

Is positive.

(참고로, 도 3(c)(d)의 경우)일 때의 관계식은 다음과 같다.Also,

(For reference, in the case of FIG. 3(c)(d)), the relational expression is as follows.

(식 5) 여기서,

는 양수이다.

(Equation 5) where:

Is positive.

As described above, in the case of different conditions, in FIGS. 3(a) to 3(d), Equation 4 (Equation 5) is derived, which corresponds to the first algorithm. That is, the processor 120 may calculate the heading angular velocity ω of the moving object to avoid collision with the obstacle through the first algorithm, and control the moving path of the moving object.

For reference, even when the heading angular velocity is changed according to the first algorithm, the velocity of the moving object may be constant.

5 is a view showing a trajectory until a moving object avoids an obstacle and reaches a destination point.

,

의 방향 모두 좌측으로 향하고, 도 5(c)(d)는

,

의 방향 모두 우측으로 향한다. 또한,

의 방향에 따라 도 5(a)와 도 5(b)를 구분하고, 도 5(c)와 도 5(d)를 구분할 수 있다.For reference, FIGS. 5(a) to 5(d) are similar to FIGS. 3(a) to 3(d), based on the reference line MO, FIGS. 5(a)(b)

,

All of the directions are toward the left, and Fig. 5(c)(d) is

,

All of the directions are towards the right. Also,

5(a) and 5(b) may be distinguished according to the direction of, and FIG. 5(c) and 5(d) may be distinguished.

Referring to FIG. 5, it can be seen that the moving object moves according to the curve QMT with S as a starting point and T as a target point. Here, when the center of the moving object is located at the point Q, the relative angle α generated from the relationship between the moving object and the obstacle corresponds to 90°. As described above, since the collision avoidance algorithm is activated when the relative angle α is smaller than the size of 90°, the collision avoidance algorithm will not be activated at point Q.

That is, i) when the relative angle is greater than or equal to 90° and less than or equal to 180°, or ii) greater than or equal to -180° and less than or equal to -90°, the mobile body centered in Q is satisfied. It can be determined that collision with an obstacle is avoided.

Therefore, the processor 120 is a second mode for reaching the target point T from the point after the point Q, and newly calculates the heading angular velocity of the moving object through the second algorithm based on the geometrical relationship between the moving object and the target point, and The movement path may be controlled (S230).

Here, the second algorithm is as follows.

(식 6) 여기서,

는 양수이다.

(Eq. 6) where:

Is positive.

As a result, the processor 120 may move at a constant speed while drawing a curve QM according to the heading angular velocity ω according to Equation 6. For reference, when the center of the moving object is located at the point M, it may be determined that it is outside the moving area of the obstacle, which will be described below.

Baseline and obstacle with a relative angle greater than or equal to 90° and less than or equal to 180° or a case greater than or equal to -180° and less than or equal to -90° at a specific point When the angle between the moving directions is p and the distance between the moving object and the obstacle is D, the moving object at a specific point when D * sin p ≥ R (radius of the moving object + radius of the obstacle) is the moving of the obstacle. It can be determined that it is located outside the area.

(

)의 역벡터와 벡터

의 역벡터 사이의 각도

라고 보고, D는 벡터

(

)의 크기에 해당하므로 결국,

를 만족해야 할 것이며, 도 5에서는 이동체의 중심이 점 M에 위치할 때 상기 조건을 만족하는 것으로 판단된다.In other words, referring to FIG. 5, vector p

(

) Inverse vector and vector

Angle between inverse vectors of

And D is a vector

(

), so in the end,

5, it is determined that the above conditions are satisfied when the center of the moving object is located at point M.

When it is determined that the center of the moving object is located at the specific point, the processor 120 is a third mode for reaching the target point T, and the moving object is moved through a third algorithm based on the geometrical relationship between the moving object and the target point. The position to be newly calculated, and the moving path of the moving object can be controlled (S240).

For reference, in FIG. 5, when the center of the moving object is located at point M, it is determined that it is outside the moving area of the obstacle. Therefore, the moving object from point M will be assigned a position to be moved according to the position (speed) control unit 120-2. At this time, the processing by the heading angular velocity control unit 120-1 may also be possible in some cases.

는 다음과 같이 계산된다. In the third mode, the processor 120 may first control the moving object to reach the target point at a constant speed. For reference, the constant speed of the moving object may be the same as the speed of the moving object in the first mode and the second mode (for reference, the angular speed is changed only in the first and second modes, and the speed is the same). Error considered at this time

Is calculated as follows.

(식 7) 여기서,

(Equation 7) Here,

는 다음과 같이 계산된다.Further, the processor 120 may decrease the speed to stop the moving object at the target point when the moving object is close to the target point. For example, the speed of the moving object may be reduced when the distance between the moving object and the target point is detected to be within 1 m. Error considered at this time

Is calculated as follows.

(식 8)

(Equation 8)

The processor (that is, the position (velocity) control unit 120-2) in the third mode is capable of controlling the moving path of the moving object through the third algorithm using Equation 7, Equation 8.

6 to 11 show results of experiments related to movement of a moving object when the collision avoidance algorithm is used.

First, FIG. 6(a) shows the change of the collision detection angle and the relative angle when the obstacle is static, and FIG. 6(b) shows the change of the collision detection angle and the relative angle when the obstacle is dynamic.

Fig. 7(a) shows the posture angle and heading angle of the mobile body while the obstacle is static, and Fig. 7(b) shows the posture angle and heading angle of the mobile body while the obstacle is dynamic.

FIG. 8(a) shows the relative distance between the moving object and the obstacle in a static state, and FIG. 8(b) shows the relative distance between the moving object and the obstacle in a dynamic state.

Fig. 9(a) shows the heading angular velocity of the mobile body in the state where the obstacle is static, and Fig. 9(b) shows the heading angular velocity of the mobile body in the state where the obstacle is dynamic.

FIG. 10(a) shows the speed of the mobile body in a state where the obstacle is static, and FIG. 10(b) shows the speed of the mobile body in a state where the obstacle is dynamic.

Fig. 11(a) shows the path of the mobile body in a state where the obstacle is static, and Fig. 11(b) shows the path of the mobile body in the state where the obstacle is dynamic.

The embodiments according to the present invention described above may be implemented in the form of program instructions that can be executed through various computer components and can be recorded in a computer-readable recording medium. The computer-readable recording medium may include program instructions, data files, data structures, or the like alone or in combination. The program instructions recorded on the computer-readable recording medium may be specially designed and configured for the present invention or may be known and usable by those skilled in the computer software field. Examples of computer-readable recording media include magnetic media such as hard disks, floppy disks, and magnetic tapes, optical recording media such as CD-ROMs, DVDs, and magneto-optical media such as floptical disks. media), and hardware devices specifically configured to store and execute program instructions such as ROM, RAM, flash memory, and the like. Examples of program instructions include not only machine language codes produced by a compiler, but also high-level language codes that can be executed by a computer using an interpreter or the like. The hardware device may be configured to operate as one or more software modules to perform processing according to the present invention, and vice versa.

In the above, the present invention has been described by specific matters such as specific components and limited embodiments and drawings, but this is provided only to help a more comprehensive understanding of the present invention, and the present invention is not limited to the above embodiments , Those skilled in the art to which the present invention pertains can make various modifications and variations from these descriptions.

Therefore, the spirit of the present invention is not limited to the above-described embodiment, and should not be determined, and all claims that are equally or equivalently modified as well as the claims below will fall within the scope of the spirit of the present invention. Would say

100: moving object control device
110: communication department
120: processor
120-1: Heading angular speed control
120-2: Position (speed) control
130: sensor unit

Claims (6)

In the method of controlling the movement path of the moving object to prevent the collision with the obstacle,
(a) When the obstacle located within a first predetermined distance from the moving object is sensed, assuming that the line including the position of the moving object and the position of the obstacle is a reference line, the moving object control device, the radius of the obstacle Obtain size and speed information, obtain a collision risk angle considering the radius size of the moving object and the radius size of the obstacle around the reference line, and center the relative speed and the reference line considering the speed of the moving object and the speed of the obstacle Obtaining a relative angle corresponding to the relative speed; And
(b) i) the size of the relative angle is less than 90°, ii) the size of the collision risk angle is greater than the size of the relative angle, and iii) the distance between the moving object and the obstacle is within a second predetermined distance. At this time, the moving object control device, as a first mode for avoiding collision with the obstacle, calculates the heading angular velocity of the moving object through a first algorithm based on the geometrical relationship between the moving object and the obstacle and moves the moving object Controlling the path;
How to include. According to claim 1,
(c) i) when the relative angle is greater than or equal to 90° and less than or equal to 180°, or ii) when greater than or equal to -180° and less than or equal to -90°, the movable body control device comprises: A second mode for reaching the target point by considering collision between the moving object and the obstacle as avoided, and newly calculating the heading angular velocity of the moving object through a second algorithm based on the geometrical relationship between the moving object and the target point And controlling the moving path of the moving object. According to claim 2,
(d) i-1) the relative angle is greater than or equal to 90° and less than or equal to 180°, or i-2) greater than or equal to -180° and less than or equal to -90°, and ii) When the angle between the reference line and the obstacle moving direction is p, and the distance between the moving object and the obstacle is D, the calculated value of D * sin p is the radius size of the moving object and the radius size of the obstacle If greater than or equal to the sum, the moving object control device considers that the current position of the moving object is outside the moving area of the obstacle and as the third mode for the moving object to reach the target point, the moving object and the The method further includes calculating a position where the moving object moves through a third algorithm based on a geometrical relationship between target points and controlling a moving path of the moving object. According to claim 3,
The moving object control device controls the moving object to reach the target point at a constant speed in the third mode, and when the moving object is within a certain distance from the target point, the moving object is stopped at the target point Method of reducing the speed of the. According to claim 1,
The obstacle is characterized by being detected by a LIDAR sensor or a vision sensor. In the apparatus for controlling the movement path of the moving object to prevent the collision with the obstacle,
Communication; And
When the obstacle located within the first predetermined distance from the mobile body is sensed by the sensor unit, assuming that the line including the location of the mobile body and the location of the obstacle is a reference line, the radius of the obstacle through the communication unit Obtain size and speed information, obtain a collision risk angle considering the radius size of the moving object and the radius size of the obstacle around the reference line, and center the relative speed and the reference line considering the speed of the moving object and the speed of the obstacle To obtain a relative angle corresponding to the relative speed, i) the size of the relative angle is less than 90°, ii) the size of the collision risk angle is greater than the size of the relative angle, and iii) the moving object and the obstacle As a first mode for avoiding collision with the obstacle when the distance between them is within a second predetermined distance, the heading angular velocity of the moving object is calculated through a first algorithm based on the geometrical relationship between the moving object and the obstacle, A processor that controls a moving path of the moving object;
Moving object control device comprising a.

Images