Solving a System of Trig Equations
66 次查看(过去 30 天)
显示 更早的评论
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
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.
回答(5 个)
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:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037370/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037375/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037380/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037385/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037390/image.png)
Given the desired end-effector pose
and the second joint configuration
is pre-determined, we want to calculate the first joint configuration
.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037395/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037400/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037405/image.png)
Step 1:
So, we start with
. From the angle sum trigonometric identities,
can be rewritten as
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037380/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037380/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037420/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037425/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037430/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037435/image.png)
Similarly,
can also be rewritten as
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037390/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037445/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037450/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037455/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037460/image.png)
Now, we can put
and
in a more compact form ... the MATRIX form:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037435/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037460/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037475/image.png)
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
.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037405/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037485/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037490/image.png)
The determinant can be simplified to
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037495/image.png)
At this point, we can write the two formulas as
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037500/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037505/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037510/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037515/image.png)
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.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037405/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037525/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037530/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037535/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037540/image.png)
Note that the real solutions only exist if the following conditions are met:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037545/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037550/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037555/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037560/image.png)
with the obvious
and
.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037565/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037570/image.png)
Example:
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
.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037575/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037580/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037585/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037590/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037405/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037530/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037540/image.png)
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)
theta1b = asin(num2/den) % Eq.(8)
In degree, the solution
. Because of the periodic nature of the trigonometric functions in
and
,
actually has an infinite number of solutions given by
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037610/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037505/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037515/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037405/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037630/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1037635/image.png)
0 个评论
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 个评论
Walter Roberson
2022-6-18
If you restrict the variables to real values then solve will return empty.
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);
x^2-2*x*a1*cos(theta1)+a1^2*cos(theta1)^2-a2*(cos(theta1)^2*cos(theta2)^2+...
sin(theta1)^2*sin(theta2)^2) = -2*a2*cos(theta1)*cos(theta2)*sin(theta1)*sin(theta2);
(x^2-2*x*a1*cos(theta1)+a1^2*cos(theta1)^2-a2*(cos(theta1)^2*cos(theta2)^2+...
sin(theta1)^2*sin(theta2)^2))^2 = 4*a2^2*cos(theta1)^2*cos(theta2)^2*sin(theta1)^2*sin(theta2)^2;
(x^2-2*x*a1*cos(theta1)+a1^2*cos(theta1)^2-a2*(cos(theta1)^2*cos(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);
0 个评论
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.conditions
So, under some rather nasty set of conditions, that will depend on the constants in your problem, we have two solutions, found in:
theta1sol.theta1
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)
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)
0 个评论
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)]
sol1 = simplify(solve(eqn(1), theta1))
sol2 = simplify(solve(eqn(2), theta1))
d = simplify((sol1-sol2.'))
T1 = subs(d, [x,a1,a2,theta2],[0.25 .5 .5 pi/3])
arrayfun(@(E) simplify(solve(E,y)), T1, 'uniform', 0)
cc1 = subs(eqn, [x,a1,a2,theta2,y], [0.25 .5 .5 pi/3, sqrt(sym(11))/4])
cc1t1 = solve(cc1(1), theta1)
cc1t2 = solve(cc1(2), theta1)
T2 = subs(d, [x,a1,a2,theta2],[1.5 .5 .5 pi/3])
arrayfun(@(E) simplify(solve(E,y)), T2, 'uniform', 0)
cc2 = subs(eqn, [x,a1,a2,theta2,y], [1.5 .5 .5 pi/3, sqrt(sym(6)*1i)/2])
cc2t1 = simplify(solve(cc2(1), theta1), 'steps', 50)
cc2t2 = simplify(solve(cc2(2), theta1), 'steps', 50)
vpa(cc2t1 - cc2t2.')
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
0 个评论
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Applications 的更多信息
产品
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!