Inverse-Kinematics: как рассчитать углы для сервоприводов роботизированной руки, чтобы достичь всех возможных точек на холсте?

Question

У меня есть роботизированная рука, состоящая из 2-х сервомоторов. Я пытаюсь вычислить обратную кинематику таким образом, что рука расположена в середине холста и может перемещаться по всем возможным точкам в обоих направлениях (влево и вправо). Это изображение системы Image. Первый серво двигается 0-180 (против часовой стрелки). Второй сервопривод перемещается на 0-180 (по часовой стрелке).

Вот мой код:

int L1 = 170; 
    int L2 = 230; 
    Vector shoulderV; 
    Vector targetV; 
    shoulderV = new Vector(0,0); 
    targetV = new Vector(0,400); 


    Vector difference = Vector.Subtract(targetV, shoulderV); 
    double L3 = difference.Length; 
    if (L3 > 400) { L3 = 400; } 
    if (L3 < 170) { L3 = 170; } 

    // a + b is the equivelant of the shoulder angle 
    double a = Math.Acos((L1 * L1 + L3 * L3 - L2 * L2)/(2 * L1 * L3)); 
    double b = Math.Atan(difference.Y/difference.X); 

    // S1 is the shoulder angle 
    double S1 = a + b; 
    // S2 is the elbow angle 
    double S2 = Math.Acos((L1 * L1 + L2 * L2 - L3 * L3)/(2 * L1 * L2)); 

    int shoulderAngle = Convert.ToInt16(Math.Round(S1 * 180/Math.PI)); 
    if (shoulderAngle < 0) { shoulderAngle = 180 - shoulderAngle; } 
    if (shoulderAngle > 180) { shoulderAngle = 180; } 
    int elbowAngle = Convert.ToInt16(Math.Round(S2 * 180/Math.PI)); 

    elbowAngle = 180 - elbowAngle; 

Первоначально, когда система запускается в первый раз, рука выпрямляется с плеча = 90, локоть = 0. Когда я даю положительные значения x, я получаю правильные результаты в левой части холста. Тем не менее, я хочу, чтобы рука тоже двигалась в правой части. При вводе негативов я не получаю правильные значения. Что я делаю не так? Нужен ли мне дополнительный сервопривод для достижения точек в правой части?

Извините, если объяснения не очень хорошие. Английский не мой первый язык.

Answer 1

Я подозреваю, что вы теряете знак, когда используете Math.Atan(). Я не знаю, какой язык программирования или среда это, но попробовать и посмотреть, если у вас есть что-то вроде этого:

Вместо этой линии:

double b = Math.Atan(difference.Y/difference.X);

использовать что-то вроде этого:

double b = Math.Atan2(difference.Y, difference.X);

Когда difference.Y и difference.X имеют одинаковый знак, разделив их результаты в положительном значении. Это мешает вам различать случаи, когда они оба положительные и оба отрицательные. В этом случае, например, вы не можете различать между 30 и 210 градусами.