JP6655410B2 - Actuator control device - Google Patents

Actuator control device Download PDF

Info

Publication number
JP6655410B2
JP6655410B2 JP2016018269A JP2016018269A JP6655410B2 JP 6655410 B2 JP6655410 B2 JP 6655410B2 JP 2016018269 A JP2016018269 A JP 2016018269A JP 2016018269 A JP2016018269 A JP 2016018269A JP 6655410 B2 JP6655410 B2 JP 6655410B2
Authority
JP
Japan
Prior art keywords
torque value
angle
drive torque
joint angle
joint
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2016018269A
Other languages
Japanese (ja)
Other versions
JP2017136658A (en
Inventor
村田　直史
直史 村田
裕久 平井
裕久 平井
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Mitsubishi Heavy Industries Ltd
Original Assignee
Mitsubishi Heavy Industries Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Mitsubishi Heavy Industries Ltd filed Critical Mitsubishi Heavy Industries Ltd
Priority to JP2016018269A priority Critical patent/JP6655410B2/en
Publication of JP2017136658A publication Critical patent/JP2017136658A/en
Application granted granted Critical
Publication of JP6655410B2 publication Critical patent/JP6655410B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Description

本発明は、作動機構の制御装置に関する。   The present invention relates to a control device for an operation mechanism.

パワーアシストスーツと呼ばれる装着型の作業補助装置や、ロボットなど、関節を介して連結された複数のリンクをアクチュエータにより駆動する作動機構がある。このような作動機構を作動する場合、そのリンクの現在の位置や姿勢や速度などから、現在の関節角度を求める必要がある。関節角度を求める場合、一般的には、ヤコビ行列が用いられる。   There is an operation mechanism that drives a plurality of links connected via joints by an actuator, such as a wearable work assist device called a power assist suit and a robot. When operating such an operating mechanism, it is necessary to obtain the current joint angle from the current position, posture, speed, and the like of the link. Generally, a Jacobian matrix is used to determine a joint angle.

しかし、ヤコビ行列には、特異点と呼ばれる姿勢が存在する。特異点とは、逆ヤコビ行列が発散する姿勢である。逆ヤコビ行列が発散した場合、関節角度を一義的に算出することができなくなるため、出力が異常となり、適切な作動を行うことができなくなる可能性がある。また、特異点でなくても、特異点近傍である場合でも、逆ヤコビ行列が大きな値となってしまうため、出力が過大となるおそれがある。この特異点における関節角度を特異点角度とすると、特異点角度は、例えば、関節が伸びきった状態が該当する。例えば歩行ロボットなどは、この特異点角度を避けるため、膝関節を意図的に曲げて特異点角度を回避している。また、特許文献１には、仮想リンク構造を用いて膝特異点における出力異常を回避する技術が記載されている。   However, the Jacobian matrix has a posture called a singular point. The singular point is a posture at which the inverse Jacobian matrix diverges. If the inverse Jacobian matrix diverges, the joint angle cannot be unambiguously calculated, so that the output may be abnormal and an appropriate operation may not be performed. In addition, even if the point is not a singular point or is near the singular point, the inverse Jacobian matrix has a large value, so that the output may be excessive. Assuming that the joint angle at the singular point is a singular point angle, the singular point angle corresponds to, for example, a state in which the joint is fully extended. For example, a walking robot or the like avoids the singularity angle by intentionally bending the knee joint to avoid the singularity angle. Further, Patent Literature 1 discloses a technique for avoiding an abnormal output at a knee singular point by using a virtual link structure.

特開２０１４−０１４９２０号公報JP 2014-014920 A

しかし、特許文献１の技術は、仮想リンク構造を用いているため、その制御導入に対し多大な事前準備が必要となり、容易に特異点角度における出力異常を回避できるものとは言えない。また、特許文献１の技術は、定位性を有する位置拘束に基づく制御であるため、例えば非定位性が求められるような自由な動作に対応することは困難である。   However, since the technology of Patent Document 1 uses a virtual link structure, a great deal of preparation is required for control introduction, and it cannot be said that output abnormalities at a singular point angle can be easily avoided. In addition, since the technique of Patent Document 1 is control based on positional constraint having localization, it is difficult to cope with a free operation requiring non-localization, for example.

従って、本発明は、特異点角度における出力異常を容易に回避し、かつ、自由な動作に対応する作動機構の制御装置を提供することを目的とする。   Therefore, an object of the present invention is to provide a control device for an operating mechanism that can easily avoid output abnormalities at a singular point angle and that can respond to free operation.

上述した課題を解決し、目的を達成するために、本開示に係る作動機構の制御装置は、関節を介して連結された複数のリンクをアクチュエータにより駆動する作動機構の制御装置であって、前記リンクの目標とする移動加速度である目標加速度を取得する目標加速度取得部と、前記リンクを前記目標加速度で移動させるためのトルクである駆動トルク値の仮の値である仮駆動トルク値を、前記目標加速度に基づき算出する仮駆動トルク値算出部と、前記作動機構が出力可能な上限のトルク値である上限トルク値に基づき、前記駆動トルク値の上限のトルク値である上限駆動トルク値を取得する上限駆動トルク値取得部と、前記仮駆動トルク値と前記上限駆動トルク値との比率であるトルク比率を算出するトルク比率算出部と、前記仮駆動トルク値が前記上限駆動トルク値以下である場合に、前記仮駆動トルク値を前記駆動トルク値に決定し、前記仮駆動トルク値が前記上限駆動トルク値より大きい場合に、前記トルク比率に基づき、前記仮駆動トルク値を前記上限駆動トルク値以下の値に変換した変換駆動トルク値を、前記駆動トルク値に決定する駆動トルク値決定部と、前記アクチュエータを駆動するトルク値として、決定した前記駆動トルク値を用いて、前記アクチュエータを制御するアクチュエータ制御部と、を有することが好ましい。   In order to solve the above-described problems and achieve the object, a control device for an operating mechanism according to the present disclosure is a control device for an operating mechanism that drives a plurality of links connected via a joint by an actuator, A target acceleration acquisition unit that acquires a target acceleration that is a target movement acceleration of the link, and a tentative drive torque value that is a tentative value of a drive torque value that is a torque for moving the link at the target acceleration. A provisional drive torque value calculation unit that calculates based on a target acceleration, and obtains an upper limit drive torque value that is an upper limit torque value of the drive torque value based on an upper limit torque value that is an upper limit torque value that can be output by the operation mechanism. An upper limit driving torque value acquiring unit, a torque ratio calculating unit that calculates a torque ratio that is a ratio between the tentative driving torque value and the upper limit driving torque value, and a tentative driving torque When the value is equal to or less than the upper limit drive torque value, the temporary drive torque value is determined as the drive torque value, and when the temporary drive torque value is larger than the upper limit drive torque value, based on the torque ratio, A drive torque value determining unit that determines a converted drive torque value obtained by converting the provisional drive torque value to a value equal to or less than the upper limit drive torque value, the drive torque value determining unit that determines the drive torque value; An actuator control unit that controls the actuator using a value.

この制御装置は、特異点角度において、仮駆動トルク値の出力過大に起因する異常（以下、出力異常と記載）が発生する場合にも、駆動トルク値を上限駆動トルク値以下に低下させることで、駆動トルク値の出力異常を容易に抑制することができる。また、この制御装置は、目標加速度に基づき駆動トルク値を算出しているため、非定位性の制御となり、自由な動作に対応することが可能となる。   This control device reduces the drive torque value to be equal to or less than the upper limit drive torque value even when an abnormality (hereinafter, referred to as an output abnormality) due to an excessive output of the temporary drive torque value occurs at the singular point angle. In addition, abnormal output of the driving torque value can be easily suppressed. In addition, since the control device calculates the drive torque value based on the target acceleration, the control device performs non-localization control and can cope with a free operation.

前記作動機構は、複数のリンクのそれぞれに対応する複数のアクチュエータを備えており、前記駆動トルク値は、複数のアクチュエータのそれぞれに対して算出されるものであり、前記駆動トルク値決定部は、複数のアクチュエータに対するそれぞれの仮駆動トルク値のうち、前記上限駆動トルク値に対する前記仮駆動トルク値の超過率が最も高い仮駆動トルク値についてのトルク比率を用いて、それぞれの仮駆動トルク値を前記変換駆動トルク値に変換し、それぞれの前記変換駆動トルク値を、それぞれのアクチュエータに対する前記駆動トルク値に決定することが好ましい。この制御装置は、アクチュエータ間の駆動トルク値の比率を変換前と同様に保つため、駆動トルク値の出力異常をより適切に抑制することができる。   The operating mechanism includes a plurality of actuators respectively corresponding to a plurality of links, the drive torque value is calculated for each of the plurality of actuators, the drive torque value determination unit, Of the respective provisional drive torque values for the plurality of actuators, using the torque ratio for the provisional drive torque value having the highest excess rate of the provisional drive torque value with respect to the upper limit drive torque value, Preferably, the driving torque value is converted into a converted driving torque value, and the converted driving torque value is determined as the driving torque value for each actuator. Since the control device maintains the ratio of the drive torque value between the actuators as before the conversion, the output abnormality of the drive torque value can be more appropriately suppressed.

前記制御装置において、前記駆動トルク値決定部は、前記上限駆動トルク値に対する前記仮駆動トルク値の比率を前記トルク比率とし、前記仮駆動トルク値を前記トルク比率で除することにより、前記変換駆動トルク値を算出することが好ましい。この制御装置は、トルク比率及び変換駆動トルク値を算出しているため、駆動トルク値を上限駆動トルク値以下の適切な値とすることができる。   In the control device, the drive torque value determination unit may set the ratio of the tentative drive torque value to the upper limit drive torque value to the torque ratio, and divide the tentative drive torque value by the torque ratio to perform the conversion drive. Preferably, the torque value is calculated. Since the control device calculates the torque ratio and the converted drive torque value, the drive torque value can be set to an appropriate value equal to or less than the upper limit drive torque value.

前記制御装置において、前記上限駆動トルク値取得部は、例えば前記作動機構の自重、及び前記作動機構にかかる負荷を保持するためのトルクである保持トルク値を取得する保持トルク値取得部と、前記上限トルク値から前記保持トルク値を差し引いた値に基づき、前記上限駆動トルク値を算出する上限駆動トルク値算出部と、を有することが好ましい。この制御装置は、上限駆動トルク値を適切に算出し、駆動トルク値の出力異常をより適切に抑制することができる。   In the control device, the upper limit driving torque value acquisition unit is, for example, a self-weight of the operation mechanism, and a holding torque value acquisition unit that acquires a holding torque value that is a torque for holding a load applied to the operation mechanism, It is preferable to have an upper limit drive torque value calculation unit that calculates the upper limit drive torque value based on a value obtained by subtracting the holding torque value from the upper limit torque value. This control device can appropriately calculate the upper limit driving torque value and can more appropriately suppress the output abnormality of the driving torque value.

前記制御装置は、前記目標加速度から算出される行列式を、予め定められたリンクの加速度の上限値の行列式で正規化して補正項を算出する補正項算出部を更に有し、前記駆動トルク値決定部は、前記変換駆動トルク値を前記補正項で補正したトルク値を、前記駆動トルク値として決定し、前記補正項は、前記駆動トルク値によって前記アクチュエータを駆動した場合に、前記リンクの実際の加速度が前記目標加速度より大きくならないように、前記変換駆動トルク値を補正するための係数であることが好ましい。この制御装置は、加速度軌道変化に対応した駆動トルク値を算出することができる。   The control device further includes a correction term calculation unit that calculates a correction term by normalizing a determinant calculated from the target acceleration with a determinant of a predetermined upper limit of the acceleration of the link, and the driving torque The value determining unit determines, as the drive torque value, a torque value obtained by correcting the converted drive torque value with the correction term, and the correction term is, when the actuator is driven by the drive torque value, the link value of the link. Preferably, the coefficient is a coefficient for correcting the converted drive torque value so that the actual acceleration does not become larger than the target acceleration. This control device can calculate a drive torque value corresponding to a change in the acceleration trajectory.

上述した課題を解決し、目的を達成するために、本開示に係る作動機構の制御装置は、関節を介して連結された複数のリンクをアクチュエータにより駆動する作動機構の制御装置であって、前記関節を介して隣接するリンク同士の現在の角度である現在関節角度を取得する関節角度取得部と、前記現在関節角度が、前記現在関節角度に基づいて算出される逆ヤコビ行列が発散する特異点角度である場合に、前記現在関節角度から所定の変位角度だけ異なった前記リンク同士の仮想の角度である仮想関節角度を算出する仮想関節角度取得部と、前記リンクの目標とする移動加速度である目標加速度を取得する目標加速度取得部と、前記目標加速度と前記仮想関節角度とに基づき、前記アクチュエータを駆動するための駆動トルク値を算出する駆動トルク値算出部と、前記アクチュエータを駆動するトルク値として前記駆動トルク値を用いて、前記アクチュエータを制御するアクチュエータ制御部と、を有する。   In order to solve the above-described problems and achieve the object, a control device for an operating mechanism according to the present disclosure is a control device for an operating mechanism that drives a plurality of links connected via a joint by an actuator, A joint angle acquisition unit that acquires a current joint angle that is a current angle between adjacent links via a joint, and a singular point at which the inverse Jacobian matrix in which the current joint angle is calculated based on the current joint angle diverges When the angle is an angle, a virtual joint angle acquisition unit that calculates a virtual joint angle that is a virtual angle between the links different from the current joint angle by a predetermined displacement angle, and a target movement acceleration of the link. A target acceleration acquisition unit that acquires a target acceleration; and a drive that calculates a drive torque value for driving the actuator based on the target acceleration and the virtual joint angle. A torque value calculation unit, by using the driving torque value as a torque value for driving the actuator, having an actuator control unit for controlling the actuator.

この制御装置は、現在関節角度が特異点角度であっても、特異点角度から変位した、逆ヤコビ行列が発散しない仮想関節角度に基づき、駆動トルク値を適切に算出することができる。従って、この制御装置は、仮想関節角度を算出するだけで、駆動トルク値の出力異常を容易に抑制することができる。また、この制御装置は、目標加速度に基づき駆動トルク値を算出しているため、非定位性の制御となり、自由な動作に対応することが可能となる。   This control device can appropriately calculate the drive torque value based on the virtual joint angle that is displaced from the singular point angle and does not diverge in the inverse Jacobian matrix, even if the current joint angle is the singular point angle. Therefore, this control device can easily suppress the abnormal output of the driving torque value only by calculating the virtual joint angle. In addition, since the control device calculates the drive torque value based on the target acceleration, the control device performs non-localization control and can cope with a free operation.

前記制御装置において、前記仮想関節角度取得部は、前記仮想関節角度である場合の前記リンク同士の距離が、前記現在関節角度である場合の前記リンク同士の距離よりも短くなるように、前記仮想関節角度を算出することが好ましい。この制御装置は、関節が伸びきった場合の特異点においても、駆動トルク値の出力異常を適切に抑制することができる。   In the control device, the virtual joint angle acquisition unit may be configured to set the virtual joint angle such that a distance between the links at the virtual joint angle is shorter than a distance between the links at the current joint angle. It is preferable to calculate the joint angle. This control device can appropriately suppress the abnormal output of the drive torque value even at a singular point when the joint is fully extended.

前記制御装置は、前記現在関節角度に基づき、前記リンクの現在の座標である現在座標を算出する座標算出部を更に有し、前記仮想関節角度取得部は、前記現在座標のうち、少なくとも１つの軸に対応する座標を所定の変位距離だけ変化させた座標である仮想座標を算出する仮想座標算出部と、前記リンクが前記仮想座標に位置すると仮定した場合の前記リンク同士の角度を、前記仮想関節角度として算出する仮想関節角度算出部と、を有することが好ましい。この制御装置は、変位距離だけ関節角度が変位した仮想関節角度に基づき駆動トルク値を算出するため、特異点であっても、駆動トルク値の出力異常を適切に抑制することができる。   The control device further includes a coordinate calculation unit that calculates a current coordinate that is a current coordinate of the link based on the current joint angle, and the virtual joint angle acquisition unit includes at least one of the current coordinates. A virtual coordinate calculation unit that calculates virtual coordinates that are coordinates obtained by changing a coordinate corresponding to an axis by a predetermined displacement distance, and an angle between the links when the links are assumed to be located at the virtual coordinates. A virtual joint angle calculation unit that calculates a joint angle. This control device calculates the drive torque value based on the virtual joint angle in which the joint angle is displaced by the displacement distance, so that even at a singular point, it is possible to appropriately suppress the abnormal output of the drive torque value.

前記制御装置において、前記仮想関節角度取得部は、前記現在関節角度と前記仮想関節角度との関係を記憶する関係記憶部と、前記現在関節角度と前記関係に基づき、前記仮想関節角度を算出する仮想関節角度算出部と、を有し、前記関係は、前記現在関節角度の値に応じて、前記変位角度が異なるものであることが好ましい。この制御装置は、関節角度関係に基づき仮想関節角度を算出するため、駆動トルク値の出力異常を適切に抑制することができる。   In the control device, the virtual joint angle acquisition unit calculates the virtual joint angle based on the current joint angle and the relationship, and a relationship storage unit that stores a relationship between the current joint angle and the virtual joint angle. And a virtual joint angle calculation unit, wherein the relationship is such that the displacement angle differs according to the value of the current joint angle. Since the control device calculates the virtual joint angle based on the joint angle relationship, it is possible to appropriately suppress the abnormal output of the driving torque value.

前記制御装置において、前記仮想関節角度取得部は、前記現在関節角度が前記特異点角度以外の角度である場合、前記変位角度を０として、前記仮想関節角度として前記現在関節角度を用いることが好ましい。この制御装置は、特異点角度である場合に仮想関節角度を用いて出力異常を抑制しつつ、特異点でない場合には、実際の関節角度を用いることで、より正確に出力を行うことができる。   In the control device, when the current joint angle is an angle other than the singular point angle, the virtual joint angle acquisition unit preferably sets the displacement angle to 0 and uses the current joint angle as the virtual joint angle. . This control device can perform more accurate output by using the actual joint angle when it is not a singular point while suppressing the output abnormality using the virtual joint angle when it is the singular point angle. .

前記制御装置において、前記変位角度は、前記現在関節角度が、前記特異点角度から、前記特異点角度と所定の角度だけ離れた特異点近傍角度までの間である場合に、０より大きい値となり、前記現在関節角度が、前記特異点角度から前記特異点近傍角度の範囲外である場合に、０となることが好ましい。この制御装置は、逆ヤコビ行列の値が過大になる特異点近傍角度においても、駆動トルク値の出力異常を抑制することができる。   In the control device, the displacement angle is a value greater than 0 when the current joint angle is between the singularity angle and the singularity near angle separated from the singularity angle by a predetermined angle. It is preferable that the current joint angle be 0 when the current joint angle is out of the range of the singular point angle and the singular point neighboring angle. This control device can suppress the abnormal output of the drive torque value even at an angle near the singular point where the value of the inverse Jacobian matrix becomes excessive.

前記制御装置は、前記変位角度だけ関節角度を変化させた場合の前記リンクの角加速度の変化量を示す係数である補正係数を記憶する補正係数記憶部を更に有し、前記駆動トルク値算出部は、前記目標加速度と前記仮想関節角度と前記補正係数とに基づき、前記駆動トルク値を算出し、前記補正係数は、所定の関節角度である第１基準関節角度に基づき算出された前記リンクの角加速度である第１基準角加速度と、前記第１基準関節角度から前記変位角度だけ異なる第２基準関節角度に基づき算出された前記リンクの角加速度である第２基準角加速度とに基づき算出されることが好ましい。この制御装置は、補正係数で補正して駆動トルク値を算出するため、制御装置は、駆動トルク値の出力異常をより適切に抑制することができる。   The control device further includes a correction coefficient storage unit that stores a correction coefficient that is a coefficient indicating a change amount of the angular acceleration of the link when the joint angle is changed by the displacement angle, and the drive torque value calculation unit Calculates the drive torque value based on the target acceleration, the virtual joint angle, and the correction coefficient, and the correction coefficient is calculated based on a first reference joint angle that is a predetermined joint angle. Calculated based on a first reference angular acceleration that is an angular acceleration and a second reference angular acceleration that is an angular acceleration of the link calculated based on a second reference joint angle different from the first reference joint angle by the displacement angle. Preferably. Since this control device calculates the drive torque value by correcting with the correction coefficient, the control device can more appropriately suppress the output abnormality of the drive torque value.

前記作動機構は、前記関節として、腰関節と膝関節と足首関節とを有し、前記リンクとして、前記腰関節と前記膝関節との間に設けられる大腿リンクと、前記膝関節と前記足首関節との間に設けられる下腿リンクと、前記足首関節の前記下腿リンクと反対側に設けられる足リンクと、を有し、前記アクチュエータにより前記関節を介して隣接するリンクの間でアシスト力を発生させて、装着者の筋力を補助するパワーアシストスーツであって、前記仮想関節角度取得部は、前記膝関節の現在関節角度が特異点角度である場合に、前記腰関節、前記膝関節及び前記足首関節の仮想関節角度を算出することが好ましい。この制御装置は、膝関節が特異点である場合に、適切に出力異常を抑制することができる。   The operating mechanism has a waist joint, a knee joint, and an ankle joint as the joint, and a thigh link provided between the hip joint and the knee joint as the link, the knee joint, and the ankle joint And a leg link provided on the opposite side of the ankle joint from the lower leg link, and the actuator generates an assist force between adjacent links via the joint by the actuator. A power assist suit for assisting a wearer's muscular strength, wherein the virtual joint angle acquisition unit is configured to control the waist joint, the knee joint, and the ankle when a current joint angle of the knee joint is a singular point angle. It is preferable to calculate a virtual joint angle of the joint. This control device can appropriately suppress the output abnormality when the knee joint is a singular point.

本発明によれば、特異点角度における出力異常を容易に回避し、かつ、自由な動作に対応することができる。   According to the present invention, it is possible to easily avoid an output abnormality at a singular point angle and to cope with a free operation.

図１は、第１実施形態に係るパワーアシストスーツを表す概略図である。FIG. 1 is a schematic diagram illustrating a power assist suit according to the first embodiment. 図２は、第１実施形態に係るパワーアシストスーツの装着状態を表す正面図である。FIG. 2 is a front view illustrating a mounted state of the power assist suit according to the first embodiment. 図３は、第１実施形態に係るパワーアシストスーツの装着状態を表す側面図である。FIG. 3 is a side view illustrating the mounted state of the power assist suit according to the first embodiment. 図４は、第１実施形態に係る制御装置のブロック図である。FIG. 4 is a block diagram of the control device according to the first embodiment. 図５は、現在関節角度及び現在座標を説明するための模式図である。FIG. 5 is a schematic diagram for explaining the current joint angle and the current coordinates. 図６は、仮想座標及び仮想関節角度を説明するための模式図である。FIG. 6 is a schematic diagram for explaining virtual coordinates and virtual joint angles. 図７は、第１実施形態に係る駆動トルク値の算出フローを説明するフローチャートである。FIG. 7 is a flowchart illustrating a calculation flow of the drive torque value according to the first embodiment. 図８は、第２実施形態における現在関節角度と変位距離との関係を示すグラフである。FIG. 8 is a graph showing the relationship between the current joint angle and the displacement distance in the second embodiment. 図９は、第３実施形態に係る制御装置のブロック図である。FIG. 9 is a block diagram of a control device according to the third embodiment. 図１０は、第３実施形態における現在関節角度と仮想関節角度との関係を示すグラフである。FIG. 10 is a graph showing the relationship between the current joint angle and the virtual joint angle in the third embodiment. 図１１は、第４実施形態に係る制御装置のブロック図である。FIG. 11 is a block diagram of a control device according to the fourth embodiment. 図１２は、第５実施形態に係る制御装置のブロック図である。FIG. 12 is a block diagram of a control device according to the fifth embodiment. 図１３は、第５実施形態に係る駆動トルク値の算出フローを説明するフローチャートである。FIG. 13 is a flowchart illustrating a drive torque value calculation flow according to the fifth embodiment.

以下に添付図面を参照して、本発明の好適な実施形態を詳細に説明する。なお、この実施形態により本発明が限定されるものではなく、また、実施形態が複数ある場合には、各実施例を組み合わせて構成するものも含むものである。   Hereinafter, preferred embodiments of the present invention will be described in detail with reference to the accompanying drawings. It should be noted that the present invention is not limited by the embodiment, and when there are a plurality of embodiments, the invention includes a combination of the embodiments.

（第１実施形態）
図１は、第１実施形態に係るパワーアシストスーツを表す概略図である。図２は、第１実施形態に係るパワーアシストスーツの装着状態を表す正面図である。図３は、第１実施形態に係るパワーアシストスーツの装着状態を表す側面図である。 (1st Embodiment)
FIG. 1 is a schematic diagram illustrating a power assist suit according to the first embodiment. FIG. 2 is a front view illustrating a mounted state of the power assist suit according to the first embodiment. FIG. 3 is a side view illustrating the mounted state of the power assist suit according to the first embodiment.

（パワーアシストスーツの概要）
パワーアシストスーツ１０は、関節を介して連結された複数のリンクをアクチュエータにより駆動する作動機構である。より具体的には、パワーアシストスーツ１０は、複数のリンクが関節を介して連結され、関節を介して連結された隣接するリンクの間でアシスト力を発生するアクチュエータを駆動して、装着した装着者の筋力を補助する作動機構である。図１から図３に示すように、パワーアシストスーツ１０は、装着者の腰に装着される腰パーツ１１と、肩に装着される肩パーツ１２と、脚に装着される脚パーツ１３Ａ、１３Ｂとを有している。 (Overview of power assist suit)
The power assist suit 10 is an operation mechanism that drives a plurality of links connected via a joint by an actuator. More specifically, the power assist suit 10 is configured such that a plurality of links are connected via a joint, and an actuator that generates an assist force between adjacent links connected via the joint is driven to be mounted. It is an operating mechanism that assists the muscle strength of the elderly. As shown in FIGS. 1 to 3, the power assist suit 10 includes a waist part 11 attached to a wearer's waist, a shoulder part 12 attached to a shoulder, and leg parts 13A and 13B attached to legs. have.

腰パーツ１１は、腰装着部２１と、給電装置２２と、大腿駆動機構２３Ａ、２３Ｂとを有する。腰装着部２１は、パワーアシストスーツ１０の装着者、すなわちパワーアシストスーツを装着する者（装着者）の腰の周囲に位置するように形成された、Ｃ形リング形状の部材である。給電装置２２は、腰装着部２１に取り付けられており、装着者の背中側に配置される。   The waist part 11 has a waist mounting part 21, a power feeding device 22, and thigh drive mechanisms 23A and 23B. The waist mounting part 21 is a C-shaped ring-shaped member formed so as to be positioned around the waist of a wearer of the power assist suit 10, that is, a wearer of the power assist suit. The power supply device 22 is attached to the waist mounting portion 21 and is arranged on the back side of the wearer.

給電装置２２は、パワーアシストスーツ１０が備える制御装置１及びアクチュエータ等の機器類に電力を供給する装置である。本実施形態において、給電装置２２は、パワーアシストスーツ１０の外部の電源２２Ｂから、電線２２Ｃを介して電力の供給を受ける。給電装置２２は、外部の電源２２Ｂからの電力を、パワーアシストスーツ１０が備える機器類に適した電圧等に変換してこれらに供給する。外部の電源２２Ｂは、例えば、ＡＣ（Alternating Current）電源である。給電装置２２は外部の電源２２Ｂから電力の供給を受ける装置に限定されるものではなく、例えば、パワーアシストスーツ１０に搭載される蓄電器から電力の供給を受ける装置であってもよい。   The power supply device 22 is a device that supplies power to devices such as the control device 1 and the actuator included in the power assist suit 10. In the present embodiment, the power supply device 22 receives power supply from a power supply 22B outside the power assist suit 10 via an electric wire 22C. The power supply device 22 converts the power from the external power supply 22B into a voltage or the like suitable for the devices included in the power assist suit 10 and supplies the converted voltage or the like. The external power supply 22B is, for example, an AC (Alternating Current) power supply. The power supply device 22 is not limited to a device that receives power supply from the external power supply 22B, and may be, for example, a device that receives power supply from a battery mounted on the power assist suit 10.

大腿駆動機構２３Ａ、２３Ｂは、腰装着部２１の下方に連結されており、それぞれ装着者の左右の側部に配置されている。大腿駆動機構２３Ａ、２３Ｂは、パワーアシストスーツ１０の腰関節に相当する。大腿駆動機構２３Ａは、脚パーツ１３Ａの大腿装着部４１Ａを腰装着部２１に対して所定の軸線Ｙｗｌの周りを回動させることで、左脚の大腿の動作に応じて筋力を補助するものである。大腿駆動機構２３Ｂは、脚パーツ１３Ｂの大腿装着部４１Ｂを腰装着部２１に対して所定の軸線Ｙｗｒの周りを回動させることで、右脚の大腿の動作に応じて筋力を補助するものである。   The thigh drive mechanisms 23A and 23B are connected to the lower part of the waist mounting part 21, and are respectively arranged on the left and right sides of the wearer. The thigh drive mechanisms 23A and 23B correspond to the waist joint of the power assist suit 10. The thigh drive mechanism 23A assists muscle strength according to the movement of the thigh of the left leg by rotating the thigh mounting portion 41A of the leg part 13A around the predetermined axis Ywl with respect to the waist mounting portion 21. is there. The thigh drive mechanism 23B assists muscle strength in accordance with the movement of the thigh of the right leg by rotating the thigh mounting portion 41B of the leg part 13B around the predetermined axis Ywr with respect to the waist mounting portion 21. is there.

肩パーツ１２は、背装着部３１と、装着者の左右の肩に掛止するように配置される左肩装着部３２Ａ及び右肩装着部３２Ｂとを有する。左肩装着部３２Ａ及び右肩装着部３２Ｂは、装着者の背中側に配置される背装着部３１によって腰パーツ１１の腰装着部２１に連結されている。   The shoulder part 12 has a back mounting part 31 and a left shoulder mounting part 32A and a right shoulder mounting part 32B that are arranged to be hooked on the left and right shoulders of the wearer. The left shoulder mounting portion 32A and the right shoulder mounting portion 32B are connected to the waist mounting portion 21 of the waist part 11 by a back mounting portion 31 arranged on the back side of the wearer.

脚パーツ１３Ａ、１３Ｂは、左脚及び右脚に対応して配置されている。左脚に対応する脚パーツ１３Ａは、大腿装着部４１Ａと、下腿装着部４２Ａと、足装着部４３Ａと、下腿駆動機構４５Ａと、脚駆動制御装置４６Ａとを有する。   The leg parts 13A and 13B are arranged corresponding to the left leg and the right leg. The leg part 13A corresponding to the left leg has a thigh mounting portion 41A, a lower leg mounting portion 42A, a leg mounting portion 43A, a lower leg driving mechanism 45A, and a leg driving control device 46A.

大腿装着部４１Ａは、装着者の左脚の大腿に沿って配置され、下腿装着部４２Ａは、装着者の左脚の下腿に沿って配置される。大腿装着部４１Ａは、パワーアシストスーツ１０の大腿リンクに相当し、下腿装着部４２Ａは、パワーアシストスーツ１０の下腿リンクに相当する。大腿装着部４１Ａの一端部は、腰パーツ１１の腰装着部２１に回動可能に連結されている。下腿装着部４２Ａの一端部は、大腿装着部４１Ａの他端部に回動可能に連結されており、下腿装着部４２Ａの他端部は、回動軸４７Ａにより足装着部４３Ａに回動可能に連結されている。足装着部４３Ａは、装着者の左足部（左足首から下の部分）に装着される靴部である。回動軸４７Ａはパワーアシストスーツ１０の足首関節に相当し、足装着部４３Ａはパワーアシストスーツ１０の足リンクに相当する。   The thigh attachment portion 41A is arranged along the thigh of the wearer's left leg, and the lower leg attachment portion 42A is arranged along the lower leg of the wearer's left leg. The thigh mounting portion 41A corresponds to a thigh link of the power assist suit 10, and the lower leg mounting portion 42A corresponds to a lower link of the power assist suit 10. One end of the thigh mounting portion 41A is rotatably connected to the waist mounting portion 21 of the waist part 11. One end of the lower leg mounting portion 42A is rotatably connected to the other end of the lower leg mounting portion 41A, and the other end of the lower leg mounting portion 42A is rotatable to the foot mounting portion 43A by a rotary shaft 47A. It is connected to. The foot mounting portion 43A is a shoe portion that is mounted on the wearer's left foot (a portion below the left ankle). The rotating shaft 47A corresponds to an ankle joint of the power assist suit 10, and the foot mounting portion 43A corresponds to a foot link of the power assist suit 10.

下腿駆動機構４５Ａは、大腿装着部４１Ａの他端部に取り付けられている。下腿駆動機構４５Ａは、下腿駆動機構４５Ａを大腿装着部４１Ａに対して所定の軸線Ｙｎｌの周りを回動させることで、筋力を補助する。下腿駆動機構４５Ａは、パワーアシストスーツ１０の膝関節に相当する。脚駆動制御装置４６Ａは、大腿装着部４１Ａに取り付けられており、大腿駆動機構２３Ａ及び下腿駆動機構４５Ａの動作を制御する。脚駆動制御装置４６Ａ、大腿駆動機構２３Ａ及び下腿駆動機構４５Ａは、給電装置２２から電力が供給される。   The lower leg drive mechanism 45A is attached to the other end of the thigh mounting portion 41A. The lower leg drive mechanism 45A assists muscle strength by rotating the lower leg drive mechanism 45A about a predetermined axis Ynl with respect to the thigh mounting portion 41A. The lower leg drive mechanism 45A corresponds to a knee joint of the power assist suit 10. The leg drive control device 46A is attached to the thigh mounting portion 41A, and controls the operations of the thigh drive mechanism 23A and the lower leg drive mechanism 45A. Power is supplied from the power supply device 22 to the leg drive control device 46A, the thigh drive mechanism 23A, and the lower leg drive mechanism 45A.

右脚に対応する脚パーツ１３Ｂは、脚パーツ１３Ａと同様に、大腿装着部４１Ｂと、下腿装着部４２Ｂと、足装着部４３Ｂと、下腿駆動機構４５Ｂと、脚駆動制御装置４６Ｂとを有する。   Like the leg part 13A, the leg part 13B corresponding to the right leg includes a thigh mounting part 41B, a lower leg mounting part 42B, a foot mounting part 43B, a lower leg driving mechanism 45B, and a leg driving control device 46B.

大腿装着部４１Ｂは、装着者の右脚の大腿に沿って配置され、下腿装着部４２Ｂは、装着者の右脚の下腿に沿って配置される。大腿装着部４１Ｂは、パワーアシストスーツ１０の大腿リンクに相当し、下腿装着部４２Ｂは、パワーアシストスーツ１０の下腿リンクに相当する。大腿装着部４１Ｂの一端部は、腰パーツ１１の腰装着部２１に回動可能に連結されている。下腿装着部４２Ｂの一端部は、大腿装着部４１Ｂの他端部に回動可能に連結されており、下腿装着部４２Ｂの他端部は、回動軸４７Ｂにより足装着部４３Ｂに回動可能に連結されている。足装着部４３Ｂは、装着者の右足部（右足首から下の部分）に装着される靴部として形成されている。回動軸４７Ｂはパワーアシストスーツ１０の足首関節に相当し、足装着部４３Ｂはパワーアシストスーツ１０の足リンクに相当する。   The thigh mounting portion 41B is disposed along the thigh of the right leg of the wearer, and the lower leg mounting portion 42B is disposed along the lower leg of the right leg of the wearer. The thigh mounting portion 41B corresponds to a thigh link of the power assist suit 10, and the lower leg mounting portion 42B corresponds to a lower link of the power assist suit 10. One end of the thigh mounting portion 41B is rotatably connected to the waist mounting portion 21 of the waist part 11. One end of the lower leg mounting portion 42B is rotatably connected to the other end of the lower leg mounting portion 41B, and the other end of the lower leg mounting portion 42B is rotatable to the foot mounting portion 43B by a rotary shaft 47B. It is connected to. The foot mounting portion 43B is formed as a shoe portion to be mounted on the right foot (a portion below the right ankle) of the wearer. The rotation shaft 47B corresponds to an ankle joint of the power assist suit 10, and the foot mounting portion 43B corresponds to a foot link of the power assist suit 10.

下腿駆動機構４５Ｂは、大腿装着部４１Ｂの他端部に取り付けられている。下腿駆動機構４５Ｂは、下腿駆動機構４５Ｂを大腿装着部４１Ｂに対して所定の軸線Ｙｎｒの周りを回動させることで、筋力を補助するものである。下腿駆動機構４５Ｂは、パワーアシストスーツ１０の膝関節に相当する。脚駆動制御装置４６Ｂは、大腿装着部４１Ｂに取り付けられており、大腿駆動機構２３Ｂ及び下腿駆動機構４５Ｂの動作を制御する。脚駆動制御装置４６Ｂ、大腿駆動機構２３Ｂ及び下腿駆動機構４５Ｂは、給電装置２２から電力が供給される。   The lower leg drive mechanism 45B is attached to the other end of the thigh mounting portion 41B. The lower leg drive mechanism 45B assists muscle strength by rotating the lower leg drive mechanism 45B around a predetermined axis Ynr with respect to the thigh mounting portion 41B. The lower leg drive mechanism 45B corresponds to a knee joint of the power assist suit 10. The leg drive control unit 46B is attached to the thigh mounting unit 41B, and controls the operations of the thigh drive mechanism 23B and the lower leg drive mechanism 45B. Electric power is supplied from the power supply device 22 to the leg drive control device 46B, the thigh drive mechanism 23B, and the lower leg drive mechanism 45B.

大腿駆動機構２３Ａ、２３Ｂ及び下腿駆動機構４５Ａ、４５Ｂは、それぞれアクチュエータを備えている。本実施形態において、これらが備えるアクチュエータは電動機であるが、これに限定されない。本実施形態において、それぞれの脚パーツ１３Ａ、１３Ｂは、大腿駆動機構２３Ａ、２３Ｂ及び下腿駆動機構４５Ａ、４５Ｂを備えているので、制御軸は２軸である。本実施形態において、パワーアシストスーツ１０が備える制御軸は、１つの脚パーツ１３Ａ又は脚パーツ１３Ｂあたりにおいて２軸に限定されるものではない。   The thigh drive mechanisms 23A and 23B and the lower leg drive mechanisms 45A and 45B each include an actuator. In the present embodiment, the actuator provided in these is an electric motor, but is not limited to this. In the present embodiment, since each leg part 13A, 13B includes the thigh drive mechanisms 23A, 23B and the lower leg drive mechanisms 45A, 45B, the control axes are two axes. In the present embodiment, the number of control axes provided in the power assist suit 10 is not limited to two axes per one leg part 13A or one leg part 13B.

図２に示すように、足装着部４３Ａの底部、すなわち靴底には、荷重検出装置４４Ａが設けられている。また、足装着部４３Ｂの底部、すなわち靴底には、荷重検出装置４４Ｂが設けられている。荷重検出装置４４Ａ、４４Ｂは、足装着部４３Ａ、４３Ｂに作用する荷重、すなわち装着者の足底荷重を検出する。本実施形態において、荷重検出装置４４Ａ、４４Ｂは、例えば、６軸のロードセルであるが、これに限定されるものではない。以下において、荷重検出装置４４Ａ、４４Ｂを区別しない場合、適宜荷重検出装置４４という。   As shown in FIG. 2, a load detection device 44A is provided on the bottom of the foot mounting portion 43A, that is, on the shoe sole. A load detecting device 44B is provided on the bottom of the foot mounting portion 43B, that is, on the shoe sole. The load detection devices 44A and 44B detect the load acting on the foot mounting portions 43A and 43B, that is, the sole load of the wearer. In the present embodiment, the load detection devices 44A and 44B are, for example, six-axis load cells, but are not limited thereto. Hereinafter, when the load detection devices 44A and 44B are not distinguished, they are referred to as the load detection device 44 as appropriate.

本実施形態において、背装着部３１の背面には、制御装置１が設けられている。制御装置１は、例えば、マイクロコンピュータである。制御装置１は、荷重検出装置４４Ａ、４４Ｂの検出値に基づいて、各リンク、すなわち、大腿駆動機構２３Ａ、２３Ｂ、下腿駆動機構４５Ａ、４５Ｂ、及び足装着部４３Ａ、４３Ｂの動作を制御する。脚駆動制御装置４５Ａ、４５Ｂは、制御装置１からの指令に基づいて、大腿駆動機構２３Ａ、２３Ｂのアクチュエータ、下腿駆動機構４５Ａ、４５Ｂのアクチュエータ、及び足装着部４３Ａ、４３Ｂのアクチュエータを駆動する。本実施形態において、脚駆動制御装置４６Ａ、４６Ｂは、電動機を制御するモータードライバである。次に、制御装置１について説明する。   In the present embodiment, the control device 1 is provided on the back surface of the back mounting part 31. The control device 1 is, for example, a microcomputer. The control device 1 controls the operation of each link, that is, the thigh drive mechanisms 23A and 23B, the lower leg drive mechanisms 45A and 45B, and the foot mounting portions 43A and 43B based on the detection values of the load detection devices 44A and 44B. The leg drive control devices 45A and 45B drive the actuators of the thigh drive mechanisms 23A and 23B, the actuators of the lower leg drive mechanisms 45A and 45B, and the actuators of the foot mounting units 43A and 43B based on the instructions from the control device 1. In the present embodiment, the leg drive controllers 46A and 46B are motor drivers that control the electric motor. Next, the control device 1 will be described.

（制御装置について）
図４は、第１実施形態に係る制御装置のブロック図である。図４に示すように、制御装置１は、第１制御部１Ａと、第２制御部１Ｂと、アクチュエータ制御部１Ｃとを有する。第１制御部１Ａは、パワーアシストスーツ１０の自重を受け持つ制御、及びパワーアシストスーツ１０の装着者の脚に作用する負荷を補助する制御のためのトルクを算出する。後者の制御は、例えば、パワーアシストスーツ１０の装着者が物体を持つ際に、物体の質量による負荷をパワーアシストスーツ１０のリンク機構で受ける制御である。一方、第２制御部１Ｂは、リンクを目標加速度で移動させるためのトルクである駆動トルク値τ１を算出する。第１制御部１Ａが算出するトルク値を保持トルク値τ２とすると、アクチュエータ制御部１Ｃは、駆動トルク値τ１及び保持トルク値τ２を取得して、それらのトルク値を合計して、アクチュエータを駆動する。 (About the control device)
FIG. 4 is a block diagram of the control device according to the first embodiment. As shown in FIG. 4, the control device 1 has a first control unit 1A, a second control unit 1B, and an actuator control unit 1C. The first control unit 1 </ b> A calculates a torque for controlling the own weight of the power assist suit 10 and a control for assisting a load acting on a leg of a wearer of the power assist suit 10. The latter control is, for example, a control in which when the wearer of the power assist suit 10 has an object, the link mechanism of the power assist suit 10 receives a load due to the mass of the object. On the other hand, the second control unit 1B calculates a drive torque value τ1, which is a torque for moving the link at the target acceleration. Assuming that the torque value calculated by the first control unit 1A is the holding torque value τ2, the actuator control unit 1C acquires the driving torque value τ1 and the holding torque value τ2, sums those torque values, and drives the actuator. I do.

以下、アクチュエータを駆動するためのトルク値についてより詳細に説明する。アクチュエータを実際に駆動するトルク値、すなわちアクチュエータが出力するトルク値を総トルク値τ０とすると、総トルク値τ０は、一般的に次の式（１）で表される。   Hereinafter, the torque value for driving the actuator will be described in more detail. Assuming that a torque value that actually drives the actuator, that is, a torque value output by the actuator is a total torque value τ0, the total torque value τ0 is generally represented by the following equation (1).

τ０＝Ｍ・θ”＋ｈ（θ，θ’）＋ｇ(θ) ・・・（１）   τ0 = M · θ ″ + h (θ, θ ′) + g (θ) (1)

ここで、Ｍはリンクの慣性モーメントであり、関節角度θから算出される値である。θ’は関節角速度であり、θ”は関節角加速度である。Ｍ・θ”は、第２制御部１Ｂによって算出される駆動トルク値τ１である。ｇ(θ)は、重力に関連する項であり、第１制御部１Ａによって算出される保持トルク値τ２である。ｈ（θ，θ’）は、コリオリ力に対応する項であり、総トルク値τ０に対する寄与率が低いため、本実施形態では無視される。すなわち、本実施形態において、アクチュエータ制御部１Ｃは、駆動トルク値τ１及び保持トルク値τ２の合計値を、総トルク値τ０として算出し、その総トルク値τ０で、アクチュエータを駆動する。ただし、アクチュエータ制御部１Ｃは、ｈ（θ，θ’）についても算出し、駆動トルク値τ１と保持トルク値τ２とｈ（θ，θ’）との合計値を総トルク値τ０として算出してもよい。   Here, M is a moment of inertia of the link, and is a value calculated from the joint angle θ. θ ′ is a joint angular velocity, θ ″ is a joint angular acceleration. M · θ ″ is a drive torque value τ1 calculated by the second control unit 1B. g (θ) is a term related to gravity, and is a holding torque value τ2 calculated by the first control unit 1A. h (θ, θ ′) is a term corresponding to the Coriolis force, and is ignored in the present embodiment because its contribution to the total torque value τ0 is low. That is, in the present embodiment, the actuator control unit 1C calculates the total value of the driving torque value τ1 and the holding torque value τ2 as the total torque value τ0, and drives the actuator with the total torque value τ0. However, the actuator control unit 1C also calculates h (θ, θ ′) and calculates the sum of the driving torque value τ1, the holding torque value τ2, and h (θ, θ ′) as the total torque value τ0. Is also good.

さらに言えば、駆動トルク値τ１は、関節角加速度θ”に関連するトルクであり、リンクの目標とする移動加速度である目標加速度を実現するためのトルクである。言い換えれば、駆動トルク値τ１は、装着者の移動を補助するために先んじてリンクを動かすためのトルク値である。また、保持トルク値τ２は、関節角度θに関連するトルクであり、パワーアシストスーツ１０の自重や負荷を支えるためのトルクである。従って、アクチュエータ制御部１Ｃは、装着者の移動を補助するためのトルク（駆動トルク値τ１）と、重さを支えるためのトルク（保持トルク値τ２）との両方を実現できる総トルク値τ０で、アクチュエータを駆動する。   Furthermore, the driving torque value τ1 is a torque related to the joint angular acceleration θ ″, and is a torque for realizing a target acceleration that is a target moving acceleration of the link. In other words, the driving torque value τ1 is The holding torque value τ2 is a torque related to the joint angle θ and supports the weight and load of the power assist suit 10 in advance of moving the link to assist the movement of the wearer. Therefore, the actuator control unit 1C realizes both a torque for supporting the movement of the wearer (driving torque value τ1) and a torque for supporting the weight (holding torque value τ2). The actuator is driven with the total torque value τ0 that can be obtained.

次に、駆動トルク値τ１の算出式について説明する。ここで、リンクの速度ｘ’と関節角速度θ’とは、ヤコビ行列Ｊを用いて、次の式（２）のような関係で表される。式（２）の両辺を微分すると式（３）のようになり、それを関節角加速度θ”でまとめると式（４）のようになる。   Next, a calculation formula of the drive torque value τ1 will be described. Here, the link speed x ′ and the joint angular speed θ ′ are expressed by the following equation (2) using the Jacobian matrix J. Equation (3) is obtained by differentiating both sides of equation (2), and is summarized as equation (4) by summing them up with joint angular acceleration θ ″.

ｘ’＝Ｊ・θ’ ・・・（２）
ｘ”＝Ｊ’・θ’＋Ｊ・θ” ・・・（３）
θ”-＝Ｊ＃（ｘ”−Ｊ’・θ’） ・・・（４） x ′ = J · θ ′ (2)
x ″ = J ′ · θ ′ + J · θ ″ (3)
θ ″ − = J # (x ″ −J ′ · θ ′) (4)

なお、式（３）におけるｘ”は直交三次元空間座標における加速度ベクトルであり、Ｊ’はヤコビ行列Ｊの一次微分値である。また、式（４）におけるＪ＃は、ヤコビ行列Ｊが正方行列でない場合におけるヤコビ行列Ｊの逆行列である。駆動トルク値τ１は、式（４）の右辺をＭ・θ”に代入すると、一般的に、次の式（５）のように求められる。 In the expression (3), x ″ is an acceleration vector in orthogonal three-dimensional space coordinates, J ′ is a first-order differential value of the Jacobi matrix J. J # in the expression (4) is a square of the Jacobi matrix J This is the inverse matrix of the Jacobi matrix J when the matrix is not a matrix.The drive torque value τ1 is generally obtained by the following equation (5) by substituting the right side of equation (4) into M · θ ″.

τ１＝Ｍ・Ｊ＃（ｘ”−Ｊ’・θ’） ・・・（５） τ1 = M · J # (x ″ −J ′ · θ ′) (5)

ヤコビ行列Ｊの逆行列であるＪ＃は、特異点において、ｄｅｔＪ＝０となってしまうためゼロ割が発生し、値が発散する。この特異点における関節角度θを特異点角度とすると、特異点角度は、例えば関節が伸びきった関節角度である。また、特異点角度でなくても、特異点角度の近傍の場合、ｄｅｔＪが小さい値となるため、ｄｅｔＪを分母に持つＪ＃の値も大きくなる。従って、Ｊ＃に基づき算出される駆動トルク値τ１は、特異点角度や特異点角度の近傍で、値が算出できなかったり、パワーアシストスーツ１０の出力可能なトルク値を超えてしまったりするなどのおそれがある。すなわち、この場合、トルク値の出力過大に起因する出力異常が生じるおそれがある。本実施形態に係る第２制御部１Ｂは、特異点角度やその近傍において、算出する駆動トルク値τ１を、後述する仮想関節角度を用いて算出することで、特異点角度における駆動トルク値τ１の出力異常を回避する。なお、保持トルク値τ２は、負荷及び自重と、関節角度θとに基づき算出することができるため、ヤコビ行列Ｊの逆行列を用いて算出する必要がない。従って、保持トルク値τ２は、特異点であっても算出することができる。 J # , which is the inverse matrix of the Jacobi matrix J, has detJ = 0 at the singularity, so that zero division occurs and the value diverges. Assuming that the joint angle θ at this singular point is the singular point angle, the singular point angle is, for example, the joint angle at which the joint is fully extended. Even if the angle is not the singular point angle, in the case of the vicinity of the singular point angle, detJ becomes a small value, so that the value of J # having detJ in the denominator also increases. Therefore, the drive torque value τ1 calculated based on J # may not be calculated in the vicinity of the singular point angle or in the vicinity of the singular point angle, or may exceed the output torque value of the power assist suit 10 or the like. There is a possibility of. That is, in this case, an output abnormality may occur due to an excessive output of the torque value. The second control unit 1B according to the present embodiment calculates the drive torque value τ1 to be calculated at or near the singular point angle using a virtual joint angle, which will be described later, to thereby calculate the drive torque value τ1 at the singular point angle. Avoid output abnormalities. Note that the holding torque value τ2 can be calculated based on the load, the own weight, and the joint angle θ, and therefore does not need to be calculated using the inverse matrix of the Jacobi matrix J. Therefore, the holding torque value τ2 can be calculated even at a singular point.

以下、第２制御部１Ｂについて詳細に説明する。図４に示すように、第２制御部１Ｂは、関節角度取得部１０２と、座標算出部１０４と、仮想関節角度取得部１０６と、目標加速度取得部１０８と、駆動トルク値算出部１１０とを有する。   Hereinafter, the second control unit 1B will be described in detail. As shown in FIG. 4, the second control unit 1B includes a joint angle acquisition unit 102, a coordinate calculation unit 104, a virtual joint angle acquisition unit 106, a target acceleration acquisition unit 108, and a drive torque value calculation unit 110. Have.

図５は、現在関節角度及び現在座標を説明するための模式図である。関節角度取得部１０２は、現在関節角度θ０を取得する。現在関節角度θ０とは、関節を介して隣接するリンク同士の現在の角度である。例えばパワースーツ１０が図５に示すような姿勢を取っている場合、腰関節の現在関節角度θ０は、大腿駆動機構２３（腰関節）を介して隣接する腰パーツ１１(腰リンク)と大腿装着部４１（大腿リンク）との間の現在関節角度θ０ａである。また、膝関節の現在関節角度θ０は、下腿駆動機構４５（膝関節）を介して隣接する大腿装着部４１（大腿リンク）と下腿装着部４２（下腿リンク）との間の現在関節角度θ０ｂである。また、足首関節の現在関節角度θ０は、回動軸４７（足首関節）を介して隣接する下腿装着部４２（下腿リンク）と足装着部４３（足リンク）との間の現在関節角度θ０ｃである。関節角度取得部１０２は、アクチュエータの制御値などから、各関節の現在関節角度θ０、ここでは現在関節角度θ０ａ、θ０ｂ、θ０ｃを取得する。   FIG. 5 is a schematic diagram for explaining the current joint angle and the current coordinates. The joint angle acquisition unit 102 acquires the current joint angle θ0. The current joint angle θ0 is the current angle between adjacent links via a joint. For example, when the power suit 10 is in the posture shown in FIG. 5, the current joint angle θ0 of the waist joint is determined by attaching the thigh to the waist part 11 (waist link) adjacent via the thigh drive mechanism 23 (lumbar joint). The current joint angle θ0a with the part 41 (thigh link). The current joint angle θ0 of the knee joint is the current joint angle θ0b between the adjacent thigh mounting portion 41 (thigh link) and the lower thigh mounting portion 42 (lower link) via the lower leg drive mechanism 45 (knee joint). is there. The current joint angle θ0 of the ankle joint is the current joint angle θ0c between the adjacent lower leg mounting portion 42 (lower leg link) and the foot mounting portion 43 (foot link) via the rotation axis 47 (ankle joint). is there. The joint angle acquisition unit 102 acquires the current joint angle θ0 of each joint, here, the current joint angles θ0a, θ0b, θ0c, from the control values of the actuators and the like.

図４に示す座標算出部１０４は、現在関節角度θ０ａ、θ０ｂ、θ０ｃに基づき、リンクの現在の座標である現在座標Ｃ０を算出する。本実施形態において、現在座標Ｃ０は、原点座標Ｃを原点とした場合の、足装着部４３（足リンク）の足先（爪先）の座標である。本実施形態においては、原点座標Ｃは、回動軸４７（足首関節）の座標である。原点座標Ｃは、ｘ方向、ｚ方向、及びφ方向の３方向で規定した座標である。図５に示すように、ｚ方向は、原点座標Ｃから大腿駆動機構２３（腰関節）に向かう方向である。ｘ方向は、ｚ方向と直交する方向であり、パワーアシストスーツ１０の前後方向である。φ方向は、原点座標Ｃと足装着部４３（足リンク）の足先との角度である。ここでは、ｘ方向座標、ｚ方向座標、φ方向座標の順に、原点座標Ｃを（０，０，０）とし、現在座標Ｃ０を（ｘ０、ｚ０、φ０）とする。なお、現在座標Ｃ０は、足装着部４３（足リンク）の座標であることが好ましいが、足リンク以外のリンクの任意の位置であってよい。現在関節角度θ０ａ、θ０ｂ、θ０ｃ及び各リンクの長さが分かっているので、座標算出部１０４は、足リンク以外のリンクのいずれの位置の座標も算出可能である。   The coordinate calculation unit 104 illustrated in FIG. 4 calculates a current coordinate C0, which is the current coordinate of the link, based on the current joint angles θ0a, θ0b, and θ0c. In the present embodiment, the current coordinate C0 is the coordinate of the toe (toe) of the foot mounting part 43 (foot link) when the origin coordinate C is the origin. In the present embodiment, the origin coordinates C are the coordinates of the rotation axis 47 (ankle joint). The origin coordinate C is a coordinate defined in three directions of the x direction, the z direction, and the φ direction. As shown in FIG. 5, the z direction is a direction from the origin coordinates C to the thigh drive mechanism 23 (lumbar joint). The x direction is a direction orthogonal to the z direction, and is a front-back direction of the power assist suit 10. The φ direction is the angle between the origin coordinate C and the tip of the foot mounting portion 43 (foot link). Here, it is assumed that the origin coordinate C is (0, 0, 0) and the current coordinate C0 is (x0, z0, φ0) in the order of x-direction coordinate, z-direction coordinate, and φ-direction coordinate. The current coordinates C0 are preferably the coordinates of the foot mounting part 43 (foot link), but may be any position of a link other than the foot link. Since the joint angles θ0a, θ0b, θ0c and the length of each link are known at present, the coordinate calculation unit 104 can calculate the coordinates of any position of the link other than the foot link.

図４に示す仮想関節角度取得部１０６は、現在関節角度θ０が特異点角度である場合に、現在関節角度θ０から所定の変位角度Δθだけ異なったリンク同士の仮想の角度である仮想関節角度θ１を算出する。特異点角度とは、上述のように特異点である場合の関節角度である。従って、現在関節角度θ０に基づいて算出される逆ヤコビ行列が発散する場合に、その現在関節角度θ０は、特異点角度であるということができる。パワーアシストスーツ１０の場合、膝関節が特異点角度である場合に、装着者の移動を補助するための駆動トルクτ１が無限大となり出力異常となる。仮想関節角度取得部１０６は、膝関節の角度、すなわち、現在関節角度θ０ｂが特異点角度である場合に、現在関節角度θ０ｂから所定の変位角度Δθｂだけ異なったリンク同士の仮想の角度である仮想関節角度θ1ｂを算出する。現在関節角度θ０ｂは、０°、すなわち膝が伸びきった場合が特異点角度である。   When the current joint angle θ0 is a singular point angle, the virtual joint angle acquisition unit 106 illustrated in FIG. 4 is a virtual joint angle θ1 that is a virtual angle between links different from the current joint angle θ0 by a predetermined displacement angle Δθ. Is calculated. The singular point angle is a joint angle when the singular point is a singular point as described above. Therefore, when the inverse Jacobian matrix calculated based on the current joint angle θ0 diverges, it can be said that the current joint angle θ0 is a singular point angle. In the case of the power assist suit 10, when the knee joint is at a singular point angle, the driving torque τ1 for assisting the movement of the wearer becomes infinite, and the output becomes abnormal. The virtual joint angle acquisition unit 106 is a virtual joint angle, that is, a virtual angle between links different from the current joint angle θ0b by a predetermined displacement angle Δθb when the current joint angle θ0b is a singular point angle. The joint angle θ1b is calculated. The current joint angle θ0b is 0 °, that is, the singular point angle when the knee is fully extended.

仮想関節角度取得部１０６は、現在関節角度θ０ｂが特異点角度である場合に、仮想関節角度θ１ｂを算出する。従って、膝関節の角度である現在関節角度θ０ｂは、全ての関節のうち、特異点問題による出力異常を回避する対象の関節である、特異点回避現在関節角度θ０ｂであると言い換えることができる。ただし、仮想関節角度取得部１０６は、現在関節角度θ０ｂだけでなく、他の現在関節角度θ０、例えば現在関節角度θ０ａ、θ０ｃが特異点角度である場合にも、仮想関節角度θ１を算出してもよい。以下、仮想関節角度θ1ｂの算出について詳細に説明する。   The virtual joint angle acquisition unit 106 calculates the virtual joint angle θ1b when the current joint angle θ0b is a singular point angle. Therefore, the current joint angle θ0b, which is the angle of the knee joint, can be rephrased as the singular point avoidance current joint angle θ0b, which is the joint whose output abnormality is to be avoided due to the singularity problem among all the joints. However, the virtual joint angle acquisition unit 106 calculates the virtual joint angle θ1 not only when the current joint angle θ0b but also other current joint angles θ0, for example, when the current joint angles θ0a and θ0c are singularity angles. Is also good. Hereinafter, the calculation of the virtual joint angle θ1b will be described in detail.

仮想関節角度取得部１０６は、仮想座標算出部１２０及び仮想関節角度算出部１２２を有する。仮想座標算出部１２０は、現在座標Ｃ０の１つの方向（軸）に対応する座標のみを所定の変位距離εだけ変化させた仮想座標Ｃ１を算出する。図６は、仮想座標及び仮想関節角度を説明するための模式図である。図６に示すように、仮想座標算出部１２０は、現在座標Ｃ０のうち、ｚ方向に対応する座標のみを変位距離εだけ変化させた仮想座標Ｃ１を算出する。εは、予め定められた所定の距離である。εの値は、例えば下腿装着部４２の長さの５％以上１０％以下である。ただし、εの値は、特異点角度にある場合にこのεの値だけ座標を変化させた場合に、その座標により算出されたトルク値が出力可能なトルク値を上回らないように設定されるものであれば、任意の値とすることができる。出力可能なトルク値は、リンク長やモータ定格出力など、パワーアシストスーツ１０の性能やその制御性に応じて異なるものである。従って、εの値は、パワーアシストスーツ１０の性能や動かしたい制御性に基づき任意に設定することができる。   The virtual joint angle acquisition unit 106 has a virtual coordinate calculation unit 120 and a virtual joint angle calculation unit 122. The virtual coordinate calculator 120 calculates a virtual coordinate C1 in which only coordinates corresponding to one direction (axis) of the current coordinates C0 are changed by a predetermined displacement distance ε. FIG. 6 is a schematic diagram for explaining virtual coordinates and virtual joint angles. As shown in FIG. 6, the virtual coordinate calculation unit 120 calculates the virtual coordinates C1 by changing only the coordinates corresponding to the z direction among the current coordinates C0 by the displacement distance ε. ε is a predetermined distance. The value of ε is, for example, not less than 5% and not more than 10% of the length of the leg attachment part 42. However, the value of ε is set so that when the coordinates are changed by the value of ε at the singular point angle, the torque value calculated by the coordinates does not exceed the outputtable torque value. If so, any value can be set. The torque value that can be output differs depending on the performance of the power assist suit 10 and its controllability, such as the link length and the rated motor output. Therefore, the value of ε can be arbitrarily set based on the performance of the power assist suit 10 and the controllability to be moved.

仮想座標算出部１２０は、座標ｚ０から変位距離εを差し引いて、座標が（ｘ０、ｚ０−ε、φ０）である仮想座標Ｃ１を算出する。すなわち、仮想座標算出部１２０は、現在座標Ｃ０のｚ方向の座標ｚ０を、下腿駆動機構４５（膝関節）の方向、すなわち仮想関節角度θ1ｂを算出する関節に向かって、変位距離εだけ移動させて、仮想座標Ｃ１を算出する。言い換えれば、仮想座標算出部１２０は、現在座標Ｃ０を、ｚ方向に沿って変位距離εだけ脚を縮める方向に移動させた仮想座標Ｃ１を算出する。なお、仮想座標算出部１２０は、少なくとも１つの方向（軸）に対応する座標を変位距離εだけ変化させればよく、２方向以上の座標を変化させて、仮想座標Ｃ１を算出してもよい。この場合、変位距離εは、方向毎に定められる。   The virtual coordinate calculation unit 120 calculates a virtual coordinate C1 having coordinates (x0, z0−ε, φ0) by subtracting the displacement distance ε from the coordinate z0. That is, the virtual coordinate calculation unit 120 moves the coordinate z0 of the current coordinate C0 in the z direction by the displacement distance ε toward the direction of the lower leg drive mechanism 45 (knee joint), that is, the joint for calculating the virtual joint angle θ1b. Then, the virtual coordinates C1 are calculated. In other words, the virtual coordinate calculation unit 120 calculates the virtual coordinates C1 by moving the current coordinates C0 along the z direction in the direction to shrink the legs by the displacement distance ε. The virtual coordinate calculation unit 120 may change the coordinates corresponding to at least one direction (axis) by the displacement distance ε, and may calculate the virtual coordinates C1 by changing the coordinates in two or more directions. . In this case, the displacement distance ε is determined for each direction.

仮想関節角度算出部１２２は、リンクが仮想座標Ｃ１に位置すると仮定した場合のリンク同士の角度を、仮想関節角度θ１として算出する。本実施形態では、仮想関節角度算出部１２２は、実際には現在座標Ｃ０に位置している足装着部４３の足先が、仮想座標Ｃ１に位置していると仮定した場合の、各関節の関節角度を、仮想関節角度θ１として算出する。さらに言えば、仮想関節角度算出部１２２は、仮想座標Ｃ１の座標（ｘ０、ｚ０−ε、φ０）に基づき、腰関節の仮想関節角度θ1aと膝関節の仮想関節角度θ1ｂと足首関節の仮想関節角度θ1ｃとを算出する。腰関節の関節角度は実際には現在関節角度θ０ａであるため、仮想関節角度θ1aは、腰関節の仮想の関節角度であるということができる。仮想関節角度θ1ｂ及び仮想関節角度θ1ｃも、同様に、それぞれ膝関節及び足首関節の仮想の関節角度である。なお、仮想座標Ｃ１及び各リンクの長さが分かっているので、仮想関節角度算出部１２２は、仮想座標Ｃ１に基づき、仮想関節角度θ１ａ、θ１ｂ、θ１ｃを算出することができる。   The virtual joint angle calculation unit 122 calculates the angle between the links assuming that the link is located at the virtual coordinate C1 as the virtual joint angle θ1. In the present embodiment, the virtual joint angle calculation unit 122 determines the position of each joint when assuming that the tip of the foot mounting unit 43 that is actually located at the current coordinate C0 is located at the virtual coordinate C1. The joint angle is calculated as a virtual joint angle θ1. Furthermore, the virtual joint angle calculation unit 122 calculates the virtual joint angle θ1a of the hip joint, the virtual joint angle θ1b of the knee joint, and the virtual joint of the ankle joint based on the coordinates (x0, z0−ε, φ0) of the virtual coordinates C1. The angle θ1c is calculated. Since the joint angle of the hip joint is actually the current joint angle θ0a, it can be said that the virtual joint angle θ1a is a virtual joint angle of the hip joint. Similarly, the virtual joint angle θ1b and the virtual joint angle θ1c are virtual joint angles of the knee joint and the ankle joint, respectively. Since the virtual coordinates C1 and the length of each link are known, the virtual joint angle calculation unit 122 can calculate the virtual joint angles θ1a, θ1b, θ1c based on the virtual coordinates C1.

仮想関節角度θ1ｂは、現在座標Ｃ０を変位距離εだけ移動させた仮想座標Ｃ１に基づき算出されたものである。そのため、仮想関節角度θ1ｂは、実際の関節角度である現在関節角度θ０ｂから、変位角度Δθｂだけ異なった角度であるということができる。さらに、仮想座標Ｃ１は、現在座標Ｃ０を下腿駆動機構４５（膝関節）に向かって移動させたものである。従って、仮想関節角度θ1ｂである場合の、下腿駆動機構４５に連結するリンク同士の距離、すなわち大腿装着部４１（大腿リンク）と下腿装着部４２（下腿リンク）との間の距離は、現在座標Ｃ０である場合の大腿装着部４１（大腿リンク）と下腿装着部４２（下腿リンク）との間の距離よりも、短い。言い換えれば、仮想座標算出部１２０及び仮想関節角度算出部１２２は、仮想関節角度θ1ｂである場合の下腿駆動機構４５に連結するリンク同士の距離が、仮想関節角度θ1ｂである場合の下腿駆動機構４５に連結するリンク同士の距離よりも短くなるように、仮想関節角度θ1ｂを算出している。また、仮想関節角度θ1ｂは、現在関節角度θ０ｂから、変位角度Δθｂだけ角度が大きく、すなわち変位角度Δθｂだけ膝をより曲げた角度になっているということもできる。   The virtual joint angle θ1b is calculated based on the virtual coordinates C1 obtained by moving the current coordinates C0 by the displacement distance ε. Therefore, it can be said that the virtual joint angle θ1b is different from the current joint angle θ0b, which is the actual joint angle, by the displacement angle Δθb. Further, the virtual coordinates C1 are obtained by moving the current coordinates C0 toward the lower leg drive mechanism 45 (knee joint). Accordingly, in the case of the virtual joint angle θ1b, the distance between the links connected to the lower leg drive mechanism 45, that is, the distance between the lower leg mounting portion 41 (the lower leg link) and the lower leg mounting portion 42 (the lower leg link) is the current coordinate. It is shorter than the distance between the thigh mounting portion 41 (thigh link) and the lower leg mounting portion 42 (lower leg link) in the case of C0. In other words, the virtual coordinate calculator 120 and the virtual joint angle calculator 122 determine that the distance between the links connected to the lower leg drive mechanism 45 when the virtual joint angle is θ1b is equal to the virtual joint angle θ1b. The virtual joint angle θ1b is calculated so as to be shorter than the distance between the links connected to. In addition, it can be said that the virtual joint angle θ1b is larger than the current joint angle θ0b by the displacement angle Δθb, that is, the knee is bent more by the displacement angle Δθb.

以上のように、仮想座標算出部１２０及び仮想関節角度算出部１２２は、現在関節角度θ０ｂが、特異点角度である場合に、仮想座標Ｃ１及び仮想関節角度θ１ａ、θ１ｂ、θ１ｃを算出するものであるが、特異点角度でない場合でも、同様に仮想座標Ｃ１及び仮想関節角度θ１ａ、θ１ｂ、θ１ｃを算出する。   As described above, the virtual coordinate calculation unit 120 and the virtual joint angle calculation unit 122 calculate the virtual coordinates C1 and the virtual joint angles θ1a, θ1b, θ1c when the current joint angle θ0b is a singular point angle. However, even when the angle is not a singular point angle, the virtual coordinates C1 and the virtual joint angles θ1a, θ1b, θ1c are calculated in the same manner.

図４に示す目標加速度取得部１０８は、リンクの目標とする移動加速度である目標加速度ａを取得する。本実施形態においては、目標加速度取得部１０８は、足装着部４３（足リンク）の目標とする移動加速度を、目標加速度ａとして取得する。本実施形態においては、目標加速度取得部１０８は、装着者の足底荷重値の変化量に基づき、目標加速度ａを算出する。具体的には、目標加速度取得部１０８は、荷重検出装置４４が逐次検出した装着者の足底荷重値を取得し、取得した足底荷重値に基づき、足底荷重値の変化量を算出する。荷重検出装置４４が検出した装着者の足底荷重値を足底荷重値Ｆとすると、目標加速度取得部１０８は、足底荷重値Ｆの時間ｔでの一階微分値に対応する荷重値変化量ｆ（Ｆ）を算出する。荷重値変化量ｆ（Ｆ）は、足底荷重値Ｆの変化量である。ただし、目標加速度取得部１０８による足底荷重値の変化量の算出方法は、足底荷重値Ｆの変化量を一階微分で算出することに限られず、例えば、今回取得された足底荷重値Ｆと前回取得された足底荷重値Ｆとの差分を、足底荷重値の変化量としてもよい。   The target acceleration obtaining unit 108 illustrated in FIG. 4 obtains a target acceleration a that is a target moving acceleration of the link. In the present embodiment, the target acceleration acquisition unit 108 acquires the target movement acceleration of the foot mounting unit 43 (foot link) as the target acceleration a. In the present embodiment, the target acceleration acquisition unit 108 calculates the target acceleration a based on the amount of change in the sole load value of the wearer. Specifically, the target acceleration acquisition unit 108 acquires the sole load value of the wearer sequentially detected by the load detection device 44, and calculates the amount of change in the sole load value based on the acquired sole load value. . Assuming that the sole load value of the wearer detected by the load detecting device 44 is the sole load value F, the target acceleration acquisition unit 108 calculates the change in the load value corresponding to the first derivative of the sole load value F at time t. Calculate the quantity f (F). The load value change amount f (F) is a change amount of the sole load value F. However, the method of calculating the amount of change in the sole load value by the target acceleration acquisition unit 108 is not limited to calculating the amount of change in the sole load value F by first-order differentiation. The difference between F and the previously obtained sole load value F may be used as the change amount of the sole load value.

目標加速度取得部１０８は、次の式（６）に示すように、補正係数Ｋと荷重値変化量ｆ（Ｆ）とを乗じて、その乗じた値にマイナスを乗じることにより、目標加速度ａを算出する。   The target acceleration acquisition unit 108 multiplies the correction coefficient K by the load value change amount f (F) and multiplies the multiplied value by minus to obtain the target acceleration a as shown in the following equation (6). calculate.

ａ＝−Ｋ・ｆ（Ｆ） ・・・（６）   a = −K · f (F) (6)

ここで、Ｋは予め定められた所定の係数である。目標加速度ａは、荷重値変化量ｆ（Ｆ）に対してマイナスが乗じられている。従って、目標加速度取得部１０８は、足底荷重値Ｆが増加した場合に、足を下げる又は踏ん張る方向へ加速度を発生させるように、目標加速度ａを算出する。また、目標加速度取得部１０８は、足底荷重値Ｆが減少した場合に、足を上げる方向へ加速度を発生させるように、目標加速度ａを算出する。   Here, K is a predetermined coefficient. The target acceleration a is obtained by multiplying the load value change amount f (F) by minus. Therefore, when the sole load value F increases, the target acceleration acquisition unit 108 calculates the target acceleration a so as to generate an acceleration in the direction of lowering or stepping on the foot. Further, the target acceleration acquisition unit 108 calculates the target acceleration “a” such that when the sole load value F decreases, the acceleration is generated in the direction in which the foot is raised.

目標加速度取得部１０８は、足底荷重値Ｆの変化量に基づき目標加速度ａを算出するものであるが、目標加速度ａを取得するものであれば、目標加速度ａの取得方法は、これに限られず任意である。なお、パワーアシストスーツ１０の制御において、厳密な定位性は不要である。装着者の体格等に応じて到達させたい位置目標は異なるためである。一方、バネとして制御を定義すると位置拘束となり、パワーアシストスーツ１０を装着した装着者の意図と異なる動作に対して剛性を持った動きになる可能性がある。そこで、第２制御部１Ｂは、目標加速度ａに基づき駆動トルクτ１を算出するため、力の入力に対してパワーアシストスーツ１０の足先の絶対的な位置が決まらないように、非定位性の制御を実現している。すなわち、第２制御部１Ｂは、位置制御ループを有していない。   The target acceleration obtaining unit 108 calculates the target acceleration a based on the amount of change in the sole load value F. However, the method of obtaining the target acceleration a is not limited to this as long as the target acceleration a is obtained. Not optional. In the control of the power assist suit 10, strict localization is not required. This is because the position target to be reached differs depending on the physique of the wearer. On the other hand, when the control is defined as a spring, the position is restricted, and there is a possibility that the movement with rigidity is performed for an operation different from the intention of the wearer wearing the power assist suit 10. Therefore, the second control unit 1B calculates the drive torque τ1 based on the target acceleration “a”, so that the absolute position of the toe of the power assist suit 10 is not determined with respect to the input of the force. Control is realized. That is, the second control unit 1B does not have a position control loop.

図４に示す駆動トルク値算出部１１０は、目標加速度ａと仮想関節角度θ１ａ、θ１ｂ、θ１ｃとに基づき、駆動トルク値τ１を算出する。駆動トルク値算出部１１０は、式（５）の加速度ｘ”を目標加速度ａに置き換え、次の式（７）により駆動トルク値τ１を算出する。   4 calculates a drive torque value τ1 based on the target acceleration a and the virtual joint angles θ1a, θ1b, θ1c. The drive torque value calculation unit 110 replaces the acceleration x ″ in Expression (5) with the target acceleration a, and calculates the drive torque value τ1 according to the following Expression (7).

τ１＝Ｍ・Ｊ＃（ａ−Ｊ’・θ’） ・・・（７） τ1 = M · J # (a−J ′ · θ ′) (7)

具体的には、駆動トルク値算出部１１０は、仮想関節角度θ１ａ、θ１ｂ、θ１ｃを用いて、Ｊ＃を算出する。仮想関節角度θ１ｂは、現在関節角度θ０ｂから変位角度Δθｂだけ変位させた角度であるため、逆ヤコビ行列であるＪ＃が発散しない。従って、駆動トルク値算出部１１０は、現在関節角度θ０ｂが特異点角度であっても、逆ヤコビ行列が発散しない仮想関節角度θ１ｂに基づき、駆動トルク値τ１を適切に算出することができる。なお、駆動トルク値算出部１１０は、Ｊ’・θ’については、実際の関節角度、すなわち現在関節角度θ０ａ、θ０ｂ、θ０ｃを用いて算出する。 Specifically, the drive torque value calculation unit 110 calculates J # using the virtual joint angles θ1a, θ1b, and θ1c. Since the virtual joint angle θ1b is an angle displaced from the current joint angle θ0b by the displacement angle Δθb, the inverse Jacobian matrix J # does not diverge. Therefore, the drive torque value calculation unit 110 can appropriately calculate the drive torque value τ1 based on the virtual joint angle θ1b at which the inverse Jacobian matrix does not diverge, even if the current joint angle θ0b is a singular point angle. The drive torque value calculation unit 110 calculates J ′ · θ ′ using the actual joint angles, that is, the current joint angles θ0a, θ0b, and θ0c.

式（４）に示すように、Ｊ＃（ａ−Ｊ’・θ’）は、加速度ｘ”を目標加速度ａとした場合の角加速度であるため、Ｊ＃（ａ−Ｊ’・θ’）は、リンクを移動させる目標とする角加速度である、目標角加速度ということができる。 As shown in equation (4), J # (a -J '· θ') are the angular acceleration when the acceleration x "and the target acceleration a, J # (a-J '· θ') Can be referred to as a target angular acceleration, which is a target angular acceleration for moving the link.

駆動トルク値決定部１４２は、決定した駆動トルク値τ１の情報をアクチュエータ制御部１Ｃに伝達する。アクチュエータ制御部１Ｃは、駆動トルク値決定部１４２から取得した駆動トルク値τ１と、第１制御部１Ａｃから取得した保持トルク値τ２の合計値を、総トルク値τ０として算出し、その総トルク値τ０で、アクチュエータを駆動する。   The drive torque value determination unit 142 transmits information on the determined drive torque value τ1 to the actuator control unit 1C. The actuator control unit 1C calculates a total value of the driving torque value τ1 obtained from the driving torque value determination unit 142 and the holding torque value τ2 obtained from the first control unit 1Ac as a total torque value τ0, and calculates the total torque value. At τ0, the actuator is driven.

次に、フローチャートを用いて、駆動トルク値τ１の算出フローを説明する。図７は、第１実施形態に係る駆動トルク値の算出フローを説明するフローチャートである。図７に示すように、第２制御部１Ｂは、関節角度取得部１０２により、現在関節角度θ０ａ、θ０ｂ、θ０ｃを取得し（ステップＳ１０）、座標算出部１０４により、現在関節角度θ０ａ、θ０ｂ、θ０ｃに基づき現在座標Ｃ０を算出する（ステップＳ１２）。   Next, a flow of calculating the drive torque value τ1 will be described using a flowchart. FIG. 7 is a flowchart illustrating a calculation flow of the drive torque value according to the first embodiment. As shown in FIG. 7, the second control unit 1B acquires the current joint angles θ0a, θ0b, and θ0c by the joint angle acquiring unit 102 (step S10), and the current joint angles θ0a, θ0b, The current coordinates C0 are calculated based on θ0c (step S12).

現在座標Ｃ０を算出した後、第２制御部１Ｂは、仮想座標算出部１２０により、現在座標Ｃ０から変位距離εだけ座標を変化させた仮想座標Ｃ１を算出する（ステップＳ１４）。仮想座標算出部１２０は、現在座標Ｃ０のｚ方向の座標ｚ０を、大腿駆動機構２３の方向に向かって、変位距離εだけ移動させて、仮想座標Ｃ１を算出する。仮想座標Ｃ１を算出した後、第２制御部１Ｂは、仮想関節角度算出部１２２により、仮想座標Ｃ１から仮想関節角度θ１ａ、θ１ｂ、θ１ｃを算出する（ステップＳ１６）。第２制御部１Ｂは、足装着部４３の足先が仮想座標Ｃ１に位置していると仮定した場合の各関節の関節角度を、仮想関節角度θ１ａ、θ１ｂ、θ１ｃとして算出する。   After calculating the current coordinates C0, the second control unit 1B causes the virtual coordinates calculation unit 120 to calculate the virtual coordinates C1 obtained by changing the coordinates from the current coordinates C0 by the displacement distance ε (step S14). The virtual coordinate calculation unit 120 calculates the virtual coordinate C1 by moving the coordinate z0 of the current coordinate C0 in the z direction by the displacement distance ε toward the direction of the thigh drive mechanism 23. After calculating the virtual coordinates C1, the second control unit 1B calculates the virtual joint angles θ1a, θ1b, θ1c from the virtual coordinates C1 by the virtual joint angle calculation unit 122 (step S16). The second control unit 1B calculates the joint angles of the joints as virtual joint angles θ1a, θ1b, and θ1c, assuming that the tip of the foot mounting unit 43 is located at the virtual coordinate C1.

また、第２制御部１Ｂは、目標加速度取得部１０８により、目標加速度ａを取得する（ステップＳ１８）。目標加速度取得部１０８は、例えば上述の式（６）に基づき、目標加速度ａを算出する。なお、ステップＳ１８は、後述するステップＳ２０の前に行われれば、その処理順は任意である。   Further, the second control unit 1B acquires the target acceleration a by the target acceleration acquisition unit 108 (Step S18). The target acceleration acquisition unit 108 calculates the target acceleration “a” based on, for example, the above equation (6). Note that the processing order is arbitrary as long as step S18 is performed before step S20 described later.

仮想関節角度θ１ａ、θ１ｂ、θ１ｃと目標加速度ａを算出した後、第２制御部１Ｂは、駆動トルク値算出部１１０により、目標加速度ａと仮想関節角度θ１ａ、θ１ｂ、θ１ｃとから、駆動トルク値τ１を算出する（ステップＳ２０）。駆動トルク値算出部１１０は、上述の式（７）に基づき、駆動トルク値τ１を算出する。駆動トルク値算出部１１０は、その駆動トルク値τ１をアクチュエータ制御部１Ｃに伝達する。アクチュエータ制御部１Ｃは、駆動トルク値算出部１１０が算出した第１制御部１Ａから取得した保持トルク値τ２の合計値を、総トルク値τ０として算出し、その総トルク値τ０で、アクチュエータを駆動する。   After calculating the virtual joint angles θ1a, θ1b, θ1c and the target acceleration a, the second control unit 1B uses the drive torque value calculation unit 110 to calculate the drive torque value from the target acceleration a and the virtual joint angles θ1a, θ1b, θ1c. τ1 is calculated (step S20). The drive torque value calculation unit 110 calculates the drive torque value τ1 based on the above equation (7). Drive torque value calculation section 110 transmits the drive torque value τ1 to actuator control section 1C. The actuator control unit 1C calculates the total value of the holding torque values τ2 obtained from the first control unit 1A calculated by the drive torque value calculation unit 110 as a total torque value τ0, and drives the actuator with the total torque value τ0. I do.

ステップＳ２０の後は、ステップＳ２２に移動し、処理が終了していない場合（ステップＳ２２；Ｎｏ）、ステップＳ１０に戻り、次のタイミングでの現在関節角度を取得し、その後同様の処理を繰り返す。処理が終了した場合（ステップＳ２２；Ｙｅｓ）、本処理を終了する。   After step S20, the process moves to step S22. If the process is not completed (step S22; No), the process returns to step S10, acquires the current joint angle at the next timing, and thereafter repeats the same process. When the processing is completed (Step S22; Yes), the processing is completed.

以上説明したように、本実施形態に係る制御装置１は、関節を介して連結された複数のリンクをアクチュエータにより駆動する作動機構（本実施形態ではパワーアシストスーツ１０）の制御装置である。制御装置１は、関節角度取得部１０２と、仮想関節角度取得部１０６と、目標加速度取得部１０８と、駆動トルク値算出部１１０と、アクチュエータ制御部１Ｃとを有する。関節角度取得部１０２は、関節を介して隣接するリンク同士の現在の角度である現在関節角度θ０ａ、θ０ｂ、θ０ｃを取得する。仮想関節角度取得部１０６は、現在関節角度θ０ｂが特異点角度である場合に、現在関節角度θ０ａ、θ０ｂ、θ０ｃから所定の変位角度Δθｂだけ異なった仮想関節角度θ1a、θ1ｂ、θ1ｃを算出する。目標加速度取得部１０８は、リンクの目標とする移動加速度である目標加速度ａを取得する。駆動トルク値算出部１１０は、目標加速度ａと仮想関節角度θ1a、θ1ｂ、θ1ｃとに基づき、駆動トルク値τ１を算出する。そして、アクチュエータ制御部１Ｃは、アクチュエータを駆動するトルク値として駆動トルク値τ１を用いて、アクチュエータを制御する。   As described above, the control device 1 according to the present embodiment is a control device for an operation mechanism (the power assist suit 10 in the present embodiment) that drives a plurality of links connected via a joint by an actuator. The control device 1 includes a joint angle acquisition unit 102, a virtual joint angle acquisition unit 106, a target acceleration acquisition unit 108, a drive torque value calculation unit 110, and an actuator control unit 1C. The joint angle acquisition unit 102 acquires the current joint angles θ0a, θ0b, θ0c, which are the current angles of the links adjacent via the joint. When the current joint angle θ0b is a singular point angle, the virtual joint angle acquisition unit 106 calculates virtual joint angles θ1a, θ1b, θ1c different from the current joint angles θ0a, θ0b, θ0c by a predetermined displacement angle Δθb. The target acceleration acquisition unit 108 acquires a target acceleration a that is a target movement acceleration of the link. The drive torque value calculation unit 110 calculates a drive torque value τ1 based on the target acceleration a and the virtual joint angles θ1a, θ1b, θ1c. Then, the actuator control unit 1C controls the actuator using the driving torque value τ1 as the torque value for driving the actuator.

この制御装置１は、仮想関節角度θ１ａ、θ１ｂ、θ１ｃを用いて、駆動トルク値τ１を算出する。仮想関節角度θ１ｂは、現在関節角度θ０ｂから変位角度Δθｂだけ変位させた角度であるため、駆動トルク値τ１の算出に用いる逆ヤコビ行列が発散しない。従って、制御装置１は、現在関節角度θ０ｂが特異点角度であっても、逆ヤコビ行列が発散しない仮想関節角度θ１ｂに基づき、駆動トルク値τ１を適切に算出することができる。従って、制御装置１は、仮想関節角度θ１ｂを算出するだけで、駆動トルク値τ１の出力異常を容易に抑制することができる。また、この制御装置１は、目標加速度ａに基づき駆動トルク値τ１を算出しているため、非定位性の制御となり、自由な動作に対応することが可能となる。   The control device 1 calculates the drive torque value τ1 using the virtual joint angles θ1a, θ1b, θ1c. Since the virtual joint angle θ1b is an angle displaced from the current joint angle θ0b by the displacement angle Δθb, the inverse Jacobian matrix used for calculating the drive torque value τ1 does not diverge. Therefore, even if the current joint angle θ0b is a singular point angle, the control device 1 can appropriately calculate the drive torque value τ1 based on the virtual joint angle θ1b at which the inverse Jacobian matrix does not diverge. Therefore, the control device 1 can easily suppress the output abnormality of the drive torque value τ1 only by calculating the virtual joint angle θ1b. In addition, since the control device 1 calculates the drive torque value τ1 based on the target acceleration a, the control device 1 performs non-localization control and can cope with a free operation.

仮想関節角度取得部１０６は、仮想関節角度θ１ａ、θ１ｂ、θ１ｃである場合のリンク同士の距離が、現在関節角度θ０ａ、θ０ｂ、θ０ｃである場合のリンク同士の距離よりも短くなるように、仮想関節角度θ１ａ、θ１ｂ、θ１ｃを算出する。すなわち、制御装置１は、関節が伸びきった場合の特異点に陥った場合、関節を曲げる方向に変位した仮想関節角度を用いて、駆動トルク値τ１を算出する。従って、制御装置１は、関節が伸びきった場合の特異点においても、駆動トルク値τ１の出力異常を適切に抑制することができる。また、関節を曲げる方向、すなわち特異点から遠ざかる方向に変位した仮想関節角度を用いるため、制御装置１は出力異常をより適切に抑制することができる。さらに、例えば歩行動作や屈伸動作で、膝関節が特異点となった場合に、現在関節角度θ０ｂよりも膝を曲げる方向に変位させた仮想関節角度θ１ｂを用いて、駆動トルク値τ１を算出する。従って、制御装置１は、足の軌跡に従った駆動トルク値τ１を算出することができる。   The virtual joint angle acquisition unit 106 sets the virtual joint angles so that the distance between the links at the virtual joint angles θ1a, θ1b, and θ1c is shorter than the distance between the links at the current joint angles θ0a, θ0b, and θ0c. The joint angles θ1a, θ1b, and θ1c are calculated. That is, when the control device 1 falls into a singular point when the joint is fully extended, the control device 1 calculates the drive torque value τ1 using the virtual joint angle displaced in the direction of bending the joint. Therefore, the control device 1 can appropriately suppress the output abnormality of the drive torque value τ1 even at the singular point when the joint is fully extended. Further, since the virtual joint angle displaced in the direction in which the joint is bent, that is, in the direction away from the singular point, is used, the control device 1 can more appropriately suppress the output abnormality. Further, when the knee joint becomes a singular point in, for example, a walking motion or a bending / extending motion, the drive torque value τ1 is calculated using the virtual joint angle θ1b displaced in the direction of bending the knee from the current joint angle θ0b. . Therefore, the control device 1 can calculate the drive torque value τ1 according to the trajectory of the foot.

また、制御装置１は、現在関節角度θ０ａ、θ０ｂ、θ０ｃに基づき、リンクの現在の座標である現在座標Ｃ０を算出する座標算出部１０４を更に有する。そして、仮想関節角度取得部１０６は、仮想座標算出部１２０と仮想関節角度算出部１２２とを有する。仮想座標算出部１２０は、現在座標Ｃ０のうち、少なくとも１つの軸に対応する座標を所定の変位距離εだけ変化させた座標である仮想座標Ｃ１を算出する。仮想関節角度算出部１２２は、リンクが仮想座標Ｃ１に位置すると仮定した場合のリンク同士の角度を、仮想関節角度θ１ａ、θ１ｂ、θ１ｃとして算出する。この制御装置１は、変位距離εだけ関節角度が変位した仮想関節角度θ１ａ、θ１ｂ、θ１ｃに基づき駆動トルク値τ１を算出するため、特異点であっても、駆動トルク値τ１の出力異常を適切に抑制することができる。   In addition, the control device 1 further includes a coordinate calculation unit 104 that calculates a current coordinate C0 that is a current coordinate of the link based on the current joint angles θ0a, θ0b, and θ0c. The virtual joint angle acquisition unit 106 has a virtual coordinate calculation unit 120 and a virtual joint angle calculation unit 122. The virtual coordinate calculation unit 120 calculates a virtual coordinate C1 which is a coordinate obtained by changing a coordinate corresponding to at least one axis among the current coordinates C0 by a predetermined displacement distance ε. The virtual joint angle calculation unit 122 calculates the angles of the links assuming that the links are located at the virtual coordinates C1 as virtual joint angles θ1a, θ1b, and θ1c. The control device 1 calculates the drive torque value τ1 based on the virtual joint angles θ1a, θ1b, θ1c in which the joint angles are displaced by the displacement distance ε. Can be suppressed.

（第２実施形態）
次に、第２実施形態について説明する。第２実施形態に係る制御装置１は、変位距離εを、現在関節角度θ０に応じて変化させる点で、第１実施形態とは異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。 (2nd Embodiment)
Next, a second embodiment will be described. The control device 1 according to the second embodiment is different from the first embodiment in that the displacement distance ε is changed according to the current joint angle θ0. In the second embodiment, description of portions having the same configuration as the first embodiment will be omitted.

図８は、第２実施形態における現在関節角度と変位距離との関係を示すグラフである。第１実施形態においては、変位距離εは予め定められた一定の値であったが、第２実施形態に係る仮想座標算出部１２０は、現在関節角度θ０ｂの値に基づき、変位距離εを変化させる。図８の横軸は現在関節角度θ０ｂであり、縦軸は変位距離εである。図８に示すように、仮想座標算出部１２０は、現在関節角度θ０ｂが０°である場合、すなわち特異点角度である場合に、変位距離εの値を、所定の値である変位距離ε１とする。このε１は、第１実施形態で示した変位距離εと同様の値のものである。仮想座標算出部１２０は、現在関節角度θ０ｂが０°から閾値角度Ｔ１まで大きくなるに従って、変位距離εの値を、連続的に低下させる。仮想座標算出部１２０は、現在関節角度θ０ｂが閾値角度Ｔ１である場合に、変位距離εの値を０とする。そして、仮想座標算出部１２０は、現在関節角度θ０ｂが、閾値角度Ｔ１から、閾値角度Ｔ１より大きい閾値角度Ｔ２までの間は、変位距離εの値を０とする。仮想座標算出部１２０は、現在関節角度θ０ｂが閾値角度Ｔ２から１８０°まで大きくなるに従い、変位距離εの値を０から連続的に増加させる。仮想座標算出部１２０は、現在関節角度θ０ｂが１８０°である場合、すなわち特異点角度である場合に、変位距離εの値を変位距離ε１とする。   FIG. 8 is a graph showing the relationship between the current joint angle and the displacement distance in the second embodiment. In the first embodiment, the displacement distance ε is a predetermined constant value. However, the virtual coordinate calculator 120 according to the second embodiment changes the displacement distance ε based on the value of the current joint angle θ0b. Let it. The horizontal axis in FIG. 8 is the current joint angle θ0b, and the vertical axis is the displacement distance ε. As shown in FIG. 8, when the current joint angle θ0b is 0 °, that is, when the imaginary point angle is a singular point angle, the virtual coordinate calculation unit 120 sets the value of the displacement distance ε to a predetermined value, the displacement distance ε1. I do. This ε1 has the same value as the displacement distance ε shown in the first embodiment. The virtual coordinate calculation unit 120 continuously decreases the value of the displacement distance ε as the current joint angle θ0b increases from 0 ° to the threshold angle T1. The virtual coordinate calculation unit 120 sets the value of the displacement distance ε to 0 when the current joint angle θ0b is the threshold angle T1. Then, the virtual coordinate calculation unit 120 sets the value of the displacement distance ε to 0 while the current joint angle θ0b is between the threshold angle T1 and the threshold angle T2 that is larger than the threshold angle T1. The virtual coordinate calculator 120 continuously increases the value of the displacement distance ε from 0 as the current joint angle θ0b increases from the threshold angle T2 to 180 °. The virtual coordinate calculation unit 120 sets the value of the displacement distance ε to the displacement distance ε1 when the current joint angle θ0b is 180 °, that is, when it is a singular point angle.

閾値角度Ｔ１及び閾値角度Ｔ２は、予め定められた値である。閾値角度Ｔ１は、例えば、１°以上５°以下であり、閾値角度Ｔ２は、１７５°以上１７９°以下である。ただし、閾値角度Ｔ１及び閾値角度Ｔ２は、現在関節角度θ０ｂが閾値角度Ｔ１と閾値角度Ｔ２との間である場合に、算出されたトルク値が出力可能なトルク値を上回らないように設定されるものであれば、任意の値とすることができる。出力可能なトルク値は、リンク長やモータ定格出力など、パワーアシストスーツ１０の性能やその制御性に応じて異なるものである。従って、閾値角度Ｔ１及び閾値角度Ｔ２の値は、パワーアシストスーツ１０の性能や動かしたい制御性に基づき任意に設定することができる。また、図８に示すように、変位距離εの値は、現在関節角度θ０ｂに対する非線形関数であるということができる。ただし、変位距離εの値は、（ｘ０２＋ｚ０２）０．５に対する非線形関数であってもよい。 The threshold angle T1 and the threshold angle T2 are predetermined values. The threshold angle T1 is, for example, 1 ° or more and 5 ° or less, and the threshold angle T2 is 175 ° or more and 179 ° or less. However, the threshold angle T1 and the threshold angle T2 are set so that the calculated torque value does not exceed the outputtable torque value when the current joint angle θ0b is between the threshold angle T1 and the threshold angle T2. Any value can be used. The torque value that can be output differs depending on the performance of the power assist suit 10 and its controllability, such as the link length and the rated motor output. Therefore, the values of the threshold angle T1 and the threshold angle T2 can be arbitrarily set based on the performance of the power assist suit 10 and the controllability to be moved. In addition, as shown in FIG. 8, the value of the displacement distance ε can be said to be a nonlinear function with respect to the current joint angle θ0b. However, the value of the displacement distance ε may be a nonlinear function with respect to (x0 2 + z0 2) 0.5 .

変位距離εは、閾値角度Ｔ１から閾値角度Ｔ２までの間、すなわち現在関節角度θ０ｂが特異点角度でない所定の範囲内の角度である場合に、０となる。変位距離εが０ということは、仮想座標Ｃ１は現在座標Ｃ０と同じ位置であるため、変位角度Δθが０である。仮想関節角度取得部１０６は、現在関節角度θ０ｂが所定の範囲内の角度である場合に、変位距離εを０とすることで変位角度Δθを０とし、仮想関節角度θ１ａ、θ１ｂ、θ１ｃとして現在関節角度θ０ａ、θ０ｂ、θ０ｃを用いる。そして、駆動トルク値算出部１１０は、現在関節角度θ０ａ、θ０ｂ、θ０ｃの値を用いて駆動トルク値τ１を算出する。   The displacement distance ε is 0 between the threshold angle T1 and the threshold angle T2, that is, when the current joint angle θ0b is an angle within a predetermined range that is not a singular point angle. When the displacement distance ε is 0, the virtual coordinate C1 is at the same position as the current coordinate C0, and thus the displacement angle Δθ is 0. When the current joint angle θ0b is within a predetermined range, the virtual joint angle acquisition unit 106 sets the displacement distance ε to 0, sets the displacement angle Δθ to 0, and sets the virtual joint angles θ1a, θ1b, and θ1c to the current values. The joint angles θ0a, θ0b, and θ0c are used. Then, the drive torque value calculation unit 110 calculates the drive torque value τ1 using the values of the current joint angles θ0a, θ0b, and θ0c.

このように、第２実施形態に係る制御装置１は、特異点角度である場合に、変位距離ε１を用いて特異点における出力異常を抑制しつつ、実際の関節角度である現在関節角度θ０ａ、θ０ｂ、θ０ｃの値を用いて駆動トルク値τ１を算出する所定の角度範囲を有する。言い換えれば、第２実施形態に係る制御装置１は、現在関節角度θ０ｂが特異点角度であるかを検出して、特異点角度である場合には仮想関節角度を用いて駆動トルク値τ１を算出し、特異点角度でない所定の範囲内の角度である場合には現在関節角度を用いて駆動トルク値τ１を算出するということもできる。従って、第２実施形態に係る制御装置１は、特異点角度である場合に変位距離ε１を用いて出力異常を抑制しつつ、特異点でない場合には、実際の関節角度を用いることで、より正確に出力を行うことができる。   As described above, the control device 1 according to the second embodiment suppresses the output abnormality at the singular point by using the displacement distance ε1 when the singular point angle is used, and the current joint angle θ0a, which is the actual joint angle, It has a predetermined angle range for calculating the drive torque value τ1 using the values of θ0b and θ0c. In other words, the control device 1 according to the second embodiment detects whether the current joint angle θ0b is a singular point angle, and calculates the drive torque value τ1 using the virtual joint angle if the current joint angle θ0b is a singular point angle. However, if the angle is within a predetermined range that is not the singular point angle, the drive torque value τ1 can be calculated using the current joint angle. Therefore, the control device 1 according to the second embodiment suppresses the output abnormality using the displacement distance ε1 when the singularity is the singularity, and uses the actual joint angle when the singularity is not the singularity. Output can be performed accurately.

また、逆ヤコビ行列は、特異点角度でなくても、特異点角度の近傍であって特異点角度から所定の角度だけ離れた特異点近傍角度である場合には、値が過大となる。従って、駆動トルク値τ１の値は、特異点近傍角度でも異常となるおそれがある。第２実施形態に係る制御装置１は、閾値角度Ｔ１と閾値角度Ｔ２との間を、特異点角度から特異点近傍角度の範囲外としている。第２実施形態に係る制御装置１は、現在関節角度θ０ｂが特異点角度から特異点近傍角度である場合、すなわち、現在関節角度θ０ｂが０°以上であって閾値角度Ｔ１より小さい範囲、及び現在関節角度θ０ｂが閾値角度Ｔ２より大きく１８０°以下の範囲である場合に、変位距離εを０より大きくしている。そして、第２実施形態に係る制御装置１は、現在関節角度θ０ｂが特異点角度から特異点近傍角度の範囲外である場合、すなわち、現在関節角度θ０ｂが閾値角度Ｔ１以上閾値角度Ｔ２以下である場合に、変位距離εを０としている。   In addition, the inverse Jacobian matrix has an excessive value if the angle is not a singularity angle but is near the singularity angle and is an angle near the singularity that is separated from the singularity angle by a predetermined angle. Therefore, the value of the drive torque value τ1 may be abnormal even at an angle near the singular point. The control device 1 according to the second embodiment sets the range between the threshold angle T1 and the threshold angle T2 outside the range of the singular point angle to the singular point vicinity angle. The control device 1 according to the second embodiment is configured such that when the current joint angle θ0b is an angle close to the singular point from the singular point angle, that is, when the current joint angle θ0b is equal to or more than 0 ° and smaller than the threshold angle T1, When the joint angle θ0b is greater than the threshold angle T2 and equal to or less than 180 °, the displacement distance ε is greater than zero. The control device 1 according to the second embodiment, when the current joint angle θ0b is out of the range of the singular point angle to the singular point vicinity angle, that is, the current joint angle θ0b is equal to or more than the threshold angle T1 and equal to or less than the threshold angle T2. In this case, the displacement distance ε is set to 0.

変位距離εが０より大きいと、変位角度Δθは０より大きくなる。従って、第２実施形態において、変位角度Δθは、現在関節角度θ０ｂが、特異点角度から特異点近傍角度までの間である場合に、０より大きい値となる。そして、変位角度Δθは、現在関節角度θ０ｂが、特異点角度から特異点近傍角度の範囲外である場合に、０となる。そのため、第２実施形態に係る制御装置１は、逆ヤコビ行列の値が過大になる特異点近傍角度においても、駆動トルク値τ１の出力異常を抑制することができる。   When the displacement distance ε is larger than 0, the displacement angle Δθ becomes larger than 0. Therefore, in the second embodiment, the displacement angle Δθ is a value greater than 0 when the current joint angle θ0b is between the singular point angle and the angle near the singular point. Then, the displacement angle Δθ becomes 0 when the current joint angle θ0b is out of the range of the singular point angle to the singular point neighboring angle. Therefore, the control device 1 according to the second embodiment can suppress the output abnormality of the drive torque value τ1 even at an angle near the singular point where the value of the inverse Jacobian matrix becomes excessive.

（第３実施形態）
次に、第３実施形態について説明する。第３実施形態に係る制御装置１ａは、予め記憶している現在関節角度と仮想関節角度との関係に基づき仮想関節角度を算出する点で、第１実施形態とは異なる。第３実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。 (Third embodiment)
Next, a third embodiment will be described. The control device 1a according to the third embodiment is different from the first embodiment in that a virtual joint angle is calculated based on a relationship between a current joint angle and a virtual joint angle stored in advance. In the third embodiment, description of portions having the same configuration as the first embodiment will be omitted.

図９は、第３実施形態に係る制御装置のブロック図である。図９に示すように、制御装置１ａが有する第２制御部１Ｂａは、仮想関節角度取得部１０６ａを有する。仮想関節角度取得部１０６ａは、関係記憶部１２１ａと仮想関節角度算出部１２２ａとを有する。   FIG. 9 is a block diagram of a control device according to the third embodiment. As shown in FIG. 9, the second control unit 1Ba of the control device 1a has a virtual joint angle acquisition unit 106a. The virtual joint angle acquisition unit 106a includes a relationship storage unit 121a and a virtual joint angle calculation unit 122a.

図１０は、第３実施形態における現在関節角度と仮想関節角度との関係を示すグラフである。関係記憶部１２１ａは、現在関節角度θ０ｂと仮想関節角度θ1ｂとの関係である関節角度関係を記憶している。図１０は、関係記憶部１２１ａが記憶する関節角度関係を示すグラフであり、横軸が現在関節角度θ０ｂであり、縦軸が仮想関節角度θ1ｂである。図１０に示すように、関節角度関係では、現在関節角度θ０ｂが０°である場合、すなわち特異点角度である場合に、仮想関節角度θ1ｂの値がθ1ｂａとなる。θ1ｂａは、例えば、１°以上５°以下である。ただし、θ1ｂａは、仮想関節角度θ1ｂの値がθ1ｂａである場合に、算出されたトルク値が出力可能なトルク値を上回らないように設定されるものであれば、任意の値とすることができる。出力可能なトルク値は、リンク長やモータ定格出力など、パワーアシストスーツ１０の性能やその制御性に応じて異なるものである。従って、θ1ｂａの値は、パワーアシストスーツ１０の性能や動かしたい制御性に基づき任意に設定することができる。関節角度関係では、現在関節角度θ０ｂが０°から閾値角度Ｔ３まで大きくなるに従って、仮想関節角度θ1ｂの値が、θ1ｂａからθ1ｂｂまで連続的に増加する。θ1ｂｂは、閾値角度Ｔ３と同じ値である。すなわち、現在関節角度θ０ｂが０°以上であって閾値角度Ｔ３より小さい場合、仮想関節角度θ1ｂは、対応する現在関節角度θ０ｂの値より大きくなる。そして、現在関節角度θ０ｂが閾値角度Ｔ３である場合、仮想関節角度θ1ｂの値が現在関節角度θ０ｂと同じになる。   FIG. 10 is a graph showing the relationship between the current joint angle and the virtual joint angle in the third embodiment. The relationship storage unit 121a stores a joint angle relationship that is a relationship between the current joint angle θ0b and the virtual joint angle θ1b. FIG. 10 is a graph showing the joint angle relationship stored in the relationship storage unit 121a. The horizontal axis is the current joint angle θ0b, and the vertical axis is the virtual joint angle θ1b. As shown in FIG. 10, in the joint angle relationship, when the current joint angle θ0b is 0 °, that is, when it is a singular point angle, the value of the virtual joint angle θ1b is θ1ba. θ1ba is, for example, 1 ° or more and 5 ° or less. However, θ1ba can be any value as long as the calculated torque value is set so as not to exceed the outputtable torque value when the value of the virtual joint angle θ1b is θ1ba. . The torque value that can be output differs depending on the performance of the power assist suit 10 and its controllability, such as the link length and the rated motor output. Therefore, the value of θ1ba can be arbitrarily set based on the performance of the power assist suit 10 and the controllability to be moved. As for the joint angle relationship, as the current joint angle θ0b increases from 0 ° to the threshold angle T3, the value of the virtual joint angle θ1b continuously increases from θ1ba to θ1bb. θ1bb is the same value as the threshold angle T3. That is, when the current joint angle θ0b is greater than or equal to 0 ° and smaller than the threshold angle T3, the virtual joint angle θ1b becomes larger than the corresponding value of the current joint angle θ0b. When the current joint angle θ0b is the threshold angle T3, the value of the virtual joint angle θ1b becomes the same as the current joint angle θ0b.

関節角度関係は、現在関節角度θ０ｂが閾値角度Ｔ３から閾値角度Ｔ４まで大きくなるに従って、仮想関節角度θ1ｂの値が、θ1ｂｂからθ1ｂｃまで直線的に増加する。θ1ｂｃは、閾値角度Ｔ４と同じ値である。すなわち、関節角度関係は、現在関節角度θ０ｂが閾値角度Ｔ３から閾値角度Ｔ４の間は、仮想関節角度θ1ｂが現在関節角度θ０ｂと同じ値になる。   As for the joint angle relationship, as the current joint angle θ0b increases from the threshold angle T3 to the threshold angle T4, the value of the virtual joint angle θ1b linearly increases from θ1bb to θ1bc. θ1bc is the same value as the threshold angle T4. That is, as for the joint angle relationship, the virtual joint angle θ1b has the same value as the current joint angle θ0b when the current joint angle θ0b is between the threshold angle T3 and the threshold angle T4.

関節角度関係は、現在関節角度θ０ｂが閾値角度Ｔ４から１８０°まで大きくなるに従って、仮想関節角度θ1ｂの値が、θ1ｂｃからθ1ｂｄまで連続的に増加する。θ1ｂｄは、１８０°より小さい値であり、例えば１７５°以上１７９°以下である。ただし、θ1ｂｄは、仮想関節角度θ1ｂの値がθ1ｂｄである場合に、算出されたトルク値が出力可能なトルク値を上回らないように設定されるものであれば、任意の値とすることができる。出力可能なトルク値は、リンク長やモータ定格出力など、パワーアシストスーツ１０の性能やその制御性に応じて異なるものである。従って、θ1ｂｄの値は、パワーアシストスーツ１０の性能や動かしたい制御性に基づき任意に設定することができる。すなわち、現在関節角度θ０ｂが閾値角度Ｔ４より大きく１８０°以下である場合、仮想関節角度θ1ｂは、対応する現在関節角度θ０ｂの値より小さくなる。   Regarding the joint angle relationship, as the current joint angle θ0b increases from the threshold angle T4 to 180 °, the value of the virtual joint angle θ1b continuously increases from θ1bc to θ1bd. θ1bd is a value smaller than 180 °, for example, 175 ° or more and 179 ° or less. However, θ1bd can be any value as long as the calculated torque value is set so as not to exceed the outputtable torque value when the value of the virtual joint angle θ1b is θ1bd. . The torque value that can be output differs depending on the performance of the power assist suit 10 and its controllability, such as the link length and the rated motor output. Therefore, the value of θ1bd can be set arbitrarily based on the performance of the power assist suit 10 and the controllability to be moved. That is, when the current joint angle θ0b is larger than the threshold angle T4 and equal to or smaller than 180 °, the virtual joint angle θ1b becomes smaller than the corresponding value of the current joint angle θ0b.

変位角度Δθは、現在関節角度θ０ｂと仮想関節角度θ1ｂとの差である。従って、変位角度Δθは、現在関節角度θ０ｂが閾値角度Ｔ３から閾値角度Ｔ４までの間は０であり、それ以外では、０より大きい値となる。すなわち、関節角度関係は、現在関節角度θ０ｂの値に応じて、変位角度Δθが異なるものであるということができる。なお、閾値角度Ｔ３及び閾値角度Ｔ４は、予め定められた値である。閾値角度Ｔ３は、例えば１°以上５°以下であり、閾値角度Ｔ４は、例えば１７５°以上１７９°以下である。ただし、閾値角度Ｔ３及び閾値角度Ｔ４は、現在関節角度θ０ｂが閾値角度Ｔ３と閾値角度Ｔ４との間である場合に、算出されたトルク値が出力可能なトルク値を上回らないように設定されるものであれば、任意の値とすることができる。出力可能なトルク値は、リンク長やモータ定格出力など、パワーアシストスーツ１０の性能やその制御性に応じて異なるものである。従って、閾値角度Ｔ３及び閾値角度Ｔ４の値は、パワーアシストスーツ１０の性能や動かしたい制御性に基づき任意に設定することができる。   The displacement angle Δθ is the difference between the current joint angle θ0b and the virtual joint angle θ1b. Accordingly, the displacement angle Δθ is 0 when the current joint angle θ0b is between the threshold angle T3 and the threshold angle T4, and is a value larger than 0 otherwise. That is, it can be said that the joint angle relationship is such that the displacement angle Δθ differs depending on the value of the current joint angle θ0b. Note that the threshold angle T3 and the threshold angle T4 are predetermined values. The threshold angle T3 is, for example, 1 ° or more and 5 ° or less, and the threshold angle T4 is, for example, 175 ° or more and 179 ° or less. However, the threshold angle T3 and the threshold angle T4 are set so that the calculated torque value does not exceed the outputtable torque value when the current joint angle θ0b is between the threshold angle T3 and the threshold angle T4. Any value can be used. The torque value that can be output differs depending on the performance of the power assist suit 10 and its controllability, such as the link length and the rated motor output. Therefore, the values of the threshold angle T3 and the threshold angle T4 can be arbitrarily set based on the performance of the power assist suit 10 and the controllability to be moved.

仮想関節角度算出部１２２ａは、この関節角度関係に基づき、取得した現在関節角度θ０ｂの値における仮想関節角度θ1ｂの値を算出する。仮想関節角度算出部１２２ａは、この仮想関節角度θ1ｂの値に基づき、他の全ての仮想関節角度θ１、ここでは仮想関節角度θ1a、θ1ｃを算出する。仮想関節角度θ1ｂとリンクの長さと現在座標Ｃ０が分かっているため、仮想関節角度算出部１２２ａは、仮想関節角度θ1ｂの値に基づき、仮想関節角度θ1a、θ1ｃを算出することができる。すなわち、第３実施形態における第２制御部１Ｂａは、仮想座標Ｃ１を算出せずに、関節関係角度に基づき、仮想関節角度θ1a、θ1ｂ、θ1ｃの値を算出する。   The virtual joint angle calculation unit 122a calculates the value of the virtual joint angle θ1b at the acquired value of the current joint angle θ0b based on the joint angle relationship. The virtual joint angle calculation unit 122a calculates all other virtual joint angles θ1, here virtual joint angles θ1a and θ1c, based on the value of the virtual joint angle θ1b. Since the virtual joint angle θ1b, the length of the link, and the current coordinate C0 are known, the virtual joint angle calculation unit 122a can calculate the virtual joint angles θ1a and θ1c based on the value of the virtual joint angle θ1b. That is, the second control unit 1Ba in the third embodiment calculates the values of the virtual joint angles θ1a, θ1b, and θ1c based on the joint relation angles without calculating the virtual coordinates C1.

変位角度Δθは、現在関節角度θ０ｂが閾値角度Ｔ３から閾値角度Ｔ４までの間、すなわち現在関節角度θ０ｂが特異点角度でない所定の角度範囲の場合に、０となる。仮想関節角度取得部１０６ａは、現在関節角度θ０ｂが所定の角度範囲内である場合に、関節角度関係に基づき変位角度Δθを０とし、仮想関節角度θ１ａ、θ１ｂ、θ１ｃとして現在関節角度θ０ａ、θ０ｂ、θ０ｃを用いる。そして、駆動トルク値算出部１１０は、現在関節角度θ０ｂが所定の角度範囲内である場合、現在関節角度θ０ａ、θ０ｂ、θ０ｃの値を用いて駆動トルク値τ１を算出する。   The displacement angle Δθ is 0 when the current joint angle θ0b is between the threshold angle T3 and the threshold angle T4, that is, when the current joint angle θ0b is in a predetermined angle range that is not a singular point angle. When the current joint angle θ0b is within a predetermined angle range, the virtual joint angle acquisition unit 106a sets the displacement angle Δθ to 0 based on the joint angle relationship, and sets the current joint angles θ0a, θ0b as virtual joint angles θ1a, θ1b, θ1c. , Θ0c. Then, when the current joint angle θ0b is within the predetermined angle range, the drive torque value calculation unit 110 calculates the drive torque value τ1 using the values of the current joint angles θ0a, θ0b, and θ0c.

このように、第３実施形態に係る制御装置１ａは、特異点角度である場合に、変位角度Δθを０より大きくすることで特異点における出力異常を抑制しつつ、実際の関節角度である現在関節角度θ０ａ、θ０ｂ、θ０ｃの値を用いて駆動トルク値τ１を算出する所定の角度範囲を有する。言い換えれば、第３実施形態に係る制御装置１ａは、現在関節角度θ０ｂが特異点角度であるかを検出して、特異点角度である場合には仮想関節角度を用いて駆動トルク値τ１を算出し、所定の角度範囲である場合には現在関節角度を用いて駆動トルク値τ１を算出するということもできる。従って、第３実施形態に係る制御装置１ａは、特異点角度である場合に仮想関節角度を用いて出力異常を抑制しつつ、特異点でない場合には、実際の関節角度を用いることで、より正確に出力を行うことができる。   As described above, the control device 1a according to the third embodiment suppresses the output abnormality at the singular point by setting the displacement angle Δθ to be greater than 0 when the singular point is the singular point angle. It has a predetermined angle range for calculating the drive torque value τ1 using the values of the joint angles θ0a, θ0b, θ0c. In other words, the control device 1a according to the third embodiment detects whether the current joint angle θ0b is a singular point angle, and calculates the driving torque value τ1 using the virtual joint angle if the current joint angle θ0b is a singular point angle. However, when the angle is within the predetermined angle range, the drive torque value τ1 can be calculated using the current joint angle. Therefore, the control device 1a according to the third embodiment suppresses the output abnormality using the virtual joint angle when the singular point is used, and uses the actual joint angle when the singular point is not used. Output can be performed accurately.

第３実施形態に係る制御装置１ａは、閾値角度Ｔ３と閾値角度Ｔ４との間を、特異点角度から特異点近傍角度の範囲外としている。制御装置１ａは、現在関節角度θ０ｂが特異点角度から特異点近傍角度である場合、すなわち、現在関節角度θ０ｂが０°以上であって閾値角度Ｔ３より小さい範囲、及び現在関節角度θ０ｂが閾値角度Ｔ４より大きく１８０°以下の範囲である場合に、変位角度Δθを０より大きくしている。そして、第３実施形態に係る制御装置１ａは、現在関節角度θ０ｂが特異点角度から特異点近傍角度の範囲外である場合、すなわち、現在関節角度θ０ｂが閾値角度Ｔ３以上閾値角度Ｔ４以下である場合に、変位角度Δθを０としている。そのため、第３実施形態に係る制御装置１ａは、逆ヤコビ行列の値が過大になる特異点近傍角度においても、駆動トルク値τ１の出力異常を抑制することができる。   The control device 1a according to the third embodiment sets the range between the threshold angle T3 and the threshold angle T4 outside the range of the singular point angle to the singular point vicinity angle. When the current joint angle θ0b is an angle close to the singular point from the singular point angle, that is, when the current joint angle θ0b is equal to or greater than 0 ° and smaller than the threshold angle T3, the control device 1a sets the current joint angle θ0b to the threshold angle. When the angle is larger than T4 and equal to or smaller than 180 °, the displacement angle Δθ is set larger than 0. Then, the control device 1a according to the third embodiment, when the current joint angle θ0b is out of the range of the singular point angle to the singular point vicinity angle, that is, the current joint angle θ0b is equal to or more than the threshold angle T3 and equal to or less than the threshold angle T4. In this case, the displacement angle Δθ is set to 0. Therefore, the control device 1a according to the third embodiment can suppress the abnormal output of the driving torque value τ1 even at an angle near the singular point where the value of the inverse Jacobian matrix becomes excessive.

以上説明したように、第３実施形態に係る制御装置１ａは、現在関節角度θ０ｂと仮想関節角度θ１ｂとの関節角度関係を記憶する関係記憶部１２１ａと、現在関節角度θ０ｂと関節角度関係に基づき、仮想関節角度θ１ａ、θ１ｂ、θ１ｃを算出する仮想関節角度算出部１２２ａと、を有する。関節角度関係は、現在関節角度θ０ｂの値に応じて、変位角度Δθが異なるものである。この制御装置１ａは、関節角度関係に基づき仮想関節角度θ１ａ、θ１ｂ、θ１ｃを算出するため、駆動トルク値τ１の出力異常を適切に抑制することができる。   As described above, the control device 1a according to the third embodiment includes a relationship storage unit 121a that stores the joint angle relationship between the current joint angle θ0b and the virtual joint angle θ1b, and the control device 1a based on the current joint angle θ0b and the joint angle relationship. And a virtual joint angle calculation unit 122a that calculates virtual joint angles θ1a, θ1b, and θ1c. The joint angle relationship is such that the displacement angle Δθ differs according to the value of the current joint angle θ0b. The control device 1a calculates the virtual joint angles θ1a, θ1b, and θ1c based on the joint angle relationship, so that the output abnormality of the drive torque value τ1 can be appropriately suppressed.

（第４実施形態）
次に、第４実施形態について説明する。第４実施形態に係る制御装置１ｂは、補正係数Ｌに基づき、駆動トルク値を算出する点で、第１実施形態とは異なる。第４実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。 (Fourth embodiment)
Next, a fourth embodiment will be described. The control device 1b according to the fourth embodiment is different from the first embodiment in that a drive torque value is calculated based on a correction coefficient L. In the fourth embodiment, description of portions having the same configuration as the first embodiment will be omitted.

図１１は、第４実施形態に係る制御装置のブロック図である。図１１に示すように、制御装置１ｂが有する第２制御部１Ｂｂは、補正係数記憶部１０９ｂと、駆動トルク値算出部１１０ｂとを有する。   FIG. 11 is a block diagram of a control device according to the fourth embodiment. As shown in FIG. 11, the second control unit 1Bb of the control device 1b includes a correction coefficient storage unit 109b and a drive torque value calculation unit 110b.

補正係数記憶部１０９ｂは、変位角度Δθだけ関節角度を変化させた場合のリンクの角加速度の変化量の割合を示す補正係数Ｌを記憶している。補正係数Ｌは、第１基準角加速度θ２”と第２基準角加速度θ３”とに基づき算出される。第１基準角加速度θ２”は、第１基準関節角度θ２と、リンクの基準加速度ａｂとに基づき算出されたリンクの角加速度である。第１基準関節角度θ２は、第１基準角加速度θ２”の算出のために任意に設定された、腰関節、膝関節、及び足首関節の計算上の関節角度である。ただし、第１基準関節角度θ２は、特異点角度ではない角度である。さらに、第１基準関節角度θ２は、特異点近傍角度の範囲内であることが好ましい。基準加速度ａｂは、第１基準角加速度θ２”の算出のために任意に設定された足装着部４３（足リンク）の加速度である。 The correction coefficient storage unit 109b stores a correction coefficient L indicating a ratio of a change amount of the angular acceleration of the link when the joint angle is changed by the displacement angle Δθ. The correction coefficient L is calculated based on the first reference angular acceleration θ2 ″ and the second reference angular acceleration θ3 ″. The first reference angular acceleration θ2 ″ is the link angular acceleration calculated based on the first reference joint angle θ2 and the link reference acceleration ab . The first reference joint angle θ2 is the first reference angular acceleration θ2. "Are calculated joint angles of the waist joint, the knee joint, and the ankle joint arbitrarily set for the calculation of"". However, the first reference joint angle θ2 is an angle that is not a singular point angle. Furthermore, it is preferable that the first reference joint angle θ2 be within the range of the singular point vicinity angle. The reference acceleration ab is an acceleration of the foot mounting portion 43 (foot link) arbitrarily set for calculating the first reference angular acceleration θ2 ″.

第２基準角加速度θ３”は、第２基準関節角度θ３と、リンクの基準加速度ａｂとに基づき算出されたリンクの角加速度である。第２基準関節角度θ３は、第１基準関節角度θ２からそれぞれ変位角度Δθだけ異なる、腰関節、膝関節、及び足首関節の計算上の関節角度である。第２基準関節角度θ３は、例えば第１基準関節角度θ２を現在関節角度θ０として、第１実施形態に示した仮想関節角度θ１の算出方法と同じ方法で算出される。第１基準角加速度θ２”及び 第２基準角加速度θ３”は、上述の式（４）のｘ”を基準加速度ａｂに置き換えて算出される。なお、第２基準関節角度θ３は、第１基準関節角度θ２から変位角度Δθだけ異なるものであったが、これに限られず、第１基準関節角度θ２から任意の角度だけ異なるものであってもよい。この場合、第２基準関節角度θ３と第１基準関節角度θ２との差分は、変位角度Δθ以下の値であることが好ましい。 The second reference angular acceleration θ3 ″ is the link angular acceleration calculated based on the second reference joint angle θ3 and the link reference acceleration ab . The second reference joint angle θ3 is the first reference joint angle θ2. Are the calculated joint angles of the hip joint, the knee joint, and the ankle joint that differ from each other by a displacement angle Δθ.The second reference joint angle θ3 is, for example, the first reference joint angle θ2 as the current joint angle θ0. The first reference angular acceleration θ2 ″ and the second reference angular acceleration θ3 ″ are calculated by the same method as the calculation method of the virtual joint angle θ1 described in the embodiment. It is calculated by replacing with b . Although the second reference joint angle θ3 is different from the first reference joint angle θ2 by the displacement angle Δθ, the present invention is not limited to this, and the second reference joint angle θ3 may be different from the first reference joint angle θ2 by an arbitrary angle. Good. In this case, the difference between the second reference joint angle θ3 and the first reference joint angle θ2 is preferably a value equal to or smaller than the displacement angle Δθ.

具体的には、補正係数Ｌは、次の式（８Ａ）に基づき算出されるものである。   Specifically, the correction coefficient L is calculated based on the following equation (8A).

Ｌ＝（θ２”−θ３”）／{Ｊ＃（θ２）−Ｊ＃（θ３）} ・・・（８Ａ） L = (θ2 ″ −θ3 ″) / {J # (θ2) −J # (θ3)} (8A)

Ｊ＃（θ２）は、第１基準関節角度θ２である場合の逆ヤコビ行列であり、Ｊ＃（θ２）は、第２基準関節角度θ３である場合の逆ヤコビ行列である。補正係数Ｌは、スカラー値である。 J # (θ2) is the inverse Jacobian matrix when the first reference joint angle θ2, and J # (θ2) is the inverse Jacobian matrix when the second reference joint angle θ3. The correction coefficient L is a scalar value.

第２基準角加速度θ３”は、第１基準関節角度θ２と変位角度Δθだけ異なった第２基準関節角度θ３から算出されるものである。従って、補正係数Ｌは、仮想関節角度θ１における仮想関節角速度に対する、現在関節角度θ０における現在関節角速度の比率に相当する。さらに言えば、補正係数Ｌは、関節角度が異なった場合の角加速度の変化量の感度であるということができる。さらに言えば、補正係数Ｌは、逆ヤコビ行列の違いが角速度に及ぼす変化量の度合いであるということもできる。なお、補正係数記憶部１０９ｂは、第１基準関節角度θ２ａ、θ２ｂ、θ２ｃの値を異ならせて算出した補正係数を平均したものを、補正係数Ｌとして記憶してもよい。   The second reference angular acceleration θ3 ″ is calculated from the second reference joint angle θ3 different from the first reference joint angle θ2 by the displacement angle Δθ. Therefore, the correction coefficient L is the virtual joint at the virtual joint angle θ1. This corresponds to the ratio of the current joint angular velocity at the current joint angle θ0 to the angular velocity, and furthermore, it can be said that the correction coefficient L is the sensitivity of the amount of change in angular acceleration when the joint angle is different. , The correction coefficient L is the degree of change in the angular velocity caused by the difference in the inverse Jacobian matrix, and the correction coefficient storage unit 109b changes the values of the first reference joint angles θ2a, θ2b, θ2c. The average of the correction coefficients calculated as above may be stored as the correction coefficient L.

駆動トルク値算出部１１０ｂは、目標加速度ａと仮想関節角度θ１ａ、θ１ｂ、θ１ｃと補正係数Ｌとに基づき、駆動トルク値τ１を算出する。具体的には、駆動トルク値算出部１１０ｂは、次の式（８Ｂ）に示すように、補正係数Ｌを目的角加速度に乗じることにより、駆動トルク値τ１を算出する。   The drive torque value calculation unit 110b calculates a drive torque value τ1 based on the target acceleration a, the virtual joint angles θ1a, θ1b, θ1c, and the correction coefficient L. Specifically, the drive torque value calculation unit 110b calculates the drive torque value τ1 by multiplying the target angular acceleration by the correction coefficient L as shown in the following equation (8B).

τ１＝Ｍ・Ｌ・Ｊ＃（ａ−Ｊ’・θ’） ・・・（８Ｂ） τ1 = MLJ # (a-J'-θ ') (8B)

以上説明したように、第４実施形態に係る制御装置１ｂは、変位角度Δθだけ関節角度を変化させた場合のリンクの角加速度の変化量を示す補正係数Ｌを記憶する補正係数記憶部１０９ｂを更に有する。駆動トルク値算出部１１０ｂは、目標加速度ａと仮想関節角度θ１と補正係数Ｌとに基づき、駆動トルク値τ１を算出する。補正係数Ｌは、所定の関節角度である第１基準関節角度θ２に基づき算出された第１基準角加速度θ２”と、第１基準関節角度θ２から変位角度Δθだけ異なる第２基準関節角度θ３に基づき算出された第２基準角加速度とに基づき算出される。   As described above, the control device 1b according to the fourth embodiment includes the correction coefficient storage unit 109b that stores the correction coefficient L indicating the amount of change in the angular acceleration of the link when the joint angle is changed by the displacement angle Δθ. Have more. The drive torque value calculation unit 110b calculates a drive torque value τ1 based on the target acceleration a, the virtual joint angle θ1, and the correction coefficient L. The correction coefficient L is set to a first reference angular acceleration θ2 ″ calculated based on a first reference joint angle θ2 which is a predetermined joint angle, and a second reference joint angle θ3 different from the first reference joint angle θ2 by a displacement angle Δθ. It is calculated based on the second reference angular acceleration calculated on the basis of this.

仮想関節角度θ１に基づき算出された駆動トルク値τ１は、実際の関節角度に基づいて算出されたものではないため、目標とする目標加速度ａを達成するために必要なトルク値と異なる値となる場合がある。例えば、足を上げる動作を行う場合であって、膝を縮める方向に仮想関節角度θ１を設定した場合は、仮想関節角度θ１に基づき算出された駆動トルク値τ１は、目標加速度ａを達成するために必要なトルク値よりも小さくなる。しかし、第４実施形態に係る制御装置１ｂは、変位角度Δθだけ関節角度を変化させた場合のリンクの角加速度の変化量を、予め補正係数Ｌとして算出している。制御装置１ｂは、この補正係数Ｌで補正して駆動トルク値τ１を算出するため、実際の目標加速度ａにより近くなるように駆動トルク値τ１を補正することができる。そのため、制御装置１ｂは、駆動トルク値τ１の出力異常をより適切に抑制することができる。   Since the drive torque value τ1 calculated based on the virtual joint angle θ1 is not calculated based on the actual joint angle, the drive torque value τ1 is different from the torque value required to achieve the target acceleration a. There are cases. For example, in the case of performing the operation of raising the foot and setting the virtual joint angle θ1 in the direction of contracting the knee, the drive torque value τ1 calculated based on the virtual joint angle θ1 is used to achieve the target acceleration a. Is smaller than the required torque value. However, the control device 1b according to the fourth embodiment calculates the amount of change in the angular acceleration of the link when the joint angle is changed by the displacement angle Δθ as the correction coefficient L in advance. Since the control device 1b calculates the drive torque value τ1 by correcting with the correction coefficient L, the control device 1b can correct the drive torque value τ1 so as to be closer to the actual target acceleration a. Therefore, control device 1b can more appropriately suppress the output abnormality of drive torque value τ1.

（第５実施形態）
次に、第５実施形態について説明する。第５実施形態に係る制御装置１ｃは、第１実施形態と同様に、算出した駆動トルク値を用いてパワーアシストスーツ１０のアクチュエータを制御するものであるが、駆動トルク値の算出方法が、第１実施形態とは異なる。第５実施形態において、第１実施形態と内容が共通する箇所は、説明を省略する。 (Fifth embodiment)
Next, a fifth embodiment will be described. The control device 1c according to the fifth embodiment controls the actuator of the power assist suit 10 using the calculated drive torque value, as in the first embodiment. This is different from the first embodiment. In the fifth embodiment, description of portions having the same contents as in the first embodiment will be omitted.

図１２は、第５実施形態に係る制御装置のブロック図である。図１２に示すように、制御装置１ｃは、第１制御部１Ａｃと、第２制御部１Ｂｃと、アクチュエータ制御部１Ｃｃとを有する。第１制御部１Ａｃは、パワーアシストスーツ１０の自重を受け持つ制御、及びパワーアシストスーツ１０の装着者の脚に作用する負荷を補助する制御のためのトルクを算出する。後者の制御は、例えば、パワーアシストスーツ１０の装着者が物体を持つ際に、物体の質量による負荷をパワーアシストスーツ１０のリンク機構で受ける制御である。一方、第２制御部１Ｂｃは、リンクを目標加速度で移動させるためのトルクである駆動トルク値τ１を算出する。第１制御部１Ａｃが算出するトルク値を保持トルク値τ２とすると、アクチュエータ制御部１Ｃｃは、駆動トルク値τ１及び保持トルク値τ２を取得して、それらのトルク値を合計して、アクチュエータを駆動する。   FIG. 12 is a block diagram of a control device according to the fifth embodiment. As shown in FIG. 12, the control device 1c includes a first control unit 1Ac, a second control unit 1Bc, and an actuator control unit 1Cc. The first control unit 1Ac calculates torque for control for taking charge of the own weight of the power assist suit 10 and for control for assisting a load acting on a leg of a wearer of the power assist suit 10. The latter control is, for example, a control in which when the wearer of the power assist suit 10 has an object, the link mechanism of the power assist suit 10 receives a load due to the mass of the object. On the other hand, the second control unit 1Bc calculates a drive torque value τ1, which is a torque for moving the link at the target acceleration. Assuming that the torque value calculated by the first control unit 1Ac is the holding torque value τ2, the actuator control unit 1Cc acquires the driving torque value τ1 and the holding torque value τ2, sums those torque values, and drives the actuator. I do.

以下、アクチュエータを駆動するためのトルク値についてより詳細に説明する。アクチュエータを駆動するトルク値を総トルク値τ０とすると、総トルク値τ０は、一般的に次の式（９）で表される。   Hereinafter, the torque value for driving the actuator will be described in more detail. Assuming that a torque value for driving the actuator is a total torque value τ0, the total torque value τ0 is generally represented by the following equation (9).

τ０＝Ｍ・θ”＋ｈ（θ，θ’）＋ｇ(θ) ・・・（９）   τ0 = M · θ ″ + h (θ, θ ′) + g (θ) (9)

ここで、Ｍはリンクの慣性モーメントであり、関節角度θから算出される値である。θ’は関節角速度であり、θ”は関節角加速度である。Ｍ・θ”は、第２制御部１Ｂｃによって算出される駆動トルク値τ１である。ｇ(θ)は、重力に関連する項であり、第１制御部１Ａｃによって算出される保持トルク値τ２である。ｈ（θ，θ’）は、コリオリ力に対応する項であり、総トルク値τ０に対する寄与率が低いため、本実施形態では無視される。すなわち、本実施形態において、アクチュエータ制御部１Ｃｃは、駆動トルク値τ１及び保持トルク値τ２の合計値を、総トルク値τ０として算出し、その総トルク値τ０で、アクチュエータを駆動する。ただし、アクチュエータ制御部１Ｃｃは、ｈ（θ，θ’）についても算出し、駆動トルク値τ１と保持トルク値τ２とｈ（θ，θ’）との合計値を総トルク値τ０として算出してもよい。   Here, M is a moment of inertia of the link, and is a value calculated from the joint angle θ. θ ′ is the joint angular velocity, θ ″ is the joint angular acceleration. M · θ ″ is the drive torque value τ1 calculated by the second control unit 1Bc. g (θ) is a term related to gravity, and is a holding torque value τ2 calculated by the first control unit 1Ac. h (θ, θ ′) is a term corresponding to the Coriolis force, and is ignored in the present embodiment because its contribution to the total torque value τ0 is low. That is, in the present embodiment, the actuator control unit 1Cc calculates the total value of the driving torque value τ1 and the holding torque value τ2 as the total torque value τ0, and drives the actuator with the total torque value τ0. However, the actuator control unit 1Cc also calculates h (θ, θ ′), and calculates the total value of the driving torque value τ1, the holding torque value τ2, and h (θ, θ ′) as the total torque value τ0. Is also good.

さらに言えば、駆動トルク値τ１は、関節角加速度θ”に関連するトルクであり、リンクの目標とする移動加速度である目標加速度を実現するためのトルクである。言い換えれば、駆動トルク値τ１は、装着者の移動を補助するために先んじてリンクを動かすためのトルク値である。また、保持トルク値τ２は、関節角度θに関連するトルクであり、パワーアシストスーツ１０の自重や負荷を支えるためのトルクである。従って、アクチュエータ制御部１Ｃｃは、装着者の移動を補助するためのトルク（駆動トルク値τ１）と、重さを支えるためのトルク（保持トルク値τ２）との両方を実現できる総トルク値τ０で、アクチュエータを駆動する。   Furthermore, the driving torque value τ1 is a torque related to the joint angular acceleration θ ″, and is a torque for realizing a target acceleration that is a target moving acceleration of the link. In other words, the driving torque value τ1 is The holding torque value τ2 is a torque related to the joint angle θ and supports the weight and load of the power assist suit 10 in advance of moving the link to assist the movement of the wearer. Therefore, the actuator control unit 1Cc realizes both the torque for assisting the movement of the wearer (the driving torque value τ1) and the torque for supporting the weight (the holding torque value τ2). The actuator is driven with the total torque value τ0 that can be obtained.

次に、駆動トルク値τ１の算出式について説明する。ここで、リンクの速度ｘ’と関節角速度θ’とは、ヤコビ行列Ｊを用いて、次の式（１０）のような関係で表される。式（１０）の両辺を微分すると式（１１）のようになり、それを関節角加速度θ”でまとめると式（１２）のようになる。   Next, a calculation formula of the drive torque value τ1 will be described. Here, the link speed x ′ and the joint angular speed θ ′ are expressed by the following equation (10) using the Jacobian matrix J. Equation (11) is obtained by differentiating both sides of the equation (10), and is summarized as an equation (12) by summing them up with the joint angular acceleration θ ″.

ｘ’＝Ｊ・θ’ ・・・（１０）
ｘ”＝Ｊ’・θ’＋Ｊ・θ” ・・・（１１）
θ”-＝Ｊ＃（ｘ”−Ｊ’・θ’） ・・・（１２） x ′ = J · θ ′ (10)
x ″ = J ′ · θ ′ + J · θ ″ (11)
θ ″ − = J # (x ″ −J ′ · θ ′) (12)

なお、式（１１）におけるｘ”は直交三次元空間座標における加速度ベクトルであり、Ｊ’はヤコビ行列Ｊの一次微分値である。また、式（１２）におけるＪ＃は、ヤコビ行列Ｊが正方行列でない場合におけるヤコビ行列Ｊの逆行列である。駆動トルク値τ１は、式（１２）の右辺をＭ・θ”に代入すると、一般的に、次の式（１３）のように求められる。 In Equation (11), x ″ is an acceleration vector in orthogonal three-dimensional space coordinates, and J ′ is a first derivative of the Jacobi matrix J. J # in Equation (12) is a square of the Jacobian matrix J. This is the inverse matrix of the Jacobi matrix J when the matrix is not a matrix.The drive torque value τ1 is generally obtained by the following equation (13) by substituting the right side of equation (12) into M · θ ″.

τ１＝Ｍ・Ｊ＃（ｘ”−Ｊ’・θ’） ・・・（１３） τ1 = M · J # (x ″ −J ′ · θ ′) (13)

ヤコビ行列Ｊの逆行列であるＪ＃は、特異点において、ｄｅｔＪ＝０となってしまうため、ゼロ割が発生し値が発散する。この特異点における関節角度θを特異点角度とすると、特異点角度は、例えば関節が伸びきった関節角度θ＝０である。また、特異点角度でなくても、特異点角度の近傍の場合、ｄｅｔＪが小さい値となるため、ｄｅｔＪを分母に持つＪ＃の値も大きくなる。従って、Ｊ＃に基づき算出される駆動トルク値τ１は、特異点角度や特異点角度の近傍で、値が算出できなかったり、パワーアシストスーツ１０の出力可能なトルク値を超えてしまったりするおそれがある。本実施形態に係る第２制御部１Ｂｃは、特異点角度やその近傍において、算出する駆動トルク値τ１を、出力可能なトルク値以下の値とすることで、特異点角度における駆動トルク値τ１の出力異常を回避する。なお、保持トルク値τ２は、負荷及び自重と、関節角度θとに基づき算出することができるため、ヤコビ行列Ｊの逆行列を用いて算出する必要がない。従って、保持トルク値τ２は、特異点であっても算出することができる。 J # , which is the inverse matrix of the Jacobi matrix J, becomes detJ = 0 at the singularity, so that zero division occurs and the value diverges. Assuming that the joint angle θ at this singular point is the singular point angle, the singular point angle is, for example, the joint angle θ at which the joint is fully extended = 0. Even if the angle is not the singular point angle, in the case of the vicinity of the singular point angle, detJ becomes a small value, so that the value of J # having detJ in the denominator also becomes large. Therefore, the drive torque value τ1 calculated based on J # may not be calculated in the vicinity of the singular point angle or the singular point angle, or may exceed the output torque value of the power assist suit 10. There is. The second control unit 1Bc according to the present embodiment sets the drive torque value τ1 to be calculated at or below the singular point angle to a value equal to or less than the outputtable torque value, thereby obtaining the drive torque value τ1 at the singular point angle. Avoid output abnormalities. It should be noted that the holding torque value τ2 can be calculated based on the load, the own weight, and the joint angle θ, and thus does not need to be calculated using the inverse matrix of the Jacobi matrix J. Therefore, the holding torque value τ2 can be calculated even at a singular point.

以下、第２制御部１Ｂｃについて詳細に説明する。図１２に示すように、第２制御部１Ｂｃは、目標加速度取得部１３２と、仮駆動トルク値算出部１３４と、上限駆動トルク値取得部１３６と、トルク比率算出部１３８と、補正項算出部１４０と、駆動トルク値決定部１４２とを有する。   Hereinafter, the second control unit 1Bc will be described in detail. As shown in FIG. 12, the second control unit 1Bc includes a target acceleration acquisition unit 132, a provisional drive torque value calculation unit 134, an upper limit drive torque value acquisition unit 136, a torque ratio calculation unit 138, and a correction term calculation unit. 140 and a drive torque value determination unit 142.

目標加速度取得部１３２は、リンクの目標とする移動加速度である目標加速度ａを取得する。本実施形態においては、目標加速度取得部１３２は、足装着部４３（足リンク）の目標とする移動加速度を、目標加速度ａとして取得する。本実施形態においては、目標加速度取得部１３２は、装着者の足底荷重値の変化量に基づき、目標加速度ａを算出する。具体的には、目標加速度取得部１３２は、荷重検出装置４４が逐次検出した装着者の足底荷重値を取得し、取得した足底荷重値に基づき、足底荷重値の変化量を算出する。荷重検出装置４４が検出した装着者の足底荷重値を足底荷重値Ｆとすると、目標加速度取得部１３２は、足底荷重値Ｆの時間ｔでの一階微分値に対応する荷重値変化量ｆ（Ｆ）を算出する。荷重値変化量ｆ（Ｆ）は、足底荷重値Ｆの変化量である。ただし、目標加速度取得部１３２による足底荷重値の変化量の算出方法は、足底荷重値Ｆの変化量を一階微分で算出することに限られず、例えば、今回取得された足底荷重値Ｆと前回取得された足底荷重値Ｆとの差分を、足底荷重値の変化量としてもよい。   The target acceleration acquisition unit 132 acquires a target acceleration a that is a target movement acceleration of the link. In the present embodiment, the target acceleration acquisition unit 132 acquires the target movement acceleration of the foot mounting unit 43 (foot link) as the target acceleration a. In the present embodiment, the target acceleration acquisition unit 132 calculates the target acceleration a based on the amount of change in the sole load value of the wearer. Specifically, the target acceleration acquisition unit 132 acquires the sole load value of the wearer sequentially detected by the load detection device 44, and calculates a change amount of the sole load value based on the acquired sole load value. . Assuming that the sole load value of the wearer detected by the load detecting device 44 is a sole load value F, the target acceleration acquisition unit 132 calculates a change in the load value corresponding to the first derivative of the sole load value F at time t. Calculate the quantity f (F). The load value change amount f (F) is a change amount of the sole load value F. However, the method for calculating the amount of change in the sole load value by the target acceleration acquisition unit 132 is not limited to calculating the amount of change in the sole load value F by first-order differentiation. The difference between F and the previously obtained sole load value F may be used as the change amount of the sole load value.

目標加速度取得部１３２は、次の式（１４）に示すように、補正係数Ｋと荷重値変化量ｆ（Ｆ）とを乗じて、その乗じた値にマイナスを乗じることにより、目標加速度ａを算出する。   The target acceleration obtaining unit 132 multiplies the correction coefficient K by the load value change amount f (F) and multiplies the multiplied value by minus, as shown in the following equation (14), to obtain the target acceleration a. calculate.

ａ＝−Ｋ・ｆ（Ｆ） ・・・（１４）   a = −K · f (F) (14)

ここで、Ｋは予め定められた所定の係数である。目標加速度ａは、荷重値変化量ｆ（Ｆ）に対してマイナスが乗じられている。従って、目標加速度取得部１３２は、足底荷重値Ｆが増加した場合に、足を下げる又は踏ん張る方向へ加速度を発生させるように、目標加速度ａを算出する。また、目標加速度取得部１３２は、足底荷重値Ｆが減少した場合に、足を上げる方向へ加速度を発生させるように、目標加速度ａを算出する。   Here, K is a predetermined coefficient. The target acceleration a is obtained by multiplying the load value change amount f (F) by minus. Therefore, when the sole load value F increases, the target acceleration acquisition unit 132 calculates the target acceleration a so as to generate an acceleration in the direction of lowering or stepping on the foot. Further, the target acceleration acquisition unit 132 calculates the target acceleration “a” such that when the sole load value F decreases, the acceleration is generated in the direction in which the foot is raised.

目標加速度取得部１３２は、足底荷重値の変化量に基づき目標加速度ａを算出するものであるが、目標加速度ａを取得するものであれば、目標加速度ａの取得方法は、これに限られず任意である。なお、パワーアシストスーツ１０の制御において、厳密な定位性は不要である。装着者の体格等に応じて到達させたい位置目標は異なるためである。一方、バネとして制御を定義すると位置拘束となり、パワーアシストスーツ１０を装着した装着者の意図と異なる動作に対して剛性を持った動きになる可能性がある。そこで、第２制御部１Ｂｃは、目標加速度ａに基づき駆動トルクτ１を算出するため、力の入力に対してパワーアシストスーツ１０の足先の絶対的な位置が決まらないように、非定位性の制御を実現している。すなわち、第２制御部１Ｂｃは、位置制御ループを有していない。   The target acceleration acquisition unit 132 calculates the target acceleration a based on the amount of change in the sole load value. However, as long as the target acceleration a is acquired, the method of acquiring the target acceleration a is not limited to this. Optional. In the control of the power assist suit 10, strict localization is not required. This is because the position target to be reached differs depending on the physique of the wearer. On the other hand, when the control is defined as a spring, the position is restricted, and there is a possibility that the movement with rigidity is performed for an operation different from the intention of the wearer wearing the power assist suit 10. Therefore, the second control unit 1Bc calculates the driving torque τ1 based on the target acceleration “a”, so that the absolute position of the toe of the power assist suit 10 is not determined with respect to the input of the force. Control is realized. That is, the second control unit 1Bc does not have a position control loop.

図１２に示す仮駆動トルク値算出部１３４は、駆動トルク値τ１の仮の値である仮駆動トルク値τ１Ａを、目標加速度ａに基づき算出する。具体的には、仮駆動トルク値算出部１３４は、式（１３）のｘ”を目標加速度に置き換え、次の式（１５）に基づき仮駆動トルク値τ１Ａを算出する。   The temporary drive torque value calculation unit 134 shown in FIG. 12 calculates a temporary drive torque value τ1A, which is a temporary value of the drive torque value τ1, based on the target acceleration a. Specifically, the provisional drive torque value calculation unit 134 replaces x ″ in Expression (13) with the target acceleration, and calculates the provisional drive torque value τ1A based on the following Expression (15).

τ１Ａ＝Ｍ・Ｊ＃（ａ−Ｊ’・θ’） ・・・（１５） τ1A = M · J # (a−J ′ · θ ′) (15)

仮駆動トルク値算出部１３４は、特異点角度、すなわちＪ＃が発散する場合は、前回の関節角度θに基づき算出したヤコビ行列Ｊを用いて、τ１Ａを算出する。ただし、仮駆動トルク値算出部１３４は、特異点角度でない場合は、特異点角度の近傍であっても、現在の関節角度θに基づいたＪ＃を用いて、τ１Ａを算出する。 When the singular point angle, ie, J # , diverges, the temporary drive torque value calculation unit 134 calculates τ1A using the Jacobian matrix J calculated based on the previous joint angle θ. However, when the tentative drive torque value is not a singular point angle, the tentative drive torque value calculating unit 134 calculates τ1A using J # based on the current joint angle θ even in the vicinity of the singular point angle.

なお、パワーアシストスーツ１０は、複数の関節（大腿駆動機構２３、下腿駆動機構４５、回動軸４７）のそれぞれにアクチュエータを備えている。仮駆動トルク値算出部１３４は、各関節の関節角度θに基づき、それぞれのアクチュエータに対する仮駆動トルク値τ１Ａを算出する。   The power assist suit 10 includes an actuator for each of a plurality of joints (thigh drive mechanism 23, lower leg drive mechanism 45, and rotating shaft 47). The temporary drive torque value calculation unit 134 calculates a temporary drive torque value τ1A for each actuator based on the joint angle θ of each joint.

図１２に示す上限駆動トルク値取得部１３６は、保持トルク値取得部１５０及び上限駆動トルク値算出部１５２を有する。上限駆動トルク値取得部１３６は、上限トルク値τ０ｍａｘに基づき、駆動トルク値τ１の上限のトルク値である上限駆動トルク値τ１ｍａｘを取得する。上限トルク値τ０ｍａｘは、パワーアシストスーツ１０の仕様などによって予め定められた、各アクチュエータが出力可能な総トルク値τ０の上限の値である。 12 includes a holding torque value acquiring section 150 and an upper limit driving torque value calculating section 152. Upper driving torque value acquisition unit 136, based on the upper limit torque value .tau.0 max, acquires the upper limit driving torque value .tau.1 max is the upper limit torque value of the driving torque value .tau.1. The upper limit torque value τ0 max is an upper limit value of the total torque value τ0 that can be output by each actuator, which is determined in advance according to the specifications of the power assist suit 10 and the like.

保持トルク値取得部１５０は、第１制御部１Ａｃが算出した、現在の関節角度θにおける保持トルク値τ２を取得する。上限駆動トルク値算出部１５２は、上限トルク値τ０ｍａｘと、保持トルク値τ２とを取得する。上限駆動トルク値算出部１５２は、上限トルク値τ０ｍａｘから保持トルク値τ２を差し引いた値に基づき、上限駆動トルク値τ１ｍａｘを算出する。より具体的には、上限駆動トルク値算出部１５２は、仮駆動トルク値τ１Ａが保持トルク値τ２と同符号、すなわち互いに同じ回転方向へのトルクである場合、上限トルク値τ０ｍａｘから保持トルク値τ２を差し引いた値を、上限駆動トルク値τ１ｍａｘとする。一方、上限駆動トルク値算出部１５２は、仮駆動トルク値τ１Ａが保持トルク値τ２と異符号、すなわち互いに逆回転方向のトルクである場合、上限トルク値τ０ｍａｘに保持トルク値τ２を加えた値を、上限駆動トルク値τ１ｍａｘとする。 The holding torque value acquisition unit 150 acquires the holding torque value τ2 at the current joint angle θ calculated by the first control unit 1Ac. The upper limit driving torque value calculator 152 acquires the upper limit torque value τ0 max and the holding torque value τ2. Upper driving torque value calculation unit 152, based on the value obtained by subtracting the holding torque τ2 from the upper limit torque value .tau.0 max, it calculates the upper limit driving torque value .tau.1 max. More specifically, if the provisional driving torque value τ1A has the same sign as the holding torque value τ2, that is, the torque in the same rotation direction as the holding torque value τ2, the upper limit driving torque value calculation unit 152 calculates the holding torque value from the upper limit torque value τ0 max. the value obtained by subtracting the .tau.2, an upper limit driving torque value .tau.1 max. On the other hand, when the provisional driving torque value τ1A has a different sign from the holding torque value τ2, that is, when the provisional driving torque value τ1A is a torque in a direction opposite to each other, the upper limit driving torque value τ1A is obtained by adding the holding torque value τ2 to the upper limit torque value τ0 max. Is the upper limit drive torque value τ1 max .

このように、仮駆動トルク値τ１Ａが保持トルク値τ２と同じ回転方向へのトルクである場合は、上限トルク値τ０ｍａｘのうちで駆動トルク値τ１に割けるトルク値が、保持トルク値τ２の分だけ減少している。ただし、仮駆動トルク値τ１Ａが保持トルク値τ２と逆回転方向へのトルクである場合は、上限トルク値τ０ｍａｘのうちで駆動トルク値τ１に割けるトルク値を、逆方向に回そうとする保持トルク値τ２の分だけ増加させることができる。 As described above, when the provisional driving torque value τ1A is a torque in the same rotation direction as the holding torque value τ2, the torque value that can be divided into the driving torque value τ1 of the upper limit torque value τ0 max is equal to the holding torque value τ2. It has decreased by a minute. However, when the provisional driving torque value τ1A is a torque in the reverse rotation direction to the holding torque value τ2, the torque value that can be divided by the driving torque value τ1 in the upper limit torque value τ0 max is turned in the reverse direction. It can be increased by the holding torque value τ2.

上限トルク値τ０ｍａｘは、アクチュエータ毎に値が定められている。また、第１制御部１Ａｃは、各アクチュエータのそれぞれに対し、保持トルク値τ２を算出する。保持トルク値取得部１５０は、これらアクチュエータ毎の上限トルク値τ０ｍａｘ及び保持トルク値τ２に基づき、それぞれのアクチュエータに対する上限駆動トルク値τ１ｍａｘを算出する。なお、保持トルク値τ２が大きく、上限駆動トルク値τ１ｍａｘが０以下の値となった場合、駆動トルク値τ１に割ける余裕がないため、第２制御部１Ｂｃは、駆動トルク値τ１を０とする。 The upper limit torque value τ0 max is determined for each actuator. Further, the first control unit 1Ac calculates a holding torque value τ2 for each of the actuators. The holding torque value acquiring unit 150 calculates the upper limit driving torque value τ1 max for each actuator based on the upper limit torque value τ0 max and the holding torque value τ2 of each actuator. If the holding torque value τ2 is large and the upper limit driving torque value τ1 max becomes a value equal to or less than 0, there is no room to divide the driving torque value τ1. Therefore, the second control unit 1Bc sets the driving torque value τ1 to 0. And

図１２に示すトルク比率算出部１３８は、仮駆動トルク値τ１Ａと上限駆動トルク値τ１ｍａｘとの比率であるトルク比率αを算出する。具体的には、トルク比率算出部１３８は、次の式（１６）に示すように、上限駆動トルク値τ１ｍａｘに対する仮駆動トルク値τ１Ａの比率をトルク比率αとする。 Torque ratio calculation section shown in FIG. 12 138, calculates the torque ratio α is a ratio of the provisional driving torque value τ1A and the upper limit driving torque value .tau.1 max. Specifically, the torque ratio calculation unit 138, as shown in the following equation (16), the ratio of the provisional driving torque value τ1A for upper drive torque value .tau.1 max torque ratio alpha.

α＝τ１Ａ／τ１ｍａｘ ・・・（１６） α = τ1A / τ1 max ··· ( 16)

トルク比率算出部１３８は、それぞれのアクチュエータに対するトルク比率αを算出する。なお、トルク比率αは、仮駆動トルク値τ１Ａと上限駆動トルク値τ１ｍａｘとの比率であれば、上述の式（１６）によるものに限られない。例えば、トルク比率αは、仮駆動トルク値τ１Ａに対する上限駆動トルク値τ１ｍａｘの比率、すなわちα＝τ１ｍａｘ／τ１Ａであってもよい。 The torque ratio calculator 138 calculates a torque ratio α for each actuator. Note that the torque ratio alpha, if the ratio of the provisional driving torque value τ1A and the upper limit driving torque value .tau.1 max, not limited to by the above equation (16). For example, the torque ratio α may be a ratio of the upper limit drive torque value τ1 max to the temporary drive torque value τ1A, that is, α = τ1 max / τ1A.

図１２に示す補正項算出部１４０は、目標加速度ａから算出される行列式を、予め定められたリンクの加速度の上限値の行列式で正規化して、補正項Ｓを算出する。ここで、予め定められたリンクの加速度の上限値の行列式を、加速度上限項Ｔｍとする。加速度上限項Ｔｍは、リンクの加速度の上限値が予め定められた場合、それに基づき算出される対角行列である。また、式（１５）の加速度に関連する行列式である（ａ−Ｊ’・θ’）を、加速度項βとする。補正項算出部１４０は、次の式（１７）に示すように、加速度項βを加速度上限項Ｔｍで正規化して、正規化加速度項β’を算出し、正規化加速度項β’を、大きさ（スカラー）である補正項Ｓと、単位ベクトルＵに分離して、補正項Ｓを算出する。   The correction term calculation unit 140 shown in FIG. 12 calculates the correction term S by normalizing the determinant calculated from the target acceleration a with the determinant of the upper limit of the link acceleration. Here, the determinant of the predetermined upper limit of the acceleration of the link is defined as an acceleration upper limit term Tm. The acceleration upper limit term Tm is a diagonal matrix calculated based on the upper limit of the link acceleration when the upper limit is predetermined. Also, let (a−J ′ · θ ′), which is the determinant related to the acceleration in Expression (15), be the acceleration term β. The correction term calculation unit 140 calculates the normalized acceleration term β ′ by normalizing the acceleration term β with the acceleration upper limit term Tm, and calculates the normalized acceleration term β ′ as The correction term S, which is a scalar, and the unit vector U are separated and the correction term S is calculated.

β’＝Ｔｍ−１・β＝Ｓ・Ｕ ・・・（１７） β ′ = Tm −1 · β = S · U (17)

補正項Ｓは目標加速度ａに応じて値が変化するため、補正項算出部１４０は、目標加速度ａが算出される毎に、補正項Ｓを算出する。補正項Ｓは、加速度項βを加速度上限項Ｔｍで正規化して算出された補正項であるため、予め定められたリンクの加速度の上限値に対する、目標加速度の比率であるということができる。なお、補正項Ｓは、全てのアクチュエータに共通する値である。   Since the value of the correction term S changes according to the target acceleration a, the correction term calculation unit 140 calculates the correction term S every time the target acceleration a is calculated. Since the correction term S is a correction term calculated by normalizing the acceleration term β with the acceleration upper limit term Tm, it can be said that the correction term S is a ratio of the target acceleration to a predetermined upper limit of the link acceleration. The correction term S is a value common to all actuators.

図１２に示す駆動トルク値決定部１４２は、仮駆動トルク値τ１Ａが上限駆動トルク値τ１ｍａｘ以下である場合、すなわちトルク比率αが１以下である場合に、仮駆動トルク値τ１Ａを、駆動トルク値τ１に決定する。また、駆動トルク値決定部１４２は、仮駆動トルク値τ１Ａが上限駆動トルク値τ１ｍａｘより大きい場合、すなわちトルク比率αが１より大きい場合に、トルク比率αに基づき、仮駆動トルク値τ１Ａを、上限駆動トルク値τ１ｍａｘ以下の値である変換駆動トルク値τ１Ｂに変換する。駆動トルク値決定部１４２は、この変換駆動トルク値τ１Ｂを、駆動トルク値τ１に決定する。すなわち、駆動トルク値決定部１４２は、仮駆動トルク値τ１Ａが上限駆動トルク値τ１ｍａｘを超過する場合、変換駆動トルク値τ１Ｂを用いて駆動トルク値τ１を上限駆動トルク値τ１ｍａｘ以下に低下させる。 When the provisional drive torque value τ1A is equal to or less than the upper limit drive torque value τ1 max , that is, when the torque ratio α is equal to or less than 1, the drive torque value determination unit 142 illustrated in FIG. The value τ1 is determined. Further, when the provisional drive torque value τ1A is larger than the upper limit drive torque value τ1 max , that is, when the torque ratio α is larger than 1, the drive torque value determination unit 142 determines the provisional drive torque value τ1A based on the torque ratio α. into a less than or equal to the rising drive torque value .tau.1 max conversion driving torque value Tau1B. The drive torque value determination unit 142 determines the converted drive torque value τ1B as the drive torque value τ1. In other words, the driving torque value determining unit 142, if the tentative drive torque value τ1A exceeds the upper limit driving torque value .tau.1 max, reduced to less than the upper limit driving torque value .tau.1 max driving torque value .tau.1 using the conversion driving torque value τ1B .

さらに詳しくは、駆動トルク値決定部１４２は、全てのアクチュエータのトルク比率αのうち、最大のトルク比率αである最大トルク比率αｍａｘを選択する。駆動トルク値決定部１４２は、最大トルク比率αｍａｘが１以下である場合、全てのアクチュエータの仮駆動トルク値τ１Ａを、それぞれのアクチュエータの駆動トルク値τ１に決定する。 More specifically, the drive torque value determination unit 142 selects the maximum torque ratio α max that is the maximum torque ratio α among the torque ratios α of all actuators. When the maximum torque ratio α max is 1 or less, the drive torque value determination unit 142 determines the provisional drive torque values τ1A of all the actuators as the drive torque values τ1 of the respective actuators.

駆動トルク値決定部１４２は、最大トルク比率αｍａｘが１より大きい場合、最大トルク比率αｍａｘを用いて、全てのアクチュエータに対する仮駆動トルク値τ１Ａを、それぞれの仮駆動トルク値τ１Ａに対応する変換駆動トルク値τ１Ｂに変換する。変換駆動トルク値τ１Ｂについてより詳細に説明する。駆動トルク値決定部１４２は、最大トルク比率αｍａｘが１より大きい場合に、次の式（１８）に示すように、仮駆動トルク値τ１Ａを最大トルク比率αｍａｘで除して、変換駆動トルク値τ１Ｂを算出する。 When the maximum torque ratio α max is greater than 1, the drive torque value determination unit 142 converts the temporary drive torque values τ1A for all the actuators using the maximum torque ratio α max corresponding to the respective temporary drive torque values τ1A. It is converted into a driving torque value τ1B. The conversion drive torque value τ1B will be described in more detail. When the maximum torque ratio α max is greater than 1, the drive torque value determination unit 142 divides the temporary drive torque value τ1A by the maximum torque ratio α max as shown in the following equation (18), and The value τ1B is calculated.

τ１Ｂ＝τ１Ａ／αｍａｘ ・・・（１８） τ1B = τ1A / α max (18)

駆動トルク値決定部１４２は、各アクチュエータに対するそれぞれの仮駆動トルク値τ１Ａを、共通する最大トルク比率αｍａｘで除する。駆動トルク値決定部１４２は、このように変換駆動トルク値τ１Ｂを算出するため、全てのアクチュエータに対する変換駆動トルク値τ１Ｂは、上限駆動トルク値τ１ｍａｘ以下の値となる。また、全てのアクチュエータにおいて、それぞれの仮駆動トルク値τ１Ａに対し、同じ最大トルク比率αｍａｘを用いて変換駆動トルク値τ１Ｂを算出しているため、駆動トルク値τ１を低下させても、各アクチュエータ間の駆動トルク値τ１の比率が保たれる。 The drive torque value determination unit 142 divides each temporary drive torque value τ1A for each actuator by a common maximum torque ratio α max . The drive torque value determination unit 142 calculates the converted drive torque value τ1B in this manner, so that the converted drive torque values τ1B for all actuators are equal to or less than the upper limit drive torque value τ1 max . Further, in all the actuators, since the conversion drive torque value τ1B is calculated using the same maximum torque ratio α max for each of the provisional drive torque values τ1A, even if the drive torque value τ1 is reduced, The ratio of the driving torque value τ1 is maintained.

さらに、駆動トルク値決定部１４２は、最大トルク比率αｍａｘが１より大きい場合、変換駆動トルク値τ１Ｂを補正項Ｓで補正した値を、駆動トルク値τ１として決定する。具体的には、駆動トルク値決定部１４２は、次の式（１９）に示すように、変換駆動トルク値τ１Ｂを補正項Ｓで乗じた値を、駆動トルク値τ１として決定する。 Further, when the maximum torque ratio α max is greater than 1, the drive torque value determination unit 142 determines a value obtained by correcting the converted drive torque value τ1B with the correction term S as the drive torque value τ1. Specifically, the driving torque value determination unit 142 determines a value obtained by multiplying the converted driving torque value τ1B by the correction term S as the driving torque value τ1, as shown in the following equation (19).

τ１＝τ１Ｂ・Ｓ ・・・（１９）   τ1 = τ1B · S (19)

ただし、補正項Ｓが１より大きい場合、駆動トルク値決定部１４２は、変換駆動トルク値τ１Ｂに補正項Ｓを乗じることなく、変換駆動トルク値τ１Ｂを、そのまま駆動トルク値τ１として決定する。変換駆動トルク値τ１Ｂは、最大トルク比率αｍａｘとなったアクチュエータに関する駆動トルク値τ１を、上限駆動トルク値τ１ｍａｘまで最大限に出力するものである。駆動トルク値決定部１４２は、変換駆動トルク値τ１Ｂを、補正項Ｓで補正することで、前回からの加速度軌道の連続性を保ち、加速度軌道変化に対応した駆動トルク値τ１を算出することができる。言い換えれば、補正項Ｓは、変換駆動トルク値τ１Ｂに基づいて算出された駆動トルクτ１によってアクチュエータを駆動した場合に、リンクの加速度が目標加速度ａより大きくならないように、変換駆動トルク値τ１Ｂを補正するための係数であるということができる。 However, when the correction term S is greater than 1, the drive torque value determination unit 142 determines the converted drive torque value τ1B as the drive torque value τ1 without multiplying the converted drive torque value τ1B by the correction term S. The converted drive torque value τ1B is to output the drive torque value τ1 relating to the actuator having the maximum torque ratio α max to the maximum drive torque value τ1 max . The drive torque value determination unit 142 can correct the converted drive torque value τ1B with the correction term S to maintain the continuity of the acceleration trajectory from the previous time and calculate the drive torque value τ1 corresponding to the change in the acceleration trajectory. it can. In other words, the correction term S corrects the converted drive torque value τ1B so that the link acceleration does not become larger than the target acceleration a when the actuator is driven by the drive torque τ1 calculated based on the converted drive torque value τ1B. It can be said that it is a coefficient to perform.

駆動トルク値決定部１４２は、決定した駆動トルク値τ１の情報をアクチュエータ制御部１Ｃｃに伝達する。アクチュエータ制御部１Ｃｃは、駆動トルク値決定部１４２から取得した駆動トルク値τ１と、第１制御部１Ａｃから取得した保持トルク値τ２の合計値を、総トルク値τ０として算出し、その総トルク値τ０で、アクチュエータを駆動する。   The drive torque value determination unit 142 transmits information on the determined drive torque value τ1 to the actuator control unit 1Cc. The actuator control unit 1Cc calculates the total value of the driving torque value τ1 obtained from the driving torque value determination unit 142 and the holding torque value τ2 obtained from the first control unit 1Ac as a total torque value τ0, and calculates the total torque value At τ0, the actuator is driven.

なお、アクチュエータが１つの場合、駆動トルク値決定部１４２は、最大トルク比率αｍａｘを用いず、式（１８）の最大トルク比率αｍａｘをそのアクチュエータに対するトルク比率αとして、同様に駆動トルク値τ１を決定する。また、トルク比率αが、α＝τ１ｍａｘ／τ１Ａである場合、駆動トルク値決定部１４２は、仮駆動トルク値τ１Ａをトルク比率αで乗じて、変換駆動トルク値τ１Ｂを算出する。 In the case the actuator is one, the driving torque value determining section 142 does not use the maximum torque ratio alpha max, the maximum torque ratio alpha max as the torque ratio alpha for the actuator of the formula (18), likewise driven torque value τ1 To determine. When the torque ratio α is α = τ1 max / τ1A, the drive torque value determination unit 142 calculates the converted drive torque value τ1B by multiplying the temporary drive torque value τ1A by the torque ratio α.

次に、フローチャートを用いて、駆動トルク値τ１の算出フローを説明する。図１３は、第５実施形態に係る駆動トルク値の算出フローを説明するフローチャートである。図１３に示すように、第２制御部１Ｂｃは、仮駆動トルク値算出部１３４により、目標加速度ａに基づき仮駆動トルク値τ１Ａを算出し（ステップＳ３０）、上限駆動トルク値取得部１３６により、上限駆動トルク値τ１ｍａｘを算出する（ステップＳ３２）。仮駆動トルク値算出部１３４は、上述の式（１５）に基づき、各アクチュエータに対する仮駆動トルク値τ１Ａを算出する。また、上限駆動トルク値取得部１３６は、上限トルク値τ０ｍａｘから保持トルク値τ２を差し引いた値に基づき、上限駆動トルク値τ１ｍａｘを算出する。また、上限駆動トルク値τ１ｍａｘが０以下の値となった場合、第２制御部１Ｂｃは、駆動トルク値τ１を０として、後述するステップＳ４６に移動する。なお、ステップＳ３０とステップＳ３２とは、後述するステップＳ３４より前に行われるものであればよく、互いの処理順は任意である。 Next, a flow of calculating the drive torque value τ1 will be described using a flowchart. FIG. 13 is a flowchart illustrating a drive torque value calculation flow according to the fifth embodiment. As shown in FIG. 13, the second control unit 1Bc calculates the provisional drive torque value τ1A based on the target acceleration a by the provisional drive torque value calculation unit 134 (step S30). calculating the upper limit driving torque value .tau.1 max (step S32). The temporary drive torque value calculation unit 134 calculates a temporary drive torque value τ1A for each actuator based on the above equation (15). The upper limit driving torque value acquisition unit 136, based on the value obtained by subtracting the holding torque τ2 from the upper limit torque value .tau.0 max, calculates the upper limit driving torque value .tau.1 max. When the upper limit driving torque value τ1 max becomes equal to or less than 0, the second control unit 1Bc sets the driving torque value τ1 to 0 and moves to step S46 described later. Note that step S30 and step S32 may be performed before step S34 described later, and the order of processing with each other is arbitrary.

仮駆動トルク値τ１Ａ及び上限駆動トルク値τ１ｍａｘを算出した後、第２制御部１Ｂｃは、トルク比率算出部１３８により、仮駆動トルク値τ１Ａ及び上限駆動トルク値τ１ｍａｘから、トルク比率αを算出する（ステップＳ３４）。トルク比率算出部１３８は、上述の式（１６）に基づきトルク比率αを算出する。 After calculating the provisional driving torque value τ1A and upper drive torque value .tau.1 max, calculating the second control unit 1Bc is by the torque ratio calculation unit 138, from the temporary driving torque value τ1A and upper drive torque value .tau.1 max, the torque ratio α (Step S34). The torque ratio calculation unit 138 calculates the torque ratio α based on the above equation (16).

トルク比率αを算出した後、第２制御部１Ｂｃは、駆動トルク値決定部１４２により、最大トルク比率αｍａｘが１より大きいかを判定する(ステップＳ３６)。最大トルク比率αｍａｘとは、全てのアクチュエータのトルク比率αのうち、最大のトルク比率αである。最大トルク比率αｍａｘが１より大きくない、すなわち最大トルク比率αｍａｘが１以下である場合（ステップＳ３６；Ｎｏ）、駆動トルク値決定部１４２は、全てのアクチュエータの仮駆動トルク値τ１Ａを、それぞれのアクチュエータの駆動トルク値τ１に決定し(ステップＳ３８)、後述するステップＳ４６に移動する。 After calculating the torque ratio α, the second control unit 1Bc causes the drive torque value determination unit 142 to determine whether the maximum torque ratio α max is greater than 1 (step S36). The maximum torque ratio α max is the maximum torque ratio α among the torque ratios α of all actuators. When the maximum torque ratio α max is not larger than 1, that is, when the maximum torque ratio α max is 1 or less (Step S36; No), the drive torque value determination unit 142 determines the temporary drive torque values τ1A of all the actuators, respectively. (Step S38), and the process moves to Step S46 described later.

最大トルク比率αｍａｘが１より大きい場合（ステップＳ３６；Ｙｅｓ）、駆動トルク値決定部１４２は、それぞれのアクチュエータの仮駆動トルク値τ１Ａと、共通する最大トルク比率αｍａｘとから、それぞれのアクチュエータに対する変換駆動トルク値τ１Ｂを算出する（ステップＳ４０）。駆動トルク値決定部１４２は、上述の式（１８）に基づき、それぞれのアクチュエータに対する変換駆動トルク値τ１Ｂを算出する。 When the maximum torque ratio α max is greater than 1 (Step S36; Yes), the drive torque value determination unit 142 determines the temporary drive torque value τ1A of each actuator and the common maximum torque ratio α max for each actuator. The conversion drive torque value τ1B is calculated (step S40). The drive torque value determination unit 142 calculates a converted drive torque value τ1B for each actuator based on the above equation (18).

第２制御部１Ｂｃは、補正項算出部１４０により、目標加速度ａから算出される行列式を、加速度の上限値の行列式で正規化して、補正項Ｓを算出する（ステップＳ４２）。補正項算出部１４０は、上述の式（１７）に基づき、補正項Ｓを算出する。ステップＳ４２は、後述するステップＳ４４の前に実行されれば、その実行順は任意である。   The second control unit 1Bc calculates a correction term S by normalizing the determinant calculated from the target acceleration a by the determinant of the upper limit value of the acceleration by the correction term calculation unit 140 (step S42). The correction term calculation unit 140 calculates the correction term S based on the above equation (17). The order of execution of step S42 is arbitrary as long as it is executed before step S44 described later.

変換駆動トルク値τ１Ｂ、及び補正項Ｓを算出した後、第２制御部１Ｂｃは、駆動トルク値決定部１４２により、変換駆動トルク値τ１Ｂを補正項Ｓで補正した値を、駆動トルク値τ１として決定する（ステップＳ４４）。駆動トルク値決定部１４２は、上述の式（１８）を用いて、駆動トルク値τ１を算出する。ステップＳ３８又はステップＳ４４で駆動トルク値τ１を決定したら、駆動トルク値決定部１４２は、その駆動トルク値τ１をアクチュエータ制御部１Ｃｃに伝達する。アクチュエータ制御部１Ｃｃは、駆動トルク値決定部１４２から取得した駆動トルク値τ１と、第１制御部１Ａｃから取得した保持トルク値τ２の合計値を、総トルク値τ０として算出し、その総トルク値τ０で、アクチュエータを駆動する。   After calculating the conversion drive torque value τ1B and the correction term S, the second control unit 1Bc uses the drive torque value determination unit 142 to correct the conversion drive torque value τ1B with the correction term S as the drive torque value τ1. It is determined (step S44). The drive torque value determination unit 142 calculates the drive torque value τ1 using the above equation (18). After determining the drive torque value τ1 in step S38 or S44, the drive torque value determination unit 142 transmits the drive torque value τ1 to the actuator control unit 1Cc. The actuator control unit 1Cc calculates the total value of the driving torque value τ1 obtained from the driving torque value determination unit 142 and the holding torque value τ2 obtained from the first control unit 1Ac as a total torque value τ0, and calculates the total torque value At τ0, the actuator is driven.

ステップＳ４４の後は、ステップＳ４６に移動し、処理が終了していない場合（ステップＳ４６；Ｎｏ）、ステップＳ３０に戻り、次のタイミングで取得された目標加速度ａに基づき仮駆動トルク値τ１Ａを算出し、その後同様の処理を繰り返す。処理が終了した場合（ステップＳ４６；Ｙｅｓ）、本処理を終了する。   After step S44, the process moves to step S46. If the process is not completed (step S46; No), the process returns to step S30 to calculate the temporary drive torque value τ1A based on the target acceleration a obtained at the next timing. Then, the same processing is repeated. When the processing is completed (Step S46; Yes), the present processing is terminated.

以上説明したように、本実施形態に係る制御装置１ｃは、関節を介して連結された複数のリンクをアクチュエータにより駆動する作動機構（本実施形態ではパワーアシストスーツ１０）の制御装置である。制御装置１ｃは、目標加速度取得部１３２と、仮駆動トルク値算出部１３４と、上限駆動トルク値取得部１３６と、トルク比率算出部１３８と、駆動トルク値決定部１４２と、アクチュエータ制御部１Ｃｃと、を有する。目標加速度取得部１３２は、リンクの目標とする移動加速度である目標加速度ａを取得する。仮駆動トルク値算出部１３４は、リンクを目標加速度ａで移動させるためのトルクである駆動トルク値τ１の仮の値である仮駆動トルク値τ１Ａを、目標加速度ａに基づき算出する。上限駆動トルク値取得部１３６は、上限トルク値τ０ｍａｘに基づき、駆動トルク値τ１の上限のトルク値である上限駆動トルク値τ１ｍａｘを取得する。トルク比率算出部１３８は、仮駆動トルク値τ１Ａと上限駆動トルク値τ１ｍａｘとの比率であるトルク比率αを算出する。駆動トルク値決定部１４２は、仮駆動トルク値τ１Ａが上限駆動トルク値τ１ｍａｘ以下である場合に、仮駆動トルク値τ１Ａを駆動トルク値τ１に決定する。また、駆動トルク値決定部１４２は、仮駆動トルク値τ１Ａが上限駆動トルク値τ１ｍａｘより大きい場合に、トルク比率αに基づき、仮駆動トルク値τ１Ａを上限駆動トルク値τ１ｍａｘ以下の値に変換した変換駆動トルク値τ１Ｂを、駆動トルク値τ１に決定する。アクチュエータ制御部１Ｃｃは、アクチュエータを駆動するトルク値（総トルク値τ０）として、決定した駆動トルク値τ１を用いて、アクチュエータを制御する。 As described above, the control device 1c according to the present embodiment is a control device for an operating mechanism (the power assist suit 10 in the present embodiment) that drives a plurality of links connected via a joint by an actuator. The control device 1c includes a target acceleration acquisition unit 132, a provisional drive torque value calculation unit 134, an upper limit drive torque value acquisition unit 136, a torque ratio calculation unit 138, a drive torque value determination unit 142, an actuator control unit 1Cc. And The target acceleration acquisition unit 132 acquires a target acceleration a that is a target movement acceleration of the link. The temporary drive torque value calculation unit 134 calculates a temporary drive torque value τ1A that is a temporary value of the drive torque value τ1 that is a torque for moving the link at the target acceleration a based on the target acceleration a. Upper driving torque value acquisition unit 136, based on the upper limit torque value .tau.0 max, acquires the upper limit driving torque value .tau.1 max is the upper limit torque value of the driving torque value .tau.1. Torque ratio calculation unit 138 calculates the torque ratio α is a ratio of the provisional driving torque value τ1A and the upper limit driving torque value .tau.1 max. When the provisional drive torque value τ1A is equal to or smaller than the upper limit drive torque value τ1 max , the drive torque value determination unit 142 determines the provisional drive torque value τ1A as the drive torque value τ1. Further, driving torque value determining unit 142, when the provisional driving torque value τ1A is greater than the upper limit driving torque value .tau.1 max, based on the torque ratio alpha, the temporary driving torque value τ1A the following values upper drive torque value .tau.1 max conversion The converted drive torque value τ1B thus determined is determined as the drive torque value τ1. The actuator control unit 1Cc controls the actuator using the determined drive torque value τ1 as the torque value for driving the actuator (total torque value τ0).

この制御装置１ｃは、仮駆動トルク値τ１Ａが上限駆動トルク値τ１ｍａｘより大きい場合に、仮駆動トルク値τ１Ａを上限駆動トルク値τ１ｍａｘ以下の値に変換した変換駆動トルク値τ１Ｂを、駆動トルク値τ１に決定する。すなわち、制御装置１ｃは、目標とする駆動トルク値τ１である仮駆動トルク値τ１Ａが、許される上限駆動トルク値τ１ｍａｘを超過する場合、変換駆動トルク値τ１Ｂを用いて駆動トルク値τ１を上限駆動トルク値τ１ｍａｘ以下に低下させる。従って、この制御装置１ｃは、特異点角度や特異点角度の近傍において、仮駆動トルク値τ１Ａが過大になった場合にも、駆動トルク値τ１を上限駆動トルク値τ１ｍａｘ以下に低下させることで、駆動トルク値τ１の出力異常を容易に抑制することができる。また、この制御装置１ｃは、目標加速度ａに基づき駆動トルク値τ１を算出しているため、非定位性の制御となり、自由な動作に対応することが可能となる。 The control unit 1c, when the provisional driving torque value τ1A is greater than the upper limit driving torque value .tau.1 max, the conversion driving torque value τ1B converting the provisional driving torque value τ1A the following values upper drive torque value .tau.1 max, drive torque The value τ1 is determined. That is, the control device 1c, the temporary driving torque value τ1A which is a driving torque value .tau.1 having a target, if it exceeds the upper limit driving torque value .tau.1 max allowed upper limit driving torque value .tau.1 using the conversion driving torque value τ1B The driving torque value is reduced to τ1 max or less. Therefore, the control device 1c in the vicinity of the singular point angle and singular point angle, even when the provisional driving torque value τ1A becomes excessive, the driving torque value .tau.1 be to drop below the upper limit driving torque value .tau.1 max In addition, abnormal output of the drive torque value τ1 can be easily suppressed. Further, since the control device 1c calculates the drive torque value τ1 based on the target acceleration “a”, the control device 1c performs non-localization control and can cope with a free operation.

作動機構（パワーアシストスーツ１０）は、複数のリンクのそれぞれに対応する複数のアクチュエータを備えており、駆動トルク値τ１は、複数のアクチュエータのそれぞれに対して算出されるものである。そして、駆動トルク値決定部１４２は、複数のアクチュエータに対するそれぞれの仮駆動トルク値τ１Ａのうち、上限駆動トルク値τ１ｍａｘに対する仮駆動トルク値τ１Ａの超過率が最も高い仮駆動トルク値τ１Ａについてのトルク比率である最大トルク比率αｍａｘを用いて、それぞれの仮駆動トルク値τ１Ａを変換駆動トルク値τ１Ｂに変換し、それぞれの変換駆動トルク値τ１Ｂを、それぞれのアクチュエータに対する駆動トルク値τ１に決定する。この制御装置１ｃは、最大トルク比率αｍａｘを用いて全ての仮駆動トルク値τ１Ａを変換駆動トルク値τ１Ｂに変換している。従って、全てのアクチュエータに対する駆動トルク値τ１を上限駆動トルク値τ１ｍａｘ以下の値としつつ、各アクチュエータ間の駆動トルク値τ１の比率が、変換前と同様に保たれる。従って、この制御装置１ｃは、駆動トルク値τ１の出力異常をより適切に抑制することができる。 The operating mechanism (power assist suit 10) includes a plurality of actuators corresponding to each of the plurality of links, and the drive torque value τ1 is calculated for each of the plurality of actuators. Then, the driving torque value determining unit 142, among the respective temporary driving torque value τ1A for a plurality of actuators, the torque of the over rate of the provisional driving torque value τ1A for upper drive torque value .tau.1 max highest provisional driving torque value τ1A Using the maximum torque ratio α max as a ratio, each temporary drive torque value τ1A is converted into a converted drive torque value τ1B, and each converted drive torque value τ1B is determined as a drive torque value τ1 for each actuator. The control device 1c converts all the provisional drive torque values τ1A into converted drive torque values τ1B using the maximum torque ratio α max . Thus, while the driving torque value .tau.1 for all actuators with the following values upper drive torque value .tau.1 max, the ratio of the driving torque value .tau.1 between the actuators is kept as before conversion. Therefore, the control device 1c can more appropriately suppress the output abnormality of the drive torque value τ1.

この駆動トルク値決定部１４２は、上限駆動トルク値τ１ｍａｘに対する仮駆動トルク値τ１Ａの比率をトルク比率αとし、仮駆動トルク値τ１Ａをトルク比率αで除することにより、変換駆動トルク値τ１Ｂを算出する。この駆動トルク値決定部１４２は、このようにトルク比率α及び変換駆動トルク値τ１Ｂを算出しているため、駆動トルク値τ１を上限駆動トルク値τ１ｍａｘ以下の適切な値とすることができる。 The driving torque value determining unit 142, a ratio of the provisional driving torque value τ1A for upper drive torque value .tau.1 max and torque ratio alpha, by dividing the provisional driving torque value τ1A torque ratio alpha, the conversion driving torque value τ1B calculate. Since the drive torque value determination unit 142 calculates the torque ratio α and the converted drive torque value τ1B in this manner, the drive torque value τ1 can be set to an appropriate value equal to or less than the upper limit drive torque value τ1 max .

上限駆動トルク値取得部１３６は、作動機構（パワーアシストスーツ１０）の自重、及び作動機構（パワーアシストスーツ１０）にかかる負荷を保持するためのトルクである保持トルク値τ２を取得する保持トルク値取得部１５０と、上限トルク値τ０ｍａｘから保持トルク値τ２を差し引いた値に基づき、上限駆動トルク値τ１ｍａｘを算出する上限駆動トルク値算出部１５２と、を有する。この上限駆動トルク値取得部１３６は、上限トルク値τ０ｍａｘから保持トルク値τ２を差し引いた値に基づき、上限駆動トルク値τ１ｍａｘを算出するため、上限駆動トルク値τ１ｍａｘを適切に算出し、駆動トルク値τ１の出力異常をより適切に抑制することができる。 The upper-limit driving torque value acquisition unit 136 is a holding torque value that acquires the own weight of the operating mechanism (power assist suit 10) and a holding torque value τ2 that is a torque for maintaining a load applied to the operating mechanism (power assist suit 10). An acquisition unit 150 and an upper limit driving torque value calculation unit 152 that calculates an upper limit driving torque value τ1 max based on a value obtained by subtracting the holding torque value τ2 from the upper limit torque value τ0 max . The upper limit driving torque value acquisition unit 136, based on the value obtained by subtracting the holding torque τ2 from the upper limit torque value .tau.0 max, in order to calculate the upper limit driving torque value .tau.1 max, properly calculate the upper limit driving torque value .tau.1 max, The output abnormality of the drive torque value τ1 can be suppressed more appropriately.

この制御装置１ｃは、目標加速度ａから算出される行列式を、予め定められたリンクの加速度の上限値の行列式で正規化して補正項Ｓを算出する補正項算出部１４０を更に有する。駆動トルク値決定部１４２は、変換駆動トルク値τ１Ｂを補正項Ｓで補正したトルク値を、駆動トルク値τ１として決定する。補正項Ｓは、駆動トルク値τ１によってアクチュエータを駆動した場合に、リンクの実際の加速度が目標加速度ａより大きくならないように、変換駆動トルク値τ１Ｂを補正するための係数である。この制御装置１ｃは、変換駆動トルク値τ１Ｂを補正項Ｓで補正するため、加速度軌道変化に対応した駆動トルク値τ１を算出することができる。なお、制御装置１ｃは、補正項算出部１４０を有さず、変換駆動トルク値τ１Ｂをそのまま駆動トルク値τ１としてもよい。   The control device 1c further includes a correction term calculation unit 140 that calculates a correction term S by normalizing the determinant calculated from the target acceleration a with the determinant of the upper limit of the link acceleration. The drive torque value determination unit 142 determines a torque value obtained by correcting the converted drive torque value τ1B with the correction term S as the drive torque value τ1. The correction term S is a coefficient for correcting the converted drive torque value τ1B so that the actual acceleration of the link does not become larger than the target acceleration a when the actuator is driven by the drive torque value τ1. The control device 1c can calculate the drive torque value τ1 corresponding to the change in the acceleration trajectory because the conversion drive torque value τ1B is corrected by the correction term S. Note that the control device 1c may not include the correction term calculation unit 140 and may use the converted drive torque value τ1B as the drive torque value τ1.

以上、本発明の実施形態を説明したが、この実施形態の内容により実施形態が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。さらに、前述した実施形態の要旨を逸脱しない範囲で構成要素の種々の省略、置換又は変更を行うことができる。例えば、各実施形態においては、作動機構としてパワーアシストスーツ１０を例示して説明したが、関節を介して連結された複数のリンクをアクチュエータにより駆動するものであれば、パワーアシストスーツに限られない。作動機構としては、例えば、歩行ロボットやロボットアームなどであってもよく、移動可能なものであってもよい。   The embodiment of the present invention has been described above, but the embodiment is not limited by the contents of this embodiment. Further, the above-mentioned components include those that can be easily assumed by those skilled in the art, those that are substantially the same, and those that are in the so-called equivalent range. Further, the components described above can be appropriately combined. Furthermore, various omissions, substitutions, or changes of the components can be made without departing from the spirit of the above-described embodiment. For example, in each embodiment, the power assist suit 10 has been described as an example of the operating mechanism. However, the present invention is not limited to the power assist suit as long as a plurality of links connected via joints are driven by an actuator. . The operating mechanism may be, for example, a walking robot or a robot arm, or may be a movable mechanism.

１ 制御装置
１０ パワーアシストスーツ
１１ 腰パーツ
１２ 肩パーツ
１３Ａ、１３Ｂ 脚パーツ
２１ 腰装着部
２２ 給電装置
２２Ｂ 電源
２２Ｃ 電線
２３、２３Ａ、２３Ｂ 大腿駆動機構
３１ 背装着部
３２Ａ 左肩装着部
３２Ｂ 右肩装着部
４１、４１Ａ、４１Ｂ 大腿装着部
４２、４２Ａ、４２Ｂ 下腿装着部
４３、４３Ａ、４３Ｂ 足装着部
４４、４４Ａ、４４Ｂ 荷重検出装置
４５、４５Ａ、４５Ｂ 下腿駆動機構
４６Ａ、４６Ｂ 脚駆動制御装置
４７、４７Ａ、４７Ｂ 回動軸
１０２ 関節角度取得部
１０４ 座標算出部
１０６、１０６ａ 仮想関節角度取得部
１０８ 目標加速度取得部
１０９ｂ 補正係数記憶部
１１０ 駆動トルク値算出部
１２０ 仮想座標算出部
１２１ａ 関係記憶部
１２２ａ 仮想関節角度算出部
１３２ 目標加速度取得部
１３４ 仮駆動トルク値算出部
１３６ 上限駆動トルク値取得部
１３８ トルク比率算出部
１４０ 補正項算出部
１４２ 駆動トルク値決定部
１５０ 保持トルク値取得部
１５２ 上限駆動トルク値算出部
ａ 目標加速度
ａｂ 基準加速度
Ｃ 原点座標
Ｃ０ 現在座標
Ｃ１ 仮想座標
ｆ 荷重値変化量
Ｆ 足底荷重値
Ｋ、Ｌ 補正係数
Ｓ 補正項
Ｔ１、Ｔ２、Ｔ３、Ｔ４ 閾値角度
Ｔｍ 加速度上限項
α トルク比率
αｍａｘ 最大トルク比率
β 加速度項
Δθ 変位角度
ε 変位距離
θ 関節角度
θ０、θ０ａ、θ０ｂ、θ０ｃ 現在関節角度
θ１、θ１ａ、θ１ｂ、θ１ｃ 仮想関節角度
τ０ 総トルク値
τ０ｍａｘ 上限トルク値
τ１ 駆動トルク値
τ１Ａ 仮駆動トルク値
τ１Ｂ 変換駆動トルク値
τ１ｍａｘ 上限駆動トルク値
τ２ 保持トルク値
DESCRIPTION OF SYMBOLS 1 Control device 10 Power assist suit 11 Waist part 12 Shoulder parts 13A, 13B Leg parts 21 Waist mounting part 22 Power supply device 22B Power supply 22C Electric wire 23, 23A, 23B Thigh drive mechanism 31 Back mounting part 32A Left shoulder mounting part 32B Right shoulder mounting part 41, 41A, 41B Thigh mounting part 42, 42A, 42B Leg mounting part 43, 43A, 43B Foot mounting part 44, 44A, 44B Load detecting device 45, 45A, 45B Leg driving mechanism 46A, 46B Leg driving control device 47, 47A , 47B Rotation axis 102 Joint angle acquisition unit 104 Coordinate calculation unit 106, 106a Virtual joint angle acquisition unit 108 Target acceleration acquisition unit 109b Correction coefficient storage unit 110 Drive torque value calculation unit 120 Virtual coordinate calculation unit 121a Relationship storage unit 122a Virtual joint Angle calculator 132 Target acceleration Part 134 provisional driving torque value calculation unit 136 upper drive torque value acquiring unit 138 torque ratio calculation unit 140 correction term calculation unit 142 drive torque value determining unit 150 holds the torque value acquisition unit 152 upper drive torque value calculating unit a target acceleration a b reference Acceleration C Origin coordinate C0 Current coordinate C1 Virtual coordinate f Load change amount F Foot load K, L Correction coefficient S Correction term T1, T2, T3, T4 Threshold angle Tm Acceleration upper limit α Torque ratio α max Maximum torque ratio β Acceleration term Δθ Displacement angle ε Displacement distance θ Joint angle θ0, θ0a, θ0b, θ0c Current joint angle θ1, θ1a, θ1b, θ1c Virtual joint angle τ0 Total torque value τ0 max Upper limit torque value τ1 Drive torque value τ1A Temporary drive torque value τ1B Conversion driving torque value τ1 max Upper limit driving torque value τ2 Holding torque value

Claims (13)

関節を介して連結された複数のリンクをアクチュエータにより駆動する作動機構の制御装置であって、
前記リンクの目標とする移動加速度である目標加速度を取得する目標加速度取得部と、
前記リンクを前記目標加速度で移動させるためのトルクである駆動トルク値の仮の値である仮駆動トルク値を、前記目標加速度に基づき算出する仮駆動トルク値算出部と、
前記作動機構が出力可能な上限のトルク値である上限トルク値に基づき、前記駆動トルク値の上限のトルク値である上限駆動トルク値を取得する上限駆動トルク値取得部と、
前記仮駆動トルク値と前記上限駆動トルク値との比率であるトルク比率を算出するトルク比率算出部と、
前記仮駆動トルク値が前記上限駆動トルク値以下である場合に、前記仮駆動トルク値を前記駆動トルク値に決定し、前記仮駆動トルク値が前記上限駆動トルク値より大きい場合に、前記トルク比率に基づき、前記仮駆動トルク値を前記上限駆動トルク値以下の値に変換した変換駆動トルク値を、前記駆動トルク値に決定する駆動トルク値決定部と、
前記アクチュエータを駆動するトルク値として、決定した前記駆動トルク値を用いて、前記アクチュエータを制御するアクチュエータ制御部と、
を有する、作動機構の制御装置。 A control device for an operating mechanism that drives a plurality of links connected via a joint by an actuator,
A target acceleration acquisition unit that acquires a target acceleration that is a target movement acceleration of the link,
A temporary drive torque value calculation unit that calculates a temporary drive torque value that is a temporary value of a drive torque value that is a torque for moving the link at the target acceleration, based on the target acceleration;
An upper-limit driving torque value acquiring unit that acquires an upper-limit driving torque value that is an upper-limit torque value of the driving torque value based on an upper-limit torque value that is an upper-limit torque value that can be output by the operation mechanism.
A torque ratio calculation unit that calculates a torque ratio that is a ratio between the temporary drive torque value and the upper limit drive torque value,
When the temporary drive torque value is equal to or less than the upper limit drive torque value, the temporary drive torque value is determined to be the drive torque value, and when the temporary drive torque value is larger than the upper limit drive torque value, the torque ratio is determined. A drive torque value determining unit that determines a converted drive torque value obtained by converting the temporary drive torque value to a value equal to or less than the upper limit drive torque value, as the drive torque value,
As a torque value for driving the actuator, using the determined drive torque value, an actuator control unit that controls the actuator,
A control device for an operating mechanism, comprising: 前記作動機構は、複数のリンクのそれぞれに対応する複数のアクチュエータを備えており、前記駆動トルク値は、複数のアクチュエータのそれぞれに対して算出されるものであり、
前記駆動トルク値決定部は、複数のアクチュエータに対するそれぞれの仮駆動トルク値のうち、前記上限駆動トルク値に対する前記仮駆動トルク値の超過率が最も高い仮駆動トルク値についてのトルク比率を用いて、それぞれの仮駆動トルク値を前記変換駆動トルク値に変換し、それぞれの前記変換駆動トルク値を、それぞれのアクチュエータに対する前記駆動トルク値に決定する、請求項１に記載の作動機構の制御装置。 The operating mechanism includes a plurality of actuators corresponding to each of a plurality of links, and the drive torque value is calculated for each of the plurality of actuators,
The drive torque value determination unit, among the respective provisional drive torque values for the plurality of actuators, using a torque ratio for the highest provisional drive torque value with respect to the upper limit drive torque value is the highest temporary drive torque value, The control device for an operating mechanism according to claim 1, wherein each temporary drive torque value is converted into the converted drive torque value, and each converted drive torque value is determined as the drive torque value for each actuator. 前記駆動トルク値決定部は、前記上限駆動トルク値に対する前記仮駆動トルク値の比率を前記トルク比率とし、前記仮駆動トルク値を前記トルク比率で除することにより、前記変換駆動トルク値を算出する、請求項１又は請求項２に記載の作動機構の制御装置。   The drive torque value determination unit calculates the converted drive torque value by setting a ratio of the provisional drive torque value to the upper limit drive torque value as the torque ratio, and dividing the provisional drive torque value by the torque ratio. The control device for an operating mechanism according to claim 1 or claim 2. 前記上限駆動トルク値取得部は、
前記作動機構の自重、及び前記作動機構にかかる負荷を保持するためのトルクである保持トルク値を取得する保持トルク値取得部と、
前記上限トルク値から前記保持トルク値を差し引いた値に基づき、前記上限駆動トルク値を算出する上限駆動トルク値算出部と、を有する、請求項１から請求項３のいずれか１項に記載の作動機構の制御装置。 The upper limit driving torque value acquisition unit,
The own weight of the operating mechanism, and a holding torque value obtaining unit that obtains a holding torque value that is a torque for holding a load applied to the operating mechanism,
4. An upper limit driving torque value calculating unit for calculating the upper limit driving torque value based on a value obtained by subtracting the holding torque value from the upper limit torque value, according to claim 1, further comprising: Actuator control. 前記目標加速度から算出される行列式を、予め定められたリンクの加速度の上限値の行列式で正規化して補正項を算出する補正項算出部を更に有し、
前記駆動トルク値決定部は、前記変換駆動トルク値を前記補正項で補正したトルク値を、前記駆動トルク値として決定し、
前記補正項は、前記駆動トルク値によって前記アクチュエータを駆動した場合に、前記リンクの実際の加速度が前記目標加速度より大きくならないように、前記変換駆動トルク値を補正するための係数である、請求項１から請求項４のいずれか１項に記載の作動機構の制御装置。 A determinant calculated from the target acceleration, further includes a correction term calculation unit that calculates a correction term by normalizing the determinant with the determinant of the upper limit of the acceleration of the link,
The drive torque value determination unit determines a torque value obtained by correcting the converted drive torque value with the correction term, as the drive torque value,
The correction term is a coefficient for correcting the converted drive torque value so that when the actuator is driven by the drive torque value, the actual acceleration of the link does not become larger than the target acceleration. The control device for an operating mechanism according to any one of claims 1 to 4. 関節を介して連結された複数のリンクをアクチュエータにより駆動する作動機構の制御装置であって、
前記関節を介して隣接するリンク同士の現在の角度である現在関節角度を取得する関節角度取得部と、
前記現在関節角度が、前記現在関節角度に基づいて算出される逆ヤコビ行列が発散する特異点角度である場合に、前記現在関節角度から所定の変位角度だけ異なった前記リンク同士の仮想の角度である仮想関節角度を算出する仮想関節角度取得部と、
前記リンクの目標とする移動加速度である目標加速度を取得する目標加速度取得部と、
前記目標加速度と前記仮想関節角度とに基づき、前記アクチュエータを駆動するための駆動トルク値を算出する駆動トルク値算出部と、
前記アクチュエータを駆動するトルク値として前記駆動トルク値を用いて、前記アクチュエータを制御するアクチュエータ制御部と、を有する、
作動機構の制御装置。 A control device for an operating mechanism that drives a plurality of links connected via a joint by an actuator,
A joint angle acquisition unit that acquires a current joint angle that is a current angle between adjacent links via the joint,
When the current joint angle is a singular point angle at which the inverse Jacobian matrix calculated based on the current joint angle diverges, a virtual angle between the links different from the current joint angle by a predetermined displacement angle. A virtual joint angle acquisition unit that calculates a certain virtual joint angle;
A target acceleration acquisition unit that acquires a target acceleration that is a target movement acceleration of the link,
A drive torque value calculation unit that calculates a drive torque value for driving the actuator based on the target acceleration and the virtual joint angle,
An actuator control unit that controls the actuator using the drive torque value as a torque value for driving the actuator,
Actuator control. 前記仮想関節角度取得部は、前記仮想関節角度である場合の前記リンク同士の距離が、前記現在関節角度である場合の前記リンク同士の距離よりも短くなるように、前記仮想関節角度を算出する、請求項６に記載の作動機構の制御装置。   The virtual joint angle acquisition unit calculates the virtual joint angle such that a distance between the links when the virtual joint angle is shorter than a distance between the links when the current joint angle is used. A control device for an operating mechanism according to claim 6. 前記現在関節角度に基づき、前記リンクの現在の座標である現在座標を算出する座標算出部を更に有し、
前記仮想関節角度取得部は、
前記現在座標のうち、少なくとも１つの軸に対応する座標を所定の変位距離だけ変化させた座標である仮想座標を算出する仮想座標算出部と、
前記リンクが前記仮想座標に位置すると仮定した場合の前記リンク同士の角度を、前記仮想関節角度として算出する仮想関節角度算出部と、を有する、請求項６又は請求項７に記載の作動機構の制御装置。 A coordinate calculation unit that calculates a current coordinate that is a current coordinate of the link based on the current joint angle;
The virtual joint angle acquisition unit,
A virtual coordinate calculator that calculates virtual coordinates that are coordinates obtained by changing coordinates corresponding to at least one axis by a predetermined displacement distance among the current coordinates;
8. The operating mechanism according to claim 6, further comprising: a virtual joint angle calculation unit that calculates an angle between the links assuming that the link is located at the virtual coordinates, as the virtual joint angle. Control device. 前記仮想関節角度取得部は、
前記現在関節角度と前記仮想関節角度との関係を記憶する関係記憶部と、
前記現在関節角度と前記関係に基づき、前記仮想関節角度を算出する仮想関節角度算出部と、を有し、
前記関係は、前記現在関節角度の値に応じて、前記変位角度が異なるものである、請求項６又は請求項７に記載の作動機構の制御装置。 The virtual joint angle acquisition unit,
A relationship storage unit that stores a relationship between the current joint angle and the virtual joint angle;
A virtual joint angle calculation unit that calculates the virtual joint angle based on the current joint angle and the relationship,
The control device for an operation mechanism according to claim 6, wherein the relationship is such that the displacement angle is different depending on a value of the current joint angle. 前記仮想関節角度取得部は、前記現在関節角度の所定の範囲内において、前記変位角度を０として、前記仮想関節角度として前記現在関節角度を用いる、請求項６から請求項９のいずれか１項に記載の作動機構の制御装置。   10. The virtual joint angle acquisition unit according to claim 6, wherein the displacement angle is set to 0 and the current joint angle is used as the virtual joint angle within a predetermined range of the current joint angle. 11. A control device for an operating mechanism according to claim 1. 前記変位角度は、
前記現在関節角度が、前記特異点角度から、前記特異点角度と所定の角度だけ離れた特異点近傍角度までの間である場合に、０より大きい値となり、
前記現在関節角度が、前記特異点角度から前記特異点近傍角度の範囲外である場合に、０となる、請求項１０に記載の作動機構の制御装置。 The displacement angle is:
If the current joint angle is between the singularity angle and the singularity near angle separated from the singularity angle by a predetermined angle, the current joint angle becomes a value greater than 0,
The control device for an operating mechanism according to claim 10, wherein the current joint angle is 0 when the current joint angle is out of the range of the singular point angle from the singular point angle. 前記変位角度だけ関節角度を変化させた場合の前記リンクの角加速度の変化量を示す係数である補正係数を記憶する補正係数記憶部を更に有し、
前記駆動トルク値算出部は、前記目標加速度と前記仮想関節角度と前記補正係数とに基づき、前記駆動トルク値を算出し、
前記補正係数は、所定の関節角度である第１基準関節角度に基づき算出された前記リンクの角加速度である第１基準角加速度と、前記第１基準関節角度から前記変位角度だけ異なる第２基準関節角度に基づき算出された前記リンクの角加速度である第２基準角加速度とに基づき算出される、請求項６から請求項１１のいずれか１項に記載の作動機構の制御装置。 A correction coefficient storage unit that stores a correction coefficient that is a coefficient indicating a change amount of the angular acceleration of the link when the joint angle is changed by the displacement angle;
The drive torque value calculation unit calculates the drive torque value based on the target acceleration, the virtual joint angle, and the correction coefficient,
The correction coefficient is different from a first reference angular acceleration that is an angular acceleration of the link calculated based on a first reference joint angle that is a predetermined joint angle by a second reference that is different from the first reference joint angle by the displacement angle. The control device for an operation mechanism according to any one of claims 6 to 11, wherein the control is performed based on a second reference angular acceleration that is an angular acceleration of the link calculated based on a joint angle. 前記作動機構は、前記関節として、腰関節と膝関節と足首関節とを有し、前記リンクとして、前記腰関節と前記膝関節との間に設けられる大腿リンクと、前記膝関節と前記足首関節との間に設けられる下腿リンクと、前記足首関節の前記下腿リンクと反対側に設けられる足リンクと、を有し、前記アクチュエータにより前記関節を介して隣接するリンクの間でアシスト力を発生させて、装着者の筋力を補助するパワーアシストスーツであって、
前記仮想関節角度取得部は、前記膝関節の現在関節角度が特異点角度である場合に、前記腰関節、前記膝関節及び前記足首関節の仮想関節角度を算出する、請求項６から請求項１２のいずれか１項に記載の作動機構の制御装置。

The operating mechanism has a waist joint, a knee joint, and an ankle joint as the joint, and a thigh link provided between the hip joint and the knee joint as the link, the knee joint, and the ankle joint And a leg link provided on the opposite side of the ankle joint from the lower leg link, and the actuator generates an assist force between adjacent links via the joint by the actuator. Power assist suit to assist the wearer's strength,
13. The virtual joint angle acquisition unit calculates the virtual joint angles of the hip joint, the knee joint, and the ankle joint when the current joint angle of the knee joint is a singular point angle. The control device for an operating mechanism according to any one of claims 1 to 4.

JP2016018269A 2016-02-02 2016-02-02 Actuator control device Active JP6655410B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2016018269A JP6655410B2 (en) 2016-02-02 2016-02-02 Actuator control device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2016018269A JP6655410B2 (en) 2016-02-02 2016-02-02 Actuator control device

Publications (2)

Publication Number Publication Date
JP2017136658A JP2017136658A (en) 2017-08-10
JP6655410B2 true JP6655410B2 (en) 2020-02-26

Family

ID=59566506

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2016018269A Active JP6655410B2 (en) 2016-02-02 2016-02-02 Actuator control device

Country Status (1)

Country Link
JP (1) JP6655410B2 (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6736525B2 (en) * 2017-07-12 2020-08-05 株式会社三共 Amusement machine
WO2020100730A1 (en) 2018-11-12 2020-05-22 日本電気株式会社 Load reduction device, load reduction method, and storage medium storing program

Also Published As

Publication number Publication date
JP2017136658A (en) 2017-08-10

Similar Documents

Publication Publication Date Title
EP2347867B1 (en) Walking control apparatus of robot and method of controlling the same
EP2343164B1 (en) Walking control apparatus of robot and method of controlling the same
US9980842B2 (en) Motion assist device and motion assist method, computer program, and program recording medium
KR101665543B1 (en) Tabilization apparatus for humanoid robot and method thereof
JP5991857B2 (en) Robot balance control apparatus and control method thereof
JP5640991B2 (en) Walking assist device
JP5661023B2 (en) Gait generator for legged mobile robot and motion target generator for robot
US20130079929A1 (en) Robot and control method thereof
US20120143376A1 (en) Walking robot and method for controlling posture thereof
US20110178636A1 (en) Humanoid robot and walking control method thereof
US8958909B2 (en) Walking robot and control method thereof
US20120158181A1 (en) Walking robot and pose control method thereof
EP2426037B1 (en) Walking robot and control method thereof
US20130144439A1 (en) Walking robot and control method thereof
Ugurlu et al. A framework for sensorless torque estimation and control in wearable exoskeletons
EP2343161A1 (en) Walking robot and method of controlling the same
US20110181223A1 (en) Motor control apparatus and motor control method thereof
US20110213495A1 (en) External force target generating device of legged mobile robot
KR20130049029A (en) Walking robot and control method for thereof
JP6655410B2 (en) Actuator control device
US20120046789A1 (en) Walking robot and control method thereof
US8396593B2 (en) Gait generating device of legged mobile robot
Guang et al. Dynamic modeling and interactive performance of PARM: A parallel upper-limb rehabilitation robot using impedance control for patients after stroke
KR20160126335A (en) High-speed walking attitude control apparatus of wearable exoskeleton robot
Ugurlu et al. Lower body exoskeleton-supported compliant bipedal walking for paraplegics: How to reduce upper body effort?

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20190125

TRDD Decision of grant or rejection written
A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20191225

A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20200107

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20200203

R150 Certificate of patent or registration of utility model

Ref document number: 6655410

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150