Dynamic control algorithm and program for power-assisted lift device

[0002] 1. Field of the Invention

[0003] The present invention relates generally to power-assist devices. More particularly, the present invention relates to a method and system for dynamically controlling a power-assisted lift system to continuously reduce operator strain in a real-time mode.

[0004] 2. Discussion of the Related Art

[0005] In the automotive industry, lift devices are often employed in car assembly line stations to assist human operators with difficult tasks. These devices are most useful in stations requiring the lifting and manipulation of heavy loads. A typical device is primarily designed to balance the gravity of a load during lifting and travel around an assembly line station. The human operator, however, must still push or pull the device in order to move it horizontally for parts assembling. These actions require the operator to either accelerate or decelerate the load-carrying device each time a change in direction is desired. This directional change is particularly difficult when each major link of the device is large in mass and has significant moments of inertia which add to the amount of work to be done. To further aggravate the problem, a typical operation in a car assembly line will often be repeated in excess of 50 times per shift. This repetition has the potential to cause cumulative wrist or arm injury after consecutive months of work. Power-assisted lift devices were therefore developed to address the major concerns of ergonomics and human factors engineering.

[0006] Typical power-assisted approaches provide lift devices with four-axis motion. These devices are driven by servo-motors and guided by a closed-loop feedback of force data. In one system manufactured by FANUC Robotics, Inc., the force data are monitored and measured by a six-axis force sensor mounted behind the manual handle of the device. The current status of the feedback loop, however, is based only on the kinematics/statics relation between Cartesian positions/forces and joint positions/torques of the device. Thus, these systems have a noticeably slow response to operator-induced changes in direction. The slow response results in significant strain on operators any time a change in direction is attempted. It is therefore desirable to use joint data to provide a dynamic compensation within a substantially shorter response time.

[0007] The present invention provides a power-assisted lift system for assisting a human operator in manipulating objects. The lift system has a power-assist device that generates and measures joint data. The lift system also has a sensing module for converting a human-applied force into force data. The lift system further includes a dynamic control system for continuously reducing operator strain in a real-time mode based on the force data and the joint data.

[0008] The present invention also provides a dynamic control system for continuously reducing strain on a human operator of the power-assisted lift system, wherein the lift system has a plurality of joints. The control system has a statics formulator for determining a set of static torques for the lift system based on force data and joint data of the power-assist device. The control system further includes a dynamics formulator for determining a dynamic torque required for each joint of the power-assist device based on the joint data and static torques. The control system also includes a torque summation module for summing the dynamic torques with the static torques to determine torque data for each joint of the power-assist device. The torque summation module applies the torque data to the power-assist device to dynamically compensate human operation.

[0009] As an additional feature, the invention includes a computer implemented method for controlling a power-assist device. The method includes the step of retrieving force data from the power-assist device. The force data results from human operation of the power-assist device. The method further includes the step of retrieving joint data from the power-assist device. The method then compensates the human operation of the power-assist device based on the force data and the joint data.

[0010] Further objects, features and advantages of the invention will become apparent from a consideration of the following description and the appended claims when taken in connection with the accompanying drawings.

[0011] The present invention will become more fully understood from the detailed description and the accompanying drawings, wherein:

[0012] FIG. 1 is an illustration of a power-assist device in accordance with the present invention;

[0013] FIG. 2 is a block diagram of a power-assisted lift system using a dynamic control system in accordance with the present invention;

[0014] FIG. 3 is a detailed block diagram of an power-assisted lift system using a dynamic control system in accordance with the present invention; and

[0015] FIG. 4 is a flowchart of a computer-implemented method for controlling a power-assisted lift system in accordance with the present invention.

[0016] FIG. 1 is an illustration of a power-assist device 20 in accordance with the present invention. The present invention directed toward a dynamic control system for continuously reducing operator strain during operation of power-assist device 20 is best shown in FIG. 2 at 10. Generally, a power-assisted lift system 30 includes a power-assist device 20, a sensing module 31, and a dynamic control system 10 which can be readily implemented in robotic control systems commonly known in the art. Control for the lift system 30 is completely dynamic.

[0017] As shown in FIGS. 1 and 2, the power-assist device 20 aids the human operator 100 in manipulating objects of significant weight. It will be appreciated that the assist device 20 generates joint data 120 while the sensing module 31 converts forces resulting from human operation into force data 121. The dynamic control system 10 uses the force data 121 and the joint data 120 to continuously reduce strain on the human operator 100 in a real-time mode via torque data 122.

[0018] Specifically, the assist device 20 has a joint-servo controller 22 for converting torque data 122 from the dynamic control system 10 into motor control data. The assist device 20 has a plurality of joints and a servo motor manipulating each joint based on the motor control data. In the preferred embodiment, assist device 20 has four joints and is anchored to base 25. The motor control data is fed to the servo motors, and each servo motor in turn operates a corresponding joint. Operation of the joints reduces the amount of strain felt by the operator 100. The assist device 20 also has a joint data module 24 for generating joint data, wherein the joint data 120 includes joint position, joint velocity and computed joint acceleration. Joint accelerations are computed from the joint velocities and partial derivative inertial matrix to be described below. The joint data module 24 includes a joint encoder and a tachometer for monitoring, measuring, and retrieving the joint data 120 from the joints.

[0019] The assist device 20 performs several important functions such as relaying the applied force from the human operator 100 to the sensing module 31 via handle 25. The assist device 20 also provides joint data 120 from each joint to the dynamic control system 10 for dynamic compensation purposes.

[0020] Preferably, the sensing module 31 includes a six-axis force sensor coupled to a steering handle 25 of the lift device 21.

[0021] The dynamic control system 10 includes a statics formulator 12 for determining a set of static torques 123 based on force data 121. Dynamic control system 10 further includes a dynamics formulator 11 for determining a set of dynamic torques 124 based on the joint data 120 and the static torques 123 as adapted by a compensation module discussed in greater detail below. An individual static torque and dynamic torque is determined for each joint in the power-assist device 20. The dynamic control system 10 also has a torque summation module 13 for summing the dynamic torques 124 the static torques 123 to determine torque data 122 for each joint. The torque summation module 13 applies the torque data 122 to the lift system 20, and the lift system 20 applies the torque data to the servo motors to continuously reduce strain on the human operator 100 in a real-time mode.

[0022] Turning now to FIG. 3, the dynamic control system 10 is shown in greater detail. It will be appreciated that the dynamics formulator 11 includes an inertial matrix module 14 for modeling the inertial matrix W of the assist device. The dynamics formulator 11 further includes a partial differential inertial matrix module 15 for modeling a partial derivative of the inertial matrix W_dof the assist device 20. A dynamic torque calculator 16 then calculates the dynamic torques 124 from the joint accelerations, the inertial matrix W, and the partial differential inertial matrix W_d. A compensator module 19 is included within the statics formulator 12. Compensator module 19 uses the static torques 123 to further adapt the inertial matrix W and the Jacobian matrix. Modeling both the Jacobian matrix and the inertial matrix begins with knowledge of certain kinematic parameters. The Denavit-Hartenberg (D-H) kinematic parameter table of a power-assisted lift device is determined as follows:

[0023] For the dynamic model, the inertial matrix W is developed as follows: $W=\left(\begin{array}{cccc}{w}_{11}& w_{21}& w_{31}& W_{51}\\ w_{21}& w_{22}& w_{32}& w_{52}\\ w_{31}& w_{32}& w_{33}& w_{53}\\ w_{51}& w_{52}& w_{53}& w_{55}\end{array}\right)$

[0024] where and hereafter $\{\begin{array}{c}{w}_{11}=m_1+m_2+m_3+m_4+m_5,\\ w_{22}=m_3{l}_3^2{c}_3^2+m_4{a}_3^2{c}_3^2+{m_5\left(b_5+a_3{c}_3{s}_5\right)}^2+m_5{a}_3^2{c}_3^2{c}_5^2+I_{\mathrm{z2}}+I_{\mathrm{y3}}+I_{\mathrm{y5}},\\ w_{33}=m_3{l}_3^2+m_4{a}_3^2{c}_3^2+m_5{a}_3^2{c}_3^2+I_{\mathrm{z3}}+I_{\mathrm{z4}},\\ w_{55}=m_5{b}_5^2+I_{\mathrm{y5}},\\ w_{21}=-m_3{l}_3{c}_2{c}_3-m_4{a}_3{c}_2{c}_3+m_5\left(b_5+a_3{c}_3{s}_5\right)s_{25}+m_5{a}_3{c}_3{c}_5{c}_{25},\\ w_{31}=-m_3{l}_3{s}_2{s}_3,\\ w_{32}=0,\\ w_{51}=m_5{b}_5{s}_{25}\\ w_{52}=m_5{b}_5\left(b_5+a_3{c}_3{s}_5\right)+I_{y_5},\\ w_{53}=0.\end{array}$  s_i=sin θ_i, c_i=cos θifor i=2,3,5 and s₂₅=sin(θ₂+θ₅) and c₂₅=cos(θ₂+θ₅).

[0025] The Dynamics Formulation is based on

[0026] τ_d=W{umlaut over (q)}+(W_d^T−½W_d){dot over (q)}+τ_g,

[0027] where τ_g=−∂P/∂q is the joint torque due to gravity, and $W_d=\left(\begin{array}{c}{\stackrel{.}{q}}^T\frac{\partial W}{{\partial }_{q_1}}\\ ⋮\\ {\stackrel{.}{q}}^T\frac{\partial W}{{\partial }_{q_4}}\end{array}\right).$

[0028] Once again, it is important to note that the joint data 120 includes information such as joint position, joint velocity, and the computed joint acceleration for each joint in the assist device 20.

[0029] The statics formulator 12 includes a Jacobian matrix module 17 for modeling the Jacobian matrix for the assist device 20. The statics formulator 12 also includes a static torque calculator 18 for calculating the static torques 123 from the Jacobian matrix and the measured Cartesian force.

[0030] The Jacobian matrix is found to be $J=\left(\begin{array}{cccc}0& 0& -a_3{c}_3& 0\\ 0& a_3{s}_2{c}_3+d_6{c}_{25}& a_3{c}_{2s}{s}_3& d_6{c}_{25}\\ 1& a_3{c}_2{c}_3-d_6{s}_{25}& -a_3{s}_{2s}{s}_3& -d_6{s}_{25}\\ 0& 1& 0& 1\end{array}\right).$

[0031] This is based on the joint position vector defined by q=(d₁θ₂θ₃θ₅)^Tand the output $\{\begin{array}{c}x=-d_2-a_3{s}_3-d_5\\ y=a_1-a_3{c}_2{c}_3+d_6{s}_{25}\\ z=d_1+a_3{s}_2{c}_3+d_6{c}_{25}\\ \phi ={\theta }_2+{\theta }_5.\end{array}$

[0032] The Statics Formulation is

[0033] τ_s=J^TF.

[0034] Returning to FIGS. 1 and 2, it can be seen that in operation a human operator 100 manipulates the power-assist device 20 via handle 25. The present invention envisions a computer-implemented method for controlling the power-assist device 20 as shown in FIG. 4 for programming purposes. The method includes the steps 200 and 210 of obtaining force data 121 and joint data 120 from the assist device 20. The method further includes the step 224 of compensating human operation of the assist device based on the force data 121 and joint data 120. A decisional loop is provided at step 201 to determine whether the force is going to zero. Compensation effectively involves the cancellation of human input along any combination of six axes. The relevant axes are the standard X,Y and Z Cartesian forces as well as torque about each axis. As the operator 100 applies various forces to the handle 25, the present invention performs the above calculations to minimize strain of the operator 100. Thus, the method includes the steps 221, 222, and 223 of determining static torque, determining dynamic torque, and generating torque data, respectively.

[0035] It is to be understood that the invention is not limited to the exact construction illustrated and described above, but that various changes and modifications may be made without departing from the spirit and the scope of the invention as defined in the following claims.