查找方程组的解,使其与先前的解尽可能接近

时间:2019-05-16 09:16:16

标签: matlab equation-solving

当前,我正在使用vpasolve命令来求解包含3个未知数的3个方程组。这3个方程式正用于对机器人机械手的运动进行建模,未知数要么是机器人连杆的长度,要么是关节的旋转角度。 vpasolve命令设法正确地解决方程,问题是我得到的解决方案不紧密。我不亲密是什么意思?如果我对其中一个角度获得了1.5、0.3,-0.4 [rad]的解,则下一步运动的下一个解决方案是可以解决该问题的任何其他随机角,但实际上没有用,因此可能是-0.8 -1.5 2 [rad]。当然,机器人将其端点从1 rad移到1.5 rad到-0.8 rad毫无意义。相反,我想获得一个与上一个解决方案尽可能接近的解决方案。

方程看起来像这样:

x == 1*cos(theta1)*cos(0) + 1*cos(theta1 + theta2)*cos(0 + 0) + 1*cos(theta1 + theta2 + theta3)*cos(0 + 0 + 0)
y == 1*cos(theta1)*sin(0) + 1*cos(theta1 + theta2)*sin(0 + 0) + 1*cos(theta1 + theta2 + theta3)*sin(0 + 0 + 0)
z == 1*sin(theta1) + 1*sin(theta1 + theta2) + 1*sin(theta1 + theta2 + theta3)

我给他们x y和z的不同值,并求解这些值的theta。例如:

t = 0:0.1:1;
x = 2 * t;
y = 0;
z = 1;

所以在这里我们有10个不同的职位,我知道有“连续的”解决方案,我只是不知道如何使vpasolve给我这些解决方案,这是我的用法:

syms x y z theta1 theta2 theta3    

xEquation = 'x == 1*cos(theta1)*cos(0) + 1*cos(theta1 + theta2)*cos(0 + 0) + 1*cos(theta1 + theta2 + theta3)*cos(0 + 0 + 0)';
yEquation = 'y == 1*cos(theta1)*sin(0) + 1*cos(theta1 + theta2)*sin(0 + 0) + 1*cos(theta1 + theta2 + theta3)*sin(0 + 0 + 0)';
zEquation = 'z == 1*sin(theta1) + 1*sin(theta1 + theta2) + 1*sin(theta1 + theta2 + theta3)';

t = 0:0.1:1;
x = 2 * t;
y = 0;
z = 1;

xEquationEv = eval(xEquation);
yEquationEv = eval(yEquation);
zEquationEv = eval(zEquation);

for f = 1:size(x, 2)
    sol(f) = vpasolve([xEquationEv(f), yEquationEv, zEquationEv], [theta1, theta2, theta3], [-pi pi; -pi pi; -pi pi]);
end

sol.theta1
sol.theta2
sol.theta3

您将看到这些值并非一直都在平稳地递增或递减,但有时它们会出现较大的跳跃。

注意:我从字符串和eval开始,因为方程可能会改变,所以我有另一种算法来生成它们。

2 个答案:

答案 0 :(得分:0)

我想我在获得第一个解决方案后通过重新计算限制来设法解决了这个问题。像这样:

parse-ietf-date

那给了我更流畅的运动(解决方案)。

答案 1 :(得分:0)

听起来您已经解决了这个问题,但是您也可以尝试在原始结果上使用unwrap函数。

对于非多项式,vpasolve仅返回方程式的一个答案(在您的情况下为两个答案)。 unwrap应该倾向于减少变量在方程的不同解决方案之间跳转的次数,尽管您将需要减小默认容差(因为设置该默认容差是为了捕获2 * pi跳转)。

例如,我将尝试绘制unwrap(sol.theta1,pi/2),并查看它是否更符合您的期望。