# Notes on: [Real-Time Obstacle Avoidance for Manipulators and Mobile Robots](https://link.springer.com/chapter/10.1007/978-1-4613-8997-2_29)
## Author - Mohammad Saad

## Abstract:
* unique real-time obstacle avoidance approach for manipulators and mobile robots based on the artificial potential field concept.
* reformulated the manipulator control problem as direct control of manipulator motion in operational space
* The artificial potential field approach has been extended to collision avoidance for all manipulator links.
* In addition, a joint space artificial potential field is used to satisfy the manipulator internal joint constraints.

## Introduction:
### Previous work
* In previous research, robot collision avoidance has been a component of higher levels of control in hierarchical robot control systems.
* Collision avoidance has been treated as a planning problem,research in this area has focused on the development of collisionfree path planning algorithms aim at providing the low level control with a path that will enable the robot to accomplish its assigned task free from any risk of collision.
* low level control is limited to the execution of elementary operations.to overcome this high level control is used which is slower than the time response of typical robot.(limitation)

### Proposed Solution
* it is possible to extend greatly the function of low level control and to carry out more complex operations by coupling environment sensing feedback with the lowest level of control.
* Collision avoidance at the low level of control is not intended to replace high level functions or to solve planning problems.
* The purpose is to make better use of low level control capabilities in performing real-time operations. At this low level of control, the degree or level of competence will remain less than that of higher level control.
* *This paper deals with Operational Space formulation and application of Artificial potential field*
* *Also deals with collision avoidance algorithm using Artificial potential fiel in Operational Spaace.*

## Operational Space Formulation:
* The operational space formulation is the basis for the application of the potential field approach to robot manipulators.
* Operational space co-ordinate system X is a set of $m_0$ independant variables that defines the position and orientation of end effecctor in refrence frame $R_0$
$X = [x,y,z,\alpha,\beta,,\gamma]^T$
* The kinetic energy of the holonomic articulated mechanism is
$T(x,\dot x)=\frac12 \dot X^T A(x) \dot X$
$A(x)\to$ kinetic energy matrix.
* Using the Lagrangian formalism, the end-effector equations of motion are
$\frac d{dt}(\frac {\partial L}{\partial \dot x})-\frac {\partial L}{\partial x}=F$
$L(x,\dot x)=T(x,\dot x)-U(x)$
$L(x,\dot x) \to$ Lagrangian; $U(x)\to$ potential energy of the gravity. **The symbol F is the operational force vector.**
* After solving lagrange equation we get
$A(x)\ddot x+\mu(x,\dot x)+p(x)=F$
$\mu(x,\dot x) \to$ centrifugal and Coriolis forces; $p(x)\to$ gravity force
* Specific forces $\tau$ must be applied with joint based actuators on the basis of selected F as a command vector.
$\tau =J^T(q)F$
J(q) is jacobian matrix and q is the vector of n joint co ordinates
* The decoupling of end effector in operational space
$F=A(x)F^{\star}+\mu(x,\dot x)+p(x)$
$F^{\star} \to$ command vector of the decoupled end effector that becomes equivalent to a single unit mass.
## The artificial potential field Approach
* The manipulator moves in a field of forces. The position to be reached is an attractive pole for the end effector and obstacles are repulsive surfaces for the manipulator parts.
* $x_d \to$ goal positon; $o \to$ obstacle
so the artificial potential field is given by:
$U_{art}(x)=U_{x_d}(x)+U_{o}(x)$
* Potential energy in Lagrangian is:
$U(x)=U_{art}(x)+U_g(x)$
* the command vector $F_{art}^\star$ of the decoupled end effector that corresponds to applying the artificial potential field $U_{art}$ can be written as
$F_{art}^\star=F_{x_d}^\star +F_{o}^\star$
* $F_{x_d}^\star=-k_p(x-x_d) =-grad[U_{x_d}(x)] \to$ attracting force on end effector to reach goal position $x_d$
* $U_{x_d}(x)=\frac 12k_p(x-x_d)^2$
* $F_{o}^\star =-grad[U_{o}(x)] \to$ *Force Inducing an Artificial Repulsion from the Surface (FIRAS)* of the obstacle created by the potential field Uo(x).
* To achieve the asymptotic stability adding dissipative force to $F_{x_d}^\star$
$F_m^\star=-k_p(x-x_d)-k_v\dot x$
* This command vector is inadequate to control the manipulator for large end-effeetor motion toward a goal position without path specification. For such a task, it is better for the end effector to move in a straight line with an upper speed limit.
* now desired speed become the upper speed limit. if end effector is moving with upper speed limit (constant speed) then no force , hence $F_m^\star=0$ which leads to
* $\dot x_d=\frac {k_p}{k_v}(x_d-x)$
* let $V_{max} be the assigned speed limit$
* $F_m^\star=-k_v(\dot x-v\dot x_d)$
* $v=min(1,\frac {V_{max}}{\sqrt{\dot x_d^T\dot x_d}})$
* With this scheme, the velocity vector $\dot x$ is controlled to be pointed toward the goal position while its magnitude is limited to $V_{max}$
## FIRAS (Force Inducing an Artificial Repulsion from the Surface) Function
* Artificial Potential Field $U_o(x)$
* should be a nonnegative continuous and differentiable function whose value tends to infinity as the end effector approaches the obstacle’s surface.
* the influence of this potential field must be limited to a given region surrounding the obstacle. (i.e force should be zero beyond the obstacle vicinity)
* Using the shortest distance to an obstacle O, the artificial potential field is
$U_o(x)=\frac 12 \eta(\frac {1}{\rho}-\frac {1}{\rho_o})^2 \space \space if \rho \leq \rho_o$
$U_o(x)=0 \space if \rho > \rho_o$
* $\rho_o \to$ limit distance of the potential field influence. it will depend on the end effector operating speed V and on its deceleration ability.
* $\rho\to$ shortest distance to the obstacle O
* The control of a Point Subjected to the Potential (PSP) with respect to an obstacle O is achieved using the FIRAS function,
* THe jont forces corresponding to $F_{(O,psp)}^\star$ is obtained using jacobian.
$\tau_{(O,psp)}=J_{psp}^T(q)A(x)F_{(O,psp)}^\star$
## Obstacle geometry modeling
* The surface, termed an n-ellipsoid, is represented by the equation
$(\frac xa)^{2n}+(\frac yb)^{2n}+(\frac zc)^{2n}=1$
and tends to a parallelepiped of dimensions (2a, 2b, 2c) as n tends to infinity.
* with n=4
## Robot Obstacle avoidance
* An obstacle $O_i$ is described by a set of primitives $P_p$.
* the control of a given point of the manipulator with respect to this obstacle by using the sum of the relevant gradients(Superposition properrty)
$F^\star_{O_i,psp}=\sum_pF^\star_{(P_p,psp)}$
* Control of this point for several obstacles is obtained using
$F^\star_{psp}=\sum_iF^\star_{(O_i,psp)}$
* The resulting joint force vector is
$\tau_{obstacles}=\sum_j J_{psp_j}^T(q)A(x)F_{psp_j}^\star$
* Specifying an adequate number of PSPs enables the protection of all of the manipulator’s parts.
* dynamic simulation for a redundant 4 dof manipulator operating in the plan

## Joint Limit avoidance
* The potential field approach can be used to satisfy the manipulator internal joint constraints.
* Corresponding forces for upper and lower joint constraints.
## Level of compitance.
* the complexity of tasks that can be implemented with this approach is limited.
* In a cluttered environment, local minima can occur in the resultant potential field. This can lead to a stable positioning of the robot before reaching its goal.
* While local procedures can be designed to exit from such configurations.
* linking local minima can overcome this problem but it is computationaly very heavy and beyond the real time control.
## Real time implimentation:
* the global control system integrating the potential field concept with the operational space approach has the following structure:
$\tau=\tau_{motion}+\tau_{obstacle}+\tau_{joint limit}$
* $\tau_{motion}=J^T(q)A(q)F_m^\star+\hat B(q)[\dot q\dot q]+\hat C(q)[\dot q^2]+g(q)$
* $\hat B(q)\to$ Coriolis forces
* $\hat C(q) \to$ Centrifugal forces
* $g(q)\to$ gravity force vector
* In this control structure, dynamic decoupling of the end effector is obtained using the end-effector dynamic parameters (EEDP) $A(q),\hat B(q),\hat C(q) and g(q)$ which are configuration dependent.
* In real time, these parameters can be computed at a lower rate than that of the servo control.
* In addition, the integration of an operational position and velocity estimator allows a reduction in the rate of end-effector position computation, which involves evaluations of the manipulator geometric model, leads to two level control
* A low rate parameter evaluation level that updates
the end-effector dynamic coefficients, the
Jacobian matrix, and the geometric model.
* A high rate servo control level that computes the
command vector using the estimator and the
updated dynamic coefficient.
## Summery:
* Described formulation and implimentation of real time obstacle avoidance using artififcial potential field.
* Collision avoidance generally treated as high level conrtrol problem, but in this paper it is demonstrated as effective component of low level real-time control.
* presented operational space formulation of manipulator control which is basis of obstacle avoidance approach.
* The integration of this low level control approach
with a high level planning system seems to be one of
the more promising solutions to the obstacle avoidance
problem in robot control. problen may be treated as:
* High level control(generating global strategy)
* low level control (producing commands to attain each goal points produced by global strategy)
* extending low level control capabilities and reducing the high level path planning burden can improve the real time performance.