IROS2019国际学术会议论文集 0558

Global Vision Based Impedance Control for Robotic Wall Polishing Yang Zhou Xiang Li Linzhu Yue Linhai Gui Guangli Sun Xin Jiang Yun Hui Liu Abstract Wall polishing is a typical and essential procedure in the interior renovation However such works are mainly carried out by humans which have the disadvantages of low effi ciency inconsistent quality and issues of safety and health A new vision based impedance controller is proposed for polishing robots to automate the labor intensive works The desired impedance model is specifi ed as the control objective to regulate the dynamic relationship between the interaction force and the motion of the robot end effector where the motion is measured with the vision feedback The use of the vision feedback guarantees the performance of the robot from two aspect First the vision feedback from the high resolution camera ensures the accuracy of measurement of the robot end effector and hence guarantees the quality of polishing Second the concept of image moment is introduced such that the image Jacobian matrix is non singular in a global sense which guarantees the large working range of the robot The dynamic stability of the closed loop system is rigorously proved with Lyapunov methods and experimental results are presented to illustrate the performance of the proposed controller I INTRODUCTION Wall polishing is a typical and important procedure in the building renovation industry Currently most wall polishing works are completed manually which has the disadvantages of low effi ciency and safety risks The manual approach is also suffering from the problems of increasing manpower costs and shortage of skilled workers at present leading to the decline in polishing quality Therefore advanced robotic technology can be applied to automate the wall polishing process guaranteeing polishing quality consistence and greatly improving the productivity In the robotic polishing process the end effector is con trolled to move along the wall surface and keep a steady contact with the wall simultaneously Such operation can be modeled as the interaction regulation between the robot and its environment hence several interaction control schemes are proposed in these years which can be framed into two major classes namely the hybrid position force control and impedance control For the former strategy 1 2 force and position controller are designed in the constrained and uncon strained directions of operational space respectively and the control objective is to track desired force and position in two Y Zhou L Yue L Gui G Sun Y H Liu are with the Department of Mechanical and Automation Engineering The Chinese University of Hong Kong X Li is with the Department of Automation Tsinghua University and he is also with the CUHK Shenzhen Research Institute X Jiang is with the School of Mechanical Engineering and Automation Harbin Institute of Technology Shenzhen This work was supported in part by the Innovation and Technology Commission of Hong Kong under Grant no GHP 021 17SZ in part by the Shenzhen and Hong Kong Joint Innovation Project under Grant no SGLH20161209145252406 and in part by the Science and Technology Innovation Council of Shenzhen under Grant no JI20170480 Corresponding author Xiang Li xiangli orthogonal subspaces On the other hand the latter approach 3 is aimed at regulating desired mechanical impedance specifi ed by target model i e the dynamic relationship between motion and force tracking errors The impedance control strategy has been adopted in several robotic systems and operations such as parallel ankle rehabilitation robot 4 human robot interaction system 5 and peg in hole assembly tasks 6 Considering the application of impedance control on polishing operation an CAD CAM based impedance controller was proposed for the mold polishing robot in 7 In this paper the target wall is considered as a rigid plane with unknown pose To sense and interact with such environment simultaneously vision feedback is employed in impedance control schemes which is referred to as vision based impedance control In 8 position based visual ser voing was used to estimate the object pose for the interaction control of a manipulator with a partially known environment In 9 and 11 stereo vision was applied to determine the geometry of external surface for the working surface tracking control In 10 image based visual servoing was integrated into a pure damping impedance model In 12 a set of image based visual impedance controllers were designed for a dual arm aerial manipulator In addition vision feedback was applied to online estimate the contact force for cell injection operation in 13 and 14 Generally point based image features were employed in the conventional vision based impedance controller however the image Jacobian matrix of such image features may have singularity problem 15 and thus the global stability of the controller cannot be guaranteed This paper presents the result on robotic polishing with global vision based impedance control method and the contributions are summarized as follows Firstly the high precision vision feedback measures motion errors and di rectly integrated with interaction force in the impedance controller which guarantees motion accuracy and ensures the polishing quality Secondly image moments based features are introduced such that its globally non singular image Jacobian matrix ensures the stability of large working range The dynamic stability of the closed loop system is strictly proved with Lyapunov methods and the experimental results on a 6 DOF industrial manipulator are presented In this way the proposed control strategy is theoretically grounded and experimentally demonstrated II BACKGROUND A Problem Formulation The proposed wall polishing robot system is shown in Fig 1 which mainly consists of an industrial manipulator 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 978 1 7281 4003 2 19 31 00 2019 IEEE6022 Fig 1 The whole robotic system confi guration for global vision based impedance control for wall polishing and a monocular camera with eye in hand confi guration Artifi cial markers are projected onto the target wall and then transformed into image features in the image plane Image features are set as the reference of end effector motion hence the wall polishing operation is carried out successfully In this paper the key novelty is that a set of image features based on image moments are implemented in our impedance control law where the non singular and decou pled image Jacobian matrix ensures the stability in the large displacement wall polishing Such image features are directly used in the impedance controller such that the combination of high precision vision and force information ensures the achievement of desired impedance model It is noted that the image plane is aligned with the wall plane when executing our proposed impedance controller which can be easily achieved through image based visual servoing method before the interaction control Therefore the robot end effector contacts with the wall in a certain angle which is not limited to the normal direction of the wall B Robot Kinematics and Dynamics As shown in Fig 1 r r 1 r2 r3 T R3denotes the Cartesian space position of end effector with respect to the robot base coordinate frame O xyz as r h q 1 where q q 1 qn T Rndenotes the vector of robot joint angles and n represents the number of degree of freedom h R3is generally a nonlinear vector function which describes the mapping between Cartesian space position and joint space angles Differentiating 1 the end effector ve locity in Cartesian space can be obtained from joint velocity by r J q q 2 where J R3 nis the Jacobian matrix from joint space to Cartesian space The dynamic model of robot manipulator is described as M q q C q q q g q e 3 where M q Rn ndenotes the inertia torque C q q q Rndenotes the centripetal and Coriolis torque g q Rn denotes the gravitational torque And Rnrepresents the input torque e Rnrepresents the interaction torque between end effector and external environment Some im portant properties of the dynamic model 3 are listed as follows i The dynamic parameters M C and g are bounded ii The matrix M is symmetric and positive defi nite iii The matrix M 2C is skew symmetric C Camera Model As shown in Fig 1 based on the classical pin hole camera model a feature point cr cr1 cr2 cr3 T R3with respect to camera coordinate frame Oc xyz is projected onto the image plane and generates the corresponding image features in image plane as 16 s 1 cr cr3 4 where s x y T R2denotes the coordinates in image plane Given a set of visual features s Rkin image plane its time variation i e the velocity of visual features s can be related to the relative camera linear velocity c r c R3with respect to the camera coordinate frame Oc xyz as 17 s Lc s r c 5 where Ls Rk 3is the image Jacobian matrix related to s III CONTROLLAW FOR ROBOTIC WALL POLISHING In this section a global vision based impedance control law is proposed for robotic wall polishing A set of image features based on image moments are employed to measure the robot end effector motion whose globally non singular image Jacobian matrix ensures the stability of large displace ment motion A novel impedance vector integrating vision feedback and contact force is designed and the convergence to the impedance vector ensures the realization of desired impedance model From section II the eye in hand camera aligns with the wall plane and the robot end effector keeps contacting with the wall plane at a constant orientation therefore the orientation components are not included in the proposed impedance control scheme only the translational components are considered A Impedance Control in Cartesian space The desired impedance model in Cartesian space consists of a generalized mass spring damping system and the trans lational impedance equation is proposed by 3 as Md r rd Cd r rd Kd r rd fe fd 6 6023 where rdis a 3 1 vector representing the desired position of end effector in Cartesian space fd R3is the desired force Md Cd Kd R3 3denote the desired inertia de sired damping and desired stiffness matrix respectively The equation 6 describes a dynamic relationship between the motion error and the interaction force Then the desired impedance model 6 is rewritten as r M 1 d Cd r M 1 d Kd r M 1 d f 0 7 where r r rdand f fe fd Introducing the matrices and the vector flfrom 18 M 1 d Cd 8 M 1 d Kd 9 fl fl M 1 d f 10 an impedance vector can be developed as z r r fl 11 thus the left side of 7 can be rewritten as r M 1 d Cd r M 1 d Kd r M 1 d f z z 12 From 12 the impedance vector z represents the low pass fi ltered signal of the desired impedance model In this paper the control objective is specifi ed as z 0 when t hence the desired impedance model is realized in the low frequency range B Image Moments based Image Features Image moments are a general and practical set of image features which can be defi ned from several planar feature points In this study the feature points derived from artifi cial markers on the wall surface form a closed contour C The feature points inside the closed contour C is projected into another closed object O in the image plane The image moments of the closed object O is generally defi ned as 19 mij O xiyjdxdy 13 where mijdenotes 2D moments of order i j x y T R2 denotes the position of point inside the closed object O in the image plane The centroid sg xg yg T R2of the closed object O in the image plane can be computed by 13 as xg yg T m10 m00 m01 m00 T 14 The image moments are obtained from 13 as m00 n 15 m10 n i 1xi 16 m01 n i 1yi 17 The centroid of closed contour O on the wall surface is defi ned as cr cr 1 cr2 cr3 T R3with respect to camera coordinate frame Oc xyz Based on the image moments from 14 three normalized image features sn x n yn an T R3are selected as sn an s g 1 withan cr 3 a a 18 where cr 3 is the desired depth inside the centroid of the closed contour C a is the desired area of the closed object O in image plane anis the visual feature derived from the above equation From section II the wall plane keeps parallel to the image plane when executing the impedance controller then the closed object centroid sgin the image plane is projected from the closed contour centroid cr on the wall plane as s g 1 cr cr3 19 When the wall plane gets parallel to the image plane the depth cr3 also satisfi es cr3 S a cr 3 a a an 20 where S is the area of closed contour C on the wall plane It can be seen from 20 that the depth of closed contour centroid is equal to the image feature Substituting 19 into 18 and using the above 20 the normalized image features snare related with the object centroid cr as sn an cr cr3 cr 21 From 21 the normalized image features snare equal to the position of closed contour centroid cr Differentiating 21 the velocity of image features snis related to the camera linear velocity c r cas 20 s n Lcs r c withLs 100 0 10 00 1 22 From 22 the image Jacobian matrix Ls R3 3is non singular and decoupled with constant terms in a global sense therefore compared with the image features proposed by previous vision based impedance controller the stability can be guaranteed in a large operation range with the introduced image features based on the image moments C Global Vision based Impedance Control The relationship between the camera linear velocity c r c with respect to the camera coordinate frame Oc xyz and the end effector velocity r with respect to the robot base frame O xyz i e is given as r Rc c r c 23 where Rcis the rotation matrix between the camera frame and the robot base frame Using 22 the robot end effector velocity errors are related with the camera linear velocity errors as r Rc c r c 24 6024 Integrating both sides of 23 the relationship between the end effector motion errors r and camera motion errors cr are given as r Rc cr 25 Substituting 21 and 22 into 24 and 25 the robot end effector velocity and position errors are obtained by r Rc sn r Rc sn 26 Substituting 26 into 12 the impedance vector is rewrit ten as z Rc sn sn fl 27 Introducing fs R 1 c fl where fsrepresents the force error transformed to the camera space from Cartesian space then 27 can be rewritten as z Rc sn sn fs 28 A joint space impedance vector can be formulated as zq q qr q J q Rc snd sn fs 29 where J q is the pseudo inverse matrix of J q and the reference vector qris proposed as q r J q r J q Rc snd sn fs 30 From 27 and 30 it is obtained by z J q zq 31 Substituting 30 into 3 the robot dynamic model can be rewritten as M q zq C q q zq M q qr C qr g q e 32 Now we can propose the impedance controller as M q qr C q q qr g q Kzzq e 33 where Kz Rn n is diagonal and positive defi nite since the impedance vector zqis obtained from image moments based image features therefore the global stability of pro posed controller in 33 is guaranteed by its non singular and decoupled image Jacobian matrix The controller in 33 leads to the closed loop equation M q zq C q q Kz zq 0 34 Proof First a Lyapunov like candidate is proposed as V 1 2z T qM q zq 35 The derivation of 35 with respect to time is given as V zT qM q zq 1 2z T q M q zq 36 Substituting 34 into 36 and using Property iii in Section II we have V 1 2z T q M q zq zT q C q q Kz zq zT qKzzq 0 37 From 37 V 0 and V 0 hence V is bounded The boundedness of V guarantees the boundedness of zq The boundedness of zqguarantees the boundedness of z The boundedness of z guarantees the boundedness of s and s from 28 From 30 qris bounded as sd s and fsare bounded Hence q zq qris bounded because of the boundedness of zqand qr Differentiating 30 with respect to time it can be concluded that the boundedness of q ris ensured From the above analysis qr qrand zqare all bounded zqis bounded see 34 hence zqis uniformly continuous Therefore zqis bounded in L2from 37 Thus it follows 21 that zq 0 and the desired impedance model is achieved IV EXPERIMENT Fig 2 An experimental setup of wall polishing robot which mainly consists of a 6 DOF robot arm a wall and a monocular camera An experimental setup of robotic wall polishing system has been setup in the Chinese University of Hong Kong as shown in Fig 2 which is mainly composed of a 6 DOF robot arm UR5 Universal Robotics a wall and a single camera The robot arm is equipped with a force sensor Mini45 ATI Industrial Automation and a polishing tool The polishing tool is a disc of 60 mm radius with a 400 grits industrial abrasive paper attached on it The object wall is a vertical white plane made of bricks and it has a estimation stiffness of 65000 N M The monocular camera MV U500 Mdvision is mounted on the end effector and calibrated with respect to the robot end effector frame it detects reference features with a resolution of 640 480 pixel and a frame rate of 100 fps In addition a fi xed 2 W 635 nm laser emitter is used to project two cross laser stripes onto the low textured and featureless wall surface which forms a convex quadrilateral on the wall plane as seen in Fig 2 6025 The control system in a PC computer operation sys tem ubuntu 16 04 obtains force and vision information and generates control input for robot arm under proposed algorithm Because of the closed programming architecture of the manipulator the manipulator is not able to accept the control input from 33 Instead we implement the controller on top i e the position velocity control loops by using the reference vector in 30 as the control input The control input is sent to the robot arm using the TCP IP protocol In our experiment the desired trajectory in the vision space was specifi ed as a rectangle as snd 0 115 0 023t 0 10 0 22 T m 0 t 5s snd 0 115 0 10 0 02t 0 22 T m 5 t 10s snd 0 115 0 023t 0 0 22 T m 10 t 15s snd 0 0 02t 0 22 T m 15 t 20s The desired force in the task space was set as Fd 0 0 4 T N i e the desired interaction force was 4 N in the normal direction