五连杆运动学解算

一个末端的坐标经过逆运动学解算会得到4个解（4个电机角度），对应四种工作模式。

正表示凸，负表示凹。

对于正运动学解算，一组电机角度会得到两个末端坐标，分别是凹和凸。

总结：4个工作模式，2个解。

运动学解算数学表达式：

 /**
 * @brief 工作模式枚举
 *
 * PP: 
 * PN: 
 * NP: 
 * NN: 
 */
enum inverse_mode {
	PP,
	PN,
	NP,
	NN
};

/**
 * @brief 工作模式枚举
 */
enum forward_mode {
	CONVEX,
  CONCAVE
};

void forwardKinematics(){
  theta1 = motor1.getangle();
  theta2 = motor2.getangle();

  xR1 = a1 * cos(theta1);
  yR1 = a1 * sin(theta1);

  // Calculate coordinates of R2
  xR2 = a2 * cos(theta2) + w;
  yR2 = a2 * sin(theta2);

  // Calculate squared distances b1 and b2
  b1_squared = pow(xP - *xR1, 2) + pow(yP - *yR1, 2);
  b2_squared = pow(xP - *xR2, 2) + pow(yP - *yR2, 2);

  // Function to calculate xP
  v1 = (yR1 - yR2) / (xR2 - xR1);
  v2 = (b1_squared - b2_squared - a1_squared + a2_squared + xR2 * xR2 + yR2 * yR2) / (2 * (xR2 - xR1));
  xP = v1 * yP + v2;

  // Function to calculate yP
  v3 = 1 + v1 * v1;
  v4 = 2 * (v1 * v2 - xR1 * v1 - yR1);
  v5 = a1 * a1 * b1_squared - 2 * v2 * xR1 + v2 * v2;
  discriminant = v4 * v4 - 4 * v3 * v5;
  yP1 = (-v4 + sqrt(discriminant)) / (2 * v3);//凸,convex
  yP2 = (-v4 - sqrt(discriminant)) / (2 * v3);//凹,concave
}


void inverseKinematics(){
   // 定义变量
  double x_p, y_p, x_M1, y_M1, x_M2, y_M2, c1_squared, c2_squared, w, a1, a2, b1_squared, b2_squared;

  // 计算 c1_squared 和 c2_squared
  c1_squared = pow(x_p - x_M1, 2) + pow(y_p - y_M1, 2);
  c2_squared = pow(x_p - x_M2, 2) + pow(y_p - y_M2, 2);

  // 简化 c2_squared 的表达式
  //c2_squared = pow(x_p - w, 2) + pow(y_p, 2);
  //           = x_p * x_p - 2 * x_p * w + pow(w, 2) + y_p * y_p;
  c2_squared = c1_squared - 2 * x_p * w + pow(w, 2);

  // 计算 alpha1
  alpha1 = acos(x_p / c1);

  // 计算 alpha2
  alpha2 = acos((-(x_p - w) / c2));

  // 计算 beta，有两个解，注意筛选
  beta1 = acos((b_i_squared - pow(a_i, 2) - ci_squared) / (-2 * a_i * ci));
  beta2 = acos((b_i_squared - pow(a_i, 2) - ci_squared) / (-2 * a_i * ci));


    //motor_set_angle(motor1, theta1);
    //motor_set_angle(motor2, theta2);
  }


  //为了顺滑输出，需要调整
void loop() {
  
	forwardKinematics();

	X = X + Kp_X * (X_demand - X);
	Y = Y + Kp_Y * (Y_demand - Y);

	inverseKinematics();
  }

参考文献：
Gamble, Brandon J., "5-Bar Linkage Kinematic Solver and Simulator" (2020). UVM Honors College Senior
Theses. 406.
https://scholarworks.uvm.edu/hcoltheses/406