First it is helpful to remember a position on a circle can be described parametrically by:
[x,y] = [r*cos(theta), r*sin(theta)]
or alternatively, r =sqrt( x^2 + y^2).
- As a circular variation of the standard 2D particle, we just add an angular velocity (omega) and angular orientation (theta). For a standard 2D particle we can describe its velocity (v) in terms of acceleration (a) and a delta in time (dt) (v += a*dt; x+= v*dt). We can modify this to include the parametric circle representation:
theta += omega * dt; x+=v*dt*cos(theta); y+=v*dt*sin(theta);
- The problem with the above method is that it is in-exact and relies on an integrator. The 2D velocity/angular velocity representation can easily be fully solved analytically. The radius of a circle produced by a given velocity (v) and angular velocity (omega, or w) is:
r = | v / w |
We can modify the parametric representation to include an offset for the centre of motion:
[x,y] = [r*cos(theta) + xc, r*sin(theta) + yc]
Thus, given an initial x,y,theta we can find the centre of the circle of motion as:
xc = x - (v/w)*sin(theta) yc = y + (v/w)*cos(theta)
Now we can update the position of the robot:
theta += w * dt; x = xc + (v/w) sin(theta) y = yc - (v/w) cos(theta)
We can expand this into a single line as:
x+= -(v/w)*sin(theta) + (v/w)*sin(theta+w*dt) y+= -(v/w)*cos(theta) - (v/w)*cos(theta+w*dt) theta += omega * dt;
Which is the form many robotics papers use.
- Finally, we may wish to represent the motion in terms of two given x,y coordinates and solve for the translation and rotation required to reach them (as is the case for odometry calculations). In this case, we can represent any movement from an original set of coordinates [x,y,theta] to a new set [x',y',theta'] as first a rotation and translation to bring you to any new x' and y', followed by another rotation to bring you to the final theta'. Using triangle trigonometry this is:
deltaRot1 = atan2(y'-y,x'-x) - theta deltaTrans = sqrt( (x-x')^2 + (y-y')^2 ) deltaRot2 = theta' - theta - deltaRot1
1 comment:
hi, do you have an algorithm to test it?? sahbinet@hotmail.com
Post a Comment