Vector Field Histogram Method to Avoid Obstacle

Today, I Show a method to find a path by using Vector Field Histogram. Currently, My algorithm is perfectly finished because there are some problems. For example, this algorithm can find a path in special case and there is no auto stop function.

figure.1

First, Look at this figure.1. You can find red points that make a path by connecting each point.
Each point is found by using relation between vector field and obstacles. At each point, we can find free direction. Look at Next figure. we assume that robot can scan surroundings(0~2*pi, unit: radian). And then, we can get pixel information about obstacle.
Finally, if obstacle region that is taken by the information is subtracted from total region. we can get free region.

figure.2

In the free region, if a next position of robot to reach a goal position is well made logically,  robot will reach the goal position without doubt. ( I have not found this way yet). I only use relation between free region and vector made by goal and center of robot.

Verifying the rotation matrix from quaternion

Reference
1.http://en.wikipedia.org/wiki/Triple_product
2.http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

Basic concepts of wheel kinematics

figure.1 :link( http://www.asl.ethz.ch/education/master/mobile_robotics/year2007/S_3a_-_Kinematics_Wheel.pdf )

To take rolling condition, we must know that body motion is determined the velocity of wheel. Therefore, when we transform the velocity of body about reference frame into wheel fixed frame, the velocity must equal wheel's velocity.

To take "No Slip Condition", we must aware that wheel can not move with axial direction of wheel. Hence if we project the velocity of body into axial direction of wheel, the value must be zero.

Applying the wheel motion kinematics

Lets take a kinematic of upper system.

figure.1

From this equation and system condition, we can get the total kinematic equations

Lets try to get  kinematics of other systems

System.1 (type(3,0)

 System.2 (type(2,1))

System.3 (type(1,2))

trajectory generation solution

When we get a one path(or trajectory) in the joint space, we sometimes use linear function with parabolic blends with via points. And then How to make one trajectory in the below figure.1

figure.1

First, we must aware that start segment and last segment are different with interior segments because of a region of end blend segment. For example, when we make path in the interior points(j, k), we will use only region between tj and tk. However, when we make  one trajectory in the start , we use region between ti and tk as well as t0(time 0).

End segment trajectory can be taken by similar concept.

 Therefore, we use next equation set for interior regions

 eq.4                               eq.5

eq.6                               eq.7

In the start and last segments, we use following equations

......eq.1

First equation means that the velocity of the end of the blend region must have same velocity of the linear section.

......eq.2

Equation.2 shows that when we make blend region with second order polynomial equation and constant acceleration value.

.......eq.3

From eq,1 and eq.2, we can get eq.3 with some skills.

By combining eq.3 and the concepts of interior equation set, we can get equations to make trajectories for the start and last segments. Next equations show that.

Free configuration space at the simple system

Today, I will show you  configuration spaces in the two link systems ; RR and RP.

figure.1

First RR system has two configuration space variables ; theta1, theta2 (unit: radian)

figure.2

Second RP system has two configuration space variables: theta1 , D(Distance) (unit: radian, meter, respectively)

figure.3

If we want to map the obstacle in the figure.1 onto each configuration space (figure.2 & figure.3 ), What I do?

First, in the RR system, it is possible that collision between obstacle and link.1 or collision between obstacle and link.2. In the first case, if collision between obstacle and link.1 is happen,  all positions of link.2 with all angles are impossible. In the second case, if a distance between second joint and center of obstacle is smaller than length of link.2, there are impossible angle sets.

According to the upper concepts, we can write plot script for finding free configuration space. Next figure is result.

(code.1: https://docs.google.com/open?id=0B7RItyl-gDWqaFBPbkJTR3oxQTA )

By  thinking similar concept that is used in the RR system, We can get a free configuration set in the RP system. Next figure is result

(code.2: https://docs.google.com/open?id=0B7RItyl-gDWqRGNZcVRGS3ZMX0k)