/* [wxMaxima batch file version 1] [ DO NOT EDIT BY HAND! ]*/ /* [ Created with wxMaxima version 13.04.2 ] */ /* [wxMaxima: comment start ] NOTE: I use the prefix "vec" to mark that a variable is a vector. For example, vecV would be a vector and V would be its modulus. [wxMaxima: comment end ] */ /* [wxMaxima: comment start ] First, we define the path we want the centre of the vehicle to follow. I chose tho define it in polar coordinates because it was handy to describe a spiral. [wxMaxima: comment end ] */ /* [wxMaxima: comment start ] /home/adria/Desktop/coords.png [wxMaxima: comment end ] */ /* [wxMaxima: comment start ] The coordinates of the centre of the vehicle, P, can be obtained by turning the polar form into cartesian: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ vecP:r(t)*matrix([cos(phi(t))],[sin(phi(t))]); /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] We can obtain the speed vector, VP, by differentiating against time: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ vecVP:diff(vecP,t); /* [wxMaxima: input end ] */ /* [wxMaxima: input start ] */ eq0:VP=trigsimp(ratsimp(sqrt(vecVP[1][1]^2+vecVP[2][1]^2))); /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] Let's keep this aside and deal with the vehicle now. The vehicle has two wheels, the direction of the axes of the wheels are fixed to the chassis. If the wheels don't slide, their centres can only move perpendicular to their axes. The only way for the vehicle to maneuvre is to have the wheels spin at different speeds. Different combinations of wheel speeds give different linear and rotational speeds of the vehicle. [wxMaxima: comment end ] */ /* [wxMaxima: comment start ] /home/adria/Desktop/vehicle.png [wxMaxima: comment end ] */ /* [wxMaxima: comment start ] The speed of the centre of the vehicle, G: middle point between the wheels, can be written as: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ eq1:VG=(VL+VR)/2; /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] where VR and VL are the speeds of the centre of the right wheel and the left wheel respectively. [wxMaxima: comment end ] */ /* [wxMaxima: comment start ] The rotational speed of the vehicle can be written as: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ eq2:diff(theta(t),t)=(VR-VL)/d; /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] where d is the distance between the wheels. [wxMaxima: comment end ] */ /* [wxMaxima: comment start ] The vector of the speed of the centre of the vehicle is: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ vecVG:VG*matrix([cos(theta)],[sin(theta)]); /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] We want to solve the "inverse kinematics problem", that is: we want to find out how do we have to turn the wheels in order to follow the desired path. The "mechanism" we are analysing is non-holonomous, which makes it impossible to find a formula that gives the angle of the wheels as a function of the desired position. However, we can obtain a formula that tells us the speed of the wheels we need as a function of the speed of the vehicle. This can be done be equating VP and VG and solving for the speeds. The equations involve trigonometric functions, which normally makes this process quite painful. [wxMaxima: comment end ] */ /* [wxMaxima: comment start ] Before equations vecVP and vecVG, it is useful to apply a rotation to these vectors (we change their reference frame to a new one). The rotation matrix is defined as: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ R(theta):=matrix([cos(theta),sin(theta)],[-sin(theta),cos(theta)]); /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] I will call the new vectors "prima" with the suffix "p": [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ vecVGp:R(theta).vecVG; /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] This can be further simplified: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ vecVGp:trigsimp(vecVGp); /* [wxMaxima: input end ] */ /* [wxMaxima: input start ] */ vecVPp:R(theta).vecVP; /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] If we equate the second component, "y", of VGp and VPp, we obtain the tangent of theta as a function of the polar coordinates of the desired path and their first derivatives: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ eq3:vecVGp[2][1]=vecVPp[2][1]; /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] The tangent can be obtained by solving for cos(theta)/sin(theta) in the previous equation. [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ tempsin:coeff(rhs(eq3),cos(theta),1); /* [wxMaxima: input end ] */ /* [wxMaxima: input start ] */ tempcos:-coeff(rhs(eq3),sin(theta),1); /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] tan(theta) would be tempsin/tempcos but rather than solving for the tangent, it is more useful for us to find the sine and the cosine themselves. [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ tempsincos:trigreduce(ratsimp(sqrt(tempsin^2+tempcos^2))); /* [wxMaxima: input end ] */ /* [wxMaxima: input start ] */ sol_sin_theta:tempsin/tempsincos; /* [wxMaxima: input end ] */ /* [wxMaxima: input start ] */ sol_cos_theta:tempcos/tempsincos; /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] We could now obtain the derivative of the angle by doing sqrt((d/dt sin(theta))^2+(d/dt cos(theta))^2). This leads to a long equation: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ trigsimp(ratsimp(sqrt(diff(sol_sin_theta,t)^2+diff(sol_cos_theta,t)^2))); /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] I must confess I became lazy at this point and instead of using this expression in my script, I decided to calculate the derivative numerically by first obtaining the angle doing the arctangent from the sine and the cosine. [wxMaxima: comment end ] */ /* [wxMaxima: comment start ] Going back to the inverse kinematics, once we know the speed of the centre of the vehicle (VG=VP) and the rotational speed of the vehicle (d/dt theta), we can obtain the speed of the centre of each wheel from equations eq1 and eq2: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ expand(solve([eq1,eq2],[VR,VL])); /* [wxMaxima: input end ] */ /* [wxMaxima: comment start ] Once we know the speed of the centre of the wheel, we can obtain the rotational speed of each wheel by dividing this by the radius of the wheel: [wxMaxima: comment end ] */ /* [wxMaxima: input start ] */ eq4:omega_r=VR/R_wheel; /* [wxMaxima: input end ] */ /* [wxMaxima: input start ] */ eq5:omega_l=VL/R_wheel; /* [wxMaxima: input end ] */ /* Maxima can't load/batch files which end with a comment! */ "Created with wxMaxima"$