How to find 2 angles with this trigonometric system of 3 equations ?

[lolo067]

Hello,

I'm new on this forum. I’m solving a robotics problem (pan-tilt robot) using the Denavit-Hartenberg convention. I end up with a system of trigonometric equations, where Θ1 and Θ2 are the 2 unknown angles. x, y, z, a and d are known. This is this system of 3 equations :
[MATH]x = a \times cos\theta_{1} \times cos\theta_{2} \\ y= a \times sin\theta_{1} \times cos\theta_{2} \\ z= a \times sin\theta_{2} + d [/MATH]Can anybody find Θ1 and Θ2 ?
Thank you in advance ☺

 

[Dr.Peterson]

To eliminate θ₁, square the first two equations and add them together.

To eliminate θ₂, subtract d from the third equation, square both sides, and add it to the one you just made by adding.

 

[lolo067]

Thank you very much for your reply Mr. Dr. Peterson.
This is what I did.
My equations system at the start :
[MATH]E_1 : x = a_2 \times cos\theta_{1} \times cos\theta_{2} \\ E_2 : y = a_2 \times sin\theta_{1} \times cos\theta{2} \\ E_3 : z = a_2 \times sin\theta_{2} + d_1 [/MATH]Raising to square E1 and E2 :
[MATH]E_4 : x^2 = a_{2}^2 \times cos^2\theta_1 \times cos^2\theta_2 \\ E_6 : y^2 = a_{2}^2 \times sin^2\theta_1 \times cos^2\theta_2 \\ \rightarrow E_5 : x^2+y^2 = a_{2}^2 cos^2\theta_2 (cos^2\theta_1 + sin^2\theta_1) = a_{2}^2 cos^2\theta_2[/MATH]So :
[MATH] \theta_{2} = acos \sqrt{ \frac{x^2+y^2}{a_{2}^2} }[/MATH]Next step :
[MATH] E_3 \leftrightarrow z-d_1 = a_2 \times sin\theta_2 \\ (z-d_1)^2 = a_{2}^2 \times sin^2\theta_2[/MATH]Then taking back E6 :
[MATH]\frac{y^2}{sin^2\theta_1} = a_{2}^2 \times cos^2\theta_2[/MATH]By adding E3 and E6 :
[MATH](z-d_{1}^2) + \frac{y^2}{sin^2\theta_1} = a_{2}^2 sin^2\theta_2 + a_{2}^2 cos^2\theta_2 = a_{2}^2 \\ \frac{y^2}{a_{2}^2-(z-d_1)^2} = sin^2\theta_1 \\ \theta_1 = \pm asin \sqrt { \frac{y^2}{a_{2}^2-(z-d_1)^2} }[/MATH]Feel free to tell me if there are any errors.
Thank you again Mr. Dr. Peterson !

 

[Dr.Peterson]

I'm sorry. I had missed what your goal was, and was distracted by the fact that the known variables are not independent. If you add these,

[MATH]x^2+y^2 = a_{2}^2 cos^2\theta_2[/MATH]​

[MATH](z-d)^2 = a_{2}^2 sin^2\theta_2[/MATH]​

you find that

[MATH]x^2+y^2+(z-d)^2 = a_2^2[/MATH]​

So if you know any four of x, y, z, a, d, you can find the other. (What is [MATH]d_1[/MATH]?) This may be important as a check; or it may be useful to simplify something.

Anyway, your work is all valid, but doesn't fully determine either angle, depending on their domains, because squaring lost information about the signs of the trig functions, and asin doesn't distinguish between acute and obtuse. If you know the angles are positive and acute, there is no issue.

To find \theta_2, I would just solve E3.

To find \theta_1, you can divide E2 by E1 and use the atan.

I haven't given enough thought to how to determine what quadrant each is in, or if they are ambiguous.

 

[lolo067]

Hello Mr. Dr. Peterson,
Yes, I do agree with you : I have to ask myself each time in which quadrant I am, to get a single angle. Input data allow me to do that; it's OK.
The attach file represents what are d1 and a2. I have to target the point M. The point P is the projection of M on the end organ of the robot. Writing these lines, I become aware of that the coordinates x, y and z in my starting system of eaquations are the coordinates of P, not of M ! And input data are the coordinates of M...

 

[lolo067]

The property that the cross-product O1P X O1M = 0 may help me ? (please see the attached file to see the position of the point O1 ; coordinates of O1 are known)

 

[Dr.Peterson]

As I understand it, OM is simply a multiple of OP; you could find OM by dividing OP by its length to make a unit vector, then multiply by a2 to find OM. But you don't need to do all that; just divide the y coordinate of OP by its x coordinate to find the tangent of theta1 (if I've got that right); in fact, you can use the atan2 function and get the right quadrant. Then do something similar for the other angle.

 

[lolo067]

Hello Mr. Dr. Peterson,
Thank you for your idea, which does work. This is how I implemented that in Matlab :

%Expressions of theta1 and theta2 gotten with 
%DH's convention and vectors O1M and O1P which are 
%aligned 

%theta1=q(1,2) and theta2=q(2,2) are between -pi and pi
%so let's use the atan2 function (4 quadrants managed)
%CAUTION with the order of the arguments !

%Note : cartesian coordinates of M are in the column vector M

q(1,2)=atan2(M(2),M(1));
q(2,2)=atan2( -(d1+M(3)) , cos(q(1,2))*M(1) + sin(q(1,2))*M(2) );

Thank you very much ?

Best regards