JP2005118466A - Controller for limb driving apparatus - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a controller for a limb driving apparatus by which an overload on a patient is evaded without deforming the posture of a limb even when the overload is generated by contracture, spasm, and muscle stress, etc., of a joint of the patient and a therapy can be continued. <P>SOLUTION: The controller includes: a target orbit setting part for generating a motion command so as to move the tip end of a driving part along a predetermined orbit; a tangent coordinate system generating means for calculating a tangential direction of the orbit from the output of the target orbit setting part, so as to derive a tangent coordinate system; a force direction control means for controlling the control coordinate system of force control so as to be soft in the tangential direction of the orbit and to be hard in a normal direction, based on the tangent coordinate system; and a therapy state presenting means for presenting a therapy state, based on sensing information of a position and angle sensor, the output of the target orbit setting part, and the output of the force direction control means. <P>COPYRIGHT: (C)2005,JPO&NCIPI

Description

The present invention relates to a limb body driving device, and more particularly, to a control device for a limb body driving device in consideration of a limb body trajectory so as to eliminate overload on human joints and other treatment targets and improve the safety of the device.

Conventionally, devices for moving a limb include a continuous passive motion device (CPM device) used in the medical field, a training device and a sports training device used in the rehabilitation field, and the like. In such a device, it can be moved while monitoring the position of the human limbs, can handle before the joint is overloaded, can easily set the operating range considering the limitations of the limbs, and does not put a load on the joint, the device itself In addition, there has been a control device for a limb body drive device (see, for example, Patent Document 1) that can perform treatment with safety.
Hereinafter, Patent Document 1 will be briefly described with reference to the drawings.
FIG. 12 is a block diagram showing a circuit configuration of the control device. In FIG. 12, 101 is an arm of a limb body drive device, 102 is a limb body, 103 is a splint (arm tip position) attached to the tip of the arm 101, 104 is a force sensor for detecting force and moment, and 105 drives the arm 101. Motor, 106 is a rotation angle detector for detecting the rotation angle of motor 105, 107 is an A / D (analog / digital) converter that converts force and moment signals of force sensor 104 into digital values, and 108 is an A / D converter. A displacement calculation processing unit that converts the output of the device into a displacement of the arm tip position 103, 109 an inverse kinematics calculation unit that calculates a target angle of the motor 105 from the arm tip position 103, and 110 sets a target trajectory of the arm tip position 103 A target trajectory setting unit 111, a rotation angle conversion circuit for converting the output of the rotation angle detection unit 106 into a digital value, 12 is a gain integrator, 113 is a D / A (digital / analog) converter that converts the output of the gain integrator 112 into an analog value, 114 is a servo amplifier that moves the drive motor 105 in accordance with the output of the D / A converter 113, 115 is a load estimation unit that estimates and calculates the motion load of the limbs, and 116 is an impedance constant conversion unit that changes the virtual spring constant according to the joint load.
In FIG. 12, the arm 101 includes three motors 105, and each motor includes a rotation angle detection unit 106. In FIG. 12, for the sake of simplicity, the rotation angle conversion circuit 111 and the servo amplifier 114 are depicted as being connected to only one of the three motors, but in reality, the rotation angle conversion circuit 111 is connected to all the motors. The servo amplifier 114 is also connected.
When the load on the limb 102 by the arm 101 enters the overload setting region, the impedance constant conversion unit 116 in a certain degree of freedom direction out of all the degrees of freedom possessed by the arm 101 increases the load on the limb 102. It is characterized by changing according to the degree of freedom and virtually freeing the motion in the direction of freedom.

JP 09-154900 A

However, the control device of the conventional limb body driving device frees the direction of the arbitrary degree of freedom in which the overload to the limb body has occurred. As the arm moves in the direction, the posture of the limbs collapses, resulting in a posture different from the desired treatment posture, and there is a problem that the treatment must be stopped at that time.
The present invention has been made in view of such a problem, and even when an overload occurs due to contracture, spasticity, muscle tone, etc. of the patient's joint, the patient is overloaded without the posture of the limbs collapsing. It is an object of the present invention to provide a control device for a limb body drive device that can avoid the above-described problem and can continue treatment.

In order to solve the above problem, the present invention is configured as follows.
The invention according to claim 1 includes a drive unit that is mounted on a limb and moves the limb, and the drive is performed by force control or position control based on sensing information of a position / angle sensor attached to the drive unit. In the control device of the limb body drive device that controls the operation of the unit, a target trajectory setting unit that generates an operation command so that the tip of the drive unit operates along a preset trajectory, and a target trajectory setting unit Based on the tangent coordinate system generation means for calculating the tangential direction of the trajectory from the output and deriving a tangential coordinate system, and based on the tangent coordinate system, so that it is soft in the tangential direction of the trajectory and hard in the normal direction A force direction control means for controlling a control coordinate system of force control, a treatment condition that presents a treatment state based on sensing information of the position / angle sensor, an output of the target trajectory setting unit, and an output of the force direction control means It is characterized in that a presentation unit.
Further, in the invention according to claim 2, the force direction control means is configured so that the tangential direction limiting force and the normal direction limiting force of the trajectory, and the direction perpendicular to the tangential direction and normal direction of the trajectory, respectively. A coordinate transformation matrix is calculated from a limiting force vector having a limiting force in each element, the tangential coordinate system, and a reference coordinate system of the limb driving apparatus, and the limiting force vector, the coordinate transformation matrix, and the Jacobian matrix The limiting torque of the drive unit is calculated from the product, and the torque is limited.
According to a third aspect of the present invention, the tangential coordinate system generating means outputs the output of the target trajectory setting unit closest to the current position (target target) from the output of the target trajectory setting unit and the position information of the drive unit. Point) is selected, the tangential direction of the trajectory is calculated based on the target point, and a tangential coordinate system is derived.
According to a fourth aspect of the present invention, the tangential coordinate system generating means outputs the output of the target trajectory setting unit closest to the current position (target target) from the output of the target trajectory setting unit and the position information of the drive unit. Point) is selected, and when the distance between them is equal to or greater than a predetermined value, warning information is output to the treatment state presenting means.
Further, the invention according to claim 5 is characterized in that the treatment state presentation means simultaneously presents a plurality of cycles of treatment state.
Further, in the invention according to claim 6, when the distance between the target point and the current position is larger than a predetermined threshold, the target trajectory setting unit determines that the operation direction of the tip of the driving unit is the target point. The operation command is generated so as to be in the opposite direction.
Further, the invention according to claim 7 is characterized in that the target trajectory setting unit stops the operation command when the time change of the distance between the target point and the current position is larger than a predetermined threshold value. It is what.

According to the control device for a limb body driving device according to claim 1, even if an overload occurs due to contraction, spasticity, muscle tone, etc. of the patient's joint, the posture of the limb body becomes free on a predetermined trajectory. Without overloading the patient, it is possible to avoid overloading the patient and continue treatment.
According to the control device for a limb body driving device according to claim 2, even when there is no force sensor information, when an overload occurs due to contraction, spasticity, muscle tension, etc. of the patient's joint, a preset trajectory Since it becomes free on the upper side, the posture of the limb does not collapse and it is possible to avoid overload on the patient and to continue treatment.
According to the control device for the limb body driving device according to claim 3, even when the feedback position deviates from the target point at the time when overload occurs, an appropriate target point is selected according to the time point and the tangential coordinate system is selected. Can be derived, and an appropriate force direction can be controlled.
According to the control device for a limb body driving device according to claim 4, when an unexpected load other than force due to contracture, spasticity, muscle tension, etc. of the patient's joint occurs, the device is promptly operated based on the warning information. It can be stopped, improving the safety of the device.
According to the control device for the limb body driving device of the fifth aspect, the occupational therapist can easily recognize the regular overload state, and a more appropriate treatment method can be selected.
According to the control device of the limb body driving device according to claim 6, since the operation direction is reversed on a desired trajectory without causing overload to the patient, the treatment can be continuously continued, and the treatment efficiency is improved. Improvements can be realized.
According to the control device for the limb body driving device according to claim 7, the device can be safely and promptly stopped when a sudden situation occurs that is different from contracture, spasticity, muscle tension, etc. that can continue normal treatment. It becomes.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.

In FIG. 1, the same reference numerals as those in FIG. Reference numeral 1 denotes a tangential coordinate system generating means for calculating the tangential direction of the trajectory from the output of the target trajectory setting unit 110 and the output of the rotation angle conversion circuit 112 and deriving a tangential coordinate system. 2 is a trajectory based on the tangential coordinate system. The force direction control means 3 controls the control coordinate system of the force control so that it is soft in the tangential direction and hard in the normal direction, and 3 outputs the output torque of the gain integrator 112 according to the output of the force direction control means. A torque limiter for limiting, 4 is a gravity / friction torque compensator for calculating and compensating the gravitational torque and friction torque of the driving device arm 101 from the rotation angle of the driving motor 105, and 5 is an output of the target trajectory setting unit 110 and the It is a treatment state presentation unit that presents a treatment state based on the output of the force direction control unit 2 and sensing information of the position / angle sensor.
As in FIG. 12, in FIG. 1, the arm 101 includes three motors 105, and each motor includes a rotation angle detection unit 106. Further, in FIG. 1, for the sake of simplicity, the rotation angle conversion circuit 111 and the servo amplifier 114 are depicted as being connected to only one of the three motors, but in reality, the rotation angle conversion circuit 111 is connected to all the motors. The servo amplifier 114 is also connected.

Next, the operation mode will be described. First, an operation form (normal form) in a state where the contracture, spasticity, and muscle tone of the patient's joint have not occurred will be described. FIG. 2 shows the arm tip position 103 at time T and the target trajectory before and after time T when the apparatus operates according to the target trajectory set to move the limb. ΣS represents the reference coordinate system.
Here, in order to simplify the description, a driving device that operates in the plane of the paper surface is considered. In FIG. 2, the posture of the limb with ΔT as the control cycle, the tangential direction of the target trajectory at time T (= ΔT · n: n is an integer of 0 or more) as the X axis, and the normal direction of the target trajectory as the Z axis. A coordinate system ΣW (n) representing is a tangential coordinate system.
FIG. 3 is a diagram showing a tangential coordinate system before and after time T. In FIG. The tangential coordinate system before one control cycle at time T is ΣW (n−1), and the tangential coordinate system after one control cycle at time T is ΣW (n + 1). In FIG. 3, ΣS represents the reference coordinate system as in FIG. The tangential coordinate system generation means 1 calculates the base vectors ex and Z in the X-axis direction of the tangential coordinate system ΣW (n) from the previous arm tip position P (n−1) and the current arm tip position P (n). An axial basis vector ez is calculated based on the following (Expression 1), (Expression 2), and (Expression 3) to generate a transformation matrix [ex ey ez] of a tangential coordinate system based on the reference coordinate system ΣS.

In (Formula 1), P (n-1) _X and P (n) _X indicate the coordinate positions in the X-axis direction based on the reference coordinate system ΣS of the arm tip positions P (n-1) and P (n), respectively. , P (n-1) _Z and P (n) _Z indicate the coordinate positions in the Z-axis direction based on the reference coordinate system ΣS of the arm tip positions P (n-1) and P (n), respectively.

Here, on the tangential coordinate system ΣW (n), the limiting force vector [limFx limFy limFz] The limFx (tangential direction) limiting force of ^T is set small and limFz (normal direction) is set large (the limiting force of limFy is also set large) To do).
The force direction control means 2 calculates the limit torque τlim of each motor of the arm 101 according to the processing flow shown in FIG. In step 2, the transformation matrix [ex ey ez] of the tangential coordinate system based on the reference coordinate system ΣS generated by the tangential coordinate system generating means 1 is read. Next, in step 3, the set limiting force vector [limFx limFy limFz] ^T is read. Next, in step 4, the limit torque τlim of each motor is calculated using a Jacobian matrix J representing the correspondence between the torque of each motor and the minute displacement of the reference coordinate system ΣS. When the limit torque τlim is output to the torque limiter 3, the generated torque is limited by the torque limiter 3 and the gravity / friction torque compensator 4, so that each motor is flexible with respect to loads exceeding the limit torque τlim. follow. In this case, since the direction of the limiting force at the arm tip position 103 is controlled to be flexible in the tangential direction of the target trajectory, the posture of the limb does not collapse.

Next, contracture, spasticity, and muscle tension of the patient related to claims 3 and 4 occur, and the deviation between the command position (indicating the next target position after the control cycle ΔT) and the feedback position is A mode of operation when it becomes larger will be described. FIG. 5A shows a schematic diagram of the trajectory when the feedback position is advanced from the command position, and FIG. 5B shows a schematic diagram of the trajectory when the feedback position is delayed from the command position. In the tangential coordinate system generating means 1, the restricted trajectory 1 and the restricted trajectory 2 shown in FIGS. 5A and 5B are set with a distance d across the target trajectory. The restricted trajectory 1 and the restricted trajectory 2 are trajectories that define the range of trajectory deviation that is allowed for safety. fp (1) ... fp (n) indicates the feedback position from the past to the present (n · ΔT), and tp (n), tp (n + 1) ... indicates the target position from the present to the future. Show. First, FIG. 5A shows a case where the feedback position is advanced from the command position, and FIG. 5B shows a case where the feedback position is delayed from the command position. The tangential coordinate system generating means 1 generates a transformation matrix [ex ey ez] of the tangential coordinate system based on the reference coordinate system ΣS according to the processing procedure shown in FIG. In step 2, the current feedback position (fp (n)) is input from the output of the rotation angle conversion circuit 111. In step 3, the current command value and m command values in the past and m in the future are input from the target trajectory setting unit 110 (m is an integer of 0 or more). In step 4, tp (k) that minimizes the distance | fp (n) −tp (k) | (k = n−m... N + m) is selected. In step 5, it is checked whether or not the distance R is within the range between the limit trajectory 1 and the limit trajectory 2 when the minimum distance in step 4 is R. In step 6, if it is outside the limit trajectory range, an alarm is output to the treatment state presentation means 5. In step 7, if within the limit trajectory range, a transformation matrix [ex ey ez] of a tangential coordinate system based on the reference coordinate system ΣS is calculated from tp (k) and tp (k + 1), and the force direction control means Output to 2. Subsequent operation forms are the same as the normal form, and thus description thereof is omitted.

Next, an operation mode according to claim 5 will be described. FIG. 7 shows three cycles of treatment status presented to the treatment status presentation means 5. In FIG. 7, time-series data of the X-direction and Z-direction positions based on the reference coordinate system ΣS of the arm tip position 103 in each operation cycle are displayed at the same time. It can be easily understood that the follow-up characteristic with respect to the target trajectory is deteriorated due to the reduction, and can be used as a reference for changing the setting of the limiting force or the like due to the difference in the follow-up characteristic.

Next, operation modes according to claims 6 and 7 will be described. FIG. 8 shows an operation locus of the sprint (arm tip position) 103 attached to the arm 101. P1 is the start position, and the reciprocating operation is normally repeated in the order of P1, P2, P3, P2, P1, P2, P3, P2, P1,. As shown in FIG. 9, at time t1 (corresponding to position P2 in FIG. 8), when the distance between the target position and the feedback position (hereinafter referred to as ΔERR) becomes larger than a preset threshold value Cr1, The target trajectory setting unit 110 switches the target position from P3 to P1, and the subsequent operation trajectory is continuously executed in the order of P1, P2, P1, P2, P1,. In addition, the target setting unit 110 can naturally set the point between P2 and P3 in a stepwise manner according to the state of treatment without setting the target point next to P1 as P2.
On the other hand, FIG. 10 shows a case where ΔERR at P2 changes more rapidly than normal time change at time t2. In this case, as described in claim 7, since the time change (ΔERR / Δt) of ΔERR shown in FIG. 11 is larger than a preset threshold Cr2, the target trajectory setting unit 110 does not generate an operation command. Stop the device.

  The present invention can be applied to a limb body driving device that considers the limb trajectory so as to eliminate overload on the patient and improve the safety of the device.

The figure which shows the basic composition of embodiment of this invention The figure which shows the whole relationship of the reference | standard coordinate system and tangent coordinate system of embodiment of this invention The figure which shows the relationship between the reference | standard coordinate system of embodiment of this invention, and a tangent coordinate system The figure which shows the processing flow of the force direction control means of embodiment of this invention. (A) The figure which shows the case where the feedback position advances from the command position in the embodiment of the present invention, (b) The figure which shows the case where the feedback position is delayed from the command position in the embodiment of the present invention. The figure which shows the processing flow of the tangent coordinate system production | generation means 1 of embodiment of this invention. The figure which shows the presentation content of the treatment status presentation means of Claim 5 of embodiment of this invention The figure which shows the implementation content of Claim 6 of embodiment of this invention The figure which shows the state detected with the threshold value of Claim 6 of embodiment of this invention The figure which shows the implementation content of Claim 7 of embodiment of this invention The figure which shows the state detected with the threshold value of Claim 7 of embodiment of this invention The figure which shows the basic composition of the conventional example

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Tangent coordinate system production | generation means 2 Force direction control means 3 Torque limiter 4 Gravity / friction compensator 5 Treatment state presentation means 101 Arm 102 of driving device Limb body 103 Sprint (arm tip position)
104 Force Sensor 105 Motor 106 Rotation Detection Unit 107 A / D Converter 108 Displacement Calculation Processing Unit 109 Inverse Kinematics Calculation Unit 110 Target Trajectory Setting Unit 111 Rotation Angle Conversion Circuit 112 Gain Integrator 113 D / A Converter 114 Servo Amplifier 115 Load estimator 116 Impedance constant converter

Claims (7)

A limb body drive device comprising a drive unit that is mounted on a limb and moves the limb body, and controls the operation of the drive unit by force control or position control based on sensing information of a position / angle sensor attached to the drive unit In the control device of
A target trajectory setting unit that generates an operation command so that the tip of the driving unit operates along a preset trajectory;
A tangential coordinate system generating means for calculating a tangential direction of the trajectory from the output of the target trajectory setting unit and deriving a tangential coordinate system;
Based on the tangential coordinate system, force direction control means for controlling the control coordinate system of force control so as to be soft in the tangential direction of the trajectory and hard in the normal direction;
A limb body driving device comprising: a treatment state presentation unit that presents a treatment state based on sensing information of the position / angle sensor, an output of the target trajectory setting unit, and an output of the force direction control unit. Control device. The force direction control means includes a limiting force vector having a limiting force in a direction perpendicular to the tangential direction and the normal direction of the trajectory, and a limiting force vector in each element perpendicular to the tangential direction and the normal direction of the trajectory. , Calculating a coordinate transformation matrix from the tangential coordinate system and a reference coordinate system of the limb driving device, and calculating a limiting torque of the driving unit from a product of the limiting force vector, the coordinate transformation matrix, and the Jacobian matrix, 2. The control device for a limb body driving device according to claim 1, wherein torque is limited. The tangential coordinate system generating means selects an output (target point) of the target trajectory setting unit closest to a current position from the output of the target trajectory setting unit and the position information of the drive unit, and based on the target point. The limb body driving device control device according to claim 1, wherein a tangential coordinate system is derived by calculating a tangential direction of the trajectory. The tangential coordinate system generation means selects the output (target point) of the target trajectory setting unit closest to the current position from the output of the target trajectory setting unit and the position information of the drive unit, and the distance between them is determined in advance. 4. The control device for a limb body driving device according to claim 1, wherein, when the value is equal to or more than a predetermined value, warning information is output to the treatment state presentation unit. 5. The control device for a limb body driving device according to claim 1, wherein the treatment state presentation unit simultaneously presents a plurality of cycles of treatment state. When the distance between the target point and the current position is greater than a predetermined threshold, the target trajectory setting unit generates an operation command so that the operation direction of the tip of the drive unit is opposite to the target point The control device for a limb body drive device according to claim 1, wherein the control device is a limb body drive device. The limb body drive according to any one of claims 1 to 6, wherein the target trajectory setting unit stops the operation command when the time change in the distance between the target point and the current position is larger than a predetermined threshold value. Control device for the device.

Images