My own understanding of velocity obstacle

velocity-obstacle

Key words: controllability, reachability, linear system Ax=b, and least-squares problem

First, we analyze the existance and uniqueness of the solutions to Ax=b

memo – solutions to Ax=b

Second, we show the controllability and reachability can be interpreted as Ax=b

memo -localizability as Ax=b

We often encounter rotating one vector. What if we rotate a reference frame to coincide with another reference frame? What will we get?

memo rotate one frame to the other

% compute the staircase form for observability analysis
clc;clear;
e1=[1; 0; 0];
e2=[0; 1; 0];
e3=[0; 0; 1];

vn=[0;0;0];
vn_sk=fcn_getSkew(vn);

a=-10*e3;
a_sk=fcn_getSkew(a);

A=[zeros(3)   eye(3)     zeros(3)   zeros(3)    zeros(3);
zeros(3)   zeros(3)   -a_sk      eye(3)       zeros(3);
zeros(3)   zeros(3)   zeros(3)   zeros(3)   eye(3);
zeros(3)   zeros(3)   zeros(3)   zeros(3)   zeros(3);
zeros(3)   zeros(3)   zeros(3)   zeros(3)   zeros(3)];

C=[zeros(3)   zeros(3)   -vn*e2'     zeros(3)   zeros(3);
zeros(3)   zeros(3)   vn*e1'      zeros(3)   zeros(3);
zeros(3)   eye(3)     -vn_sk      zeros(3)   zeros(3)];

B=zeros(15,1);

[Abar,Bbar,Cbar,T,k] = obsvf(A,B,C) ;

% use symbolic computation
syms px py pz vx vy vz phi tht psi bax bay baz bp bq br X
% X is the oringinal state vector
X=[px; py; pz; vx; vy; vz; phi; tht; psi; bax; bay; baz; bp; bq; br];
% TX is the transformed state vector, it is clear to find what states are
% observable
T*X

Given a nonlinear model: $\dot{x}=f(x)$

In practice, we need to both discretize and linearize the nonlinear model. The question is which procedure should be first?

(1) Method 1: Linearizing first, then discretizing

first linearizing: $\dot{x}=\frac{\partial f}{\partial x}x$

then discretizing: $x(t+1)=(I+\Delta t \frac{\partial f}{\partial x})x$

(2) Method 2: Discretizing first, then  linearizing

first discretizing: $x(t+1)=x(t)+\Delta t f[x(t)]$

then linearizing: $x(t+1)=(I+\Delta t \frac{\partial f}{\partial x})x$

Conclusions: the two methods are equivalent.

In robotics, holonomicity refers to the relationship between the controllable degrees of freedom and total degrees of freedom of a given robot (or part thereof).

1. Holonomic system:
If the controllable degrees of freedom is equal to the total degrees of freedom then the robot is said to be holonomic.
2. Non-holonomic system:
If the controllable degrees of freedom are less than the total degrees of freedom it is non-holonomic.
3. Redundant system:
A robot is considered to be redundant if it has more controllable degrees of freedom than degrees of freedom in its task space.

A redundant system is also called holonomic. But a holonomic system may not be redundant.

Non-holonomic example:

An automobile (a car) is an example of a non-holonomic vehicle. The vehicle has three degrees of freedom—its position in two axes, and its orientation relative to a fixed heading. Yet it has only two controllable degrees of freedom—acceleration/braking and the angle of the steering wheel—with which to control its position and orientation.

The resulting phenomenon is: the velocity of the car is always the same or inverse as the orientation of the car, if there is no skidding or sliding. Thus, not every path in phase space is achievable. The non-holonomicity of a car makes parallel parking and turning in the road difficult.

Remark: we just compare the number of DOF. A controllable state may not be one of the total state. For example, the state of a care is position and orientation angle. The controllable state is acceleration and orientation (input). The holonomic property is only determined by the number.

Holonomic & Redundant example:

A human arm, by contrast, is a holonomic, redundant system because it has seven (controllable) degrees of freedom (three in the shoulder – rotations about each axis, two in the elbow – bending and rotation about the lower arm axis, and two in the wrist, bending up and down (i.e. pitch), and left and right (i.e. yaw)) and there are only six physical degrees of freedom in the task of placing the hand (x, y, z, roll, pitch and yaw), while fixing the seven degrees of freedom fixes the hand. See also sub-Riemannian geometry for a discussion of holonomic constraints in robotics.

Referencehttp://en.wikipedia.org/wiki/Holonomic

Edit: The holonomic, non-holonomic and redundant  look the same as fully-actuated, under-actuated and over-actuated.

Edit: 2015-10-05

Useful fact: if $A\ge B$, then $\sigma_{\min}(A)\ge \sigma_{\min}(B)$ and $\sigma_{\max}(A)\ge \sigma_{\max}(B)$

Proof: For the eigenvector x1 that associated with the smallest eigenvalue of A

$\sigma_{min}(A)=x_1^TAx_1\ge x_1^TBx_1\ge \sigma_{min}(B)$

For the eigenvector yn that associated with the largest eigenvalue of B

$\sigma_{max}(B)=y_n^T B y_n\le y_n^TAy_n\le \sigma_{max}(A)$

Remark: it is not correct that $\sigma_{min}(A)\ge \sigma_{max}(B)$. Counterexamples can be easily found if you consider the geometric interpretation of the ellipsoids x^T*A*x=1 and x^T*B*x=1.

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Edit: 2015-09-23

Fact 1.
If $x^TAx=1$, then it defines an ellipsoid.
If $x^TAx<1$, then the vector x is inside the ellipsoid.
If $x^TAx>1$, then x is outside the ellipsoid.

Proof: suppose $x^T Ax<1$. Then there exists $k^2=1/(x^TAx)$ such that $(kx)^TA(kx)=1$. Therefore, $kx$ is on the surface of the ellipsoid and since $k>1$, we know $x$ is inside the ellipsoid.

Fact 2. For any PD matrices A and B, if A>B (i.e., A-B>0), then $x^TAx=1$ is contained inside of $x^TBx=1$.

Remark: The more positive A is, the smaller the ellipsoid it is. Roughly speaking, that is because the length of each semi-axis is the inverse of the eigenvalue.
Remark: note A and B may have different eigenvectors. So if you plot the two ellipsoids, they are not parallel.
Remark: you cannot naively say that the singular values of A are larger than those of B even if A>B. This can be intuitively seen by plotting the two ellipsoid. But we can say the largest (smallest) eigenvalue of A is larger than the largest (smallest) one of B. This can be seen from the plot and also proved. We cannot say the smallest eigenvalue of A is larger than then maximum eigenvalue.

Proof: If $x^TAx=1$, then x is on the surface of the ellipsoid of A. Then $x^TBx=x^TAx-x^T(A-B)x. Hence x is inside of the ellipsoid of B. Hence any point on the ellipsoid of A is inside the ellipsoid of B.

=====================================================

$x^T A x=1$ represents an ellipsoid.

1. The axis of the ellipsoid is along the eigenvectors of A.
2. The length of the semi-axis is $1/\sqrt{\lambda_i}$.
3. The volume of the ellipsoid is $\frac{4}{3}\pi \frac{1}{\sqrt{\lambda_1 \lambda_2 \lambda_3}}$.

Here is a detailed memo.