PRecision localization in the calibration procedure of

Transcription

1 Proceedings of the 007 IEEE/RSJ International Conference on Intelligent Robots and Systems San Diego, CA, USA, Oct 9 - Nov, 007 TuA.4 High Precision PSD Guided Robot Localization: Design, Mapping, and Position Control Sebastian Blank,YantaoShen,NingXi,ChiZhang, and Uchechukwu C. Wejinya Abstract This paper introduces our recently developed high precision robot localization system employing position sensitive detectors (PSD). A lateral effect PSD is an ideal position sensitive device with a resolution of approx. 0.5µm. Such a device can be used to enhance the precision capabilities of robot localization or robot calibration. The developed PSD guided localization system is comprised of a structured laser pointer system attached to the robot end-effector and a PSD fixture. It is capable of feeding back the movement information of the TCP relative to the PSD frame. This is achieved by detecting the position of the laser beams emitted from the laser pointers onto the PSD surface. The system was well designed, mapped, and constructed and hence allows it to perform precision localization of the robot. Simulations and preliminary experiments demonstrated the effectiveness of the developed PSD guided localization system including hardware, software, and algorithms. Therefore, it is capable of improving the localization performance of robots as well as promoting the application of precision robot calibration. I. INTRODUCTION PRecision localization in the calibration procedure of robots has become a research field of great importance over the last decades especially in the field industrial robots. The main reason for this is that the field of application was significantly broadened due to an increasing number of fully automated or robot assisted tasks to be performed []. Those applications require a significantly higher level of precision and accuracy due to more delicate tasks that need to be fulfilled (e.g. assembly in the semiconductor industry or robot assisted medical surgery [9]). Currently, most methods used to perform robot localization for calibration purposes require human interaction for every single robot and have to be performed under lab conditions in a long and cost intensive process. Expensive and complex measurement systems have to be operated by highly trained personnel. In a particular case, as shown in Figure 1, the operator has to align a needle tip with both the robot end-effector and the CMM using the eyes. This is obviously both too time consuming and inaccurate. Hence a new method is needed that is less time consuming, is more precise, and involves less (or in the long term even no) human interaction in the entire localization/calibration process. Sebastian Blank is with the Dept. of Computer Sciences, University of Kaiserslautern, Germany. s [email protected] Yantao Shen, Chi Zhang, and Uche C. Wejinya are with the Department of Electrical and Computer Engineering, Michigan State University. [email protected] Ning Xi is with the Department of Electrical and Computer Engineering, Michigan State University, and also serving as an affiliated professor at the Shenyang Institute of Automation of Chinese Academy of Science, China. [email protected] Fig. 1. Manually robot localizing for calibration tasks. Adapted from [8], Fig , page 68. Recently, vision based localization systems were developed for realizing the automated localization tasks [5][8]. However, such systems suffer from low servoing speed due to the low frame-per-second of the cameras and the related time-consuming image processing [4]. The optical approach using PSDs for localization is a good choice since it promises fast response, high precision, and low computation load. In [], Nejat et al. placed the PSDs directly on the endeffector to achieve high-precision short-range positioning of the robot s pose in a planar workspace. Nam et al. investigated fast real-time dynamic visual tracking using PSD cameras [3]. In this paper, a new PSD guided localization system is proposed to improve the calibration process in respect to time, reliability, and costs aspects. Another important aspect is to reduce or even eliminate the need for error-prone related human participation in the process. As shown in Figure, in our system, a fixture housing 4 laser pointers is mounted at the end-effector of the robot. The pointers are correspondingly aligned with the 4 PSD devices on the surface of the PSD fixture. Theoretically speaking, at least three laser beams have to be centered on the three PSDs to be able to uniquely determine the pose of the robot end-effector. The fourth one is used to compensate for the occurring alignment errors or fabrication tolerances of the devices. This paper is structured as follows: Section II reviews the proposed PSD guided robot localization system. Here the basic concept is going to be presented in detail. In addition, the experimental results of the PSD fixture, especially in respect to the mapping performance are demonstrated. In Section III an uncalibrated PSD-guided localization control method is developed using the PSD guided localization system for automated localization tasks. Numerical simulations and preliminary experiments are also presented. Finally, the /07/$ IEEE. 5

2 Fig.. Overview of PSD guided localization system work is concluded in Section IV. II. PSD CALIBRATION, MAPPING, AND EVALUATION A. Review of Principle of Lateral Effect PSD PSDs play an important role in the developed localization system. A PSD is a sensor capable of tracking the location of a laser beam on its surface [1][6]. As shown in Figure 3, we chose the lateral effect PSD with 4mm 4mm active area for the developed system. In a lateral PSD, the relative two-dimensional position of e.g. a laser beam on the active surface of the chip can be expressed as: (Signatone CAP 945) perpendicular to the PSD surface. The probe allows it to position the laser beam in DOF (x, y) over the PSD surface with high precision (accuracy: ±.5µm, repeatability: ±1µm, resolution: 40nm per step). The PSD chip was fixed on a floating, shock absorbing workbench together with the probe to eliminate influence of e.g. operator movement and vibrations. Based on this setup, the calibration experiments were conducted. The results are presented in Figure 4 on the top. The segments of the graph highlighted in green, blue, and red corresponding with the physical structures (shown on the bottom) of the chip design. As the laser passes the first section and gradually moves its full beam profile to the active area, a rise in the signal can be observed until the gap is completely passed (signal peak). After this peak the beam travels over the actual sensor surface (blue). A rather linear output in respect to the change in position can be observed in the signal. As the laser leaves the active sensing area the same signal characteristics as before occurs. The signal for the y-channel is presented in a similar manner in Figure 4 b). Here however the signal outside of the active area shows differences to the x-channel caused by the differences in the chip design. Fig. 3. a) Photograph of lateral effect PSD (OSI Optoelectronics DL-4). b) Schematic illustration of a two-dimensional lateral PSD. x = I x1 I x (1) I x1 + I x y = I y1 I y I y1 + I y where x is the relative position of laser spot on X-axis and y the relative position on Y -axis. I x1,i x,i y1,i y are photo currents measured in the direction indicated by the index. The major advantage of the lateral type is that the accuracy of the output is not affected by the laser spot profile or its intensity distribution. The positional resolution of about 0.5µm is more than sufficient for the given application in robot localization. Another outstanding property of this type is the position linearity over large areas of the active surface of the chip. This is important for our task since it allows us to keep the error at a low level during the mapping and compensation process. The related signal conditioning circuit of the PSD can be employed and found in [6]. B. Calibration, Mapping, and Evaluation To map the PSD sensing system (PSD and the conditioning circuit), a laser pointer was fixed on a computer aided probe Fig. 4. PSD calibration: a) Top: Plot of the signal during a sweep with constant y-position along the x-axis. Bottom: Photograph of a lateral PSD surface. b) Top: Plot of the signal during a sweep with constant x-position along the y-axis. Bottom: Photograph of a lateral PSD surface. Figure 5 shows the investigation on hysteresis effects along x and y directions of the PSD. From the results, one can see that the chosen PSD chip in combination with the designed interface circuit does not show significant hysteresis effects. The results of the whole D area calibration along x and y directions are shown in Figure 6. In Fig. 6, the y-spacing used is 0. mm between two sweeps. To cover the whole chip 3 lines were used resulting in approx. 90,000 points recorded. The graph clearly shows the linear characteristics of the output over the full active area (plane within the two peaks) for all y-positions (0 to approx 4500µm). Furthermore the location of the electrodes (neutral green plane next to the peaks) is clearly visible. In Fig. 6, the spacing in between the sweep lines is also 0. mm. This results in 0 lines and therefore includes approx. 80,000 points of recorded data. The two green planes at approx. zero potential can be identified as the electrodes framed by two peaks. They are 53

3 To evaluate the mapping performance, a laser pointer mounted at a 3-D precision manual stage (1µm x or y movement per turn) was used to generate various laser traces onto the active area (4mm 4mm) of the PSD. The straight line patterns along x and y directions was first tested and shown in Figure 7. This pattern is best suited in this context to determine if the signal decoupling and rotation were sufficient. In addition, more complex patterns were used to determine the mapping functions quality. In Figure 8, two patterns such as meander and triangle show the mapping results. In the area around the center of the PSD good result could be achieved. On the outer regions however a deviation can be found that originates from the mapping function. This however is not critical since only the middle area will later on be used for the final positioning. The feedback obtained in the outer regions will only be used to reposition the beam towards the center. Fig. 5. PSD. Hysteresis effect results along x and y directions of the caused by two gaps on the chip that respond to stimulation by the laser beam. Fig. 7. Screenshot of interface software showing trace of laser beam movement (red) along a straight line (black,dashed) in x-direction (left) and y-direction (right). Fig. 6. 3D plot of the data of all sweeps parallel to the x-axis 3D plot of the data of all sweeps parallel to the y-axis. Based on the calibration results, a mapping function is needed in order to perform position feedback during the automated localization task. To find the proper mapping formulation, an average polynomial was fitted based on the recorded PSD output u in x and y direction: Pos x (u) = 0.004u Pos y (u) = u () The result shows a rather good match between the calibration data and the fitting curve over most of the active surface. The mapping error however increases significantly when the area close to the edge is reached on either side. To enhance the mapping performance, it becomes necessary that the x and y signals need to be decoupled first to achieve satisfying mapping results. In this paper, we use the affine mapping method to find the decoupling matrix D for decoupling the signals (x, y) in two directions [10]: [ ] D = Fig. 8. Additional traces of triangular (left) and meander (right) pattern. Both patterns show the deviation of the signal (red) from the actual beam movement (black,dashed) when the outer regions of the chip are approached. III. PSD GUIDED ROBOT LOCALIZATION A. Uncalibrated Robot Localization Based on the developed PSD system and an adaptive control method modified from our previous work [7], we first consider a PSD guided D localization approach of a planar robot without pre-calibration. Fig. 9 shows the set-up of the system including a PSD fixture and a planar manipulator with the structured laser pointer system. Three coordinate frames, namely the robot base frame B, the structured laser pointer coordinate frame (related to the end-effector) L, the PSD plane frame P which locates at the geometric center of one of the PSDs in one plane. Here, assume Z p axis in P and Z b axis in B parallel to each other. The homogeneous transformation matrix P T B between the 54

4 robot base frame and the[ PSD fixture ] frame is generally R p represented as P T B = where R SO(3) 0 1 denotes the rotation matrix. p R 3 is position of the origin of B with respect to P. The matrix P T B is unknown since no calibration was pre-performed. Structured laser pointer frame L values [x d,y d ] T (the center positions of the embedded PSDs in one plane) on the PSD plane frame. For a fixed PSD fixture system, suppose that the structured laser pointers (mounted at the end-effector of the planar robot) are moving with velocity P Ẋ L =[T x,t y,t z,ω x,ω y,ω z ] T with respect to the PSD fixture frame. The first three components of P Ẋ L denote the translational velocity P L L, and the last three are angular velocity P Ω L. For any point that is rigidly attached to the structure laser pointer frame, the velocity in respect to the PSD fixture frame is given by P Ẋ = P Ω L P X + P L L (6) Robot base frame B? Fig. 9. PSD fixture PSD ZP PSD plane frame P (Top view) The coordinate frames. By P X L =[X, Y, Z, γ, ρ, φ] T the position and orientation of the structured laser pointer with respect to the PSD plane frame are denoted. The first three components of P X L denote the position, and the last three are the roll, pitch and yaw angles representing the orientation. q R r are the joint angles of the planar robot. Applying forward kinematics, we receive: Bẋ L = J(q) q (3) where B ẋ L R 6 is the velocity of the structured laser pointer with respect to the robot base frame. J(q) R 6 r is the Jacobian matrix of the robot. q R r denotes the joint velocity. According to the relation of robot kinematics, we also have [ ] P R 0 Ẋ L = J(q) q (4) 0 R A where P Ẋ L R 6 denotes the velocity of the structured laser pointer frame with respect to the PSD fixture frame. One can assume that 6 r and J(q) is nonsingular. Note that R in equations (4) is unknown if no calibration is pre-performed. As depicted in Fig.9, a physical feature point (one of the emitting centers of the laser pointers) on the structured laser pointer, P X =[X p,y p,z p ] T, whose coordinates are expressed with respect to the PSD plane frame P,and projects a laser point onto one of the PSDs, therefore the projected point locating on the PSD plane frame P has the coordinates P x =[x, y] T. The relationship between P x and P X is given by [ ] [ ] x P Xp x = = (5) y Y p In this paper, we assume that the structured laser-pointers can be characterized by a set of feature points (i.e., the 4 feature points (4 emitting centers) if 4 laser pointers are embedded). For a PSD guided localization task, we are interested in determining a set of feature points representing the structured laser pointers to achieve/approach some desired Note that P X =[X p,y p,z p ] T. Z p is an unknown constant depth. Substituting equation (5) into equation (6), we can write the derivatives of the coordinates of P X in terms of the PSD plane frame coordinates (x, y) as X p = Z p ω y + yω z + T x Y p = xω z Z p ω x + T y Z p = 0 (7) Equation (7) can be written in such a matrix form as follows [ x y ] = J I T x T y T z ω x ω y ω z [ ] Zp y J I = (8) Z p 0 x In order to guarantee that the structured laser pointer motion can be fully observed, three or more feature points (determined by the number of laser beams) are required for the PSD guided localization to be solvable. By simply stacking the Jacobian J I for each pair of laser points onto the PSD plane coordinates, we have x 1 y 1 T x x T y y = J S T z x 3 ω x y 3 ω y. ω z. P Ẋ L ξ where J S R n 6, n 3 is the number of the laser points onto the PSD planes. For example, when n =3, the above Jacobian can be extended to a case of using three laser points, that is 1 0 k 1 0 Z p1 y k Z p1 0 x 1 J S = 1 0 k 3 0 Z p y 0 1 k 4 Z p 0 x (10) 1 0 k 5 0 Z p3 y k 6 Z p3 0 x 3 (9) 55

5 where Z p1,z p,z p3 are the depth of the laser pointers with respect to the PSD plane frame. In this particular case they are constants since a planar robot is used. Also note that in the above equation, since T z equals to zero with reference to the system configuration in this paper, the elements of the third column in the matrix J S can be set to an arbitrary value since this does not affect the relation between the velocities of laser points on PSD plane frame and the velocity of structured laser-pointer system with respect to the PSD plane frame. In order to avoid the singularity of J S explicitly, we re-set the third column to the non-zero one whose elements k i are constants with i =1...6, k i k j for i j. To combine the equation (9) and equation (4), we obtain ξ = J S AJ(q) q (11) Assume J S is nonsingular by referring to the weak nonsingular condition, we then have q = J + (q)a T J + S ξ (1) Then we propose: Proposition 1 Arrange the m unknown elements of the composite Jacobian matrix J S A in a m 1 vector Θ. For any 6 1 vector λ, the product J S Aλ of the matrix J S A with the vector λ can be represented in the following linear form: J S Aλ = Y (λ, x, y)θ (13) where Y (λ, x, y) is a n m matrix whose elements do not depend on the unknown parameters, the transformation matrix or the depth of laser emitting points attached to the structured laser pointer frame. This proposition forms the basis of the adaptive algorithm to be proposed for on-line estimation of the unknown parameters. We consider the problem of moving the planar manipulator so that the specified laser point values ξ d =[x d1,y d1, ] T (the center coordinates of the PSDs) on the PSD plane frame correspond to the relative position of structured laser-pointer frame can be reached. Firstly, we adopt the popular PD plus gravity compensation scheme for the set-point control (localization): [ ˆRT τ = G(q) K v q K p J T 0 (q) 0 ˆRT ] Â T ĴS T ξ (14) where ˆR T denotes an estimated value of transposed matrix of R. Â T represents the estimated value of transposed matrix of A. ĴS T denotes an estimated value of transposed image Jacobian matrix of J S. ξ = ξ ξ d denotes the laser point features error with respect to the PSD plane frame. K p and K v are the positive scalar constant gains. Substituting this control law into robot dynamic equation results in the following closed-loop dynamics equation (Note that the right side of equation is re-written according to Proposition 1): H(q) q + C(q, q) q = K v q K p J T (q)a T JS T ξ K p J T (q)(ât ĴS T AT JS T ) ξ = K v q K p J T (q)a T JS T ξ K p J T (q)y ( ξ,x,y) Θ (15) where we can arrange the unknown elements of A T JS T into a m 1 vector Θ. ˆΘ is an estimated value of Θ and Θ = ˆΘ Θ. Y ( ξ,x,y) is a regressor matrix that does not depend on any unknown elements of ÂT ĴS T. Obviously, an adaptive law is necessary to update the estimated matrix ĴSÂ. Based on the kinematic equation (11), we consider the following approach for leading the estimated unknown matrix ĴSÂ to tend to the actual matrix J SA, thus, an equation is defined Ĵ S ÂJ(q) q ξ = (ĴSÂ J SA)J(q) q = Y 1 (J(q) q,x,y) Θ (16) where ξ can be obtained by differentiating the laser points on the PSD planes. Y 1 (J(q) q,x,y) is also a regressor matrix without depending on any unknown elements of ĴSÂ. Consequently, the following adaptive law is proposed: Θ T T = [ĴSÂJ(q) q ξ] B Y 1 (J(q) q,x,y) Θ T Y1 T (J(q) q,x,y) + 1 B 1 q T K p J T (q)y ( ξ,x,y) (17) where B 1 and B are the positive constant gains. Then, we summarize the above mentioned control scheme into Theorem 1: Under the control of the proposed localization control law in equation (14) with the adaptive algorithm in equation (17), the PSD guided planar manipulator system yields the estimated elements in ˆΘ are bound, and asymptotic convergence of laser feature points error ξ on the PSD plane frame to zero as the time approaches to the infinity. The similar proof can be found in [7]. B. Numerical Study of Uncalibrated PSD Guided Localization In this section, we show the performance of the proposed PSD guided localization method by simulations. In this simulation, we use a two-link planar robot model and consider three laser pointers shooting onto three PSDs. The rotation matrix between the robot base frame and the PSD fixture frame is a function of θ. For the simulation the following values were used: the real rotation matrix R(θ = π/15), the initial estimated one ˆR 0 (θ = π/3); the positive gains are K p =80, K v =50, B 1 = 8000, andb = ; the initial distance of the laser pointer with respect to the PSD fixture frame was set to 1m. The sampling time of the simulation was 1ms. As shown in Fig. 10, both the position errors of 3 laser points and the robot joint velocity errors decreases rapidly with increasing time. 56

6 Position Errors of Laser Points on PSD Plane (mm) Time (s) x 1 y 1 x y x 3 y 3 The initial laser point in the PSD plane is (.6,.)mm. The desired point is set as the center of the PSD D plane. As shown in Fig. 1, the controlled laser point converges to the desired one asymptotically. The results ensures the effectiveness of the proposed localization algorithm dq 1 dq Joint Velocity Errors (rad/s) Time (s) Fig. 10. Simulation results: : x and y position errors of three laser points on PSD plane frame, : Two joint velocity errors of the robot C. Preliminary Experiments The setup of the experiment is shown in Figure 11. The Stäubli RX 60 robot was used in the localization experiment and was controlled to only move in the X-Y plane (planar format). The structured laser pointers (focusable) are mounted at the end-effector of the robot. The PSD fixture is fixed under the structured laser pointers. The laser pointer is about 50cm away from the PSD fixture. The emitted laser beam was focused and adjusted to be a 0.5mm circular laser spot perpendicularly projecting onto the PSD plane surface. In an experiment, the initial estimation of the rotation matrix Structured laser pointers PSD fixture PSD signal conditioning circuit Fig. 11. The experimental setup. PSD ˆR 0 between the robot base frame and the PSD fixture frame is given as a 3 3 identity matrix. The gains are chosen as: K p =3N.mm/pixel, K v =15N.mm.s/deg, B 1 = 3000,B = The estimated depth is 1m. Figure 1 shows the position control result of one of 4 PSD interface windows. Fig. 1. Experimental results: : Initial laser point on the PSD plane, : Localizing trace of laser point on the PSD plane. IV. CONCLUSIONS In this paper, we introduced a precision robot localization system using position sensitive detector (PSD) guided method. The developed PSD guided localization system is comprised of a structured laser pointer system attached to a robot and a PSDs fixture capable of detecting and feeding back the movement information of laser points emitted by the structured laser pointers. Preliminary experiments demonstrated the effectiveness of the developed uncalibrated PSD guided localization method. Therefore, it will greatly enhance and improve the automated localization performance of robots as well as promote the application of precision robot calibration. REFERENCES [1] Hamamatsu Corporation. Operating Principle and Features of PSD. Hamamatsu Corporation, 360 Foothill Road P.O. Box 6910 Bridgewater, NJ , USA, 006. [] Goldie Nejat and Beno Benhabib. High-Precision Task-Space Sensing and Guidance for Autonomous Robot Localization. Proceedings of 003 IEEE International Conference on Robotics and Automation, pp , 003. [3] Soo-Hyuk Nam and Se-Young Oh. Real-Time Dynamic Visual Tracking Using PSD Sensors and Extended Trapezoidal Motion Planning. Applied Intelligence, 10, pp , [4] Seth Hutchinson, Greg Hager, Peter Cork. A Tutorial Introduction to Visual Servo Control. IEEE Transactions on Robotics and Automation, 1(5): , [5] Yan Meng and Hanqi Zhuang. Self-Calibration of Camera-Equipped Robot Manipulators. The International Journal of Robotics Research, 0(11):909-91, November 001. [6] Hamamatsu Photonics. PSD (Position Sensitive Detector). Hamamatsu Photonics K.K., Solid State Division, 116-1, Ichino-cho, Hamamatsu City, , Japan, 003. [7] Yantao Shen, Guoliang Xiang, Yun-Hui Liu, Kejie Li. Uncalibrated Visual Servoing of Planar Robots. Proceedings of 00 IEEE International Conference on Robotics and Automation, pp , May 00. [8] Hanqi Zhuang, Zvi S. Roth. Camera-Aided Robot Calibration. CRC Press, ISBN [9] S. Charles, R. E. Williams, and B. Hamel. Design of a surgeonmachine interface for teleoperated microsurgery. Proceedings of IEEE Engineering in Medicine and Biology, 11th Ann. international conference, pp , [10] L. Shapiro and G. Stockman, Computer Vision. Prentice Hall, New Jersey,