基于视觉的六个自由度机器人英文文献和中文翻译(2)
2.1 Puma model validation The robot model was tested with a PI controller using the parameters of the real system. Fig. 2 shows the torque values for a 20º amplitude trajectory during 9 s. It was used an algorithm that allows to obtain a planning periodic trajectory (Fig.2). The error obtained from the comparison between the real and simulate robot trajectory is shown in Fig. 3. Fig. 2. Real and simulation trajectory result of joint 3. Fig. 3. Error result between real and simulate to joint 3. 3. VISUAL SERVOING ARCHITECTURES 3.1 Basic definitions A few basic definitions for 2D visual servoing architecture are here introduced (see Fig. 4.): pe - Actual pose of the camera (with respect to a fixed reference frame R0) p - Required displacement of the camera referential from the actual position to the desire position, related to the observed object frame. p* - Pose of the camera (with respect to a fixed reference frame R0) in a desired position. Mcr – Homogeneous transformation between the camera frame in the current position, Rc, and camera frame in the desired position Rr. Fig. 4. Reference frames definition. Let T6 be the transformation which converts a homogeneous matrix into 6 operational coordinates: p (q)=T6 (Mcr(q)) (2) where Mcr=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡1 0 0 0T r r rT r r rT r r rz 33 32 31y 23 22 21x 13 12 11 (3) p(q)= [ ]Tz y x z y x T T T θ θ θ (4) Then the homogeneous matrix can be transformed into a vector that describes the displacement as function of rotation and translation. p(q) = T6(M )= ⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣⎡−−)r cosr,cosr( 2 arctan)cosr,cosr( 2 arctan) r arcsin(TTT33r23r11r1213zyxθ θθ θ (5) where ) arcsin( 13 r r = θ (6) From the definition of robot Jacobian [9] the relationship between the joint velocities and the end-effector linear and angular velocities is given by the expression: () () c pq J qq = (7) where Jc is the robot Jacobian. 3.2 2D Visual Servoing The modelled system describes the PUMA 560 dynamics with an eye in hand configuration controlling its 6 degrees R b x x yM cr = p y *p p e xyz z y x R cR r Z R 0 of freedom. The camera is placed in such a way that objects in workspace are in its field of vision. An error image is measured and used to evaluate the displacement related to the end effector. Thus the robot pose is estimated through the visual information. Within this framework image feature measures are converted in 6 operational coordinates. Fig. 5 2D Visual Servoing. The used control architecture represented in Fig. 5 is a 2D type [10]. The reference is obtained from image features. This approach uses the robot pose error p=(p*-pe) obtained from the difference between the reference image features s* and the obtained current image features s. The image Jacobian, that relates the kinematics torsor of the camera frame and the variation of image primitives, plays an important role. The extracted visual measure is expressed under the form of a pose p, which contains six operational coordinates. This pose is obtained from the relation between the frame Rr in the desired position and the camera frame in current position Rc. The control goal consists of bringing the measure p to the desired measure pr. When p is very close to pr, Mcr= *cr M , and Mcr=I (8) Let 2ns∈\ be the vector of current coordinates of n points of the image: s [] pn pn p p p p y x y x y x ... 2 2 1 1 = (9) The image Jacobian Jv(s)∈R2nx6 is a matrix that relates the velocity s of the image points and the kinematics torsor r of the camera frame. () v s Jsr = (10) where [ ]cTyz x y zR xrvvv ωωω = (11) and the image Jacobian is given by: 2111 2111122111111222200()00ppp ppppppvpn pn pn pnpnpn pn pn pnpnnnxxy fx fyzzf fxy ffypyxZffxxy fx fyZn zn f fyfy xy fxzz f f⎡ ⎤ +−− ⎢ ⎥⎢ ⎥⎢ ⎥ +⎢ ⎥ −−−⎢ ⎥⎢ ⎥ℑ=⎢ ⎥⎢ ⎥ +−− ⎢ ⎥⎢ ⎥⎢ ⎥ +⎢ ⎥ −−−⎢ ⎥ ⎣ ⎦### # # # J (12) The image Jacobian is a function of the image features, focal distance f and of the camera frame z-coordinates of the target points. The relation between the kinematics torsor r and p is given by p rJp = (13) where Jp is the Jacobian between the referential in initial and final position and relates the velocity in these two frames: 0lim ppJ I→=− (14) Assuming Pr ≈ 0, in case of displacement, and that servoing is fast enough the following approach is valid: r P − ≈ (15) Let + v J be the pseudo inverse image Jacobian ( ) v J s , then ( ) Tv vTv v J J J J1 −= + (16) v rJs+= (17) From equation (15): v p Js+≈− (18) Let s0 be the vector of primitives corresponding to the reference image. When 0 s s = , crR R = and 0 0 pp = = . From equation (18) one can deduce: ( )20 =*() vpp Jss Os+−− −+ (19) where 2() Os is a second order error component, and+v J is the pseudo-inverse image Jacobian s is the primitive of the target for the actual configuration of the robot and s0 is the primitive of the target for the desired robot configuration. Then ( ) 0 v pJss+≈− − (20) From equation (20) is possible to convert image primitive measures into six operational coordinates P. pe 0 R Robot comand s Target Comand law Image primitives measure Image processing s* 4. PREDICTIVE CONTROL