Stabilizing the End-effector of a Mobile Manipulator Using Kinematics Modeling

This paper proposes a kinematics system to stabilize and end effectors of a manipulator connected on a wheeled mobile platform. The idea to connect hybridlly two different robotics systems, serial and parallel kinematics robotics systems. The inverse and forward kinematics is modeled for the wheeled platform on the velocity level, while for the manipulator the kinematics are modeled with respect to the position. For connecting both kinematics configuration, the end-effector and the robot co-ordinates were transformed to the floor co-ordinates. Simulation system was created to check the feasibility of the solution. Four simulation examples were used to test the performance of the Kinematics control based solution. The simulation result showed the feasibility and acceptable responses of the system. In addition to, a workspace on the end-effector degrees of freedom was defined and the simulation results showed the effect of violating its limits. The practical results show performance of the hardware setup to stabilize the end effector with acceptable results.


Introduction
Mobile manipulator robots are a combination of a mobile platform and a manipulator arm. They are widely used in applications involve transportation of material in cars factories, assembly lines, and agriculture [1]. Also, they are used in the areas where human existence is hard like nuclear reactors stations where radiation range is too high and dangerous military operations like mines searching. And the applications that require working for long hours under water surface like oil stations in the bottom of the oceans [2] , moreover mobile manipulators are also used in medicine in surgeries that require high accuracy in tiny workspaces [3]. This paper is concerned with stabilizing the position of the manipulator end-effector in the space during the movement of the mobile platform. In this case the mobile platform is a twowheeled mobile robot (WMR). The mobile robot has built and controlled to follow certain reference trajectory, in order to achieve that the angular velocities of the two drive wheels must be controlled. On other hand, to achieve the mobile manipulator stabilization the most important task is building the system kinematics. There are many related researches on mobile manipulators kinematic and stabilization, as building the hybrid kinematics of a mobile modular manipulator in [4] and [5], Also designing a selfstabilizing two-wheeled platform with a rotating trunk and two 4DOF arms in [6], and developing a simulation platform for testing different control tactics to stabilize a single wheel mobile robot in [7], moreover a control scheme is designed using Lyaponov stability theory to achieve pendulum stabilization and to control the position of multi-DOF manipulator in [8], building a dynamically stable single-wheeled mobile robot with inverse mouse-ball drive in [9]. The system configuration is explained ad described in section (2). The system kinematics equations are established in section (3) including the WMR kinematics, the manipulator kinematics, and the hybrid kinematics. Section (4) presents the control system and the simulation results. It also contains the validation of the system on a small hardware setup, where as simulation example and experimental results are compared. Section (5) gives some conclusions and proposing the future work for better performance.

System Configuration
This section represents the configuration of the mobile manipulator system shown in Figure(1). The system consists of two different types of robotic systems. The first system is described by two degree of freedom wheeled mobile robot (2DOF WMR), which is considered as an example for a typical parallel (closed chain) kinematics problem. The second system, is described by a three DOF manipulator arm, which is considered as a serial (open chain) kinematics modeling. The WMR is supposed to have two DC motors attached to the driven wheels (velocity controlled), while the third wheel is a castor wheel for keeping the WMR body stable. The manipulator arm is fixed on the center of the WMR surface. It consists of three links with three revolute joints. Each joint is actuated by a servomotor attached within angular position controlled loop.

The Kinematics Modelling
The kinematics modeling problem consists of two parts : Forward kinematics and Inverse kinematics. Since we have two different types of kinematics configuration, then we will have some sort of hybrid connection between them. Such kind of configuration is considered as hybrid robotic kinematics configuration. The system modeling consists of two parts, the first part is the closed chain kinematics "WMR kinematics", and the second part is the opened chain kinematics "the manipulator kinematics". This section explains the theoretical modeling of these two system and how they are inter-connected.

Wheeled Mobile Robot (WMR) Kinematics
In this system the wheeled mobile platform is considered as the base platform, the one where the disturbance will be subjected to. The kinematic model for the two-wheeled differential drive mobile robot is a well-known model, where its kinematics modeling was obtained in previous literatures [10], [11]. The base platform co-ordinates are assigned on the WMR as described in Figure  . These variables will be in a great assessment for the next two sub sections. For simplification"C" will stand for "Cos" and "S" stands for "Sin"

WMR Sensed Forward Kinematics:
In [12] the sensed forward kinematics described by equations. (1) And Where the first one is for the linear velocity and the second is for the angular velocity of the mobile robot. By using different method presented in [13], the sensed forward kinematics depends on distinguishing between the sensed and non-sensed velocities.
In addition to defining the Jacobean Matrix for each wheel, this method was used to obtain the following solution.
Where ̇ is the robot velocities, this solution is logic and does not contradict with the one proposed earlier, where the velocity on the X axis is Zero all the time. However, these velocities are in the robot co-ordinates and will be transformed in the floor coordinates using the following equation.

Actuated Inverse Kinematics:
From the same work of [13], a proposed method of obtaining the inverse kinematics solution based on separating the actuated and non-actuated variables of each wheel and combining them in the robot composite equation and the inverse solution will be as follows:

The Manipulator Kinematics
The manipulator is defined by its configuration in Figure (3). The manipulator has three revolute joints connecting three links and the WMR with each other. The WMR platform is the base coordinate system which is defined by the variable Po = and the end effector co-ordinates is defined by Pe = , where is a constant value described by the height of the car as shown in Figure (4)

Forward kinematics
The forward kinematics problem is concerned with the relationship between the individual joints of the robot manipulator and the position of end-effector (the end-effector is the final point on the arm). In other words, the forward kinematics problem is to determine the position of the endeffector, given the values for the joint variables of the robot. The joint variables are the angles between the links. One of the main known methods for obtaining the manipulator forward kinematics is the Denavit Hartenberg [14].After using the D-H method, the end-effector position can be obtained from the equations below: Where: end-effector position on X axis the end-effector position on Y axis

Inverse kinematics
The inverse kinematics of the manipulator is determination of the joint variables (angles) in terms of the end-effector position and orientation. By using simple geometric and trigonometric relation in [14], we can obtain the following equation:

Hybrid kinematics
Since, mobile manipulator system consists of a mobile platform (WMR) and manipulator arm, then during the WMR movement in a certain trajectory its position with respect to the floor coordinate will change. As a result, the manipulator local coordinate will change with respect to the floor co-ordinates. Some relations linking these two co-ordinates are developed from Figure (4). Where:

The Control System and Simulation Results
The stabilization problem is concerned with fixing the manipulator end-effector at a specific point in the space regardless to the mobile base movement. In order to achieve the stabilization of the manipulator the mobile base must should drive within the system workspace which depends on the length of manipulator links and the servo motors angles range.
The system control includes both the WMR (Wheeled Mobile Robot) trajectory control and the manipulator position control. In Figure (5) the input is the desire position of WMR with respect to the floor co-ordinates. The position contains the desired values of position on the XF and YF floor axes. Since the kinematics of the WMR is normally developed for the robot velocities level, the input position has been differentiated to obtain the robot velocity ̇. Once the robot velocity is calculated each wheel angular velocity and can be obtained by using the WMR inverse kinematics calculations. As appears in the block diagram in Figure (5) the implementation of the axis level control is required to control the drive motors output velocities using a PID controller. The next step is using the WMR forward kinematic calculations to find the robot measured velocity ̇ from the obtained angular velocities and . The final step in the WMR control is finding the robot measured position from the integration of the robot measured velocity ̇ . As for the manipulator control the system has two inputs the first one is the instant platform position which changes according to the WMR movement in the space, the second input is the desired stabilization position this input is fixed. The system output is the measured end-effector position . The first step is using the manipulator inverse kinematics calculations to obtain the manipulator angles , and from the position inputs, then by using a PID controller to control the manipulator servomotors position. Finally, by using the manipulator forward kinematics calculations given the motors measured position , and the end-effector position can be obtained.

Simulation and Experimental Results
The simulation is carried out using the m-files under matlab environment to test the performance of this system using several types of inputs. Four simulation examples are presented in this section for two types of trajectory disturbances on the wheeled mobile platform. As it was mentioned before that the main objectives to stabilize the end effector of the manipulator with any disturbances introduced using the wheeled platform. The disturbances are generated within a pre-defined workspace on the X and Y axes f the mobile platform. This workspace depends on the manipulator end-effector height (Ze), for example, if ze = 5 mm then the work space of the mobile platform is x = ±21.6 mm,y = ±21.6 mm, if ze = 15 mm then x = ±19.4 mm,y = ±19.4 mm. The first two simulation examples introduce shape trajectory disturbances to the WMR. The end effector may be stabilized within work space displacement for the WMPlatform. Therefore, the following examples present the disturbances of the wheeled platform once within the workspace limits and once out of the workspace limits as shown in Figure(12-c). The two trajectories start from the bottom of each shape at co-ordinates of p1 = Tand p2 = respectively as shown in Figure (12-d). The Figures (12-a,b and c) shows the response of the WMR velocities as in Vx,Vy and ̇ velocities with respect to the floor co-ordinates system. The shape is chosen because it introduces variable robot velocities and accelerations, which will show the response and effect of the dynamical response of the system. It is noticed that the control system for the wheeled mobile platform proves its stability and acceptable performance. The estimated output velocities from the forward kinematics follow the reference robot velocities for both examples (Within and Out of workspace). However, the end effector stabilization coordinates will show different responses as shown in Figure (7).
The figure shows the differences in responses for the two examples for stabilizing the co-ordinates at the position of [X Y Z] = [15mm 15mm 20mm], which gives mobile platform's workspace limits x = ±16 mm, y = ±16 mm as the Ze = 20mm. The X and Y co-ordinates are chosen to be the center starting point on the wheeled mobile trajectory. and the Z co-ordinate it the middle range of the manipulator height. As noticed from the figure, for the example of "within work space trajectory", the end effector is stabilized for the given end effector co-ordinates through the whole trajectory. However, for the example of "out of workspace", there are some disturbances for end effector co-ordinates appearing when the wheeled platform violate the workspace limits during the interval of 8 to 16 seconds. This interval of time is represented when the wheeled base platform start motion in 15mm < Y < −15mm. As shown in figure (11), the manipulator angles' and have similar trajectories for both the simulation and practical results. The deviation in their parameters is normal, as the simulation results ignore the motor delay, friction and mechanical delays.   10) and (11)) responses validate the hardware-setup. The end-effector position trajectory cannot be measured using the forward kinematics estimator, because it will not show the errors generated due to the ignored dynamic parameters. Therefore, a marker was placed by the end effector to draw the expected end effector results on a white board as shown in Figure (12). The shown trajectory is the end-effector trajectory, which is the error resulted due to the disturbances of the wheeled base. The error shown is less than 18 mm on the Y axis and less than 3 mm on the X axis. The percentage error on the Y axis is almost 5.6%, which is acceptable for such system. This system requires an endeffector compensator that reduces such error to 1%.

Conclusion
This paper presented the stabilization Kinematics Based Control system for stabilizing end-effector of three linked manipulator attached to a wheeled mobile base platform. The system was kinematicalty modeled for the 2DOF base platform and the 3DOF manipulator. The two different robotic systems were hybirdly connected using obtained modeling equations.
The system was simulated for four examples to show the effective performance of the proposed system. It is concluded that, firstly the system will operate effectively as long as the base platform not violating the limits of the workspace. Secondly, for different base disturbances frequencies, the system will generate no singularities in the manipulator kinematics configurations. As the manipulator singularity is a very important problem to be discussed in future work. The system was validate on a hardware setup and the experimental results gave an acceptable error for such open loop systems. As a future work, the system will be dynamically modeled to overcome the mathematical approximations found in the kinematics modeling.