找到尽可能接近先前解的方程组的解答案

【问题标题】：Find solutions of a system of equations as close as possible to the previous solutions找到尽可能接近先前解的方程组的解
【发布时间】：2019-05-16 09:16:15
【问题描述】：

目前我正在使用 vpasolve 命令来求解一个由 3 个方程组成的系统，其中包含 3 个未知数。这 3 个方程用于模拟机器人机械臂的运动学运动，未知数是机器人连杆的长度或关节的旋转角度。 vpasolve 命令设法正确求解方程，问题是我得到的解并不接近。不关闭是什么意思？如果我得到一个角度为 1.5、0.3、-0.4 [rad] 的解，则运动下一步的下一个解将是任何其他可以解决它的随机角度，但实际上没有用，所以它可能类似于 -0.8 -1.5 2 [rad]。当然，机器人从 1.5 rad 跳到 -0.8 rad 只是将其终点移动 1 cm 是没有意义的。相反，我希望得到一个尽可能接近前一个解决方案的角度的解决方案。

方程看起来像这样：

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 值并求解这些值的 thetas。例如：

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 开始，因为方程可能会改变，我有另一个生成它们的算法。

【问题讨论】：

为什么你的一些周期函数有常量参数？在我看来，整个问题就像你应该研究解决微分方程。
这些常数参数是连杆的长度和其他没有变化的关节角度，我只是从这些广义方程开始，并将我的值代入其中

标签： matlab equation-solving

【解决方案1】：

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

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

例如，我会尝试绘制unwrap(sol.theta1,pi/2)，看看它是否看起来更像您的预期。

【讨论】：

【解决方案2】：

我想我设法通过在获得第一个解决方案后重新计算限制来解决它。比如：

limits = [previousTheta1 - step previousTheta1 + step;
          previousTheta2 - step previousTheta2 + step;
          previousTheta3 - step previousTheta3 + step];

这让我的动作更加流畅（解决方案）。

【讨论】：