Найти решения системы уравнений как можно ближе к предыдущим решениям - PullRequest
Новая
       19

Найти решения системы уравнений как можно ближе к предыдущим решениям

0 голосов
/ 16 мая 2019

В настоящее время я использую команду vpasolve для решения системы из 3 уравнений с 3 неизвестными. Эти 3 уравнения используются для моделирования кинематического движения робота-манипулятора, а неизвестными являются либо длины звеньев робота, либо углы поворота шарниров. Команде vpasolve удается правильно решить уравнения, проблема в том, что решения, которые я получаю, не близки. Что я имею в виду под не близко? Если я получу решение 1,5, 0,3, -0,4 [рад] для одного из углов, следующим решением для следующего шага движения будет любой другой случайный угол, который может его решить, но в действительности это бесполезно, поэтому это может быть что-то вроде -0,8 -1,5 2 [рад]. Конечно, для робота было бы бессмысленно прыгать с 1,5 рад до -0,8 рад, просто чтобы переместить свою конечную точку на 1 см. Вместо этого я хотел бы получить решение, максимально приближенное к углам предыдущего решения.

Уравнения выглядят примерно так: 

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 и решаю тэты для этих значений. Например: 

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 голосов
/ 16 мая 2019

Похоже, вы уже решили эту проблему, но вы также можете попробовать использовать функцию unwrap в исходном результате.

Для неполиномов vpasolve возвращает только один ответ (из двух, в вашем случае) на уравнения. unwrap должно уменьшать количество раз, которое переменные переходят между различными решениями ваших уравнений, хотя вам нужно будет уменьшить допуск по умолчанию (так как он был настроен для ловли 2 * пи скачков).

Я бы попробовал построить, например, unwrap(sol.theta1,pi/2) и посмотреть, выглядит ли это больше, чем вы ожидали.

0 голосов
/ 16 мая 2019

Думаю, мне удалось решить эту проблему путем пересчета пределов после получения первого решения.Что-то вроде: 

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

Это дало мне гораздо более плавное движение (решения).

...