主权项 |
1. A control device for a robot provided with a plurality of links interconnected by joints and actuators which drive the joints, the control device comprising:
an element for determining control inputs classified by state quantity which receives respective target values of a plurality of types of state quantities whose values are defined dependently of one or more component values of a generalized variable vector of the robot which is a vector including at least a displacement amount of each joint of the robot as a component, and then determines control inputs classified by state quantity, which are control inputs defining operations of the actuators necessary to achieve the target values corresponding to the state quantities, for each of the respective types of state quantities on the basis of at least the target values corresponding to the state quantities; a synthesized control input determining element which determines a synthesized control input by determining control inputs classified by frequency region, which are control inputs defining operations of the actuators in a plurality of respective preset frequency regions, according to one or more of the control inputs classified by state quantity, which are determined correspondingly to the plurality of types of state quantities, respectively, while carrying out processing for synthesizing the control inputs classified by frequency region corresponding to the plurality of frequency regions, respectively; an actuator control element which controls the operation of the actuators according to at least the synthesized control input, wherein the processing carried out by the synthesized control input determining element includes processing for determining the control inputs classified by frequency region, which correspond to at least one of the plurality of frequency regions, by synthesizing a plurality of control inputs classified by state quantity of the control inputs classified by state quantity, which correspond to the plurality of types of state quantities, respectively, in a mutually non-interfering manner, wherein, assuming that τtotal is the synthesized control input, n is the total number of the plurality of frequency regions, an i-th frequency region (i=1, 2, . . . , n) is each of n frequency regions, and W(i) is a frequency weight having a frequency pass characteristic set so that the i-th frequency region is a passable frequency band, the synthesized control input determining element determines the synthesized control input τtotal by processing represented by the following expression 01 from n τf(i) (i=1, 2, . . . , n) calculated by processing represented by the following expression 02a and the frequency weight W(i) (i=1, 2, . . . , n):[MATH.1]τtotal=W(1)*τf(1)+W(2)*τf(2)+…+W(n)*τf(n)=∑i=1n(W(i)*τf(i))Expression01τf(i)=τ(i,1)+Nu(i,1)*τ(i,2)+…+Nu(i,1)*Nu(i,2)*…*Nu(i,m(i)-1)*τ(i,m(i))))=τ(i,1)+∑j=1m(i)-1((∏k=1jNu(i,k))*τ(i,j+1))))Expression02awhere[MATH.2]ifm(i)=1,∑j=1m(i)-1((∏k=1jNu(i,k))*τ(i,j+1))))≡0 where W(i)*τf(i) is the i-th control input classified by frequency region, which is a control input classified by frequency region corresponding to the i-th frequency region; m(i) is an integer which indicates the number of control inputs classified by state quantity determined as composition components of the i-th control input classified by frequency region W(i)*τf(i) out of the control inputs classified by state quantity corresponding to the plurality of types of state quantities, respectively, and which is set so as to satisfy conditions 1≦m(i)≦M (M: the number of types of the state quantities) and m(i)>1 for any i within a range of satisfying a predetermined constraint condition; τ(i, j) is a (i, j)-th control input classified by state quantity (vector), which is a j-th control input classified by state quantity out of the m(i) control inputs classified by state quantity τ(i, j) (j=1, 2, . . . , m(i)) determined as composition components of the i-th control input classified by frequency W(i)*τf(i) within a range of satisfying the predetermined constraint condition; the predetermined constraint condition is a condition that the following is not satisfied: m(i)=M hold for all i (i=1, 2, . . . , n), and τ(i1, j)=τ(i2, j) hold for all j (j=1, 2, . . . , M) from 1 to M with respect to arbitrary i values i1 and i2 (1≦i1≦n, 1≦i2≦n) different from each other; Nu(i, j) is a matrix defined by Nu(i, j)≡I−Js(i, j)T*(Js#(i, j))T; Js#(i, j) is a matrix defined by Js#(i, j)≡(Wat)T*Js(i, j)T*(Js(i, j)*Wat*Js(i, j)T)−1; I is an identity matrix; Js(i, j) is a Jacobian satisfying S(i, j)=Js(i, j)·q′; Wat is a matrix defined by Wat≡Wd*M−1; Wd is a preset weight matrix (diagonal matrix); M is an inertia matrix transforming q″ into a generalized force vector (M*q″=generalized force vector); S(i, j) is a state quantity corresponding to the (i, j)-th control input classified by state quantity (a vector or a scalar); q is a generalized variable vector; q′ is a first-order differential value of q (=dq/dt); and q″ is a second-order differential value of q (=d2q/dt2). |