JP2008093762A

JP2008093762A - Walking robot

Info

Publication number: JP2008093762A
Application number: JP2006276312A
Authority: JP
Inventors: Hiroyuki Nishizawa; 博之西澤
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2006-10-10
Filing date: 2006-10-10
Publication date: 2008-04-24

Abstract

<P>PROBLEM TO BE SOLVED: To provide a walking robot capable of executing movement during a turnover to reduce an impact force at the time of the turnover, and suppressing damage caused thereby. <P>SOLUTION: This walking robot 10 is provided with a determining means determining whether the robot 10 turns over or not, a generating means generating a plurality of driving patterns of each foot section during the turnover when the robot is determined to turn over, an impact force calculating means calculating the predicted value of the impact force for each of the generated driving patterns, which is applied to the robot 10 when it turns over while each foot section is driven according to the driving patterns. a selecting means selecting the driving pattern with the minimum predicted value of the calculated impact force among a plurality of the driving patterns, and a control means controlling each foot section according to the selected driving pattern when the driving pattern is selected. <P>COPYRIGHT: (C)2008,JPO&INPIT

Description

本発明は、本体に接続された２以上の脚部を駆動することによって歩行するロボットに関する。 The present invention relates to a robot that walks by driving two or more legs connected to a main body.

本体に接続された２以上の脚部を駆動することによって歩行するロボットが知られている。このような歩行ロボットは、転倒しないように脚部の駆動を制御しながら歩行する。しかしながら、このような歩行ロボットは、外力等が作用することによって倒立状態を維持することができず、転倒してしまう場合がある。歩行ロボットが転倒すると、歩行ロボットが破損することがあり、問題となる。
かかる問題を解決する歩行ロボットとして、特許文献１の歩行ロボットが知られている。この歩行ロボットは、床反力センサ、加速度センサを有しており、それらのセンサの検出値に基づいて歩行ロボットが転倒するか否かを判定する。転倒すると判定したときは、床面に対する支持多角形の面積変化量を最小とするとともに転倒時（すなわち、床面への衝突時）における支持多角形の面積（接触面積）を最大にするように、転倒中における各部の駆動パターンを生成する。そして、生成した駆動パターンにしたがって歩行ロボットの各部を制御する。このように、生成した駆動パターンを転倒中に実行することで、転倒時に歩行ロボットに加わる衝撃力をより小さくし、歩行ロボットの破損を抑制することができるとされている。
特開２００４−１０６１９４ A robot that walks by driving two or more legs connected to a main body is known. Such a walking robot walks while controlling the driving of the legs so as not to fall. However, such a walking robot cannot maintain an inverted state due to an external force or the like, and may fall over. If the walking robot falls, the walking robot may be damaged, which is a problem.
As a walking robot that solves this problem, a walking robot of Patent Document 1 is known. This walking robot has a floor reaction force sensor and an acceleration sensor, and determines whether or not the walking robot falls over based on detection values of these sensors. When it is determined to fall, the amount of change in the area of the support polygon relative to the floor is minimized and the area of the support polygon (contact area) at the time of the fall (that is, at the time of collision with the floor) is maximized. The drive pattern of each part during the fall is generated. And each part of a walking robot is controlled according to the produced | generated drive pattern. Thus, by executing the generated drive pattern during a fall, it is said that the impact force applied to the walking robot at the time of the fall can be further reduced and damage to the walking robot can be suppressed.
JP 2004-106194 A

上述した歩行ロボットは、歩行ロボットが転倒すると判定したときに、歩行ロボットの支持多角形の面積変化量を最小とするとともに転倒時における支持多角形の面積を最大とするように、転倒中における駆動パターンを生成する。しかしながら、歩行ロボットの転倒は突発的に発生することが多く、転倒開始時の歩行ロボットの姿勢はさまざまである。したがって、転倒時に歩行ロボットに加わる衝撃力はさまざまな要因によって異なることとなる。このため、上述したように生成した駆動パターンにしたがって各部を制御しても、転倒時の衝撃力がそれほど小さくならず、歩行ロボットが破損してしまうことがあった。 The above-described walking robot is driven during a fall so that when the walking robot is determined to fall, the area change amount of the support polygon of the walking robot is minimized and the area of the support polygon at the fall is maximized. Generate a pattern. However, the walking robot often falls suddenly, and the posture of the walking robot at the start of the falling varies. Therefore, the impact force applied to the walking robot at the time of falling differs depending on various factors. For this reason, even if each part is controlled according to the drive pattern generated as described above, the impact force at the time of falling is not so small, and the walking robot may be damaged.

本発明は、上記した実情に鑑みてなされたものであり、転倒時の衝撃力がより小さくなるように転倒中に動作を実行し、転倒による破損を抑制することができる歩行ロボットを提供することを目的とする。 The present invention has been made in view of the above-described circumstances, and provides a walking robot that can perform an operation during a fall so that an impact force at the time of the fall is smaller and can suppress damage due to the fall. With the goal.

本発明の歩行ロボットは、本体に接続された２以上の脚部と、歩行ロボットが転倒するか否かを判定する判定手段と、歩行ロボットが転倒すると判定されたときに、転倒中における各脚部の駆動パターンを複数生成する生成手段と、生成された駆動パターン毎に、その駆動パターンにしたがって各脚部を駆動して転倒したときに歩行ロボットに加わる衝撃力の予測値を算出する衝撃力算出手段と、複数の駆動パターンのうち、算出された衝撃力の予測値が最も小さい駆動パターンを選択する選択手段と、駆動パターンが選択されたときに、選択された駆動パターンにしたがって各脚部を制御する制御手段を有している。
この歩行ロボットでは、判定手段によって歩行ロボットが転倒すると判定されると、まず、生成手段が転倒中における脚部の駆動パターンを複数生成する。次に、衝撃力算出手段は、生成した駆動パターン毎に、その駆動パターンにしたがって脚部を駆動して転倒したときに歩行ロボットに加わる衝撃力の予測値を算出する。すると、選択手段が衝撃力の予測値が最も小さい駆動パターンを選択し、制御手段が選択された駆動パターンで各脚部を駆動する。この歩行ロボットでは、生成手段によって複数の駆動パターンが生成され、衝撃力算出手段が各駆動パターンについて衝撃力の予測値を算出しているので、選択手段は転倒時に歩行ロボットに加わる衝撃力が最も小さくなる駆動パターンを正確に選択することができる。したがって、制御手段が選択された駆動パターンにしたがって脚部を駆動することで、転倒時に歩行ロボットに加わる衝撃力が小さくなり、歩行ロボットの破損を抑制することができる。 The walking robot according to the present invention includes two or more legs connected to the main body, determination means for determining whether or not the walking robot falls, and each leg in the fall when it is determined that the walking robot falls. Generating means for generating a plurality of driving patterns for the part, and for each generated driving pattern, an impact force for calculating a predicted value of the impact force applied to the walking robot when each leg is driven and falls according to the driving pattern A calculating means; a selecting means for selecting a driving pattern having the smallest predicted value of the calculated impact force among the plurality of driving patterns; and each leg portion according to the selected driving pattern when the driving pattern is selected. It has a control means to control.
In this walking robot, when the determination means determines that the walking robot falls, first, the generation means generates a plurality of leg drive patterns during the fall. Next, for each generated drive pattern, the impact force calculation means calculates a predicted value of the impact force applied to the walking robot when the leg is driven and falls according to the drive pattern. Then, the selection means selects the drive pattern with the smallest predicted value of impact force, and the control means drives each leg with the selected drive pattern. In this walking robot, a plurality of driving patterns are generated by the generating means, and the impact force calculating means calculates the predicted value of the impact force for each driving pattern, so that the selecting means has the highest impact force applied to the walking robot during a fall. A drive pattern that becomes smaller can be accurately selected. Therefore, by driving the leg portion according to the selected drive pattern, the impact force applied to the walking robot at the time of falling can be reduced, and damage to the walking robot can be suppressed.

上述した歩行ロボットは、本体に接続された腕部をさらに有し、生成手段は、脚部及び腕部の駆動パターンを複数生成し、衝撃力算出手段は、生成された駆動パターン毎に、その駆動パターンにしたがって脚部及び腕部を駆動して転倒したときに歩行ロボットに加わる衝撃力の予測値を算出し、制御手段は、駆動パターンが選択されたときに、選択された駆動パターンにしたがって脚部及び腕部を制御することが好ましい。
このような構成によれば、腕部を駆動して転倒時の衝撃力がより小さくなる駆動パターンが生成手段によって生成される。したがって、そのように生成された駆動パターンから衝撃力の予測値が最も小さい駆動パターンを選択し、選択した駆動パターンに基づいて脚部及び腕部を制御することで、より転倒時の衝撃力を小さくすることができる。 The walking robot described above further includes an arm portion connected to the main body, the generation unit generates a plurality of drive patterns for the leg and arm, and the impact force calculation unit calculates the drive pattern for each generated drive pattern. The predicted value of the impact force applied to the walking robot when the leg and arm are driven to fall according to the drive pattern is calculated, and when the drive pattern is selected, the control means follows the selected drive pattern. It is preferable to control the legs and arms.
According to such a configuration, a driving pattern is generated by the generating unit so that the arm portion is driven and the impact force at the time of falling is further reduced. Therefore, by selecting the drive pattern with the smallest impact force predicted value from the generated drive pattern and controlling the leg and arm based on the selected drive pattern, the impact force at the time of falling can be further increased. Can be small.

上述した歩行ロボットは、本体の傾斜角を検出する傾斜角センサをさらに有し、判定手段は、傾斜角センサが検出した本体の傾斜角に基づいて、歩行ロボットが転倒するか否かを判定することが好ましい。
このような構成によれば、好適に歩行ロボットが転倒するか否かを判定することができる。 The walking robot described above further includes an inclination angle sensor that detects the inclination angle of the main body, and the determination unit determines whether or not the walking robot falls over based on the inclination angle of the main body detected by the inclination angle sensor. It is preferable.
According to such a configuration, it can be determined whether or not the walking robot falls down.

上述した歩行ロボットは、本体の目標傾斜角を算出する目標傾斜角算出手段をさらに有し、制御手段は、駆動パターンが選択されていないときは、本体が目標傾斜角に傾斜するように脚部を制御し、判定手段は、傾斜角センサが検出した本体の傾斜角と目標傾斜角とのずれが所定角度以上となったときに歩行ロボットが転倒すると判定することが好ましい。
このような構成によれば、より好適に歩行ロボットが転倒するか否かを判定することができる。 The walking robot described above further includes target inclination angle calculating means for calculating a target inclination angle of the main body, and the control means has a leg portion so that the main body is inclined to the target inclination angle when a drive pattern is not selected. The determination means preferably determines that the walking robot falls when the deviation between the inclination angle of the main body detected by the inclination angle sensor and the target inclination angle becomes equal to or larger than a predetermined angle.
According to such a configuration, it can be determined whether or not the walking robot falls down more suitably.

本発明の一実施形態に係る歩行ロボット１０について図面を参照して説明する。図１は、歩行ロボット１０の概略斜視図を示している。図１に示すように、歩行ロボット１０は、本体１１と、本体１１に接続された頭部１２、右腕部１３、左腕部１４、右脚部１５、左脚部１６によって構成されている。
頭部１２は、本体１１と首関節２０を介して接続されている。首関節２０は、頭部１２を本体１１に対してヨー軸周りに相対回転させる。
右腕部１３は、本体１１と２つの肩関節２１、２２を介して接続されている。肩関節２１は、右腕部１３を本体１１に対してロール軸周りに相対回転させる。肩関節２２は、右腕部１３を本体１１に対してピッチ軸周りに相対回転させる。したがって、肩関節２１を駆動することで右腕部１３を前後方向に振ることができ、肩関節２２を駆動することで右腕部１３を左右方向に振ることができる。また、右腕部１３は、複数のパーツが関節１８（肘関節，手首関節）を介して接続されることによって構成されている。
左腕部１４は、本体１１と２つの肩関節２３、２４を介して接続されている。肩関節２３は、左腕部１４を本体１１に対してロール軸周りに相対回転させる。肩関節２４は、左腕部１４を本体１１に対してピッチ軸周りに相対回転させる。したがって、肩関節２３を駆動することで左腕部１４を前後方向に振ることができ、肩関節２４を駆動することで左腕部１４を左右方向に振ることができる。また、左腕部１４は、複数のパーツが関節１８（肘関節，手首関節）を介して接続されることによって構成されている。
右脚部１５は、本体１１と股関節２５、２６を介して接続されている。右脚部１５は、一端が本体１１と股関節２５、２６を介して接続された上腿部１５ａ、その上腿部１５ａの他端と膝関節２７を介して接続された下腿部１５ｂ、その下腿部１５ｂの他端と２つの足首関節２８、２９を介して接続された足平１５ｃによって構成されている。股関節２５は、上腿部１５ａを本体１１に対してピッチ軸周りに相対回転させる。股関節２６は、上腿部１５ａを本体１１に対してロール軸周りに相対回転させる。膝関節２７は、下腿部１５ｂを上腿部１５ａに対してピッチ軸周りに相対回転させる。足首関節２８は、足平１５ｃを下腿部１５ｂに対してピッチ軸周りに相対回転させる。足首関節２９は、足平１５ｃを下腿部１５ｂに対してロール軸周りに相対回転させる。
左脚部１６は、本体１１と２つの股関節３０、３１を介して接続されている。左脚部１６は、一端が本体１１と股関節３０、３１を介して接続された上腿部１６ａ、その上腿部１６ａの他端と膝関節３２を介して接続された下腿部１６ｂ、その下腿部１６ｂの他端と足首関節３３，３４を介して接続された足平１６ｃによって構成されている。股関節３０は、上腿部１６ａを本体１１に対してピッチ軸周りに相対回転させる。股関節３１は、上腿部１６ａを本体１１に対してロール軸周りに相対回転させる。膝関節３２は、下腿部１６ｂを上腿部１６ａに対してピッチ軸周りに相対回転させる。足首関節３３は、足平１６ｃを下腿部１６ｂに対してピッチ軸周りに相対回転させる。足首関節３４は、足平１６ｃを下腿部１６ｂに対してロール軸周りに相対回転させる。
上述したように、歩行ロボット１０は、多数の関節によって複数のパーツが接続されることによって構成されている。歩行ロボット１０は、各関節を駆動することによって種々の動作を実行する。例えば、右脚部１５の股関節２５、２６、膝関節２７、足首関節２８、２９と、左脚部１６の股関節３０、３１、膝関節３２、足首関節３３，３４を所定の駆動パターンにしたがって駆動させることで、歩行ロボット１０は歩行する。 A walking robot 10 according to an embodiment of the present invention will be described with reference to the drawings. FIG. 1 is a schematic perspective view of the walking robot 10. As shown in FIG. 1, the walking robot 10 includes a main body 11, a head 12 connected to the main body 11, a right arm part 13, a left arm part 14, a right leg part 15, and a left leg part 16.
The head 12 is connected to the main body 11 via the neck joint 20. The neck joint 20 rotates the head 12 relative to the main body 11 around the yaw axis.
The right arm portion 13 is connected to the main body 11 via two shoulder joints 21 and 22. The shoulder joint 21 rotates the right arm portion 13 relative to the main body 11 around the roll axis. The shoulder joint 22 rotates the right arm portion 13 relative to the main body 11 around the pitch axis. Therefore, the right arm 13 can be swung in the front-rear direction by driving the shoulder joint 21, and the right arm 13 can be swung in the left-right direction by driving the shoulder joint 22. The right arm portion 13 is configured by connecting a plurality of parts via joints 18 (elbow joints and wrist joints).
The left arm portion 14 is connected to the main body 11 via two shoulder joints 23 and 24. The shoulder joint 23 rotates the left arm portion 14 relative to the main body 11 around the roll axis. The shoulder joint 24 rotates the left arm portion 14 relative to the main body 11 around the pitch axis. Therefore, the left arm portion 14 can be swung in the front-rear direction by driving the shoulder joint 23, and the left arm portion 14 can be swung in the left-right direction by driving the shoulder joint 24. Further, the left arm portion 14 is configured by connecting a plurality of parts via joints 18 (elbow joints and wrist joints).
The right leg 15 is connected to the main body 11 via the hip joints 25 and 26. The right leg 15 includes an upper thigh 15a having one end connected to the main body 11 via the hip joints 25 and 26, a lower thigh 15b connected to the other end of the upper thigh 15a via the knee joint 27, The foot 15c is connected to the other end of the lower leg 15b via two ankle joints 28 and 29. The hip joint 25 rotates the upper leg 15a relative to the main body 11 around the pitch axis. The hip joint 26 rotates the upper leg 15a relative to the main body 11 around the roll axis. The knee joint 27 rotates the lower leg 15b relative to the upper leg 15a around the pitch axis. The ankle joint 28 rotates the foot 15c relative to the lower leg 15b around the pitch axis. The ankle joint 29 rotates the foot 15c relative to the lower leg 15b around the roll axis.
The left leg 16 is connected to the main body 11 via two hip joints 30 and 31. The left leg 16 has an upper thigh 16a having one end connected to the main body 11 via the hip joints 30 and 31, a lower thigh 16b connected to the other end of the upper thigh 16a via the knee joint 32, The foot 16c is connected to the other end of the lower leg 16b via the ankle joints 33 and 34. The hip joint 30 rotates the upper leg 16a relative to the main body 11 around the pitch axis. The hip joint 31 rotates the upper leg 16a relative to the main body 11 around the roll axis. The knee joint 32 rotates the lower leg 16b relative to the upper leg 16a around the pitch axis. The ankle joint 33 rotates the foot 16c relative to the lower leg 16b around the pitch axis. The ankle joint 34 rotates the foot 16c relative to the lower leg 16b around the roll axis.
As described above, the walking robot 10 is configured by connecting a plurality of parts by a number of joints. The walking robot 10 performs various operations by driving each joint. For example, the hip joints 25 and 26, the knee joint 27, and the ankle joints 28 and 29 of the right leg 15 and the hip joints 30 and 31, the knee joint 32, and the ankle joints 33 and 34 of the left leg 16 are driven according to a predetermined drive pattern. By doing so, the walking robot 10 walks.

図２は、歩行ロボット１０の制御系のブロック図を示している。図２に示すように、歩行ロボット１０は、各関節を制御する制御装置４０と、本体１１の傾斜角を検出する傾斜角センサ５０を備えている。また、図２に示すように、各関節は、関節を駆動するモータと、関節の角度を検出するエンコーダを備えている。 FIG. 2 shows a block diagram of the control system of the walking robot 10. As shown in FIG. 2, the walking robot 10 includes a control device 40 that controls each joint and a tilt angle sensor 50 that detects the tilt angle of the main body 11. As shown in FIG. 2, each joint includes a motor that drives the joint and an encoder that detects the angle of the joint.

図２に示すように、首関節２０はモータ２０ａを備えている。モータ２０ａは制御装置４０と電気的に接続されている。モータ２０ａは、制御装置４０から入力される制御指令値にしたがって駆動する。これによって、首関節２０の角度が変更される。また、モータ２０ａには、エンコーダ２０ｂが設置されている。エンコーダ２０ｂは、制御装置４０と電気的に接続されている。エンコーダ２０ｂは、モータ２０ａの回転量から、首関節２０の角度を検出する。すなわち、エンコーダ２０ｂは、歩行ロボット１０が直立している状態（図１の状態）における首関節２０の角度に対する角度（変化量）を検出する。エンコーダ２０ｂが検出した首関節２０の角度は、制御装置４０に出力される。
同様に、他の関節（肩関節２１〜２４、股関節２５、２６、３０、３１、膝関節２７、３２、足首関節２８、２９、３３、３４、各関節１８）も、図２に示すように対応するモータ（モータ１８、２１〜３４）とエンコーダ（エンコーダ１８、２１〜３４）を備えている。各モータは制御装置４０から入力される制御指令値にしたがって駆動され、対応する関節の角度を変更する。各エンコーダは、対応する関節の角度（歩行ロボット１０が直立している状態における関節の角度に対する角度変化量）を検出し、制御装置４０に出力する。 As shown in FIG. 2, the neck joint 20 includes a motor 20a. The motor 20 a is electrically connected to the control device 40. The motor 20a is driven according to a control command value input from the control device 40. As a result, the angle of the neck joint 20 is changed. The motor 20a is provided with an encoder 20b. The encoder 20b is electrically connected to the control device 40. The encoder 20b detects the angle of the neck joint 20 from the rotation amount of the motor 20a. That is, the encoder 20b detects an angle (amount of change) with respect to the angle of the neck joint 20 in a state where the walking robot 10 is standing upright (state in FIG. 1). The angle of the neck joint 20 detected by the encoder 20b is output to the control device 40.
Similarly, the other joints (shoulder joints 21 to 24, hip joints 25, 26, 30, 31, knee joints 27, 32, ankle joints 28, 29, 33, 34, and each joint 18) are also shown in FIG. Corresponding motors (motors 18, 21 to 34) and encoders (encoders 18, 21 to 34) are provided. Each motor is driven in accordance with a control command value input from the control device 40, and changes the angle of the corresponding joint. Each encoder detects a corresponding joint angle (angle change amount with respect to the joint angle in a state where the walking robot 10 is standing upright), and outputs the detected joint angle to the control device 40.

傾斜角センサ５０は、本体１１に設置されている。傾斜角センサ５０は、２軸ジャイロセンサであり、鉛直方向に対するピッチ軸方向の本体１１の傾斜角Ｐと、鉛直方向に対するロール軸方向の本体１１の傾斜角Ｒを検出する。傾斜角センサ５０は、傾斜角Ｐについては本体１１の前方向への傾きをプラスとして検出し、傾斜角Ｒについては本体１１の右方向への傾きをプラスとして検出する。傾斜角センサ５０は制御装置４０と接続されている。傾斜角センサ５０が検出した本体１１の傾斜角Ｐ、Ｒは制御装置４０に出力される。 The tilt angle sensor 50 is installed in the main body 11. The tilt angle sensor 50 is a biaxial gyro sensor, and detects the tilt angle P of the main body 11 in the pitch axis direction with respect to the vertical direction and the tilt angle R of the main body 11 in the roll axis direction with respect to the vertical direction. The inclination angle sensor 50 detects the inclination of the main body 11 in the forward direction as positive for the inclination angle P, and detects the inclination of the main body 11 in the right direction as positive for the inclination angle R. The tilt angle sensor 50 is connected to the control device 40. The inclination angles P and R of the main body 11 detected by the inclination angle sensor 50 are output to the control device 40.

制御装置４０は、ＣＰＵ、ＲＯＭ、ＲＡＭ等によって構成されている。制御装置４０は、傾斜角センサ５０と、各関節のモータ及びエンコーダと接続されている。制御装置４０は、傾斜角センサ５０が検出する本体１１の傾斜角Ｐ、Ｒを読取り、読取った傾斜角Ｐ、Ｒに基づいて、歩行ロボット１０が転倒するか否かを判定する。制御装置４０は、歩行ロボット１０が転倒すると判定しない限り、所定の動作パターンにしたがって各関節に制御指令値を出力する。これによって、歩行ロボット１０の各関節が制御され、歩行ロボット１０が所定の動作を実行する。また、制御装置４０は、歩行ロボット１０が転倒すると判定すると、転倒動作パターンを生成し、その転倒動作パターンにしたがって歩行ロボット１０の各関節を制御する。 The control device 40 is constituted by a CPU, a ROM, a RAM, and the like. The control device 40 is connected to the tilt angle sensor 50 and the motor and encoder of each joint. The control device 40 reads the inclination angles P and R of the main body 11 detected by the inclination angle sensor 50 and determines whether or not the walking robot 10 falls based on the read inclination angles P and R. The control device 40 outputs a control command value to each joint according to a predetermined motion pattern unless it is determined that the walking robot 10 falls. Thereby, each joint of the walking robot 10 is controlled, and the walking robot 10 executes a predetermined operation. Moreover, if it determines with the walking robot 10 falling, the control apparatus 40 will produce | generate a fall motion pattern and will control each joint of the walk robot 10 according to the fall motion pattern.

制御装置４０が歩行ロボット１０の各関節に制御指令値を出力し、各関節を制御する処理について説明する。各関節に制御指令値を出力する際には、制御装置４０は、まず、歩行ロボット１０に実行させる動作を規定する動作パターンを生成する。動作パターンを生成すると、制御装置４０は、生成した動作パターンに基づいて図３のフローチャートを所定周期で実行し、各関節に制御指令値を出力する。 A process in which the control device 40 outputs a control command value to each joint of the walking robot 10 and controls each joint will be described. When a control command value is output to each joint, the control device 40 first generates an operation pattern that defines an operation to be executed by the walking robot 10. When the motion pattern is generated, the control device 40 executes the flowchart of FIG. 3 at a predetermined cycle based on the generated motion pattern, and outputs a control command value to each joint.

ステップＳ２では、制御装置４０は、生成した動作パターンに基づいて、今回の周期における歩行ロボット１０の各関節の目標角度を算出する。次に、制御装置４０は、算出した各関節の目標角度から、本体１１の目標傾斜角ＡＰ、ＡＲを算出する（ステップＳ４）。制御装置４０は、目標傾斜角ＡＰについては本体１１の前方向への傾きをプラスとして算出し、目標傾斜角ＡＲについては本体１１の右方向への傾きをプラスとして算出する。 In step S2, the control device 40 calculates the target angle of each joint of the walking robot 10 in the current cycle based on the generated motion pattern. Next, the control device 40 calculates the target inclination angles AP and AR of the main body 11 from the calculated target angles of the joints (step S4). The control device 40 calculates the forward inclination of the main body 11 as a positive for the target inclination angle AP, and calculates the positive inclination of the main body 11 as a positive for the target inclination angle AR.

目標傾斜角ＡＰ、ＡＲを算出すると、制御装置４０は、各関節のエンコーダが検出する各関節の現在の角度を読取る。そして、読取った各関節の現在の角度と算出した各関節の目標角度に基づいて、各関節のモータに制御指令値を出力する（ステップＳ６）。これによって、各関節が駆動し、各関節の角度が目標角度と略一致する角度に変更される。また、外力が作用する等、歩行ロボット１０に異常がない限り、各関節の角度が目標角度に制御されることで、本体１１の傾斜角が目標傾斜角と略一致する傾斜角となる。各関節を駆動すると、制御装置４０は、傾斜角センサ５０から本体１１の現在の傾斜角Ｐ、Ｒを読取る。そして、読取った傾斜角Ｐと目標傾斜角ＡＰとの差分値ΔＰと、読取った傾斜角Ｒと目標傾斜角ＡＲとの差分値ΔＲをそれぞれ算出する。そして、以下の計算式によって、本体１１の目標傾斜角と現在の傾斜角のずれ量Δθを算出する（ステップＳ８）。 When the target inclination angles AP and AR are calculated, the control device 40 reads the current angle of each joint detected by the encoder of each joint. Then, based on the read current angle of each joint and the calculated target angle of each joint, a control command value is output to the motor of each joint (step S6). As a result, each joint is driven, and the angle of each joint is changed to an angle that substantially matches the target angle. In addition, as long as there is no abnormality in the walking robot 10 such as when an external force is applied, the angle of each joint is controlled to the target angle, so that the inclination angle of the main body 11 becomes substantially the same as the target inclination angle. When each joint is driven, the control device 40 reads the current inclination angles P and R of the main body 11 from the inclination angle sensor 50. Then, a difference value ΔP between the read inclination angle P and the target inclination angle AP and a difference value ΔR between the read inclination angle R and the target inclination angle AR are calculated. Then, a deviation amount Δθ between the target inclination angle of the main body 11 and the current inclination angle is calculated by the following calculation formula (step S8).

制御装置４０は、傾斜角ずれ量Δθを算出すると、算出した傾斜角ずれ量Δθが角度αよりも大きいか否かを判定する（ステップＳ１０）。すなわち、上述したように、通常、歩行ロボット１０の各関節の角度が目標角度に制御されると、本体１１の傾斜角が目標傾斜角と略一致する。したがって、傾斜角ずれ量Δθは略０となる。しかし、歩行ロボット１０に外力が作用したり、脚部が障害物に乗り上げている場合等には、本体１１の傾斜角ずれ量Δθが大きくなる。傾斜角ずれ量Δθが所定の角度よりも大きくなると、歩行ロボット１０は転倒してしまう。したがって、傾斜角ずれ量Δθが角度αよりも大きいか否かを判定することで、歩行ロボット１０が転倒するか否かを判定することができる。 When calculating the tilt angle deviation amount Δθ, the control device 40 determines whether or not the calculated tilt angle deviation amount Δθ is larger than the angle α (step S10). That is, as described above, normally, when the angle of each joint of the walking robot 10 is controlled to the target angle, the inclination angle of the main body 11 substantially matches the target inclination angle. Therefore, the tilt angle deviation amount Δθ is substantially zero. However, when an external force is applied to the walking robot 10 or when the leg is riding on an obstacle, the tilt angle deviation amount Δθ of the main body 11 increases. When the tilt angle deviation amount Δθ becomes larger than a predetermined angle, the walking robot 10 falls. Therefore, it can be determined whether or not the walking robot 10 falls by determining whether or not the tilt angle deviation amount Δθ is larger than the angle α.

制御装置４０は、傾斜角ずれ量Δθが角度αよりも小さいと判定すると、図３の処理を終了する。制御装置４０は、傾斜角ずれ量Δθが角度αよりも大きいと判定されない限り、図３の処理を所定周期で繰り返し実行する。したがって、歩行ロボット１０の各関節の角度が生成した動作パターンにしたがって所定周期で変更され、歩行ロボット１０がその動作パターンにしたがって動作を実行する。 If the control device 40 determines that the tilt angle deviation amount Δθ is smaller than the angle α, the process of FIG. Unless it is determined that the tilt angle deviation amount Δθ is larger than the angle α, the control device 40 repeatedly executes the process of FIG. 3 at a predetermined period. Therefore, the angle of each joint of the walking robot 10 is changed at a predetermined cycle according to the generated operation pattern, and the walking robot 10 performs an operation according to the operation pattern.

また、制御装置４０は、傾斜角ずれ量Δθが角度αよりも大きいと判定すると、転倒動作処理を実行する（ステップＳ１２）。すなわち、傾斜角ずれ量Δθが角度αよりも大きいということは、歩行ロボット１０が転倒し始めていることを示している。したがって、制御装置４０は歩行ロボット１０が転倒するまでの間（すなわち、足平１５ｃ、１６ｃ以外の部分が床面と接触するまでの間）に、転倒動作処理を実行する。ステップＳ１２では、制御装置４０は、図４に示すサブルーチンを実行することで転倒動作処理を実行する。 If the control device 40 determines that the tilt angle deviation amount Δθ is larger than the angle α, the control device 40 executes a toppling operation process (step S12). That is, the fact that the tilt angle deviation amount Δθ is larger than the angle α indicates that the walking robot 10 is starting to fall. Therefore, the control device 40 executes the fall operation process until the walking robot 10 falls (that is, until a portion other than the feet 15c and 16c comes into contact with the floor surface). In step S12, the control device 40 executes the overturning operation process by executing a subroutine shown in FIG.

図４のステップＳ２０では、制御装置４０は、ステップＳ１０で算出した差分値ΔＰ、ΔＲに基づいて、歩行ロボット１０の転倒方向を特定する。制御装置４０は、まず、以下の計算式によって本体１１の傾斜角のずれ方向ψを算出する。
ψ＝ａｔａｎ（ｔａｎΔＰ／ｔａｎΔＲ）
上式で算出されるずれ方向ψは、目標傾斜角に対する本体１１の傾斜角のずれの方向を表している。すなわち、図５に示すように、縦軸をｔａｎΔＰとし、横軸をｔａｎΔＲとする座標系を考えると、縦軸のプラス方向が本体１１の前方向を示し、横軸のプラス方向が本体１１の右方向を示すこととなる。この座標系に所定の座標（ｔａｎΔＲ１，ｔａｎΔＰ１）をプロットすると、上式により算出されるずれ方向ψ１は、横軸（すなわち、本体１１の右方向）からの角度を示すこととなる（図５参照）。例えば、ずれ方向ψが０°の場合は本体１１の傾斜角が目標傾斜角に対して右方向にずれていることを表し、ずれ方向ψが９０°の場合は本体１１の傾斜角が目標傾斜角に対して前方向にずれていることを表している。 In step S20 of FIG. 4, the control device 40 specifies the falling direction of the walking robot 10 based on the difference values ΔP and ΔR calculated in step S10. First, the control device 40 calculates the shift direction ψ of the tilt angle of the main body 11 by the following calculation formula.
ψ = atan (tan ΔP / tan ΔR)
The shift direction ψ calculated by the above formula represents the direction of shift of the tilt angle of the main body 11 with respect to the target tilt angle. That is, as shown in FIG. 5, when considering a coordinate system in which the vertical axis is tanΔP and the horizontal axis is tanΔR, the positive direction of the vertical axis indicates the front direction of the main body 11, and the positive direction of the horizontal axis is the positive direction of the main body 11. It will indicate the right direction. When predetermined coordinates (tan ΔR1, tan ΔP1) are plotted in this coordinate system, the displacement direction ψ1 calculated by the above equation indicates an angle from the horizontal axis (that is, the right direction of the main body 11) (see FIG. 5). ). For example, when the shift direction ψ is 0 °, the tilt angle of the main body 11 is shifted to the right with respect to the target tilt angle. When the shift direction ψ is 90 °, the tilt angle of the main body 11 is the target tilt. It shows that it is shifted forward with respect to the corner.

制御装置４０は、ずれ方向ψを算出すると、算出したずれ方向ψに基づいて、歩行ロボット１０が何れの方向に転倒中であるかを特定する。制御装置４０は、以下のように歩行ロボット１０の転倒方向を特定する。
・−２２．５°≦ψ＜２２．５°の場合は、右方向に転倒中であると特定する。
・２２．５°≦ψ＜６７．５°の場合は、右前方向に転倒中であると特定する。
・６７．５°≦ψ＜１１２．５°の場合は、前方向に転倒中であると特定する。
・１１２．５°≦ψ＜１５７．５°の場合は、左前方向に転倒中であると特定する。
・１５７．５°≦ψ＜２０２．５°の場合は、左方向に転倒中であると特定する。
・２０２．５°≦ψ＜２４７．５°の場合は、左後方向に転倒中であると特定する。
・２４７．５°≦ψ＜２９２．５°の場合は、後方向に転倒中であると特定する。
・２９２．５°≦ψ＜３３７．５°の場合は、右後方向に転倒中であると特定する。 When calculating the deviation direction ψ, the control device 40 specifies in which direction the walking robot 10 is falling based on the calculated deviation direction ψ. The control device 40 specifies the falling direction of the walking robot 10 as follows.
・ If -22.5 ° ≦ ψ <22.5 °, specify that the vehicle is falling rightward.
・ If 22.5 ° ≦ ψ <67.5 °, specify that the vehicle is falling to the right front.
・ If 67.5 ° ≦ ψ <112.5 °, specify that the vehicle is tipping forward.
・ If 112.5 ° ≦ ψ <157.5 °, specify that the vehicle is falling in the left front direction.
・ If 157.5 ° ≦ ψ <202.5 °, specify that the vehicle is falling to the left.
・ If 202.5 ° ≦ ψ <247.5 °, specify that the vehicle is falling in the rear left direction.
・ If 247.5 ° ≦ ψ <292.5 °, specify that the vehicle is falling backward.
・ If 292.5 ° ≦ ψ <337.5 °, specify that the vehicle is falling in the right rear direction.

転倒方向を特定すると、制御装置４０は、歩行ロボット１０の目標重心位置を設定する（ステップＳ２２）。
このとき、制御装置４０は、目標重心位置を、ロボット内座標系（歩行ロボット１０の前後方向の位置をｘ座標、左右方向の位置をｙ座標とした座標系）により設定する。また、制御装置４０は、現在の重心位置に対する相対座標（すなわち、現在の重心位置を原点（０，０）とした座標）によって目標重心位置を設定する。
具体的には、まず、制御装置４０は、目標重心位置を設定する方向を決定する。目標重心位置を設定する方向は、ステップＳ２２で特定した転倒方向と逆方向に決定される。例えば、ステップＳ２２で特定した転倒方向が前方向であった場合、制御装置４０は目標重心位置を設定する方向を後方向に決定する。次に、制御装置４０は、現在の重心位置から決定した方向に水平距離βだけ離れた位置を目標重心位置として設定する。例えば、特定された転倒方向が前方向である場合は、制御装置４０は、現在の重心位置より後方向に水平距離βだけ離れた位置を目標重心位置に設定する（すなわち、座標（−β，０）を目標重心位置に設定する）。この場合、図６に示す点線６０が設定された目標重心位置となる（ｚ座標が設定されていないので、目標重心位置は線として設定される）。なお、図６では、左方向をロボット内座標系のｘ方向としているので、歩行ロボット１０が直立しているように見えるが、実際には転倒中であるので、歩行ロボット１０は前方向に傾いている。また、例えば、特定された転倒方向が右前方向である場合は、制御装置４０は、現在の重心位置から左後方向に水平距離βだけ離れた位置に目標重心位置を設定する（すなわち、座標（−β／√２，−β／√２）を目標重心位置に設定する）。 When the falling direction is specified, the control device 40 sets the target center-of-gravity position of the walking robot 10 (step S22).
At this time, the control device 40 sets the target center-of-gravity position by a robot coordinate system (a coordinate system in which the position in the front-rear direction of the walking robot 10 is the x coordinate and the position in the left-right direction is the y coordinate). Further, the control device 40 sets the target center-of-gravity position based on relative coordinates with respect to the current center-of-gravity position (that is, coordinates with the current center-of-gravity position as the origin (0, 0)).
Specifically, first, the control device 40 determines a direction in which the target center-of-gravity position is set. The direction in which the target center-of-gravity position is set is determined in the direction opposite to the falling direction specified in step S22. For example, when the falling direction specified in step S22 is the forward direction, the control device 40 determines the direction in which the target gravity center position is set as the backward direction. Next, the control device 40 sets a position separated by the horizontal distance β in the direction determined from the current center of gravity position as the target center of gravity position. For example, when the specified falling direction is the forward direction, the control device 40 sets a position separated by the horizontal distance β in the rearward direction from the current center of gravity position as the target center of gravity position (ie, coordinates (−β, 0) is set as the target center-of-gravity position). In this case, the dotted line 60 shown in FIG. 6 is the set target gravity center position (since the z coordinate is not set, the target gravity center position is set as a line). In FIG. 6, since the left direction is the x direction of the coordinate system in the robot, the walking robot 10 seems to be standing upright, but actually the walking robot 10 is tilted forward, so the walking robot 10 is tilted forward. ing. For example, when the specified falling direction is the front right direction, the control device 40 sets the target center of gravity position at a position separated from the current center of gravity position by the horizontal distance β in the left rear direction (that is, the coordinate ( −β / √2, −β / √2) is set as the target center-of-gravity position).

目標重心位置を設定すると、制御装置４０は、右脚部１５と左脚部１６の目標姿勢を３つ生成する（ステップＳ２４）。 When the target center-of-gravity position is set, the control device 40 generates three target postures for the right leg portion 15 and the left leg portion 16 (step S24).

すなわち、まず、制御装置４０は、右脚部１５と左脚部１６のうち、どちらの脚部を軸として転倒しているかを特定する。制御装置４０は、歩行ロボット１０の現在の姿勢が、脚部１５、１６のうち一方の脚部だけを接地している姿勢である場合には、その接地している脚部を軸として転倒していると特定する。また、制御装置４０は、歩行ロボット１０の現在の姿勢が、脚部１５、１６の両方を接地している姿勢である場合には（実際に両方の脚部を接地していなくても、目標関節角から予測される現在の姿勢が両方の脚部を接地する姿勢であれば、両方の脚部を接地している姿勢であると判定される）、ステップＳ２０で特定した転倒方向に近い方の脚部を軸として転倒していると特定する。 That is, first, the control device 40 specifies which leg portion of the right leg portion 15 and the left leg portion 16 is falling about the axis. When the current posture of the walking robot 10 is a posture in which only one of the leg portions 15 and 16 is grounded, the control device 40 falls around the grounded leg portion as an axis. To be identified. Further, when the current posture of the walking robot 10 is a posture in which both the leg portions 15 and 16 are grounded (the target device can be used even if both the leg portions are not actually grounded). If the current posture predicted from the joint angle is a posture in which both legs are grounded, it is determined to be a posture in which both legs are grounded), which is closer to the fall direction specified in step S20 It is specified that the leg is falling on the axis.

軸となる脚部を特定すると、制御装置４０は、特定した脚部の各関節の目標角度を算出する。このとき、制御装置４０は、図７に示す各駆動規則Ａ〜Ｆにしたがった３つの目標姿勢を算出する。
駆動規則Ａ〜Ｃは、歩行ロボット１０が前後方向に転倒するときの駆動規則であり、股関節（２５または３０）、膝関節（２７または３２）、足首関節（２８または３３）の駆動比率をそれぞれ規定している。駆動規則Ｄ〜Ｆは、歩行ロボット１０が左右方向に転倒するときの駆動規則であり、股関節（２６または３１）と足首関節（２９または３４）の駆動比率をそれぞれ規定している。駆動比率は、図７に示すように、１つの駆動規則で規定されているすべての駆動比率を加算すると１００になるように規定されている。各駆動比率は、各駆動規則で規定されている関節のうち、その関節をどの程度駆動させて歩行ロボット１０の重心位置を目標重心位置に移動させるかを示している。
制御装置４０は、ステップＳ２０で歩行ロボット１０が前方向または後方向に転倒すると判定したときは、特定した脚部の目標姿勢として、駆動規則Ａに基づいた第１の目標姿勢（すなわち、駆動規則Ａに基づいて股関節と、膝関節と、足首関節の目標角度を規定した目標姿勢）と、駆動規則Ｂに基づいた第２の目標姿勢と、駆動規則Ｃに基づいた第３の目標姿勢の３つを算出する。制御装置４０は、ステップＳ２０で歩行ロボット１０が左右方向に転倒すると判定したときは、特定した脚部の目標姿勢として、駆動規則Ｄに基づいた第１の目標姿勢と、駆動規則Ｅに基づいた第２の目標姿勢と、駆動規則Ｆに基づいた第３の目標姿勢の３つを算出する。制御装置４０は、ステップＳ２０で歩行ロボット１０が斜め方向（すなわち、右前方向、左前方向、右後方向、左後方向）に転倒すると判定したときは、特定した脚部の目標姿勢として、駆動規則Ａ及びＤに基づいた第１の目標姿勢と、駆動規則Ｂ及びＥに基づいた第２の目標姿勢と、駆動規則Ｃ及びＦに基づいた第３の目標姿勢の３つを算出する。各駆動規則に基づいて目標姿勢を算出する際には、制御装置４０は、駆動規則により駆動比率が規定されている各関節についてはそれらの関節の駆動量が駆動比率に応じた駆動量となり、かつ、重心位置が目標重心位置となるように目標角度を算出する。また、制御装置４０は、駆動比率が規定されていない各関節（例えば、ステップＳ２０で歩行ロボット１０が前方向に転倒すると判定した場合であって特定した脚部が右脚部１５であった場合、駆動規則Ａだけが適用されるので、股関節２６と足首関節２９の駆動比率が規定されない）についてはそれらの関節の現在の角度を目標角度とする。このように、特定した脚部の各関節の目標角度が算出されることによって、特定した脚部の目標姿勢が算出される。 If the leg part used as an axis | shaft is specified, the control apparatus 40 will calculate the target angle of each joint of the specified leg part. At this time, the control device 40 calculates three target postures according to the drive rules A to F shown in FIG.
The driving rules A to C are driving rules when the walking robot 10 falls in the front-rear direction, and the driving ratios of the hip joint (25 or 30), the knee joint (27 or 32), and the ankle joint (28 or 33), respectively. It prescribes. The driving rules D to F are driving rules when the walking robot 10 falls in the left-right direction, and respectively define the driving ratio of the hip joint (26 or 31) and the ankle joint (29 or 34). As shown in FIG. 7, the drive ratio is defined to be 100 when all the drive ratios defined by one drive rule are added. Each drive ratio indicates how much of the joints defined by each drive rule is driven to move the center of gravity position of the walking robot 10 to the target center of gravity position.
When determining that the walking robot 10 falls forward or backward in step S20, the control device 40 uses the first target posture based on the driving rule A (that is, the driving rule) as the specified target posture of the leg. 3) of the third target posture based on the driving rule C, the second target posture based on the driving rule B, and the third target posture based on the driving rule C. Calculate one. When determining that the walking robot 10 falls in the left-right direction in step S20, the control device 40 is based on the first target posture based on the driving rule D and the driving rule E as the target posture of the identified leg. Three of the second target posture and the third target posture based on the driving rule F are calculated. When the control device 40 determines in step S20 that the walking robot 10 falls in an oblique direction (that is, right front direction, left front direction, right rear direction, left rear direction), the driving rule is determined as the specified leg target posture. Three of a first target attitude based on A and D, a second target attitude based on drive rules B and E, and a third target attitude based on drive rules C and F are calculated. When calculating the target posture based on each drive rule, the control device 40, for each joint for which the drive ratio is defined by the drive rule, the drive amount of those joints becomes the drive amount according to the drive ratio, And a target angle is calculated so that a gravity center position may turn into a target gravity center position. Further, the control device 40 is a case where each of the joints for which the drive ratio is not defined (for example, when the walking robot 10 determines to fall forward in step S20 and the identified leg is the right leg 15). Since only the driving rule A is applied, the driving ratio of the hip joint 26 and the ankle joint 29 is not defined), the current angle of those joints is set as the target angle. Thus, the target posture of the identified leg is calculated by calculating the target angle of each joint of the identified leg.

特定した脚部の第１目標姿勢〜第３目標姿勢を算出すると、制御装置４０は、他方の脚部の目標姿勢を算出する。他方の脚部の目標姿勢は、所定のアルゴリズムにより、特定した脚部の目標姿勢に応じて算出される。このとき、制御装置４０は、他方の脚部の目標姿勢として、特定した脚部の第１目標姿勢、第２目標姿勢、第３目標姿勢のそれぞれに対応する３つの目標姿勢を算出する。これによって、右脚部１５及び左脚部１６の各関節の目標角度を規定した第１目標姿勢〜第３目標姿勢がそれぞれ算出される。
例えば、ステップＳ２０で歩行ロボット１０が前方向に転倒すると判定され、図６に示すように目標重心位置が設定されている場合、右脚部１５及び左脚部１６の第１目標姿勢（駆動規則Ａにしたがった目標姿勢）は図８に示すように算出され、第２目標姿勢（駆動規則Ｂにしたがった目標姿勢）は図９に示すように算出され、第３目標姿勢（駆動規則Ｃにしたがった目標姿勢）は図１０に示すように算出される。 When the first to third target postures of the identified leg are calculated, the control device 40 calculates the target posture of the other leg. The target posture of the other leg is calculated according to the specified target posture of the leg by a predetermined algorithm. At this time, the control device 40 calculates three target postures corresponding to the first target posture, the second target posture, and the third target posture of the specified leg as the target posture of the other leg. As a result, the first to third target postures that define the target angles of the joints of the right leg portion 15 and the left leg portion 16 are calculated.
For example, if it is determined in step S20 that the walking robot 10 falls forward, and the target center-of-gravity position is set as shown in FIG. 6, the first target postures (drive rules) of the right leg portion 15 and the left leg portion 16 are set. The target posture according to A) is calculated as shown in FIG. 8, and the second target posture (target posture according to the driving rule B) is calculated as shown in FIG. Therefore, the target posture) is calculated as shown in FIG.

制御装置４０は、両脚部の第１目標姿勢〜第３目標姿勢を算出すると、それらの目標姿勢に対応させて右腕部１３及び左腕部１４の目標姿勢を算出する（ステップＳ２６）。すなわち、まず、制御装置４０は、歩行ロボット１０の転倒方向に近い腕部を特定する。なお、歩行ロボット１０の転倒方向が前方向及び後方向で有る場合は、右腕部１３と左腕部１４の両方が特定される。腕部を特定すると、制御装置４０は、特定しなかった腕部の各関節（肩関節、肘関節、手首関節）の目標角度を０°に決定する。また、制御装置４０は、特定した腕部の関節１８（肘関節及び手首関節）の目標角度を０°に決定する。次に、制御装置４０は、特定した腕部の肩関節からその腕部側の脚部の接地部までの距離Ｌを、両脚部の目標姿勢毎に算出する。例えば、図８〜１０に示すように両脚部の目標姿勢が算出されている場合、第１目標姿勢における距離Ｌとして図８のＬ１が算出され、第２目標姿勢における距離Ｌとして図９のＬ２が算出され、第３目標姿勢における距離Ｌとして図１０のＬ３が算出される。 When calculating the first target posture to the third target posture of both legs, the control device 40 calculates the target postures of the right arm portion 13 and the left arm portion 14 in correspondence with these target postures (step S26). That is, first, the control device 40 specifies an arm portion close to the falling direction of the walking robot 10. Note that when the falling direction of the walking robot 10 is the forward direction and the backward direction, both the right arm portion 13 and the left arm portion 14 are specified. When the arm portion is specified, the control device 40 determines the target angle of each joint (shoulder joint, elbow joint, wrist joint) of the arm portion not specified as 0 °. Further, the control device 40 determines the target angle of the joint 18 (elbow joint and wrist joint) of the identified arm as 0 °. Next, the control device 40 calculates the distance L from the shoulder joint of the identified arm part to the ground contact part of the leg part on the arm part side for each target posture of both leg parts. For example, when the target postures of both legs are calculated as shown in FIGS. 8 to 10, L1 in FIG. 8 is calculated as the distance L in the first target posture, and L2 in FIG. 9 as the distance L in the second target posture. Is calculated, and L3 in FIG. 10 is calculated as the distance L in the third target posture.

次に、制御装置４０は、以下の計算式によって、転倒方向における本体１１と特定した腕部との目標角度φを決定する。目標角度φは、第１目標姿勢〜第３目標姿勢のそれぞれに対応した角度φ１〜φ３の３つの角度が求められる。
φ＝ａｃｏｓ（Ｋ／ｌ）
ここで、上式のＫは、腕部を真っ直ぐに伸ばしたときの腕部の先端から肩関節までの距離を示している（右腕部１３と左腕部１４の距離Ｋは同一である）。上式により算出される角度φは、歩行ロボット１０が脚部１５、１６を目標姿勢にしたがって駆動して転倒したときに、腕部が床面と略垂直に接触するための肩関節の角度である。例えば、図１１に示すように、脚部１５、１６を目標姿勢となるように駆動して転倒する場合は、角度φは、特定した腕部が床面と略垂直に接触するための本体１１と特定した腕部との角度となる。このように特定した腕部が床面と略垂直に接触すると、腕部により転倒時の衝撃力が分散し、床面から歩行ロボット１０に加わる衝撃力が比較的小さくなるとともに、床面との接触時に歩行ロボット１０の各部が破損し難い。制御装置４０は、角度φ１〜φ３を算出すると、転倒方向において本体１１と特定した腕部との角度がφ１〜φ３となるように、両脚部の第１目標姿勢〜第３目標姿勢に対応する肩関節２１〜２４の目標角度をそれぞれ算出する。これによって両脚部の第１目標姿勢〜第３目標姿勢に対応する両腕部の第１目標姿勢〜第３目標姿勢が算出される。以下では、両脚部の目標姿勢と両腕部の目標姿勢を合わせて、歩行ロボット１０の目標姿勢という（両脚部の第１目標姿勢と両腕部の第１目標姿勢を合わせて歩行ロボット１０の目標姿勢Ａ、両脚部の第２目標姿勢と両腕部の第２目標姿勢を合わせて歩行ロボット１０の目標姿勢Ｂ、両脚部の第３目標姿勢と両腕部の第３目標姿勢を合わせて歩行ロボット１０の目標姿勢Ｃという）。例えば、図８に示すように両脚部の目標姿勢が算出されている場合、図１２に示すように歩行ロボット１０の目標姿勢が算出される。また、ステップＳ２０で歩行ロボット１０が右方向に転倒すると判定された場合には、例えば図１３に示すように歩行ロボット１０の目標姿勢が算出される。 Next, the control device 40 determines the target angle φ between the main body 11 and the identified arm part in the falling direction by the following calculation formula. As the target angle φ, three angles of angles φ1 to φ3 corresponding to the first target posture to the third target posture are obtained.
φ = acos (K / l)
Here, K in the above expression indicates the distance from the tip of the arm portion to the shoulder joint when the arm portion is straightened (the distance K between the right arm portion 13 and the left arm portion 14 is the same). The angle φ calculated by the above equation is an angle of the shoulder joint for the arm portion to contact the floor surface substantially perpendicularly when the walking robot 10 drives the leg portions 15 and 16 according to the target posture and falls. is there. For example, as shown in FIG. 11, when the legs 15 and 16 are driven to fall in a target posture and fall down, the angle φ is the main body 11 for the specified arm portion to contact the floor surface substantially perpendicularly. And the specified arm part. When the arm portion specified in this way comes into contact with the floor surface substantially perpendicularly, the impact force at the time of the fall is dispersed by the arm portion, the impact force applied to the walking robot 10 from the floor surface becomes relatively small, and Each part of the walking robot 10 is not easily damaged during contact. When calculating the angles φ1 to φ3, the control device 40 corresponds to the first target posture to the third target posture of both legs so that the angle between the main body 11 and the specified arm portion is φ1 to φ3 in the falling direction. The target angles of the shoulder joints 21 to 24 are calculated. Accordingly, the first target posture to the third target posture of both arms corresponding to the first target posture to the third target posture of both legs are calculated. Below, the target posture of both legs and the target posture of both arms are combined to be referred to as the target posture of the walking robot 10 (the first target posture of both legs and the first target posture of both arms are combined. The target posture A, the second target posture of both legs and the second target posture of both arms are combined, the target posture B of the walking robot 10, the third target posture of both legs and the third target posture of both arms are combined. The target posture C of the walking robot 10). For example, when the target postures of both legs are calculated as shown in FIG. 8, the target posture of the walking robot 10 is calculated as shown in FIG. When it is determined in step S20 that the walking robot 10 falls to the right, the target posture of the walking robot 10 is calculated as shown in FIG. 13, for example.

制御装置４０は、歩行ロボット１０の目標姿勢Ａ〜Ｃを算出すると、それらの目標姿勢Ａ〜Ｃに対応する転倒動作パターンＡ〜Ｃを算出する（ステップＳ２８）。転倒動作パターンＡ〜Ｃは、その転倒動作パターンの実行後に、歩行ロボット１０が対応する目標姿勢となるように算出される。 When calculating the target postures A to C of the walking robot 10, the control device 40 calculates the overturning motion patterns A to C corresponding to the target postures A to C (step S28). The fall motion patterns A to C are calculated so that the walking robot 10 has a corresponding target posture after the fall motion pattern is executed.

転倒動作パターンＡ〜Ｃを算出すると、制御装置４０は、歩行ロボット１０がそれらの転倒動作パターンＡ〜Ｃにしたがって動作を実行しながら転倒したときに、歩行ロボット１０に加わる衝撃力の予測値Ｆ１〜Ｆ３をシミュレーションにより算出する（ステップＳ３０）。
すなわち、まず、制御装置４０は、データ上で仮想的な空間を作成し、その空間内に歩行ロボット１０の３次元形状を示す３次元形状データ９０を作成する。次に、制御装置４０は、水平で平坦な床面を示す床面データ９２を作成し、その床面データ９２上に図１４に示すように３次元形状データ９０を配置する。次に、制御装置４０は、傾斜角センサ５０から本体１１の現在の傾斜角を読取るとともに、各関節のエンコーダから各関節の現在の角度を読取る。そして、読取った本体１１の傾斜角と各関節の角度に応じて、三次元形状データ９０が示す状態を変更する（すなわち、３次元形状データ９０の本体１１を示すパーツを読取った傾斜角に傾斜させ、３次元形状データ９０の各関節の角度を読みとった各関節の角度に変更する）。これによって、歩行ロボット１０の現在の状態、例えば、図１５に示すような歩行ロボット１０の現在の床面上での状態が再現される。次に、制御装置４０は、歩行ロボット１０の３次元形状データの各パーツに、そのパーツの質量を示す質量データを付与する。
次に、制御装置４０は、３次元形状データ９０の各関節の角度をステップＳ２８で算出した転倒動作パターンＡにしたがって変更するとともに、所定のアルゴリズムにしたがって自然法則により作用する重力やモーメント等を仮想的な空間内で再現する。これによって、歩行ロボット１０が転倒動作パターンＡを実行して転倒する状態がシミュレーションされる。制御装置４０は、このシミュレーションにより、転倒時に床面と最初に接触する歩行ロボット１０の部位を特定し、その部位に加わる衝撃力の予測値Ｆ１を算出する。すなわち、歩行ロボット１０が転倒動作パターンＡを実行して転倒したときに、歩行ロボット１０に加わる衝撃力の予測値Ｆ１が算出される。次に、制御装置４０は、衝撃力の予測値Ｆ１の算出と同様にして、転倒動作パターンＢ及びＣの実行時の衝撃力の予測値Ｆ２、Ｆ３を順に算出する。 When the falling motion patterns A to C are calculated, the control device 40 predicts the predicted value F1 of the impact force applied to the walking robot 10 when the walking robot 10 falls while executing the motion according to the falling motion patterns A to C. -F3 is calculated by simulation (step S30).
That is, first, the control device 40 creates a virtual space on the data, and creates three-dimensional shape data 90 indicating the three-dimensional shape of the walking robot 10 in the space. Next, the control device 40 creates floor surface data 92 indicating a horizontal and flat floor surface, and arranges the three-dimensional shape data 90 on the floor surface data 92 as shown in FIG. Next, the control device 40 reads the current inclination angle of the main body 11 from the inclination angle sensor 50 and reads the current angle of each joint from the encoder of each joint. Then, the state indicated by the three-dimensional shape data 90 is changed according to the read inclination angle of the main body 11 and the angle of each joint (that is, the inclination indicating the inclination angle obtained by reading the part indicating the main body 11 of the three-dimensional shape data 90). The angle of each joint in the three-dimensional shape data 90 is changed to the angle of each joint read out). Thereby, the current state of the walking robot 10, for example, the current state of the walking robot 10 on the floor as shown in FIG. 15 is reproduced. Next, the control device 40 gives mass data indicating the mass of the part to each part of the three-dimensional shape data of the walking robot 10.
Next, the control device 40 changes the angle of each joint in the three-dimensional shape data 90 in accordance with the overturning motion pattern A calculated in step S28, and virtually calculates the gravity, moment, etc. acting by the natural law according to a predetermined algorithm. Reproduce in a typical space. As a result, the walking robot 10 is simulated to execute the falling motion pattern A and to fall. Based on this simulation, the control device 40 identifies the part of the walking robot 10 that first contacts the floor surface during a fall, and calculates the predicted value F1 of the impact force applied to the part. That is, the predicted value F1 of the impact force applied to the walking robot 10 when the walking robot 10 falls by executing the falling motion pattern A is calculated. Next, similarly to the calculation of the predicted value F1 of the impact force, the control device 40 sequentially calculates the predicted values F2 and F3 of the impact force when the overturning motion patterns B and C are executed.

転倒動作パターンＡ〜Ｃのそれぞれの実行時の衝撃力Ｆ１〜Ｆ３の予測値を算出すると、制御装置４０は、算出した衝撃力の予測値が最も小さい転倒動作パターンを選択する（ステップＳ３２）。次に、制御装置４０は、選択した転倒動作パターンにしたがって所定周期で各関節のモータに制御指令値を出力する（ステップＳ３４）。これによって、歩行ロボット１０の各関節が選択した転倒動作パターンにしたがって駆動され、歩行ロボット１０が選択した転倒動作パターンを実行しながら転倒していく。このとき、歩行ロボット１０は、制御装置４０が選択した転倒動作パターンを実行しているので、ステップＳ３０で制御装置４０が実行したその転倒動作パターンについてのシミュレーションと略同様にして転倒していく。したがって、歩行ロボット１０が完全に転倒し、歩行ロボット１０の足平１５ｃ、１６ｃ以外の部位が床面と接触すると、その部位にシミュレーションで算出された衝撃力の予測値と略同一の衝撃力が加わる。上述したように、歩行ロボット１０は、衝撃力の予測値が最も小さい転倒動作パターンを実行しているので、このとき歩行ロボット１０に加わる衝撃力は非常に小さい。したがって、この歩行ロボット１０は、転倒した時に破損し難い。制御装置４０は、歩行ロボット１０に選択した動作パターンを実行させると、図４の処理を終了し、歩行ロボット１０の各関節のモータを停止させる。 When the predicted values of the impact forces F1 to F3 at the time of execution of the overturning operation patterns A to C are calculated, the control device 40 selects the overturning operation pattern having the smallest predicted value of the calculated impact force (step S32). Next, the control device 40 outputs a control command value to the motor of each joint at a predetermined cycle according to the selected falling motion pattern (step S34). Thus, each joint of the walking robot 10 is driven according to the selected falling motion pattern, and the walking robot 10 falls while executing the selected falling motion pattern. At this time, since the walking robot 10 executes the falling motion pattern selected by the control device 40, the walking robot 10 falls in substantially the same manner as the simulation of the falling motion pattern executed by the control device 40 in step S30. Therefore, when the walking robot 10 completely falls down and a part other than the feet 15c and 16c of the walking robot 10 contacts the floor surface, an impact force substantially the same as the predicted value of the impact force calculated by the simulation is applied to the part. Join. As described above, since the walking robot 10 executes the falling motion pattern with the smallest predicted value of the impact force, the impact force applied to the walking robot 10 at this time is very small. Therefore, this walking robot 10 is not easily damaged when it falls. When the control device 40 causes the walking robot 10 to execute the selected operation pattern, the control device 40 ends the processing of FIG. 4 and stops the motors of the joints of the walking robot 10.

以上に説明したように、本実施形態の歩行ロボット１０は、歩行ロボット１０が転倒すると判定すると、転倒中における脚部の転倒動作パターンを複数生成する。そして、生成した転倒動作パターン毎に、転倒動作パターンにしたがって各関節を駆動して転倒したときに歩行ロボット１０に加わる衝撃力の予測値を算出する。そして、衝撃力の予測値が最も小さい転倒動作パターンを選択し、選択した転倒動作パターンにしたがって各関節を駆動する。したがって、転倒時に歩行ロボット１０に加わる衝撃力が小さくなり、歩行ロボット１０の破損が抑制される。 As described above, when the walking robot 10 according to the present embodiment determines that the walking robot 10 falls, the walking robot 10 generates a plurality of falling motion patterns of the legs during the fall. Then, for each generated fall motion pattern, the predicted value of the impact force applied to the walking robot 10 when each joint is driven and falls according to the fall motion pattern is calculated. Then, the falling motion pattern having the smallest impact force prediction value is selected, and each joint is driven according to the selected falling motion pattern. Therefore, the impact force applied to the walking robot 10 at the time of falling is reduced, and damage to the walking robot 10 is suppressed.

また、上述した歩行ロボット１０は、腕部１３、１４も駆動する転倒動作パターンを生成し、転倒時にその転倒動作パターンにしたがって腕部１３、１４を駆動する。したがって、より転倒時の衝撃力を小さくすることができ、歩行ロボット１０の破損がより抑制される。 Further, the walking robot 10 described above generates a falling motion pattern that also drives the arm portions 13 and 14, and drives the arm portions 13 and 14 according to the falling motion pattern when falling. Therefore, the impact force at the time of the fall can be further reduced, and breakage of the walking robot 10 is further suppressed.

また、上述した歩行ロボット１０は、傾斜角センサ５０が検出した本体１１の傾斜角Ｐ、Ｒと、目標傾斜角ＡＰ、ＡＲの差分値に基づいて、歩行ロボット１０が転倒するか否かを判定する。したがって、好適に歩行ロボット１０が転倒するか否かを判定することができる。 Further, the walking robot 10 described above determines whether or not the walking robot 10 falls based on the difference values between the inclination angles P and R of the main body 11 detected by the inclination angle sensor 50 and the target inclination angles AP and AR. To do. Therefore, it is possible to determine whether or not the walking robot 10 falls down.

なお、上述した歩行ロボット１０は、二足歩行ロボットであったが、本発明の技術は３つ以上の脚部を駆動して歩行する歩行ロボットに適用することもできる。このような歩行ロボットにおいて本発明を実施しても、転倒時の破損を抑制することができる。 In addition, although the walking robot 10 described above is a biped walking robot, the technique of the present invention can also be applied to a walking robot that walks by driving three or more legs. Even if the present invention is implemented in such a walking robot, breakage during a fall can be suppressed.

また、上述した歩行ロボット１０は、制御装置４０が、傾斜角ずれψの大きさに基づいて歩行ロボット１０が転倒するか否かを判定したが、本発明はこのような実施形態に限られない。例えば、差分値ΔＰ、ΔＲまたは傾斜角ずれψ等の変化量に基づいて歩行ロボット１０が転倒するか否かを判定してもよい。 In the walking robot 10 described above, the control device 40 determines whether or not the walking robot 10 falls based on the magnitude of the tilt angle deviation ψ, but the present invention is not limited to such an embodiment. . For example, it may be determined whether or not the walking robot 10 falls based on the amount of change such as the difference values ΔP and ΔR or the inclination angle deviation ψ.

以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例をさまざまに変形、変更したものが含まれる。
本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。 Specific examples of the present invention have been described in detail above, but these are merely examples and do not limit the scope of the claims. The technology described in the claims includes various modifications and changes of the specific examples illustrated above.
The technical elements described in this specification or the drawings exhibit technical usefulness alone or in various combinations, and are not limited to the combinations described in the claims at the time of filing. In addition, the technology illustrated in the present specification or the drawings achieves a plurality of objects at the same time, and has technical utility by achieving one of the objects.

歩行ロボット１０の斜視図。The perspective view of the walking robot 10. FIG. 歩行ロボット１０の制御系を示すブロック図。FIG. 2 is a block diagram showing a control system of the walking robot 10. 歩行ロボット１０の通常時の動作処理を示すフローチャート。The flowchart which shows the operation | movement process at the normal time of the walking robot. 歩行ロボット１０の転倒時の動作処理を示すフローチャート。The flowchart which shows the operation | movement process at the time of the fall of the walking robot. 歩行ロボット１０の転倒方向の説明図。Explanatory drawing of the fall direction of the walking robot. 歩行ロボット１０の目標重心位置の説明図。Explanatory drawing of the target gravity center position of the walking robot. 各関節の転倒時の駆動規則を示す図。The figure which shows the drive rule at the time of the fall of each joint. 歩行ロボット１０の脚部の目標姿勢の例を示す図。The figure which shows the example of the target attitude | position of the leg part of the walking robot. 歩行ロボット１０の脚部の目標姿勢の例を示す図。The figure which shows the example of the target attitude | position of the leg part of the walking robot. 歩行ロボット１０の脚部の目標姿勢の例を示す図。The figure which shows the example of the target attitude | position of the leg part of the walking robot. 肩関節の目標角度φの説明図。Explanatory drawing of the target angle (phi) of a shoulder joint. 歩行ロボット１０の目標姿勢の例を示す図。The figure which shows the example of the target attitude | position of the walking robot. 歩行ロボット１０の目標姿勢の例を示す図。The figure which shows the example of the target attitude | position of the walking robot. シミュレーション時の３次元形状データ９０の説明図。Explanatory drawing of the three-dimensional shape data 90 at the time of simulation. シミュレーション時の３次元形状データ９０の説明図。Explanatory drawing of the three-dimensional shape data 90 at the time of simulation.

Explanation of symbols

１０：歩行ロボット
１１：本体
１２：頭部
１３：右腕部
１４：左腕部
１５：右脚部
１５ａ：上腿部
１５ｂ：下腿部
１５ｃ：足平
１６：左脚部
１６ａ：上腿部
１６ｂ：下腿部
１６ｃ：足平
１８：関節
２０：首関節
２０ａ：モータ
２０ｂ：エンコーダ
２１〜２４：肩関節
２５、２６、３０、３１：股関節
２７、３２：膝関節
２８、２９、３３、３４：足首関節
４０：制御装置
５０：傾斜角センサ 10: Walking robot 11: Body 12: Head 13: Right arm 14: Left arm 15: Right leg 15a: Upper leg 15b: Lower leg 15c: Foot 16: Left leg 16a: Upper leg 16b: Lower leg 16c: foot 18: joint 20: neck joint 20a: motor 20b: encoders 21-24: shoulder joints 25, 26, 30, 31: hip joint 27, 32: knee joints 28, 29, 33, 34: ankle Joint 40: Control device 50: Inclination angle sensor

Claims

A robot that walks by driving two or more legs connected to the body,
Determining means for determining whether or not the walking robot falls;
Generating means for generating a plurality of drive patterns for each leg during a fall when the walking robot is determined to fall;
For each generated drive pattern, impact force calculating means for calculating a predicted value of the impact force applied to the walking robot when each leg is driven and falls according to the drive pattern;
A selection means for selecting a drive pattern having the smallest predicted value of the calculated impact force among a plurality of drive patterns;
And a control means for controlling each leg according to the selected drive pattern when the drive pattern is selected.

It further has an arm connected to the main body,
The generation means generates a plurality of leg and arm drive patterns,
For each generated drive pattern, the impact force calculation means calculates a predicted value of the impact force applied to the walking robot when the leg portion and the arm portion are driven and falls according to the drive pattern,
The walking robot according to claim 1, wherein the control means controls the leg portion and the arm portion according to the selected drive pattern when the drive pattern is selected.

A tilt angle sensor for detecting the tilt angle of the main body;
3. The walking robot according to claim 1, wherein the determination unit determines whether or not the walking robot falls over based on the inclination angle of the main body detected by the inclination angle sensor.

A target inclination angle calculating means for calculating a target inclination angle of the main body;
When the drive pattern is not selected, the control means controls the leg so that the main body tilts to the target tilt angle.
4. The walking robot according to claim 3, wherein the determining means determines that the walking robot falls when the deviation between the inclination angle of the main body detected by the inclination angle sensor and the target inclination angle becomes a predetermined angle or more. .