Solving a System of Trig Equations

46 ビュー (過去 30 日間)
Michael Boyte
Michael Boyte 2022 年 6 月 18 日
編集済み: Sam Chak 2022 年 6 月 19 日
I have a system of two equations both with trig functions in them and I'm trying to solve for one variable. In this case, the equations are:
x = a2*cos(theta1+theta2) + a1*cos(theta1)
y = a2*sin(theta1+theta2) + a1*sin(theta1)
In these equations x, c, a1, a2, and theta2 are all known. The only unknown is theta1. I've tried several techniques and either I'm choosing the wrong methods are I'm doing them wrong. How can I solve this system of equations for theta1?
  5 件のコメント
Sam Chak
Sam Chak 2022 年 6 月 18 日
Thanks for your explanation, @Michael Boyte. No wonder that the problem looks difficult. It's an Inverse Kinematics problem. Is it possible to make use of the inverseKinematics() solver? The solver requires the Robotics System Toolbox.
If you have the Symbolic Math Toolbox, try solving for as advised by @Torsten in the Answer.


回答 (5 件)

Sam Chak
Sam Chak 2022 年 6 月 19 日
編集済み: Sam Chak 2022 年 6 月 19 日
Back to the basics when solving the inverse kinematics problem. The kinematics of a 2-link robot arm (where the lengths of the links {} are known), is governed by these two trigonometric equations:
... ... ...
... ... ...
Given the desired end-effector pose and the second joint configuration is pre-determined, we want to calculate the first joint configuration .
Step 1:
So, we start with . From the angle sum trigonometric identities, can be rewritten as
... ... ...
Similarly, can also be rewritten as
... ... ...
Now, we can put and in a more compact form ... the MATRIX form:
Step 2:
The matrix form is a System of Linear Equations. There are a few ways to solve the system and MATLAB can easily get this done. For educational purposes, let's continue to derive the formulas to calculate the first joint configuration .
The determinant can be simplified to
At this point, we can write the two formulas as
... ... ...
... ... ...
Step 3:
Either one of the formulas can be used to calculate the first joint configuration by taking the inverses of the cosine and sine.
... ... ...
... ... ...
Note that the real solutions only exist if the following conditions are met:
... ... ...
... ... ...
with the obvious and .
A 2-link robot arm, where the lengths are and , is used in this example. Given the desired end-effector pose and the second joint configuration is pre-determined, the first joint configuration can be calculated using the formula or .
a1 = 2/sqrt(3); % length of 1st link
a2 = (sqrt(3) - 1)/sqrt(3); % length of 2nd link
x = 1; % desired x-coordinate of end-effector pose
y = 1; % desired y-coordinate of end-effector pose
theta2 = pi/3; % 2nd joint config
den = a1^2 + a2^2 + 2*a1*a2*cos(theta2);
num1 = (a2*cos(theta2) + a1)*x + (a2*sin(theta2))*y; % from Eq.(5)
num2 = (a2*cos(theta2) + a1)*y - (a2*sin(theta2))*x; % from Eq.(6)
theta1a = acos(num1/den) % Eq.(7)
theta1a = 0.5236
theta1b = asin(num2/den) % Eq.(8)
theta1b = 0.5236
In degree, the solution . Because of the periodic nature of the trigonometric functions in and , actually has an infinite number of solutions given by
and , where is the set of integers.

Ayush Singh
Ayush Singh 2022 年 6 月 18 日
Hi Michael
From my understanding of the question you are trying to solve linear equations in a single variable.
You can refer to the article given below on how to solve it using "solve" function.
  2 件のコメント
John D'Errico
John D'Errico 2022 年 6 月 18 日
They are NOT linear equations, because theta1 is the unknown.


Torsten 2022 年 6 月 18 日
Use the symbolic toolbox to solve the last equation for cos(theta1).
It's a polynomial equation of degree 4 which will have 4 roots:
x = a2*cos(theta1+theta2) + a1*cos(theta1);
x = a2*(cos(theta1)*cos(theta2)-sin(theta1)*sin(theta2))+a1*cos(theta1);
(x-a1*cos(theta1))^2 = a2*(cos(theta1)*cos(theta2)-sin(theta1)*sin(theta2))^2;
x^2-2*x*a1*cos(theta1)+a1^2*cos(theta1)^2 = a2*(cos(theta1)^2*cos(theta2)^2-...
2*cos(theta1)*cos(theta2)*sin(theta1)*sin(theta2) + sin(theta1)^2*sin(theta2)^2);
sin(theta1)^2*sin(theta2)^2) = -2*a2*cos(theta1)*cos(theta2)*sin(theta1)*sin(theta2);
sin(theta1)^2*sin(theta2)^2))^2 = 4*a2^2*cos(theta1)^2*cos(theta2)^2*sin(theta1)^2*sin(theta2)^2;
(1-cos(theta1)^2)*sin(theta2)^2))^2 = 4*a2^2*cos(theta1)^2*cos(theta2)^2*sin(theta2)^2*(1-cos(theta1)^2);

John D'Errico
John D'Errico 2022 年 6 月 18 日
編集済み: John D'Errico 2022 年 6 月 18 日
If the only unknown is theta1, then what is y?
If you actually have a value for y, then you have two equations in one unknown variable, in which case there will generally be no exact solution for the pair of equations with one unknown. So I'll assume that is not the case.
If you do not know the value for y, then the second "equation" is completely irrelevant. You merely solve for theta1 from the first equation, since you know everything else.
syms x a2 a1 theta2 real
syms theta1 real
EQ = x == a2*cos(theta1+theta2) + a1*cos(theta1);
theta1sol = solve(EQ,theta1,'returnconditions',true)
theta1sol = struct with fields:
theta1: [2×1 sym] parameters: k conditions: [2×1 sym]
ans = 
So, under some rather nasty set of conditions, that will depend on the constants in your problem, we have two solutions, found in:
ans = 
The problem is, there are some sets of parameters x, a1 and a2 that will create problems, where only complex solutions exist. When k==0, we get what would normally be called the primary solutions.
vpa(subs(theta1sol.theta1,[x,a1,a2,theta2],[0.25 .5 .5 pi/3]),10)
ans = 
We can ignore the tiny imaginary part there in the second solution. as just floating point trash. As you can see, there are infinitely many solutions, separated by multiples of 2*pi.
But for other sets of the parameters, only complex solutions exist. For example:
vpa(subs(theta1sol.theta1,[x,a1,a2,theta2],[1.5 .5 .5 pi/3]),10)
ans = 

Walter Roberson
Walter Roberson 2022 年 6 月 18 日
syms a1 a2
syms theta1 theta2 x y
eqn = [x == a2*cos(theta1+theta2) + a1*cos(theta1)
y == a2*sin(theta1+theta2) + a1*sin(theta1)]
eqn = 
sol1 = simplify(solve(eqn(1), theta1))
sol1 = 
sol2 = simplify(solve(eqn(2), theta1))
sol2 = 
d = simplify((sol1-sol2.'))
d = 
T1 = subs(d, [x,a1,a2,theta2],[0.25 .5 .5 pi/3])
T1 = 
arrayfun(@(E) simplify(solve(E,y)), T1, 'uniform', 0)
ans = 2×2 cell array
{[-11^(1/2)/4]} {0×1 sym} {[11^(1/2)/4 ]} {0×1 sym}
cc1 = subs(eqn, [x,a1,a2,theta2,y], [0.25 .5 .5 pi/3, sqrt(sym(11))/4])
cc1 = 
cc1t1 = solve(cc1(1), theta1)
cc1t1 = 
cc1t2 = solve(cc1(2), theta1)
cc1t2 = 
T2 = subs(d, [x,a1,a2,theta2],[1.5 .5 .5 pi/3])
T2 = 
arrayfun(@(E) simplify(solve(E,y)), T2, 'uniform', 0)
ans = 2×2 cell array
{[-(6^(1/2)*1i)/2]} {0×1 sym} {[(6^(1/2)*1i)/2 ]} {0×1 sym}
cc2 = subs(eqn, [x,a1,a2,theta2,y], [1.5 .5 .5 pi/3, sqrt(sym(6)*1i)/2])
cc2 = 
cc2t1 = simplify(solve(cc2(1), theta1), 'steps', 50)
cc2t1 = 
cc2t2 = simplify(solve(cc2(2), theta1), 'steps', 50)
cc2t2 = 
vpa(cc2t1 - cc2t2.')
ans = 
So... for some combinations of x, a1, a2, theta2, there exists a y such that theta1 is equal between the two equations... but for other combinations, although you can calculate y, it does not lead to self-consistent outcomes


Help Center および File ExchangeProgramming についてさらに検索



Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by