KR100617963B1 - Control apppratus of robot - Google Patents

Abstract

The present invention relates to a control device for a robot. The control device of the robot according to the present invention includes a main controller for generating a speed command for rotation of the motor; A first encoder unit configured to sense an output of the motor input to the driven unit, a second encoder unit configured to sense an output of the driven unit that receives and drives the output of the motor, and the first based on the speed command; It characterized in that it comprises a local controller having a drive control unit for controlling the driving of the motor in response to the detection result of the first encoder unit and the second encoder unit. Accordingly, the robot's walking performance is improved and limit cycle is eliminated by controlling the saturation nonlinearity which is a problem in driving and the nonlinear backlash mechanically, using the switching PID controller and the dual feedback control for stable walking of the robot. The tracking of the walking trajectory is improved.

Description

Robot Control Unit {CONTROL APPPRATUS OF ROBOT}

1 is a view for explaining the coordinate system of the robot according to the present invention,

2 is a control block diagram of a robot and a control device of the robot according to the present invention;

3 is a control block diagram showing an example of a local controller of the control device of the robot of FIG.

4 is a control block diagram showing an example of the switching PID controller of FIG.

5 to 9 are views for explaining the effect of the control device of the robot according to the present invention.

The present invention relates to a robot control device, and more particularly, to a control device for a robot in which a backlash caused by saturation nonlinearity and mechanical causes is improved by using a switching PDI controller and a dual feedback control.

Today, with the rapid development of industrialization, the proportion of robots is increasing, and various diversifications in shape, use, and function have been made. From small machine automation to large plant-wide automation, it is expected to carry out work in space, deep sea, and nuclear reactors where human work activities are impossible.

In recent years, many systems are equipped with artificial intelligence, vision, and various sensors to improve their functions, but from a structural point of view, a large number of fixed robots have been used so far, and even mobile devices use rolling devices such as wheels and crawlers. The working environment was limited. Therefore, in order to be able to work in a more diverse environment, the need for implementing a human-like robot system is required.

Currently, researches on humanoid robots are being actively conducted in various places, and representative examples are Honda's ASIMO, Sony's SDR-4X, and Fujitsu's HOAP. Such humanoid robots have a lot of restraints compared to other systems having high stability because the humanoid robot needs to perform walking while maintaining balance in a state supported by one leg, and it is difficult to implement because it is a multi-axis simultaneous driving system.

Another feature of the humanoid robot is that it has a greater ripple effect in other fields than its practicality. Vision system for the analysis of the mechanical and motor characteristics of the robot, such as the analysis of links with slack joints, as well as the linking movements of the main joints according to the various trajectories, the development of the limbs instead of the human legs, and autonomous driving Various studies have been carried out as a model of human walking, such as the installation of gravity sensors and various sensors for maintaining the balance.

On the other hand, in the development of humanoid robots, backlash occurs in the operation of gears such as reducers and bevel gears structurally, and saturation nonlinearity occurs in driving a motor. This is recognized as a necessary control for the stable walking of the robot.

Therefore, various methods for solving the problems caused by the saturation nonlinearity and backlash of the robot have been studied.

Accordingly, an object of the present invention relates to a robot control apparatus in which a backlash caused by a saturation nonlinearity of a motor and a mechanical cause is improved by using a switching PID controller and a double feedback control when the robot is driven.

According to the present invention, there is provided a control apparatus of a robot having at least one motor and a driven part driven by the motor and generating a backlash, the controller comprising: a main controller for generating a speed command for rotation of the motor; A first encoder unit configured to sense an output of the motor input to the driven unit, a second encoder unit configured to sense an output of the driven unit that receives and drives the output of the motor, and the first based on the speed command; It is achieved by the controller of the robot, characterized in that it comprises a local controller having a drive control unit for controlling the driving of the motor in response to the detection result of the first encoder unit and the second encoder unit.

The driving controller may include: a first comparator configured to output a first deviation between an output sensed by the first encoder and the second encoder; A second comparing unit outputting a second deviation between the speed command and the first deviation output from the first comparing unit; A third comparator for outputting a third deviation between the second deviation output from the second comparator and the output sensed by the first encoder; The driving voltage output unit may output a driving voltage for driving the motor based on the third deviation output from the third comparing unit.

The driving voltage output unit may include a driving voltage generation unit generating the driving voltage; A plurality of PID controllers that receive the third deviation and have mutually different PID gain values, and a switching PID having the third deviation selectively inputted to any one of the plurality of PID controllers based on the third deviation; It may include a controller.

The driving voltage output unit may include a driving voltage generation unit generating the driving voltage; A plurality of PID controllers receiving the third deviation and having mutually different PID gain values, and switching to output an output from any one of the plurality of PID controllers to the driving voltage generator based on the magnitude of the third deviation; It may also include a switching PID controller having a portion.

In addition, at least one of the PID gain values of each PID control unit may be calculated through a genetic algorithm.

Hereinafter, with reference to the accompanying drawings will be described in detail the present invention.

1 is a diagram illustrating an example of a robot in which a control apparatus for a robot according to the present invention is implemented. The humanoid robot is a kind of modularized robot in which a control module is embedded in each joint. The humanoid robot is similar to a human being by vertically attaching the motors 40a, 40b, and 40c using bevel gears to each pitch joint. It may have a shape.

As shown in Fig. 2, the robot has motors 40a, 40b and 40c and driven parts 50a, 50b and 50c which are rotated by the motors 40a, 40b and 40c. The motors 40a, 40b, and 40c may be DC motors 40a, 40b, and 40c, and the ankle yaw, knee pitch, hip yaw, and other ankle pitches are applied to the robot by a large torque. Different types of DC motors 40a, 40b, and 40c may be used for the hips and the rolls and rolls, respectively.

The entire robot system including the robot and its control apparatus according to the present invention, as shown in Figure 2, the above-described motor (40a, 40b, 40c), the driven parts (50a, 50b, 50c), the simulator (10) ), The main controller 20 and a plurality of local controllers (30a, 30b, 30c).

The simulator 10 extracts data necessary for the robot to walk. Based on the information of the waypoints given by the user, a trajectory is generated using a fifth-order spline function and each joint value of the trajectory is calculated through inverse kinematics. In addition, the movement of the robot can be confirmed in 3D graphics using pure kinematics and pure dynamics. By calculating ZMP (Zero Moment Point) and torque, it is possible to check the stability of the robot while moving and the torque generated at each joint.

Then, the data is transmitted to the master controller 20 using a wireless network such as 802.11b standard. Such a simulator 10 may be implemented through a PC, it may be arranged physically separated from the robot shown in FIG.

The master controller 20 transmits the information of each joint variable provided from the simulator 10 to the local controllers 30a, 30b, and 30c. Then, the controller receives the joint variable information on the current position from the local controllers 30a, 30b, and 30c. Here, the main controller 20 and the local controllers 30a, 30b, and 30c may perform data communication with each other through controller area network (CAN) communication.

Each local controller 30a, 30b, 30c performs precise control of the motors 40a, 40b, 40c, communication with the main controller 20, and sensor recognition. Hereinafter, the configuration of each local controller 30a, 30b, 30c will be described in detail with reference to FIG.

As shown in FIG. 3, the local controllers 30a, 30b, and 30c according to the present invention may include a first encoder unit 34, a second encoder unit 35, and a driving control unit.

The drive control unit controls the driving of the motors 40a, 40b, and 40c. Here, the drive control unit controls the rotation speed of the motors 40a, 40b, 40c by outputting a drive voltage for rotating the motors 40a, 40b, 40c. At this time, the driving controller according to the present invention is fed back to the detection result of the first encoder 34 and the second encoder 35 based on the speed command (Vcom) from the main controller 20, the feedback feedback The rotation speeds of the motors 40a, 40b, and 40c are controlled by outputting the driving voltages corresponding to the results and the speed command Vcom to the motors 40a, 40b and 40c.

The first encoder 34 senses the output of the motors 40a, 40b, and 40c. That is, the first encoder 34 detects the actual rotation position of the motors 40a, 40b, and 40c.

The second encoder unit 35 is installed on the output side of the driven units 50a, 50b, 50c to sense the output of the actual driven units 50a, 50b, 50c. Accordingly, the influence of the backlash generated in the driven parts 50a, 50b, and 50c may be sensed. For example, when the driven parts 50a, 50b, 50c rotate in response to the driving of the motors 40a, 40b, 40c, the motors 40a, which are generated in the reducer or bevel gear provided in the driven parts 50a, 50b, 50c, Deviation from the output of 40b, 40c, i.e. backlash, is detected.

Meanwhile, as shown in FIG. 3, the driving controller may include a driving voltage output unit 33, a first comparator 36, a second comparator 31, and a third comparator 39. It may also include a scaling unit 37, a filter unit 38, and a P controller 32.

The output from the second encoder unit 35 may be input to the scaling unit 37 and scaled. Accordingly, the scale between the data of the detection result of the first encoder 34 and the detection result of the second output shaft by the resolution and the reduction ratio between the input and the output of the driven parts 50a, 50b, 50c. By scaling the difference from the second encoder section 35 by the scaling section 37, the scale between them is matched.

Meanwhile, the detection result from the first encoder unit 34 and the detection result from the second encoder unit 35 scaled and output by the scaling unit 37 are compared by the first comparator 36 to compare the first deviation. Is outputted and fed back. Accordingly, the detection result of the first encoder unit 34, that is, the input of the driven units 50a, 50b, 50c, and the detection result of the second encoder unit 35, that is, the driven units 50a, 50b, 50c. ) Is compared by the first comparator 36 and fed back with the first deviation, so that information on the backlash generated in the driven parts 50a, 50b, and 50c is reflected in the drive control of the motors 40a, 40b, and 40c. Thus, the backlash can be controlled.

Here, the first deviation output from the first comparator 36 may be input to the filter unit 38 and filtered, and then fed back to the driving controller. Here, the filter part 38 removes a limit cycle caused by nonlinearity of the backlash generated in the driven parts 50a, 50b, and 50c, and removes a limit cycle applied to reach a desired target point. As a filter, a low pass filter can be used.

The second comparator 31 compares the speed command Vcom from the main controller 20 with the first deviation from the first comparator 36 filtered out by the filter unit 38 and outputs the second deviation. Output Here, the second deviation output from the second comparator 31 is input to the P controller 32, amplified, and output to the third comparator 39. As a result, an error of an error occurring in the output from the filter unit 38 is amplified.

The second deviation amplified by the P controller 32 and the output fed back from the first encoder are compared by the third comparator 39 and outputted as the third deviation to be input to the driving voltage generator 33b. do. Accordingly, the first encoder unit 34 is detected by performing dual feedback control by sensing the inputs and outputs of the driven units 50a, 50b, and 50c through the first encoder unit 34 and the second encoder unit 35. On the basis of the control by, the error caused by the influence of the backlash and the load of the driven parts 50a, 50b, and 50c is regarded as disturbance, and the difference between the detection result of the first encoder part 34 and the second encoder part 35 By observing with a feedback and feeding it back, the effect of backlash on the load is minimized.

On the other hand, the driving voltage generator 33b generates driving voltages for driving the motors 40a, 40b, 40c based on the third deviation from the third comparator 39. Here, the driving voltage generator 33b according to the present invention may include a switching PID controller 33a and a driving voltage generator 33b.

The switching PID controller 33a receives a third deviation amplified and output by the P controller 32 and has a plurality of PID controllers 33a-1, 33a-2, 33a-3, 33a- having different PID gain values. 4) and the third deviation outputted from the third comparison unit based on the magnitude of the third deviation is selectively applied to any one of the plurality of PID control units 33a-1, 33a-2, 33a-3, 33a-4. It may include a switching unit (33a-5) to be input.

In one embodiment of the present invention, the switching unit 33a-5 is switched such that the third deviation is selectively input to any one of the plurality of PID control units 33a-1, 33a-2, 33a-3, 33a-4. As an example, as shown in FIG. 4, the third deviation is input to the plurality of PID controllers 33a-1, 33a-2, 33a-3, 33a-4, and the switching unit 33a-5 is applied. Of course, any one of the outputs of the plurality of PID controllers 33a-1, 33a-2, 33a-3, 33a-4 may be selectively switched to be input to the driving voltage generation unit.

Here, the PID gains applied to each of the plurality of PID control units 33a-1, 33a-2, 33a-3, 33a-4 are calculated through a genetic algorithm by the simulator 10. In the simulation, the genetic algorithm is applied to determine the result, and the modeling of the system is decided first for optimal control. In this case, the difference between the simulation and the actual experiment using the mathematical modeling may be caused by the modeling error, the influence of nonlinearity, the measurement error, and the change of the experimental environment. Modeling is estimated by applying genetic algorithm to minimize modeling error.

Genetic algorithms are computational models based on evolutionary phenomena, which are parallel and global search algorithms, and are based on the theory of survival of the fittest that only the right ones survive in a given variety of environments. Genetic algorithms represent possible solutions in a form of data structure, and then progressively produce better solutions by reproducing, mating, and mutating them.

Here, [Equation 1] shows the transfer function of the mathematical DC motor (40a, 40b, 40c).

Where Ξ (s) is the output angle [rad], V (s) is the input voltage [V], Jm is the rotor inertia, fm is the viscous friction coefficient, La is the amateur inductance, and Ra is the amateur resistance And Ka is the torque constant and Kb is the counter electromotive force constant.

In order to obtain the modeling of the motors 40a, 40b, and 40c using the genetic algorithm, the coefficients of the transfer functions of the motors 40a, 40b, and 40c are set as variables of the genetic algorithm, as shown in Equation 2, The transfer function of the motors 40a, 40b, and 40c is to be found based on the output shaft position data.

In this case, a, b, and c mean variables of the transfer function to be found.

An example of the values of the parameters used to find the motor (40a, 40b, 40c) modeling using genetic algorithms is 500 populations, 500 generations, 90 variable lengths, 0.95 breeding rate, 0.03 mutation rate, and the number of variables. Let be 3. In this case, as the fitness function, the inverse of the sum of the sum of squares of the difference between the actual data and the modeling result is selected. When the fitness function is maximized, the modeling of the motors 40a, 40b, and 40c becomes the closest model to the real system.

[Equation 3] represents the mathematical modeling found by inputting a constant value to the variable of [Equation 1], and [Equation 4] represents the transfer function found using the genetic algorithm.

4 shows outputs for modeling using genetic algorithms for step inputs of the motors 40a, 40b, and 40c, outputs of actual motors 40a, 40b, and 40c, and outputs for mathematical modeling.

When the motors 40a, 40b, and 40c are mathematically modeled and the error is modeled using the genetic algorithm, it can be seen that the modeling using the genetic algorithm is more similar to the real system than the mathematical modeling.

Even if the optimal PID gain is obtained using the genetic algorithm, the limit cycle occurs due to the saturation nonlinearity. In order to eliminate this, it is possible to maximize the robustness of the PID controller by changing the gain of the PID according to the range of the third deviation. Here, the changed PID gain value is set to the PID gain values of the respective PID controllers 33a-1, 33a-2, 33a-3, 33a-4 of the switching PID controller 33a.

In the control apparatus according to the present invention, the switching PID controller 33a has four PID controllers 33a-1, 33a-2, 33a-3, 33a-4 as an example, and the range of the third deviation. Accordingly, PID gain values are set in the PID control units 33a-1, 33a-2, 33a-3, 33a-4 as shown in [Equation 5].

Here, if the absolute value of the third deviation is between 200 pulses and 300 pulses, it is controlled by the first PID controller 33a-1, and if the absolute value of the third deviation is between 100 pulses and 200 pulses, the second PID controller ( 33a-2), when the absolute value of the third deviation is 100 pulses or less, controlled by the third PID controller 33a-3, and when the absolute value of the third deviation is 300 pulses or more, the fourth PID controller ( 33a-4) respectively.

Table 1 below shows the calculation of each PID gain value according to the third deviation through the genetic algorithm according to an example of the present invention.

Kp Ki Kd Error ｜ <300 60 0 0.2 Error ｜ <200 40 0 2.0 Error ｜ <100 25 0 0.05    otherwise 2358.32 10.65 45.66

Considering the effects of the control device of the robot according to the present invention as described above are as follows. This is based on the simulation results for the control device of the robot according to the present invention.

First, FIG. 5 is a diagram illustrating a simulation result using the PID algorithm and switching PID control using a genetic algorithm, and FIG. 6 is a diagram illustrating a simulation result using the PID algorithm and switching PID control using a genetic algorithm. The figure shown.

Here, in the simulation using the PID gain value found using the genetic algorithm, a limit cycle of 0.25 ° and 50Hz occurs, and in actual experiments, a limit cycle of 0.03 ° and 167Hz occurs. Based on the PID gain value using the genetic algorithm, and as shown in [Table 1], the PID gain value according to the range of the third deviation is determined and the simulation confirms that the limit cycle caused by the saturation nonlinearity is eliminated. You can see that the limit cycle is removed even when applied to.

On the other hand, Figures 7 to 9 measure the performance of the double feedback control using the right ankle module of the robot according to the present invention. FIG. 7 is a diagram illustrating a case of controlling only the detection result of the first encoder 34, FIG. 8 is a diagram illustrating a case of controlling only the detection result of the second encoder 35, and FIG. 9 is a diagram of the first encoder. FIG. 3 illustrates a case in which both the detection result of the unit 34 and the second encoder unit 35 are used, that is, a case of performing dual feedback control.

As shown in these figures, when controlling only the detection result of the first encoder 34 can be seen that the robot does not properly follow the trajectory, when controlling only the detection result of the second encoder 35 It can be seen that there is a limit cycle. On the other hand, when performing the double feedback control according to the present invention, it can be seen that the robot follows the trajectory well.

Although some embodiments of the invention have been shown and described, it will be apparent to those skilled in the art that modifications may be made to the embodiment without departing from the spirit or spirit of the invention. . The scope of the invention will be defined by the appended claims and equivalents thereof.

According to the above configuration, according to the present invention, the robot walking performance by controlling the saturation nonlinearity and mechanical nonlinearity backlash, which are a problem during driving, for stable walking of the robot by using the switching PID controller and the dual feedback control. The robot control apparatus is improved, the limit cycle is eliminated, and the tracking of the walking trajectory is improved.

Claims (5)

In the control device of the robot having at least one motor and a driven portion driven by the motor and the backlash occurs, A main controller for generating a speed command for rotation of the motor; A first encoder unit configured to sense an output of the motor input to the driven unit, a second encoder unit configured to sense an output of the driven unit that receives and drives the output of the motor, and the first based on the speed command; It includes a local controller having a drive control unit for controlling the driving of the motor in response to the feedback results of the detection of the first encoder unit and the second encoder, The drive control unit A first comparator for outputting a first deviation between the outputs respectively sensed by the first encoder and the second encoder; A second comparing unit which outputs a second deviation between the speed command and the first deviation output from the first comparing unit; A third comparator for outputting a third deviation between the second deviation output from the second comparator and the output sensed by the first encoder; And a driving voltage output unit for outputting a driving voltage necessary for driving the motor based on the third deviation output from the third comparing unit. delete The method of claim 1, The driving voltage output unit, A driving voltage generation unit generating the driving voltage; A plurality of PID controllers that receive the third deviation and have mutually different PID gain values, and a switching PID having the third deviation selectively inputted to any one of the plurality of PID controllers based on the third deviation; Control device for a robot comprising a controller. The method of claim 1, The driving voltage output unit, A driving voltage generation unit generating the driving voltage; A plurality of PID controllers receiving the third deviation and having mutually different PID gain values, and switching to output an output from any one of the plurality of PID controllers to the driving voltage generator based on the magnitude of the third deviation; Control device for a robot comprising a switching PID controller having a portion. The method of claim 4, wherein At least one of the respective PID gain values of each PID control unit is calculated by a genetic algorithm.

Images