Knee joint, posture calculation device, control method of knee joint, control program for knee joint and control program of posture calculation device
The knee joint system corrects angular velocity sensor errors using angle sensor data and filters, enabling precise and versatile posture control for prosthetic legs.
Patent Information
- Application Number
- JP2024151210
- Authority / Receiving Office
- JP · JP
- Patent Type
- Applications
- Current Assignee / Owner
- Filing Date
- 2024-09-03
- Publication Date
- 2024-12-03
- Estimated Expiration
- 2040-12-25
AI Technical Summary
Existing prosthetic leg control systems using angular velocity sensors face inaccuracies due to detection errors, which hinder precise attitude control, and existing filters lack versatility for various operating conditions.
A knee joint system incorporating an angular velocity sensor, an angle sensor, and a posture calculation unit that utilizes quaternions and ternions to correct posture calculations, employing filters like Kalman or complementary filters, and a calibration unit to adjust for sensor drift, allowing versatile and accurate posture control.
The system provides accurate and versatile posture control for prosthetic legs by correcting angular velocity sensor errors using angle sensor data, ensuring precise movement adjustments and reducing user burden through automated calibration.
Smart Images

Figure 2024167370000001_ABST
Abstract
Description
[Technical field]
[0001] The present invention relates to a knee joint, a posture calculation device, a control method for a knee joint, a control program for a knee joint, and a control program for a posture calculation device. [Background technology]
[0002] Conventionally, various developments have been made on prosthetic limbs that assist the wearer in walking. As a result of technological developments in prosthetic limbs, prosthetic limbs have been developed to the point where they do not simply support the weight of the wearer, but can 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] JP 2020-110332 A Summary of the Invention [Problem to be solved by the invention]
[0004] By controlling the posture of a prosthetic leg using quaternions, it has become possible to calculate not only the rotation of the prosthetic leg but also its posture, and technology has been developed that allows control to be performed in accordance with the walking motion of the wearer. However, the detection results of the angular velocity sensor are not always correct, and if an error occurs in the detection result, it is not possible to perform highly accurate posture control. It is possible to use a pre-defined filter to correct the error in the detection result of the angular velocity sensor. However, the filter is set for each scene assuming various operating situations, and it lacks versatility.
[0005] An object of the present invention is to provide a knee joint that enables the posture of the knee joint to be corrected in accordance with various situations when the posture of the knee joint is controlled using the detection results of an angular velocity sensor. [Means for solving the problem]
[0006] A knee joint for achieving the above-mentioned object comprises a thigh connecting part to which a socket corresponding to the thigh of a user is connected, a lower leg part connected to the thigh connecting part and rotatable around a predetermined axis, a drive auxiliary part connecting the thigh connecting part and the lower leg part and limiting or assisting the rotational movement of the lower leg part about the predetermined axis relative to the thigh connecting part, an angular velocity sensor detecting the angular velocity of the lower leg part, an angle sensor detecting the angle of the lower leg part relative to a stationary coordinate system, a posture calculation part calculating the posture of the lower leg part based on a first multi-dimensional number derived based on the detection result of the angular velocity sensor and a second multi-dimensional number derived based on the detection result of the angle sensor and having the same number of terms as the first multi-dimensional number, and a control part controlling the drive auxiliary part based on the calculation result of the posture calculation part. [Brief description of the drawings]
[0007] [Figure 1] FIG. 1 is a schematic diagram of a femoral prosthesis including an electronically controlled knee joint according to an embodiment. [Diagram 2] FIG. 2 is a block diagram showing a schematic configuration of a knee joint. [Diagram 3] FIG. 1 is a flow chart showing a series of operations of the knee joint. [Figure 4] FIG. 4 is a flow chart specifically showing the process of step S2 in FIG. 3. [Diagram 5] 1 is a graph showing the relationship between value qa and value qg. [Figure 6] A schematic diagram of the knee joint is shown. [Figure 7] FIG. 4 is a block diagram showing a series of processes performed by a posture calculation unit. [Figure 8] FIG. 4 is a block diagram of a control program for controlling the knee joint. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS
[0008] Hereinafter, an embodiment of the present invention will be described. The present invention is applicable to various devices that perform posture control independently using the detection results of an angular velocity sensor provided in the device itself without receiving mechanical support from the outside. Such devices include electronically controlled knee joints for prosthetic legs described in the following embodiments, as well as moving 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 as terms to indicate directions. The stationary coordinate system has three axes defined based on the state in which the wearer of the prosthesis is standing upright. The X-axis extends in the width direction of the wearer, the Y-axis extends in the front-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 thigh prosthesis including an electronically controlled knee joint (hereinafter simply referred to as "knee joint") according to an embodiment. The thigh prosthesis 10 includes 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 includes a thigh connection part 102, a lower leg part 104, and a driving assistant part 106. The thigh connection part 102 is non-rotatably connected to the socket 12. The lower leg part 104 is non-rotatably connected to the thigh connection part 102 around the X-axis. The lower leg part 104 is non-rotatably connected to the foot part 14. The driving assistant part 106 is connected between the thigh connection part 102 and the lower leg part 104. The driving assistant part 106 is composed of a telescopic driving mechanism such as a pneumatic or hydraulic cylinder. One end of the driving assistant part 106 is connected to the thigh connection part 102, and the other end is connected to the lower leg part 104. The driving or stopping of the driving assistant part 106 is controlled by a control part described later. When the drive assistant unit 106 extends, the angle between the thigh connection unit 102 and the lower leg unit 104 around the X-axis increases and approaches 180 degrees, and the thigh connection unit 102 and the lower leg unit 104 are aligned in a straight line. When the drive assistant unit 106 contracts, the angle between the thigh connection unit 102 and the lower leg unit 104 around the X-axis decreases, and the thigh connection unit 102 and the lower leg unit 104 form a predetermined angle (for example, 90 degrees) to accommodate the wearer's sitting motion, etc. When the extension and contraction of the drive assistant unit 106 is restricted, 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 and contraction of the drive assistant unit 106, and therefore controls the angle of the foot unit 14 relative to the socket 12.
[0012] Fig. 2 is a block diagram showing the schematic configuration of the knee joint. Each block shown in Fig. 2 can be realized in hardware by elements such as a computer processor, CPU, and memory, electronic circuits, and mechanical devices, and in software by a computer program, etc., but here, functional blocks realized by the cooperation of these are depicted. Therefore, those skilled in the art will understand that these functional blocks can be realized in various forms by combining hardware and software. These hardware may be housed in either the thigh connection part 102 or the lower leg part 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 angular velocity sensor 108 , an angle sensor 110 , a posture calculation unit 112 , and a control unit 114 in addition to the thigh connection unit 102 , the drive assistant unit 106 , and the lower leg unit 104 .
[0014] The angular velocity sensor 108 detects the angular velocity of the lower leg part 104. A gyro sensor can be used as the angular velocity sensor 108. The angular velocity sensor 108 is housed in the lower leg part 104. The angular velocity sensor 108 detects the angular velocity of the lower leg part 104 around the X-axis, Y-axis, and Z-axis with the joint with the thigh connection part 102 as the center, and supplies the detection results to the posture calculation part 112.
[0015] The angle sensor 110 detects the angle of the lower leg 104 with respect 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 a 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 section 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 obtain the posture of the lower leg 104 will be described later.
[0018] The first derivation unit 116 derives a first ternion based on the detection result of the angular velocity sensor 108. The first ternion is obtained by deriving a quaternion indicating how much the rotating coordinate system that rotates together with the knee joint 100 has rotated relative to the stationary coordinate system, and converting the quaternion into a ternion. The derived ternion is supplied to the correction unit 122. The 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 calculation, and it is not necessarily necessary to derive a quaternion, hold 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 polynomial based on the detection result of the angle sensor 110. The second polynomial indicates how much the rotating coordinate system that rotates together with the knee joint 100 has rotated with respect to the stationary coordinate system. The second polynomial is a ternion having 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 a predetermined condition, such as every time the posture of the lower leg 104 is calculated, at every predetermined time period, 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 auxiliary 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 a value calculated by the posture calculation unit 112, or a value calculated and corrected by the posture calculation unit 112. For example, when the landing motion of the wearer is estimated from the posture of the lower leg unit 104 indicated by the posture information, the control unit 114 extends the drive auxiliary 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 center of gravity of the wearer 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 the drift.
[0024] Next, the operation of the knee joint 100 will be described. Fig. 3 is a flow chart showing a series of operations of the knee joint. When the power supply of the knee joint 100 is turned on and a series of operations is started, the knee joint 100 acquires the sensor values in step S1. This process is executed 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 assistant unit 106. This process is executed by the control unit 1144 controlling the drive assistant unit 106 based on the posture information obtained in step S2.
[0025] FIG. 4 is a flow diagram showing a series of processes of 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. Then, 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. By the processes of steps S11 and S12, the first multiplier and the second multiplier become in a state in which they can be 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 in 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 has caused 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 inclination 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 inclination 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 the rotation of point r once based on quaternion q. By applying this, if the point is continuously rotated by quaternions q1, q2, q3, qn, the quaternion q is expressed by Equation 3 below.
number
[0029] Using Equation 3, the quaternion q(t) at time t becomes the quaternion q(t+Δt) after a time Δt. If the rotation during this time is expressed as the quaternion q(Δt), it can be expressed by the following Equation 4.
number
[0030] In Equation 4, if the change in vector n during the infinitesimal time Δt is due to an infinitesimal angle Δθ, the inclination 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. Substituting this into Equation 2, the following Equation 6 is obtained.
number
[0032] By differentiating the quaternion q in Equation 6 with respect to time t, we obtain Equation 7.
number
[0033] By rearranging Equation 7, Equation 8 is obtained.
number
[0034] By using Equation 8, the inclination angle of the lower leg 104 can be calculated. However, if the inclination angle is calculated repeatedly using Equation 8 when the detection value of the angular velocity sensor 108 includes drift, errors due to the drift accumulate and an accurate inclination angle may not be obtained. Also, 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] Since 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, that is, angle sensor 110. However, since 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 the embodiment, the rotation around the Z axis means that the wearer turns to change the direction in which he / she is facing (for example, while walking north, he / she changes direction and walks west). In other words, the direction of the wearer and the extension / contraction operation of the drive assistant unit 106 are not directly related, so the turning operation does not affect the control of the knee joint 100 (i.e., the extension / contraction operation of the drive assistant unit 106). The inventors focused on this point and obtained a new finding that if the number of terms of the quaternion derived from the detection result of the angular velocity sensor 108 is reduced so as to ignore (regard as 0) the rotation around the Z axis and the detection result of the angular velocity sensor 108 is made the same number of terms as the detection result of the angle sensor 110, correction can be made between the number of terms of the ternion derived from the detection result of the angle sensor 110. This makes it possible to correct the detection result of the angular velocity sensor 108 with the detection result of the angle sensor 110, or conversely, to correct the detection result of the angle sensor 110 with the detection result of the angular velocity sensor 108. This point will be explained in further detail below.
[0037] The current rotation of the lower leg 104 obtained from the detection result of the angular velocity sensor 108 is represented by the quaternion q'=(q0'+q1'i+q2'j+q3'k). When the rotation around the Z axis is ignored, this means that the lower leg 104 may 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 formula 9. Note that the value q'' indicates a ternion with one less term than the quaternion q', ignoring the rotation around the Z axis.
number
[0038] Of the four terms in Equation 9, if we try to set, for example, the term k to 0, 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] By substituting Equation 11 into Equation 9, we obtain the following Equation 12.
number
number
[0041] In Equation 12 and Equation 13, the term k is regarded as 0 and the rotation of the lower leg 104 about the Z axis is ignored, but this ternion can 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 determines a 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 a quaternion q' after a change in time.
number
[0043] Next, substitute Equation 14 into Equation 15 below.
number
number
[0044] By performing similar calculations for the ternions q1'' and q2'', we obtain the following Equations 17 and 18.
number
number
[0045] By using Formulas 16 to 18, the inclination 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 in which the second derivation unit 118 calculates the posture of the lower leg 104 from the detection result of the angle sensor 110 will be described. In this case, 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 the magnitude of the vector is normalized in advance to 1. The ternion qa obtained from the detection result of the angle sensor 110 is qa=(qa0, qa1, qa2), and the acceleration measured in a rotating coordinate system in which the gravitational acceleration (0, 0, -1) is rotated by the ternion qa is a. Therefore, the relationship of the following formula 19 is established from the formula 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 formula 22, the relationship of formula 23 is obtained.
number
[0049] By substituting Expression 23 into Expressions 20 and 21, respectively, the following Expressions 24 and 25 are obtained.
number
number
[0050] As described above, by calculating the posture of the lower leg 104 while ignoring the rotation around the Z axis, the detection result of the angle sensor 110 and the detection result of the angular velocity sensor 108 can be treated as being of the same dimension. By the posture calculation unit 112 treating the detection result of the angle sensor 110 and the detection result of the angular velocity sensor 108 as a ternion, it is possible to simplify the calculation when, for example, correcting each other's values in a complementary manner. In other words, by treating the detection result of the angular velocity sensor 108 and the detection result of the angle sensor 110 as being of the same dimension, it is possible to create a state in which the detection result of the angular velocity sensor 108 can be corrected using the detection result of the angle sensor 110. This creates an environment in which the detection value of the angular velocity sensor can be corrected according to various situations. For example, even if the detection result of the angular velocity sensor 108 contains an error due to drift, it becomes possible to correct the error using the detection result of the angle sensor 110. The posture calculation unit 112 repeatedly continues to calculate the posture during driving, but if, for example, the wearer stops for a long time and a long time has passed since the previous posture calculation, an error is likely to occur in the detection result of the angular velocity sensor 108. In addition, an energy saving mode that reduces the energy consumption of the entire knee joint 100 may be installed. In the non-energy saving mode, the posture of the lower leg part 104 is calculated, for example, every 5 ms, whereas in the energy saving mode, the posture of the lower leg part 104 is calculated, for example, every 100 ms. When the energy saving mode is entered, the calculation frequency decreases, and the error in the detection result of the angular velocity sensor 108 may become large. In such a case, it is very useful to be able to correct the posture of the lower leg part 104 using the detection result 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 part 104 while the knee joint 100 is being driven. The correction unit 122 may perform the correction every time the posture calculation unit 112 calculates the posture of the lower leg part 104, or may perform the correction when a predetermined condition is satisfied. The predetermined condition is when the system returns from the energy saving mode, and when a predetermined time has elapsed since the previous correction. Another predetermined condition is 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 previous correction. This is because in such a case, there is a high possibility that the amount of drift included 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 according to the time that has elapsed since the previous correction. The first and second weighting coefficients A and B are coefficients that determine how much of the corresponding multiplier is reflected in the correction result. If the ternion derived from the angular velocity sensor obtained by Formulas 16 to 18 is qg and the ternion derived from the angle sensor obtained by Formulas 23 to 25 is qa, the correction unit 122 can correct the calculation result of the posture of the lower leg 104 by the following Formula 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 of 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 correction is set as a specified condition such that a correction is performed when returning from an 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 was performed, 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 vibration occurs, the detection result of the angle sensor 110 contains a large amount of noise. On the other hand, the detection result of the angular velocity sensor 108 is not affected by vibrations, so it is considered that the noise is small in the short term. Therefore, for example, when correction is performed at short time intervals, it is better to emphasize the detection result of the angular velocity sensor 108 and perform correction so as to cancel the influence of drift of the angular velocity sensor 108 with 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 formula 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 of the observation value of the angle sensor 110 (i.e., sudden vibration). 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 formula 29, when the reliability of the angle sensor 110 is high, the value of the term (qa-qg) becomes small, and when the reliability of the angle sensor 110 is low, the value of the term (qa-qg) becomes large.
[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 shows the values qa and qg arranged in a norm space, with the vectors being the same in magnitude. As shown in FIG. 5, the correction value qh is obtained by moving 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 norm space, and its magnitude is smaller than the value 1. However, the output a(ax, ay, az) of the angle sensor 110 is normalized in advance and fixed at the value 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 stable at a value close to the value 1 (a value slightly smaller than the value 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 the correction, the attitude calculation unit 112 supplies uncorrected attitude information to the control unit 114 while the predetermined condition is not satisfied. Only when the predetermined condition is satisfied, the attitude calculation unit 112 corrects the attitude information and supplies the corrected attitude information to the control unit 114. Note that the attitude calculation unit 112 may calculate the attitude information and the 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 determine the attitude information to be used.
[0063] Next, a method of 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 considered that drift has occurred. In this case, the calibration unit 124 handles 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 can improve the accuracy of the detection results of the angular velocity sensor 108. In particular, with a specific device such as the thigh 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 is also possible to reduce the burden on the user.
[0065] Next, the process 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 portion 104 based on the posture information including the supplied ternion. The control unit 114 controls the amount of expansion and contraction of the drive assistance unit 106 based on the pitch angle p and roll angle r obtained in the following example.
[0066] A 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] If we convert Equation 33 into a ternion using Equation 12, we get the following Equations 34 to 36.
number
number
number
[0068] By rearranging Equation 36 with respect to p, we obtain Equation 37 below.
number
[0069] Furthermore, by rearranging Equations 34 and 35, we obtain Equation 38 below.
number
[0070] The control unit 114 controls the drive assistant unit 106 based on the results obtained from Equations 37 and 38.
[0071] Furthermore, for example, when a term including the value q0 exists 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 in which the squared term of dt is ignored 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] Fig. 6 shows a schematic diagram of a knee joint. As shown in Fig. 6, when the value q0 is large, the posture calculation unit 112 calculates the posture using a stationary coordinate system G1 (first coordinate system) in the lower hemisphere, and when the value q0 is small, the posture calculation unit 112 calculates the posture using a stationary coordinate system G2 (second coordinate system) obtained by converting the stationary coordinate system G1 in the upper hemisphere. When calculating the posture using the stationary coordinate system G1 and the stationary coordinate system G2, the direction of the rotation axis is maintained, and the calculation of the rotation angle is performed 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 the formulas 40 and 42, the following formula 45 is obtained.
number
[0076] Furthermore, by rearranging Equation 40 and Equation 43, we obtain Equation 46 below.
number
[0077] Furthermore, by rearranging Equation 40 and Equation 44, we obtain Equation 47 below.
number
[0078] By such coordinate conversion, 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. In order to simplify the illustration, FIG. 7 illustrates an example of performing correction every 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 formulas 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 formulas 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 (for example, formula 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 the output q112 of the posture calculation unit 112. In addition, the output q112 of element 1128 is fed back to element 1122 and used for the next calculation. In this case, the correction result is substituted into qg1 in Formula 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 multiplier deriving unit 202 supplies the first derivation unit 116 with an instruction to derive a ternion based on the detection result of the angular velocity sensor 108. The second multiplier deriving unit 204 supplies the second derivation unit 118 with an instruction to derive a ternion based on the detection result of the angle sensor 110. The calculation instructing unit 208 supplies the posture calculation unit 112 with an instruction to calculate the posture of the knee joint 100 based on the two obtained ternions. The instruction from the calculation instructing unit 208 includes an instruction to the correction unit 122 to correct the posture information according to a predetermined condition. The drive instructing unit 210 supplies the control unit 114 with an instruction to control the drive assist unit 106 based on the calculation result of the calculation instructing 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 complementarily correction on both detection results.
[0083] In addition, by using the first weighting coefficient A and the second weighting coefficient B, correction can be performed by weighting 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. This improves the accuracy of calculating the attitude.
[0084] Furthermore, when the specified conditions are not satisfied, the control unit 114 controls the drive assistance 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, and can be applied to devices in which ignoring rotation around the Z axis does not affect attitude calculation. The present invention can be applied to devices that do not require consideration of the direction of travel for attitude control, or rotating bodies in which ignoring rotation in the pitch direction does not affect attitude calculation. 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 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 thigh of a user 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 lower leg unit and limits or assists the rotational movement of the lower leg unit 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 an angle of the lower leg with respect to a stationary coordinate system; a posture calculation unit that calculates a posture of the lower leg based on a first multi-dimensional number derived based on a detection result of the angular velocity sensor and a second multi-dimensional number derived based on a detection result of the angle sensor and having the same number of terms as the first multi-dimensional number; a control unit that controls the drive assist unit based on a calculation result of the posture calculation unit.
2. The knee joint according to claim 1 , wherein the posture calculation unit includes a correction unit that corrects a calculation result of the posture of the lower leg by using the first multidimensional number and the second multidimensional number.
3. 3. The knee joint according to claim 2, wherein the correction unit 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, and the first weighting coefficient and the second weighting coefficient are changeable.
4. The correction unit repeatedly performs correction, The knee joint according to claim 3 , wherein the first weighting coefficient and the second weighting coefficient are changed according to an amount of time that has elapsed since a previous correction by the correction unit.
5. The attitude calculation unit is a first derivation unit that derives a quaternion based on a detection result of the angular velocity sensor and converts the quaternion into a ternion by setting an arbitrary term of the quaternion to 0; The knee joint according to any one of claims 1 to 4, further comprising: a second derivation unit that derives a ternion as the second multi-number based on a detection result of the angle sensor.
6. The knee joint according to any one of claims 2 to 5, wherein the control unit controls the drive assistant unit based on a calculation result corrected by the correction unit each time the control unit controls the drive assistant unit based only on the first multiplicity a predetermined number of times.
7. The knee joint according to claim 5, wherein when the control of the previous drive assist unit is based on a corrected calculation result, the first derivation unit derives the ternion using the calculation result used in the control of the previous drive assist unit.
8. The knee joint according to claim 1 , wherein the posture calculation unit calculates the posture by setting a value of a vertical axis direction in a stationary coordinate system to 0.
9. 9. The knee joint according to any one of claims 1 to 8, wherein the posture calculation unit calculates the posture using a first coordinate system when an angle of the lower leg part relative 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 part relative to the socket is equal to or greater than a predetermined angle.
10. 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.
11. An attitude calculation device that calculates the attitude of a moving body based on a first multiplier derived based on the detection result of an angular velocity sensor that detects the angular velocity of the moving body, and a second multiplier derived based on the 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.
12. 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; 12. The attitude calculation device according to claim 11, further comprising: a correction unit that corrects a calculation result of the attitude of the moving body by using the ternion derived by the first derivation unit and the ternion derived by the second derivation unit.
13. The attitude calculation device according to claim 11 , further comprising a calibration unit that calibrates the angular velocity sensor using a detection result of the angle sensor.
14. A thigh connection portion to which a socket corresponding to the thigh of a user 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 lower leg unit and limits or assists the rotational movement of the lower leg unit 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 an angle of the lower leg with respect to a stationary coordinate system; a posture calculation unit that calculates a posture of the lower leg based on a first multi-dimensional number derived based on a detection result of the angular velocity sensor and a second multi-dimensional number derived based on a detection result of the angle sensor and having the same number of terms as the first multi-dimensional number; A control unit that controls the drive assist unit based on a calculation result of the posture calculation unit, deriving a first multiplier based on a detection result of the angular velocity sensor, and 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; and controlling the drive assist unit based on the calculated posture of the knee joint.
15. A thigh connection portion to which a socket corresponding to the thigh of a user 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 lower leg unit and limits or assists the rotational movement of the lower leg unit 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 an angle of the lower leg with respect to a stationary coordinate system; a posture calculation unit that calculates a posture of the lower leg based on a first multi-dimensional number derived based on a detection result of the angular velocity sensor and a second multi-dimensional number derived based on a detection result of the angle sensor and having the same number of terms as the first multi-dimensional number; A control unit that controls the drive assist unit based on a calculation result of the posture calculation unit, a first multiplier derivation unit that causes the attitude calculation unit to derive a first multiplier based on a detection result of the angular velocity sensor; a second multiplier derivation unit that causes the attitude calculation unit to derive a second multiplier having a different number of terms from the first multiplier based on a detection result of the angle sensor; A calculation instruction unit that causes the posture calculation unit to calculate a posture of the knee joint based on the first multidimensional number and the second multidimensional number; A program for controlling a knee joint, comprising: a drive instruction unit that causes the control unit to control the drive assistance unit based on a calculation result of the calculation instruction unit.
16. 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 an angle of the moving body and having the same number of terms as the first multiplier, a first multiplier derivation unit that causes the attitude calculation device to derive a first multiplier based on a detection result of the angular velocity sensor; a second multiplier derivation unit that causes the attitude calculation device to derive a second multiplier based on a detection result of the angle sensor; a calculation instruction unit that causes the attitude calculation device to calculate the attitude of the moving body based on the first multivariate and the second multivariate,
Citation Information
Patent Citations
Attitude estimating device and method, attitude controlling device and method, and program
JP2011220825A
Hybrid terrain-adaptive lower limb system
JP2012501739A
Posture estimation device, posture estimation method and posture estimation program
JP2013054009A
Attitude angle calculation device, attitude angle calculation method, and program
JP2013104665A
Implementing a standing sequence using a prosthetic leg or lower limb orthosis.
JP2013503026A
