KR100557440B1 - walking robot and control method thereof - Google Patents

Abstract

)와 상기 제1 기준점(A)으로부터 ZMP(Z)에 이르는 제2 벡터(

)의 외적의 크기(K)를 산출하는 단계와, 산출된 외적의 크기에 따라 상기 구동로봇의 움직임을 제어하는 단계를 포함하는 것을 특징으로 한다. 이에 의해 보행로봇의 안정도를 간단하게 판별할 수 있다.The present invention relates to a walking robot and a control method thereof. In the control method of the walking robot having a plurality of feet, the control method of the present invention selects a first reference point (A) and a second reference point (B) on a dangerous boundary line among the stable areas including the area between the feet in contact with the ground. Calculating a ZMP of the walking robot, and a first vector from the first reference point A to the second reference point B

) And a second vector from the first reference point A to ZMP (Z)

Calculating a magnitude (K) of the cross product and controlling the movement of the driving robot according to the calculated size of the cross product. As a result, the stability of the walking robot can be easily determined.

Stability Determination

Description

Walking robot and control method

1 is a partial perspective view of a walking robot in which both feet are in contact with the ground during walking;

2 is a schematic block diagram of a walking robot according to an embodiment of the present invention;

3 (a) and 3 (b) show a stable area formed by a walking robot,

4 is a flowchart for determining stability according to an embodiment of the present invention;

5 (a) to 5 (c) show the sign of the magnitude K of the cross product according to the relative position between the segment and ZMP.

The present invention relates to a walking robot and a control method thereof, and more particularly, to a walking robot for determining stability by a simple algorithm and a control method thereof.

Walking robots are inferior in stability to robots moving by wheels, and there are technical difficulties in controlling stability so that they are continuously maintained.

In the case of a walking robot having four or more legs, it is possible to consider stability using the center of gravity. However, in the case of biped walking robot, it is very important to consider the stability in the dynamic state. For this purpose, the concept of Zero Moment Point (ZMP) is used, which represents a point on the ground where the sum of all moments of the force is zero.

1 is a partial perspective view of a walking robot in which both feet abut the ground during walking.

As shown in Fig. 1, the areas of the two feet and the two launchers, which contact the ground, form a convex hull. The convex hull formed by the hatching formed at this time is a stable region.

Stability is determined from the relative position between ZMP and the stable region. That is, the robot is determined to be in a stable state when the ZMP is in the stable region, and the robot is determined to be in an unstable state when it is outside the stable region.

However, the conventional walking robot solved the complex motion equation to determine the stability of the robot posture and walking state. The controller for controller could be overloaded because it performed many operations in solving the equation of motion. This causes the walking robot to make it difficult to control the walking robot in real time.

Accordingly, an object of the present invention is to provide a walking robot and its control method for determining stability through simple calculations.

)와 상기 제1 기준점(A)으로부터 ZMP(Z)에 이르는 제2 벡터(

)의 외적의 크기(K)를 산출하는 단계와, 산출된 외적의 크기에 따라 상기 보행로봇의 움직임을 제어하는 단계를 포함하는 것을 특징으로 하는 보행로봇의 제어방법에 의해 달성될 수 있다.The above object is, according to the present invention, in the method of controlling a walking robot having a plurality of feet, the first reference point (A) and the second reference point on the dangerous boundary line of the stable area including the area between the feet in contact with the ground ( Selecting B) and calculating a ZMP of the walking robot, and a first vector (from the first reference point A to the second reference point B)

) And a second vector from the first reference point A to ZMP (Z)

Calculating the size (K) of the cross product and controlling the movement of the walking robot according to the calculated size of the cross product.

Here, the size K of the cross product may be calculated by the following equation.

로부터 반시계방향으로

에 이르는 각도이다.)Here, the coordinate of the first reference point A is (x _a , y _a ), the coordinate of the second reference point B is (x _b , y _b ), and the coordinate of ZMP is (x _z , y _z ), θ is

Counterclockwise from

Is the angle to.)

And further comprising the step of detecting the stability margin by the following formula,

│)Where L is the separation between the first and second reference points, ie L = │

│)

The controlling of the movement of the walking robot according to the calculated size of the cross product is effective to ensure stability by controlling based on the stability margin. In the controlling of the movement of the walking robot according to the calculated size of the cross product, the movement of the walking robot may be controlled so that the size of the stability margin with respect to each outline of the stable area is within an allowable range. The first reference point and the second reference point may be selected in an inner region having a predetermined distance from an edge of the stable region.

And determining whether the walking robot is stable according to the sign of the size of the cross product, and controlling the movement of the walking robot according to the calculated size of the cross product includes: It can control based on stability. And the stable area is limited to the area including the ground in contact with the sole when there is only one foot in contact with the ground.

)와 상기 제1 기준점(A)으로부터 ZMP(Z)에 이르는 제2 벡터(

)의 외적의 크기(K)를 산출하고 산출된 외적의 크기에 따라 상기 구동부를 제어하는 제어부를 포함하는 것을 특징으로 하는 보행로봇에 의해 달성될 수 있다.In addition, the above object, according to another field of the present invention, in the walking robot, a plurality of feet; A driving unit for driving an operation; The first vector (1) from the first reference point (A) of the stable region including the area between the foot in contact with the ground and the second reference point (B) of the stable region (

) And a second vector from the first reference point A to ZMP (Z)

It can be achieved by a walking robot, characterized in that it comprises a control unit for calculating the size (K) of the cross product and controls the drive unit according to the calculated size of the cross product.

Here, the size K of the cross product may be calculated by the following equation.

로부터 반시계방향으로

에 이르는 각도이다.)Here, the coordinate of the first reference point A is (x _a , y _a ), the coordinate of the second reference point B is (x _b , y _b ), and the coordinate of ZMP is (x _z , y _z ), θ is

Counterclockwise from

Is the angle to.)

The controller is effective to determine whether the walking robot is stable according to the sign of the size of the cross product and to control the driving unit based on the determination result so that the stability is maintained.

In addition, the controller detects a stability margin according to the following equation, and controlling the drive unit based on the stability margin is preferable to ensure stability.

|)Where L is the separation between the first and second reference points, ie L = |

|)

The stable area is an area including the ground in contact with the sole when the foot is in contact with the ground.

Hereinafter, with reference to the accompanying drawings will be described an embodiment of the present invention;

Figure 2 is a schematic block diagram of a walking robot according to an embodiment of the present invention.

As shown in FIG. 2, the walking robot has a plurality of feet 10a and 10b, a driving unit 20, and a control unit 30. The driving unit 20 includes a motor for driving the movement of the joints formed on the legs, the trunk, the arms, and the like including the feet 10a and 10b. The controller 30 includes a controller for determining stability and controlling the driver 20 accordingly.

3 (a) and 3 (b) show a stable area formed by a walking robot.

Each stable region is a convex hull with six vertices and region boundary lines connecting the vertices.

Referring to Fig. 3 (a), when the ZMP is inside, the ZMP is located in the clockwise direction with respect to the adjacent vertex located in the clockwise direction from any vertex.

Referring to FIG. 3 (b), when the ZMP is located outside the stable region, the ZMP is located in the counterclockwise direction with respect to the adjacent vertex located clockwise from any vertex.

 In this way, the connection direction of the segment connecting the vertex and the line connecting the ZMP is different according to the position of the ZMP. Based on this difference, the controller 30 determines the stability by a simple algorithm.

4 is a flowchart for determining stability according to an embodiment of the present invention.

First, the controller 30 calculates the stable region and the ZMP according to the movement of the robot (S1). The stable area is generated between the two feet supporting the robot, and the controller 30 calculates the positions of the vertices of the convex polygon in real time using direct kinematics. In addition to the boundary line of the stable area, the dangerous area line may be selected as an arbitrary reference line that is used as a standard for determining stability. An area boundary line is adopted as the danger area line, and an arbitrary first reference point and a second reference point on a segment of the area boundary line are selected.

)와 상기 제1 기준점(A)으로부터 ZMP(Z)에 이르는 제2 벡터(

)의 외적의 크기(K)를 산출한다(S3).Next, the first vector from the first reference point (A) to the second reference point (B) in the stable region (

) And a second vector from the first reference point A to ZMP (Z)

The size K of the cross product of () is calculated (S3).

식(1)

Formula (1)

| 및 |

|는 각각

및

의 크기이며, θ는

로부터 반시계방향으로

에 이르는 각도이다.Where |

| And |

| Each

And

Is the size of θ

Counterclockwise from

Is the angle to.

The controller 30 determines the stability of the walking robot according to the size K of the cross product and controls the driving unit 20 accordingly.

The ± sign of the size (K) of the cross product serves as a criterion for determining whether the walking robot is stable (S4). Referring to Equation (1), it can be seen that the sign of the magnitude K of the cross product is determined according to θ. That is, the characteristic of the sine function is positive when θ <180 ° and negative when θ> 180 °.

5 (a) to 5 (c) show the sign of K according to the relative position between the segment and ZMP.

)로부터 반시계방향으로 제2 벡터(

)에 이르는 각이 180°상이므로 외적의 크기(K)는 음수가 된다. 이 때, 외적의 크기(K)의 부호에 따라 ZMP는 안정영역 내부에 위치하는 것으로 판정되어, 로봇이 안정상태에 있는 것으로 최종 판단된다.Referring to FIG. 5A, when the point P0 is the first reference point A and the point P1 is the second reference point B, the first vector (

Counterclockwise from the second vector (

Since the angle leading to) is 180 °, the magnitude (K) of the cross product is negative. At this time, according to the sign of the magnitude K of the cross product, it is determined that the ZMP is located inside the stable area, and finally the robot is determined to be in a stable state.

)로부터 반시계방향으로 제2 벡터(

)에 이르는 각이 180°미만인 경우 외적의 크기(K)는 양수가 된다. 이 때, ZMP는 안정영역의 외부에 위치하는 것으로 판정되고, 로봇이 불안정한 상태에 있는 것으로 최종 판별된다.On the other hand, as shown in Fig. 5 (b), the first vector (

Counterclockwise from the second vector (

If the angle leading to) is less than 180 °, the cross product size (K) becomes positive. At this time, it is determined that ZMP is located outside the stable area, and finally it is determined that the robot is in an unstable state.

On the other hand, as shown in Fig. 5C, when the first vector and the second vector are parallel, K becomes zero. The control unit 30 accordingly determines that the robot is at the critical point of the stable and unstable state.

If the Cartesian coordinate system is set, the magnitude (K) of the cross product can be obtained by the following equation (2) using the coordinates of the first reference point (A), the second reference point (B), and ZMP (Z).

식(2)

Formula (2)

로부터 반시계방향으로

에 이르는 각도이다. 좌표계는 로봇의 발목을 원점으로 하여 설정될 수 있지만, 그 밖에 어떠한 좌표계를 기준으로 한다고 하더라도 식(2)가 적용될 수 있음을 알 수 있을 것이다.Here, the coordinate of the first reference point A is (x _a , y _a ), the coordinate of the second reference point B is (x _b , y _b ), and the coordinate of ZMP is (x _z , y _z ), and θ Is

Counterclockwise from

Is the angle to. The coordinate system may be set based on the ankle of the robot as an origin, but it will be appreciated that Equation (2) may be applied to any other coordinate system.

)와 ZMP를 잇는 제2 벡터(

)간의 외적의 결과의 부호에 따라 안정도를 판별할 수 있으며, 이 결과에 따라 구동부(20)를 제어한다.In this way, the control unit 30 may include a first vector representing a segment (

) And the second vector (ZMP)

The stability can be determined according to the sign of the result of the cross product between the two and the control unit 20 is controlled according to this result.

That is, when the robot is determined to be in an unstable state, the controller 30 controls the driver 20 to control the attitude of the robot (S5).

On the other hand, if it is determined that the robot is in a stable state, the stability is measured through the distance between the ZMP and the segment (S6). That is, since the absolute magnitude of the cross product represents the area of the parallelogram formed by the first vector and the second vector, if K is divided by the separation distance between the first and second reference points, the distance from ZMP to the first vector ( Hereinafter referred to as "stability margin" can be calculated (S6).

식(3)

Formula (3)

|)Where L is the separation between the first and second reference points, ie L = |

|)

The controller 30 may control the driving unit 20 according to the stability margin in advance to prevent the robot from becoming in an unstable state (S7).

In order to secure the stability, it is possible to make the stability margin have a allowable range of a certain size or more, or to select a segment, which is a reference for stability determination, within the region as shown in the solid lines shown in FIGS. 5 (a) to 5 (c). Do. Accordingly, the stability area can be arbitrarily reduced to increase the sensitivity to the stability of the robot. The controller 30 may maintain the stability of the walking robot to the maximum by controlling the stability margin to have a constant value based on each segment.

In the above-described embodiment, in the case of the biped walking robot, the stable area formed when both feet touch the ground is exemplified. However, in other cases, the stability area may be appropriately determined to determine the stability.

For example, when one foot is separated from the ground in the biped walking robot, the stable area is based only on an area formed by one foot in contact with the ground. Similarly, in the case of a robot having one foot or a robot having three or more feet, stability stability and stability margin can be obtained simply by setting a stable area.

Equations (1) to (3) can also be applied when the stable region is extended to a three-dimensional space region. Equation (2) should be modified as follows.

로부터 반시계방향으로

에 이르는 각도이다.Here, the coordinate of the first reference point A is (x _a , y _a , z _a ), the coordinate of the second reference point B is (x _b , y _b , z _b ), and the coordinate of ZMP is (x _z , y _z , z _z ), and θ is

Counterclockwise from

Is the angle to.

Although some embodiments of the invention have been shown and described, it will be apparent to those skilled in the art that the embodiments may be modified without departing from the spirit or spirit of the invention. . It is intended that the scope of the invention be defined by the claims appended hereto and their equivalents.

According to the present invention, it is possible to easily determine the stability of the walking robot.

Claims (12)

In the control method of a walking robot having a plurality of feet, Calculating a ZMP of the walking robot by selecting a first reference point A and a second reference point B on a dangerous boundary line among the stable areas including the areas between the feet in contact with the ground; A first vector from the first reference point A to the second reference point B

) And a second vector from the first reference point A to ZMP (Z)

Calculating the magnitude (K) of the cross product of And controlling the movement of the walking robot according to the calculated size of the cross product. The method of claim 1, The control method of the walking robot, characterized in that the size (K) of the cross product is calculated by the following equation:

Here, the coordinate of the first reference point A is (x _a , y _a ), the coordinate of the second reference point B is (x _b , y _b ), and the coordinate of ZMP is (x _z , y _z ), θ is

Counterclockwise from

Is the angle to.) The method of claim 1, Detecting a stability margin according to the following formula;

Where L is the separation between the first and second reference points, ie L = |

|) And controlling the movement of the walking robot according to the calculated size of the cross product is based on the stability margin. The method of claim 3, Controlling the movement of the walking robot according to the calculated size of the cross product, characterized in that for controlling the movement of the walking robot so that the size of the stability margin for each of the contour line of the stable area is within an acceptable range. Robot control method. The method of claim 1, Determining whether the walking robot is stable according to the sign of the size of the cross product; And controlling the movement of the walking robot according to the calculated size of the cross product based on whether the walking robot is stable. The method of claim 1, And the first reference point and the second reference point are selected in an inner region having a predetermined distance from an edge of the stable region. The method of claim 1, Wherein the stable area is a control method for a walking robot, characterized in that the area including the ground in contact with the sole when the foot is in contact with the ground. In walking robot, A plurality of feet; A driving unit for driving an operation; The first vector (1) from the first reference point (A) of the stable region including the area between the foot in contact with the ground and the second reference point (B) of the stable region (

) And a second vector from the first reference point A to ZMP (Z)

And a control unit for calculating a size K of the cross product and controlling the driving unit according to the calculated size of the cross product. The method of claim 8, The size of the cross product (K) is a walking robot, characterized in that calculated by the following formula:

Here, the coordinate of the first reference point A is (x _a , y _a ), the coordinate of the second reference point B is (x _b , y _b ), and the coordinate of ZMP is (x _z , y _z ), θ is

Counterclockwise from

Is the angle to.) The method of claim 9, The control unit determines whether the walking robot is stable according to a sign of the size of the cross product and controls the driving unit based on a determination result. The method of claim 8, The control unit detects a stability margin according to the following equation, and walk robot, characterized in that for controlling the drive unit based on the stability margin:

Where L is the separation between the first and second reference points, ie L = |

|) The method of claim 8, Wherein the stable area is a walking robot, characterized in that the area including the ground in contact with the sole of the foot when the foot is in contact with the ground.

Images