Guidance-Based Motion Planning of Autonomous Systems

*Bülent Özkan*

## **Abstract**

Motion planning is a significant stage in the control of autonomous systems. As an alternative method, guidance approach is proposed for the motion planning of those systems. In guided munitions, guidance laws determine the success of the guidance systems designed to steer systems such as missiles and guided bombs towards predefined targets. The guidance laws designated according to determinative agents such as the firing position of the munition, target type, and operational requirements try to provide the munition with arriving at the target point even under the disturbing effects. In this study, the applicability of the guidance laws to autonomous systems is investigated in a manner similar to the approach for the guided munitions. For this purpose, the motion planning of the selected robotic arm, tracked land vehicle, and quadrotor is tried to be performed in order to move them to predefined target points. Having designed the control systems compatible to the selected guidance laws for the considered systems, the corresponding guidance scheme is constructed. Eventually, after conducting the relevant computer simulations, it is observed that the desired target chase can be made in a successive manner for all cases.

**Keywords:** motion planning, guidance, guidance law, linear homing, autonomous system

## **1. Introduction**

Motion planning constitutes one of the primary stages in the control of autonomous systems. The autonomous systems can perform their planned tasks under several environmental conditions as per the designated motion planning algorithms in accordance certain performance criteria. The mentioned criteria may include minimum energy or minimum time consumption and shortest path length. For the motion planning purpose, several different algorithms are proposed by relevant researchers. These methods have certain advantages and disadvantages over the others [1–4].

As an alternative approach, guidance schemes can be used in motion planning. Those schemes involve an upper-level guidance algorithm and a lower-level control system. In fact, guidance and control loops can be introduced as officer and soldier, respectively. In other words, as the guidance algorithm behaves as the "master," the control system takes the "slave" role in this scene [5].

The guidance schemes are widely implemented to munitions. Guidance and control systems are designed in a compatible manner with munition dynamics so that the munitions including missiles and guided bombs can carry payloads towards specified target points as planned. The guidance part of the mentioned guidance

and control system constitutes the kinematic relationships established as per the relative position between the munition and intended target point, while the control system is the closed-loop control system constructed based on the dynamic model of the munition under consideration in order to realize the guidance commands generated by the guidance part. In this extent, the guidance approach enrolls as a motion planning scheme for the munition [5].

regarding the motion characteristics of the autonomous systems dealt with, the robotic arm operates on the horizontal plane, and the vertical displacement of the tracked land vehicle is in a negligible level compared to its longitudinal and lateral motion on the ground plane. Thus, the motion profiles are described on a plane for both the systems. Unlike them, the quadrotor flies in the sky towards all three directions. This fact leads to handle its dynamic behavior in a three-dimensional space. Having completed the computer simulations, it is observed that all three

As a motion planning strategy, guidance approach can be applied to service robots which are utilized to accomplish certain motion profiles apart from the industrial systems. In this way, it is intended to perform hazardous, tedious, and time-consuming tasks in a more efficient and accurate manner in daily use. The mentioned category for service robots, actually, involves not only articulated robot

The schematic views of the robotic arm and mounting line, i.e., trajectory, containing the slot on which the component grasped by the gripper of the arm is

The system whose schematic view is presented in **Figure 1** consists of a twodegree-of-freedom robotic arm and moving mounting line. Here, the object to be put on the slot on the mounting line by the gripper of the robotic arm is taken in spherical geometry, and thus its orientation is ignored. This way, the degree of freedom of the carried object is reduced to two. In other words, it becomes possible to define the instantaneous planar location of the object by regarding the lateral and vertical position components of a point, i.e., point P, on the object. So, a robotic arm is required with minimum degree of freedom of two in order for the object with two degrees of freedom to be carried upon a specified point on the plane without any control deficiency. The definitions made in **Figure 1** are listed as follows [8]: x and y: lateral and vertical axes of the Earth-fixed frame symbolized by *F0.*

<sup>2</sup> : unit vectors denoting the x- and y-axis of *F0.*

systems can be carried to the intended target points by LHG law [8].

*Guidance-Based Motion Planning of Autonomous Systems*

*DOI: http://dx.doi.org/10.5772/intechopen.91830*

manipulators but also moving and flying autonomous structures as well.

**2. Description of the systems**

**2.1 Description of the robotic arm**

placed is given in **Figure 1**.

!ð Þ <sup>0</sup>

O and A: joints of the robotic arm.

*System consisting of a robotic arm and moving mounting line [8].*

*u* !ð Þ <sup>0</sup> <sup>1</sup> and *u*

**Figure 1.**

**61**

The type of the command yielded by the guidance system depends on the selected guidance law. Namely, as the output of the proportional navigation guidance law which constitutes the most widely used guidance law in guided munitions is the lateral component of the linear acceleration vector of the munition or change of the lateral angular component of the linear velocity vector in time as per the application, the command of the body pursuit guidance law becomes the components of the angle between the body longitudinal axis of the munition and the lateral axis of the Earth-fixed frame [5, 6].

Guidance laws designated to move the guided munitions towards specified target points make their motion planning in a sense. Regarding this property, there seems no serious obstacle on their implementation on autonomous systems other than guided munition. In this study, the orientation of the sample autonomous mechatronic systems involving a robotic arm, tracked land vehicle, and quadrotor to predefined target points using the linear homing guidance (LHG) law and the relevant computer simulations is carried out. Here, these systems are chosen as very common systems encountered in the physical world. In the considered cases, the LHG law is an angle-based approach, and it takes the flight path angle components of the systems into consideration. Also, the selection of the mentioned mechatronic systems allows evaluating the convenience of the proposed approach in the planar and spatial engagement situations. Moreover, single- and two-stage control systems are utilized in accordance with the LHG law.

Guidance-based motion planning schemes are developed for certain robotic arm configurations [7]. In this scene, the indicated strategy allows the operators to run the moving belt of the robotic arm-belt assembly line within a mounting line in a more continuous and faster manner than the usual methods. Moreover, it may become to suppress or at least minimize the drawbacks of the conventional approaches by regarding the guidance-based motion planning method. Namely, although many conventional methods require the belt assembly to halt at intermediate placing instants, the guidance-based approach makes possible to place the objects under consideration onto the belt while it remains running during operation [8].

Different from wheeled vehicles, tracked land vehicles are directed as per a sliding motion which depends on the rotation of the vehicle about an instantaneous rotation center. In other words, they can be oriented to left or right by rotating about their instantaneous rotation center in the convenient sense. Regarding the motion of these vehicles on soil surfaces especially, the motion planning becomes harder. As a remedy to this inconvenience, the guidance-based path planning approach is proposed in the present study [8].

As the third application, the motion planning of a quadrotor which is intended to carry a payload from a stationary initial point to a prescribed moving land platform at a moderate distance for either military or civil purpose is investigated. Here, the payload can be munition, food, or first aid material. Since the moving platform specified as the target is assumed to be far away from the initial point, it is desired for the quadrotor to catch it within the shortest time duration possible and consume the energy at a minimum level [8, 9].

In the computer simulations in which the planar motion of the robotic arm and tracked land vehicle and the spatial motion of the quadrotor are taken into consideration, it is assumed that the targets are moving along specified trajectories. Here, *Guidance-Based Motion Planning of Autonomous Systems DOI: http://dx.doi.org/10.5772/intechopen.91830*

regarding the motion characteristics of the autonomous systems dealt with, the robotic arm operates on the horizontal plane, and the vertical displacement of the tracked land vehicle is in a negligible level compared to its longitudinal and lateral motion on the ground plane. Thus, the motion profiles are described on a plane for both the systems. Unlike them, the quadrotor flies in the sky towards all three directions. This fact leads to handle its dynamic behavior in a three-dimensional space. Having completed the computer simulations, it is observed that all three systems can be carried to the intended target points by LHG law [8].

As a motion planning strategy, guidance approach can be applied to service robots which are utilized to accomplish certain motion profiles apart from the industrial systems. In this way, it is intended to perform hazardous, tedious, and time-consuming tasks in a more efficient and accurate manner in daily use. The mentioned category for service robots, actually, involves not only articulated robot manipulators but also moving and flying autonomous structures as well.

## **2. Description of the systems**

and control system constitutes the kinematic relationships established as per the relative position between the munition and intended target point, while the control system is the closed-loop control system constructed based on the dynamic model of the munition under consideration in order to realize the guidance commands generated by the guidance part. In this extent, the guidance approach enrolls as a

The type of the command yielded by the guidance system depends on the selected guidance law. Namely, as the output of the proportional navigation guidance law which constitutes the most widely used guidance law in guided munitions is the lateral component of the linear acceleration vector of the munition or change of the lateral angular component of the linear velocity vector in time as per the application, the command of the body pursuit guidance law becomes the components of the angle between the body longitudinal axis of the munition and the

Guidance laws designated to move the guided munitions towards specified target points make their motion planning in a sense. Regarding this property, there seems no serious obstacle on their implementation on autonomous systems other than guided munition. In this study, the orientation of the sample autonomous mechatronic systems involving a robotic arm, tracked land vehicle, and quadrotor to predefined target points using the linear homing guidance (LHG) law and the relevant computer simulations is carried out. Here, these systems are chosen as very common systems encountered in the physical world. In the considered cases, the LHG law is an angle-based approach, and it takes the flight path angle components of the systems into consideration. Also, the selection of the mentioned mechatronic systems allows evaluating the convenience of the proposed approach in the planar and spatial engagement situations. Moreover, single- and two-stage control systems

Guidance-based motion planning schemes are developed for certain robotic arm configurations [7]. In this scene, the indicated strategy allows the operators to run the moving belt of the robotic arm-belt assembly line within a mounting line in a more continuous and faster manner than the usual methods. Moreover, it may become to suppress or at least minimize the drawbacks of the conventional approaches by regarding the guidance-based motion planning method. Namely, although many conventional methods require the belt assembly to halt at intermediate placing instants, the guidance-based approach makes possible to place the objects under consideration onto the belt while it remains running during operation [8].

Different from wheeled vehicles, tracked land vehicles are directed as per a sliding motion which depends on the rotation of the vehicle about an instantaneous rotation center. In other words, they can be oriented to left or right by rotating about their instantaneous rotation center in the convenient sense. Regarding the motion of these vehicles on soil surfaces especially, the motion planning becomes harder. As a remedy to this inconvenience, the guidance-based path planning

As the third application, the motion planning of a quadrotor which is intended to carry a payload from a stationary initial point to a prescribed moving land platform at a moderate distance for either military or civil purpose is investigated. Here, the payload can be munition, food, or first aid material. Since the moving platform specified as the target is assumed to be far away from the initial point, it is desired for the quadrotor to catch it within the shortest time duration possible and consume

In the computer simulations in which the planar motion of the robotic arm and tracked land vehicle and the spatial motion of the quadrotor are taken into consideration, it is assumed that the targets are moving along specified trajectories. Here,

motion planning scheme for the munition [5].

*Service Robotics*

lateral axis of the Earth-fixed frame [5, 6].

are utilized in accordance with the LHG law.

approach is proposed in the present study [8].

the energy at a minimum level [8, 9].

**60**

#### **2.1 Description of the robotic arm**

The schematic views of the robotic arm and mounting line, i.e., trajectory, containing the slot on which the component grasped by the gripper of the arm is placed is given in **Figure 1**.

The system whose schematic view is presented in **Figure 1** consists of a twodegree-of-freedom robotic arm and moving mounting line. Here, the object to be put on the slot on the mounting line by the gripper of the robotic arm is taken in spherical geometry, and thus its orientation is ignored. This way, the degree of freedom of the carried object is reduced to two. In other words, it becomes possible to define the instantaneous planar location of the object by regarding the lateral and vertical position components of a point, i.e., point P, on the object. So, a robotic arm is required with minimum degree of freedom of two in order for the object with two degrees of freedom to be carried upon a specified point on the plane without any control deficiency. The definitions made in **Figure 1** are listed as follows [8]:

x and y: lateral and vertical axes of the Earth-fixed frame symbolized by *F0. u* !ð Þ <sup>0</sup> <sup>1</sup> and *u* !ð Þ <sup>0</sup> <sup>2</sup> : unit vectors denoting the x- and y-axis of *F0.* O and A: joints of the robotic arm.

**Figure 1.** *System consisting of a robotic arm and moving mounting line [8].*

a1 and a2: lengths of the first and second links of the arm.

θ<sup>1</sup> and θ2: first and second joint angles of the robotic arm.

P: point defined on the gripper of the robotic arm.

xP and yP: lateral and vertical position components of point P.

S: midpoint of the slot on the mounting platform.

Si: form changing points of the mounting line (i = 1, 2, 3, and 4).

vS: speed of the slot on the mounting line.

xS and yS: lateral and vertical position components of point S.

ρ: turn radius of the mounting line.

ψ: rotation angle on the circular tip parts of the mounting line.

L: total length of the mounting line.

d: perpendicular distance between the connection point of the robotic arm to the ground and the point of the mounting line closest to that connection point.

*g* !: gravity vector (g = 9.81 m/s<sup>2</sup> ).

Regarding these definitions, the mathematical model of the robotic arm can be expressed in a compact matrix form as follows [8]:

$$
\dot{\overline{T}} = \hat{M}(\overline{\theta}) \ddot{\overline{\theta}} + \hat{H}(\dot{\overline{\theta}}, \overline{\theta}) \dot{\overline{\theta}} \tag{1}
$$

*W* !

**Figure 2.**

*R* !

*F* ! *<sup>L</sup>*and *F* !

*σψ*\_ *m g*

**63**

*b Iz <sup>b</sup>σx*\_ *<sup>v</sup>* � *<sup>a</sup>*

2 *<sup>μ</sup><sup>x</sup> μy*

axis indicated by the unit vector *u*

**2.3 Description of the quadrotor**

presented in **Figures 3** and **4**, respectively [8].

*<sup>L</sup>* and *W* !

*XL* and *R* ! *<sup>R</sup>*: weighting forces on the left and right tracks.

*Notable kinematic parameters of the tracked land vehicle and forces acting on the vehicle [8].*

*<sup>R</sup>*: actuation forces acting on the left and right tracks. ρxL and ρxR: lateral friction force density acting on the left and right tracks. For the tracked land vehicle, as ux, uy, and uz denote the inputs and bx, by, and b<sup>ψ</sup> indicate the gravity and frictional force components, governing differential

where *ux* ¼ *uF* cosð Þ *ψ* , *uy* ¼ *uF* sin ð Þ *ψ* , *uF* ¼ ð Þ *TL* þ *TR =*ð Þ *m rS* , *u<sup>ψ</sup>* ¼

2

the power transmission gears on the left and right tracks; rS and Iz represent the radius of its actuation gear and moment of inertia of the vehicle about the rotation

friction coefficients between the tracks of the vehicle and surface on the lateral planes. Here, the symbols *σx*\_ and *σψ*\_ are introduced as *σx*\_ ¼ sgn ½ � *x*\_ cosð Þþ *ψ y*\_ sin ð Þ *ψ*

engagement geometry between the quadrotor and moving land platform are

Regarding these definitions, TL and TR stand for the actuation torques exerted by

The schematic view of the quadrotor, the third system under consideration, and

*a T*ð Þ *<sup>R</sup>* � *TL =*ð Þ 2 *Iz rS* , *bx* ¼ *σx*\_ *μ<sup>x</sup> g* cosð Þ *ψ* , *by* ¼ *σx*\_ *μ<sup>x</sup> g* sin ð Þ *ψ* , and *b<sup>ψ</sup>* ¼

!ð Þ *<sup>b</sup>*

and *σψ*\_ ¼ sgn ð Þ *ψ*\_ where sgn(�) shows the signum function [8].

<sup>þ</sup> *xC* ð Þ� *<sup>c</sup>* � *<sup>d</sup> <sup>c</sup>*2þ*d*<sup>2</sup> ð Þ

.

equations can be written in the following manner [8]:

*Guidance-Based Motion Planning of Autonomous Systems*

*DOI: http://dx.doi.org/10.5772/intechopen.91830*

*XR*: longitudinal friction components acting on the left and right tracks.

*x*€ ¼ *ux* � *bx* (2) €*y* ¼ *uy* � *by* (3) *ψ*€ ¼ *u<sup>ψ</sup>* þ *b<sup>ψ</sup>* (4)

<sup>3</sup> ; and eventually μ<sup>x</sup> and μ<sup>y</sup> stand for the static

where, as *θ* ¼ ½ � *θ*<sup>1</sup> *θ*<sup>2</sup> *<sup>T</sup>* and *<sup>T</sup>* <sup>¼</sup> ½ � *<sup>T</sup>*<sup>1</sup> *<sup>T</sup>*<sup>2</sup> *<sup>T</sup>*, *M*^ *θ* and *<sup>H</sup>*^ \_ *θ*, *θ* which denote the inertia matrix and compound friction and Coriolis effect matrix, respectively, are defined as *M*^ *θ* <sup>¼</sup> *<sup>m</sup>*<sup>11</sup> *<sup>m</sup>*<sup>12</sup> *<sup>m</sup>*<sup>12</sup> *<sup>m</sup>*<sup>22</sup> and *<sup>H</sup>*^ \_ *θ*, *θ* <sup>¼</sup> *<sup>h</sup>*<sup>11</sup> *<sup>h</sup>*<sup>12</sup> *<sup>h</sup>*<sup>21</sup> *<sup>h</sup>*<sup>22</sup> with *<sup>m</sup>*<sup>11</sup> <sup>¼</sup> *<sup>m</sup>*<sup>1</sup> *<sup>d</sup>*<sup>2</sup> <sup>1</sup> þ *m*<sup>2</sup> *a*<sup>2</sup> <sup>1</sup> <sup>þ</sup> *<sup>d</sup>*<sup>2</sup> <sup>2</sup> þ 2*a*<sup>1</sup> *d*<sup>2</sup> cosð Þ *θ*<sup>2</sup> <sup>þ</sup> *Ic*<sup>1</sup> <sup>þ</sup> *Ic*2, *<sup>m</sup>*<sup>12</sup> <sup>¼</sup> *<sup>m</sup>*<sup>2</sup> *<sup>d</sup>*<sup>2</sup> <sup>½</sup>*d*<sup>2</sup> <sup>þ</sup> *<sup>a</sup>*<sup>1</sup> cosð Þ *<sup>θ</sup>*<sup>2</sup> � þ *Ic*2, *<sup>m</sup>*<sup>22</sup> <sup>¼</sup> *m*<sup>2</sup> *d*<sup>2</sup> <sup>2</sup> <sup>þ</sup> *Ic*2, *<sup>h</sup>*<sup>11</sup> <sup>¼</sup> *<sup>b</sup>*<sup>1</sup> � <sup>2</sup>*m*<sup>2</sup> *<sup>a</sup>*<sup>1</sup> *<sup>d</sup>*<sup>2</sup> \_ *<sup>θ</sup>*<sup>2</sup> sin ð Þ *<sup>θ</sup>*<sup>2</sup> , *<sup>h</sup>*<sup>12</sup> <sup>¼</sup> *<sup>b</sup>*<sup>2</sup> � *<sup>m</sup>*<sup>2</sup> *<sup>a</sup>*<sup>1</sup> *<sup>d</sup>*<sup>2</sup> \_ *θ*<sup>2</sup> sin ð Þ *θ*<sup>2</sup> , *h*<sup>21</sup> ¼ *m*<sup>2</sup> *a*<sup>1</sup> *d*<sup>2</sup> \_ *θ*<sup>1</sup> sin ð Þ *θ*<sup>2</sup> , and *h*<sup>22</sup> ¼ *b*<sup>2</sup> [8].

In the shorthand definitions above, m1, m2, Ic1, and Ic2 denote the masses of the first and second links of the manipulator and the moments of inertia of these links with respect to their mass centers indicated by C1 and C2, respectively. Also, b1 and b2 represent the viscous friction coefficients at the first and second joints as well as the definitions of *d*<sup>1</sup> ¼ j j *OC*<sup>1</sup> and *d*<sup>2</sup> ¼ j j *AC*<sup>2</sup> [8].

#### **2.2 Description of the tracked land vehicle**

As the second system examined, the schematic description of the tracked land vehicle is shown in **Figure 2**. The explanations of the quantities labeled in **Figure 2** are given as follows [8]:

O, G, and C: origin of *F0*, mass canter of the tracked vehicle, and instantaneous rotation center of the vehicle.

*u* !ð Þ <sup>0</sup> *<sup>i</sup>* : unit vectors of *F0.*

*u* !ð Þ *<sup>b</sup> <sup>i</sup>* : unit vectors of the tracked land vehicle frame, i.e., *Fb.*

x and y: position components of point G on *F0.*

ψ: orientation angle of the vehicle on the vertical plane.

*r* ! *<sup>G</sup>=<sup>O</sup>*: relative position of point G with respect to point O.

*r* ! *<sup>G</sup>=<sup>C</sup>*: relative position of point G with respect to point C.

xC and yC: position components point C in *Fb.*

a, b, c, d, and v: dimensional parameters of the vehicle. m: mass of the tracked land vehicle.

*W* ! : weight vector of the tracked land vehicle. *Guidance-Based Motion Planning of Autonomous Systems DOI: http://dx.doi.org/10.5772/intechopen.91830*

a1 and a2: lengths of the first and second links of the arm. θ<sup>1</sup> and θ2: first and second joint angles of the robotic arm. P: point defined on the gripper of the robotic arm.

S: midpoint of the slot on the mounting platform.

vS: speed of the slot on the mounting line.

expressed in a compact matrix form as follows [8]:

ρ: turn radius of the mounting line.

L: total length of the mounting line.

 <sup>¼</sup> *<sup>m</sup>*<sup>11</sup> *<sup>m</sup>*<sup>12</sup> *m*<sup>12</sup> *m*<sup>22</sup> 

*θ*<sup>1</sup> sin ð Þ *θ*<sup>2</sup> , and *h*<sup>22</sup> ¼ *b*<sup>2</sup> [8].

the definitions of *d*<sup>1</sup> ¼ j j *OC*<sup>1</sup> and *d*<sup>2</sup> ¼ j j *AC*<sup>2</sup> [8].

**2.2 Description of the tracked land vehicle**

<sup>2</sup> þ 2*a*<sup>1</sup> *d*<sup>2</sup> cosð Þ *θ*<sup>2</sup>

<sup>2</sup> <sup>þ</sup> *Ic*2, *<sup>h</sup>*<sup>11</sup> <sup>¼</sup> *<sup>b</sup>*<sup>1</sup> � <sup>2</sup>*m*<sup>2</sup> *<sup>a</sup>*<sup>1</sup> *<sup>d</sup>*<sup>2</sup> \_

!: gravity vector (g = 9.81 m/s<sup>2</sup>

where, as *θ* ¼ ½ � *θ*<sup>1</sup> *θ*<sup>2</sup>

defined as *M*^ *θ*

<sup>1</sup> <sup>þ</sup> *<sup>d</sup>*<sup>2</sup>

are given as follows [8]:

rotation center of the vehicle.

*<sup>i</sup>* : unit vectors of *F0.*

*m*<sup>2</sup> *a*<sup>2</sup>

*m*<sup>2</sup> *d*<sup>2</sup>

*m*<sup>2</sup> *a*<sup>1</sup> *d*<sup>2</sup> \_

*u* !ð Þ <sup>0</sup>

*u* !ð Þ *<sup>b</sup>*

*r* !

*r* !

*W* !

**62**

*g*

*Service Robotics*

xP and yP: lateral and vertical position components of point P.

xS and yS: lateral and vertical position components of point S.

ψ: rotation angle on the circular tip parts of the mounting line.

ground and the point of the mounting line closest to that connection point.

€

inertia matrix and compound friction and Coriolis effect matrix, respectively, are

*θ*, *θ* 

<sup>þ</sup> *Ic*<sup>1</sup> <sup>þ</sup> *Ic*2, *<sup>m</sup>*<sup>12</sup> <sup>¼</sup> *<sup>m</sup>*<sup>2</sup> *<sup>d</sup>*<sup>2</sup> <sup>½</sup>*d*<sup>2</sup> <sup>þ</sup> *<sup>a</sup>*<sup>1</sup> cosð Þ *<sup>θ</sup>*<sup>2</sup> � þ *Ic*2, *<sup>m</sup>*<sup>22</sup> <sup>¼</sup>

In the shorthand definitions above, m1, m2, Ic1, and Ic2 denote the masses of the first and second links of the manipulator and the moments of inertia of these links with respect to their mass centers indicated by C1 and C2, respectively. Also, b1 and b2 represent the viscous friction coefficients at the first and second joints as well as

As the second system examined, the schematic description of the tracked land vehicle is shown in **Figure 2**. The explanations of the quantities labeled in **Figure 2**

O, G, and C: origin of *F0*, mass canter of the tracked vehicle, and instantaneous

*<sup>i</sup>* : unit vectors of the tracked land vehicle frame, i.e., *Fb.*

*<sup>G</sup>=<sup>O</sup>*: relative position of point G with respect to point O.

*<sup>G</sup>=<sup>C</sup>*: relative position of point G with respect to point C.

ψ: orientation angle of the vehicle on the vertical plane.

a, b, c, d, and v: dimensional parameters of the vehicle.

x and y: position components of point G on *F0.*

xC and yC: position components point C in *Fb.*

: weight vector of the tracked land vehicle.

m: mass of the tracked land vehicle.

).

*<sup>T</sup>* <sup>¼</sup> *<sup>M</sup>*^ *<sup>θ</sup>*

*<sup>T</sup>* and *<sup>T</sup>* <sup>¼</sup> ½ � *<sup>T</sup>*<sup>1</sup> *<sup>T</sup>*<sup>2</sup>

and *<sup>H</sup>*^ \_

d: perpendicular distance between the connection point of the robotic arm to the

Regarding these definitions, the mathematical model of the robotic arm can be

*<sup>θ</sup>* <sup>þ</sup> *<sup>H</sup>*^ \_

*θ*, *θ* \_

<sup>¼</sup> *<sup>h</sup>*<sup>11</sup> *<sup>h</sup>*<sup>12</sup> *h*<sup>21</sup> *h*<sup>22</sup> 

and *<sup>H</sup>*^ \_

*<sup>T</sup>*, *M*^ *θ*

*<sup>θ</sup>*<sup>2</sup> sin ð Þ *<sup>θ</sup>*<sup>2</sup> , *<sup>h</sup>*<sup>12</sup> <sup>¼</sup> *<sup>b</sup>*<sup>2</sup> � *<sup>m</sup>*<sup>2</sup> *<sup>a</sup>*<sup>1</sup> *<sup>d</sup>*<sup>2</sup> \_

*θ* (1)

with *<sup>m</sup>*<sup>11</sup> <sup>¼</sup> *<sup>m</sup>*<sup>1</sup> *<sup>d</sup>*<sup>2</sup>

*θ*<sup>2</sup> sin ð Þ *θ*<sup>2</sup> , *h*<sup>21</sup> ¼

which denote the

<sup>1</sup> þ

*θ*, *θ* 

Si: form changing points of the mounting line (i = 1, 2, 3, and 4).

**Figure 2.** *Notable kinematic parameters of the tracked land vehicle and forces acting on the vehicle [8].*

*W* ! *<sup>L</sup>* and *W* ! *<sup>R</sup>*: weighting forces on the left and right tracks.

*R* ! *XL* and *R* ! *XR*: longitudinal friction components acting on the left and right tracks.

*F* ! *<sup>L</sup>*and *F* ! *<sup>R</sup>*: actuation forces acting on the left and right tracks.

ρxL and ρxR: lateral friction force density acting on the left and right tracks.

For the tracked land vehicle, as ux, uy, and uz denote the inputs and bx, by, and b<sup>ψ</sup> indicate the gravity and frictional force components, governing differential equations can be written in the following manner [8]:

$$
\ddot{\boldsymbol{x}} = \boldsymbol{u}\_{\boldsymbol{x}} - \boldsymbol{b}\_{\boldsymbol{x}} \tag{2}
$$

$$
\ddot{\mathbf{y}} = \mathbf{u}\_{\mathbf{y}} - \mathbf{b}\_{\mathbf{y}} \tag{3}
$$

$$
\ddot{\boldsymbol{\mu}} = \boldsymbol{u}\_{\boldsymbol{\Psi}} + \boldsymbol{b}\_{\boldsymbol{\Psi}} \tag{4}
$$

where *ux* ¼ *uF* cosð Þ *ψ* , *uy* ¼ *uF* sin ð Þ *ψ* , *uF* ¼ ð Þ *TL* þ *TR =*ð Þ *m rS* , *u<sup>ψ</sup>* ¼

 $\mathfrak{a}\left(T\_R - T\_L\right)/(2I\_x r\_S)$ ,  $b\_\mathbf{x} = \sigma\_{\hat{\mathbf{x}}} \mu\_\mathbf{x} \mathbf{g}$   $\cos\left(\boldsymbol{\mu}\right)$ ,  $b\_\mathbf{y} = \sigma\_{\hat{\mathbf{x}}} \mu\_\mathbf{x} \mathbf{g}$   $\sin\left(\boldsymbol{\mu}\right)$ ,  $\mathbf{a}$  and  $b\_\mathbf{y} = \frac{\sigma\_{\hat{\mathbf{y}}} \cdot \mathbf{m} \mathbf{g}}{\mathbf{b} \, l\_\mathbf{z}} \left[\mathrm{b}\,\sigma\_{\hat{\mathbf{x}}} \left(\boldsymbol{\nu} - \frac{\boldsymbol{a}}{2}\right) \left(\frac{\mu\_\mathbf{z}}{\mu\_\mathbf{r}}\right) + \mathrm{x}\_\mathbf{C} \left(\boldsymbol{c} - \boldsymbol{d}\right) - \frac{\left(\boldsymbol{c}^2 + \boldsymbol{d}^2\right)}{2}\right]$ .

Regarding these definitions, TL and TR stand for the actuation torques exerted by the power transmission gears on the left and right tracks; rS and Iz represent the radius of its actuation gear and moment of inertia of the vehicle about the rotation axis indicated by the unit vector *u* !ð Þ *<sup>b</sup>* <sup>3</sup> ; and eventually μ<sup>x</sup> and μ<sup>y</sup> stand for the static friction coefficients between the tracks of the vehicle and surface on the lateral planes. Here, the symbols *σx*\_ and *σψ*\_ are introduced as *σx*\_ ¼ sgn ½ � *x*\_ cosð Þþ *ψ y*\_ sin ð Þ *ψ* and *σψ*\_ ¼ sgn ð Þ *ψ*\_ where sgn(�) shows the signum function [8].

2

#### **2.3 Description of the quadrotor**

2

*μy*

The schematic view of the quadrotor, the third system under consideration, and engagement geometry between the quadrotor and moving land platform are presented in **Figures 3** and **4**, respectively [8].

Considering the related kinematic and dynamic parameters of the quadrotor system, the dynamic model of the quadrotor can be set as follows using the relevant kinematic and dynamic parameters with angular position parameters of ϕ, θ, and ψ

*<sup>θ</sup>* <sup>þ</sup> *<sup>J</sup><sup>θ</sup>* <sup>Ω</sup>*<sup>e</sup>* \_

*θ* þ *u<sup>ϕ</sup>* (5)

*θϕ*\_ <sup>þ</sup> *<sup>u</sup><sup>ψ</sup>* (7)

<sup>1</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup>

<sup>2</sup> � <sup>Ω</sup><sup>2</sup>

*=Jz*,

!ð Þ *<sup>b</sup>*

*θ*<sup>1</sup> and \_

*θ*<sup>2</sup> [6]. Since

<sup>3</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> 4

<sup>3</sup> , respec-

*<sup>θ</sup>* <sup>þ</sup> *<sup>J</sup>ψϕ <sup>ψ</sup>*\_ *<sup>ϕ</sup>*\_ � *<sup>J</sup><sup>ϕ</sup>* <sup>Ω</sup>*<sup>e</sup> <sup>ϕ</sup>*\_ <sup>þ</sup> *<sup>u</sup><sup>θ</sup>* (6)

*x*€ ¼ �*cx x*\_ þ *ux* (8) €*y* ¼ �*cy y*\_ þ *uy* (9) €*z* ¼ �*cz z*\_ � *g* þ *uz* (10)

*<sup>=</sup>Jx*, *<sup>J</sup><sup>θ</sup>* <sup>¼</sup> *Jv=Jx*, <sup>Ω</sup>*<sup>e</sup>* <sup>¼</sup> <sup>Ω</sup><sup>1</sup> � <sup>Ω</sup><sup>2</sup> <sup>þ</sup> <sup>Ω</sup><sup>3</sup> � <sup>Ω</sup>4,

!ð Þ *<sup>b</sup>* <sup>1</sup> , *u* !ð Þ *<sup>b</sup>* <sup>2</sup> , and *u*

*<sup>=</sup>Jz*, *<sup>u</sup><sup>ψ</sup>* <sup>¼</sup> *<sup>d</sup>* �Ω<sup>2</sup>

½ � *<sup>c</sup>*ð Þ *<sup>ψ</sup> <sup>s</sup>*ð Þ*<sup>θ</sup> <sup>c</sup>*ð Þþ *<sup>ϕ</sup> <sup>s</sup>*ð Þ *<sup>ψ</sup> <sup>s</sup>*ð Þ *<sup>ϕ</sup> <sup>=</sup>m*, *cy* <sup>¼</sup> *Ky=m*,

and translational position parameters of x, y, and z [8]:

*Guidance-Based Motion Planning of Autonomous Systems*

*DOI: http://dx.doi.org/10.5772/intechopen.91830*

where *c<sup>ϕ</sup>* ¼ *LKϕ=Jx*, *Jψθ* ¼ *Jy* � *Jz*

<sup>2</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup>

<sup>3</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> 4 *<sup>c</sup>*ð Þ*<sup>θ</sup> <sup>c</sup>*ð Þ *<sup>ϕ</sup> <sup>=</sup>m*.

*<sup>=</sup>Jy*, *<sup>c</sup><sup>ψ</sup>* <sup>¼</sup> *<sup>K</sup><sup>ψ</sup> <sup>=</sup>Jz*, *<sup>J</sup>θ ϕ* <sup>¼</sup> *Jx* � *Jy*

<sup>3</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> 4

<sup>1</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup>

<sup>2</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup>

the quadrotor around the axes defined by unit vectors *u*

Furthermore, b indicates thrust factor of the motors [8].

target acquired by certain means like a camera.

**3.1 Robotic arm control system**

<sup>3</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> 4

<sup>2</sup> � <sup>Ω</sup><sup>2</sup> 4

*<sup>u</sup><sup>ϕ</sup>* <sup>¼</sup> *L b* <sup>Ω</sup><sup>2</sup>

*uy* <sup>¼</sup> *<sup>b</sup>* <sup>Ω</sup><sup>2</sup>

<sup>1</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup>

<sup>1</sup> � <sup>Ω</sup><sup>2</sup> 3

*cx* <sup>¼</sup> *Kx=m*, *ux* <sup>¼</sup> *<sup>b</sup>* <sup>Ω</sup><sup>2</sup>

**3. Control systems**

<sup>1</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup>

<sup>2</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup>

*L b* Ω<sup>2</sup>

*b* Ω<sup>2</sup>

**65**

€*<sup>θ</sup>* ¼ �*c<sup>θ</sup>* \_

*<sup>ϕ</sup>*€ ¼ �*c<sup>ϕ</sup> <sup>ϕ</sup>*\_ <sup>þ</sup> *<sup>J</sup>ψθ <sup>ψ</sup>*\_ \_

*<sup>ψ</sup>*€ ¼ �*c<sup>ψ</sup> <sup>ψ</sup>*\_ <sup>þ</sup> *<sup>J</sup>θϕ* \_

*<sup>=</sup>Jx*, *<sup>c</sup><sup>θ</sup>* <sup>¼</sup> *LKθ=Jy*, *<sup>J</sup>ψ ϕ* <sup>¼</sup> *Jz* � *Jx* ð Þ*=Jy*, *<sup>J</sup><sup>ϕ</sup>* <sup>¼</sup> *Jv=Jy*, *<sup>u</sup><sup>θ</sup>* <sup>¼</sup>

½ � *<sup>s</sup>*ð Þ *<sup>ψ</sup> <sup>s</sup>*ð Þ*<sup>θ</sup> <sup>c</sup>*ð Þ� *<sup>ϕ</sup> <sup>c</sup>*ð Þ *<sup>ψ</sup> <sup>s</sup>*ð Þ *<sup>ϕ</sup> <sup>=</sup>m*, *cz* <sup>¼</sup> *Kz=m*, and *uz* <sup>¼</sup>

In the definitions above, Jx, Jy, and Jz show the moment of inertia components of

Since the guidance commands generated by the LHG law which is considered to make the motion planning of the autonomous systems so as to bring them to the specified point on the target trajectories are in terms of the linear velocity components of the mass centers of those systems, the main control variables of the systems are selected to be velocity components [8]. Also, for the sake of maintaining the stability of the systems, the gain matrices of the relevant control systems are continuously updated throughout the planned motion using the state information of

Because the guidance signals produced by the LHG law are in terms of the angle between the velocity vector of point P and lateral axis, the control variable of the

the main objective is the speed control of point P, the control system based on the joint speeds corresponds to an indirect control scheme. Regarding the difficulty in the measurement of the instantaneous speed values of point P along with the fact that joint angles and their rates can easily be acquired by means of the sensors put

robotic arm control system is chosen to be the joint speeds, i.e., \_

tively. Jv represents the moment of inertia of each rotor about its axis of rotation, d indicates the drag factor, and Kϕ, Kθ, Kψ, Kx, Ky, and Kz stand for the aerodynamic moment and force components acting on the system in the roll, pitch, and yaw planes and along the longitudinal, lateral, and vertical planes, respectively.

**Figure 3.** *Schematic view of the quadrotor [8].*

**Figure 4.** *Engagement geometry for the quadrotor [8].*

As indicated in **Figure 3**, the front and rear rotors expressed as numbers 1 and 3, respectively, have rotations in positive sense around the axis represented by unit vector *u* !ð Þ *<sup>b</sup>* <sup>3</sup> of the body-fixed frame of the quadrotor, i.e., *Fb*, whose origin is attached at point C and whose axes are shown by unit vectors *u* !ð Þ *<sup>b</sup> <sup>i</sup>* (i = 1, 2, and 3), while the left and right rotors, i.e., numbers 2 and 4 rotors, rotate in negative sense [8].

In **Figure 3**, L and Ω<sup>j</sup> (j = 1, 2, 3, and 4) stand for the distance between the center of rotation of each motor and point C and angular speed of the electromechanical actuator, i.e., electrical servomotor, used to move the propeller j, respectively. In addition to those parameters, the symbols in **Figure 3** can be listed as follows [8]:


*Guidance-Based Motion Planning of Autonomous Systems DOI: http://dx.doi.org/10.5772/intechopen.91830*

Considering the related kinematic and dynamic parameters of the quadrotor system, the dynamic model of the quadrotor can be set as follows using the relevant kinematic and dynamic parameters with angular position parameters of ϕ, θ, and ψ and translational position parameters of x, y, and z [8]:

$$\ddot{\Phi} = -\varepsilon\_{\phi}\dot{\Phi} + J\_{\varphi\theta}\dot{\varphi}\,\dot{\theta} + J\_{\theta}\,\Omega\_{\varepsilon}\,\dot{\theta} + u\_{\phi} \tag{5}$$

$$\ddot{\theta} = -\mathfrak{c}\_{\theta}\dot{\theta} + J\_{\mathfrak{y}\phi}\dot{\mathfrak{y}}\,\dot{\phi} - J\_{\phi}\mathfrak{Q}\_{\mathfrak{e}}\dot{\phi} + \mathfrak{u}\_{\theta} \tag{6}$$

$$
\ddot{\boldsymbol{\Psi}} = -\boldsymbol{\varepsilon}\_{\boldsymbol{\Psi}} \dot{\boldsymbol{\Psi}} + \boldsymbol{J}\_{\partial \dot{\boldsymbol{\Phi}}} \dot{\boldsymbol{\theta}} \dot{\boldsymbol{\Phi}} + \boldsymbol{u}\_{\boldsymbol{\Psi}} \tag{7}
$$

$$
\ddot{\mathbf{x}} = -\mathbf{c}\_{\mathbf{x}}\dot{\mathbf{x}} + \mathbf{u}\_{\mathbf{x}} \tag{8}
$$

$$
\ddot{\mathbf{y}} = -\mathbf{c}\_{\mathbf{y}}\dot{\mathbf{y}} + \mathbf{u}\_{\mathbf{y}} \tag{9}
$$

$$
\ddot{\mathbf{z}} = -\mathbf{c}\_x \dot{\mathbf{z}} - \mathbf{g} + \mathbf{u}\_x \tag{10}
$$

where *c<sup>ϕ</sup>* ¼ *LKϕ=Jx*, *Jψθ* ¼ *Jy* � *Jz <sup>=</sup>Jx*, *<sup>J</sup><sup>θ</sup>* <sup>¼</sup> *Jv=Jx*, <sup>Ω</sup>*<sup>e</sup>* <sup>¼</sup> <sup>Ω</sup><sup>1</sup> � <sup>Ω</sup><sup>2</sup> <sup>þ</sup> <sup>Ω</sup><sup>3</sup> � <sup>Ω</sup>4, *<sup>u</sup><sup>ϕ</sup>* <sup>¼</sup> *L b* <sup>Ω</sup><sup>2</sup> <sup>2</sup> � <sup>Ω</sup><sup>2</sup> 4 *<sup>=</sup>Jx*, *<sup>c</sup><sup>θ</sup>* <sup>¼</sup> *LKθ=Jy*, *<sup>J</sup>ψ ϕ* <sup>¼</sup> *Jz* � *Jx* ð Þ*=Jy*, *<sup>J</sup><sup>ϕ</sup>* <sup>¼</sup> *Jv=Jy*, *<sup>u</sup><sup>θ</sup>* <sup>¼</sup> *L b* Ω<sup>2</sup> <sup>1</sup> � <sup>Ω</sup><sup>2</sup> 3 *<sup>=</sup>Jy*, *<sup>c</sup><sup>ψ</sup>* <sup>¼</sup> *<sup>K</sup><sup>ψ</sup> <sup>=</sup>Jz*, *<sup>J</sup>θ ϕ* <sup>¼</sup> *Jx* � *Jy <sup>=</sup>Jz*, *<sup>u</sup><sup>ψ</sup>* <sup>¼</sup> *<sup>d</sup>* �Ω<sup>2</sup> <sup>1</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> <sup>2</sup> � <sup>Ω</sup><sup>2</sup> <sup>3</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> 4 *=Jz*, *cx* <sup>¼</sup> *Kx=m*, *ux* <sup>¼</sup> *<sup>b</sup>* <sup>Ω</sup><sup>2</sup> <sup>1</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> <sup>2</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> <sup>3</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> 4 ½ � *<sup>c</sup>*ð Þ *<sup>ψ</sup> <sup>s</sup>*ð Þ*<sup>θ</sup> <sup>c</sup>*ð Þþ *<sup>ϕ</sup> <sup>s</sup>*ð Þ *<sup>ψ</sup> <sup>s</sup>*ð Þ *<sup>ϕ</sup> <sup>=</sup>m*, *cy* <sup>¼</sup> *Ky=m*, *uy* <sup>¼</sup> *<sup>b</sup>* <sup>Ω</sup><sup>2</sup> <sup>1</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> <sup>2</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> <sup>3</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> 4 ½ � *<sup>s</sup>*ð Þ *<sup>ψ</sup> <sup>s</sup>*ð Þ*<sup>θ</sup> <sup>c</sup>*ð Þ� *<sup>ϕ</sup> <sup>c</sup>*ð Þ *<sup>ψ</sup> <sup>s</sup>*ð Þ *<sup>ϕ</sup> <sup>=</sup>m*, *cz* <sup>¼</sup> *Kz=m*, and *uz* <sup>¼</sup> *b* Ω<sup>2</sup> <sup>1</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> <sup>2</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> <sup>3</sup> <sup>þ</sup> <sup>Ω</sup><sup>2</sup> 4 *<sup>c</sup>*ð Þ*<sup>θ</sup> <sup>c</sup>*ð Þ *<sup>ϕ</sup> <sup>=</sup>m*.

In the definitions above, Jx, Jy, and Jz show the moment of inertia components of the quadrotor around the axes defined by unit vectors *u* !ð Þ *<sup>b</sup>* <sup>1</sup> , *u* !ð Þ *<sup>b</sup>* <sup>2</sup> , and *u* !ð Þ *<sup>b</sup>* <sup>3</sup> , respectively. Jv represents the moment of inertia of each rotor about its axis of rotation, d indicates the drag factor, and Kϕ, Kθ, Kψ, Kx, Ky, and Kz stand for the aerodynamic moment and force components acting on the system in the roll, pitch, and yaw planes and along the longitudinal, lateral, and vertical planes, respectively. Furthermore, b indicates thrust factor of the motors [8].

#### **3. Control systems**

As indicated in **Figure 3**, the front and rear rotors expressed as numbers 1 and 3, respectively, have rotations in positive sense around the axis represented by unit

In **Figure 3**, L and Ω<sup>j</sup> (j = 1, 2, 3, and 4) stand for the distance between the center of rotation of each motor and point C and angular speed of the electromechanical actuator, i.e., electrical servomotor, used to move the propeller j, respectively. In addition to those parameters, the symbols in **Figure 3** can be listed as follows [8]:

Ti: points at which the shape of the trajectory of the moving platform changes

ψ: rotation angle of the rounded tip portions of the trajectory of the moving

D: perpendicular distance between the origin of *F0*, i.e., point O, and the midline of the section of the trajectory of the moving platform closest to this point.

left and right rotors, i.e., numbers 2 and 4 rotors, rotate in negative sense [8].

at point C and whose axes are shown by unit vectors *u*

T: target point on the moving platform for the quadrotor.

ρ: radius of curvature of the trajectory of the moving platform.

vT: linear speed of point T on the moving platform.

H: total length of the trajectory of the moving platform.

<sup>3</sup> of the body-fixed frame of the quadrotor, i.e., *Fb*, whose origin is attached

!ð Þ *<sup>b</sup>*

*<sup>i</sup>* (i = 1, 2, and 3), while the

vector *u* !ð Þ *<sup>b</sup>*

**Figure 4.**

**Figure 3.**

*Service Robotics*

*Schematic view of the quadrotor [8].*

(i = 0, 1, and 2).

*Engagement geometry for the quadrotor [8].*

platform.

**64**

Since the guidance commands generated by the LHG law which is considered to make the motion planning of the autonomous systems so as to bring them to the specified point on the target trajectories are in terms of the linear velocity components of the mass centers of those systems, the main control variables of the systems are selected to be velocity components [8]. Also, for the sake of maintaining the stability of the systems, the gain matrices of the relevant control systems are continuously updated throughout the planned motion using the state information of target acquired by certain means like a camera.

#### **3.1 Robotic arm control system**

Because the guidance signals produced by the LHG law are in terms of the angle between the velocity vector of point P and lateral axis, the control variable of the robotic arm control system is chosen to be the joint speeds, i.e., \_ *θ*<sup>1</sup> and \_ *θ*<sup>2</sup> [6]. Since the main objective is the speed control of point P, the control system based on the joint speeds corresponds to an indirect control scheme. Regarding the difficulty in the measurement of the instantaneous speed values of point P along with the fact that joint angles and their rates can easily be acquired by means of the sensors put

on the joints, the use of such an indirect control algorithm seems to be proper. In this sense, the linear position and velocity components of point P can be calculated using the measured joint positions and speeds via the loop closure equations, and consequently the trajectory of point P on the horizontal plane can be determined as a function of time [8].

Here, as \_ *θ*1*<sup>d</sup>* and \_ *<sup>θ</sup>*2*<sup>d</sup>* indicate the reference joint speeds and \_ *<sup>θ</sup><sup>d</sup>* <sup>¼</sup> \_ *θ*1*<sup>d</sup>* \_ *θ*2*<sup>d</sup>* � �*<sup>T</sup>* stands for the column matrix corresponds to these quantities, the error between the desired, i.e., reference, and actual joint speeds, i.e., *e*, can be set as follows [8]:

$$
\overline{\boldsymbol{e}} = \dot{\overline{\theta}}\_d - \dot{\overline{\theta}} \tag{11}
$$

position control such that the orientation angle requirement arising during the

Here, the expressions which will be considered for the design of the linear velocity control system which is termed as "the primary control system" can be arranged in the forthcoming state space form from Eqs. (2) and (3) as *xp* <sup>¼</sup> ½ � *<sup>x</sup>*\_ *<sup>y</sup>*\_ *<sup>T</sup>*, *up*, and *bp* stand for the column matrices for the state variables, system inputs, and

The control rule of the primary control system can be established using the computed torque control method according to the PI control action like the robotic

*up* <sup>¼</sup> *<sup>x</sup>*\_ *pd* <sup>þ</sup> *bp* <sup>þ</sup> *<sup>K</sup>*^ *pp ep* <sup>þ</sup> *<sup>K</sup>*^ *pi* <sup>ð</sup>

Substituting Eq. (18) into Eq. (17) yields the next equation for the error

€*ep* <sup>þ</sup> *<sup>K</sup>*^ *pp* \_

second-order ideal system with two degrees of freedom as follows [8]:

€*ep* <sup>þ</sup> *<sup>D</sup>*^ *<sup>p</sup>* \_

portional plus derivative) control action in the following fashion [8]:

0 2*ζ<sup>p</sup>*<sup>2</sup> *ω<sup>p</sup>*<sup>2</sup> " #

In the above equation, *xpd*, *K*^ *pp*, *K*^ *pi*, and *ep* denote the column matrix for the desired inputs, gain matrix for the proportional control action, gain matrix for the integral control action, and column matrix for the error, respectively, for the

As ωpi and ζpi denote the desired bandwidth and damping ratio of link i (i = 1 and 2) for the primary control system, the error dynamics can be written for a

and *<sup>W</sup>*^ *<sup>p</sup>* <sup>¼</sup> *<sup>ω</sup>*<sup>2</sup>

Equating Eq. (19) to Eq. (20), the gain matrices are found for the primary

The control rule for the angular control system called "the secondary control system" can be set using the computed torque method according to the PD (pro-

In the above expression, bψ, ψd, Ksp, Ksd, and es represent the inertia gain for the vehicle, desired input variable for the secondary control system, gain of the proportional control action, gain of the derivative control action, and error,

arm control system so as to nullify the steady-state errors [8]:

primary control system. Here, *ep* ¼ *xpd* � *xp* is introduced.

dynamics of the primary control system [8]:

where *<sup>D</sup>*^ *<sup>p</sup>* <sup>¼</sup> <sup>2</sup> *<sup>ζ</sup><sup>p</sup>*<sup>1</sup> *<sup>ω</sup><sup>p</sup>*<sup>1</sup> <sup>0</sup>

respectively. Also, *es* ¼ *ψ<sup>d</sup>* � *ψ* is introduced.

**67**

control system as follows [8]:

*<sup>x</sup>*\_ *<sup>p</sup>* ¼ �*bp* <sup>þ</sup> *up* (17)

*ep* <sup>þ</sup> *<sup>K</sup>*^ *piep* <sup>¼</sup> <sup>0</sup> (19)

*ep* <sup>þ</sup> *<sup>W</sup>*^ *<sup>p</sup> ep* <sup>¼</sup> <sup>0</sup> (20)

*<sup>K</sup>*^ *pp* <sup>¼</sup> *<sup>D</sup>*^ *<sup>p</sup>* (21)

*<sup>K</sup>*^ *pi* <sup>¼</sup> *<sup>W</sup>*^ *<sup>p</sup>* (22)

*u<sup>ψ</sup>* ¼ *ψ*€*<sup>d</sup>* � *b<sup>ψ</sup>* þ *Ksp es* þ *Ksd e*\_*<sup>s</sup>* (23)

.

*<sup>p</sup>*<sup>1</sup> 0 0 *ω*<sup>2</sup> *p*2

" #

*ep dt* (18)

linear velocity control is realized [8].

*DOI: http://dx.doi.org/10.5772/intechopen.91830*

*Guidance-Based Motion Planning of Autonomous Systems*

gravity effect, respectively [8]:

The relevant control rule can be written according to the computed torque method with the addition of the integral action to nullify the steady-state errors by regarding the error definition in Eq. (11) as follows [10]:

$$
\hat{T} = \hat{M}\ddot{\overline{\theta}}\_d + \hat{H}\dot{\overline{\theta}} + \hat{K}\_p \overline{\varepsilon} + \hat{K}\_i \int \overline{\varepsilon}dt\tag{12}
$$

Here, as "T" represents the matrix transpose, *T* ¼ ½ � *T*<sup>1</sup> *T*<sup>2</sup> *<sup>T</sup>* demonstrates the torque column matrix for joint torques T1 and T2. Also, *<sup>M</sup>*^ <sup>¼</sup> *<sup>M</sup>*^ *<sup>θ</sup>* � � and *<sup>H</sup>*^ <sup>¼</sup> *<sup>H</sup>*^ \_ *θ*, *θ* � �denote the inertia and centrifugal effect matrices, while *<sup>K</sup>*^ *<sup>p</sup>* and *<sup>K</sup>*^*<sup>i</sup>* correspond to the proportional and integral gain matrices, respectively. As implied, the resulting control system becomes in PI (proportional plus integral) form [8].

The differential equation corresponding to the error dynamics of the control system for the robotic arm are obtained using Eqs. (11) and (12) in the following fashion:

$$
\ddot{\overline{e}} + \hat{\boldsymbol{M}}^{-1} \left( \dot{\hat{M}} + \hat{\boldsymbol{K}}\_p \right) \dot{\overline{e}} + \hat{\boldsymbol{M}}^{-1} \hat{\boldsymbol{K}}\_i \overline{e} = \overline{\mathbf{0}} \tag{13}
$$

As ωci and ζci denote the desired bandwidth and damping ratio of link i (i = 1 and 2), the error dynamics can be written for a second-order ideal system with two degrees of freedom as follows [10]:

$$
\ddot{\overline{e}} + \hat{D}\dot{\overline{e}} + \hat{W}\overline{e} = \overline{0} \tag{14}
$$

$$\text{where } \hat{D} = \begin{bmatrix} 2\zeta\_{c1}o\_{c1} & 0\\ 0 & 2\zeta\_{c2}o\_{c2} \end{bmatrix} \text{ and } \hat{W} = \begin{bmatrix} o\_{c1}^2 & 0\\ 0 & o\_{c2}^2 \end{bmatrix}.$$

Consequently, *K*^ *<sup>p</sup>* and *K*^*<sup>i</sup>* are determined by equating Eq. (13) to Eq. (14) as follows [8]:

$$
\hat{K}\_p = \hat{M}\hat{D} - \dot{\hat{M}}\tag{15}
$$

$$
\hat{K}\_i = \hat{M}\hat{W} \tag{16}
$$

#### **3.2 Tracked land vehicle control system**

During the control of the tracked land vehicle for the angular variables, the angular position requirement for ψ should also be satisfied. For this purpose, a twostage cascaded control scheme is constructed for the tracked land vehicle. In this algorithm, the outer loop is responsible of making linear velocity control in accordance with the guidance commands, while the inner loop makes the angular

on the joints, the use of such an indirect control algorithm seems to be proper. In this sense, the linear position and velocity components of point P can be calculated using the measured joint positions and speeds via the loop closure equations, and consequently the trajectory of point P on the horizontal plane can be determined as

*<sup>θ</sup>*2*<sup>d</sup>* indicate the reference joint speeds and \_

stands for the column matrix corresponds to these quantities, the error between the desired, i.e., reference, and actual joint speeds, i.e., *e*, can be set as follows [8]:

> *<sup>e</sup>* <sup>¼</sup> \_ *<sup>θ</sup><sup>d</sup>* � \_

*<sup>θ</sup><sup>d</sup>* <sup>þ</sup> *<sup>H</sup>*^ \_

Here, as "T" represents the matrix transpose, *T* ¼ ½ � *T*<sup>1</sup> *T*<sup>2</sup>

the torque column matrix for joint torques T1 and T2. Also, *<sup>M</sup>*^ <sup>¼</sup> *<sup>M</sup>*^ *<sup>θ</sup>*

regarding the error definition in Eq. (11) as follows [10]:

*<sup>T</sup>* <sup>¼</sup> *<sup>M</sup>*^ €

€*<sup>e</sup>* <sup>þ</sup> *<sup>M</sup>*^ �<sup>1</sup> \_

0 2*ζ<sup>c</sup>*<sup>2</sup> *ω<sup>c</sup>*<sup>2</sup> � �

The relevant control rule can be written according to the computed torque method with the addition of the integral action to nullify the steady-state errors by

*<sup>θ</sup>* <sup>þ</sup> *<sup>K</sup>*^ *<sup>p</sup> <sup>e</sup>* <sup>þ</sup> *<sup>K</sup>*^*<sup>i</sup>*

denote the inertia and centrifugal effect matrices, while *K*^ *<sup>p</sup>* and *K*^*<sup>i</sup>*

*<sup>e</sup>* <sup>þ</sup> *<sup>M</sup>*^ �<sup>1</sup>

As ωci and ζci denote the desired bandwidth and damping ratio of link i (i = 1 and 2), the error dynamics can be written for a second-order ideal system with two degrees of

and *<sup>W</sup>*^ <sup>¼</sup> *<sup>ω</sup>*<sup>2</sup>

Consequently, *K*^ *<sup>p</sup>* and *K*^*<sup>i</sup>* are determined by equating Eq. (13) to Eq. (14) as

*<sup>K</sup>*^ *<sup>p</sup>* <sup>¼</sup> *<sup>M</sup>*^ *<sup>D</sup>*^ � \_

During the control of the tracked land vehicle for the angular variables, the angular position requirement for ψ should also be satisfied. For this purpose, a twostage cascaded control scheme is constructed for the tracked land vehicle. In this algorithm, the outer loop is responsible of making linear velocity control in accordance with the guidance commands, while the inner loop makes the angular

correspond to the proportional and integral gain matrices, respectively. As implied, the resulting control system becomes in PI (proportional plus integral) form [8]. The differential equation corresponding to the error dynamics of the control system for the robotic arm are obtained using Eqs. (11) and (12) in the following fashion:

> *<sup>M</sup>*^ <sup>þ</sup> *<sup>K</sup>*^ *<sup>p</sup>* � � \_

€*<sup>e</sup>* <sup>þ</sup> *<sup>D</sup>*^ \_

ð

*<sup>θ</sup><sup>d</sup>* <sup>¼</sup> \_

*e dt* (12)

*<sup>K</sup>*^*ie* <sup>¼</sup> <sup>0</sup> (13)

*<sup>e</sup>* <sup>þ</sup> *<sup>W</sup>*^ *<sup>e</sup>* <sup>¼</sup> <sup>0</sup> (14)

*M*^ (15)

.

*<sup>K</sup>*^*<sup>i</sup>* <sup>¼</sup> *<sup>M</sup>*^ *<sup>W</sup>*^ (16)

*<sup>c</sup>*<sup>1</sup> 0 0 *ω*<sup>2</sup> *c*2

" #

*<sup>T</sup>* demonstrates

� � and

*θ* (11)

*θ*1*<sup>d</sup>* \_ *θ*2*<sup>d</sup>* � �*<sup>T</sup>*

a function of time [8]. Here, as \_

*Service Robotics*

*<sup>H</sup>*^ <sup>¼</sup> *<sup>H</sup>*^ \_

*θ*, *θ* � �

freedom as follows [10]:

follows [8]:

**66**

where *<sup>D</sup>*^ <sup>¼</sup> <sup>2</sup>*ζ<sup>c</sup>*1*ω<sup>c</sup>*<sup>1</sup> <sup>0</sup>

**3.2 Tracked land vehicle control system**

*θ*1*<sup>d</sup>* and \_

position control such that the orientation angle requirement arising during the linear velocity control is realized [8].

Here, the expressions which will be considered for the design of the linear velocity control system which is termed as "the primary control system" can be arranged in the forthcoming state space form from Eqs. (2) and (3) as *xp* <sup>¼</sup> ½ � *<sup>x</sup>*\_ *<sup>y</sup>*\_ *<sup>T</sup>*, *up*, and *bp* stand for the column matrices for the state variables, system inputs, and gravity effect, respectively [8]:

$$
\dot{\overline{\varpi}}\_p = -\overline{b}\_p + \overline{u}\_p \tag{17}
$$

The control rule of the primary control system can be established using the computed torque control method according to the PI control action like the robotic arm control system so as to nullify the steady-state errors [8]:

$$
\overline{\boldsymbol{u}}\_p = \dot{\overline{\boldsymbol{x}}}\_{pd} + \overline{\boldsymbol{b}}\_p + \hat{\boldsymbol{K}}\_{pp}\overline{\boldsymbol{e}}\_p + \hat{\boldsymbol{K}}\_{pi} \left\{ \overline{\boldsymbol{e}}\_p \, dt \right. \tag{18}
$$

In the above equation, *xpd*, *K*^ *pp*, *K*^ *pi*, and *ep* denote the column matrix for the desired inputs, gain matrix for the proportional control action, gain matrix for the integral control action, and column matrix for the error, respectively, for the primary control system. Here, *ep* ¼ *xpd* � *xp* is introduced.

Substituting Eq. (18) into Eq. (17) yields the next equation for the error dynamics of the primary control system [8]:

$$
\ddot{\overline{e}}\_p + \hat{K}\_{pp}\dot{\overline{e}}\_p + \hat{K}\_{pi}\overline{e}\_p = \overline{\mathbf{0}} \tag{19}
$$

As ωpi and ζpi denote the desired bandwidth and damping ratio of link i (i = 1 and 2) for the primary control system, the error dynamics can be written for a second-order ideal system with two degrees of freedom as follows [8]:

$$
\ddot{\overline{e}}\_p + \hat{D}\_p \dot{\overline{e}}\_p + \hat{\mathcal{W}}\_p \overline{e}\_p = \overline{\mathbf{0}} \tag{20}
$$

$$
\text{where } \hat{D}\_p = \begin{bmatrix} 2\zeta\_{p1}o\_{p1} & 0 \\ 0 & 2\zeta\_{p2}o\_{p2} \\ \vdots & \vdots \end{bmatrix} \text{ and } \hat{W}\_p = \begin{bmatrix} o\_{p1}^2 & 0 \\ 0 & o\_{p2}^2 \end{bmatrix}.
$$

Equating Eq. (19) to Eq. (20), the gain matrices are found for the primary control system as follows [8]:

$$
\hat{K}\_{pp} = \hat{D}\_p \tag{21}
$$

$$
\hat{K}\_{pi} = \hat{W}\_p \tag{22}
$$

The control rule for the angular control system called "the secondary control system" can be set using the computed torque method according to the PD (proportional plus derivative) control action in the following fashion [8]:

$$
\mu\_{\psi} = \ddot{\psi}\_d - b\_{\psi} + K\_{sp}e\_s + K\_{sd}\dot{e}\_s \tag{23}
$$

In the above expression, bψ, ψd, Ksp, Ksd, and es represent the inertia gain for the vehicle, desired input variable for the secondary control system, gain of the proportional control action, gain of the derivative control action, and error, respectively. Also, *es* ¼ *ψ<sup>d</sup>* � *ψ* is introduced.

Plugging Eq. (23) into Eq. (4), the error dynamics of the secondary control system is determined as follows [8]:

$$
\ddot{e}\_s + K\_{sd}\dot{e}\_s + K\_{sp}e\_s = 0 \tag{24}
$$

where *<sup>D</sup>*^ *<sup>p</sup>* <sup>¼</sup>

system as follows:

2 6 4

*DOI: http://dx.doi.org/10.5772/intechopen.91830*

secondary control system [8]:

where *bs* <sup>¼</sup> �*c<sup>ϕ</sup> <sup>ϕ</sup>*\_ <sup>þ</sup> *<sup>J</sup>ψθ <sup>ψ</sup>*\_ <sup>þ</sup> *<sup>J</sup><sup>θ</sup>* <sup>Ω</sup>*<sup>e</sup>*

motion in the yaw plane [11, 12].

system is obtained as

following equation [8]:

2 6 4

where *<sup>D</sup>*^ *<sup>s</sup>* <sup>¼</sup>

**69**

2*ζp*1*ωp*<sup>1</sup> 0 0 0 2*ζp*<sup>2</sup> *ωp*<sup>2</sup> 0 0 02*ζp*<sup>3</sup> *ωp*<sup>3</sup>

*Guidance-Based Motion Planning of Autonomous Systems*

� � \_

3 7 5

Matching Eqs. (30) and (31), the gain matrices appear for the primary control

As the corresponding columns of the state variables and inputs of the system are shown to be *xs* <sup>¼</sup> ½ � *ϕθψ <sup>T</sup>* and *us* <sup>¼</sup> ½ � *<sup>u</sup><sup>ϕ</sup> <sup>u</sup><sup>θ</sup> <sup>u</sup><sup>ψ</sup> <sup>T</sup>*, the matrix equality below is reached via Eqs. (5)–(7) for the orientation, or attitude, control system as the

*<sup>θ</sup>* �*c<sup>θ</sup>* \_

*us* <sup>¼</sup> *<sup>x</sup>*€*sd* � *bs* <sup>þ</sup> *<sup>K</sup>*^*sp es* <sup>þ</sup> *<sup>K</sup>*^*sd* \_

Here, *xsd*, *K*^*sp*, *K*^*sd*, and *es* indicate the desired input column, proportional gain matrix, derivative gain matrix, and error column for the secondary control system,

In the proposed entire control scheme, the desired values of ϕ and θ are calculated using ux, uy, and uz inputs along with the parameter uF by regarding the definitions within Eqs. (5)–(10). In other words, the reference inputs, or commands, to ϕ and θ are generated by the outer loop. On the other hand, the remaining angular position variable ψ is adjusted to be a constant value. That means its reference value is set as a fixed quantity. In this sense, the desired value of ψ is then specified to be zero as the decision on keeping the quadrotor with no angular

Inserting Eq. (35) into Eq. (34), the error dynamics of the secondary control

As ωsi and ζsi denote the desired bandwidth and damping ratio of the ith state variable (i = 1, 2, and 3) of the secondary control system, the error dynamics of a second-order ideal system with three degrees of freedom can be described using the

> 3 7 5

and *<sup>W</sup>*^ *<sup>s</sup>* <sup>¼</sup>

respectively. Also, the definition *es* ¼ *xsd* � *xs* is made for the error term.

€*es* <sup>þ</sup> *<sup>K</sup>*^*sd* \_

€*es* <sup>þ</sup> *<sup>D</sup>*^ *<sup>s</sup>* \_

2*ζ<sup>s</sup>*1*ω<sup>s</sup>*<sup>1</sup> 0 0 0 2*ζ<sup>s</sup>*<sup>2</sup> *ω<sup>s</sup>*<sup>2</sup> 0 0 02*ζ<sup>s</sup>*<sup>3</sup> *ω<sup>s</sup>*<sup>3</sup>

In a similar manner, the control law can be written according to the computed torque method with PD action for the secondary control system as follows [8]:

and *<sup>W</sup>*^ *<sup>p</sup>* <sup>¼</sup>

*ω*2

*<sup>K</sup>*^ *pp* <sup>¼</sup> *<sup>D</sup>*^ *<sup>p</sup>* (32)

*<sup>K</sup>*^ *pi* <sup>¼</sup> *<sup>W</sup>*^ *<sup>p</sup>* (33)

*<sup>x</sup>*€*<sup>s</sup>* <sup>¼</sup> *bs* <sup>þ</sup> *us* (34)

*es* <sup>þ</sup> *<sup>K</sup>*^*sp es* <sup>¼</sup> <sup>0</sup> (36)

*es* <sup>þ</sup> *<sup>W</sup>*^ *ses* <sup>¼</sup> <sup>0</sup> (37)

*ω*2

2 6 4

*<sup>s</sup>*<sup>1</sup> 0 0 0 *ω*<sup>2</sup>

*<sup>s</sup>*<sup>2</sup> 0 0 0 *ω*<sup>2</sup>

*s*3

3 7 5.

� �*ϕ*\_ �*c<sup>ψ</sup> <sup>ψ</sup>*\_ <sup>þ</sup> *<sup>J</sup>θϕ* \_

*es* (35)

*θ* þ *Jψϕ ψ*\_ � *J<sup>ϕ</sup>* Ω*<sup>e</sup>*

*θϕ*\_ h i*<sup>T</sup>*

*<sup>p</sup>*<sup>1</sup> 0 0

0 0 *ω*<sup>2</sup>

*<sup>p</sup>*<sup>2</sup> 0

*p*3

.

0 *ω*<sup>2</sup>

As ω<sup>s</sup> and ζ<sup>s</sup> indicate the desired bandwidth and damping ratio for variable ψ for the secondary control system, the error dynamics can be written for a second-order ideal system with single degree of freedom as given below:

$$
\ddot{\mathbf{e}}\_s + \mathbf{2}\,\zeta\_s \,\boldsymbol{o}\_s \dot{\mathbf{e}}\_s + \boldsymbol{o}\_s^2 \mathbf{e}\_s = \mathbf{0} \tag{25}
$$

Matching Eqs. (24) and (25) results in the forthcoming gains for the secondary control system [8]:

$$K\_{sp} = o\_s^2 \tag{26}$$

$$K\_{sd} = \mathcal{Z}\zeta\_s a\_s \tag{27}$$

#### **3.3 Quadrotor control system**

The same two-stage control system is designed for the quadrotor as that for the tracked land vehicle. For the primary control system, these expressions can be rearranged in the state space form by assigning the columns of the state variables and inputs of the system to be *xp* <sup>¼</sup> ½ � *<sup>x</sup>*\_ *<sup>y</sup>*\_ *<sup>z</sup>*\_ *<sup>T</sup>* and *up* <sup>¼</sup> *ux uy uz* � �*<sup>T</sup>* , respectively, as letter "T" indicates the transpose operation as follows [8]:

$$
\dot{\overline{\varpi}}\_p = -\hat{\mathbf{C}}\_p \overline{\varpi}\_p + \overline{\mathbf{u}}\_p \tag{28}
$$

where *<sup>C</sup>*^*<sup>p</sup>* <sup>¼</sup> *cx* 0 0 0 *cy* 0 0 0 *cz* 2 6 4 3 7 5.

The control law of the primary control system including an integral action can be designed as per the computed torque method with PI action in the following manner [8]:

$$
\overline{\boldsymbol{\omega}}\_{p} = \dot{\overline{\boldsymbol{\infty}}}\_{pd} + \dot{\mathcal{C}}\_{p}\overline{\boldsymbol{\infty}}\_{p} + \dot{\mathcal{K}}\_{pp}\overline{\boldsymbol{e}}\_{p} + \dot{\mathcal{K}}\_{pi} \left\{ \overline{\boldsymbol{e}}\_{p} \, dt \right\} \tag{29}
$$

In the expression above, *xpd*, *K*^ *pp*, *K*^ *pi*, and *ep* stand for the desired input column, proportional gain matrix, integral gain matrix, and error column for the primary control system, respectively, and the definition *ep* ¼ *xpd* � *xp* is given.

Substituting Eq. (29) into Eq. (28), the error dynamics of the primary control system is obtained as follows [8]:

$$
\ddot{\overline{e}}\_p + \dot{K}\_{pp}\dot{\overline{e}}\_p + \dot{K}\_{pi}\overline{e}\_p = \overline{\mathbf{0}} \tag{30}
$$

As ωpi and ζpi represent the desired bandwidth and damping ratio of the ith state variable (i = 1, 2, and 3) of the primary control system, respectively, the error dynamics of a second-order ideal system with three degrees of freedom can be described using the forthcoming expression [8]:

$$
\ddot{\overline{e}}\_p + \hat{D}\_p \dot{\overline{e}}\_p + \hat{W}\_p \overline{e}\_p = \overline{\mathbf{0}} \tag{31}
$$

*Guidance-Based Motion Planning of Autonomous Systems DOI: http://dx.doi.org/10.5772/intechopen.91830*

Plugging Eq. (23) into Eq. (4), the error dynamics of the secondary control

As ω<sup>s</sup> and ζ<sup>s</sup> indicate the desired bandwidth and damping ratio for variable ψ for the secondary control system, the error dynamics can be written for a second-order

Matching Eqs. (24) and (25) results in the forthcoming gains for the secondary

*Ksp* <sup>¼</sup> *<sup>ω</sup>*<sup>2</sup>

The same two-stage control system is designed for the quadrotor as that for the

The control law of the primary control system including an integral action can be

In the expression above, *xpd*, *K*^ *pp*, *K*^ *pi*, and *ep* stand for the desired input column, proportional gain matrix, integral gain matrix, and error column for the primary

Substituting Eq. (29) into Eq. (28), the error dynamics of the primary control

As ωpi and ζpi represent the desired bandwidth and damping ratio of the ith state

variable (i = 1, 2, and 3) of the primary control system, respectively, the error dynamics of a second-order ideal system with three degrees of freedom can be

designed as per the computed torque method with PI action in the following

*up* <sup>¼</sup> *<sup>x</sup>*\_ *pd* <sup>þ</sup> *<sup>C</sup>*^*<sup>p</sup> xp* <sup>þ</sup> *<sup>K</sup>*^ *pp ep* <sup>þ</sup> *<sup>K</sup>*^ *pi*

control system, respectively, and the definition *ep* ¼ *xpd* � *xp* is given.

€*ep* <sup>þ</sup> *<sup>K</sup>*^ *pp* \_

€*ep* <sup>þ</sup> *<sup>D</sup>*^ *<sup>p</sup>* \_

tracked land vehicle. For the primary control system, these expressions can be rearranged in the state space form by assigning the columns of the state variables

and inputs of the system to be *xp* <sup>¼</sup> ½ � *<sup>x</sup>*\_ *<sup>y</sup>*\_ *<sup>z</sup>*\_ *<sup>T</sup>* and *up* <sup>¼</sup> *ux uy uz*

tively, as letter "T" indicates the transpose operation as follows [8]:

€*es* <sup>þ</sup> <sup>2</sup>*ζsωse*\_*<sup>s</sup>* <sup>þ</sup> *<sup>ω</sup>*<sup>2</sup>

€*es* þ *Ksd e*\_*<sup>s</sup>* þ *Ksp es* ¼ 0 (24)

*<sup>s</sup> es* ¼ 0 (25)

*<sup>s</sup>* (26)

� �*<sup>T</sup>*

*ep dt* (29)

, respec-

*Ksd* ¼ 2*ζ<sup>s</sup> ω<sup>s</sup>* (27)

*<sup>x</sup>*\_ *<sup>p</sup>* ¼ �*C*^*<sup>p</sup> xp* <sup>þ</sup> *up* (28)

ð

*ep* <sup>þ</sup> *<sup>K</sup>*^ *piep* <sup>¼</sup> <sup>0</sup> (30)

*ep* <sup>þ</sup> *<sup>W</sup>*^ *<sup>p</sup> ep* <sup>¼</sup> <sup>0</sup> (31)

system is determined as follows [8]:

control system [8]:

*Service Robotics*

where *<sup>C</sup>*^*<sup>p</sup>* <sup>¼</sup>

manner [8]:

**68**

**3.3 Quadrotor control system**

*cx* 0 0 0 *cy* 0 0 0 *cz* 3 7 5.

2 6 4

system is obtained as follows [8]:

described using the forthcoming expression [8]:

ideal system with single degree of freedom as given below:

$$
\text{where } \hat{D}\_p = \begin{bmatrix} 2\zeta\_{p1}\alpha\_{p1} & 0 & 0 \\ 0 & 2\zeta\_{p2}\alpha\_{p2} & 0 \\ 0 & 0 & 2\zeta\_{p3}\alpha\_{p3} \end{bmatrix} \text{ and } \hat{W}\_p = \begin{bmatrix} \alpha\_{p1}^2 & 0 & 0 \\ 0 & \alpha\_{p2}^2 & 0 \\ 0 & 0 & \alpha\_{p3}^2 \end{bmatrix}.
$$

Matching Eqs. (30) and (31), the gain matrices appear for the primary control system as follows:

$$
\hat{K}\_{pp} = \hat{D}\_p \tag{32}
$$

$$
\hat{K}\_{pi} = \hat{\mathcal{W}}\_p \tag{33}
$$

As the corresponding columns of the state variables and inputs of the system are shown to be *xs* <sup>¼</sup> ½ � *ϕθψ <sup>T</sup>* and *us* <sup>¼</sup> ½ � *<sup>u</sup><sup>ϕ</sup> <sup>u</sup><sup>θ</sup> <sup>u</sup><sup>ψ</sup> <sup>T</sup>*, the matrix equality below is reached via Eqs. (5)–(7) for the orientation, or attitude, control system as the secondary control system [8]:

$$
\ddot{\overline{\mathfrak{X}}}\_{\mathfrak{s}} = b\_{\mathfrak{s}} + \overline{\mathfrak{u}}\_{\mathfrak{s}} \tag{34}
$$

where *bs* <sup>¼</sup> �*c<sup>ϕ</sup> <sup>ϕ</sup>*\_ <sup>þ</sup> *<sup>J</sup>ψθ <sup>ψ</sup>*\_ <sup>þ</sup> *<sup>J</sup><sup>θ</sup>* <sup>Ω</sup>*<sup>e</sup>* � � \_ *<sup>θ</sup>* �*c<sup>θ</sup>* \_ *θ* þ *Jψϕ ψ*\_ � *J<sup>ϕ</sup>* Ω*<sup>e</sup>* � �*ϕ*\_ �*c<sup>ψ</sup> <sup>ψ</sup>*\_ <sup>þ</sup> *<sup>J</sup>θϕ* \_ *θϕ*\_ h i*<sup>T</sup>* .

In a similar manner, the control law can be written according to the computed torque method with PD action for the secondary control system as follows [8]:

$$
\overline{\boldsymbol{u}}\_{s} = \stackrel{\cdots}{\overline{\mathbf{x}}}\_{sd} - \overline{\boldsymbol{b}}\_{s} + \dot{\boldsymbol{K}}\_{sp}\overline{\boldsymbol{e}}\_{s} + \dot{\boldsymbol{K}}\_{sd}\dot{\overline{\boldsymbol{e}}}\_{s} \tag{35}
$$

Here, *xsd*, *K*^*sp*, *K*^*sd*, and *es* indicate the desired input column, proportional gain matrix, derivative gain matrix, and error column for the secondary control system, respectively. Also, the definition *es* ¼ *xsd* � *xs* is made for the error term.

In the proposed entire control scheme, the desired values of ϕ and θ are calculated using ux, uy, and uz inputs along with the parameter uF by regarding the definitions within Eqs. (5)–(10). In other words, the reference inputs, or commands, to ϕ and θ are generated by the outer loop. On the other hand, the remaining angular position variable ψ is adjusted to be a constant value. That means its reference value is set as a fixed quantity. In this sense, the desired value of ψ is then specified to be zero as the decision on keeping the quadrotor with no angular motion in the yaw plane [11, 12].

Inserting Eq. (35) into Eq. (34), the error dynamics of the secondary control system is obtained as

$$
\ddot{\overline{e}}\_{\mathfrak{s}} + \hat{K}\_{\mathfrak{sl}} \dot{\overline{e}}\_{\mathfrak{s}} + \hat{K}\_{\mathfrak{sp}} \overline{e}\_{\mathfrak{s}} = \overline{\mathbf{0}} \tag{36}
$$

As ωsi and ζsi denote the desired bandwidth and damping ratio of the ith state variable (i = 1, 2, and 3) of the secondary control system, the error dynamics of a second-order ideal system with three degrees of freedom can be described using the following equation [8]:

$$
\ddot{\overline{e}}\_{\mathfrak{s}} + \hat{D}\_{\mathfrak{s}} \dot{\overline{e}}\_{\mathfrak{s}} + \hat{W}\_{\mathfrak{s}} \overline{e}\_{\mathfrak{s}} = \overline{\mathbf{0}} \tag{37}
$$

$$
\text{where } \hat{D}\_{\sharp} = \begin{bmatrix}
2\zeta\_{s1}a\_{\Omega} & 0 & 0 \\
0 & 2\zeta\_{s2}a\_{\Omega} & 0 \\
0 & 0 & 2\zeta\_{s3}a\_{\Omega}
\end{bmatrix} \text{ and } \hat{W}\_{\sharp} = \begin{bmatrix}
a\_{\imath 1}^{2} & 0 & 0 \\
0 & a\_{\imath 2}^{2} & 0 \\
0 & 0 & a\_{\imath 3}^{2}
\end{bmatrix}.
$$

Equating Eqs. (47) and (48) to each other yields the gain matrices of the secondary control system as shown below [8]:

$$
\hat{K}\_{sp} = \hat{W}\_s \tag{38}
$$

*dmiss* ¼

*Guidance-Based Motion Planning of Autonomous Systems*

*DOI: http://dx.doi.org/10.5772/intechopen.91830*

Δ*y t*ð Þ¼ *<sup>F</sup> yC*ð Þ� *tF yT*ð Þ *tF* , and Δ*z t*ð Þ¼ *<sup>F</sup> zC*ð Þ� *tF zT*ð Þ *tF* .

geometry is depicted in **Figure 6** [13].

predicted intercept point, respectively. Also, *v*

*<sup>q</sup>* and *γ<sup>c</sup>*

*ηc*

*Mactual* is turned into *v*

**5. Guidance law**

*v* !

*Mideal*. Once *v*

and direction, *v*

and vertical axes (*η<sup>c</sup>*

intercept [13].

**Figure 6.**

**71**

*Linear homing guidance law geometry [13].*

!

!

q

ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi Δ*x*2ð Þþ *tF* Δ*y*2ð Þþ *tF* Δ*z*2ð Þ *tF*

Here, as xC, yC, and zC stand for the position components of point C and xT, yT, and zT stand for the position components of point T on *F0*, Δ*x t*ð Þ¼ *<sup>F</sup> xC*ð Þ� *tF xT*ð Þ *tF* ,

In order for the considered point on the relevant system (point P for the robotic arm, point C for the tracked land vehicle, and point C for the quadrotor) to catch the desired point on the moving platform (point S for the robotic arm and point T for the tracked land vehicle and quadrotor), the guidance commands can be derived according to the LHG law for which the mechatronic system-target engagement

In **Figure 6**, M, T, and P stand for the mechatronic system, the target, and the

the mechatronic system at the beginning of the guidance. The velocity vector of the mechatronic system in order to be on the collision triangle is then indicated by

on the collision triangle so as to collide the intended target at point P. In applying this method, unless the target has a velocity vector constant in both magnitude

Regarding the LHG geometry expressed above verbally, the relevant guidance commands can be derived in terms of the orientation angles of vC from the lateral

!

*<sup>q</sup>*) as follows [6, 8]:

!

*Mideal* should be updated continuously in order to guarantee the

*<sup>q</sup>* <sup>¼</sup> *<sup>a</sup>* tan *vTy* <sup>Δ</sup>*<sup>t</sup>* � <sup>Δ</sup>*<sup>y</sup>* � �*=*ð Þ *vTx* <sup>Δ</sup>*<sup>t</sup>* � <sup>Δ</sup>*<sup>x</sup>* � � (45)

*Mactual* shows the velocity vector of

*Mideal*, it means that the mechatronic system is

(44)

$$
\hat{K}\_{sd} = \hat{D}\_s \tag{39}
$$

#### **4. Engagement geometry**

Since the motion of the moving land platform for the quadrotor, i.e., trajectory of the target point, is dealt with in the three-dimensional space, the lateral projection of the engagement geometry drawn for the quadrotor can be used for the engagements of the robotic arm and tracked land vehicle with their targets as well.

The engagement geometry between the quadrotor and the moving land platform is described in the horizontal and vertical planes separately. Thus, the vertical engagement between point C, the mass center of the quadrotor, and point T on the moving platform can be depicted as seen in **Figure 5**. The lateral engagement geometry between points C and T can be similarly constructed using the same expressions [8].

In **Figure 5**, vC, γq, rT/C, γt, and λ<sup>p</sup> show the resultant speed of point C, orientation angle of vC from the horizontal plane, relative position of point T with respect to point C, orientation angle of vT from the horizontal axis, and orientation angle of rT/C from the horizontal axis. Here, the next equations are held for vC, γt, and λ<sup>p</sup> [8]:

$$
\upsilon\_C = \sqrt{\dot{\mathbf{x}}\_C^2 + \dot{\mathbf{y}}\_C^2 + \dot{\mathbf{z}}\_C^2} \tag{40}
$$

$$\gamma\_t = \begin{cases} -\pi/2, t\_0 < t \le t\_1 \\ (\pi/2) - \psi, t\_1 < t \le t\_2 \text{ (rad)} \\ \pi/2, t > t\_2 \end{cases} \tag{41}$$

$$
\lambda\_p = a \tan\left[ (\mathcal{y}\_T - \mathcal{y}\_C) / (\mathbf{x}\_T - \mathbf{x}\_C) \right] \tag{42}
$$

Similarly, the orientation angle of rT/C from the horizontal plane, i.e., λy, can be obtained as follows [8]:

$$\lambda\_{\mathcal{I}} = a \tan\left[ (\mathbf{z}\_T - \mathbf{z}\_C)/(\mathbf{x}\_T - \mathbf{x}\_C) \right] \tag{43}$$

The final offset between points C and T at the end of the engagement, i.e., dmiss, is calculated using the next formula as tF indicates the termination time [8]:

**Figure 5.**

*Vertical engagement geometry between the mass center of the quadrotor and point defined on the moving platform [8].*

*Guidance-Based Motion Planning of Autonomous Systems DOI: http://dx.doi.org/10.5772/intechopen.91830*

$$d\_{\rm miss} = \sqrt{\Delta \mathbf{x}^2(t\_F) + \Delta \mathbf{y}^2(t\_F) + \Delta \mathbf{z}^2(t\_F)}\tag{44}$$

Here, as xC, yC, and zC stand for the position components of point C and xT, yT, and zT stand for the position components of point T on *F0*, Δ*x t*ð Þ¼ *<sup>F</sup> xC*ð Þ� *tF xT*ð Þ *tF* , Δ*y t*ð Þ¼ *<sup>F</sup> yC*ð Þ� *tF yT*ð Þ *tF* , and Δ*z t*ð Þ¼ *<sup>F</sup> zC*ð Þ� *tF zT*ð Þ *tF* .

### **5. Guidance law**

Equating Eqs. (47) and (48) to each other yields the gain matrices of the sec-

Since the motion of the moving land platform for the quadrotor, i.e., trajectory of the target point, is dealt with in the three-dimensional space, the lateral projection of the engagement geometry drawn for the quadrotor can be used for the engagements of the robotic arm and tracked land vehicle with their targets as well. The engagement geometry between the quadrotor and the moving land platform

In **Figure 5**, vC, γq, rT/C, γt, and λ<sup>p</sup> show the resultant speed of point C, orientation angle of vC from the horizontal plane, relative position of point T with respect to point C, orientation angle of vT from the horizontal axis, and orientation angle of rT/C from the horizontal axis. Here, the next equations are held for vC, γt, and λ<sup>p</sup> [8]:

ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi

� �*=*ð Þ *xT* � *xC*

*x*\_ 2 *<sup>C</sup>* þ *y*\_ 2 *<sup>C</sup>* þ *z*\_ 2 *C*

�*π=*2, *t*<sup>0</sup> <*t*≤*t*<sup>1</sup> ð Þ� *π=*2 *ψ*, *t*<sup>1</sup> <*t* ≤*t*<sup>2</sup> *π=*2, *t*>*t*<sup>2</sup>

Similarly, the orientation angle of rT/C from the horizontal plane, i.e., λy, can be

The final offset between points C and T at the end of the engagement, i.e., dmiss,

is calculated using the next formula as tF indicates the termination time [8]:

*Vertical engagement geometry between the mass center of the quadrotor and point defined on the moving*

q

is described in the horizontal and vertical planes separately. Thus, the vertical engagement between point C, the mass center of the quadrotor, and point T on the moving platform can be depicted as seen in **Figure 5**. The lateral engagement geometry between points C and T can be similarly constructed using the same

*vC* ¼

*λ<sup>p</sup>* ¼ *a* tan *yT* � *yC*

8 ><

>:

*γ<sup>t</sup>* ¼

*<sup>K</sup>*^*sp* <sup>¼</sup> *<sup>W</sup>*^ *<sup>s</sup>* (38) *<sup>K</sup>*^*sd* <sup>¼</sup> *<sup>D</sup>*^ *<sup>s</sup>* (39)

(40)

ð Þ rad (41)

� � (42)

*λ<sup>y</sup>* ¼ *a* tan ½ � ð Þ *zT* � *zC =*ð Þ *xT* � *xC* (43)

ondary control system as shown below [8]:

**4. Engagement geometry**

expressions [8].

*Service Robotics*

obtained as follows [8]:

**Figure 5.**

**70**

*platform [8].*

In order for the considered point on the relevant system (point P for the robotic arm, point C for the tracked land vehicle, and point C for the quadrotor) to catch the desired point on the moving platform (point S for the robotic arm and point T for the tracked land vehicle and quadrotor), the guidance commands can be derived according to the LHG law for which the mechatronic system-target engagement geometry is depicted in **Figure 6** [13].

In **Figure 6**, M, T, and P stand for the mechatronic system, the target, and the predicted intercept point, respectively. Also, *v* ! *Mactual* shows the velocity vector of the mechatronic system at the beginning of the guidance. The velocity vector of the mechatronic system in order to be on the collision triangle is then indicated by *v* ! *Mideal*. Once *v* ! *Mactual* is turned into *v* ! *Mideal*, it means that the mechatronic system is on the collision triangle so as to collide the intended target at point P. In applying this method, unless the target has a velocity vector constant in both magnitude and direction, *v* ! *Mideal* should be updated continuously in order to guarantee the intercept [13].

Regarding the LHG geometry expressed above verbally, the relevant guidance commands can be derived in terms of the orientation angles of vC from the lateral and vertical axes (*η<sup>c</sup> <sup>q</sup>* and *γ<sup>c</sup> <sup>q</sup>*) as follows [6, 8]:

$$\eta\_q^\varepsilon = a \tan\left[\left(\upsilon\_{T\circ} \Delta t - \Delta \mathfrak{y}\right) / \left(\upsilon\_{T\circ} \Delta t - \Delta \mathfrak{x}\right)\right] \tag{45}$$

**Figure 6.** *Linear homing guidance law geometry [13].*

$$\gamma\_q^\epsilon = a \tan \left[ \frac{\Delta z - v\_{T\natural} \Delta t}{(v\_{T\natural} \Delta t - \Delta \pi) \cos \left(\eta\_q^\epsilon \right) + (v\_{T\natural} \Delta t - \Delta \eta) \sin \left(\eta\_q^\epsilon \right)} \right] \tag{46}$$

where Δ*x* ¼ *xC* � *xT*, Δ*y* ¼ *yC* � *yT*, and Δ*z* ¼ *zC* � *zT*.

In Eqs. (45) and (46), the components of the linear velocity vector of point T whose amplitude is vT, i.e., vTx, vTy, and vTz, are defined in the following equations [6, 8]:

$$\boldsymbol{\nu}\_{T\mathbf{x}} = \boldsymbol{\nu}\_{T} \cos\left(\boldsymbol{\chi}\_{t}\right) \tag{47}$$

$$\boldsymbol{\nu}\_{T\boldsymbol{\gamma}} = \boldsymbol{\nu}\_{T} \sin \left( \boldsymbol{\chi}\_{t} \right) \tag{48}$$

$$\boldsymbol{\nu}\_{T\mathbf{z}} = \mathbf{0} \tag{49}$$

**6. Computer simulations**

*Guidance-Based Motion Planning of Autonomous Systems*

*DOI: http://dx.doi.org/10.5772/intechopen.91830*

**Table 2.**

**Table 3.**

**73**

The system trajectories acquired from the computer simulations performed in accordance with the numerical values given in **Tables 1**–**3** for the robotic arm, tracked land vehicle, and quadrotor are submitted in **Figures 7**–**10** along with the corresponding target motions. Having constructed the engagement geometry between the mechatronic system under consideration and target, the LHG law is applied for these situations. In the simulations, disturbance effects due to the nonlinear friction characteristic and noise on the sensors on the joints are assumed

**Parameter Numerical value** a 2.5 m b 4 m v 1.25 m m 25,000 kg Iz 60,000 kg<sup>m</sup><sup>2</sup> rs 0.3 m μ<sup>x</sup> and μ<sup>y</sup> 0.4 ωp1 and ωp2 10 Hz fs 30 Hz ζp1, ζp2, and ζ<sup>s</sup> 0.707

**Parameter Numerical value** L 0.25 m <sup>b</sup> <sup>5</sup> <sup>10</sup><sup>5</sup> <sup>N</sup><sup>s</sup>

<sup>d</sup> <sup>1</sup> <sup>10</sup><sup>6</sup> <sup>N</sup><sup>m</sup><sup>s</sup>

m 2 kg Ix and Iy 0.2 kg<sup>m</sup><sup>2</sup> Iz 0.3 kg<sup>m</sup><sup>2</sup> Jv <sup>1</sup> <sup>10</sup><sup>3</sup> kg<sup>m</sup><sup>2</sup> Kx, Ky, and Kz 0.01 Ns/m Kϕ, Kθ, and K<sup>ψ</sup> 0.012 Ns/m ωpi 5 Hz ωsi 15 Hz ζpi and ζsi 0.707 H 50 m ρ 15 m D 50 m Solver step 1 ms

2

2

*Numerical values used in the simulations for the tracked land vehicle [8].*

*Numerical values used in the simulations for the quadrotor [8].*

Furthermore, the remaining time till the end of the engagement, i.e., Δt, is formulated below:

$$
\Delta t = \left[\sqrt{\sigma^2 + \left(v\_C^2 - v\_T^2\right)\Delta r^2} - \sigma\right] / \left(v\_C^2 - v\_T^2\right) \tag{50}
$$

In the above equation, *<sup>σ</sup>* <sup>¼</sup> *vTx* <sup>Δ</sup>*<sup>x</sup>* <sup>þ</sup> *vTy* <sup>Δ</sup>*<sup>y</sup>* <sup>þ</sup> *vTz* <sup>Δ</sup>*<sup>z</sup>* andΔ*r*<sup>2</sup> <sup>¼</sup> <sup>Δ</sup>*x*<sup>2</sup> <sup>þ</sup> <sup>Δ</sup>*y*<sup>2</sup> <sup>þ</sup> <sup>Δ</sup>*z*2.

In this work, it is assumed that the speed and orientation parameters of the moving target are obtained by processing the data acquired by the camera on the system under control. Since the control inputs of the designed control system are linear velocity components, the guidance commands given in Eqs. (45) and (46) should be expressed in terms of the linear velocity parameters for realization. This transformation can be done by writing the velocity vector of point C with amplitude vC in terms of its components on *F0* as follows [6, 8]:

$$
\overline{\mathbf{x}}\_{pd} = \nu\_C \begin{bmatrix}
\cos\left(\eta\_q^\epsilon\right)\cos\left(\chi\_q^\epsilon\right) \\
\sin\left(\eta\_q^\epsilon\right)\cos\left(\chi\_q^\epsilon\right) \\
\end{bmatrix} \tag{51}
$$


**Table 1.** *Numerical values used in the simulations for the robotic arm [8].*
