International Journal of Advanced Robotic Systems a Study of Force and Position Tracking Control for Robot Contact with an Arbitrarily Inclined Plane Regular Paper

This paper proposes an adaptive impedance control method for a robot's end‐effector while it slides steadily on an arbitrarily inclined panel; it concentrates on robot force position tracking control for the inclined plane with an unknown normal direction and varying environmental damping and stiffness. The proposed control strategy uses the Recursive Least Squares (RLS) algorithm to estimate environmental damping and stiffness parameters during the impact‐contact process between the robot and the environment. It achieves the expected posture adjustment of the robot's end‐effector based on the measured contact torques and, during the robot's end‐ effectorʹs sliding on the inclined plane, a fuzzy control is developed to adjust the robot impedance model parameters on‐line and adaptively for changes in environmental damping and stiffness. The designed robot force position control method is robust to the changes of the environmental parameters but the implementation of the proposed control algorithms is simple. Finally, experiments demonstrate the effectiveness of the proposed method.


Introduction
Research into force/position control between robots and unknown environments is currently a hot topic. The focus is mainly on improving the robot control systemʹs performance while the environment's geometry parameters and dynamics parameters (damping and stiffness) are unknown or changing; it aims to increase the robustness of the robot force/position control. This paper focuses on robot contact with an arbitrarily inclined plane. The force/position control between robots and inclined planes has been studied by scholars recently. Karayiannidis Y. and Doulgeri Z. proposed the sloping surface normal direction vector online adaptive calculation based on the robot's jointsʹ angle displacement and velocity while its algorithms slowly converge. [1][2][3] In other papers, they designed an algorithm that can achieve sloping surface normal direction estimation and force/position control at the same time, based on the robot's joint displacement and the contact force applied at the robot's end-effector. The contact surface tangential velocity is constant but the algorithm is too complex to be achieved in projects. [4][5][6] Furthermore, they used the approximation capabilities of neural networks to achieve the force and position trajectory tracking of a robot in compliant contact with a flat surface under nonparametric uncertainties existing in the dynamic and contact model. [7] Olsson T. et al. combine force sensing, visual perception and displacement sensors to achieve robot force/position control along arbitrary slopes. [8] Erickson D. et al. compare several methods of parameter (damping and stiffness) estimation for unknown environments and summarize the advantages and disadvantages of each method for the robot's force/position control. [9] Mallapragada V. et al. try to estimate environmental damping and stiffness with RLS and design a PI controller to compensate for the environmental parameters so as to estimate errors based on neural networks. [10] This article introduces an adaptive impedance control for the force and position tracking control between a robot and an arbitrarily inclined plane. First, in order to stimulate environmental dynamic features -commanding the robot's end-effector to collide with the contact surface -it improves the precision of the environmental damping and stiffness estimation by RLS. Second, when the contact is stable, we adjust the posture of robot's end-effector to the expected value according to the contact torque. Finally, during the sliding of the robot's end-effector on the inclined plane, the changes of force and position are tiny compared to the measuring noise. This article introduces a fuzzy control based on position and contact force feedback so as to modify the damping and stiffness parameter of the robot impedance model online, being adaptive to environmental damping and variances of stiffness.

Robot impedance control model
Hogan, H. proposed a robot impedance control method in 1985. This method makes the robot end-effector's force/position control equivalent to the Spring-Weight-Damping model. It determines the relationship between the robot's end-effector displacement and the contact force by adjusting inertial, damping and stiffness parameters. Robot impedance control overcomes the disadvantages of frequently switching between the force control and position control of the robotʹs force position control. The ideal model of robot impedance control is: [11] (1) where X(t) refers to the robot's current position, Xd refers to the robot end-effector's expected position, Md refers to the robot impedance modelʹs ideal inertial, Bd refers to ideal damp, Kd refers to ideal stiffness. Bd, Kd and Md should be a l*l positive definite diagonal matrix in order to achieve decoupling control. l refers to the dimension of the robot work space, E(t)= Fd-Fe(t), Fd refers to robot end-effector's expected contact force, Fe(t) refers to the actual contact force. Robot impedance control seeks to achieve the robot end-effector's expected position and force control by adjusting Md, Bd and Kd. The environment in robot impedance control is usually regarded as a springdamping model: [10] e e e e e In the formula, Be and Ke refer to environmental damping and stiffness respectively, Xe refers as the initial position in the environment, X(t) is the position when the environment is changed and Fe represents the total external force applied in the environment.

The influence of environmental parameters on robot impedance control
When robot impedance control is used for force/position control between the robot and the environment, the environmental parameters are unknown or changing or else cause the system's dynamic and static performance to decrease. Research into environmental stiffness and the influence of damping on the force/position control between the robot and the environment is the basis of using robot impedance control. [12][13]

Stable error influence
When robot's end-effector is in contact with the environment and the system is in steady state then according to robot impedance model Eq(1), we get

Dynamic performance influence
Substituting Eq(2) into Eq(1), the interaction between the robot's end-effector and the environment can be equivalent to a second-order system and the damping coefficient ξ and un-damped vibration angular frequency wn should satisfy [10] where the parameters Wn and ξ determine the dynamic performance of the second-order system. When the variation of the environmental parameters (Ke, Be) causes the changes of (Wn, ξ), in order to maintain the expected dynamic performance of force/position control between the robot and the environment, the impedance control model parameters (Md, Kd, Bd) should be adjusted according to variations in the environment. One of the adjustment methods is to measure (Ke, Be) in real-time or else calculate (Ke,Be) according to the environment model, modifying (Md, Kd, Bd) to compensate for the environmental changes. Another adjustment method is to design a smart controller to adjust (Md,Kd,Bd) online based on the robot's real-time position and contact force feedback; it reduces the influence of changes of the environmental parameters on the systemʹs dynamic performance. The proposed robot force/position control method in this article applies the two adjustment methods to improve the control system's self-adaptive ability in relation to unknown (changing) environmental parameters during different phases of the interaction between the robot and the environment.

Robot's force position control along the inclined plane
Robot force position control along the inclined plane mainly contains the force control along the normal direction of the contact surface and the position control in the contact surfaceʹs tangential direction. At present, the robot impedance control which is used to achieve robot force position tracking control between the robot and the environment mainly focuses on uncertainties or else changing environmental damping and stiffness. [14][15] In order to satisfy these demands, this article estimates the initial value of the environmental parameters (Be, Ke) with RLS during the impact-contact process between the robot and the environments when the contact becomes steadily; it calculates the robot posture rotation matrix according to the contact torque. During the robot's end-effectorʹs sliding on the inclined plane, we design the fuzzy turning mechanism to modify (Bd, Kd) online in order to achieve adaptive robot impedance control for the variance of the environmental parameters (Be,Ke).

Estimation of the initial values of the environmental parameters (Be,Ke)
In the environmental model Eq (2), let δX=X(t)-Xe yield δ δ  F = B X + K X , whereby it is discretized as [11] such that the corresponding difference equation is: which it can be written as:  and A2, we can get the formula for (Be,Ke): 1 2 . Maintaining the control system in an over-damping state during the impact-contact, we can make the estimation of (Be,Ke) more precise.

Robot end-effector's velocity control along the inclined plane
The sliding of the robot's end-effector on the inclined plane is shown in figure 1. Vn refers to the velocity of the robot's end-effector. We adjust the robotʹs posture relative to the inclined plane when it is in contact with it and it is no longer necessary to make adjustments when the robot's end-effector is sliding on the inclined plane. Accordingly, in this article the posture is adjusted according to the contact torque when the contact between the robot's end-effector and the inclined plane is kept stable. The sliding of the robot's end-effector on the inclined plane is shown in figure 1. Fn refers to the force in the inclined planeʹs normal direction and Ff refers to the friction force. When the robot's end-effector begins to make contact with the inclined plane in a steady state, the trend of relative motion is faint and Ff can be omitted. In the current posture, Fn causes the torque Mn(k) at the robot's end-effector. We assume that Fn causes a torque Md at the robot's end-effector in the expected posture and define em(k)=||Md-Mn(k)||; thus, the rotation matrix for the adjustment of the posture of the robot's end-effector in the robot's tool coordinate frame is T (k 1)  R (the force sensor coordinate frame coincides with the robot's tool coordinate frame) [16] : where u and θ refer to the equivalent rotation axis and rotation angles, such that the expression of S(u) is described in [16], and T R refers to the robot's end-effector current posture matrix.
for the robotʹs inverse kinematics, we can get the robot's joint angle displacement in the expected posture.
The robotʹs end-effector's translational velocity in the inclined plane can be described as where k is the sampling period k=0,1,2,…, the superscript w represents the robot workplace coordinate frame, w t v (k) is the velocity of the robot's end-effector along the inclined plane, is the difference between the robot's adjacent position, w n v (k) is the velocity in the inclined plane's normal direction and w is the normal unit vector of the inclined plane. the calculation of w norm v (k) is presented below.
We define the robot impedance control model as where the parameters are the same as with Eq(1). We get and solving the above differential equation yields: where t is time for the discrete process of Eq (3) x and f n d e f f   refer to the displacement error and force error in the N direction. During the robot's end-effectorʹs sliding on the inclined plane, the environmental (the inclined plane) damping and stiffness are changing while the equivalent damping and stiffness of the robot's end-effector vary in different postures. As such, the impedance model parameters need to be adjusted in real-time to be adaptive to the environmental changes. Since the velocity is low in robot force control, only the impedance modelʹs damping and stiffness parameters need to be modified.

Figure 2. Robot adaptive impedance control
The designed robot adaptive impedance control is shown in figure 2. DK and IK refer to the robotʹs forward and inverse kinematics, such that the robotic end-effectorʹs gravity compensation is intended to make the contact force measurement more precise. b T R refers to the rotation matrix of the robot's tool coordinate frame to the robot's basic coordinate frame. The fuzzy controller (FLC) achieves the adjustment of the impedance modelʹs parameters (bd,kd) and we can estimate (kd,bd) by mainly scoping for the robotʹs impedance control in the whole robot work space by trials:  and k  are defined as the damping and stiffness adjustment factors respectively, with ranges from (-1 +1) and 0 b and 0 k referring to the initial damping and stiffness parameters in section 3.1. We define the deviation between the robotʹs current position and its expected trajectory as x e and its differential value as x e  , the difference between the current contact force and the expected value as f e , the designed FLC with inputs of x e and f e and an output k We multiply k  and b  by their proportional divisors to update (bd, kd) and substitute the new (bd, kd) to Eq(1) to yield the displacement n (k) X in the normal direction of the contact surface; n (k) X is regarded as displacement compensation added in order to yield the robot reference trajectory r (k) X to achieve the expected contact force and position tracking control, substituting r (k) X in the inverse kinematics to yield the robot joint space reference trajectory θr(k). Finally, in the robot joint space the proportional-derivative control is used to achieve position tracking control.

Experiment and its conclusion
The designed experiment is based on a 5-DOF industrial robot with a Wuhan company numerical control HNC-210B controller and a HSV-18 servo-drive as the hardware platform for the robot control system. The software platform is based on the DOS system, the position control cycle is 4ms, the precision of the ATI Delta force sensor used is 0.1N and the sampling frequency is 7KHz, with an aluminium, smooth round stick installed at the robot's end as the probe. The experiment is shown in figure 3. A smooth and elastic wood is fixed at the wall at a given sliding angle -the probe is intended to contact and slide on the wooden plane in the expected posture. For a simple, we define the robotʹs expected posture as perpendicular to the plane where the expected contact force is 2N in the planeʹs normal direction and the tangential movement along the plane is controlled by the direction keys on the control panel. In the experiment, the designed force control would not trigger until the contact between the probe and the plane occurred, as the initial contact force is smaller than 2N and the probe moves close to the plane until the contact force rises to 2N. During the probeʹs contact with the plane, RLS is used to estimate the initial value of the environmental parameters (Be,Ke) after the contact force becomes stable, adjusting the probe posture according to the contact torque measured (the contact torque should be 0 -in theory -in the expected posture) to make the centre of axis of the probe perpendicular to the plane. Finally, pushing the directional keys on the control panel manually in order to control the movements of the probe along the plane, it first moves along the -x direction and then along the +x, -z directions at the same time in the robotʹs basic coordinate frame with a velocity of 20mm/s.
In the experiment, the force in the normal direction of the plane measured by the sensor is shown in figure 4 (the sampling frequency is 250Hz); after the contact is stable, the deviation between the force in the normal direction and the expected value is within the range of 0.2N (a sudden change in the movement direction may cause the fluctuation of the contact force), the relative displacement of the probe in the robotʹs base coordinate -x, -y, -z directions is calculated by the robotʹs joint angular encodersʹ measure, as shown in figure 5. The displacement changes smoothly in all directions, indicating that the sliding of the probe along the plane is smooth. Since the plane does not coincide completely with the robotʹs basic coordinate frame, there is some displacement in the -z direction when the probe moves in the -x direction. Since the inclination angle of the fixed plane is arbitrary in the experiment, it can be concluded that the designed control method has achieved the contact force and position tracking control for the robot contact with an arbitrarily inclined plane.  The design of adaptive impedance control realizes the contact force and position control simultaneously along an arbitrarily inclined plane. it decreases the influence of the unknown inclined planeʹs normal direction, the uncertainties of environmental damping and stiffness, and the effect of any such changes on the robotʹs force/position control. The design method has the advantage of: (1) Improving the precision of estimation with RLS, stimulating environmental damping and stiffness estimation with the impact contact; (2) In order to reduce the influence of the contact surfaceʹs frictional force on the adjustment of the robot's end-effector posture, we adjust the robot's end-effector to the expected posture according to the contact torque when the robot makes contact stably with the environment. (3) During the robot's end-effectorʹs sliding on the inclined plane, we design the fuzzy controller to modify the damping and stiffness parameters in real-time according to the robotʹs position error and its deviation and contact force error in order to adapt to the environmental changes and compensate for the impedance modelʹs parameterʹs estimation error.