Knee joint, posture calculation device, knee joint control method, knee joint control program, and posture calculation device control program

Abstract

To provide a knee joint, when a posture of the knee joint is controlled by using a detection result of an angular velocity sensor, capable of correcting the posture of the knee joint by using a detection result of an angle sensor.SOLUTION: A knee joint 100 includes a thigh connection part 102, a crus 104, and a drive assist part 106 for connecting the thigh connection part 102 and the crus 104 and limiting or assisting rotational movement of the crus 104 around a prescribed axis with respect to the thigh connection part 102, an angular velocity sensor 108 for detecting an angular velocity of the crus 104; an angle sensor 110 for detecting an angle of the crus 104 with respect to a stationary coordinate system; a first hypercomplex number 116 derived on the basis of a detection result of the angular velocity sensor 108; a posture calculation part 112 for calculating a posture of the crus 104 on the basis of a second hypercomplex number 118 derived on the basis of the detection result of the angle sensor 110 and having the same number of terms as that of the first hypercomplex number 116, and a control part 114 for controlling the drive assist part 106 on the basis of the calculation result of the posture calculation part 112.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present invention relates to a knee joint, a posture calculation device, a knee joint control method, a knee joint control program, and a posture calculation device control program. [Background technology]

[0002] Various developments have been made in the past regarding prosthetic limbs that assist the wearer in walking. As a result of technological developments in prosthetic limbs, prosthetic limbs have evolved to the point where they can not only support the weight of the wearer but also assume a desired posture in accordance with the wearer's movements, such as walking. A technology has been proposed for calculating the posture of a prosthetic limb by using an angular velocity sensor to perform control that takes into account rotation and posture (posture control using a quaternion) (for example, Patent Document 1). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Publication No. 2020-110332 Summary of the Invention [Problem to be solved by the invention]

[0004] By using quaternions to control the posture of a prosthetic limb, it has become possible to calculate not only the rotation of the prosthetic limb but also its posture, leading to the development of technology that allows control to be tailored to the wearer's walking movements. However, the detection results of the angular velocity sensor are not always accurate, and if an error occurs in the detection results, highly accurate posture control cannot be performed. It is possible to use a pre-defined filter to correct the error in the detection results of the angular velocity sensor. However, the filter is set for each situation, assuming various movement scenarios, and is therefore not very versatile.

[0005] An object of the present invention is to provide a knee joint that can correct the posture of the knee joint in accordance with various situations when controlling the posture of the knee joint using the detection results of an angular velocity sensor. [Means for solving the problem]

[0006] A knee joint for achieving the above object includes a thigh connection part to which a socket corresponding to the thigh of a user is connected; a lower leg part connected to the thigh connection part and rotatable around a predetermined axis; a drive assist part that connects the thigh connection part and the lower leg part and limits or assists the rotational movement of the lower leg part around the predetermined axis relative to the thigh connection part; an angular velocity sensor that detects the angular velocity of the lower leg part; an angle sensor that detects the angle of the lower leg part with respect to a stationary coordinate system; a posture calculation part that calculates the posture of the lower leg part based on a first multiplier derived based on the detection result of the angular velocity sensor and a second multiplier derived based on the detection result of the angle sensor and having the same number of terms as the first multiplier; and a control part that controls the drive assist part based on the calculation result of the posture calculation part. [Brief explanation of the drawings]

[0007] [Figure 1] 1 is a schematic diagram of a femoral prosthesis including an electronically controlled knee joint according to an embodiment. FIG. [Figure 2] FIG. 2 is a block diagram showing a schematic configuration of a knee joint. [Figure 3] FIG. 10 is a flow chart showing a series of movements of the knee joint. [Figure 4] FIG. 4 is a flowchart specifically showing the processing of step S2 in FIG. 3. [Figure 5] 10 is a graph showing the relationship between the value qa and the value qg. [Figure 6] A schematic diagram of the knee joint is shown. [Figure 7] FIG. 10 is a block diagram showing a series of processes performed by the attitude calculation unit. [Figure 8] FIG. 10 is a block diagram of a control program for controlling the knee joint. DETAILED DESCRIPTION OF THE INVENTION

[0008] The following describes embodiments of the present invention. The present invention can be applied to various devices that independently control their posture using the detection results of an angular velocity sensor installed in the device itself without external mechanical support. Examples of such devices include an electronically controlled knee joint for a prosthetic leg, which will be described in the following embodiments, as well as mobile objects such as drones and autonomous transport vehicles.

[0009] In the following explanation, two types of three-dimensional Cartesian coordinate systems, a fixed system and a rotating system, may be used to indicate directions. The stationary coordinate system has three axes defined based on the state in which the wearer of the prosthetic limb is standing upright. The X-axis extends in the width direction of the wearer, the Y-axis extends in the front-to-back direction of the wearer, and the Z-axis extends in the height direction of the wearer.

[0010] FIG. 1 is a schematic diagram of a femoral prosthesis including an electronically controlled knee joint (hereinafter simply referred to as "knee joint") according to an embodiment. The femoral prosthesis 10 comprises a socket 12, a knee joint 100, and a foot 14. The socket 12 houses the wearer's residual limb. The foot 14 functions as the wearer's foot. The knee joint 100 is connected between the socket 12 and the foot 14. The knee joint 100 serves as a knee joint that controls the angle of the foot 14 relative to the socket 12 depending on the wearer's state, such as posture.

[0011] The knee joint 100 comprises a thigh connection portion 102, a lower leg portion 104, and a drive auxiliary portion 106. The thigh connection portion 102 is non-rotatably connected to the socket 12. The lower leg portion 104 is rotatably connected to the thigh connection portion 102 around the X-axis. The lower leg portion 104 is non-rotatably connected to the foot portion 14. The drive auxiliary portion 106 is connected between the thigh connection portion 102 and the lower leg portion 104. The drive auxiliary portion 106 is composed of a telescopic drive mechanism such as a pneumatic or hydraulic cylinder. One end of the drive auxiliary portion 106 is connected to the thigh connection portion 102, and the other end is connected to the lower leg portion 104. The drive auxiliary portion 106 is driven or stopped under the control of a control portion, which will be described later. When the drive assistant unit 106 extends, the angle between the thigh connection unit 102 and the lower leg unit 104 about the X-axis increases and approaches 180 degrees, so that the thigh connection unit 102 and the lower leg unit 104 are aligned in a straight line. When the drive assistant unit 106 retracts, the angle between the thigh connection unit 102 and the lower leg unit 104 about the X-axis decreases, so that the thigh connection unit 102 and the lower leg unit 104 form a predetermined angle (e.g., 90 degrees), allowing the wearer to sit down or perform other movements. When the extension / contraction of the drive assistant unit 106 is limited, the angle between the thigh connection unit 102 and the lower leg unit 104 is fixed at the current angle. The knee joint 100 controls the angle between the thigh connection unit 102 and the lower leg unit 104 by extending or restricting the extension / contraction of the drive assistant unit 106, and thereby controls the angle of the foot unit 14 relative to the socket 12.

[0012] Figure 2 is a block diagram showing the schematic configuration of the knee joint. Each block shown in Figure 2 can be realized in hardware terms using elements such as a computer processor, CPU, and memory, electronic circuits, and mechanical devices, and in software terms using computer programs, etc. However, the functional blocks shown here are realized by the cooperation of these elements. Therefore, those skilled in the art will understand that these functional blocks can be realized in various ways by combining hardware and software. These pieces of hardware may be housed in either the thigh connection portion 102 or the crus portion 104, or may be attached to the knee joint 100 as an independently provided control device.

[0013] As shown in FIG. 2, the knee joint 100 includes an upper leg connecting portion 102, a driving assistant portion 106, a lower leg portion 104, an angular velocity sensor 108, an angle sensor 110, a posture calculation portion 112, and a control portion 114.

[0014] The angular velocity sensor 108 detects the angular velocity of the lower leg portion 104. A gyro sensor can be used as the angular velocity sensor 108. The angular velocity sensor 108 is housed in the lower leg portion 104. The angular velocity sensor 108 detects the angular velocity of the lower leg portion 104 around the X-axis, Y-axis, and Z-axis around the joint with the thigh connecting portion 102, and supplies the detection results to the posture calculation unit 112.

[0015] The angle sensor 110 detects the angle of the lower leg 104 relative to a stationary coordinate system (vertical or azimuth). An acceleration sensor or a geomagnetic sensor can be used as the angle sensor 110. The detection result of the angle sensor 110 is supplied to the posture calculation unit 112. Note that a six-axis inertial sensor combining the angle sensor 110 and the angular velocity sensor 108 may also be used.

[0016] The posture calculation unit 112 calculates the posture of the lower leg 104 based on the detection results of the angular velocity sensor 108 and the angle sensor 110 .

[0017] The posture calculation unit 112 includes a first derivation unit 116, a second derivation unit 118, and a correction unit 122. The posture calculation unit 112 calculates the posture of the lower leg 104 based on the ternion derived by the first derivation unit 116 and the ternion derived by the second derivation unit 118. How to determine the posture of the lower leg 104 will be described later.

[0018] The first derivation unit 116 derives a first multiplier based on the detection result of the angular velocity sensor 108. The first multiplier is obtained by deriving a quaternion that indicates the degree to which a rotating coordinate system that rotates together with the knee joint 100 has rotated relative to a stationary coordinate system, and then converting the quaternion into a ternion. The derived ternion is supplied to the correction unit 122. A quaternion is expressed as an equation having four terms related to unit vectors or rotation angles. The following equation 1 is a quaternion that represents a general rotation. Note that deriving a quaternion is an expression used to explain the logic of the calculation, and it is not necessarily necessary to derive a quaternion, store it as information, or write it out in the series of calculation processes described below.

number

[0019] The second derivation unit 118 derives a second multiplier based on the detection result of the angle sensor 110. The second multiplier indicates how much a rotating coordinate system that rotates together with the knee joint 100 has rotated relative to a stationary coordinate system. The second multiplier is a ternion that has fewer terms than a quaternion.

[0020] When correction is required, the correction unit 122 corrects the posture of the lower leg 104 calculated from the detection results of the angular velocity sensor 108 and the angle sensor 110. The correction unit 122 performs the correction using a filter such as a Kalman filter or a complementary filter. The correction unit 122 performs the correction according to predetermined conditions, such as every time the posture of the lower leg 104 is calculated, at predetermined time intervals, or after the posture of the lower leg 104 has been calculated a predetermined number of times.

[0021] The posture calculation unit 112 supplies the control unit 114 with information on the posture of the lower leg 104 that has not been corrected by the correction unit 122 and information on the posture of the lower leg 104 that has been corrected by the correction unit 122 as posture information.

[0022] The control unit 114 controls the drive assist unit 106 based on the posture information supplied from the posture calculation unit 112. The posture information of the lower leg unit 104 used by the control unit 114 is calculated by the posture calculation unit 112, or is a value calculated and corrected by the posture calculation unit 112. For example, when the wearer's landing motion is estimated from the posture of the lower leg unit 104 indicated by the posture information, the control unit 114 extends the drive assist unit 106 to prepare for landing, and further restricts the drive so that the lower leg unit 104 does not rotate around the X-axis with respect to the thigh connection unit 102 when the wearer's center of gravity is on the prosthetic leg side.

[0023] The knee joint 100 further includes a calibration unit 124. The calibration unit 124 estimates the drift of the angular velocity sensor 108 using the ternion obtained in the first derivation unit 116, and calibrates the angular velocity sensor 108 to cancel out the drift.

[0024] Next, the operation of the knee joint 100 will be described. Figure 3 is a flow chart showing a series of operations of the knee joint. When the power of the knee joint 100 is turned on and a series of operations begins, the knee joint 100 acquires sensor values ​​in step S1. This process is performed by the posture calculation unit 112 acquiring the detection results of the angular velocity sensor 108 and the angle sensor 110. Next, in step S2, the knee joint 100 calculates the posture of the lower leg 104. This process will be described later. Next, in step S3, the knee joint 100 controls the drive assist unit 106. This process is performed by the control unit 1144 controlling the drive assist unit 106 based on the posture information obtained in step S2.

[0025] FIG. 4 is a flow diagram showing a series of processes performed by the posture calculation unit in step S2 of FIG. 3. When the series of processes starts, in step S11, the first derivation unit 116 derives a first multiplier based on the detection result of the angular velocity sensor 108. Next, in step S12, the second derivation unit 118 derives a second multiplier based on the detection result of the angle sensor 110. Note that steps S11 and S12 may be executed in reverse order. The processes of steps S11 and S12 make the first multiplier and the second multiplier capable of being corrected complementarily. Next, in step S13, the posture calculation unit 112 calculates the posture of the lower leg 104 to generate posture information. The process of step S13 includes correcting the obtained posture information in the correction unit 122.

[0026] Next, a method for determining the posture of the lower leg 104 by the posture calculation unit 112 will be described in detail. The posture of the lower leg 104 refers to the orientation of the lower leg 104 in a rotational coordinate system and the type of rotation that resulted in that orientation. To determine the posture of the lower leg 104, the detection value of the angular velocity sensor 108 is integrated to determine the tilt angle. For ease of explanation, a conventionally used calculation method will also be described in detail. Conventionally, equations 2 to 8 have generally been used to determine the tilt angle.

[0027] First, rotation of an arbitrary point r(x, y, z) of the lower leg 104 in the stationary coordinate system by a quaternion q is expressed by the following Equation 2.

number

[0028] Equation 2 represents a rotation of point r once based on quaternion q. Applying this, if a point is continuously rotated by quaternions q1, q2, q3, qn, quaternion q is expressed by the following Equation 3.

number

[0029] Using Equation 3, the quaternion q(t) at time t becomes the quaternion q(t+Δt) after a time Δt, and the rotation during this time is expressed by the quaternion q(Δt), as shown in Equation 4 below.

number

[0030] In Equation 4, if the change in vector n during the small time Δt is due to a small angle Δθ, the tilt is expressed by the following Equation 5.

number

[0031] In Equation 5, if the infinitesimal time Δt approaches 0, the output obtained from the angular velocity sensor 108 becomes a value rotated by the quaternion q, and when this is substituted into Equation 2, the following Equation 6 is obtained.

number

[0032] By differentiating the quaternion q with respect to time t in Equation 6, Equation 7 is obtained.

number

[0033] By rearranging Equation 7, Equation 8 is obtained.

number

[0034] By using Equation 8, it is possible to calculate the tilt angle of the lower leg 104. However, if the tilt angle is repeatedly calculated using Equation 8 when the detection value of the angular velocity sensor 108 contains drift, errors due to the drift may accumulate and an accurate tilt angle may not be obtained. Furthermore, in Equation 5, Δt is made as close to 0 as possible. In other words, Equation 8 is only valid when the calculation interval by the posture calculation unit 112 is short.

[0035] Because it is difficult to reduce the error due to drift to zero, it is sufficient to correct the result obtained by Equation 8 based on the detection result of a sensor other than angular velocity sensor 108, i.e., angle sensor 110. However, because angle sensor 110 cannot accurately detect rotation around the Z axis (around the yaw axis), it is difficult to correct the detection result of angular velocity sensor 108 by directly using the detection result of angle sensor 110.

[0036] In the knee joint 100 of this embodiment, rotation around the Z axis means turning to change the direction the wearer is facing (for example, changing direction while walking north and walking west). In other words, the orientation of the wearer and the extension / retraction operation of the assist drive unit 106 are not directly related, so the turning operation does not affect the control of the knee joint 100 (i.e., the extension / retraction operation of the assist drive unit 106). The inventors focused on this point and discovered a new finding: if the number of terms in the quaternion derived from the detection results of the angular velocity sensor 108 is reduced so that the number of terms in the detection results of the angular velocity sensor 108 is the same as the number of terms in the detection results of the angle sensor 110, so that rotation around the Z axis is ignored (regarded as 0), correction can be made between the number of terms in the ternion derived from the detection results of the angle sensor 110. This makes it possible to correct the detection results of the angular velocity sensor 108 with the detection results of the angle sensor 110, or conversely, to correct the detection results of the angle sensor 110 with the detection results of the angular velocity sensor 108. This point will be explained in more detail below.

[0037] The current rotation of the lower leg 104 obtained from the detection results of the angular velocity sensor 108 is represented by the quaternion q' = (q0' + q1'i + q2'j + q3'k). If rotation around the Z axis is ignored, this means that the lower leg 104 can be rotated by any angle θ using the quaternion q'. Therefore, the value q'' obtained by rotating the quaternion q' by an angle θ around the Z axis is represented by the following equation 9. Note that the value q'' represents a ternion with one less term than the quaternion q', ignoring rotation around the Z axis.

number

[0038] If we try to set the k term of the four terms in Equation 9 to 0, for example, we obtain the relationship shown in Equation 10 below.

number

[0039] Based on Equation 10, the sine and cosine of the angle θ are expressed by Equation 11 below.

number

[0040] Substituting Equation 11 into Equation 9, we obtain Equation 12 below.

number

number

[0041] In Equation 12 and Equation 13, the term k is considered to be 0 and the rotation of the lower leg 104 about the Z axis is ignored, but this ternion can also be said to be equivalent to the original quaternion when considering the properties of the knee joint 100. Equation 13 indicates that a new axis is defined by rotating the X axis around the Z axis by a predetermined angle, and the lower leg 104 is rotated by an angle θ around the new axis.

[0042] When first derivation unit 116 calculates the ternion q'' after a change in time based on the quaternion q derived from the current detection result of angular velocity sensor 108, for example, the following calculation method is used. First, as shown in Equation 14, the current quaternion q is substituted into Equation 8 to calculate the quaternion q' after a change in time.

number

[0043] Next, Equation 14 is substituted into Equation 15 below.

number

number

[0044] When the same calculation is performed for the ternions q1'' and q2'', the following Equations 17 and 18 are obtained.

number

number

[0045] By using Formulas 16 to 18, the tilt angle of the lower leg 104 can be calculated as a ternion q″ based on the previous ternion q and the output ω obtained from the current detection result of the angular velocity sensor 108.

[0046] Next, a method will be described in which the second derivation unit 118 calculates the posture of the lower leg 104 from the detection results of the angle sensor 110. In this case, it is assumed that the angle sensor 110 is configured with a three-axis acceleration sensor. The output of the angle sensor 110 is a(ax, ay, az), and is normalized in advance so that the magnitude of the vector becomes 1. The ternion qa obtained from the detection results of the angle sensor 110 is qa=(qa0, qa1, qa2), and the acceleration measured in a rotated coordinate system in which the gravitational acceleration (0, 0, -1) is rotated by the ternion qa is a. Therefore, the relationship in the following Equation 19 holds from the equation q*rq when a point in a stationary coordinate system is viewed from a coordinate system rotated by the ternion qa.

number

[0047] By rearranging Equation 19, the following Equations 20 to 22 are obtained.

number

number

number

[0048] From Equation 22, the relationship of Equation 23 is obtained.

number

[0049] By substituting Equation 23 into Equation 20 and Equation 21, the following Equations 24 and 25 are obtained.

number

number

[0050] As described above, by calculating the posture of the lower leg 104 while ignoring rotation around the Z axis, the detection results of the angle sensor 110 and the angular velocity sensor 108 can be treated as having the same dimension. By having the posture calculation unit 112 treat the detection results of the angle sensor 110 and the angular velocity sensor 108 as a ternion, it is possible to simplify calculations, for example, when correcting each other's values ​​in a complementary manner. In other words, by treating the detection results of the angular velocity sensor 108 and the detection results of the angle sensor 110 as having the same dimension, it is possible to create a state in which the detection results of the angular velocity sensor 108 can be corrected using the detection results of the angle sensor 110. This creates an environment in which the detection values ​​of the angular velocity sensors can be corrected according to various situations. For example, even if the detection results of the angular velocity sensor 108 contain errors due to drift, the errors can be corrected using the detection results of the angle sensor 110. The posture calculation unit 112 repeatedly calculates the posture during operation. However, if, for example, the wearer stops for a long time and a long time has passed since the last posture calculation, errors are likely to occur in the detection results of the angular velocity sensor 108. Furthermore, the knee joint 100 may be equipped with an energy-saving mode that reduces the overall energy consumption. In non-energy-saving mode, the posture of the lower leg 104 is calculated every 5 ms, for example, whereas in energy-saving mode, the posture of the lower leg 104 is calculated every 100 ms, for example. When the energy-saving mode is entered, the calculation frequency decreases, which can lead to larger errors in the detection results of the angular velocity sensor 108. In such cases, it is very useful to be able to correct the posture of the lower leg 104 using the detection results of the angle sensor 110.

[0051] Next, the correction by the correction unit 122 will be described in detail. The correction unit 122 repeatedly corrects the calculation result of the posture of the lower leg portion 104 while the knee joint 100 is being driven. The correction unit 122 may perform correction each time the posture calculation unit 112 calculates the posture of the lower leg portion 104, or may perform correction when a predetermined condition is met. Examples of the predetermined condition include when returning from energy saving mode and when a predetermined time has elapsed since the last correction. Another example of the predetermined condition is when the control unit 114 controls the drive assist unit 106 based on calculation results that have not been corrected a predetermined number of times since the last correction. This is because in such cases, there is a high possibility that the amount of drift contained in the detection result of the angular velocity sensor 108 is large.

[0052] When the predetermined condition is set to perform correction when a predetermined time has elapsed since the previous correction, the correction unit 122 can use a first weighting coefficient A and a second weighting coefficient B that change depending on the time elapsed since the previous correction. The first and second weighting coefficients A and B are coefficients that determine to what extent the corresponding ternion is reflected in the correction result. If the ternion derived from the angular velocity sensor obtained by Equations 16 to 18 is qg and the ternion derived from the angle sensor obtained by Equations 23 to 25 is qa, the correction unit 122 can correct the calculation result of the posture of the lower leg 104 using the following Equation 26.

number

[0053] If a long time has passed since the previous correction, the value of the second weighting coefficient B can be increased to increase the weight of the ternion based on the angle sensor 110, and the value of the first weighting coefficient A can be decreased to decrease the weight of the ternion based on the angular velocity sensor 108. This makes it possible to obtain a correction value qh that is less affected by drift in the angular velocity sensor 108.

[0054] When the posture calculation unit 112 performs a correction each time it calculates the posture of the lower leg 104, or when a predetermined condition is set such that a correction is performed when returning from energy saving mode or when the control unit 114 controls the drive assistance unit 106 based on a calculation result that has not been corrected a predetermined number of times since the last correction, the correction unit 122 can perform the correction by the following method.

[0055] In this case, the correction unit 122 may correct the posture of the lower leg 104 using a complementary filter. The detection result of the angle sensor 110 cannot distinguish between vibrations caused by movements other than walking and vibrations caused by changes in gravitational acceleration. Therefore, when vibrations occur, the detection result of the angle sensor 110 will contain a large amount of noise. On the other hand, the detection result of the angular velocity sensor 108 is not affected by vibrations, and therefore the noise is considered to be small in the short term. Therefore, for example, when correction is performed at short time intervals, it is advisable to prioritize the detection result of the angular velocity sensor 108 and perform correction such that the influence of drift of the angular velocity sensor 108 is counteracted by the detection result of the angle sensor 110. In this case, the correction unit 122 corrects the posture of the lower leg 104 using the following Equation 27.

number

[0056] As another example, the correction unit 122 may use a Kalman filter to correct the posture of the lower leg 104. The Kalman filter is expressed by the following Equation 28.

number

[0057] When the correction unit 122 corrects the posture of the lower leg 104 using a Kalman filter, it can also be said that the correction unit 122 has a function of correcting the calculation result of the posture calculation unit 112 according to the variance (i.e., sudden vibration) of the observed values ​​of the angle sensor 110. The correction unit 122 may change the variable K based on a change in amplitude or a change in frequency within a predetermined period.

[0058] As an alternative to the Kalman filter expressed by Equation 28, a filter expressed by Equation 29 below may be used.

number

[0059] In Equation 29, the value of the term (qa-qg) is small when the reliability of the angle sensor 110 is high, and the value of the term (qa-qg) is large when the reliability of the angle sensor 110 is low.

[0060] Equation 29 can also be expressed as the following Equations 30 to 32.

number

number

number

[0061] FIG. 5 is a graph showing the relationship between the value qa and the value qg. Specifically, FIG. 5 illustrates the values ​​qa and qg arranged in a normed space, with the vectors assumed to have the same magnitude. As shown in FIG. 5, the correction value qh is obtained by shifting the value qg toward the value qa by a fixed value μ. As can be seen from FIG. 5, the correction value qh always falls inside the arc in the normed space and has a magnitude smaller than 1. However, the output a(ax, ay, az) of the angle sensor 110 is previously normalized and fixed at 1, and is brought close to the value qa in Equation 29. Therefore, even if the correction unit 122 does not perform a calculation to normalize the correction value qh at this stage, the correction value qh is stabilized at a value close to 1 (slightly smaller than 1). This reduces the amount of calculations performed by the correction unit 122.

[0062] The attitude information obtained by the attitude calculation unit 112 or the corrected attitude information is supplied to the control unit 114 at the discretion of the attitude calculation unit 112. For example, if a predetermined condition is set for correction, the attitude calculation unit 112 supplies uncorrected attitude information to the control unit 114 while the predetermined condition is not satisfied. The attitude calculation unit 112 corrects the attitude information and supplies the corrected attitude information to the control unit 114 only when the predetermined condition is satisfied. Note that the attitude calculation unit 112 may calculate the attitude information and corrected attitude information in all cases and supply them to the control unit 114, and the control unit 114 may determine the predetermined condition and decide which attitude information to use.

[0063] Next, a method for calibrating the angular velocity sensor 108 by the calibration unit 124 will be described. The calibration unit 124 estimates the drift of the angular velocity sensor 108 using the ternion obtained by the first derivation unit 116, and brings the bias closer to zero. For example, if a detection result of the angular velocity sensor 108 is obtained even though the wearer is not moving, it can be assumed that drift has occurred. In this case, the calibration unit 124 treats the average value of the detection results of the angular velocity sensor 108 as the detection result of the angular velocity sensor 108. The calibration unit 124 may also calibrate the detection result of the angular velocity sensor 108 by applying a filter of Formula 29 to the values ​​obtained by Formulas 16 to 18. The calibration unit 124 may perform calibration every time a detection result of the angular velocity sensor 108 is obtained, or may perform calibration when the above-mentioned predetermined condition is satisfied.

[0064] Providing the calibration unit 124 improves the accuracy of the detection results of the angular velocity sensor 108. In particular, with certain devices such as the above-knee prosthesis 10, it is difficult for the user to perform calibration while holding the device in the correct position. Therefore, by making it possible to continuously calibrate the angular velocity sensor 108 using the calibration unit 124, it also has the effect of reducing the burden on the user.

[0065] Next, the processing of the control unit in step S3 will be described. As described above, the posture information is supplied as a ternion. The control unit 114 controls the posture of the lower leg unit 104 based on the posture information including the supplied ternion. The control unit 114 controls the amount of expansion and contraction of the drive assist unit 106 based on the pitch angle p and roll angle r obtained in the following example.

[0066] The quaternion representing rotating the pitch angle p and then rotating the roll angle r is expressed by the following Equation 33 using Equations 1 and 3.

number

[0067] When Equation 33 is converted into a ternion using Equation 12, the following Equations 34 to 36 are obtained.

number

number

number

[0068] When formula 36 is rearranged with respect to p, the following formula 37 is obtained.

number

[0069] Furthermore, rearranging Equations 34 and 35, we obtain Equation 38 below.

number

[0070] The control unit 114 controls the drive assist unit 106 based on the results obtained from Equations 37 and 38.

[0071] Furthermore, for example, when there is a term including the value q0 in the denominator as in Equation 17, the attitude calculation unit 112 may perform the following process.

[0072] For example, in Equation 17, the calculation is performed by ignoring the squared term of dt, assuming that the value q0 is sufficiently large, but when the value q0 is small (i.e., when the knee joint 100 has rotated nearly 180 degrees in the fixed coordinate system), it may not be possible to ignore the term including the value q0 in the denominator. For this reason, when the knee joint 100 has rotated nearly 180 degrees in the stationary coordinate system, the posture calculation unit 112 calculates the posture by rotating the coordinate system as follows:

[0073] Figure 6 shows a schematic diagram of a knee joint. As shown in Figure 6, when the value q0 is large, the posture calculation unit 112 calculates the posture using a stationary coordinate system G1 (first coordinate system) located in the lower hemisphere, and when the value q0 is small, it calculates the posture using a stationary coordinate system G2 (second coordinate system) obtained by converting the stationary coordinate system G1 located in the upper hemisphere. When calculating the posture using the stationary coordinate system G1 and the stationary coordinate system G2, the orientation of the rotation axis is maintained, and the rotation angle is calculated by adding 180 degrees to the rotation angle in the stationary coordinate system G1.

[0074] When converting the coordinate system, the following formulas 39 to 44 are used.

number

number

number

number

number

number

[0075] By rearranging Equation 40 and Equation 42, the following Equation 45 is obtained.

number

[0076] Furthermore, rearranging Equation 40 and Equation 43, we obtain Equation 46 below.

number

[0077] Furthermore, rearranging Equation 40 and Equation 44, we obtain Equation 47 below.

number

[0078] By converting the coordinates in this way, a highly accurate correction value can be obtained even if the value q0 is small.

[0079] FIG. 7 is a block diagram showing a series of processes performed by the attitude calculation unit. For simplicity of illustration, FIG. 7 illustrates an example in which correction is performed each time attitude information is calculated. As shown in FIG. 7, the attitude calculation unit 112 derives a ternion from the detection result of the angle sensor 110 in element 1120. This process is performed by the second derivation unit 118 using Equations 23 to 25. The attitude calculation unit 112 derives a ternion from the detection result of the angular velocity sensor 108 in element 1122. This process is performed by the first derivation unit 116 using Equations 16 to 18. The attitude calculation unit 112 derives the difference between the output of element 1120 and the output of element 1122 in element 1124. This difference is filtered in element 1126 (e.g., Equation 29) and output to element 1228 as a correction value. In element 1128, the sum of the ternion obtained in element 1122 and the correction value obtained in element 1126 is derived. That is, in element 1122, the posture information of the lower leg 104 once calculated in element 1122 is corrected by the correction value. The corrected posture information is supplied to the control unit 114 as output q112 of the posture calculation unit 112. In addition, output q112 of element 1128 is fed back to element 1122 and used in the next calculation. In this case, the correction result is substituted into qg1 in Equation 30.

[0080] 8 is a block diagram of a control program for controlling the knee joint. As shown in FIG. 8, the control program 200 includes a first variable deriving unit 202, a second variable deriving unit 204, a calculation instructing unit 208, and a drive instructing unit 210.

[0081] The first ternion derivation unit 202 supplies an instruction to the first derivation unit 116 to derive a ternion based on the detection results of the angular velocity sensor 108. The second ternion derivation unit 204 supplies an instruction to the second derivation unit 118 to derive a ternion based on the detection results of the angle sensor 110. The calculation instruction unit 208 supplies an instruction to the posture calculation unit 112 to calculate the posture of the knee joint 100 based on the two obtained ternions. The instruction from the calculation instruction unit 208 includes an instruction to the correction unit 122 to correct the posture information in accordance with predetermined conditions. The drive instruction unit 210 supplies an instruction to the control unit 114 to control the drive assistance unit 106 based on the calculation results of the calculation instruction unit 208.

[0082] As described above, the knee joint 100 creates an environment in which one of the two detection results obtained from the angular velocity sensor 108 and the angle sensor 110 can be used to correct the other. By creating an environment in which correction is possible, it becomes possible to use a simple filter to perform complementary correction on both detection results.

[0083] Furthermore, by using the first weighting coefficient A and the second weighting coefficient B, correction can be performed by placing weight on the detection result of the angle sensor 110 when there is a high possibility that the reliability of the detection result of the angular velocity sensor 108 has decreased, thereby improving the accuracy of attitude calculation.

[0084] Furthermore, if the predetermined conditions are not satisfied, the control unit 114 controls the drive assist unit 106 based only on the detection results of the angular velocity sensor 108, thereby shortening the time required for calculation and improving the response speed of the knee joint 100.

[0085] The present invention is not limited to the above-described embodiment, and each configuration of the embodiment can be appropriately modified without departing from the spirit of the present invention.

[0086] In particular, the present invention is not limited to knee joints, but can be applied to devices where attitude calculations are not affected even if rotation around the Z axis is ignored. The present invention is applicable to devices where the direction of travel does not need to be taken into account for attitude control, or to rotating bodies where attitude calculations are not affected even if rotation in the pitch direction is ignored. When the present invention is implemented as an attitude calculation device that calculates the attitude of a moving body, the attitude can be calculated using a ternion derived from the angular velocity of the moving body and a ternion derived from the detection of the angle of the moving body in a stationary coordinate system. [Explanation of symbols]

[0087] 10 thigh prosthesis, 100 knee joint, 102 thigh connection part, 104 lower leg part, 106 drive auxiliary part, 108 angular velocity sensor, 110 angle sensor, 112 posture calculation part, 114 control part, 116 first derivation part, 118 second derivation part, 122 correction part, 124 calibration part

Claims

1. a thigh connection portion to which a socket corresponding to the user's thigh is connected; a lower leg portion connected to the thigh connection portion and rotatable around a predetermined axis; a drive assisting unit that connects the thigh connecting unit and the crus and limits or assists rotation of the crus around the predetermined axis relative to the thigh connecting unit; an angular velocity sensor that detects the angular velocity of the lower leg; An angle sensor that detects the angle of the lower leg with respect to a stationary coordinate system; a posture calculation unit that calculates the posture of the lower leg based on a first multiplier derived based on the detection result of the angular velocity sensor and a second multiplier derived based on the detection result of the angle sensor and having the same number of terms as the first multiplier; a control unit that controls the drive assist unit based on a calculation result of the attitude calculation unit, the posture calculation unit includes a correction unit that corrects a calculation result of the posture of the lower leg using the first multivariate and the second multivariate; the correction unit performs iterative correction and corrects the calculation result of the posture of the lower leg using a complementary filter including a first weighting coefficient related to the detection result of the angular velocity sensor and a second weighting coefficient related to the detection result of the angle sensor; the first weighting coefficient and the second weighting coefficient are changed according to the time elapsed since the previous correction by the correction unit.

2. 2. The knee joint according to claim 1, wherein the control unit controls the drive assist unit based on the calculation result corrected by the correction unit each time the control unit controls the drive assist unit based only on the first multiplicity a predetermined number of times.

3. The knee joint according to claim 1 or 2, wherein the posture calculation unit calculates the posture by setting a value of the vertical axis direction in a stationary coordinate system to 0.

4. 4. The knee joint according to claim 1, wherein the posture calculation unit calculates the posture using a first coordinate system when an angle of the lower leg with respect to the thigh connection part is less than a predetermined angle, and calculates the posture using a second coordinate system obtained by rotating the first coordinate system when an angle of the lower leg with respect to the socket is equal to or greater than a predetermined angle.

5. The knee joint according to claim 1 , further comprising a calibration unit that calibrates the angular velocity sensor using a detection result of the angle sensor.

6. An attitude calculation device, comprising: calculating the attitude of the moving body based on a first multidimensional number derived based on a detection result of an angular velocity sensor that detects the angular velocity of the moving body and a second multidimensional number derived based on a detection result of an angle sensor that detects the angle of the moving body and having the same number of terms as the first multidimensional number; the attitude calculation device includes a correction unit that corrects a calculation result of the attitude of the moving body using the first multivariate and the second multivariate; the correction unit performs iterative correction and corrects the calculation result of the attitude of the moving body using a complementary filter including a first weighting coefficient related to the detection result of the angular velocity sensor and a second weighting coefficient related to the detection result of the angle sensor; The attitude calculation device, wherein the first weighting coefficient and the second weighting coefficient are changed according to the time elapsed since the last correction by the correction unit.

7. a first derivation unit that derives a ternion as the first multi-dimensional number based on a detection result of the angular velocity sensor; a second derivation unit that derives a ternion as the second multi-dimensional number based on a detection result of the angle sensor; 7. The attitude calculation device according to claim 6, further comprising: a correction unit that corrects a calculation result of the attitude of the moving body using the ternion derived by the first derivation unit and the ternion derived by the second derivation unit.

8. The attitude calculation device according to claim 6 or 7, further comprising a calibration unit that calibrates the angular velocity sensor using a detection result of the angle sensor.

9. a thigh connection portion to which a socket corresponding to the user's thigh is connected; a lower leg portion connected to the thigh connection portion and rotatable around a predetermined axis; a drive assisting unit that connects the thigh connecting unit and the crus and limits or assists rotation of the crus around the predetermined axis relative to the thigh connecting unit; an angular velocity sensor that detects the angular velocity of the lower leg; An angle sensor that detects the angle of the lower leg with respect to a stationary coordinate system; a posture calculation unit that calculates the posture of the lower leg based on a first multiplier derived based on the detection result of the angular velocity sensor and a second multiplier derived based on the detection result of the angle sensor and having the same number of terms as the first multiplier; a control unit that controls the drive assist unit based on a calculation result of the posture calculation unit, the posture calculation unit includes a correction unit that corrects a calculation result of the posture of the lower leg using the first multivariate and the second multivariate; the correction unit performs iterative correction and corrects the calculation result of the posture of the lower leg using a complementary filter including a first weighting coefficient related to the detection result of the angular velocity sensor and a second weighting coefficient related to the detection result of the angle sensor; The control method includes: a step of deriving a first multiplier based on a detection result of the angular velocity sensor, and a step of deriving a second multiplier having the same number of terms as the first multiplier based on a detection result of the angle sensor; calculating a posture of the knee joint based on the first multivariate and the second multivariate; correcting a calculation result of the posture of the lower leg using the first multivariate and the second multivariate; and controlling the drive assist unit based on the calculated posture of the knee joint, the correcting step is performed repeatedly and includes a step of correcting the calculation result of the posture of the knee joint using a complementary filter including a first weighting coefficient related to the detection result of the angular velocity sensor and a second weighting coefficient related to the detection result of the angle sensor, a first weighting coefficient and a second weighting coefficient that are changed according to an elapsed time since a previous correction;

10. A control program for an attitude calculation device that calculates an attitude of a moving body based on a first multiplier derived based on a detection result of an angular velocity sensor that detects the angular velocity of the moving body, and a second multiplier derived based on a detection result of an angle sensor that detects the angle of the moving body and having the same number of terms as the first multiplier, The attitude calculation device Deriving a first multiplier based on a detection result of the angular velocity sensor; Deriving a second multiplier based on a detection result of the angle sensor; calculating the attitude of the moving body based on the first multivariate and the second multivariate; correcting a calculation result of the attitude of the moving body using the first multivariate and the second multivariate; the correcting is performed repeatedly and includes correcting the calculation result of the attitude of the moving body using a complementary filter including a first weighting coefficient related to the detection result of the angular velocity sensor and a second weighting coefficient related to the detection result of the angle sensor; A control program for an attitude calculation device, wherein the first weighting coefficient and the second weighting coefficient are changed according to the time elapsed since the previous correction.

Images