WO2022244441A1 - マニピュレータ - Google Patents

マニピュレータ Download PDF

Info

Publication number
WO2022244441A1
WO2022244441A1 PCT/JP2022/012503 JP2022012503W WO2022244441A1 WO 2022244441 A1 WO2022244441 A1 WO 2022244441A1 JP 2022012503 W JP2022012503 W JP 2022012503W WO 2022244441 A1 WO2022244441 A1 WO 2022244441A1
Authority
WO
WIPO (PCT)
Prior art keywords
vibration
end effector
arm
duration
control
Prior art date
Application number
PCT/JP2022/012503
Other languages
English (en)
French (fr)
Inventor
箕浦康祐
吉原康二
名和政道
Original Assignee
株式会社豊田自動織機
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 株式会社豊田自動織機 filed Critical 株式会社豊田自動織機
Publication of WO2022244441A1 publication Critical patent/WO2022244441A1/ja

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B11/00—Automatic controllers
    • G05B11/01—Automatic controllers electric
    • G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential

Definitions

  • the present invention relates to manipulators.
  • the robot disclosed in Patent Document 1 includes an angular velocity sensor as an angle sensor and an inertial sensor. After a certain period of time has passed since the angular velocity of the control command value became 0 and the target stop position was reached, control using the control command value and the angle information is performed without using the angular velocity information.
  • a manipulator for solving the above problems comprises an arm whose links are connected via a joint, whose proximal end is fixed to a base member, and whose end effector provided at its distal end can be moved to a target position by the operation of the joint; an actuator for operating a joint to move the end effector to a target position; a position sensor for detecting the operation of the actuator; an acceleration sensor provided on either the arm or the end effector; and a position command.
  • the actuator is driven based on the above to perform feedback control to move the end effector to a target position while feeding back the output value of the position sensor, and the vibration of the end effector is controlled based on the output value of the acceleration sensor.
  • control unit that performs vibration suppression control to suppress vibration
  • control unit has a time setting unit that sets a duration time of the vibration suppression control
  • time setting unit includes the end effector
  • the gist is to determine the duration according to the vibration frequency information of the.
  • the duration is determined according to the vibration frequency information of the end effector. It is possible to prevent deterioration of driving efficiency.
  • the vibration frequency information may be posture information of the arm. Further, in the manipulator, the vibration frequency information may be load information applied to the end effector.
  • the vibration frequency information may be a parameter used for feedforward control when moving the end effector to the target position.
  • the vibration frequency information may be the vibration frequency of the end effector obtained from the detection value of the acceleration sensor.
  • deterioration of the drive efficiency of the actuator can be prevented while suppressing residual vibration.
  • FIG. 1 is a schematic configuration diagram of a manipulator in an embodiment
  • FIG. FIG. 2 is a block diagram showing the electrical configuration of the manipulator
  • Functional block diagram of the controller Schematic configuration diagram of a manipulator.
  • (a) is a time chart showing a position command
  • (b) is a time chart showing acceleration
  • (c) is a time chart showing use/non-use of an acceleration sensor in the embodiment
  • (d) is a use/non-use of an acceleration sensor in a comparative example. Time chart showing non-use. Map explanatory drawing for demonstrating another example.
  • (a) is a time chart showing a position command for explaining another example
  • (b) is a time chart showing an acceleration sensor output for explaining another example.
  • the manipulator 10 includes an arm 20, a plurality of actuators 30 for operating joints, a plurality of position sensors 40 for detecting the motion of the actuators 30, and an acceleration sensor 50. , a plurality of controllers 60 as control units, and a storage unit 80 .
  • the horizontal direction is indicated by the x and y directions
  • the vertical direction is indicated by the z direction.
  • the arm 20 has links 21a, 21b, 21c, 21d, 21e, and 21f connected via joints 22a, 22b, 22c, 22d, and 22e.
  • a joint 22a is provided on the distal end side of the rod-shaped link 21a on the base end side, and the joint 22a and the joint 22b are connected by a rod-shaped link 21b.
  • a rod-like link 21c connects the joint 22b and the joint 22c.
  • a rod-shaped link 21d connects the joint 22c and the joint 22d.
  • a rod-shaped link 21e connects the joint 22d and the joint 22e.
  • a rod-like link 21f on the tip side is connected to the joint 22e.
  • the proximal end of the link 21a is fixed to the base member 24.
  • the base end of the arm 20 is fixed to the base member 24 .
  • An end effector (hand effector) 23 is attached to the tip of the link 21f.
  • the end effector 23 provided at the tip of the arm 20 can be moved to a target position by the motions of the joints 22a, 22b, 22c, 22d, and 22e.
  • An actuator 30, a position sensor 40 and a controller 60 are provided for each joint (22a, 22b, 22c, 22d, 22e).
  • the joint 22a is provided with an actuator 30a, a position sensor 40a, and a controller 60a.
  • the joint 22b is provided with an actuator 30b, a position sensor 40b and a controller 60b.
  • the joint 22c is provided with an actuator 30c, a position sensor 40c and a controller 60c.
  • the joint 22d is provided with an actuator 30d, a position sensor 40d and a controller 60d.
  • the joint 22e is provided with an actuator 30e, a position sensor 40e and a controller 60e. In FIG. 2, only two actuators 30a and 30e are shown for actuator 30, only two position sensors 40a and 40e are shown for position sensor 40, and only two controllers 60a and 60e are shown for controller 60.
  • FIG. 2 only two actuators 30a and 30e are shown for actuator 30, only two position sensors 40a and 40e are shown for position sensor 40
  • Actuators 30 provided at joints 22a, 22b, 22c, 22d, and 22e of arm 20 operate joints 22a, 22b, 22c, 22d, and 22e of arm 20 to move end effector 23 to a target position. belongs to.
  • Each actuator 30 is composed of, for example, a joint motor.
  • the end effector 23 is, for example, a gripping device, and can grip and convey the work W.
  • Each position sensor 40 detects the motion of the corresponding actuator 30 .
  • Each position sensor 40 can detect, for example, the rotation angle of the motor as the operation of the actuator 30, thereby making it possible to detect the joint angle.
  • Joint angles ⁇ 1 to ⁇ 5 are illustrated on the xz plane in FIG.
  • the acceleration sensor 50 is provided on the end effector 23 .
  • the acceleration sensor 50 is for detecting vibration of the end effector 23 .
  • Each controller 60 is composed of a microcomputer.
  • a microcomputer is composed of a CPU that decodes commands and executes data processing, a memory that stores programs and data, external sensors, an input/output unit that controls transmission and reception of data with corresponding actuators, and the like.
  • each controller 60 is connected to a host computer 100, which is a host device.
  • a position command is sent from the host computer 100 to each controller 60 .
  • the position command includes not only the position command itself sent from the host computer 100 but also the position command through the filter.
  • a corresponding actuator 30 is connected to each controller 60 .
  • Each controller 60 issues a command to the corresponding actuator 30 to operate the joints 22a, 22b, 22c, 22d, and 22e of the arm 20 to move the end effector 23 to the target position.
  • a corresponding position sensor 40 is connected to each controller 60 .
  • Each controller 60 receives the output value of the corresponding position sensor 40 and can detect the operation of the corresponding actuator 30 .
  • An acceleration sensor 50 is connected to each controller 60 . The output value of the acceleration sensor 50 is inputted to each controller 60 so that the vibration of the end effector 23 can be detected.
  • Each controller 60 drives the corresponding actuator 30 based on the position command, feeds back the output value of the corresponding position sensor 40, and performs feedback control to move the end effector 23 to the target position.
  • Each controller 60 performs vibration suppression control based on the output value of the acceleration sensor 50 to suppress vibration of the end effector 23 .
  • each controller 60 includes a subtraction unit 61, a subtraction unit 62, an opening/closing unit 63, an opening/closing control unit 64, a detected vibration calculation unit 65, a vibration suppression control unit 66, a position control unit 67, and a speed control unit 68. and a speed estimator 69 .
  • the P-PI cascade structure of position control and speed control is shown in this example, similar effects can be expected from PID control and I-PD control that do not have a speed estimator or speed controller.
  • a drive command value for the actuator which is a position command
  • An output value of the position sensor 40 is input to the subtractor 61 .
  • the subtraction unit 61 outputs the difference between the position command (drive command value for the actuator) and the actual position, which is the output value of the position sensor 40 .
  • the position control section 67 receives the output of the subtraction section 61 and outputs a control signal ⁇ ref.
  • the detected vibration calculator 65 receives the actual acceleration, which is the output value of the acceleration sensor 50, performs coordinate conversion processing according to the posture, and outputs the detected vibration.
  • the detected vibration calculated here is the vibration in the tangential direction viewed from the rotating shaft of the actuator that performs vibration suppression control. In FIG. 1, the direction of the detected vibration when performing vibration suppression control with the actuator in the joint 22c is the tangential direction of the rotation axis of the actuator in the joint 22c.
  • the vibration suppression controller 66 in FIG. 3 receives the detected vibration and outputs the control signal ⁇ acc.
  • the speed estimator 69 receives the output value of the position sensor 40 and outputs the actuator or arm speed ⁇ m.
  • the subtraction unit 62 receives the control signal ⁇ ref, the control signal ⁇ acc, and the speed ⁇ m.
  • the subtraction unit 62 outputs the difference between the control signal ⁇ acc with respect to the control signal ⁇ ref and the speed ⁇ m.
  • the output of the subtractor 62 is input to the speed controller 68, and the speed controller 68 outputs a drive command.
  • the output of the speed control unit 68 is sent to the actuator (joint motor) 30 as a drive command, and the actuator (joint motor) 30 is driven based on this drive command.
  • An opening/closing section 63 is provided on the signal transmission line from the detected vibration calculation section 65 to the vibration suppression control section 66 .
  • the opening/closing portion 63 is opened/closed by the opening/closing control portion 64 .
  • the opening/closing control unit 64 receives the position command and the output value of the detected vibration calculation unit 65 .
  • the opening/closing control section 64 opens and closes the opening/closing section 63 based on the position command and the output value of the detected vibration calculation section 65 .
  • Each controller 60 detects the joint angle from the drive command value to the actuator or the actual measurement value of the position sensor 40 .
  • the controllers 60 detect each joint angle through mutual communication.
  • Each controller 60 can detect the posture information of the arm 20 (extension amount of the arm 20, which will be described later) from each joint angle.
  • each controller 60 has a time setting section 70 that sets the duration Tacc of the vibration suppression control.
  • the time setting section 70 can determine the duration Tacc according to the vibration frequency information of the end effector 23 .
  • the vibration frequency information is posture information of the arm 20 obtained from each joint angle.
  • the posture information of the arm 20 is the extension amount of the arm 20 .
  • FIG. 1 shows the contracted state in which the arm 20 is relatively contracted
  • FIG. 4 shows the maximum extended state in which the arm 20 is extended to the maximum.
  • the map shown in FIG. 5 is stored in the storage unit 80 in FIG.
  • the vertical axis represents the arm posture information, that is, the amount of extension of the arm 20, and the horizontal axis represents the duration Tacc of the vibration suppression control.
  • a map characteristic line Lm1 is set in FIG. 5, and the map characteristic line Lm1 is a characteristic line in which the duration Tacc increases as the extension amount of the arm 20 increases.
  • the controller 60 can use the map (map characteristic line Lm1) of FIG. 5 to calculate the duration Tacc corresponding to the arm posture information (the amount of extension of the arm 20).
  • the posture information of the arm 20 is not limited to the extension amount of the arm 20, and any posture information that affects the vibration of the end effector 23 can be used.
  • the OFF condition of the opening/closing portion 63 which is the output of the opening/closing control portion 64 in FIG. is determined by a map with input. If the frequency of the residual vibration is low, it will take time to converge, and if the frequency is high, the time to converge will be short. Since the frequency of the residual vibration depends on the posture of the arm 20 (extension amount of the arm 20), the time until the damping control by the acceleration sensor ends is determined based on this. Specifically, since the frequency of the residual vibration increases as the arm 20 shrinks, the duration Tacc of the vibration suppression control is shortened. On the other hand, the longer the arm 20 extends, the lower the frequency of the residual vibration, so the duration Tacc of the vibration suppression control is lengthened.
  • FIG. 6(a) shows a position command.
  • FIG. 6B shows output values of the detected vibration calculator 65.
  • FIG. 6(c) shows a command to open/close the opening/closing portion 63, that is, to use or not to use the output value of the detected vibration calculating portion 65 for control.
  • FIG. 6D shows a command to open/close the opening/closing portion 63 in the comparative example, that is, to use or not to use the output value of the detected vibration calculating portion 65 for control.
  • t1 is the operation start timing
  • t2 is the operation stop timing.
  • the time setting unit 70 in one of the controllers 60 provided for each joint uses the map (map characteristic line Lm1) in FIG. amount) is calculated. Therefore, the duration Tacc is a variable that changes according to the arm posture.
  • the controller 60 starts vibration suppression control at the operation stop timing t2. Then, the vibration suppression control ends at timing t3 when the duration Tacc has elapsed from the operation stop timing t2.
  • the duration time is a fixed value
  • the vibration suppression control using the acceleration sensor is ended when a certain time elapses from the operation stop command, for example.
  • the time required for the residual vibration to subside after stopping differs.
  • the constant time (fixed value) is determined under the conditions that take the longest time for the residual vibration to converge, under other conditions, the acceleration sensor is used even after the residual vibration subsides. Therefore, damping control will be implemented. Therefore, an extra motor current is output to drive the actuator, which may lead to deterioration in power efficiency.
  • the duration Tacc changes according to the arm posture as shown in FIG. shorter than Therefore, deterioration of power efficiency can be prevented.
  • t1 is the operation start timing
  • t2 is the operation stop timing.
  • the time setting unit 70 in one of the controllers 60 provided for each joint uses the map (map characteristic line Lm1) in FIG. 5 to calculate the duration Tacc corresponding to the arm posture.
  • the controller 60 starts vibration suppression control at timing t2 as shown in FIGS. 7(a) to 7(c). Then, the vibration suppression control ends at timing t10 when the duration Tacc has elapsed from the operation stop timing t2.
  • the duration is a fixed value as in FIG. 6(d).
  • the condition that determines the duration time is not the condition that takes the longest time to converge the residual vibration, depending on the operating conditions, after the end of the vibration suppression control (elapse of the duration time), There is a risk that the vibration cannot be converged and the vibration cannot be suppressed.
  • the duration Tacc changes according to the arm posture and is longer than the comparative example shown in FIG. 7(d). Therefore, residual vibration at the time of stopping can be suppressed.
  • the damping control may end too late, but in the present embodiment of FIG. Terminate damping control and do not output unnecessary motor current. As a result, deterioration of power efficiency can be prevented.
  • the structure of the manipulator 10 is such that the links 21a to 21f are connected via the joints 22a to 22e, the proximal end is fixed to the base member 24, and the end effector 23 provided at the distal end moves the target by the operation of the joints 22a to 22e. It has an arm 20 that can move into position.
  • the manipulator 10 includes an actuator 30 for operating the joints 22a-22e to move the end effector 23 to a target position.
  • the manipulator 10 includes a position sensor 40 that detects the motion of the actuator 30 and an acceleration sensor 50 provided on the end effector 23 .
  • the manipulator 10 has a controller 60 as a control section.
  • the controller 60 drives the actuator 30 based on the position command, performs feedback control to move the end effector 23 to a target position while feeding back the output value of the position sensor 40, and controls the end effector 23 based on the output value of the acceleration sensor 50. Vibration suppression control for suppressing the vibration of 23 is performed.
  • the controller 60 has a time setting section 70 that sets the duration Tacc of the vibration suppression control, and the time setting section 70 determines the duration Tacc according to the vibration frequency information of the end effector 23 . Therefore, when setting the duration Tacc of the vibration suppression control, the time setting unit 70 of the controller 60 determines the duration Tacc according to the vibration frequency information of the end effector 23, thereby suppressing the residual vibration and controlling the actuator. It is possible to prevent deterioration of the driving efficiency of
  • Vibration frequency information is posture information of the arm 20, so it is practical. Embodiments are not limited to the above, and may be embodied as follows, for example. - The vibration frequency information may be load information applied to the end effector 23 . Specifically, each controller 60 acquires information on the work W through communication with the host computer 100 and detects the work mass m as load information applied to the end effector 23 . A map shown in FIG. 8 is stored in the storage unit 80 . In FIG. 8, the vertical axis represents the workpiece mass m, and the horizontal axis represents the duration Tacc of the vibration suppression control. A map characteristic line Lm2 is set in FIG. 8, and the map characteristic line Lm2 is a characteristic line in which the duration Tacc increases as the work mass m increases. The controller 60 can use the map (map characteristic line Lm2) in FIG. 8 to calculate the duration Tacc corresponding to the workpiece mass m.
  • the duration Tacc is determined using a map with the load information applied to the end effector 23 as input. If the frequency of the residual vibration is low, it will take time to converge, and if the frequency is high, the time to converge will be short. Since the frequency of the residual vibration is determined by the load applied to the end effector 23, which is the load on the hand, the time until the damping control ends is determined based on this. Depending on the load applied to the end effector 23, the time required for the residual vibration to subside after stopping differs. As in the comparative example shown in Fig.
  • the vibration may not be damped.
  • the duration Tacc varies according to the load applied to the end effector 23 . Therefore, as shown in FIG. 7(c), residual vibration at the time of stopping can be suppressed.
  • the damping control may end too late, but in the present example of FIG. To end the vibration control, do not output unnecessary motor current. As a result, deterioration of power efficiency can be prevented.
  • the damping control ends too quickly, there is a possibility that the vibration cannot be completely damped.
  • FIG. 7(c) since damping control is performed until the damping is completed, convergence within the damping target acceleration range is fast. Therefore, it is possible to suppress the residual vibration at the time of stopping and to prevent deterioration of the drive power efficiency of the actuator.
  • the duration Tacc may be determined using the posture of the arm 20 and the load information applied to the end effector 23 as the vibration frequency information. At least one of such load information may be used. Specifically, the continuation time Tacc is determined by a map in which at least one of the posture of the arm 20 and load information on the end effector 23 is input.
  • the duration of vibration suppression control is calculated using a map based on the posture of the arm 20 and the load information applied to the end effector 23. may be calculated.
  • the vibration frequency information is a parameter used for feedforward control when moving the end effector 23 to the target position. A specific description will be given with reference to FIGS. 9 and 10. FIG.
  • the controller 60 has a feedforward control section 90 and an addition section 91.
  • a feedforward control unit 90 inputs a position command.
  • a feedforward control unit 90 outputs a drive command using a model formula.
  • the model formula is a formula for finding the required torque in the absence of disturbance when the arm 20 is assumed to be an ideal rigid body when attempting to move the arm 20 in response to a position command.
  • the adder 91 adds the drive command output from the speed control unit 68 and the drive command output from the feedforward control unit 90 and outputs the result.
  • the output of the adder 91 is sent to the actuator (joint motor) 30 as a drive command, and the actuator (joint motor) 30 is driven based on this drive command.
  • FIG. 10 shows an example of a model formula used for feedforward control.
  • a two-inertia model is used as a model formula.
  • the two-inertia model is a physical model representing the vibration of the joints of the arm, and particularly uses a spring constant, moment of inertia, and coefficient of viscosity.
  • the model formula is a physical material mechanics formula from the output torque of the motor to the actual position of the hand of the arm, and the formula from the torque to the position of the hand is calculated by creating a physical equation of motion.
  • ⁇ m is the motor angle
  • ⁇ l is the load side angle
  • Accl is the acceleration
  • ⁇ m is the actual torque
  • Irms* is the current command value.
  • Jm is the moment of inertia of the motor
  • Jl is the moment of inertia of the load side
  • Ng is the reduction ratio
  • K0 is the spring constant of the motor bearing
  • D0 is the viscosity coefficient of the motor bearing
  • K1 is the spring constant of the gear
  • D1 is the viscosity coefficient of the gear
  • Kt is The torque constant, L1, is the link length.
  • feedforward control is generally used for arm control to improve responsiveness.
  • Feedforward control can improve responsiveness while suppressing vibration by modeling a controlled object in advance and storing parameters.
  • the frequency of the residual vibration is estimated based on the parameters used for this feedforward control.
  • the frequency f that is, the resonance frequency is calculated by the following formula (1).
  • K1, Jm and Jl are as follows.
  • the vibration frequency information is the gear spring constant K1, the motor moment of inertia Jm, and the load-side moment of inertia Jl, which are parameters used for feedforward control when moving the end effector 23 to the target position.
  • the vibration frequency information is the vibration frequency f of the end effector 23 obtained from the detection value of the acceleration sensor 50 .
  • the vibration frequency f is calculated from the period T between the center level of the vibration detected by the acceleration sensor 50 and the crossing timing, and from the vibration frequency f A duration Tacc may be determined.
  • the detection value of the acceleration sensor installed in the end effector is recorded and the frequency of the residual vibration is directly measured. Then, the frequency of the residual vibration is obtained, and the duration until the end of the vibration suppression control is determined by referring to the previously mapped relationship between the frequency of the residual vibration and the duration Tacc of the vibration suppression control.
  • the merit of directly calculating the frequency of the detected vibration and determining the duration Tacc based on the frequency When obtaining the amplitude of the detected vibration to determine the duration time Tacc of the vibration suppression control, the minimum and maximum values of the amplitude are calculated. It may go away.
  • the duration is obtained from the vibration frequency by detecting the time between the cross timings without detecting the amplitude, so that it is less susceptible to noise. That is, by measuring the frequency of the output of the acceleration sensor and obtaining the duration Tacc based on the frequency instead of using the output of the acceleration sensor as it is, the disadvantage of obtaining the amplitude can be eliminated.
  • the frequency of the residual vibration is obtained based on the method of [Part 1] or [Part 2], and by referring to the relationship between the residual vibration frequency and the duration that has been mapped in advance, vibration suppression Determines the time to end control.
  • the vibration frequency information is at least the vibration frequency determined from the parameters used for feedforward control when the arm posture and the load information on the end effector and the end effector is moved to the target position, and the detection value of the acceleration sensor.
  • the vibration frequency information is at least the vibration frequency determined from the parameters used for feedforward control when the arm posture and the load information on the end effector and the end effector is moved to the target position, and the detection value of the acceleration sensor.
  • the acceleration sensor 50 is provided on the end effector 23 , the acceleration sensor 50 may be provided at any position on the arm 20 . In this case, the acceleration of the end effector 23 is estimated from the acceleration of the arm 20 at any position.
  • the duration Tacc of the vibration suppression control was determined using the map showing the relationship between the vibration frequency information and the duration Tacc, a functional expression may be used instead. Specifically, the duration Tacc of the vibration suppression control may be determined using a relational expression indicating the relationship between the vibration frequency information and the duration Tacc.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Human Computer Interaction (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Feedback Control In General (AREA)

Abstract

アーム（２０）と、アクチュエータ（３０）と、位置センサ（４０）と、加速度センサ（５０）と、コントローラ（６０）を備える。コントローラ（６０）は、位置指令に基づいてアクチュエータ（３０）を駆動して位置センサ（４０）の出力値をフィードバックしつつエンドエフェクタを目標の位置に移動させるフィードバック制御を行うとともに加速度センサ（５０）の出力値に基づいてエンドエフェクタの振動を抑制する振動抑制制御を行う。コントローラ（６０）は、振動抑制制御の継続時間を設定する時間設定部（７０）を有しており、時間設定部（７０）はエンドエフェクタの振動周波数情報に応じて継続時間を決定する。

Description

マニピュレータ
　本発明は、マニピュレータに関するものである。
　特許文献１に開示のロボットにおいては、角度センサ及び慣性センサとしての角速度センサを備える。制御指令値の角速度が０になって目的の停止位置に到達した時点から一定時間が経過すると、角速度情報を用いることなく制御指令値及び角度情報を用いた制御を実施する。
特開２０１０－２８４７７０号公報
　ところで、ロボットアームの手先の振動を抑制するために加速度（慣性）センサを用いて制御する場合に、動作停止指令から一定時間経過時に加速度センサを用いた振動抑制制御を終了すると、以下の課題がある。一定時間が残留振動収束時間よりも長いと、アクチュエータ駆動のために余分な動力を使うことになり、効率悪化を招く懸念がある。一方、一定時間が残留振動収束時間よりも短いと、振動を収束できない懸念がある。
　上記課題を解決するためのマニピュレータは、リンクが関節を介して繋がり、基端がベース部材に固定され、先端に設けたエンドエフェクタが前記関節の動作により目標の位置に移動可能なアームと、前記関節を動作させて前記エンドエフェクタを目標の位置に移動させるためのアクチュエータと、前記アクチュエータの動作を検出する位置センサと、前記アームまたは前記エンドエフェクタのいずれかに設けられた加速度センサと、位置指令に基づいて前記アクチュエータを駆動して前記位置センサの出力値をフィードバックしつつ前記エンドエフェクタを目標の位置に移動させるフィードバック制御を行うとともに、前記加速度センサの出力値に基づいて前記エンドエフェクタの振動を抑制する振動抑制制御を行う制御部と、を備えるマニピュレータであって、前記制御部は、前記振動抑制制御の継続時間を設定する時間設定部を有しており、前記時間設定部は前記エンドエフェクタの振動周波数情報に応じて前記継続時間を決定することを要旨とする。
　これによれば、制御部の時間設定部が、振動抑制制御の継続時間を設定する際に、エンドエフェクタの振動周波数情報に応じて継続時間を決定することにより、残留振動を抑制しつつアクチュエータの駆動効率悪化を防止することができる。
　また、マニピュレータにおいて、前記振動周波数情報は、前記アームの姿勢情報であるとよい。
　また、マニピュレータにおいて、前記振動周波数情報は、前記エンドエフェクタにかかる負荷情報であるとよい。
　また、マニピュレータにおいて、前記振動周波数情報は、前記エンドエフェクタを目標の位置に移動させる際のフィードフォワード制御に用いるパラメータであるとよい。
　また、マニピュレータにおいて、前記振動周波数情報は、前記加速度センサの検出値から求めた前記エンドエフェクタの振動周波数であるとよい。
　本発明によれば、残留振動を抑制しつつアクチュエータの駆動効率悪化を防止することができる。
実施形態におけるマニピュレータの概略構成図。 マニピュレータの電気的構成を示すブロック図。 コントローラの機能ブロック図。 マニピュレータの概略構成図。 マップ説明図。 （ａ）は位置指令を示すタイムチャート、（ｂ）は加速度を示すタイムチャート、（ｃ）は実施形態における加速度センサ使用／不使用を示すタイムチャート、（ｄ）は比較例における加速度センサ使用／不使用を示すタイムチャート。 （ａ）は位置指令を示すタイムチャート、（ｂ）は加速度を示すタイムチャート、（ｃ）は実施形態における加速度センサ使用／不使用を示すタイムチャート、（ｄ）は比較例における加速度センサ使用／不使用を示すタイムチャート。 別例を説明するためのマップ説明図。 別例を説明するためのコントローラの機能ブロック図。 別例を説明するためのアームの２慣性モデル図。 （ａ）は別例を説明するための位置指令を示すタイムチャート、（ｂ）は別例を説明するための加速度センサ出力を示すタイムチャート。
　以下、本発明を具体化した一実施形態を図面に従って説明する。
　図１、図２に示すように、マニピュレータ１０は、アーム２０と、関節を動作させるための複数のアクチュエータ３０と、アクチュエータ３０の動作を検出するための複数の位置センサ４０と、加速度センサ５０と、制御部としての複数のコントローラ６０と、記憶部８０と、を備える。図１において、水平方向をｘ，ｙ方向で示すとともに、上下方向をｚ方向としている。
　図１に示すように、アーム２０は、リンク２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅ，２１ｆが関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅを介して繋がっている。詳しくは、基端側の棒状のリンク２１ａの先端側には関節２２ａが設けられ、関節２２ａと関節２２ｂとの間が棒状のリンク２１ｂで連結されている。関節２２ｂと関節２２ｃとの間が棒状のリンク２１ｃで連結されている。関節２２ｃと関節２２ｄとの間が棒状のリンク２１ｄで連結されている。関節２２ｄと関節２２ｅとの間が棒状のリンク２１ｅで連結されている。関節２２ｅには先端側の棒状のリンク２１ｆが繋がっている。
　リンク２１ａの基端がベース部材２４に固定されている。これにより、アーム２０は、基端がベース部材２４に固定されている。リンク２１ｆの先端にはエンドエフェクタ（手先効果器）２３が取り付けられている。これにより、アーム２０は、先端に設けたエンドエフェクタ２３が関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅの動作により目標の位置に移動可能である。
　関節（２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅ）毎に、アクチュエータ３０、位置センサ４０及びコントローラ６０が設けられている。具体的には、関節２２ａに、アクチュエータ３０ａ、位置センサ４０ａ及びコントローラ６０ａが設けられている。関節２２ｂに、アクチュエータ３０ｂ、位置センサ４０ｂ及びコントローラ６０ｂが設けられている。関節２２ｃに、アクチュエータ３０ｃ、位置センサ４０ｃ及びコントローラ６０ｃが設けられている。関節２２ｄに、アクチュエータ３０ｄ、位置センサ４０ｄ及びコントローラ６０ｄが設けられている。関節２２ｅに、アクチュエータ３０ｅ、位置センサ４０ｅ及びコントローラ６０ｅが設けられている。図２においては、アクチュエータ３０については２つのアクチュエータ３０ａ，３０ｅのみを示し、位置センサ４０については２つの位置センサ４０ａ，４０ｅのみを示し、コントローラ６０については２つのコントローラ６０ａ，６０ｅのみを示す。
　アーム２０の関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅに設けられた各アクチュエータ３０は、アーム２０の関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅを動作させてエンドエフェクタ２３を目標の位置に移動させるためのものである。各アクチュエータ３０は、例えば、関節モータで構成されている。エンドエフェクタ２３は、例えば、把持機器であり、ワークＷを把持して搬送できる。
　各位置センサ４０は、対応するアクチュエータ３０の動作を検出する。各位置センサ４０により、例えば、アクチュエータ３０の動作としてモータの回転角を検出でき、これにより関節角度を検知することが可能となる。図１におけるｘ－ｚ平面において関節角度θ１～θ５を例示している。
　加速度センサ５０は、エンドエフェクタ２３に設けられている。加速度センサ５０は、エンドエフェクタ２３の振動を検出するためのものである。
　各コントローラ６０は、それぞれ、マイクロコンピュータで構成されている。マイクロコンピュータは、命令を解読しデータ処理を実行するＣＰＵ、プログラムやデータを保存するメモリ、外部のセンサ、対応するアクチュエータとのデータの送受信を制御する入出力部等から構成されている。
　図２に示すように、各コントローラ６０には上位機器であるホストコンピュータ１００が接続されている。ホストコンピュータ１００から各コントローラ６０に位置指令が送られてくる。なお、位置指令とは、ホストコンピュータ１００から送られてくる位置指令そのものだけでなくフィルタを介した位置指令も含む。各コントローラ６０には、対応するアクチュエータ３０が接続されている。各コントローラ６０は、対応するアクチュエータ３０に指令を出してアーム２０の関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅを動作させてエンドエフェクタ２３を目標の位置に移動させる。
　各コントローラ６０には、対応する位置センサ４０が接続されている。各コントローラ６０には、対応する位置センサ４０の出力値が入力され、対応するアクチュエータ３０の動作を検知することができる。加速度センサ５０が各コントローラ６０に接続されている。各コントローラ６０には加速度センサ５０の出力値が入力されてエンドエフェクタ２３の振動を検知することができる。
　各コントローラ６０は、位置指令に基づいて、対応するアクチュエータ３０を駆動して、対応する位置センサ４０の出力値をフィードバックしつつエンドエフェクタ２３を目標の位置に移動させるフィードバック制御を行う。各コントローラ６０は、エンドエフェクタ２３の振動を抑制すべく加速度センサ５０の出力値に基づいて振動抑制制御を行う。
　図３に示すように、各コントローラ６０は、減算部６１と減算部６２と開閉部６３と開閉制御部６４と検出振動算出部６５と振動抑制制御部６６と位置制御部６７と速度制御部６８と速度推定部６９を有する。また、本例では位置制御－速度制御のＰ－ＰＩカスケード構造を示しているが、速度推定部や速度制御部を有さないＰＩＤ制御やＩ－ＰＤ制御も同様の効果を期待できる。
　位置指令であるアクチュエータへの駆動指令値が減算部６１に入力される。位置センサ４０の出力値が減算部６１に入力される。減算部６１において、位置指令（アクチュエータへの駆動指令値）と位置センサ４０の出力値である実位置の差が出力される。
　位置制御部６７において、減算部６１の出力が入力され、制御信号ωｒｅｆが出力される。
　検出振動算出部６５において、加速度センサ５０の出力値である実加速度が入力され、姿勢に応じた座標変換処理がなされ、検出振動が出力される。ここで算出される検出振動は、振動抑制制御を行うアクチュエータの回転軸から見た接線方向の振動である。図１において、関節２２ｃ内のアクチュエータで振動抑制制御を実施する際の検出振動の方向は、関節２２ｃ内のアクチュエータの回転軸の接線方向となる。
　図３の振動抑制制御部６６において、検出振動が入力され、制御信号ωａｃｃが出力される。
　速度推定部６９において、位置センサ４０の出力値が入力されて、アクチュエータないしはアームの速度ωｍが出力される。
　減算部６２において、制御信号ωｒｅｆと、制御信号ωａｃｃと、速度ωｍが入力される。減算部６２において、制御信号ωｒｅｆに対する制御信号ωａｃｃと、速度ωｍの差が出力される。
　減算部６２の出力が速度制御部６８に入力され、速度制御部６８は駆動指令を出力する。
　速度制御部６８の出力がアクチュエータ（関節モータ）３０に駆動指令として送られ、この駆動指令に基づいてアクチュエータ（関節モータ）３０が駆動する。
　検出振動算出部６５から振動抑制制御部６６への信号伝達ライン上には開閉部６３が設けられている。開閉部６３は開閉制御部６４により開閉される。開閉制御部６４には、位置指令が入力されるとともに検出振動算出部６５の出力値が入力される。開閉制御部６４は、位置指令と検出振動算出部６５の出力値に基づいて開閉部６３を開閉する。
　各コントローラ６０は、アクチュエータへの駆動指令値あるいは位置センサ４０の実測値から関節角度を検知する。コントローラ６０同士は相互通信により各関節角度を検知する。各コントローラ６０は、各関節角度からアーム２０の姿勢情報（後述するアーム２０の伸び量）を検知することができるようになっている。
　図２に示すように、各コントローラ６０は、振動抑制制御の継続時間Ｔａｃｃを設定する時間設定部７０を有する。時間設定部７０は、エンドエフェクタ２３の振動周波数情報に応じて継続時間Ｔａｃｃを決定することができるようになっている。
　振動周波数情報は、各関節角度から求められるアーム２０の姿勢情報である。具体的には、アーム２０の姿勢情報とは、アーム２０の伸び量である。例えば図１がアーム２０を比較的縮めた縮小状態であり、図４がアーム２０を最大に伸ばした最大伸長状態である。図２の記憶部８０には図５に示すマップが記憶されている。図５において縦軸にはアーム姿勢情報、すなわちアーム２０の伸び量をとり、横軸には振動抑制制御の継続時間Ｔａｃｃをとっている。図５においてマップ特性線Ｌｍ１が設定されており、マップ特性線Ｌｍ１はアーム２０の伸び量が増えるほど継続時間Ｔａｃｃが長くなる特性線である。コントローラ６０は、図５のマップ（マップ特性線Ｌｍ１）を用いて、アーム姿勢情報（アーム２０の伸び量）に対応する継続時間Ｔａｃｃを算出することができるようになっている。なお、アーム２０の姿勢情報としては、アーム２０の伸び量に限らず、エンドエフェクタ２３の振動に影響する任意の姿勢情報を採用し得る。
　図３の開閉制御部６４の出力である開閉部６３のオフ条件は、閉じた後の振動抑制制御の継続時間Ｔａｃｃであり、振動抑制制御の継続時間Ｔａｃｃの決定方法として、アーム２０の姿勢情報を入力としたマップにより決定する。残留振動の周波数が低ければ収束までに時間がかかり、周波数が高ければ収束までの時間は短くなる。残留振動の周波数はアーム２０の姿勢（アーム２０の伸び量）に依存するため、これを元に加速度センサによる制振制御を終了するまでの時間を決定する。具体的には、アーム２０が縮むほど残留振動の周波数が高くなるので、振動抑制制御の継続時間Ｔａｃｃが短くされる。一方、アーム２０が伸びるほど残留振動の周波数が低くなるので、振動抑制制御の継続時間Ｔａｃｃが長くされる。
　＜作用＞
　次に、作用について説明する。
　図６（ａ）には位置指令を示す。図６（ｂ）には検出振動算出部６５の出力値を示す。図６（ｃ）には開閉部６３の開閉、即ち、検出振動算出部６５の出力値を制御に使用する、あるいは、不使用とする指令を示す。図６（ｄ）には比較例における開閉部６３の開閉、即ち、検出振動算出部６５の出力値を制御に使用する、あるいは、不使用とする指令を示す。
　図６（ａ）においてｔ１が動作開始タイミングであり、ｔ２が動作停止タイミングである。
　関節毎に設けられたコントローラ６０のうちのいずれかのコントローラ６０における時間設定部７０は、図５のマップ（マップ特性線Ｌｍ１）を用いて、動作停止タイミングｔ２におけるアーム姿勢情報（アーム２０の伸び量）に対応する継続時間Ｔａｃｃを算出する。よって、継続時間Ｔａｃｃは、アーム姿勢に応じて変わる変数である。
　コントローラ６０は、動作停止タイミングｔ２のタイミングで振動抑制制御を開始する。そして、動作停止タイミングｔ２から、継続時間Ｔａｃｃが経過したｔ３のタイミングにおいて振動抑制制御を終了する。
　図６（ｄ）に示す比較例においては、継続時間は固定値であり、例えば動作停止指令から一定時間経過時に加速度センサを用いた振動抑制制御を終了する。
　アーム２０の姿勢によって停止後の残留振動が収まるまでの時間が異なる。図６（ｄ）に示す比較例のように残留振動の収束までに最も時間のかかる条件で一定時間（固定値）を決定した場合、その他の条件では残留振動が収まった後も加速度センサを用いた制振制御を実施することになる。そのため、アクチュエータ駆動のために余分なモータ電流を出力し、電力効率の悪化につながるおそれがある。
　本実施形態では例えば図１に示すようにアーム２０を比較的縮めた状態において図６（ｃ）に示すように、継続時間Ｔａｃｃは、アーム姿勢に応じて変わり、図６（ｄ）の比較例よりも短くされる。そのため、電力効率の悪化を防止することができる。
　アーム２０の姿勢が図４に示すようにアーム２０を最大に伸ばした状態である場合には、図７（ａ）、図７（ｂ）、図７（ｃ）のようになる。
　図７（ａ）においてｔ１が動作開始タイミングであり、ｔ２が動作停止タイミングである。
　関節毎に設けられたコントローラ６０のうちのいずれかのコントローラ６０における時間設定部７０は、図５のマップ（マップ特性線Ｌｍ１）を用いて、アーム姿勢に対応する継続時間Ｔａｃｃを算出する。
　コントローラ６０は、図７（ａ）～図７（ｃ）に示すようにｔ２のタイミングで振動抑制制御を開始する。そして、動作停止タイミングｔ２から、継続時間Ｔａｃｃが経過したｔ１０のタイミングにおいて振動抑制制御を終了する。
　図７（ｄ）に示す比較例においては、図６（ｄ）と同様に継続時間は固定値である。
　図７（ｄ）に示す比較例において、継続時間を決定する条件が残留振動の収束までに最も時間のかかる条件でなかった場合、動作条件によっては振動抑制制御終了（継続時間の経過）後、振動を収束できずに振動を制振しきれないおそれがある。
　図７（ｃ）に示す実施形態においては、継続時間Ｔａｃｃは、アーム姿勢に応じて変わり、図７（ｄ）の比較例よりも長くされる。そのため、停止時の残留振動を抑制することができる。
　このようにして、図６（ｄ）の比較例では、制振制御の終了が遅すぎる場合が生じ得るが、図６（ｃ）の本実施形態では、制振完了後、速やかに加速度センサによる制振制御を終了するため、無駄なモータ電流を出力しない。これにより、電力効率悪化を防止できる。
　また、図７（ｄ）の比較例では、制振制御の終了が速すぎる場合においては振動を制振しきれないおそれがある。図７（ｃ）の本実施形態では、制振完了まで加速度センサによる制振制御を行っているので、制振目標加速度範囲内への収束が速い。
　よって、停止時の残留振動を抑制し、かつ、アクチュエータの駆動電力効率の悪化を防ぐことができる。
　＜実施形態の効果＞
　上記実施形態によれば、以下のような効果を得ることができる。
　（１）マニピュレータ１０の構成として、リンク２１ａ～２１ｆが関節２２ａ～２２ｅを介して繋がり、基端がベース部材２４に固定され、先端に設けたエンドエフェクタ２３が関節２２ａ～２２ｅの動作により目標の位置に移動可能なアーム２０を備える。マニピュレータ１０は、関節２２ａ～２２ｅを動作させてエンドエフェクタ２３を目標の位置に移動させるためのアクチュエータ３０を備える。マニピュレータ１０は、アクチュエータ３０の動作を検出する位置センサ４０と、エンドエフェクタ２３に設けられた加速度センサ５０を備える。マニピュレータ１０は、制御部としてのコントローラ６０を備える。コントローラ６０は位置指令に基づいてアクチュエータ３０を駆動して位置センサ４０の出力値をフィードバックしつつエンドエフェクタ２３を目標の位置に移動させるフィードバック制御を行うとともに加速度センサ５０の出力値に基づいてエンドエフェクタ２３の振動を抑制する振動抑制制御を行う。コントローラ６０は、振動抑制制御の継続時間Ｔａｃｃを設定する時間設定部７０を有しており、時間設定部７０はエンドエフェクタ２３の振動周波数情報に応じて継続時間Ｔａｃｃを決定する。よって、コントローラ６０の時間設定部７０は、振動抑制制御の継続時間Ｔａｃｃを設定する際に、エンドエフェクタ２３の振動周波数情報に応じて継続時間Ｔａｃｃを決定することにより、残留振動を抑制しつつアクチュエータの駆動効率悪化を防止することができる。
　（２）振動周波数情報は、アーム２０の姿勢情報であるので、実用的である。
　実施形態は前記に限定されるものではなく、例えば、次のように具体化してもよい。
　・振動周波数情報は、エンドエフェクタ２３にかかる負荷情報であってもよい。具体的には、各コントローラ６０はホストコンピュータ１００との通信によりワークＷの情報を取得して、エンドエフェクタ２３にかかる負荷情報としてワーク質量ｍを検知する。記憶部８０には図８に示すマップが記憶されている。図８において縦軸にはワーク質量ｍをとり、横軸には振動抑制制御の継続時間Ｔａｃｃをとっている。図８においてマップ特性線Ｌｍ２が設定されており、マップ特性線Ｌｍ２はワーク質量ｍが大きいほど継続時間Ｔａｃｃが長くなる特性線である。コントローラ６０は、図８のマップ（マップ特性線Ｌｍ２）を用いて、ワーク質量ｍに対応する継続時間Ｔａｃｃを算出することができる。
　振動抑制制御の継続時間Ｔａｃｃの決定方法として、エンドエフェクタ２３にかかる負荷情報を入力としたマップにより、継続時間Ｔａｃｃを決定する。残留振動の周波数が低ければ収束までに時間がかかり、周波数が高ければ収束までの時間は短くなる。残留振動の周波数は手先の負荷であるエンドエフェクタ２３にかかる負荷により決定されるため、これを元に制振制御を終了するまでの時間を決定する。エンドエフェクタ２３にかかる負荷によって停止後の残留振動が収まるまでの時間が異なる。図６（ｄ）に示す比較例のように残留振動の収束までに最も時間のかかる条件で一定時間（固定値）を決定した場合、その他の条件では残留振動が収まった後も加速度センサを用いた制振制御を実施することになる。そのため、余分なモータ電流を出力し、電力効率の悪化につながるおそれがある。本例においては、継続時間Ｔａｃｃは、エンドエフェクタ２３にかかる負荷に応じて変わるため、図６（ｃ）のように電力効率の悪化を防止することができる。
　図７（ｄ）に示す比較例のように継続時間を決定する条件が残留振動の収束までに最も時間のかかる条件でなかった場合、動作条件によっては制振制御終了（継続時間の経過）後、振動を制振しきれないおそれがある。本例においては継続時間Ｔａｃｃは、エンドエフェクタ２３にかかる負荷に応じて変わる。そのため、図７（ｃ）のように停止時の残留振動を抑制することができる。
　このようにして、図６（ｄ）の比較例では、制振制御の終了が遅すぎる場合が生じ得るが、図６（ｃ）の本例では、制振完了後、速やかに加速度センサによる制振制御を終了するため、無駄なモータ電流を出力しない。これにより、電力効率悪化を防止できる。また、図７（ｄ）の比較例では、制振制御の終了が速すぎる場合においては振動を制振しきれないおそれがある。図７（ｃ）の本例では、制振完了まで制振制御を行っているので、制振目標加速度範囲内への収束が速い。よって、停止時の残留振動を抑制し、かつ、アクチュエータの駆動電力効率の悪化を防ぐことができる。
　なお、振動周波数情報として、アーム２０の姿勢及びエンドエフェクタ２３にかかる負荷情報を用いて、継続時間Ｔａｃｃを決定してもよく、要は、振動周波数情報は、アーム２０の姿勢及びエンドエフェクタ２３にかかる負荷情報の少なくとも一方であればよい。具体的には、アーム２０の姿勢及びエンドエフェクタ２３にかかる負荷情報の少なくとも一方を入力としたマップにより継続時間Ｔａｃｃを決定する。
　・上述したようにアーム２０の姿勢、エンドエフェクタ２３にかかる負荷情報に基づいたマップで振動抑制制御の継続時間を算出したが、エンドエフェクタ２３における振動の周波数を推定して振動抑制制御の継続時間を算出してもよい。
　予めエンドエフェクタ２３における残留振動の周波数と振動抑制制御の継続時間Ｔａｃｃの関係をマップ化しておき、下記の［その１］または［その２］のように残留振動の周波数を推定する。
　［その１］
　振動周波数情報は、エンドエフェクタ２３を目標の位置に移動させる際のフィードフォワード制御に用いるパラメータである。図９、図１０を用いて具体的に説明する。
　図３に代わる図９において、コントローラ６０は、フィードフォワード制御部９０と加算部９１を有する。フィードフォワード制御部９０は、位置指令を入力する。フィードフォワード制御部９０は、モデル式を用いて駆動指令を出力する。モデル式は、位置指令に応じてアーム２０を動かそうとした場合にアーム２０が理想の剛体であると仮定したときにおいて外乱が無い状態で必要トルクを求めるための式である。加算部９１は、速度制御部６８の出力である駆動指令と、フィードフォワード制御部９０の出力である駆動指令とを加算して出力する。加算部９１の出力がアクチュエータ（関節モータ）３０に駆動指令として送られ、この駆動指令に基づいてアクチュエータ（関節モータ）３０が駆動する。
　ここで、フィードフォワード制御に使用されるモデル式の一例を、図１０に示す。モデル式として、２慣性モデルを用いている。
　２慣性モデルは、アームの関節の振動を物理モデルで表したものであり、特に、バネ定数、慣性モーメント、粘性係数を用いている。モデル式はモータの出力トルクから実際のアームの手先の位置までの物理的な材料力学の式であり、トルクから手先の位置までの式を、物理的に運動方程式を作って計算している。
　図１０において、θｍがモータ角度、θｌが負荷側角度、Ａｃｃｌが加速度、τｍが実トルク、Ｉｒｍｓ＊が電流指令値である。Ｊｍがモータ慣性モーメント、Ｊｌが負荷側慣性モーメント、Ｎｇが減速比、Ｋ０がモータ軸受のバネ定数、Ｄ０がモータ軸受の粘性係数、Ｋ１がギアのバネ定数、Ｄ１がギアの粘性係数、Ｋｔがトルク定数、Ｌ１がリンク長である。
　このように、アームの制御には一般的に応答性向上のため、フィードフォワード制御が用いられる。フィードフォワード制御は、予め制御対象をモデル化してパラメータを記憶しておくことで、振動を抑制しつつ応答性を向上させることができる。
　このフィードフォワード制御に用いているパラメータを元に、残留振動の周波数を推定する。例えば、下記式（１）により周波数ｆ、即ち、共振周波数を算出する。
Figure JPOXMLDOC01-appb-M000001
 　上記式（１）において、Ｋ１、Ｊｍ、Ｊｌは、以下のとおりである。
　Ｋ１：ギアのバネ定数
　Ｊｍ：モータ慣性モーメント
　Ｊｌ：負荷側慣性モーメント
　このようにして残留振動の周波数ｆを取得し、予めマップ化しておいた残留振動の周波数と振動抑制制御の継続時間Ｔａｃｃの関係を参照することで、振動抑制制御を終了するまでの継続時間を決定する。
　このように、振動周波数情報は、エンドエフェクタ２３を目標の位置に移動させる際のフィードフォワード制御に用いるパラメータであるギアのバネ定数Ｋ１、モータ慣性モーメントＪｍ、負荷側慣性モーメントＪｌである。
　［その２］
　振動周波数情報は、加速度センサ５０の検出値から求めたエンドエフェクタ２３の振動周波数ｆである。
　例えば、図１１（ａ）、図１１（ｂ）に示すように、加速度センサ５０による検出振動における振動の中心レベルとクロスするタイミング間の周期Ｔから振動周波数ｆを算出して、振動周波数ｆから継続時間Ｔａｃｃを決定してもよい。加速度センサの検出値を元に推定する際に、エンドエフェクタに設置されている加速度センサの検出値を記録し、残留振動の周波数を直接計測する。そして、残留振動の周波数を取得し、予めマップ化しておいた残留振動の周波数と振動抑制制御の継続時間Ｔａｃｃの関係を参照することで、振動抑制制御を終了するまでの継続時間を決定する。
　検出振動の周波数を直接計算して周波数に基づいて継続時間Ｔａｃｃを求めることによるメリットについて言及する。
　振動抑制制御の継続時間Ｔａｃｃを決定するために検出振動の振幅を求める場合においては、振幅の最小値と最大値を算出するが、この際、ノイズを検出していまい、例えば無駄な制振を行ってしまう可能性がある。これに対し、［その２］では、振幅を検出せずにクロスタイミング間の時間検出により振動周波数から継続時間を求めることによりノイズの影響を受けにくい。即ち、加速度センサ出力をそのままではなく周波数を計測して周波数に基づいて継続時間Ｔａｃｃを求めることにより振幅を求める際のデメリットを無くすることができる。
　このようにして、［その１］または［その２］の手法を元に残留振動の周波数を取得し、予めマップ化しておいた残留振動の周波数と継続時間の関係を参照することで、振動抑制制御を終了するまでの時間を決定する。
　広義には、振動周波数情報は、アームの姿勢及びエンドエフェクタにかかる負荷情報及びエンドエフェクタを目標の位置に移動させる際のフィードフォワード制御に用いるパラメータ及び加速度センサの検出値から求めた振動周波数の少なくとも一つであればよい。
　・加速度センサ５０は、エンドエフェクタ２３に設けたが、加速度センサ５０は、アーム２０の任意の位置に設けられていてもよい。この場合は、アーム２０の任意の位置における加速度からエンドエフェクタ２３の加速度を推定する。
　・振動周波数情報と継続時間Ｔａｃｃとの関係を示すマップを用いて振動抑制制御の継続時間Ｔａｃｃを決定したが、これに代わり関数式を用いてもよい。詳しくは、振動周波数情報と継続時間Ｔａｃｃとの関係を示す関係式を用いて振動抑制制御の継続時間Ｔａｃｃを決定してもよい。
　１０　　マニピュレータ
　２０　　アーム
　２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅ，２１ｆ　　リンク
　２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅ　　関節
　２３　　エンドエフェクタ
　２４　　ベース部材
　３０　　アクチュエータ
　４０　　位置センサ
　５０　　加速度センサ
　６０　　コントローラ
　７０　　時間設定部

Claims (5)

  1. 　リンクが関節を介して繋がり、基端がベース部材に固定され、先端に設けたエンドエフェクタが前記関節の動作により目標の位置に移動可能なアームと、
    　前記関節を動作させて前記エンドエフェクタを目標の位置に移動させるためのアクチュエータと、
    　前記アクチュエータの動作を検出する位置センサと、
    　前記アームまたは前記エンドエフェクタのいずれかに設けられた加速度センサと、
    　位置指令に基づいて前記アクチュエータを駆動して前記位置センサの出力値をフィードバックしつつ前記エンドエフェクタを目標の位置に移動させるフィードバック制御を行うとともに、前記加速度センサの出力値に基づいて前記エンドエフェクタの振動を抑制する振動抑制制御を行う制御部と、
    を備えるマニピュレータであって、
    　前記制御部は、前記振動抑制制御の継続時間を設定する時間設定部を有しており、前記時間設定部は前記エンドエフェクタの振動周波数情報に応じて前記継続時間を決定する
    ことを特徴とするマニピュレータ。
  2. 　前記振動周波数情報は、前記アームの姿勢情報である
    ことを特徴とする請求項１に記載のマニピュレータ。
  3. 　前記振動周波数情報は、前記エンドエフェクタにかかる負荷情報である
    ことを特徴とする請求項１に記載のマニピュレータ。
  4. 　前記振動周波数情報は、前記エンドエフェクタを目標の位置に移動させる際のフィードフォワード制御に用いるパラメータである
    ことを特徴とする請求項１に記載のマニピュレータ。
  5. 　前記振動周波数情報は、前記加速度センサの検出値から求めた前記エンドエフェクタの振動周波数である
    ことを特徴とする請求項１に記載のマニピュレータ。
PCT/JP2022/012503 2021-05-18 2022-03-18 マニピュレータ WO2022244441A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2021083992A JP2022177607A (ja) 2021-05-18 2021-05-18 マニピュレータ
JP2021-083992 2021-05-18

Publications (1)

Publication Number Publication Date
WO2022244441A1 true WO2022244441A1 (ja) 2022-11-24

Family

ID=84141184

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2022/012503 WO2022244441A1 (ja) 2021-05-18 2022-03-18 マニピュレータ

Country Status (2)

Country Link
JP (1) JP2022177607A (ja)
WO (1) WO2022244441A1 (ja)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6234788A (ja) * 1985-08-02 1987-02-14 日立建機株式会社 柔構造作業機
JPH01173117A (ja) * 1987-12-28 1989-07-07 Mitsubishi Heavy Ind Ltd 柔軟構造物の振動制御装置
JPH06138950A (ja) * 1992-10-22 1994-05-20 Toyoda Mach Works Ltd フィードバック制御方式のロボット制御装置
JPH10235582A (ja) * 1997-02-26 1998-09-08 Yaskawa Electric Corp ロボットのウィービング制御装置
JP2015231663A (ja) * 2014-06-10 2015-12-24 シーメンス アクチエンゲゼルシヤフトＳｉｅｍｅｎｓ Ａｋｔｉｅｎｇｅｓｅｌｌｓｃｈａｆｔ 多数の要素システムを制御調節する装置および方法
JP2016083713A (ja) * 2014-10-24 2016-05-19 キヤノン株式会社 ロボット制御方法、ロボット装置、プログラム、記録媒体及び組立部品の製造方法
JP2018171666A (ja) * 2017-03-31 2018-11-08 セイコーエプソン株式会社 制御装置、ロボット、およびロボットシステム

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6234788A (ja) * 1985-08-02 1987-02-14 日立建機株式会社 柔構造作業機
JPH01173117A (ja) * 1987-12-28 1989-07-07 Mitsubishi Heavy Ind Ltd 柔軟構造物の振動制御装置
JPH06138950A (ja) * 1992-10-22 1994-05-20 Toyoda Mach Works Ltd フィードバック制御方式のロボット制御装置
JPH10235582A (ja) * 1997-02-26 1998-09-08 Yaskawa Electric Corp ロボットのウィービング制御装置
JP2015231663A (ja) * 2014-06-10 2015-12-24 シーメンス アクチエンゲゼルシヤフトＳｉｅｍｅｎｓ Ａｋｔｉｅｎｇｅｓｅｌｌｓｃｈａｆｔ 多数の要素システムを制御調節する装置および方法
JP2016083713A (ja) * 2014-10-24 2016-05-19 キヤノン株式会社 ロボット制御方法、ロボット装置、プログラム、記録媒体及び組立部品の製造方法
JP2018171666A (ja) * 2017-03-31 2018-11-08 セイコーエプソン株式会社 制御装置、ロボット、およびロボットシステム

Also Published As

Publication number Publication date
JP2022177607A (ja) 2022-12-01

Similar Documents

Publication Publication Date Title
US10486309B2 (en) Robot controlling method, robot apparatus, program, recording medium, and method for manufacturing assembly component
US9393687B2 (en) Method for programming an industrial robot and industrial robot
JP6659096B2 (ja) ロボット装置の制御方法、およびロボット装置
JP6717768B2 (ja) 生産ラインにおける運用を考慮した学習制御を行うロボット及びその制御方法
JP5895628B2 (ja) ロボットの制御方法及びロボット制御装置、並びにロボット制御システム
CN110076773B (zh) 机器人设备、制造物品的方法及电机驱动设备
JP6512790B2 (ja) ロボット制御方法、ロボット装置、プログラム、記録媒体及び物品の製造方法
JP7091422B2 (ja) ロボット装置、物品の製造方法、ロボット装置の制御方法、プログラム、記録媒体、駆動装置、駆動装置の制御方法
JPWO2019087357A1 (ja) キャリブレーション装置、キャリブレーション方法および制御装置
US11141861B2 (en) Method for operating a robot and robotic arm
JP6946057B2 (ja) ロボットハンド、ロボットハンドの制御方法、ロボット装置
JP7336215B2 (ja) ロボットシステム、制御方法、物品の製造方法、プログラム、及び記録媒体
JP6862604B2 (ja) 垂直多関節ロボットの慣性パラメータ同定システム及び慣性パラメータ同定方法並びに垂直多関節ロボットの制御装置及び制御方法
WO2022244441A1 (ja) マニピュレータ
JP2007066001A (ja) ロボットの制御装置
JP2006281330A (ja) ロボットシミュレーション装置
US20230052996A1 (en) Method of obtaining vibrational properties of robot arm
US11890759B2 (en) Robot control method
JP6697544B2 (ja) 最適化装置及びそれを備えた垂直型多関節ロボット
JPH0916241A (ja) ロボットの加減速動作の設定方法
JP7111498B2 (ja) ロボットの制御装置およびロボットの制御方法
JP3937108B2 (ja) ロボットの制御方法およびロボット
JP7227018B2 (ja) 学習制御装置、ロボット制御装置およびロボット
JP2002331479A (ja) 産業用ロボットの制御方法
JP2017127932A (ja) ロボット装置、ロボット制御方法、部品の製造方法、プログラム及び記録媒体

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22804359

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 22804359

Country of ref document: EP

Kind code of ref document: A1