AXY:Element Coordination/Multi Dimensional Tracking Algorithm (Orientation)

Multi Dimensional Tracking Algorithm

This algorithm is used for multi-dimensional robot based MF tracking.
In this document position-tracking is presented.
Orinetation is considered to be a 3-dimensional vector with it's derivatives (velocity, acceleration, jerk): ${\vec {e}}$ , ${\vec {d}}e$ , ${\vec {d}}de$

${\vec {e}}=(yaw,pitch,roll)$

The algorithm consist of a state machine with the following states:

1. Tracking process
2. Full Synchronization follow the master
3. Stopping (de-synchronization process)

State: Full Synchronization follow the master

• Check if master velocity or acceleration exceeds max master frame's values if yes return an error

• ${\overrightarrow {tack}}:={\overrightarrow {master}}$ copying all derivatives

• reset: flopstime = flops = 0

State: Tracking process

Initial Setup and testing

Value of AccMaxRot is used of checking of acceleration limit.

Value of VelMaxRot is used of checking of velocity limit.

• Check if master velocity or acceleration exceeds max master frame's values if yes return an error
  

• Compute differences:
  

${\overrightarrow {ori}}_{diff}={\overrightarrow {master}}_{ori}-{\overrightarrow {track}}_{ori}$    pure delta-orientation
${\overrightarrow {vel}}_{diff}={\overrightarrow {master}}_{vel}-{\overrightarrow {track}}_{vel}$    pure delta-velocity

Time-to-Reach initial estimation

${sync_{jrk}}$ defined as SyncJerkRot

${sync_{acc}}$ defined as SyncAcclerationRot

This is an estimation of time needed to reach the target. It is not an exact formula
if the slave velocity is lower then master's we need more time: once to get to a higher velocity then the masters and once
to get from that (higher) velocity to the masters - therefore the multiplication by 2

$time_{toreach}={\sqrt {\frac {K*|{{\overrightarrow {vel}}_{diff}}|}{sync_{jrk}}}}+{\sqrt {\frac {4*|{{\overrightarrow {ori}}_{diff}}|}{sync_{acc}}}}$

where $K={\begin{cases}4,&{\mbox{if }}|{\overrightarrow {master}}_{vel}|>|{\overrightarrow {track}}_{vel}|\\2,&{\mbox{if }}|{\overrightarrow {master}}_{vel}|\leq |{\overrightarrow {track}}_{vel}|\end{cases}}$

DampingFactor - user defined by: MF.DAMPINGFACTOR

Additionaly we mutliply by a prediction factor - to decrease acc/jerk
(rounding to integer number of samples):

time_to_reach *= DampingFactor
time_to_reach = T*int(time_to_reach/T+1) 

Prediction of the Rendezvous-Point

The predicted orientation of the rendezvous (minus T, if it will happen in this sample!)
${\overrightarrow {pred}}_{ori}$:  orientation prediction assuming constant acceleration
${\overrightarrow {pred}}_{diff}$: Predicted orientation difference
${\overrightarrow {pred}}_{vel}$:  velocity prediction

$t=(time_{toreach}-T)$
${\overrightarrow {pred}}_{ori}={\overrightarrow {master}}_{ori}+t*{\overrightarrow {master}}_{vel}+t^{2}*{\overrightarrow {master}}_{acc}/2$
${\overrightarrow {pred}}_{diff}={\overrightarrow {pred}}_{ori}-{\overrightarrow {track}}_{ori}$
${\overrightarrow {pred}}_{vel}={\overrightarrow {master}}_{vel}+t*{\overrightarrow {master}}_{acc}$

Assuming the acceleration will be zero:
${\overrightarrow {pred}}_{acc}=0$

Polynomial coefficients of 5-degree polynom connecting the current p,v,a to the randevous p,v,a
${\overrightarrow {a}}0={\overrightarrow {track}}_{ori}$ - not used, directly written into code
${\overrightarrow {a}}1={\overrightarrow {track}}_{vel}$ - not used, directly written into code
${\overrightarrow {a}}2=0.5*{\overrightarrow {track}}_{acc}$ - not used, directly written into code
${\overrightarrow {a}}3=(0.5*({\overrightarrow {pred}}_{acc}-3*{\overrightarrow {track}}_{acc})~+~(10*{\overrightarrow {pred}}_{diff}/time_{toreach}$$-~6*{\overrightarrow {track}}_{vel}~-~4*{\overrightarrow {pred}}_{vel})/time_{toreach})/time_{toreach}$
${\overrightarrow {a}}4=(((5*{\overrightarrow {pred}}_{diff}/time_{toreach}~-~({\overrightarrow {pred}}_{vel}+4*track_{vel}))/time_{toreach}$$-1.5*{\overrightarrow {track}}_{acc})/time_{toreach}~-~2*{\overrightarrow {a}}3)/time_{toreach}$
${\overrightarrow {a}}5=(((({\overrightarrow {pred}}_{diff}/time_{toreach}~-~{\overrightarrow {track}}_{vel})/time_{toreach}$$-0.5*{\overrightarrow {track}}_{acc})/time_{toreach}~-~{\overrightarrow {a}}3)/time_{toreach}~-~{\overrightarrow {a}}4)/time_{toreach}$

Next Sample Orinetation

The orinetation at the next sample will be:
Initial increment (full displacement): ${\overrightarrow {dp}}={\overrightarrow {a}}5*T^{5}+{\overrightarrow {a}}4*T^{4}+{\overrightarrow {a}}3*T^{3}+0.5*{\overrightarrow {track}}_{acc}*T^{2}+{\overrightarrow {track}}_{vel}*T$

The accleration and jerk increments could be computed in this way also.
But they are computed as average values for the time of sample duration, this approach doeas fit more to
the tools we are using to test this feature, (BMDS...) as they all compute accleration & jerk by numeric differentiation
inside one sample.
The side effect of all this is that we can have a change in jerk/acc sign inside one sample and the average value we are using
ddp,dddp could have an opposite sign, so we are seeing effects like jerk or accleeration is limited from a "wrong side" i.e. instead
using jmax -jmax is used. There is not much we can do against it, using exact jerk/acc values (from polynom) returns than wrong avergae values
we observe as jerk/acc exceed over theri max values. So the avergae value approach is not perefect but gives results within the given limits.

Initial setting of predicted time and the filter limit flag (if the filter did work - this flag will be on)

limit        = false 
state.satVel = false 
state.satAcc = false  
state.satJrk = false 

Velocity, Acceleration and Jerk Filter

sync_vel defined as VelSyncRot

sync_acc defined as AccSyncRot

sync_jrk defined as JrkSyncRot

T is sampling time

${\overrightarrow {de}}={\begin{cases}sync_{velT}\cdot {\overrightarrow {de^{*}}},&{\mbox{if }}|{\overrightarrow {de}}|>sync_{velT}\\limit=true\\{\overrightarrow {de}},&{\mbox{if }}|{\overrightarrow {de}}|\leq sync_{velT}\end{cases}}$

where ${\vec {a}}^{*}={\vec {a}}/|{\vec {a}}|$ i.e. unit vector

Setting Initial velocity delta

${\overrightarrow {dde}}={\overrightarrow {de}}-{\overrightarrow {de_{0}}}$

${\overrightarrow {dde}}={\begin{cases}sync_{acc}T^{2}\cdot {\overrightarrow {dde^{*}}},&{\mbox{if }}|{\overrightarrow {dde}}|>sync_{acc}T^{2}\\{\overrightarrow {de}}=sync_{acc}T^{2}\cdot {\overrightarrow {dde^{*}}}+{\overrightarrow {de_{0}}}\\limit=true\\{\overrightarrow {dde}},&{\mbox{if }}|{\overrightarrow {dde}}|\leq sync_{acc}T^{2}\end{cases}}$

Setting Initial acceleration delta

${\overrightarrow {ddde}}={\overrightarrow {dde}}-{\overrightarrow {dde_{0}}}$

${\overrightarrow {ddde}}={\begin{cases}sync_{jrk}T^{3}\cdot {\overrightarrow {ddde^{*}}},&{\mbox{if }}|{\overrightarrow {ddde}}|>sync_{jrk}T3\\{\overrightarrow {dde}}=sync_{jrk}T^{3}\cdot {\overrightarrow {ddde^{*}}}+{\overrightarrow {dde_{0}}}\\{\overrightarrow {de}}={\overrightarrow {dde}}+{\overrightarrow {de_{0}}}\\limit=true\\{\overrightarrow {ddde}},&{\mbox{if }}|{\overrightarrow {ddde}}|\leq sync_{jrk}T^{3}\end{cases}}$

New orientation is:

$track_{ori}=track_{ori}+de$

If the filter was activated then:

if the velocity,accleration or jerk is limited (limit = true) then the velocity computed using polynomials does
${\overrightarrow {track}}_{vel}={\overrightarrow {de}}/T$
${\overrightarrow {track}}_{acc}={\overrightarrow {dde}}/T^{2}$

Velocity & Accleration will be computed exactly not using de and dde, it has been shown that this approach is much better - means more stable

${\overrightarrow {track}}_{vel}=5*{\overrightarrow {a}}5*T^{4}+4*{\overrightarrow {a}}4*T^{3}+3*{\overrightarrow {a}}3*T^{2}+{\overrightarrow {track}}_{acc}*T+{\overrightarrow {track}}_{vel}$
${\overrightarrow {track}}_{acc}=20*{\overrightarrow {a}}5*T^{3}+12*{\overrightarrow {a}}4*T^{2}+6*{\overrightarrow {a}}3*T+{\overrightarrow {track}}_{acc}$

keep the old values for next sample

${\overrightarrow {dde}}_{0}={\overrightarrow {dde}}$
${\overrightarrow {de}}_{0}={\overrightarrow {de}}$

System is synchronized if NEW delta's are under certesian thresholds:

${\overrightarrow {dv}}_{0}={\overrightarrow {dv}}$; - keep the previous dv
$dv={\overrightarrow {master}}_{vel}-{\overrightarrow {track}}_{vel}$

syncGain defined by the user with: MF.FilterFactor

Successful tracking completion

if   $(|{\overrightarrow {dv}}|\leq syncGain*sync_{acc}T)$  and  $(|{\overrightarrow {master}}_{ori}-{\overrightarrow {track}}_{ori}|\leq syncGain*sync_{acc}T^{2})$
Synchronization
Take the middle for an additional smoothing:
${\overrightarrow {track}}_{ori}=0.5*({\overrightarrow {track}}_{ori}+{\overrightarrow {master}}_{ori})$
${\overrightarrow {track}}_{vel}=0.5*({\overrightarrow {track}}_{vel}+{\overrightarrow {master}}_{vel})$

ResetFilter();		// Reset the filter
state.tracking     =    MOT_MASTER_SYNC; // System synchronized<br>

else if( dv0*dv < 0)
Check against over-oscillation

if (flops == -1) flops = 0; // ignore the first sample
else	         flops++;
flopstime = 0;

Over-Oscillation Check

maxFlops defined by the user with: MF.MaxFlops

if (flops > maxFlops && (flopstime > maxFlopsTime && fabs(dv) > syncGain * sync_accT2))

state.tracking 	= MOT_MASTER_OSC; // OScilationss

else

flopstime++;

State: Stopping (de-synchronization process)

flopstime = flops = 0;

• De-Syncing profile, follow the given profile, ignore the real source

• determine the curvature parameters path:

${\overrightarrow {desync}}_{start}$ , ${\overrightarrow {desync}}_{normal}$, ${\overrightarrow {desync}}_{center}$

or linear movement:

${\overrightarrow {desync}}_{direction}$

• initialize stopping profile with current values of curvature angle:

desync.pos = $\alpha$
desync.vel = ${\dot {\alpha }}$
desync.acc = ${\ddot {\alpha }}$

See: Theoretical_Background

• initialize the profiler for stopping - computing the stopping (deceleration path) ad set as new target position for the profiler.

• during stopping:

desync.Profile();

${\overrightarrow {p}}={\overrightarrow {desync}}_{start}\cdot cos(\alpha (t))+{\overrightarrow {desync}}_{normal}\cdot sin(\alpha (t))+{\overrightarrow {desync}}_{center}$

or in case of linear motion:

${\overrightarrow {p}}={\overrightarrow {desync}}_{direction}\cdot l(t)+{\overrightarrow {track}}_{pos}$

if(desync.path.Status.stop)			// if stopped,change the state to OFF
	 SetState(MOT_MASTER_OFF);