JP2019000918A - System and method for controlling arm attitude of working robot - Google Patents

Abstract

To improve operability of a remote-controlled robot.SOLUTION: A system for controlling arm attitude of a working robot comprising an arm having a movable part is provided. The system performs processing with a camera image obtained by photographing a marker added to the arm, as input. The system comprises, as a constituent, an image analyzer generating three-dimensional coordinate of the marker by extracting the marker from the camera image, an arm attitude estimation device generating arm attitude data showing an attitude of the arm based on the three-dimensional coordinate of the marker and a structure model of the arm, and an arm attitude control device for controlling the attitude of the arm based on arm attitude data. When detecting a prescribed error in generation of the arm attitude data, an attitude is returned to the attitude of the arm before detecting the prescribed error or a basic attitude or a state of detecting the prescribed error is stored, and in control of the attitude of the arm thereafter, avoidance control such as avoiding a state in which the prescribed error occurs is performed.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a technique for controlling the attitude of a robot, and more particularly to a technique for controlling an arm attitude of a working robot.

  As an example of a conventional working robot, Patent Document 1 proposes a hydraulic drive actuator that handles a large weight and a muscle robot incorporating the actuator.

  Generally, in the operation of a working robot, it is conceivable that the operator operates the robot while directly observing the robot or watching the movement of the remote robot taken by a camera or the like. Further, when the actual operation of the robot is determined, it is possible to receive assistance from the information processing apparatus.

  Patent Document 2 discloses a method for determining the trajectory so as to minimize the cost based on a predetermined cost function as a robot motion planning method by sampling an infinite number of postures that interpolate the initial posture and the target posture. The

JP, 2006-203359, A JP, 2015-160253, A

  In the decommissioning work of a nuclear reactor and the like, it is assumed that the surrounding structure is unknown and the work is in a high radiation environment where it is difficult for a person to enter. For this reason, in principle, work by a remotely operated robot is assumed.

  When a robot works, it is necessary to control interference with surrounding structures in the work environment in order to avoid damage to the robot itself and damage to surrounding facilities. For this reason, it is required to accurately control the position and posture of the robot.

  However, a sensor using an electronic component such as an encoder that directly measures the angle of a joint, which is mounted on a general industrial robot, may malfunction in a high radiation environment. For this reason, the posture of the robot may not be directly grasped by the sensor.

  In addition, when an image is acquired by a video camera and an attempt is made to grasp the posture of the robot based on the image, high radiation may cause noise in the camera image. In addition, in a high temperature and high humidity environment, the camera image may become unclear due to the influence of water vapor or the like. There are also restrictions on the placement of video cameras and the installation of lighting fixtures. When the clarity of the image of the video camera cannot be ensured sufficiently, there is a possibility that the working efficiency of the operator who operates the robot while viewing the image may be lowered.

  Accordingly, an object of the present invention is to improve the operability by an operator of a remote control robot that can be used in a high radiation environment.

  One aspect of the present invention is an arm posture control system for a working robot including an arm having a movable portion. The system performs processing by inputting a camera image obtained by photographing a marker added to an arm. The system components include an image analysis device that extracts a marker from a camera image and generates a three-dimensional coordinate of the marker, and arm posture data that indicates the posture of the arm based on the three-dimensional coordinate of the marker and the structural model of the arm And an arm posture control device for controlling the posture of the arm based on the arm posture data. In addition, when a predetermined error is detected in the generation of arm posture data, the posture of the arm before the detection of the predetermined error, or the basic posture is restored, or the situation where the predetermined error is detected is stored, In the subsequent arm posture control, avoidance control is performed to avoid the situation.

  Another aspect of the present invention is an arm posture control method for a working robot including an arm having a movable part. This method includes an input step of inputting a camera image obtained by photographing a marker added to an arm, an image analysis step of extracting a marker from the camera image and generating a three-dimensional coordinate of the marker, a three-dimensional coordinate of the marker, and an arm An arm posture estimation step for generating arm posture data indicating the posture of the arm on the basis of the structural model, an arm minute movement target amount based on the arm posture data, and an arm minute movement target amount based on the arm minute movement target amount. An arm posture control step for controlling the posture is executed. Also, if arm posture data indicating the posture of the arm could not be generated, return control to return to the arm posture controlled based on the arm posture data or the basic posture, or generate arm posture data The situation that could not be recorded is stored, and in the subsequent arm posture control, avoidance control is performed to avoid the situation.

  The operability of the remote control robot can be improved.

The perspective view which shows the whole hardware structure of the arm attitude | position control system of the robot of an Example. The flowchart which shows the flow of the whole process of the arm attitude | position control system of the robot of an Example. The block diagram which shows the functional block of the arm attitude | position control system of a robot. The flowchart which shows the processing flow of an image analyzer. The flowchart which shows the processing flow of an arm attitude | position estimation process in an arm attitude | position estimation apparatus. The flowchart which shows the processing flow of an arm attitude | position control process in an arm attitude | position control apparatus.

  In one example of the embodiment described below, a robot system including a robot having an arm (working robot) uses a non-contact sensor installed in the work environment or the robot itself. As a non-contact sensor, there is a stereo camera, for example. A plurality of markers are arranged on the arm, and the robot system grasps the posture of the arm by detecting the marker with a non-contact sensor. According to this configuration, the posture of the robot can be grasped by a configuration having excellent radiation resistance.

  When the arm cannot be detected by the non-contact sensor, the posture returns to the previous posture or the basic posture. The basic posture is, for example, the posture when the hydraulic pressure is maximized or minimized in the working robot disclosed in Patent Document 1 described above. The basic posture can be defined as a posture that is uniquely determined when a control parameter such as hydraulic pressure is set to a predetermined condition or a predetermined value. According to this configuration, even when the arm cannot be detected by the non-contact sensor and the posture cannot be recognized, the posture can be returned to the recognizable posture.

  Further, the position where the arm cannot be detected is stored, and in the subsequent operation, the arm is driven avoiding the position. When the arm cannot be detected by the non-contact sensor, it is assumed that the marker cannot be seen from the camera due to an obstacle, for example. Therefore, by avoiding such a position in advance and controlling the posture of the arm, it is possible to continuously grasp the posture of the arm.

(1. Overall hardware configuration of the system)
FIG. 1 is a perspective view showing the overall hardware configuration of a robot arm posture control system according to an embodiment of the present invention.

  The robot 1 having the arm 1001 can be moved by a moving mechanism 1002, for example. The arm 1001 includes one or a plurality of joints 1003, and the joint 1003 can change an angle or be rotated. The arm 1001 can change the posture in accordance with an instruction from an operator or the like located at a remote place, and can work on the work object 7 with the holding unit 1004 or the like. The mechanical structure of the arm 1001 is disclosed in Patent Document 1, for example. The working robot disclosed in Patent Document 1 has a structure that operates with a hydraulic pressure, and in particular, the water pressure can be expected to have a small influence on the working environment due to oil leakage from the robot.

  One or more markers 3 are arranged on the arm 1001. In this embodiment, the marker has a configuration with good visibility. The marker 3 is, for example, a white disk having a predetermined size, and is attached to each part of the arm 1001. The marker 3 may emit light with a light emitting diode or the like. Moreover, you may change the magnitude | size and color of a marker for every site | part. As the configuration of the marker 3, it is desirable to select a color or shape having a clear contrast with respect to the background so that the marker 3 can be sufficiently detected in the work environment of the robot.

  The marker 3 attached to the arm 1001 is photographed by the stereo camera 2. The stereo camera 2 is, for example, two video cameras. An image captured by the stereo camera 2 is transmitted to the image analysis device 6. The output of the image analysis device 6 is input to the arm posture estimation device 5, and the output of the arm posture estimation device 5 is input to the arm posture control device 4. An output from the arm posture control device 4 is input to the arm 1001 to control the posture of the arm 1001.

  For example, in the technique of Patent Document 1, hydraulic pressure is used to control the posture of the arm. When this method is adopted, the arm posture control device 4 includes a pump for controlling the pressure of the liquid, and the liquid itself delivered from the pump may be considered as an output from the arm posture control device 4.

(2. Overall processing flow of the system)
FIG. 2 is a diagram showing the overall processing flow of the robot arm posture control system according to the present embodiment.

  In step S101, the stereo camera 2 captures the arm 1001 and the marker 3 of the robot 1, and acquires a stereo camera image.

  In process S102, the image analysis apparatus 6 performs image analysis processing. This will be described in detail later with reference to FIG.

  In step S103, the arm posture estimation apparatus 5 performs an arm posture estimation process. By this processing, the operator or the system can grasp the posture of the robot 1 or the arm 1001. This will be described in detail later with reference to FIG.

  In process S <b> 104, arm posture control processing is performed by the arm posture control device 4. This process is a process in which the operator or the system instructs the robot 1 or the arm 1001 of the next posture. This will be described in detail later with reference to FIG.

  In step S <b> 105, an arm operation instruction is output from the arm posture control device 4 to the robot 1.

(3. System functional block configuration)
FIG. 3 is a block diagram showing functional blocks of a robot arm posture control system according to an embodiment of the present invention. The arm posture control device 4, the arm posture estimation device 5, and the image analysis device 6 are connected by a signal cable or a wireless link, and can transmit or receive signals.

  In this embodiment, the arm posture control device 4, the arm posture estimation device 5, and the image analysis device 6 are realized by a normal server including a processor, a memory, an input interface, and an output interface. Functions such as calculation and control of each device are realized in cooperation with other hardware by executing a program stored in the memory by the processor. A program executed by a computer, its function, or means for realizing the function may be referred to as “function”, “means”, “unit”, “unit”, “module”, or the like. The above configuration may be configured by a single computer, or may be configured by another computer in which arbitrary portions of the processor, the memory, the input interface, and the output interface are connected to each other.

  1 and 3, the arm posture control device 4, the arm posture estimation device 5, and the image analysis device 6 are described as separate devices, but may be a single device. That is, it may be configured by three servers, or may be configured by one server having a plurality of functions.

  In the configuration illustrated in FIG. 3, the robot 1 and the stereo camera 2 that captures the robot 1 may be arranged in a high radiation work environment. In this case, normal server components, particularly electronic devices such as SRAM (Static Random Access Memory), have poor radiation resistance, so the arm posture control device 4, arm posture estimation device 5, and image analysis device 6 are used. It is desirable to install the server to be configured in a remote place from the work environment. A signal cable for transmitting image data can be arranged between the server and the stereo camera 2 to ensure a distance.

  Further, a signal line for sending an arm operation instruction signal to the robot 1 can be arranged between the server and the robot 1 so as to take a distance. Alternatively, as described above, when the arm 1001 operates at a liquid pressure such as water or oil, a pipe for sending the liquid can be installed. In this case, a pump or the like (not shown) is controlled by a signal from the arm operation control unit 703, and the pressure of the liquid supplied to each part of the arm 1001 is controlled by the function of the pump or the like.

(3-1. Stereo camera)
Since the stereo camera 2 generally has lower radiation resistance as the element scale of the electronic component is smaller, the stereo camera 2 only needs to have a resolution that allows the marker 3 to be recognized, and does not require a high-definition image. The stereo camera 2 generates a stereo camera image signal V and transmits it to the image analysis device 6. Further, the stereo camera 2 is assumed to start or end shooting or change the shooting angle in response to an instruction signal C from the image analysis management unit 601.

  The number of stereo cameras is one or more. Since it is ideal that the image of the arm 1001 has no blind spot, three or more may be installed. The relative positional relationship and optical axis direction of the stereo camera are converted into data by a method such as setting at the time of installation, actual measurement after setting, or estimation from a camera image.

(3-2. Image analysis device (outline))
FIG. 4 shows a processing flow of the image analysis apparatus 6, and the processing contents will be described with reference to FIG. In the image analysis apparatus 6 of this embodiment, the image analysis management unit 601 controls the overall operation. When receiving the stereo camera image signal V from the stereo camera 2, the image analysis management unit 601 of the image analysis device 6 inputs the stereo camera image signal V to the marker detection unit 602 (S201). A stereo camera image includes images from two or more cameras.

  The marker detection unit 602 detects a marker portion from the stereo camera image signal V by image processing, and detects the three-dimensional coordinates of each marker according to the principle of the stereo camera. Furthermore, if necessary, the marker coordinates are complemented using the marker three-dimensional predicted coordinates 3DE (S202). The marker three-dimensional coordinates 3D obtained as a result of the process S202 are sent to the image analysis management unit 601. The coordinate conversion unit 603, the marker three-dimensional predicted coordinates 3DE, and the complement of the marker coordinates will be described later.

  In the image processing for detecting the marker, for example, when the marker 3 is a white circle, a high-contrast circular portion is detected. It may be detected by changing the color or reflectance of the marker and detecting the color or reflected light. Then, marker two-dimensional coordinates indicating the coordinates of the marker 3 in the two-dimensional image are generated.

  The image analysis device 6 has sensor arrangement information 604 as data. The sensor arrangement information 604 is a position coordinate and an angle of view of a sensor, for example, a camera (the direction of the optical axis and the viewing angle of the camera). The marker detection unit 602 converts the two-dimensional coordinates viewed from the camera into three-dimensional coordinates using the sensor arrangement information 604, and outputs the converted three-dimensional coordinates 3D to the image analysis management unit 601. At this time, the marker three-dimensional coordinates are complemented as necessary as described above.

  As a method for restoring three dimensions from a plurality of images, there is a method known as SfM (Structure from Motion). The origin and coordinate system of the three-dimensional coordinates may be arbitrary. The image analysis management unit 601 transmits the marker three-dimensional coordinates 3D to the arm posture estimation device 5 (S203).

(3-3. Arm posture estimation device)
FIG. 5 shows a process flow of the arm posture estimation process in the arm posture estimation apparatus 5, and the processing content will be described with reference to FIG. As illustrated in FIG. 5, the posture update unit 501 of the arm posture estimation device 5 receives the marker three-dimensional coordinates 3D from the image analysis management unit 601 of the image analysis device 6. The stereo camera image signal V includes a plurality of time-series frame images, and the movement of the marker 3 can be tracked by generating a marker three-dimensional coordinate 3D from each frame.

  The posture update unit 501 of the arm posture estimation device 5 performs a process of estimating the arm posture and updating it as arm posture data AP. The arm posture estimation device 5 accumulates the arm posture data AP as arm posture history data 503 in time series. The details of the process of estimating the arm posture data AP will be described later. In this embodiment, the arm predicted from the marker three-dimensional coordinates 3D based on the detected marker and the arm posture history data 503 (past arm posture data AP). Both attitude prediction data APE are used.

  As shown in FIG. 3, the arm posture history data 503 is input to the posture prediction unit 504. The posture prediction unit 504 generates arm posture prediction data APE from the arm posture history data 503 using the arm motion model 505 (S301). The arm motion model 505 is, for example, a model that complies with a constraint condition that each joint of the arm operates at a predetermined equiangular velocity. Such prediction can be performed using a Kalman filter, for example.

  The posture update unit 501 receives the marker three-dimensional coordinates 3D from the image analysis management unit 601 (S302). The posture update unit 501 uses the arm model 502 to calculate the posture of the arm from the marker three-dimensional coordinates 3D. Here, the arm model 502 includes information on the connection relationship between the joints of the arm 1001, the joint-to-joint spacing, the movable range of the joints, the marker position based on each joint axis, and the like. When the arm is driven by liquid pressure, the liquid pressure corresponding to the posture may be used.

  The posture update unit 501 calculates a parameter for uniquely specifying the posture of the arm from the arm model 502 and the marker three-dimensional coordinates 3D. This parameter is, for example, the angle of each joint of the arm or the pressure of the liquid for taking the angle. In the case where the arm 1001 has a telescopic function, the arm 1001 may include a telescopic length. In this embodiment, the arm 1001 is driven by the pressure of the liquid, and the posture of the arm and the control signal for driving the arm are defined by the pressure of the liquid supplied to each part of the arm.

  The posture update unit 501 estimates the arm posture from the arm posture calculated from the marker three-dimensional coordinates 3D and the arm model 502 and the arm posture prediction data APE predicted by the posture prediction unit 504, and as arm posture data AP, This is input to the arm posture estimation management unit 506.

  As a specific method for estimating the arm posture, for example, a likelihood function is evaluated from the posture of the arm calculated from the marker three-dimensional coordinates 3D, the parameters of the Kalman filter are corrected, and the arm posture data AP is estimated and updated. (S303). The arm posture data AP is transmitted from the arm posture estimation management unit 506 to the arm posture control device 4.

  While the arm posture history data 503 is not sufficiently accumulated, the arm posture calculated from the marker three-dimensional coordinates 3D may be used as it is as the arm posture data AP.

  On the other hand, in this embodiment, as shown in FIG. 3, the arm posture prediction data APE generated by the posture prediction unit 504 is fed back to the image analysis management unit 601 of the image analysis device 6. When the arm posture prediction data APE is used, the marker detection leakage can be complemented.

(3-4. Image analysis device (complement of marker coordinates))
Returning to FIG. 4, the marker detection leakage compensation process will be described. The arm posture prediction data APE is input to the coordinate conversion unit 603 (S204), and converted to the marker three-dimensional prediction coordinates 3DE based on the arm model 605 (which may be the same content as the arm model 502) (S205). The marker three-dimensional predicted coordinates 3DE are sent to the marker detection unit 602 via the image analysis management unit 601.

  In the marker detection unit 602, the marker three-dimensional coordinates based on the signal detected from the image of the stereo camera 2 are complemented by the marker three-dimensional predicted coordinates 3DE to generate the marker three-dimensional coordinates 3D. That is, the marker three-dimensional predicted coordinate 3DE is the coordinate of the marker that should be based on the arm posture predicted by the posture prediction unit 504, and is caused by missing data even when the marker 3 is hidden by an obstacle or water vapor. It is possible to compensate for the omission of marker detection. For example, when two markers are detected from the image of the stereo camera 2 and no marker is detected between them, the marker three-dimensional predicted coordinates 3DE can indicate the presence of the marker between them. However, for example, complementation of the marker at the arm tip is difficult in a situation where the arm angle cannot be known.

  As described above, as shown in FIG. 4, the marker detection unit 602 uses the marker three-dimensional predicted coordinates 3DE to supplement the marker information obtained from the image of the stereo camera 2 (S202). Then, the marker three-dimensional coordinates 3D are generated and output to the image analysis management unit 601 (S203). In the present embodiment, the arm posture prediction data APE in the posture prediction unit 504 is used for complementing marker detection in the image analysis device 6 as described above, but the complement processing may be omitted depending on circumstances.

  In the work environment of the robot 1, even if the above supplement is performed, there may be a case where a part or all of the posture of the robot 1 cannot be sufficiently recognized due to surrounding obstacles or water vapor. For example, when the image analysis management unit 601 fails to recognize the coordinates of a predetermined number of markers or loses the marker at the end of the arm, the image analysis management unit 601 adds or adds to the marker three-dimensional coordinates 3D. Then, an error signal indicating that the posture of the robot cannot be recognized is transmitted to the arm posture estimation apparatus 5. The arm posture estimation device 5 that has received the error signal stores the error signal in the arm posture history data 503 instead of or in addition to the arm posture data AP, and transmits the error signal to the arm posture control device 4. At this time, the immediately preceding marker three-dimensional coordinate 3D or the immediately preceding arm posture data AP may be further added to the error signal.

  The error signal may be generated when the arm posture cannot be estimated by the posture update unit 501 instead of being generated by the image analysis management unit 601 as described above. The case where the arm posture cannot be estimated is, for example, a case where the arm posture calculated from the marker three-dimensional coordinates 3D and the arm model 502 differs from the arm posture prediction data APE predicted by the posture prediction unit 504 by a predetermined amount or more.

(3-5. Arm posture control device)
FIG. 6 shows a processing flow of arm posture control processing in the arm posture control device 4, and processing contents will be described with reference to FIG. The arm motion planning unit 701 of the arm posture control device 4 receives the arm posture data AP from the arm posture estimation device 5. The arm motion planning unit 701 sends the arm posture data AP to the final target setting unit 702.

  In the initial setting of the arm posture control, the final target setting unit 702 sets a final target posture FP that is a final target of the arm 1001 (S401). In the final target posture FP, for example, the posture of the arm 1001 is reproduced as an image from the arm posture data AP and displayed on an image display device (not shown), and the operator confirms, for example, a position where the holding unit 1004 is to be moved. You may make it designate on an image. Alternatively, other known methods may be used. At this time, an image based on the stereo camera image signal V may be displayed together. For the sake of explanation, it is assumed that the arm 1001 is visible when the final target posture FP is set.

  The final target posture FP set as data handled by the system is defined by, for example, the angle and hydraulic pressure of each joint of the arm 1001 for taking that posture. In the present embodiment, the final target posture FP is defined by the pressure of the liquid supplied to each part of the arm 1001 as in the case of the arm posture and the control signal for driving the arm. At this time, the posture of the arm instructed by the operator may be converted into the pressure of the liquid supplied to each part of the arm in order to take the posture. The set final target posture FP is transmitted to the arm motion planning unit 701.

  The arm motion planning unit 701 generates a motion plan from the current arm posture data AP and the final target posture FP (S402). In robot control, a technique called motion planning is known in which a trajectory is dynamically generated according to the environment, initial posture, and target posture in order to cope with changes in the environment and changes in the target posture. The motion plan is a technology that determines the trajectory of the motion of the robot based on some criteria with the input of the initial posture, the target posture and the surrounding environment. The motion planning problem is a problem in an industrial arm-type robot in which it is searched how to move to the designated posture. An example of such an operation planning method is also disclosed in Patent Document 2.

  The arm motion planning unit 701 inputs the latest arm posture data AP (S403). Then, it is determined whether the posture of the arm 1001 can be grasped (S404). This determination can be made based on the content of the arm posture data AP generated by the image analysis management unit 601 or the posture update unit 510 or the presence / absence of an error signal (described above). If it is guaranteed that the arm is visible when the final target posture FP is set, this step may be omitted before the first minute movement.

  The arm motion planning unit 701 determines whether the posture of the arm 1001 is the final target posture FP based on the latest arm posture data AP (S405). When the posture of the arm is grasped and the current posture of the arm is the final target posture, the arm operation control is ended.

  The arm motion planning unit 701 keeps track of the arm posture, and continues the minute movement of the arm when the current arm posture has not reached the final target posture. The arm motion planning unit 701 obtains the arm minute movement target amount TA based on the arm motion plan (S406). The arm minute movement target amount corresponds to, for example, a predetermined amount of the trajectory of the arm 1001 determined based on the arm operation plan.

  The arm motion planning unit 701 instructs the arm motion control amount TA to the arm motion control unit 703, and the arm motion control unit 703 issues an arm motion command CONT according to the arm motion small target amount TA to the robot 1 (S407). ). By these controls, the minute movement of the arm 1001 is executed (S408). In this embodiment, the arm operation instruction CONT is a hydraulic pressure supplied to each part of the arm 1001.

  After the minute movement is executed, the latest arm posture data AP reflecting the minute movement is input (S403), it is determined whether the posture of the arm 1001 is grasped (S404), and the minute movement is continued.

  On the other hand, when the arm is not recognized in the determination S404, that is, when an error signal is input, the arm motion planning unit 701 moves the arm slightly so that the arm returns to the previous state (for example, the immediately previous state). By setting the target amount TA, converting the arm minute movement target amount TA into a control variable (for example, hydraulic pressure) and instructing the arm operation, the arm 1001 returns to the previous state (S409).

  By returning the arm 1001 to the previous state, it is possible to return to the state before losing sight of the posture of the arm 1001, and stable control becomes possible. Moreover, you may return to the state before a predetermined step other than returning to the state immediately before. Further, the arm 1001 may be returned to the basic posture (described above).

  When the posture of the arm 1001 is controlled by the liquid pressure as described above, in order to return to the immediately previous state, for example, the difference between the fluid pressure or the fluid amount before and after the error signal is detected, and the difference is calculated. It can be controlled on the basis. In order to return to the basic posture, for example, control can be performed by minimizing the fluid pressure or fluid amount of all cylinders of the joint driven by fluid pressure. In this case, the arm 1001 is in the most contracted state, for example. In order to return to the basic posture, for example, the control can be performed by maximizing the fluid pressure or the fluid amount of all the cylinders of the joint driven by the fluid pressure. In this case, the arm 1001 is in the most extended state, for example. In order to return to the basic posture, for example, the hydraulic pressure or liquid volume of one or more cylinders driven by hydraulic pressure is maximized, and the hydraulic pressure or liquid volume of one or more cylinders is minimized. Can be controlled. In this case, the arm 1001 is in a bent state, for example.

  On the other hand, the arm minute movement target amount TA generated by the arm motion planning unit 701 is accumulated as an arm minute movement target amount history 705. If the arm 1001 is lost as a result of being controlled by a certain arm minute movement target amount TA, that is, if an error signal is received, the history is recorded in the arm minute movement target amount history 705 (S410).

  If the arm is not recognized in the determination S404, the arm operation plan is corrected through steps S409 and S410. That is, for example, if the process returns to the previous state in Step 409, the motion plan for taking the final target posture FA set in Step S401 is re-created starting from the posture returned in Step 409. However, in this operation plan, an operation plan is created so as to avoid a state in which the arm 1001 is lost. For this purpose, control is performed using the arm minute movement target amount history 705 so as not to use the arm minute movement target amount TA immediately before losing sight of the arm 1001. Alternatively, control may be performed so as to avoid the posture of the immediately preceding arm posture data AP.

  Alternatively, the arm 1001 can be controlled so as to avoid the area by recording the corresponding marker 3D coordinates 3D immediately before receiving the error signal and mapping the outer edge of the area where the marker cannot be seen. .

  According to the present embodiment, it is possible to grasp the posture of the arm without using a sensor such as an encoder that directly measures the angle of the joint, which is mounted on a conventional industrial robot. Further, it becomes possible to grasp the posture of the arm even in an environment where high-quality camera images cannot be expected due to the influence of radiation or water vapor. Furthermore, even in situations where there is a possibility of losing sight of the robot or arm due to unknown surrounding structures, it is possible to control the arm posture safely and reliably, avoiding interference with surrounding structures.

  In the present embodiment, functions equivalent to those configured by software can be realized by replacing with hardware such as FPGA (Field Programmable Gate Array) and ASIC (Application Specific Integrated Circuit).

  The present invention is not limited to the embodiments described above, and includes various modifications. For example, a part of the configuration of one embodiment can be replaced with the configuration of another embodiment, and the configuration of another embodiment can be added to the configuration of one embodiment. Further, it is possible to add, delete, and replace the configurations of other embodiments with respect to a part of the configurations of the embodiments.

  Robot 1, arm posture control device 4, arm posture estimation device 5, image analysis device 6, arm 1001, moving mechanism 1002, joint 1003

Claims (15)

An arm posture control system for a working robot including an arm having a movable part,
With the camera image taken of the marker attached to the arm as input,
An image analysis device that extracts the marker from the camera image and generates three-dimensional coordinates of the marker;
An arm posture estimation device that generates arm posture data indicating the posture of the arm based on the three-dimensional coordinates of the marker and the structural model of the arm;
An arm posture control device for controlling the posture of the arm based on the arm posture data;
In the generation of the arm posture data, when a predetermined error is detected,
Return to the posture of the arm before the detection of the predetermined error, or the basic posture,
Or
The situation in which the predetermined error is detected is stored, and in the subsequent control of the posture of the arm, avoidance control is performed so as to avoid the situation.
Arm posture control system for work robots. The arm posture control device controls the hydraulic pressure to control the posture of the arm,
When returning to the posture of the arm before the detection of the predetermined error,
Detecting a difference in fluid pressure or fluid volume before and after the predetermined error, and controlling the arm based on the difference;
The arm posture control system for a working robot according to claim 1. The arm posture control device controls the hydraulic pressure to control the posture of the arm,
When returning to the basic posture,
Control by minimizing the fluid pressure or fluid volume related to all of the plurality of drive locations of the arm,
The arm posture control system for a working robot according to claim 1. The arm posture control device controls the hydraulic pressure to control the posture of the arm,
When returning to the basic posture,
Control is performed by maximizing the hydraulic pressure or the liquid amount related to all of the plurality of driving locations of the arm,
The arm posture control system for a working robot according to claim 1. The arm posture control device controls the hydraulic pressure to control the posture of the arm,
When returning to the basic posture,
By minimizing the hydraulic pressure or amount of fluid relating to at least one of the plurality of driving locations of the arm, and maximizing the hydraulic pressure or amount of fluid relating to at least one of the plurality of driving locations of the arm. Control,
The arm posture control system for a working robot according to claim 1. In the avoidance control,
Control is performed so as not to use the arm minute movement target amount immediately before detection of the predetermined error.
The arm posture control system for a working robot according to claim 1. In the avoidance control,
Control is performed so as to avoid the posture corresponding to the arm posture data immediately before detection of the predetermined error.
The arm posture control system for a working robot according to claim 1. In the avoidance control,
Mapping the three-dimensional coordinates of the marker corresponding to at least one immediately before and after detection of the predetermined error, defining an area to be avoided, and performing control to avoid the area;
The arm posture control system for a working robot according to claim 1. The predetermined error is
It is at least one of a case where a predetermined number of the markers could not be extracted, a case where the predetermined markers could not be extracted, and a case where the arm posture data could not be generated under a predetermined condition.
The arm posture control system for a working robot according to claim 1. An arm posture control method for a working robot including an arm having a movable part,
An input step of inputting a camera image obtained by photographing the marker added to the arm;
An image analysis step of extracting the marker from the camera image and generating three-dimensional coordinates of the marker;
An arm posture estimation step for generating arm posture data indicating the posture of the arm based on the three-dimensional coordinates of the marker and the structural model of the arm;
Obtaining an arm minute movement target amount based on the arm posture data, and executing an arm posture control step for controlling the posture of the arm based on the arm minute movement target amount;
When the arm posture data indicating the posture of the arm could not be generated,
The arm posture controlled based on the arm posture data, or return control to return to the basic posture,
Or
The situation where the arm posture data could not be generated is stored, and in the subsequent control of the posture of the arm, avoidance control is performed so as to avoid the situation.
Arm posture control method for work robot. Controlling the hydraulic pressure to control the posture of the arm,
When returning to the arm posture controlled based on the arm posture data,
Detecting a difference in fluid pressure or fluid amount before and after the timing at which the arm posture data can no longer be generated, and controlling the arm based on the difference;
The arm posture control method for a working robot according to claim 10. Controlling the hydraulic pressure to control the posture of the arm,
When returning to the basic posture,
Control by setting the fluid pressure or fluid amount to a plurality of drive locations of the arm to a predetermined set value,
The arm posture control method for a working robot according to claim 10. Controlling the hydraulic pressure to control the posture of the arm,
When returning to the basic posture,
Control by minimizing the fluid pressure or fluid volume related to all of the plurality of drive locations of the arm,
The arm posture control method for a working robot according to claim 12. Controlling the hydraulic pressure to control the posture of the arm,
When returning to the basic posture,
Control is performed by maximizing the hydraulic pressure or the liquid amount related to all of the plurality of driving locations of the arm,
The arm posture control method for a working robot according to claim 12. If the arm posture data indicating the posture of the arm could not be generated,
Including a case where a predetermined number of the markers could not be extracted, a case where the predetermined markers could not be extracted, and a case where the arm posture data could not be generated under a predetermined condition.
The arm posture control method for a working robot according to claim 10.

Images