Solving a System of Trig Equations

107 次查看（过去 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 个评论
显示 3更早的评论
Michael Boyte
Michael Boyte 2022-6-18
This is part of an equation to solve for the joint angles of a serial robot. I know where the end of the robot is. The x- and y-coordinates of the end of the robot are the two equations I have. The z-coordinate has already been solved and plays no role in this part. There are only two revolute joints in this robot. So when I have a given position, which again are represented by the two equations I have, there are two sets of joint angles that can give me that position. I am able to use another equation, which does not have theta1 in it, to solve for two different theta2's. Each of those theta2s should have a corresponding theta1 that when combined, will give me the position. For example, in one of my tests, I gave theta1 a value of 45 degrees and theta2 a value of 60 degrees. Those values, along with the constants, gave me a particular position as a real value. When I used that real value, I ended up with two possible theta2's, the 60 degrees I gave it, and -60 degrees. I know logically that a solution for theta1 give theta2 = 60 degrees is 45 degrees. For theta2 = -60, theta1 must be -45 degrees. Unfortunately I can't seem to solve for theta1 generally. Did that answer your question?
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
Ran in:
Hi @Michael Boyte
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 .
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 .
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.
https://in.mathworks.com/help/symbolic/solve-a-system-of-linear-equations.html?searchHighlight=solving%20linear%20equations&s_tid=srchtitle_solving%2520linear%2520equations_1
  2 个评论
Walter Roberson
Walter Roberson 2022-6-18
If you restrict the variables to real values then solve will return empty.
John D'Errico
John D'Errico 2022-6-18
They are NOT linear equations, because theta1 is the unknown.

请先登录，再进行评论。

Torsten
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);
John D'Errico
John D'Errico 2022-6-18
编辑：John D'Errico 2022-6-18
Ran in:
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]
theta1sol.conditions
ans = 
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
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
Ran in:
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 Exchange 中查找有关 Code Generation 的更多信息

标签

产品

Community Treasure Hunt

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

Start Hunting!

Translated by