CN101432103B - Apparatus for controlling robot arm - Google Patents

Abstract

A control apparatus (1) controls a robot arm (8) by a means (4), which is provided for controlling operation for coping with collision between human and the robot arm (8), by separately setting mechanical impedance for each joint of the robot arm (8) when a human proximity detecting means (3) detects proximity of human based on the human movement detected by a human movement detecting means (2).

Description

The control device of robot arm

Technical field

The present invention relates to control device and control method, robot, and the control program of robot arm of the robot arm that home-use robot etc. might contact with people's physics.

Background technology

In recent years, the exploitation of home-use robot such as pet type robot is very active, and the housework that waits in expectation in the future support robot etc. are the practicability of practical home-use robot more.Because home-use robot need get into family and people's community life, it is indispensable therefore contacting with people's physics, consider from the security aspect, and need be soft.

For problem so, as technology in the past, in patent documentation 1 (TOHKEMY is put down the 10-329071 communique) a kind of control device is disclosed; Its detection imposes on robot arm and the contact force people; When arm is applied bigger power, reduce recuperability, improve security; When arm is applied small power, increase recuperability, guarantee operation precision.

In addition, in patent documentation 2, also disclose a kind of control device, shown in Figure 11 A, it has: sensor mechanism 515, and it is the residing environment of detecting obstacles thing and the contact site and the contact force thereof that contact with robot 513 respectively; Posture measurement mechanism 514, it measures the posture of travel mechanism 511 and manipulator 512 respectively; Computer 517, its contact force from said contact force and posture calculating robot 513 is avoided action; Driver 516, it drives travel mechanism 511 and manipulator 512 according to the result of calculation of this computer 517.When the contacting of residing environment of barrier and robot 513, avoid action through both teamwork of travel mechanism 511 and manipulator 512.

In addition; A kind of impedance Control device is disclosed in patent documentation 3; Shown in figure 12; It detects the power from external environment condition that acts on the robot 401 with hand end operating means 411 for sense sensor 412 firmly, infers the stiffness coefficient of external environment condition in real time, the arm 410 of the control drive controlling robot 401 through motor 413a.

Patent documentation 1:JP spy opens flat 10-329071 communique

Patent documentation 2:JP spy opens communique 2005-No. 59161

Patent documentation 3:JP spy opens communique 2004-No. 223663

But, in above-mentioned control device in the past, reckon without the application on articulated robot arm, when being articulated robot arm, can not become the control device that guarantees security.

In addition, in above-mentioned control device in the past, also reckon without the people's who contacts activity, can not become the best control that conforms to people's activity.

In addition; In patent documentation 2; Shown in Figure 11 B and Figure 11 C, avoid path 526 and move along contacting to the direction of contact force 531 through making in the robot 513 and contact site 532 environment that has barrier, avoid action 527; Reduce contact force 531, finally avoid contact from environment.In addition; Shown in Figure 11 D and Figure 11 E; Contact-making surface through making in the robot 513 and contact site 532 environment that has barrier tilts to the direction of the moment 533 that takes place through contact force 531, and aforementioned contact site is moved with the mode of the circle 526 of describing arbitrary dimension, avoids action 527; Reduce contact force 531, finally avoid contact from environment.But, have no open for each concrete action control of avoiding action 527.

In addition; About patent documentation 3, shown in figure 12, have dependency relation ground with the stiffness coefficient of the external environment condition of calculating; The impedance operator of robot side is changed; Adopt these values to calculate the position of hand end operating means 411, only be conceived to the position of hand end operating means 411, not open fully for the concrete action control of the arm 410 that carries out in order the people not to be applied the contact infringement.

Summary of the invention

The object of the invention just is to solve the problem of above-mentioned control device in the past; A kind of control device and control method, robot, and the control program of robot arm of robot arm are provided; Even its articulated robot arm also can contact with the people safely, and, the contact action of the best that can carry out corresponding people's activity; Can the people not applied contact infringement ground and people's coexistence, can realize the action control of the safety of robot arm.

For achieving the above object the present invention such as following formation.

According to the 1st mode of the present invention, a kind of control device of articulated robot arm is provided, it has with lower unit:

Position of collision is obtained the unit, the position of collision of its acquisitor or moving body and said articulated robot arm;

Collision respective action control module; It obtains the said position of collision of people that the unit is obtained, said or moving body and said articulated robot arm based on said position of collision, and control makes connecting rod than the said articulated robot arm under the said position of collision near the rigidity of the joint portion of the root one side collision respective action lower than the rigidity of other joint portion.

According to the 9th mode of the present invention, a kind of control method of articulated robot arm is provided, wherein,

Detect near the people of said robot arm or the motion of moving body by motion detection unit;

Based on the said people who detects by said motion detection unit or the motion detection result of moving body, by surveying said people or moving body near probe unit near said robot arm;

Through said near probe unit detect said people or moving body near the time; Based on the said people who detects by said motion detection unit or the said motion of moving body; Infer the said position of collision of said people or moving body and said articulated robot arm; Obtain the unit through position of collision and obtain said position of collision; And; Through utilizing collision respective action control module, control makes than obtains through said position of collision the connecting rod of the said articulated robot arm under the said position of collision of obtaining the unit lower than the rigidity of other joint portion near the rigidity of the joint portion of root one side, carries out the collision respective action corresponding to the collision of said people and said robot arm.

According to the 10th mode of the present invention, a kind of robot is provided, it is characterized in that:

Have,

Said articulated robot arm,

Control the control device of any one the described robot arm in the 1st mode～the 8th mode of said robot arm;

Carry out said collision respective action through said collision respective action control module control corresponding to the collision of said people or moving body and said robot arm.

According to the 11st mode of the present invention, a kind of control program of robot arm is provided, be used to make computer to have as function like lower unit:

Position of collision is obtained the unit, the position of collision of its acquisitor or moving body and articulated robot arm;

Collision respective action control module; It obtains the said people that obtains the unit or the said position of collision of moving body and said articulated robot arm based on said position of collision, and control makes connecting rod than the said articulated robot arm under the said position of collision near the rigidity of the joint portion of the root one side collision respective action lower than the rigidity of other joint portion.

According to the control device of robot arm of the present invention and control method, robot, and the control program of robot arm, possess position of collision and obtain the unit with collision respective action control module or possess these functions.Therefore, the people during near robot arm, can be by the suitable gesture actions of corresponding people's motion or by the mechanical impedance action of suitable joint portion.In other words; Above-mentioned position of collision based on above-mentioned people or moving body and above-mentioned articulated robot arm; Be lower than the rigidity of other joint portion through making than the connecting rod of the above-mentioned articulated robot arm under the above-mentioned position of collision near the rigidity of the joint portion of root one side, it is movable to make easily near the joint portion of root one side, the power that applies when subduing collision by this joint portion; Thereby can reduce to impose on people's impulsive force, improve security.Thereby even articulated robot arm also can contact with the people safely, and the contact action of the best that can carry out corresponding people's activity can realize the people is not applied the action control of contact infringement ground and the robot arm of the safety of people's coexistence.

Description of drawings

Above-mentioned purpose of the present invention and characteristic, can from reference to accompanying drawing to illustrating the following narration of preferred embodiment carrying out of the present invention.Wherein:

Fig. 1 is that the formation and the control object of the control device of the robot arm in expression the 1st embodiment of the present invention is the key diagram of the formation of robot arm.

Fig. 2 is the sketch map of the collision warning region of the robot arm in the control device of the above-mentioned robot arm of explanation in above-mentioned the 1st embodiment of the present invention.

Fig. 3 A is the people's motion detection unit of the above-mentioned control device in above-mentioned the 1st embodiment of explanation and the vertical view of the function that position of collision is inferred the unit.

Fig. 3 B is the people's motion detection unit of the above-mentioned control device in above-mentioned the 1st embodiment of explanation and the vertical view of the function that position of collision is inferred the unit.

Fig. 3 C is the people's motion detection unit of the above-mentioned control device in above-mentioned the 1st embodiment of explanation and the vertical view of the function that position of collision is inferred the unit.

Fig. 3 D is the people's motion detection unit of the above-mentioned control device in above-mentioned the 1st embodiment of explanation and the vertical view of the function that position of collision is inferred the unit.

Fig. 4 is the block diagram of detailed formation of the action control unit of the above-mentioned control device of expression in above-mentioned the 1st embodiment.

Fig. 5 is the flow chart of the action step of the control program in the action control unit of the above-mentioned control device of expression in above-mentioned the 1st embodiment.

Fig. 6 is the flow chart of all action step of the above-mentioned control device in above-mentioned the 1st embodiment of expression.

Fig. 7 is the sketch map of target track of the control device of the above-mentioned robot arm of explanation in above-mentioned the 1st embodiment of the present invention.

Fig. 8 A is that the formation and the control object of the control device of the robot arm in expression the 2nd embodiment of the present invention is the sketch map of the formation of robot arm.

Fig. 8 B is the block diagram of detailed formation of the action control unit of the above-mentioned control device of expression in above-mentioned the 2nd embodiment.

Fig. 9 A is the sketch map of action of the control device of the above-mentioned robot arm of explanation in above-mentioned the 2nd embodiment of the present invention.

Fig. 9 B is the sketch map of action of the control device of the above-mentioned robot arm of explanation in above-mentioned the 2nd embodiment of the present invention.

Fig. 9 C is the sketch map of action of the control device of the above-mentioned robot arm of explanation in above-mentioned the 2nd embodiment of the present invention.

Fig. 9 D is the sketch map of action of the control device of the above-mentioned robot arm of explanation in above-mentioned the 2nd embodiment of the present invention.

Figure 10 is that the control object of the control device of the robot arm in expression the 2nd embodiment of the present invention is the sketch map of other formation of robot arm.

Figure 11 A relates to the concise and to the point pie graph of the robot controller of technology in the past of patent documentation 2.

Figure 11 B is the sketch map of the concrete example of the avoidance contact action in the robot controller of presentation graphs 11A.

Figure 11 C is the sketch map of the above-mentioned concrete example of the avoidance contact action in the robot controller of presentation graphs 11A.

Figure 11 D is the sketch map of another concrete example of the avoidance contact action in the robot controller of presentation graphs 11A.

Figure 11 E is the sketch map of the above-mentioned another concrete example of the avoidance contact action in the robot controller of presentation graphs 11A.

Figure 12 relates to the block diagram of the robot controller of technology in the past of patent documentation 3.

The specific embodiment

Before continuation narration of the present invention, need to prove, for parts additional phase identical in accompanying drawing reference marks together.

Below, with reference to accompanying drawing embodiment of the present invention is at length explained.

Below, before the embodiment among the present invention at length being explained, earlier variety of way of the present invention is described with reference to accompanying drawing.

According to the 1st mode of the present invention, a kind of control device of articulated robot arm is provided, it has:

Position of collision is obtained the unit, the position of collision of its acquisitor or moving body and said articulated robot arm;

Collision respective action control module; It obtains the said people that obtains the unit or the said position of collision of moving body and above-mentioned articulated robot arm based on said position of collision, and respective action is collided in control to be made lower than the rigidity of other joint portion near the rigidity of the joint portion of root one side than the connecting rod of the said articulated robot arm under the said position of collision.

According to the 2nd mode of the present invention; A kind of control device of as the described robot arm of the 1st mode is provided; Wherein, Said collision respective action control module, control carry out said collision respective action makes the rigidity near the joint portion of said root one side of connecting rod of the said articulated robot arm under the said position of collision lower than the rigidity of other joint portion.

According to the 3rd mode of the present invention; Control device a kind of as the 1st mode or the described robot arm of the 2nd mode is provided; Wherein, Said collision respective action control module, through keeping than the connecting rod of the said articulated robot arm under said position of collision rigidity or the additional bigger rigidity near the joint portion of a side of wrist portion, said collision respective action is carried out in control.

According to the 4th mode of the present invention; Control device a kind of as the 1st mode or the described robot arm of the 2nd mode is provided; Wherein, Said collision respective action control module is controlled joint angles through the said connecting rod that contrasts the said articulated robot arm under the said position of collision respectively near each joint portion of a side of wrist portion, and said collision respective action is carried out in control.

According to the 5th mode of the present invention, control device a kind of as the 1st mode or the described robot arm of the 2nd mode is provided, wherein,

The motion detection unit that also possesses the motion that detects said people or moving body;

Said position of collision is obtained the unit, based on the testing result of said motion detection unit, infers the said position of collision of said people or moving body and said articulated robot arm, obtains said position of collision;

Said collision respective action control module, based on obtained the said position of collision that infer the unit by said position of collision, said collision respective action is carried out in control.

According to the 6th mode of the present invention, a kind of control device of as the described robot arm of the 5th mode is provided, wherein,

Also possess testing result based on said motion detection unit, survey said people or moving body near said robot arm near probe unit;

Said collision respective action control module, said near probe unit detect said people or moving body near the time, said collision respective action is carried out in control.

According to the 7th mode of the present invention; Control device a kind of as the 5th mode or the described robot arm of the 6th mode is provided; Wherein, Said collision respective action control module, the speed composition of the said people who detects based on said motion detection unit or the motion of moving body, said collision respective action is carried out in control.

According to the 8th mode of the present invention, the control device of any one described robot arm in a kind of as the 5th mode to the 7 modes is provided, wherein, said motion detection unit detects the position and the translational speed of said people or moving body, and detects said people's motion.

According to the 9th mode of the present invention, a kind of control method of articulated robot arm is provided, wherein,

Detect near the people of said robot arm or the motion of moving body by motion detection unit;

Based on the said people who detects by said motion detection unit or the motion detection result of moving body, by surveying said people or moving body near probe unit near said robot arm;

Through said near probe unit detect said people or moving body near the time; Based on the said people who detects by said motion detection unit or the said motion of moving body; Infer the said position of collision of said people or moving body and said articulated robot arm; Obtain the unit through position of collision and obtain said position of collision; And; Through utilizing collision respective action control module, control makes than obtains through said position of collision the connecting rod of the said articulated robot arm under the said position of collision of obtaining the unit lower than the rigidity of other joint portion near the rigidity of the joint portion of root one side, carries out the collision respective action corresponding to the collision of said people and said robot arm.

According to the 10th mode of the present invention, a kind of robot is provided, it is characterized in that:

Have, said articulated robot arm,

Control the control device of any one the described robot arm in the 1st mode～the 8th mode of said robot arm;

Carry out said collision respective action through said collision respective action control module control corresponding to the collision of said people or moving body and said robot arm.

According to the 11st mode of the present invention, a kind of control program of robot arm is provided, be used to make computer to have as function like lower unit:

Position of collision is obtained the unit, the position of collision of its acquisitor or moving body and articulated robot arm;

Collision respective action control module; It obtains the said position of collision of people that the unit is obtained, said or moving body and said articulated robot arm based on said position of collision, and control makes connecting rod than the said articulated robot arm under the said position of collision near the rigidity of the joint portion of the root one side collision respective action lower than the rigidity of other joint portion.

Below, with reference to accompanying drawing embodiment of the present invention is at length explained.

(the 1st embodiment)

Fig. 1 is that formation and the control object thereof of the control device 1 of the articulated robot arm of expression in the 1st embodiment of the present invention is the sketch map of the formation of articulated robot arm 8.The control device 1 of this robot arm is the control device of control method of implementing the robot arm of above-mentioned the 1st embodiment; Can be used for possessing the robot of articulated robot arm 8; As after state, also can be used for the control program of articulated robot arm 8.

In Fig. 1; The 2nd, as motion detection unit one the example people's motion detection unit; According to using first-class image pickup device 36 shot image data of shooting; Carry out image recognition processing by people's motion detection unit 2, detect near the people's 38 of robot arm 8 the position and the information of translational speed and moving direction by people's motion detection unit 2.

The 3rd, as near probe unit one the example the people near probe unit; By the testing result of people's motion detection unit 2 be the people position information and from after the relation of information of posture of the robot arm 8 obtained of the joint angles information that obtains of the action control unit 7 stated by robot arm 8, detect people 38 near robot arm 8.

The 5th, obtain the position of collision of an example of unit as position of collision and infer the unit; The approaching people's 38 who is detected by people's motion detection unit 2 positional information and people 38 translational speed and moving direction are moving velocity vector information; Infer robot arm 8 and people's 38 collision precalculated position (can be used as the position that position of collision is handled), with inferring the collision respective action generation unit of stating after the result exports to 6.

The 4th, collision respective action control module; The control of the action of the robot arm 8 when using robot arm 8 to carry out operation (specifically is; The control of the value of the mechanical impedance of above-mentioned robot arm 8); The people near probe unit 3 detect people 38 to robot arm 8 near the time, based on the testing result of people's motion detection unit 2, the collision respective action the during collision of control people 38 and robot arm 8.

Collision respective action control module 4 by collision respective action generation unit 6, and action control unit 7 constitute.

Collision respective action generation unit 6; The collision precalculated position of inferring unit 5 is inferred in calculating at position of collision; The joint target track of the robot arm 8 when robot arm 8 contacts with people 38 and the mechanical impedance value of joint target are exported to action control unit 7 with result of calculation.

The posture of the finger of action control unit 7 control robot arms 8; Realize target job with the finger of robot arm 8, and, under the approaching situation of people 38; Switching controls makes follows the joint target track that collision respective action generation unit 6 generates; Carry out impedance Control simultaneously, the value of mechanical impedance of each joint portion of control robot arm 8 reaches the mechanical impedance value of the joint target that collision respective action generation unit 6 generates.Here, each joint portion of robot arm 8, as after state, via the motor driver 19 that is connected on D/A converter 21 and the A/D converter 22, under impedance Control, drive motor 34 as an example of the drive unit of each joint portion, carry out flexure operation respectively.

Control device 1; Constitute by common universal microcomputer as hardware; Except people's motion detection unit 2, people near probe unit 3, position of collision infer unit 5, collision respective action generation unit 6, and action control unit 7 in input and output IF (input/output interface) 20 the part utilization can realize by software by the control program of universal microcomputer execution 18.

Input and output IF20 is made up of the D/A converter 21 on the docking station of the pci bus that is connected universal microcomputer etc., A/D converter 22, counting converter 23.

The control program 18 that is used to control the action of robot arm 8 through execution plays a role control device 1; Joint angles information by 35 outputs of the encoder on the motor 34 of each joint portion that is connected robot arm 8; Input to control device 1 through counting converter 23, calculate the control instruction value of the spinning movement of each joint portion through control device 1.Each control instruction value of calculating flows to motor driver 19 through D/A converter 21, according to each control instruction value of seeing off from motor driver 19, the motor 34 of each joint portion of drive machines human arm 8.

Robot arm 8 is many link robot of 4DOF, has: hand 9, have at front end the 2nd connecting rod 11 of wrist portion 10 that hand 9 is installed, rotatably link the 1st connecting rod 12 of the 2nd connecting rod 11, rotatably link the base portion 13 of supporting the 1st connecting rod 12.Wrist portion 10 has 2 rotating shafts (rotating shaft along the vertical direction and along horizontal rotating shaft) of the 3rd joint portion 16 and the 4th joint portion 17, can make hand 9 and the 2nd connecting rod 11 relative posture (towards) variation.The other end of the 2nd connecting rod 11 is with respect to the front end of the 1st connecting rod 12; Can be around the rotating shaft rotation along above-below direction of the 2nd joint portion 15; The other end of the 1st connecting rod 12 is with respect to base portion 13; Can be around the rotating shaft rotation along above-below direction of the 1st joint portion 14, total can so constitute many link robot of above-mentioned 4DOF around 4 axle rotations.

Each joint portion at the rotating part that constitutes each possesses: on the parts in 2 parts that link by each joint portion set and by after motor driver 19 drive controlling stated as the motor 34 (in fact being located at the inside of each joint portion of robot arm 8) of an example of rotating driving device and detect the encoder 35 (in fact being provided in the inside of each joint portion of robot arm 8) at rotatable phase angle (being joint angle) of the rotating shaft of motor 34.The rotating shaft of motor 34 is attached on another parts of 2 parts that linked by each joint portion, through making the positive and negative rotation of above-mentioned rotating shaft, makes another parts center on each rotation with respect to parts.

The 32nd, the absolute coordinate system with respect to the relative position relation of the downside fixed part of base portion 13 is fixed is made as O with its initial point ₀The 33rd, concern the finger coordinate system that is fixed with respect to hand 9 relative positions.The posture (φ, θ) that the origin position Oe (x, y) of the finger coordinate system 33 that will see from absolute coordinate system 32 is defined as the finger position of robot arm 8, the finger coordinate system 33 that will see from absolute coordinate system 32 is defined as the finger gesture of robot arm 8, and finger position/posture vector r is defined as r=[x, y, φ, θ] ^TUnder the situation of the finger position of controlling robot arm 8 and posture, make finger position/posture vector r follow target finger position/posture vector r _Wd, so availability control unit 1 is controlled.

Then, detailed formation and the effect to each functional module of constituting control device 1 describes.

By image pickup device 36 shot image data,, detect the people's 38 when the ceiling in the room that disposes robot arm 8 is overlooked ground two-dimensional position X to people's motion detection unit 2 input through the image recognition processing in people's motion detection unit 2 _p(x _p, y _p) and people 38 moving velocity vector V _p(v _x, v _y), with two-dimensional position X _p(x _p, y _p) and moving velocity vector V _p(v _x, v _y) information export to the people by people's motion detection unit 2 and infer unit 5 near probe unit 3 and position of collision.

The people is near probe unit 3 definition collision warning region 100 shown in Figure 2, survey and judge whether people 38 gets into should zone 100, whether people 38 near robot arm 8.

The sector region 100A of the base end side of collision warning region 100 is the length L with the 1st connecting rod 12 ₁α ₁Doubly be α ₁L ₁Being radius, is round dot with the 1st joint shaft (rotating shaft of the 1st joint portion 14), with the joint angle q of the 1st joint portion 14 ₁The central shaft of for example the 1st connecting rod 12 be the center ± β ₁° the zone of angle.In addition, the sector region 100B of the front of collision warning region 100 is the length L with the 2nd connecting rod 11 ₂Length L with hand 9 _Hα ₂Doubly, i.e. α ₂(L ₂+ L _H) be radius, be round dot with the 2nd joint shaft (rotating shaft of the 2nd joint portion 15), with the joint angle q of the 2nd joint portion 15 ₂The central shaft of for example the 2nd connecting rod 11 be the center ± β ₂° the zone of angle.Thus, even make the range of sector region with speed, can prevent really that also the finger of hand 9 from reaching outside the sector region 100B of front with being directly proportional.

The α of definition collision warning region 100 ₁, α ₂, β ₁, β ₂Value be to be worth arbitrarily, but can be based on the digit speed V that for example makes robot arm 8 action _r={ (dx/dt) ²+ (dy/dt) ²} ^1/2Confirm these values, if with digit speed V _rStrengthen α with being directly proportional ₁, α ₂, β ₁, β ₂The mode of value confirm that then under the situation of the quick action of robot arm 8, the area of collision warning region 100 increases, and can improve security.

In the 1st embodiment, for example, with digit speed V _rUnit be set at m/s, and with V _KBe set at according to V _rSize and V _rThe value that is directly proportional or for the value of constant is set at α ₁=1.2V _K, α ₂=1.2V _K, β ₁=24V _K, β ₂=24V _KWherein, at V _KDuring for following [formula 1], be by V _K=1.9V _rThe value (parameter) of definition is at V _r<0.5 when [ms], be to be defined as V _K=0.95 with the value (parameter) of constant.

[formula 1]

V _r≥0.5[m/s]

Thus, be 0.5 [m/s] when above in digit speed, confirm to collide the size of warning region 100 with the size of digit speed with being directly proportional.In addition, when being lower than 0.5 [m/s], become fixed value α respectively ₁=1.14, α ₂=1.14, β ₁=22.8, β ₂=22.8, even under the low situation of digit speed, also can guarantee the collision warning region 100 of size to a certain degree.

The people is near probe unit 3; The joint angles information of the information of the people's 38 who detects by people's motion detection unit 2 position and the robot arm 8 that obtains from action control unit 7; Calculate the relativeness of people 38 and robot arm 8; If people 38 gets into collision warning region 100, just the people is invaded this advisory and give collision respective action control module 4.

Position of collision is inferred unit 5, and the information of the people's 38 who is detected by people's motion detection unit 2 position and the information of moving velocity vector are inferred the collision precalculated position [formula 2] with people 38, and output it to collision respective action generation unit 6.

[formula 2]

X _p(x _i，y _i)

For example, shown in Fig. 3 A, imagination and velocity V _p(v _x, v _y) extended line intersect vertically the straight line A of the rotating shaft of the 1st joint portion 14 through robot arm 8.If the angle initialization that the x axle of straight line A and absolute coordinate system 32 is formed is q _1i, with velocity V _p(v _x, v _y) the intersection point of extended line and straight line A be set at collision precalculated position [formula 3], then provide angle q by following [formula 4]～[formula 7] _1i

[formula 3]

X _p(x _i，y _i)

[formula 4]

X _p(x _i，y _i)

[formula 5]

q _1i＝π/2+atan2(v _x，v _y)

[formula 6]

$x_i=\frac{(\mathrm{\&pi;}/2-q_{1i})x_p+y_p}{\tan q_{1i}-q_{1i}+\mathrm{\&pi;}/2}.$

[formula 7]

y _i＝x _itanq _1i

Then, with reference to Fig. 3 A～Fig. 3 C, the details that collides respective action generation unit 6 is described.

Collision respective action generation unit 6, the state (current state) during from the common operation of Fig. 3 A generates the joint target track to the prevention collision posture of Fig. 3 B.

The line of the central shaft of the 1st connecting rod 12 of robot arm 8 is confirmed the angle q of the 1st joint portions 14 through colliding respective action generation unit 6 overlappingly with collision precalculated position [formula 8] _1iThat is q, _1i=atan2 (y _i, x _i).Wherein, atan2 (y _i, x _i)=arg (y _i+ jx _i), be that j is that imaginary number, arg are plural drift angle.In addition, the angle q of the 2nd joint portion 15 _2i, surround people 38 in order to make robot arm 8, for example be set at 60 ° fixed value.

[formula 8]

X _p(x _i，y _i)

The angle initialization of the 1st joint portion 14 of the robot arm 8 during with the corresponding posture of collision is q _1iIn addition, about the angle q of the 2nd joint portion 15 _2i, regulation is according to the initial point O of collision precalculated position [formula 9] and absolute coordinate system 32 ₀Between distance L _Xi, make its variation by these 3 patterns of following contact mode (1)～(3).

[formula 9]

X _p(x _i，y _i)

Contact mode shown in Fig. 3 B (1):

[formula 10]

L _xi≤L ₁×0.8

In this case, set q _2i=90 °.

Contact mode shown in Fig. 3 C (2):

[formula 11]

L ₁×0.8<L _xi≤L ₁×1.2

In this case, set q _2i={ (L _Xi-L ₁)/0.4 * L ₁} * 90 °.

At the contact mode (3) shown in Fig. 3 D: L ₁* 1.2<l _XiThe time, be set at q _2i=0 °.

Thus, robot arm 8 can follow following such posture to carry out the impedance Control action.

When above-mentioned contact mode (1), shown in Fig. 3 B,, be the posture of the 2nd connecting rod 11 with respect to 90 ° of the 1st connecting rod 12 bendings owing to occur in the 1st connecting rod 12 with contacting of people 38, therefore become the posture that robot arm 8 surrounds people 38.

When above-mentioned contact mode (2), shown in Fig. 3 C, become with near contacting of people 38 occurs in the 2nd joint portion 15 the opening angle q of the 2nd joint portion 15 _2iThe posture that changes according to contact position with people 38.

When above-mentioned contact mode (3), shown in Fig. 3 D, occur in the angle q of the 2nd connecting rod 11, the 2 joint portions 15 with contacting of people 38 _2iBe 0 °, robot arm 8 becomes the 1st connecting rod 12 and the 2nd connecting rod 11 posture point-blank.

In the action of impedance Control so, from the currency (q of the joint angles of robot arm 8 ₁, q ₂), 5 order polynomial interpolations through following [formula 12] are by collision respective action generation unit 6, calculate when obtaining contact action to joint angles (q _1i, q _2i) target track be joint angles desired value q _Nd(t).Here, t is the time.

[formula 12]

q _nd(t)＝a ₀+a ₁t+a ₂t ²+a ₃t ³+a ₄t ⁴+a ₅t ⁵

Wherein,

[formula 13]

a _o＝q _n，

[formula 14]

$a_1={\stackrel{.}{q}}_n$

[formula 15]

$a_2=\frac{1}{2}{\stackrel{..}{q}}_n$

[formula 16]

$a_3=\frac{1}{2{t_f}^3}(20q_{n1}-20q_n-12{\stackrel{.}{q}}_0{t}_f-3{\stackrel{..}{q}}_n{t_f}^2)$

[formula 17]

$a_4=\frac{1}{2{t_f}^4}(30q_n+16{\stackrel{.}{q}}_n{t}_f+3{\stackrel{..}{q}}_n{t_f}^2)$

[formula 18]

$a_5=\frac{1}{2{t_f}^5}(-12q_n-6{\stackrel{.}{q}}_n{t}_f-{\stackrel{..}{q}}_n{t_f}^2).$

[formula 19]

n＝1，2，

In addition, t _fBe the release time of impedance Control action.

Collision respective action generation unit 6, as the joint angles desired value, will be by the joint angles desired value q of formula (12) calculating _Nd(t) export to action control unit 7.

In addition, collision respective action generation unit 6, the people's 38 during according to contact mode (1)～(3) and collision speed composition, the mechanical impedance value of each joint portion when confirming the corresponding posture of the collision of Fig. 3 B～Fig. 3 D.As the setup parameter of mechanical impedance value, inertia I, viscosity D, rigidity K are arranged.

When contact mode (1) and (2): the composition v vertical in the people's 38 during according to contact the speed with the 1st and the 2nd connecting rod 12,11 _I1, v _I2, through I _n=K _I/ v _In, D _n=K _D/ V _In, K _n=K _K/ V _InConfirm the mechanical impedance parameter of each joint portion.Wherein, n=1,2.In addition, K _I, K _D, K _KBe constant gain, obtain concrete numerical value through experiment in order to reach safe action.In addition, confirm the higher limit Imax of each parameter _n, Dmax _n, Kmax _n, when calculated value surpasses these higher limits, be regarded as each parameter and equate with higher limit.

When contact mode (3):, be made as I about the 1st joint portion 14 ₁=Imax ₁, D ₁=Dmax ₁, K ₁=Kmax ₁, about the 2nd joint portion 15, according to composition v vertical in the people's 38 in when contact the speed with the 2nd connecting rod 11 _I2, through I ₂=K _I/ v _I2, D ₂=K _D/ V _I2, K ₂=K _K/ V _I2Confirm the mechanical impedance parameter of the 2nd joint portion 15.

About the mechanical impedance of wrist portion 10, usually, higher limit is I _n=Imax _n, D _n=Dmax _n, K _n=Kmax _n, wherein, establish n=3,4.

Higher limit is made as enough big value, for example is Imax _n=Dmax _n=Kmax _n=10000, wherein establish n=1,2,3,4.

If confirmed the mechanical impedance of joint portion as stated; Then in contact mode (1) and (2); For example, when people 38 and the collision of the 1st connecting rod 12 approximate vertical, because

; Therefore at the 1st joint portion 14; Set the mechanical impedance value with comparing before the impedance Control action, it is soft that the 1st joint portion 14 becomes, at the 2nd joint portion the 15, the 3rd joint portion 16 and the 4th joint portion 17 smallerly; The mechanical impedance value is set in higher limit with comparing before the impedance Control action, the 2nd joint portion the 15, the 3rd joint portion 16 and the 4th joint portion 17 become firm.That is to say, can through collision respective action control module 4 make the 1st connecting rod 12 under the position of collision with the 1st connecting rod 12 compare near a side of root (base portion 13) promptly the rigidity of the 1st joint portion 14 be lower than the for example collision respective action of the rigidity of the 2nd joint portion the 15, the 3rd joint portion 16 and the 4th joint portion 17 of another joint portion.Therefore, through the flexibility of utilizing the 1st joint portion 14 bump of collision is relaxed, make the 2nd joint portion 15 firm simultaneously, the 2nd connecting rod 11 can be inadvertently unmovable because of the reaction force of collision, and the 2nd connecting rod 11 can not collide people 38 yet, can guarantee security.In addition; Through constituting the L font by the 1st connecting rod 12 and the 2nd connecting rod 11; Surround people 38, can play effect, can prevent that people 38 from falling down (through when people 38 falls down, catching people 38 by the 1st connecting rod 12 or the 2nd connecting rod 11 as the support of people 38 front side and horizontal side two directions; Can prevent that people 38 from falling down), can improve security more.

In addition, in contact mode (2), even the speed composition v vertical with the 2nd connecting rod 11 arranged _I2Situation under because according to speed composition v _I2, relatively the rigidity of the 2nd joint portion 15 is set in the lowland, and the 2nd joint portion 15 becomes soft, therefore can guarantee security.

In addition, in contact mode (3) since with impedance Control action before compare the 1st joint portion 14 and become firm, according to composition v vertical in people 38 the translational speed with the 2nd connecting rod 11 _I2More mildly set than the collision precalculated position of the 2nd connecting rod 11 under the collision precalculated position near the joint portion of a side of root (base portion 13) i.e. the 2nd joint portion 15 with comparing before the impedance Control action; Thereby the bump of flexibility absorption people 38 collisions the 2nd connecting rod 11 of enough the 2nd joint portions 15 of ability, so can guarantee security.In addition, with above-mentioned opposite, and mildly set the 1st joint portion 14, compare when firmly setting the 2nd joint portion 15, the inertia that applies during collision mainly is the inertia of the 2nd connecting rod 11, owing to do not apply the inertia of the 1st connecting rod 12, the bump in the time of therefore can reducing to collide.

Have again; In contact mode (3); Except mildly set the 1st joint portion 14, when firmly setting the 2nd joint portion 15; Also can consider to make simultaneously the 1st joint portion 14 and the 2nd joint portion 15 softer, that is to say, make than 2nd connecting rod 11 of collision under the precalculated position near all joint portions of a side of root (base portion 13) than its soft method of joint portion in addition than the 3rd other joint portion 16 and the 4th joint portion 17.

In the method, have at joint portion under the situation of movable range, can be safer.When each joint portion has movable range,,,, therefore also just can not bring into play flexibility owing to can not make the joint portion over-activity if exceed movable range because of collision makes joint portion movable.; Make near the joint portion of root (base portion a 13) side when all soft than the joint portion beyond it; Because movable range through addition the 1st joint portion 14 and the 2nd joint portion 15; Can obtain bigger movable range, therefore can enlarge the scope that to bring into play flexibility, thereby can improve security.

Then, with reference to Fig. 4 the detailed formation of action control unit 7 is described.

In Fig. 4, the 8th, the control object is a robot arm shown in Figure 1.From robot arm 8, export currency (joint angles vector) q=[q of the joint angle of encoder 35 measurements of passing through joint shaft separately ₁, q ₂, q ₃, q ₄] ^T, input to control device through counting converter 23.Wherein, q ₁, q ₂, q ₃, q ₄It is respectively the joint angles of the 1st joint portion the 14, the 2nd joint portion the 15, the 3rd joint portion the 16, the 4th joint portion 17.

The 24th, operative goals track generation unit, output is as the finger position/posture target vector r of operation that is used to realize robot arm 8 of target _WdAs shown in Figure 7, the target action of robot arm 8 according to the purpose operation, provides the position (r of each taught point in advance _Wd0, r _Wd1, r _Wd2), target track generation unit 24 uses polynomial interpolators, repairs the track between the position of each point, generates finger position/posture target vector r _WdWherein, above-mentioned position r _Wd0Position when being assumed to be time t=0, r _Wd1Be assumed to be time t=t ₁The time position, r _Wd2Be assumed to be time t=t ₂The time the position.

Input to the finger position/posture target vector r of inverse kinematics computing unit 25 by target track generation unit 24 _Wd, be transformed into joint target vector q through inverse kinematics computing unit 25 _Wd

The 26th, the target track switch unit is to target track switch unit 26, by inverse kinematics computing unit 25 input joint target vector q _Wd, by collision respective action generation unit 6 input collision respective action joint target vector q _Id, collide the respective action switching commands near probe unit 3 inputs by the people.Usually, the finger position/posture target vector r that generates by operative goals track generation unit 24 _WdBe transformed to joint target vector q by inverse kinematics computing unit 25 _Wd, target track switch unit 26 is selected the joint target vector q of above-mentioned conversion _Wd, as joint target vector q _dBy 26 outputs of target track switch unit.But, if the people near the time target track switch unit 26 accept collision respective action switching commands from the people near probe unit 3, then target track switch unit 26 is selected the contact action joint target vector q by 6 inputs of collision respective action generation unit _Id, as joint target vector q _dBy 26 outputs of target track switch unit.

The 27th, moment of torsion is inferred the unit, infers the external force moment of torsion that imposes on each joint portion of robot arm 8 because of people 38 and contacting of robot arm 8.Infer unit 27 to moment of torsion, via in the motor 34 of each joint portion of drive machines human arm 8 the value of current flowing i=[i of A/D converter 22 input by the current sensor measurement of motor driver 19 ₁, i ₂, i ₃, i ₄] ^T, in addition, by the currency q of joint error compensation unit 30 input joint angles and after the joint angles error compensation output u that states _QeMoment of torsion is inferred unit 27 and is had the function as observer, by currency q, the joint angles error compensation output u of above current value i, joint angle _QeCalculate the torque T that occurs in each joint portion because of the external force that imposes on robot arm 8 _Ext=[T _1ex, T _2ext, T _3ext, T _4ext] ^T

Impedance computation unit, joint 28 is to have the part that can realize the function of mechanical impedances at robot arm 8, when the common action that people 38 keeps off is 31 input side output 0 to Position Control by impedance computation unit 28, joint.On the other hand; If people 38 is approaching; Receive the contact action switching command from collision respective action generation unit 6; The external force torque T of inferring unit 27 through following formula (20), is inferred from the currency q and the moment of torsion of the mechanical impedance parameter I set by collision respective action generation unit 6, D, K, joint angle in the impedance computation unit 28 in the joint then _Ext, calculate the joint target correction output q that is used for realizing mechanical impedance at each joint portion of robot arm 8 _{The d Δ}, export to Position Control then and be 31 input side.With joint target correction output q _{The d Δ}, be the joint target q of 31 input side and 26 outputs of target track switch unit in Position Control _dAddition generates and revises joint target vector q _Dm, and to input to Position Control be 31.

[formula 20]

$q_{\mathrm{d\&Delta;}}={(s^2\hat{I}+s\hat{D}+\hat{K})}^{-1}{\mathrm{\&tau;}}_{\mathrm{ext}}$

In the formula, s is a Laplace operator, in addition,

[formula 21]

$\hat{I}=\left[\begin{array}{cccc}{I}_1& 0& 0& 0\\ 0& I_2& 0& 0\\ 0& 0& I_3& 0\\ 0& 0& 0& {\mathrm{<figure-callout\; id="4"\; label="I"\; filenames="H2007800149419E00011.png,H2007800149419E00091.png"\; state="\{\{state\}\}">I</figure-callout>}}_4\end{array}\right]$

[formula 22]

$\hat{D}=\left[\begin{array}{cccc}{\mathrm{<figure-callout\; id="1"\; label="D"\; filenames="H2007800149419E00132.png"\; state="\{\{state\}\}">D</figure-callout>}}_1& 0& 0& 0\\ 0& {\mathrm{<figure-callout\; id="2"\; label="D"\; filenames="H2007800149419E00091.png,H2007800149419E00101.png"\; state="\{\{state\}\}">D</figure-callout>}}_2& 0& 0\\ 0& 0& {\mathrm{<figure-callout\; id="3"\; label="D"\; filenames="H2007800149419E00051.png,H2007800149419E00091.png"\; state="\{\{state\}\}">D</figure-callout>}}_3& 0\\ 0& 0& 0& {\mathrm{<figure-callout\; id="4"\; label="D"\; filenames="H2007800149419E00011.png,H2007800149419E00091.png"\; state="\{\{state\}\}">D</figure-callout>}}_4\end{array}\right]$

[formula 23]

$\hat{K}=\left[\begin{array}{cccc}{\mathrm{<figure-callout\; id="1"\; label="K"\; filenames="H2007800149419E00132.png"\; state="\{\{state\}\}">K</figure-callout>}}_1& 0& 0& 0\\ 0& {\mathrm{<figure-callout\; id="2"\; label="K"\; filenames="H2007800149419E00091.png,H2007800149419E00101.png"\; state="\{\{state\}\}">K</figure-callout>}}_2& 0& 0\\ 0& 0& {\mathrm{<figure-callout\; id="3"\; label="K"\; filenames="H2007800149419E00051.png,H2007800149419E00091.png"\; state="\{\{state\}\}">K</figure-callout>}}_3& 0\\ 0& 0& 0& {\mathrm{<figure-callout\; id="4"\; label="K"\; filenames="H2007800149419E00011.png,H2007800149419E00091.png"\; state="\{\{state\}\}">K</figure-callout>}}_4\end{array}\right]$

The 30th, Position Control is the joint error compensation unit in 31, the currency q and the joint revise goal vector q of the joint angles vector of in robot arm 8, measuring _DmError q _eBe transfused to joint error compensation unit 30, joint error compensation exported u by joint error compensation unit 30 _QeTo robot arm 8 outputs.

Joint error compensation output u _Qe, via D/A converter 21, passing to motor driver 19 as voltage instruction value, positive and negative rotation drives each joint shaft, robot arm 8 actions.

Have again, above-mentioned action control unit 7, as stated, inferring unit 27, impedance computation unit 28, joint and Position Control by target track generation unit 24, inverse kinematics computing unit 25, target track switch unit 26, moment of torsion is 31 to constitute.

Action control unit 7 about by above formation describes its work principle.

What work is the FEEDBACK CONTROL (Position Control) of joint vector (currency of the joint angle) q of joint error compensation unit 30 formation basically, and the part that the dotted line of Fig. 4 surrounds is that Position Control is 31.As joint error compensation unit 30, for example, if use the PID compensator, then with joint error q _e0 the mode of being punctured into is controlled, and realizes the action as the robot arm 8 of target.

When detecting people 38 to the intrusion of collision warning region 100 near probe unit 3 through the people, be 31 with respect to the Position Control of above-mentioned explanation, through impedance computation unit, joint 28, be 31 input side in Position Control, with joint target correction output q _{The d Δ}Joint target q with 26 outputs of target track switch unit _dThe correction of the desired value of joint portion is carried out in addition.Therefore, above-mentioned Position Control be the desired value of 31 joint portion than original value slight misalignment, the result can realize mechanical impedance.Because through type (20) is by impedance computation unit, joint 28 calculating joint target correction output q _{The d Δ}Therefore, can realize the mechanical impedance of inertia I, viscosity D, rigidity K.

To realizing real work step, describe based on the flow chart of Fig. 5 based on the control program of the work of the action control unit 7 of above principle.

In step 1, will input to control device 1 by the joint angles data (joint parameter vector or joint angles vector q) that encoder 35 is measured.

Then; In step 2; Based on the operation control program 18 of the robot arm 8 in the memory that is stored in control device 1 in advance (not shown), through the operative goals track generation unit 24 of action control unit 7, the finger position of computing machine human arm 8/posture target vector r _Wd

Then, in step 3, in inverse kinematics computing unit 25 with finger position/posture target vector r _WdBe transformed into joint target q _Wd

Then, in step 4, carry out the switching of target track through target track switch unit 26., under the situation of the intrusion of collision warning region 100, carry out the operation action, and get into step 5A nobody 38.On the other hand, people 38 being arranged under the situation of the intrusion of collision warning region 100, carry out the collision respective action, and carry out processing (in the processing of target track switch unit 26) to step 5B.

In step 5B, with joint target vector q _dContact action joint target vector q as 6 generations of collision respective action generation unit _Id(in the processing of target track switch unit 26).Then, get into step 6.

In step 5A, with joint target vector q _dAs joint target q _Wd(in the processing of target track switch unit 26).Then, get into step 8.

In step 6, from driving current value i, joint angles data (joint angles vector (currency of joint angle) q), the joint angles error compensation output u of separately motor 34 _Qe, infer the external force torque T on the joint portion of unit 27 computing machine human arm 8 through moment of torsion _Ext(inferring the processing of unit 27 at moment of torsion).

Then, in step 7, from mechanical impedance parameter I, D, K, the joint angles data (joint angles vector q) collision respective action generation unit 6, set, infer the external force torque T that imposes on robot arm 8 that unit 27 calculates by moment of torsion _Ext, through impedance computation unit, joint 28 calculating joint target correction output q _{The d Δ}(processing of impedance computation unit 28) in the joint.

Then, in step 8, as joint target vector q _dWith joint target correction output q _{The d Δ}With, calculate to revise joint target vector qdm, and to input to Position Control be 31.

Then, in step 9, will revise joint target vector q _DmWith the difference of current joint vector q be the error q of joint portion _eInput to joint error compensation unit 30, error compensation unit 30 computes joint angles errors guarantee output u in the joint _Qe(processing of error compensation unit 30) in the joint.As the concrete example of joint error compensation unit 30, can consider the PID compensator.The diagonal angle ranks of adjustment constant are ratio, differential, these 3 gains of integration through controlling suitably, and the joint error is punctured into 0.

Then, in step 10, with joint angles error compensation output u _QePass to motor driver 19 through D/A converter 21,, the rotatablely moving of each joint shaft of robot arm 8 taken place through making along the current change of motor 34 circulations of separately joint portion.

Computation cycles through as control repeats above step 1～step 10, realizes the control of the action of robot arm 8.

Then, the work all to the control device in the 1st embodiment of the present invention 1 is controlled object with the hand 9 with robot arm 8, and carrying out transport operation is example, describes based on the flow chart of Fig. 6.

In step 21, in people's motion detection unit 2, carry out image recognition processing based on the view data of image pickup device 36, detect information near the people's 38 of robot arm 8 position and translational speed, moving direction.

Then, in step 22, judge near the intrusion that probe unit 3 carries out people 38, be judged as near probe unit 3 under the situation that does not have to invade, get into step 23 the people to collision warning region 100 through the people.

In step 23, through 8 actions of action control unit 7 control robot arms, control object with the hand 9 of robot arm 8, carry out the transport operation action.

After step 23, turn back to step 21, a very important person does not confirm people 38 intrusion near probe unit 3, with regard to repeating step 21 → step 22 → step 23 → step 21 ... Circulation, robot arm 8 is carried out common above-mentioned object and is controlled and transport operation.

In step 22, under the situation of the intrusion of having confirmed people 38, get into step 24, infer unit 5 through position of collision and infer collision precalculated position [formula 24].

[formula 24]

X _p(x _i，y _i)

Then, in step 24.1, calculate collision precalculated position X by collision respective action generation unit 6 _pInitial point O with absolute coordinate system 32 ₀Between distance L _Xi, collision respective action generation unit 6 is distance L relatively _XiAnd length L ₁, judge when the inequality of formula (10) to be contact mode (1) through collision respective action generation unit 6, during inequality be in formula (11) contact mode (2), other the time be contact mode (3).

Then, in step 25, in collision respective action target track generation unit 6, in collision precalculated position [formula 25], through type (12) generates the collision respective action target track q that is used to obtain the corresponding posture of collision _d(t).

[formula 25]

X _p(x _i，y _i)

Then; In step 26; For the mechanical impedance value of the joint portion that will set aspect the corresponding posture in collision, according to contact mode (1)～(3), in addition according to approaching people 38 translational speed; By that kind of before having introduced the mechanical impedance of joint portion as the method for confirming, set through collision respective action target track generation unit 6.

Then, in step 27, in action control unit 7, collision respective action target track q is selected in 26 work of target track switch unit _d(t), make robot arm 8 actions, become the corresponding posture of collision through Position Control system.In addition, also work in impedance computation unit, joint 28, each joint portion of robot arm 8 is controlled at (independence) the mechanical impedance value of setting respectively.

Then, in step 28, in people's motion detection unit 2, carry out image recognition processing, detect information near the people's 38 of robot arm 8 position and translational speed, moving direction by the view data of image pickup device 36.

Then; In step 29, judge near the intrusion that probe unit 3 carries out people 38 the people to collision warning region 100, be judged as under the situation of intrusion near probe unit 3 the people; Turn back to step 27; Short of affirmation people 38 breaks away from from collision warning region 100, with regard to repeating step 27 → step 28 → step 29 → step 27 ... Circulation, robot arm 8 continues the collision respective action.

On the other hand, in step 29, have no talent 38 the people under the situation that collision warning region 100 is invaded near probe unit 3 judgements, turn back to step 21, robot arm 8 is controlled and transport operation action recovery to common above-mentioned object.

Through above action step 21～step 29, can realize utilizing the above-mentioned object of robot arm 8 to control and transport operation, people 38 near the time, realize switching to the collision respective action.

As previously discussed; Infer unit 5, collision respective action control module 4 through possessing people's motion detection unit 2, people near probe unit 3, position of collision; People 38 during near robot arm 8; With the suitable gesture actions of corresponding people's 38 motion, can be with the mechanical impedance of suitable joint portion, the corresponding collision respective action of collision of control and people 38 and above-mentioned robot arm 8.

Because robot arm 8 is that base portion 13 is fixed on the ground through root; Therefore on the joint portion of root (base portion 13) side of colliding the precalculated position; Rigidity is lower than other joint portion, the joint portion of the state of easy activity if do not have; Then produce resistance, produce impact collision.; According to above-mentioned the 1st embodiment of the present invention; Because when on each each joint portion of robot arm 8, setting mechanical impedance respectively, compare the joint portion of the state that can on the joint portion of root (base portion 13) side, set the low and easy activity of rigidity with other joint portion, therefore can discharge the power that apply when colliding at this joint portion; Impact to the people reduces, and can improve security.

Therefore; Control device 1 according to above-mentioned the 1st embodiment of the present invention; Even articulated robot arm 8 also can contact with the people safely, and; Can carry out the contact action of the best of corresponding people's action, can realize not damaging with the people contiguously with the action control of the safety of the robot arm 8 of people's coexistence.

Have again, in above-mentioned the 1st embodiment, the quantity of the joint portion of robot arm 8 is defined as 3, but also is not limited to this, can both bring into play same effect for 2 robot arms 8 more than the joint portion.

In addition, in above-mentioned the 1st embodiment, for example clear robot arm 8 and people's collision, but also be not limited to this for the mobile robot or the collision of other moving body such as the lorry in moving and robot arm 8, also can be brought into play same effect.

Have again, in above-mentioned the 1st embodiment, get under the situation of collision warning region 100, once collide inferring of precalculated position, but also be not limited to this, also can adopt (continuation) to collide the method for inferring in precalculated position continuously people 38.In this case, because the precision of inferring of collision improves, therefore can improve security more with better posture, contact mode correspondence.On the other hand,, once colliding under the situation of inferring in precalculated position, having and to reduce amount of calculation, can alleviate advantage the burden of CPU like above-mentioned the 1st embodiment.

(the 2nd embodiment)

Fig. 8 A and Fig. 8 B be the control device 1 of the robot arm of expression in the 2nd embodiment of the present invention formation and sketch map that the control object is the formation of robot arm 8, and above-mentioned the 2nd embodiment of expression in the block diagram of detailed formation of action control unit of above-mentioned control device.Because the basic comprising of the control device 1 in the 2nd embodiment of the present invention, same during with the 1st embodiment shown in Figure 1, therefore the explanation of common part is omitted, below only different portions is at length explained.

In the 2nd embodiment of the present invention, not through image recognition processing, but, carry out the approaching detection of people's motion detection and people through the power that detection machine human arm 8 produces when contact with people 38.Thereby, form the structure of inferring unit 27 input external force torque information from the moment of torsion of action control unit 7 as people's motion detection unit 2.Therefore, in the control device 1 in the 2nd embodiment, do not need image pickup device 36.

People's motion detection unit 2 is inferred the external force torque T of inferring unit 27 based on moment of torsion _Ext=[T _1ex, T _2ext, T _3ext, T _4ext] ^T,, infer robot arm 8 and people's 38 relative position relation by following pattern.Wherein, in Fig. 9 A～Fig. 9 D, (overlooking robot arm 8 from upper side) makes joint portion be defined as positive moment of torsion to the moving moment of torsion of anticlockwise.

(1) in the external force torque T of the 1st joint portion 14 _1ext>0, the external force moment of torsion of the 2nd joint portion 15

The time, shown in Fig. 9 A, infer the contact at the 1st connecting rod 12 into people 38 by people's motion detection unit 2.

(2) in the external force torque T of the 1st joint portion 14 _1ext<0, the external force moment of torsion of the 2nd joint portion 15 The time, shown in Fig. 9 B, infer the contact at the 1st connecting rod 12 into people 38 by people's motion detection unit 2.

(3) in the external force torque T of the 1st joint portion 14 _1ext>0, the external force torque T of the 2nd joint portion 15 _2ext>, shown in Fig. 9 C, infer the contact at the 2nd connecting rod 11 at 0 o'clock into people 38 by people's motion detection unit 2.

(4) in the external force torque T of the 1st joint portion 14 _1ext<0, the external force torque T of the 2nd joint portion 15 _2ext<, shown in Fig. 9 D, infer the contact at the 2nd connecting rod 11 at 0 o'clock into people 38 by people's motion detection unit 2.

As long as carry out classification, just can learn robot arm 8 and people's 38 general position relation according to 4 above patterns.In addition; The people through detecting state the arbitrary patterns of change to the pattern of above-mentioned (1)～(4) of external force moment of torsion from the external force moment of torsion

of the external force moment of torsion

of the 1st joint portion 14, the 2nd joint portion 15, just can survey the approaching of people 38 near probe unit 3.

Through the approximate location of inferring people 38 with above method, with posture, the joint moment of torsion of collision respective action control module 4 control correspondence positions relations, can carry out can with the action control of the safety of the robot arm 8 of people's 38 coexistences.

Have again; In above-mentioned the 1st embodiment, people's motion detection unit 2 is defined as and utilizes image recognition processing; But also be not limited to this; As long as with other sensing detection unit, for example laser radar sensor or ultrasonic sensor etc. can detect people 38 position and speed, also can bring into play same effect.

In addition, in above-mentioned the 2nd embodiment, infer the external force moment of torsion of inferring unit 27 based on moment of torsion and carry out people's detection; But also be not limited to this; Shown in figure 10, connecting rod 12,11 of robot arm 8 etc. maybe with position that people 38 contacts on, an example that obtains the unit as position of collision is provided with feeler 39; Contact with feeler 39 through people 38 and to carry out people's detection, also can bring into play same effect.In addition,, then can improve and infer precision, can control robot arm 8 more safely if also survey with the people who utilizes feeler and external force moment of torsion.

Have again, in above-mentioned the 2nd embodiment, the quantity of the joint portion of robot arm 8 is defined as 3, but also is not limited to this, can both bring into play same effect for 2 robot arms 8 more than the joint portion.

In addition, in above-mentioned the 2nd embodiment, illustrational robot arm 8 and people's collision, but also be not limited to this, for the mobile robot or the collision of other moving body such as the lorry in moving and robot arm 8 also can bring into play same effect.

In addition; In the above-mentioned the 1st and the 2nd embodiment exemplified robot arm 8 is illustrated; But also be not limited to arm; Even mobile robot or moving bodys such as 2 leg walking vehicle device people, multiway walking robot through wheel movement through in the contacting of body and people, using the present invention, also can obtain the effect same with the present invention.

Have again,, can bring into play the effect that has separately through embodiment arbitrarily in the above-mentioned numerous embodiments of appropriate combination.

Utilizability on the industry

The control device of robot arm of the present invention and control method, robot, and the control program of robot arm, reach the control program of robot arm at control device and control method, the robot of the robot arm of the control of the action of the robot arm that effectively carries out contact with the people as home-use robot etc.In addition, also be not limited to home-use robot, also can be used as the control device of the movable agency in industrial robot or the production equipment etc.

The present invention has fully narrated preferred embodiment with reference to accompanying drawing, but obviously for the personage who is familiar with this technology, can carry out various deformation or correction.Such distortion or correction only otherwise exceed the scope of the present invention of the scope defined of appended technical scheme, all are interpreted as containing in the present invention.

Claims (2)

1. the control device of a robot arm is the control device with articulated robot arm of at least 2 connecting rods and at least 2 joint portions, wherein, comprising:

Position of collision is obtained the unit, the position of collision of its acquisitor or moving body and said articulated robot arm;

Collision respective action control module; It obtains the said people that obtains the unit or the said position of collision of moving body and said articulated robot arm based on said position of collision; Respective action is collided in control to be made lowlyer than the rigidity of other joint portion near the rigidity of the joint portion of root one side than the connecting rod of the said articulated robot arm under the said position of collision, and makes than the connecting rod of the said articulated robot arm under the said position of collision and keeps or add bigger rigidity near the rigidity of the joint portion of wrist portion one side.

2. the control device of robot arm according to claim 1; Wherein, The control of said collision respective action control module makes in the joint portion of connecting rod near said root one side than the said articulated robot arm under the said position of collision, is lower than than the connecting rod of the said articulated robot arm under this position of collision rigidity near other joint portion of said root one side near the rigidity of the joint portion of the said connecting rod under the said position of collision.

Images