古人智慧

Just Do it!
上士聞道,勤而行之;中士聞道,若存若亡;下士聞道,大笑之。不笑,不足以爲道。
~ 道德經 41

搜尋此網誌

Translation

2014年11月22日 星期六

蜘蛛Robot upgrade v3.0 - Quadruped Robot upgrade v3.0

經過數次的實驗,腳的關節的結構是會直接影響robot的機體穩定,之前在設計Femur與Tibia的關節時覺得影響不大,結果機體做運動時會搖晃,
甚至發生servo的連杆都脫落

本次升級著重在3(Coxa, Femur, Tibia)個關節強化,各關節在運動可以維持在軸心旋轉,並且確實不會左右晃動。這次升級體會機構件的設計必須非常紮實,不然software再怎麼改都無法讓機械腳移動到正確位置而難以開發gait步態。








《成果圖》



設計說明如下:
工具 & 材料:
3D繪圖: Sketch up
3D printer: Prusa i3
切片: Cura 14.09
材料: PLA 1.75mm

Arduino Pro mini x 1
DC-DC: 12v->5v/3A x 1
SG-90 servo x 12
Lithium battery 12v 
2mmx6mm screw x 14


因為Servo底端並無支撐的結構,所以我特地多了一片2mm的底座,長出支柱讓旋轉桿可以套入。


《Sketchup 3D design》


Coxa與Femur的連桿用2mm螺絲鎖緊,要記得連桿有方向性(鎖Servo連杆)。

這是用Cura把列印件排好,要印兩份。


這個也是要印兩份。


我有試著用Slic3r切片,結果慘不忍睹,每個洞都要銼刀磨,最麻煩是支撐柱,磨到手軟。
建議用Cure,切出來的孔非常精准,套上去很精確!
《3關節強化是重點》





組合後,哈哈,超級穩定!



電路圖(simple schematic)


IK source code:
呼叫 DO_IK() 前把要移動腳的座標In_Pos_X/Y/Z還有旋轉的座標設定好,就可以計算出該腳位3個關節角度,然後驅動servo進行運動,你會發現腳會靈活地變化,非常簡單!


void Do_IK_Init(void)
{
  float x, y, z;
  In_Pos_X = In_Pos_Y = In_Pos_Z = In_ROT_X = In_ROT_Y = In_ROT_Z = 0;

  x = cos(INIT_X * DEG_TO_RAD) * (COXA_LEN + FEMUR_LEN);
  y = sin(INIT_Z * DEG_TO_RAD) * (COXA_LEN + FEMUR_LEN);
  z = TIBIA_LEN;

  Leg_Init_Pos[RF].X = x;
  Leg_Init_Pos[RF].Y = y;
  Leg_Init_Pos[RF].Z = z;

  Leg_Init_Pos[RR].X = x;
  Leg_Init_Pos[RR].Y = -y;
  Leg_Init_Pos[RR].Z = z;

  Leg_Init_Pos[LF].X = -x;
  Leg_Init_Pos[LF].Y = y;
  Leg_Init_Pos[LF].Z = z;

  Leg_Init_Pos[LR].X = -x;
  Leg_Init_Pos[LR].Y = -y;
  Leg_Init_Pos[LR].Z = z;

  Leg_Total[RF].X = Leg_Init_Pos[RF].X + BODY_W / 2;
  Leg_Total[RF].Y = Leg_Init_Pos[RF].Y + BODY_L / 2;
  Leg_Total[RF].Z = TIBIA_LEN;
  Leg_Total[RR].X = Leg_Init_Pos[RR].X + BODY_W / 2;
  Leg_Total[RR].Y = Leg_Init_Pos[RR].Y - BODY_L / 2;
  Leg_Total[RR].Z = TIBIA_LEN;
  Leg_Total[LF].X = Leg_Init_Pos[LF].X - BODY_W / 2;
  Leg_Total[LF].Y = Leg_Init_Pos[LF].Y + BODY_L / 2;
  Leg_Total[LF].Z = TIBIA_LEN;
  Leg_Total[LR].X = Leg_Init_Pos[LR].X - BODY_W / 2;
  Leg_Total[LR].Y = Leg_Init_Pos[LR].Y - BODY_L / 2;
  Leg_Total[LR].Z = TIBIA_LEN;
}

void Do_IK(int leg)
{
  float Body_X, Body_Y, Body_Z, New_X, New_Y, New_Z, Total_X, Total_Y, Total_Z;
  float SIN_X, COS_X, SIN_Y, COS_Y, SIN_Z, COS_Z;

  // Body-IK
  SIN_X = sin(In_ROT_X * DEG_TO_RAD);
  COS_X = cos(In_ROT_X * DEG_TO_RAD);
  SIN_Y = sin(In_ROT_Y * DEG_TO_RAD);
  COS_Y = cos(In_ROT_Y * DEG_TO_RAD);
  SIN_Z = sin(In_ROT_Z * DEG_TO_RAD);
  COS_Z = cos(In_ROT_Z * DEG_TO_RAD);

  Total_X = Leg_Total[leg].X + In_Pos_X;
  Total_Y = Leg_Total[leg].Y + In_Pos_Y;
  Total_Z = Leg_Total[leg].Z;

  Body_X = (Total_X * COS_Y * COS_Z - Total_Y * COS_Y * SIN_Z + Total_Z * SIN_Y) - Total_X;
  Body_Y = (Total_X * COS_X * SIN_Z + Total_X * COS_Z * SIN_Y * SIN_X + Total_Y * COS_Z * COS_X - Total_Y * SIN_Z * SIN_Y * SIN_X - Total_Z * COS_Y * SIN_X) - Total_Y;
  Body_Z = (Total_X * SIN_X * SIN_Z - Total_X * COS_Z * SIN_Y * COS_X + Total_Y * COS_Z * SIN_X + Total_Y * SIN_Z * SIN_Y * COS_X + Total_Z * COS_Y * COS_X) - Total_Z;

  New_X = Body_X + In_Pos_X + Leg_Init_Pos[leg].X;
  New_Y = Body_Y + In_Pos_Y + Leg_Init_Pos[leg].Y;
  New_Z = Body_Z + In_Pos_Z + Leg_Init_Pos[leg].Z;

  Leg_Pos[leg].X = New_X * cos((PI / 4) + (PI / 2 * leg)) -  New_Y * sin((PI / 4) + (PI / 2 * leg));
  Leg_Pos[leg].Y = New_X * sin((PI / 4) + (PI / 2 * leg)) +  New_Y * cos((PI / 4) + (PI / 2 * leg));
  Leg_Pos[leg].Z = New_Z;


  Do_Leg_IK_Cal(leg);
}

/*
  leg: 0~3
*/
void Do_Leg_IK_Cal(int leg)
{
  float leg_len, HF, a1, a2, b1, T_angle, F_angle, C_angle;

  C_angle = 90 - atan2(Leg_Pos[leg].Y, Leg_Pos[leg].X) * RAD_TO_DEG;

  leg_len = cos(C_angle * DEG_TO_RAD) * Leg_Pos[leg].Y;
  //leg_len = sqrt(pow(Leg_Pos[leg].X, 2) + pow(Leg_Pos[leg].Y, 2));

  HF = sqrt(pow((leg_len - COXA_LEN), 2) + pow(Leg_Pos[leg].Z, 2));
  a1 = atan2((leg_len - COXA_LEN), Leg_Pos[leg].Z);
  a2 = acos((pow(TIBIA_LEN, 2) - pow(FEMUR_LEN, 2) - pow(HF, 2)) / (-2 * FEMUR_LEN * HF));
  F_angle = (a1 + a2) * RAD_TO_DEG - 90;
  b1 = acos((pow(HF, 2) - pow(TIBIA_LEN, 2) - pow(FEMUR_LEN, 2)) / (-2 * FEMUR_LEN * TIBIA_LEN));
  T_angle = 90 - b1 * RAD_TO_DEG;

  if (leg == 1 || leg == 3)
  {
    angle_req[leg][TIBIA] = 90 + T_angle;
    angle_req[leg][FEMUR] = 90 - F_angle;
  }
  else
  {
    angle_req[leg][TIBIA] = 90 - T_angle;
    angle_req[leg][FEMUR] = 90 + F_angle;
  }

  angle_req[leg][COXA] = 90 - C_angle;

}

《servo旋轉方向》










2 則留言:

  1. 一個很棒的本土化4腳蜘蛛專案。
    是個搞Arduino與3DP 很值得仿作的專案。
    不知道有沒機會請您來做個分享。

    回覆刪除
  2. 我將您的專案分享到FB上,如有冒犯,請通知我移除,謝謝
    https://www.facebook.com/profile.php?id=1493168496

    回覆刪除