Systematic Analysis of an IEED Unit Based in a New Methodology for M&S

Abstract

In the field of modeling and simulation of mechatronics designs of high complexity, is presented a systematic analysis of an IEDD unit [1] (improvised explosive device disposal) based in a new methodology for modeling and simulation divided into 6 stages in order to increase the accuracy of validation of whole system, this mechatronic unit is a Non-holonomic Unmanned wheeled mobile manipulator, MU-NH-WMM, formed by a Differential Traction and a manipulator arm with 4 degrees of freedom mounted on wheeled mobile base, hence the contribution of this work is a novel methodology based on a practice proposal of philosophy of mechatronics design, which establishes the suitable kinematics to coupled wheeled mobile manipulator, where the motion equations and kinematics transformations are the base of the specific stages in order to obtain the dynamic of coupled system, validating the behavior and the trajectories tracking, in order to achieve the complex tasks of approaching to work area and the appropiatehandling of explosive device, this work is focused in the first of them; such that the errors in the model can be detected and later confined by proposed control.

1. Introduction

This Mechatronic Unit EOD/IEDD (explosive ordinancedisposal/improvised explosive device disposal) takes theplace of humans in the work of bomb detection, removal, transportation and detonation (Graham, T., 2006). Bombs thatare difficult to neutralize or demolish because of the location may not be suitable to explode for fear of human injury orbuilding damage. Due to this fact, it is clear the disposaloperation is a big challenge for the operators especially whenthe surroundings are complex and cluttered (State Agency for Civil Protection, 2005).

The MU-NH-WMM is a multi-link arm with ability tomove in a plane or space which is mounted on a vehicle(Zuñiga, L.A., 2008). These systems may have especial andimportant tasks such that these missions can result instabilityleading to overturning. Although the motion of manipulatormay have universe effects on the stability of the card, becauseof dynamic interface between the vehicle and manipulator, the action of manipulator may be controlled to compensatethe threshold of stability (Zuñiga, L.A., 2008).

In previous work, a modeling and simulation proposal for thismechatronic unit was made by considering the mobile unitand the manipulator in a separated way (Ghaffari, A., 2004). Thepresent work considers the modeling and simulation of boththe mobile unit and the manipulator in a coupled way; thisrepresents more accurately the behavior of the whole robot.

The kinematics of the mechatronic unit for its orientation andsteering angle along the path is derived. Next unit dynamicsare derived in terms of path geometry, and the constraints onunit motions are formulated in this work. The stability of the MU-NH-WMM has a close relation with the unit's motion. In this document, the nonlinear equations of the MU-NH-WMM which moves in a vertical plane havebeen derived.

The measure for determining the stability criterion of the MU-NH-WMM is the tires upward force (Bayle, B., 2003). This work is structured first with the explanation about methodology for modeling and simulation, next is presented the case of Approaching to work area using the Stage 2, after is depicted the case of Transport, using ythe Stage 4, in the next section of this document is shown the case of Disposal of explosive device using the stage 5, finally is validated the methodology in the last section.

2. Modeling and Simulation Methodology

In order to model and simulate the mechatronics unit is necessary to propose a methodology to validate the correct behavior of the coupled system, which considers the interaction between various tools, this interaction is shown in the figure 2. The next proposal methodology is applied for modeling and simulation the behavior of mechatronic unit in the handling task, which is depicted in the next figure.

Fig. 1.

Block Diagram of Methodology Proposed

Fig. 2.

Methodology proposal of M&S

The next proposal methodology is applied for modeling and simulation the behavior of mechatronic unit.

To model and simulate the complete system are considered six stages, which in order to the case of approaching task of explosive device are omitted the stages 1 and 3, in addition the stage 2, 4 and 5 are done according to stage. For modeling the complete system are considered six stages:

• Stage 1.

The coupled MU-UWMM in steady state, the manipulator uses its 4 degree of freedom for a test of trajectory tracking, considering that the platform rotates over itself.

Stage 2.

The MU-NH-UWMR with the mounted manipulator in its platform, executes the test of trajectories tracking.

Stage 3.

The coupled MU-UWMM is moved forward in a straight line moving its manipulator arm.

Stage 4.

The coupled MU-NH-UWMM executes the test of trajectories tracking.

Stage 5.

Simulation static (FEM) and dynamic of coupled system.

Stage 6.

Simulation static (FEM) and dynamic of coupled system.

First is considered the modeling of the manipulator coupled over a platform rotation and then develops a mathematical model of the unit coupled in movement. To begin with the modeling and simulation is necessary to establish the parameters of design and the conceptual design according with the mechatronic design of MU-NH-UWMM, these are shown in the Table 1.

Table 1.

Design parameters of MU-NH-UWMM

Parameter	Amount
Max. Load	10 kg.
Speed	1.7 km/hr.
Gripper	Interchangeable
Climbing	Stair of NOM-001-STPS-1999
Long, Width, Weight.	1.7 m, 1 m, 60 kg.

Fig. 3.

Conceptual design of MU-NH-UWMM

The nomenclature that will be used for the modeling and simulation of MU-NH-UWMM is shown in the next table.

Remark 1, Like is shown in the figure below the locomotion of the traction system is formed by 2 drive wheels and 2 free wheels, the drive wheels are set in appositive way, which it allows the rotation of platform when v_r = -v₁, constituting the joint of torso for the case of handling task (Lewis, F. L., 1993).

Remark 2, Like is shown in the figure below the kinematic scheme for the case of handling task is governed by the equation q ^WMM = [q ^WMM q ^WMM ] ^T = [φ_c θ₁ d₂ θ₃ θ₄]^T, which omitted de translation in x_c and y_c Remark 3, Assumptions of Motion in a horizontal plane:

• 1)

Punt of contact of wheels

2)

Wheels not deformables

3)

Rotate pure

4)

In the punt of contact v=0

5)

Not slip

6)

Axis of steer ortogonales to the surface

7)

Wheels connected by a rigid body (chassis)

Fig. 4.

Kinematic Schematic for coupled system

3. Approaching {WM} (Stage 2)

The platform has two driving wheels and two passive supporting wheels. The two driving wheels are independently driven by the dc motors. The following notation will be used in the derivation of the constraint equations and dynamic equation (Yamamoto Y. & Yun X., 1994).

Fig. 5.

Kinematic Schematic for coupled system

Instantaneous center of rotation (ICR) or instantaneous center of curvature (ICC) is a cross point of all axes of the wheels (Jackey, A., 2002), with mobility degree 2 and degree of steereability 0.

$$\mathrm{q}=(x_c,y_c,{\phi }_c,{\theta }_r,{\theta }_l)$$

(1)

Three variables (x_c, y_c, φ_c) describe the position and orientation of the platform. Two variables specify the angular positions for the driving wheels (Choset, H., 2005). Adding non-holonomic constraints Is that the platform must move in the direction of the axis of symmetry (holonomic).

$$\dot{\mathrm{y}}\cos {\phi }_c-{\dot{\mathrm{x}}}_c\sin {\phi }_c-\mathrm{d}\dot{{\phi }_{\mathrm{c}}}=0$$

(2)

Are the rolling constraints, not slip (non-holonomic).

$${\dot{\mathrm{x}}}_c\cos {\phi }_c+{\dot{\mathrm{y}}}_c\sin {\phi }_c+\mathrm{b}\dot{{\phi }_{\mathrm{c}}}=\mathrm{r}\dot{\theta }\mathrm{r}$$

(3)

$${\dot{\mathrm{x}}}_c\cos {\phi }_c+{\dot{\mathrm{y}}}_c\sin {\phi }_c-\mathrm{b}\dot{{\phi }_{\mathrm{c}}}=\mathrm{r}\dot{\theta }\mathrm{r}$$

(4)

The Mobile platform's equation of motion is described by:

$$\mathrm{M}(\mathrm{q})\ddot{\mathrm{q}}+\mathrm{V}(\mathrm{q},\dot{\mathrm{q}})=\mathrm{E}(\mathrm{q})\tau -{\mathrm{A}}^{\mathrm{T}}(\mathrm{q})\lambda$$

(5)

• M(q) N × N inertia matrix

• $\mathrm{V}(\mathrm{q},\dot{\mathrm{q}})$ N × 1 positions and velocity dependent forces

• E(q) N × r input transformation matrix

• τ r-dimensional input vector

$$\mathrm{M}(\mathrm{q})=\left[\begin{array}{ccccc}m& 0& -m_{c}d\sin {\phi }_c& 0& 0\\ 0& m& m_{c}d\cos {\phi }_c& 0& 0\\ -m_{c}d\sin {\phi }_c& m_{c}d\cos {\phi }_c& I& 0& 0\\ 0& 0& 0& I_w& 0\\ 0& 0& 0& 0& I_w\end{array}\right]$$

(6)

$$\begin{gathered} \mathrm{m}={\mathrm{m}}_c+2{\mathrm{m}}_{w,}\mathrm{I}={\mathrm{I}}_c+2{\mathrm{m}}_w({\mathrm{d}}^2+{\mathrm{b}}^2)+2\text{Im}, \\ \mathrm{V}(\mathrm{q},\dot{\mathrm{q}})=\left[\begin{array}{c}-{\mathrm{m}}_{\mathrm{c}}\mathrm{d}{\dot{\phi }}_c^2\cos {\phi }_c\\ -{\mathrm{m}}_{\mathrm{c}}\mathrm{d}{\dot{\phi }}_c^2\sin {\phi }_c\\ 0\\ 0\\ 0\end{array}\right]\mathrm{E}(\mathrm{q})=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 1& 0\\ 0& 1\end{array}\right] \end{gathered}$$

(7)

4. Transport {WM+M} (Stage 4)

The system is subjected to three nonholonomic constraints of:

$${\mathrm{A}}_{\mathrm{W}\mathrm{M}\underset{\_}{\dot{\mathrm{q}}}\mathrm{W}\mathrm{M}}=\underset{\_}{0}$$

(8)

By taking the independent joint velocities ${\underset{\_}{\mathrm{v}}}_{\mathrm{B}}^{\mathrm{T}}=[{\dot{\theta }}_{\mathrm{R}}{\dot{\theta }}_{\mathrm{L}}]$ , the corresponding null-space matrix that annihilates the constraint matrix can be determined as:

$${\underset{\_}{\dot{\mathrm{q}}}}_{\mathrm{W}\mathrm{M}}={\mathrm{S}}_{\mathrm{W}\mathrm{M}}{\underset{\_}{\mathrm{V}}}_{\mathrm{W}\mathrm{M}}$$

(9)

This is valid for the case when the wheels are not slipping or skidding (Bayle B., Fourquet J.Y. & Renaud M., 2003). We define a look-ahead point P_a with Cartesian coordinates:

$${\underset{\_}{\mathrm{X}}}_{\mathrm{W}\mathrm{M}}=({\mathrm{X}}_a,{\mathrm{Y}}_a)=({\mathrm{x}}_c+{\mathrm{L}}_a{\mathrm{C}}_{\phi },{\mathrm{Y}}_c+{\mathrm{L}}_a{\mathrm{S}}_{\phi })$$

(10)

Fig. 6.

Kinematic Schematic for coupled system

The corresponding Jacobian that relates independent joint velocities to velocity of the look-ahead point is:

$${\underset{\_}{\dot{\mathrm{x}}}}_{\mathrm{W}\mathrm{M}}={{\mathrm{J}}_{\mathrm{W}\mathrm{M}}{\displaystyle \underset{\_}{\mathrm{v}}}}_{\mathrm{W}\mathrm{M}}\text{and}{\mathrm{J}}_{\mathrm{W}\mathrm{M}}=\frac{r}{2b}\left[\begin{array}{cc}{\mathrm{b}C}_{\phi }-{\mathrm{L}}_{\mathrm{d}}{\mathrm{S}}_{\phi }& {\mathrm{b}C}_{\phi }+{\mathrm{L}}_{\mathrm{d}}{\mathrm{S}}_{\phi }\\ {\mathrm{b}S}_{\phi }-{\mathrm{L}}_{\mathrm{d}}{\mathrm{S}}_{\phi }& {\mathrm{b}S}_{\phi }+{\mathrm{L}}_{\mathrm{d}}{\mathrm{S}}_{\phi }\end{array}\right]$$

(11)

$${\mathrm{H}}_{\mathrm{W}\mathrm{M}}({\underset{\_}{\mathrm{q}}}_{\mathrm{W}\mathrm{M}}){\dot{\underset{\_}{\mathrm{V}}}}_{\mathrm{W}\mathrm{M}}+{\underset{\_}{\mathrm{c}}}_{\mathrm{W}\mathrm{M}}({\underset{\_}{\mathrm{q}}}_{\mathrm{W}\mathrm{M}},{\underset{\_}{\mathrm{V}}}_{\mathrm{W}\mathrm{M}})={\mathrm{E}}_{\mathrm{W}\mathrm{M}}({\underset{\_}{\mathrm{q}}}_{\mathrm{W}\mathrm{M}}){\underset{\_}{\tau }}_{\mathrm{W}\mathrm{M}}$$

(12)

The rest of the parameters are listed in Table 2. The full set of extended generalized coordinates in this case, including the manipulator configuration variables, are q^T = [q_WM^Tq_M^T] = [x_cy_c φ θ_Rθ_Lθ₁d₂θ₃θ₄]The MU-NH-WMM possesses the same set of constraints described, Hence, the constraint matrix can be extended to:

$$\mathrm{A}\underset{\_}{\dot{\mathrm{q}}}=\underset{\_}{0}\text{and}\mathrm{A}=[{\mathrm{A}}_{\mathrm{W}\mathrm{M}}{0}_{3\times 2}]$$

(13)

Table 2.

Modeling and simulation nomenclature

Sym.	Description	Sym.	Description
W	Applied load	Pi	Potential energy of any link
mi	Mass of any link	M(q)q;	Inertia matrix
g	Gravity constant	$\mathrm{V}(\mathrm{q},\dot{\mathrm{q}})$	Vector of the centrifuges forces
l	Gyro ratio to the mass of the base	${\mathrm{F}}_{\mathrm{v}}\dot{\mathrm{q}}$	Vector of the friction forces
a	Length of link first	G(q)	Vector of the gravitational forces
b	Length of link second	τ	Torque required
Ii	Inertia of any link	Vi	Known constant coefficient of friction
Vi	Lineal velocity of any link	Fv	Diagonal matrix with terms Vi
Ki	Kinetic energy of any link	Pc	Center of mass of the platform
Pa	Location of the manipulator	mw	Mass of driving wheel plus motors.
Pr	Reference to be followed by WM	Ic	Moment of inertia of the motors
b	Distance between driving wheels	Iw	Moment of inertia of wheel & motor
r	Radius of each driving wheel	Im	Moment of inertia of wheel & motor
mc	Mass of platform without driving wheels & motors.

Table 3.

Denavit-Hartenberg kinematic parameters

Link	αi [rad]	li [m]	Δi [m]	θi [rad]	φi [rad]	xi [m]	yi [m]
C	0	0	Δc=0.2	θR, θL	φc(θR, θL)	xc	yc
cs	−π/2	lcs=0.5	0	θcs	0	0	0
cs1	0	lcs1=0.25	0	θcs1(θR, θL)	0	0	0
0	π/2	−l0=−0.4	Δ0=0.3	0	0	0	0
1	π/2	0	0	θ1	0	0	0
2	−π/2	0	Δ2=1.5	0	0	0	0
3	π/2	l3=0.2	0	θ3	0	0	0
4	0	0	0	θ4	0	0	0
E	0	0	Δ5=0.35	0	0	0	0

Taking the independent velocities ${\underset{\_}{\mathrm{v}}}^{\mathrm{T}}=\left[{\underset{\_}{\mathrm{v}}}_{\text{WM}}^{\mathrm{T}}{\underset{\_}{\dot{\mathrm{q}}}}_{\mathrm{M}}^{\mathrm{T}}\right]$ , we can similarly determine the null-space matrix S that annihilates the constraint matrix as:

$$\underset{\_}{\dot{\mathrm{q}}}=\mathrm{S}\underset{\_}{v}\text{and}\mathrm{S}=\left[\begin{array}{cc}{\mathrm{S}}_{\mathrm{W}\mathrm{M}}& 0_{5\times 2}\\ 0_{2\times 2}& {\mathrm{I}}_{2\times 2}\end{array}\right]$$

(14)

We consider the task-space to be the Cartesian coordinates of the end-effectorx = (x_e, y_e). The Jacobian relating independent velocities to task-velocity is written as:

$$\underset{\_}{\dot{\mathrm{x}}}={\mathrm{J}}_{\underset{\_}{\mathrm{V}}},\mathrm{J}=[{\mathrm{J}}_{\mathrm{W}\mathrm{M}}{\mathrm{J}}_{\mathrm{M}}]and{\mathrm{J}}_{\mathrm{M}}=\left[\begin{array}{cc}-{\mathrm{L}}_1{\mathrm{S}}_1& -{\mathrm{L}}_2{\mathrm{S}}_2\\ {\mathrm{L}}_1{\mathrm{C}}_1& {\mathrm{L}}_2{\mathrm{S}}_2\end{array}\right]$$

(15)

Finally, assume that the constrained dynamics of the MU-NH-WMM is written as:

$${\mathrm{H}}_{\mathrm{c}}(\underset{\_}{\mathrm{q}})\ddot{\underset{\_}{\mathrm{q}}}+{\mathrm{c}}_{\underset{\_}{\mathrm{c}}}(\underset{\_}{\mathrm{q}},\dot{\underset{\_}{\mathrm{q}}})={\mathrm{E}}_{\mathrm{M}}{\underset{\_}{\tau }}_{\mathrm{M}}+{\mathrm{E}}_{\mathrm{E}}\underset{\_}{\mathrm{F}}-{\mathrm{A}}^{\mathrm{T}}\underset{\_}{\lambda },\mathrm{A}\dot{\underset{\_}{\mathrm{q}}}=\underset{\_}{0}$$

(16)

$$\mathrm{H}(\underset{\_}{\mathrm{q}})\underset{\_}{\dot{\mathrm{v}}}+\underset{\_}{\mathrm{c}}(\underset{\_}{\mathrm{q}},\underset{\_}{\mathrm{v}})=\underset{\_}{\tau }(\underset{\_}{\mathrm{q}})+{\underset{\_}{\tau }}_{\mathrm{E}}(\underset{\_}{\mathrm{q}})$$

(17)

Where H = S^TH_CS is the inertia, $\underset{\_}{\mathrm{c}}={\mathrm{S}}^{\mathrm{T}}({\mathrm{H}}_{\mathrm{c}}\dot{\mathrm{S}}+{\underset{\_}{\mathrm{c}}}_{\mathrm{c}})$ is the Coriolis/centrifugal/damping terms, $\underset{\_}{\tau }={\mathrm{S}}^{\mathrm{T}}{\mathrm{E}}_{\mathrm{M}}{\underset{\_}{\tau }}_{\mathrm{M}}$ is the input torque, and ${\underset{\_}{\tau }}_{\mathrm{E}}={\mathrm{S}}^{\mathrm{T}}{\mathrm{E}}_{\mathrm{E}}\underset{\_}{\mathrm{F}}$ is the external forces/torque, all described in the feasible motion space. For a kinematic redundant system, the dimension of v is greater than the dimension $\underset{\_}{\dot{\mathrm{x}}}$ Hence, the general solution for v is written as:

$$\mathrm{v}={\mathrm{J}}^{\mathrm{\#}}\dot{\underset{\_}{\mathrm{x}}}+\mathrm{N}\dot{\underset{\_}{\mathrm{z}}}$$

(18)

Where ${\mathrm{j}}^{\mathrm{\#}}$ and N are the generalized pseudo-inverse and the null-space of J, respectively, and $\underset{\_}{\dot{\mathrm{z}}}$ is the consistent joint velocity due to the internal motion of the manipulator. A resolved acceleration scheme can be written as:

$$\dot{\underset{\_}{\mathrm{v}}}={\mathrm{J}}^{\mathrm{\#}}\ddot{\underset{\_}{\mathrm{x}}}+{\mathrm{J}}^{\mathrm{\#}}\dot{\underset{\_}{\mathrm{x}}}+\mathrm{N}\ddot{\underset{\_}{\mathrm{z}}}+\mathrm{N}\dot{\underset{\_}{\mathrm{z}}}$$

(19)

Where the first two terms are the joint-space acceleration required performing the task, and the last two terms represent the consistent joint-space acceleration due to the internal motion. The relationship between the task forces F and the joint forces $\underset{\_}{\tau }$ can be similarly written as:

$$\underset{\_}{\tau }={\mathrm{J}}^{\mathrm{T}}\underset{\_}{\mathrm{F}}={\mathrm{N}}^{\mathrm{T}}\underset{\_}{\tau }\mathrm{z}$$

(20)

Where N^T = I - J^TJ^#T projects the arbitrary (internal) joint torques ${\underset{\_}{\tau }}_{\mathrm{Z}}$ to the null-space of J^T to provide consistent manipulator motion. However, the pseudo-inverse J^# might not be “consistent” with the dynamic of the manipulator. Hence, we define a weighted pseudo-inverse of J by the inverse of the manipulator inertia matrix H⁻¹ as:

$$\bar{\mathrm{J}}={\mathrm{J}}_{{\mathrm{H}}^{-1}}^{\mathrm{\#}}={\mathrm{H}}^{-1}{\mathrm{J}}^{\mathrm{T}}({\mathrm{J}\mathrm{H}}^{-1}{\mathrm{J}}^{\mathrm{T}}{)}^{-1}$$

(21)

$$\bar{\mathrm{N}}=\mathrm{I}-\bar{\mathrm{J}}\mathrm{J}$$

(22)

Such an inertia-matrix weighted pseudoinverse has geometric significance since it endows the final task space with a kinetic energy metric defined on the tangent space of a selected manipulator (Bullo, F. & Lewis, A. D., 2004).

$$\begin{array}{rl}& {\mathrm{J}}^{\mathrm{T}}\underset{\_}{\mathrm{F}}+{\mathrm{N}}^{\mathrm{T}}\underset{\_}{\tau }={\gamma }_1+{\gamma }_2+{\gamma }_3\\ & {\gamma }_1={\mathrm{J}}^{\mathrm{T}}{\mathrm{J}}^{\mathrm{\#}\mathrm{T}}[{\mathrm{H}\mathrm{J}}^{\mathrm{\#}}(\underset{\_}{\ddot{\mathrm{x}}}-\dot{\mathrm{J}}\underset{\_}{\dot{z}})+\underset{\_}{\mathrm{c}}-{\underset{\_}{\tau }}_{\mathrm{E}}]\\ & {\gamma }_2={\mathrm{N}}^{\mathrm{T}}(\mathrm{H}\mathrm{N}\underset{\_}{\ddot{\mathrm{z}}}+\underset{\_}{\mathrm{c}}+{\underset{\_}{\tau }}_{\mathrm{E}})\\ & {\gamma }_3={\mathrm{J}}^{\mathrm{T}}{\mathrm{J}}^{\mathrm{\#}\mathrm{T}}(\mathrm{H}\mathrm{N}\underset{\_}{\ddot{\mathrm{z}}}+{\mathrm{N}}^{\mathrm{T}}{\mathrm{H}\mathrm{J}}^{\mathrm{\#}}(\underset{\_}{\ddot{\mathrm{x}}}-\dot{\mathrm{J}}\underset{\_}{\dot{\mathrm{z}}})\end{array}$$

(23)

In (23), Γ₁ filters the overall dynamics using J^TJ^#T to allow only the task-space dynamics to pass through. Likewise, Γ₂ uses NT to retain the null-space component of the combined dynamics. Γ₃Includes all the cross-coupling dynamic terms. Thus, taking ${\mathrm{J}}^{\mathrm{\#}}=\bar{\mathrm{J}}$ , is:

$$\mathrm{M}(\dot{\mathrm{q}})\underset{\_}{\dot{\mathrm{v}}}+\underset{\_}{\mu }(\underset{\_}{\mathrm{q}},\underset{\_}{\mathrm{v}})=\underset{\_}{\mathrm{F}}+{\underset{\_}{\mathrm{F}}}_{\mathrm{E}}$$

(24)

Where $\mathrm{M}={\bar{\mathrm{J}}}^{\mathrm{T}}\mathrm{H}\bar{\mathrm{J}}=({\text{JH}}^{-1}{\mathrm{J}}^{\mathrm{T}}{)}^{-1}$ is the apparent inertia, $\underset{\_}{\mu }={\bar{\mathrm{J}}}^{\mathrm{T}}\underset{\_}{\mathrm{c}}-\text{Mj}\underset{\_}{\mathrm{v}}$ is the apparent Coriolis/centrifugal/damping terms, $\underset{\_}{\mathrm{F}}={\bar{\mathrm{J}}}^{\mathrm{T}}\underset{\_}{\tau }$ is the apparent input force, and ${\underset{\_}{\mathrm{F}}}_{\mathrm{E}}={\bar{\mathrm{J}}}^{\mathrm{T}}{\underset{\_}{\tau }}_{\mathrm{E}}$ is the apparent external force, all described in the task-space. The control input that decouples the task-and nullspace can then be defined as:

$$\underset{\_}{\tau }={\mathrm{J}}^{\mathrm{T}}\mathrm{M}(\underset{\_}{\mathrm{u}}-\mathrm{j}\underset{\_}{\mathrm{v}})+{\bar{\mathrm{N}}}^{\mathrm{T}}\mathrm{H}(\underset{\_}{\mathrm{u}}+\overline{\stackrel{̇}{\mathrm{J}}}\underset{\_}{\dot{\mathrm{x}}})+\underset{\_}{\mu }+{\mathrm{J}}^{\mathrm{T}}{\underset{\_}{\mathrm{F}}}_{\mathrm{E}}$$

(25)

Where u and v are the new control inputs to the task- and null-space, respectively. After substituting (25) into (23) and simplifying, the final equivalent controlled closed loop dynamics in the independent task- and null-spaces may be written, respectively, as:

$$\underset{\_}{\mathrm{u}}-\underset{\_}{\ddot{\mathrm{x}}}=\underset{\_}{0}\text{and}\underset{\_}{\mathrm{u}}-\underset{\_}{\dot{\mathrm{v}}}+\overline{\stackrel{̇}{\mathrm{J}}}\underset{\_}{\dot{\mathrm{x}}}=\underset{\_}{0}$$

(26)

We note that due to the explicit performance of nonlinear feedback linearization, the two subspace dynamics correspond to those of equivalent linear systems. Hence for the task-space, we select an impedance controller (Tang, C. P.; Bhatt, R. M.; Abou-Samah, M.; & Krovi, V., 2006):

$$\underset{\_}{\mathrm{u}}={\underset{\_}{\ddot{\mathrm{x}}}}^{\mathrm{d}}+{\mathrm{K}}_{\mathrm{v}}\underset{\_}{\dot{\mathrm{e}}}+{\mathrm{K}}_{\mathrm{P}}\underset{\_}{\mathrm{e}}+{\mathrm{K}}_{\mathrm{F}}({\underset{\_}{\mathrm{F}}}_{\mathrm{E}}^{\mathrm{d}}-{\underset{\_}{\mathrm{F}}}_{\mathrm{E}})$$

(27)

Where the superscript d indicates the desired quantities; $\underset{\_}{\mathrm{e}}={\underset{\_}{\mathrm{x}}}^{\mathrm{d}}-\underset{\_}{\mathrm{x}},\underset{\_}{\dot{\mathrm{e}}}={\dot{\underset{\_}{\mathrm{x}}}}^{\mathrm{d}}-\underset{\_}{\dot{\mathrm{x}}}$ are the end-effector position and velocity errors, respectively; K_V, K_P and K_F are positive definite constant matrices (to maintain stability); and F_E^d is the desired end-effector force which creates the closed-loop task-space behavior of a second-order spring-damper-mass system with a driving force determined by the desired impedance as:

$$\underset{\_}{\ddot{\mathrm{e}}}+{\mathrm{K}}_{\mathrm{v}}\underset{\_}{\dot{\mathrm{e}}}+{\mathrm{K}}_{\mathrm{p}}\underset{\_}{\mathrm{e}}=-{\mathrm{K}}_{\mathrm{F}}({\underset{\_}{\mathrm{F}}}_{\mathrm{E}}^{\mathrm{d}}-{\underset{\_}{\mathrm{F}}}_{\mathrm{E}})$$

(28)

Where to ë = ẍ^d - ẍ is the acceleration errors. The selection of the control gains can now be done by appropriate linear pole-placement techniques. In this paper, we consider F_E^d = 0 and F_E is an “external disturbance force” at the end-effector. However, this can be extended to the case with force regulation. Finally, it is noted that such a nonlinear feedback linearization allows one to explicitly cancel the nonlinearities and replace the original dynamics with an (exact) linearized dynamical system. Such a model-based controller design tends to be dependent on accurate dynamic modeling. However, as noted in (Tan, J.; Xi, N.;& Wang, Y., 2003), even in the absence of a model, PD control with gravity compensation, is sufficient to ensure point stabilization (and even permits tracking albeit with some lag). Hence, inadequate modeling here would allow a graceful but stable degradation of the performance during the transition from the completely linearized (computed torque) case to the uncompensated (unmodeled) case. In the first case, a kinematic controller for the MU-WMR is derived from the kinematic equations in (11). As long as the look-ahead point does not coincide with the midpoint of the wheel axle J_B is nonsingular. Hence the wheel velocities v_B can be determined from the velocity of the look-ahead point as:

$${\underset{\_}{\mathrm{V}}}_{\mathrm{B}}={\mathrm{J}}_{\mathrm{B}}^{-1}{\underset{\_}{\dot{\mathrm{x}}}}_{\mathrm{B}}$$

(29)

This can be extended to factor in a closed-loop Cartesian error dynamic for the look-ahead point as:

$${\underset{\_}{\mathrm{V}}}_{\mathrm{B}}={\mathrm{J}}_{\mathrm{B}}^{-1}[{\underset{\_}{\dot{\mathrm{X}}}}_{\mathrm{B}}^{\mathrm{d}}+{\mathrm{K}}_{\mathrm{P}\mathrm{B}}({\underset{\_}{\mathrm{X}}}_{\mathrm{B}}^{\mathrm{d}}-{\underset{\_}{\mathrm{X}}}_{\mathrm{B}})]$$

(30)

Where x_B^d and ${\dot{\underset{\_}{\mathrm{x}}}}_{\mathrm{B}}^{\mathrm{d}}$ are the desired position and velocity of the look ahead point, respectively. In the second case, we choose the dynamic tracking algorithm to achieve input-output linearization and input-output decoupling. In our case, choosing the look-ahead point to be the output of the system is corresponding to the Type I output. The necessary and sufficient condition for input-output linearization for the proposed choice of outputs is that the decoupling matrix has full rank. Thus, similarly, the only singularity occurs when the look-ahead point coincides with the midpoint of the wheel axle. Using the dynamic equation in (12), the nonlinear feedback ${\underset{\_}{\mathrm{T}}}_{\mathrm{B}}={\mathrm{E}}_{\mathrm{B}}^{-1}[{\mathrm{H}}_{\mathrm{B}}{\underset{\_}{\eta }}_{\mathbf{B}}+{\underset{\_}{\mathrm{c}}}_{\mathrm{B}}]$ , and a change of input $\underset{\_}{\eta }={\mathrm{J}}_{\mathrm{B}}^{-1}(\underset{\_}{\xi }-{\dot{\mathrm{J}}}_{\mathrm{B}}\mathrm{v})$ , the linearized and decoupled system can be converted to ${\ddot{\underset{\_}{\mathrm{x}}}}_{\mathrm{b}}=\underset{\_}{\xi }$ , whose controller is designed using standard linear control techniques.

5. Disposal {WMM} (Stage 5)

In this section, a mechatronic unit with ability to move in a vertical plane which is mounted on a 4-wheeled vehicle is considered. Because of the especial task of the mobile manipulator, the path of mechatronic unit and the desired task of the end-effector are predefined. Thus there are some limited configurations of manipulator while moving along its desired path. These arrangements of manipulator do not satisfy the zero. The schematic picture of redundant manipulator with attached coordinates to its links is shown in Figure 7, the free body diagram of the mechatronic unit coupled is also shown in this figure.

Fig. 7.

Schematic diagram of mechatronic unit.

The forces and torques equations in x-y plane are as follows:

$${\mathrm{F}}_{\mathrm{y}1}+{\mathrm{F}}_{\mathrm{y}2}+{\mathrm{F}}_{\mathrm{y}3}+{\mathrm{F}}_{\mathrm{y}4}+{\mathrm{f}}_{\mathrm{y}}-{\mathrm{M}}_{\mathrm{g}}={\text{ma}}_{\mathrm{y}}$$

(31)

$$\mathrm{b}({\mathrm{F}}_{\mathrm{y}2}+{\mathrm{F}}_{\mathrm{y}4})+(-{\mathrm{M}}_{\mathrm{g}}+{\mathrm{f}}_{\mathrm{y}}+{\text{ma}}_{\mathrm{y}})\frac{\mathrm{b}}{2}-{\text{hf}}_{\mathrm{x}}+{\mathrm{M}}_{\mathrm{z}}-\frac{\mathrm{h}}{2}{\text{ma}}_{\mathrm{x}}=0$$

(32)

Assuming that the vehicle of mobile manipulator is symmetric with respect to x-y plane, it is clear that the forces F_y1 and F_y3 are equal and also F_y2 is equal to F_y4 Considering the unit at the steady state conditions, where velocities at different directions are constant, the kinematic equations may be found as follows:

$${\mathrm{P}}_{\mathrm{x}}=\mathrm{a}+{\mathrm{L}}_1\cos ({\theta }_1)+{\lambda }_2{\mathrm{L}}_2\cos ({\theta }_1)+{\lambda }_2{\mathrm{L}}_3\cos ({\theta }_1+{\theta }_3)$$

(33)

$${\mathrm{P}}_{\mathrm{y}}=\mathrm{a}+{\mathrm{L}}_1\sin ({\theta }_1)+{\lambda }_2{\mathrm{L}}_2\sin ({\theta }_1)+{\lambda }_2{\mathrm{L}}_3\sin ({\theta }_1+{\theta }_3)$$

(34)

The velocities and acceleration equations of the end-effector can also be determined by differentiating from equations with respect to time (Fruchard, M.; Morin, P. & Samson, C., 2006). Therefore, for solving the inverse kinematics of the system, two equations for the position of the end-effector in the x-y plane, two equations for velocity of the end-effector are employed. These are further to the two equations for the acceleration of the end-effector which are zero in this work. The 9 unknown variables are; θ₁, Λ₂, θ₃ for angular positions, ${\dot{\theta }}_1,{\dot{\lambda }}_2,{\dot{\theta }}_3$ for angular velocities and ${\ddot{\theta }}_1,{\ddot{\lambda }}_2,{\ddot{\theta }}_3$ for angular accelerations, respectively. Subsequently, the method for determining the 9 unknown variables with 6 equations will be discussed.

For optimal stability of the mobile manipulator, the summation of tires upward forces F_y1, F_y3 should be set equal to the summation of forces F_y2 and F_y4. This leads us to define a performance index to be at its minimum value during the motion of mobile manipulator. This performance index can be set equal to the torque exerted on the first joint of manipulator attached to theunit. This equation is calculated by Lagrangian method and its accuracy is checked with the Newton- Euler iteration technique (Velinsky S. A. & Gardner, J. F., 2000). The torque acting on the first joint in the case of shooting to disposal the explosive device attached to the unit is as follow:

$${\mathrm{T}}_1={\mathrm{m}}_3{\lambda }_2{\mathrm{L}}_3^2({\ddot{\theta }}_1+{\ddot{\theta }}_3)+{\mathrm{m}}_3\mathrm{g}{\mathrm{L}}_3{\mathrm{C}}_{13}$$

(35)

6. Results and Validation

The Performance index of WMM when v = 1(m/s), b = 0.5(m), α = 0.5(rad). The design criterion was to control the mobile platform so that the manipulator is maintained at a configuration which maximizes the manipulability measure. We could use the desired path y^d to feed back the error (Zuñiga, L.A.; Pedraza, J.C.; Gorrostieta E. & Ramos J.M., 2009)e = y^d − y.

$$\ddot{\mathrm{y}}=\mathrm{v}={\ddot{\mathrm{y}}}^{\mathrm{d}}+{\mathrm{K}}_{\mathrm{d}}({\dot{\mathrm{y}}}^{\mathrm{d}}-\dot{\mathrm{y}})+{\mathrm{K}}_{\mathrm{p}}({\mathrm{y}}^{\mathrm{d}}-\mathrm{y})$$

(36)

Fig. 8.

Demostration of rotation of mobile base about itself

Fig. 9.

Tracking trayectory in approaching

Fig. 10.

Tracking trayectory in approaching

Fig. 11.

Tracking trayectory in approaching

Fig. 12.

Displacement of mobile base

Fig. 13.

Velocity and acceleration of left wheel

Fig. 14.

Displacement of left wheel

Fig. 15.

Velocity and acceleration of right wheel

Fig. 16.

Displacement of right wheel.

In the next figure is presented the workspace of MU-WMM (Zuñiga, L.A., 2009).

Fig. 17.

Workspace of mechatronic unit

The validation of mechanic structure is done by CAE software applying the analysis for element finite (fem) (Zuñiga, L.A., 2009), manufacturing the parts by aluminum 6061-T6, to load of 10 kg.

Fig. 18.

Prototype in test of trajectory tracking

Fig. 12.

Final technical specifications

7. Conclusion

Was satisfactory the methodology proposed of modeling and simulation which the Virtual rapid prototyping emulate the behavior of the robot validating the load and the mechanic system. In addition, the simulations displayed using a solid model, in such way that is possible observe the simulated motion system response. Dynamics and simulation environments were easily changed in either the simulation and in the control algorithm for the handling task, hence the proposed methodology gives the big picture of whole system, makes it easy understand the function of MU-NH-WMM. The stability of mechanical system is attached to symmetry of geometry and of loads. As future work, is considering the behavioral analysis in the approaching and transportation tasks considering the dynamic of load as well as the controller design for the coupled system and the methodology including the mechatronic design and the interaction with the mechanical, electronically and control systems.