机械臂运动学正逆解

application/cmd/robot_cmd.c

@@ -156,16 +156,17 @@ static void RemoteControlSet() {
 
     // 右侧开关状态为[下],机械臂
     if (switch_is_down(rc_data[TEMP].rc.switch_right)) {
-        // 左侧开关状态为[下],前三轴
+        // 左侧开关状态为[下],前两轴
         if(switch_is_down(rc_data[TEMP].rc.switch_left))
         {
             gimbal_cmd_send.pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1;
             gimbal_cmd_send.yaw += 0.001f * (float) rc_data[TEMP].rc.rocker_r_;
-            gimbal_cmd_send.roll += 0.001f * (float) rc_data[TEMP].rc.rocker_l_;
+
         }
-        // 左侧开关状态为[中],后两轴
+        // 左侧开关状态为[中],后三轴
         if(switch_is_mid(rc_data[TEMP].rc.switch_left))
         {
+            gimbal_cmd_send.roll += 0.001f * (float) rc_data[TEMP].rc.rocker_l_;
             gimbal_cmd_send.diff_pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1;
             gimbal_cmd_send.diff_roll += 0.001f * (float) rc_data[TEMP].rc.rocker_r_;
         }
@@ -173,7 +174,7 @@ static void RemoteControlSet() {
     // 云台软件限位
     gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch,PITCH_MIN,PITCH_MAX);
     gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw,-YAW,YAW);
-    //gimbal_cmd_send.roll = float_constrain(gimbal_cmd_send.roll,-ROLL,ROLL);
+    gimbal_cmd_send.roll = float_constrain(gimbal_cmd_send.roll,-ROLL,ROLL);
 
     gimbal_cmd_send.diff_pitch = float_constrain(gimbal_cmd_send.diff_pitch,-DIFF_PITCH,DIFF_PITCH);
     gimbal_cmd_send.diff_roll = float_constrain(gimbal_cmd_send.diff_roll,-DIFF_ROLL,DIFF_ROLL);

application/gimbal/gimbal.cpp

@@ -37,8 +37,31 @@ static float pitch_spd,yaw_spd,roll_spd;
 first_order_filter_type_t diff_r_spd_filter,diff_l_spd_filter;
 static float diff_r_spd,diff_l_spd;
 
+//机械臂参数初始化
+float arm_q[5] = {0}; // 机械臂各关节位置
+robotics::Link link[5];
+robotics::Serial_Link<5> engineer_arm(link);
+Matrixf<4, 4> fk_T; //正运动学  末端变换矩阵
+Matrixf<3, 1> fk_p; //正运动学  末端位置向量
+Matrixf<3, 1> fk_rpy; //正运动学  末端欧拉角
+
+Matrixf<5, 2> ik_q; //逆运动学  关节位置
+const float l1 = 0.151 ,l3 = 0.350, l4 = 0.139;
+void Arm_Init()
+{
+    link[0] = robotics::Link(0,l1,0,PI/2);
+    link[1] = robotics::Link(0,0,0,PI/2); link[1].offset_ = PI/2;
+    link[2] = robotics::Link(0,l3,0,-PI/2);
+    link[3] = robotics::Link(0,0,0,PI/2);
+    link[4] = robotics::Link(0,l4,0,0);
+
+    engineer_arm = robotics::Serial_Link<5>(link);
+    engineer_arm.ikine_analytic = robotics::my_analytic_ikine;
+}
+
 void GimbalInit()
 {
+    Arm_Init();
     Motor_Init_Config_s yaw_motor_config = {
             .controller_param_init_config = {
                     .other_speed_feedback_ptr = &yaw_spd,
@@ -147,9 +170,19 @@ void GimbalInit()
     roll_motor = DMMotorInit(&roll_motor_config);
 
     Motor_Init_Config_s diff_motor_config = {
+            .controller_param_init_config = {
+                    .speed_PID = {
+                            .Kp = 0.6f,
+                            .Ki = 0.1f,
+                            .Kd = 0.02f,
+                            .MaxOut = 10,
+                            .Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
+                            .IntegralLimit = 10.0F,
+                    },
+            },
             .controller_setting_init_config = {
-                    .outer_loop_type = OPEN_LOOP,
+                    .outer_loop_type = SPEED_LOOP,
-                    .close_loop_type = OPEN_LOOP,
+                    .close_loop_type = SPEED_LOOP,
                     .angle_feedback_source = MOTOR_FEED,
                     .speed_feedback_source = OTHER_FEED,
                     //.feedforward_flag = CURRENT_FEEDFORWARD,
@@ -230,16 +263,12 @@ void GimbalInit()
 /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
 void GimbalTask()
 {
-    /* Example 1: PUMA560 ------------------------------------------------------------------------*/
+
-    float m[6] = {0.2645, 0.17, 0.1705, 0, 0, 0};
-    float data[18]={0, -8.5e-2, 0, 0, 0, 0,
-                13.225e-2, 0, 0, 0, 0, 0,
-                0, 3.7e-2, 8.525e-2, 0, 0, 0};
-    Matrixf<3, 6> rc(data);
-    Matrixf<3, 3> I[6];
     // 获取云台控制数据
     // 后续增加未收到数据的处理
     SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
+
+    /* 控制参数计算 ------------------------------------------------------------------------*/
     //大臂重力补偿力矩
     arm_gravity_feedforward =  - GRAVITY_COMP * arm_cos_f32(- pitch_motor->measure.position);
     //电机速度滤波
@@ -249,7 +278,6 @@ void GimbalTask()
     yaw_spd = yaw_spd_filter.out;
     first_order_filter_cali(&roll_spd_filter,roll_motor->measure.velocity);
     roll_spd = roll_spd_filter.out;
-
     //手腕关节
     first_order_filter_cali(&diff_r_spd_filter,diff_r_motor->measure.velocity);
     diff_r_spd = diff_r_spd_filter.out;
@@ -262,13 +290,25 @@ void GimbalTask()
     float pitch_angle_out = PIDCalculate(&diff_pitch_loop,diff_pitch_angle,gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD);
     float roll_angle_out = PIDCalculate(&diff_roll_loop,diff_roll_angle,gimbal_cmd_recv.diff_roll * DEGREE_2_RAD);
 
-    float diff_pitch_spd = (diff_r_spd + (-diff_l_spd)) / 2;
+//    float diff_pitch_spd = (diff_r_spd + (-diff_l_spd)) / 2;
-    float diff_roll_spd = (diff_r_spd - (-diff_l_spd)) /2 * 18/52;
+//    float diff_roll_spd = (diff_r_spd - (-diff_l_spd)) /2 * 18/52;
-    float pitch_out = PIDCalculate(&diff_pitch_spd_loop,diff_pitch_spd,pitch_angle_out);
+//    float pitch_out = PIDCalculate(&diff_pitch_spd_loop,diff_pitch_spd,pitch_angle_out);
-    float roll_out = PIDCalculate(&diff_roll_spd_loop,diff_roll_spd,roll_angle_out);
+//    float roll_out = PIDCalculate(&diff_roll_spd_loop,diff_roll_spd,roll_angle_out);
 
-    float r_speed_set = pitch_out + roll_out;
+    float r_speed_set = pitch_angle_out + roll_angle_out;
-    float l_speed_set = pitch_out - roll_out;
+    float l_speed_set = pitch_angle_out - roll_angle_out;
+
+    //正运动学
+    arm_q[0] = yaw_motor->measure.position;
+    arm_q[1] = pitch_motor->measure.position;
+    arm_q[2] = roll_motor->measure.position;
+    arm_q[3] = diff_pitch_angle;
+    arm_q[4] = diff_roll_angle;
+    fk_T = engineer_arm.fkine(arm_q);
+    fk_p = robotics::t2p(fk_T);
+    fk_rpy = robotics::t2rpy(fk_T);
+    //逆运动学
+    ik_q = engineer_arm.ikine_analytic(fk_T);
 
     // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
     // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref

engineering.jdebug.user

@@ -1,35 +1,32 @@
 
 
 
-Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:259:5, State=BP_STATE_DISABLED
+Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:265:1, State=BP_STATE_DISABLED
-GraphedExpression="(((yaw_motor)->motor_controller).speed_PID).Ref", Color=#e56a6f
+GraphedExpression="(diff_pitch_loop).Measure", Color=#e56a6f
-GraphedExpression="(((yaw_motor)->motor_controller).speed_PID).Measure", Color=#35792b
+GraphedExpression="(diff_pitch_loop).Ref", Color=#35792b
-GraphedExpression="(((yaw_motor)->motor_controller).angle_PID).Ref", Color=#769dda
+OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=72
-GraphedExpression="(((yaw_motor)->motor_controller).angle_PID).Measure", Color=#b14f0d
-OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69
 OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3422
-OpenDocument="arm_mat_mult_f32.c", FilePath="C:/Users/clamar01/fork3/CMSIS_5/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c", Line=0
 OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=0
 OpenDocument="stm32f4xx_it.c", FilePath="D:/CLion/Project/engineering/Src/stm32f4xx_it.c", Line=107
 OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=51
 OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=315
-OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=153
+OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=5
 OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=1255
 OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=0
 OpenDocument="stm32f4xx_hal_can.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c", Line=1412
 OpenDocument="chassis.c", FilePath="D:/CLion/Project/engineering/application/chassis/chassis.c", Line=165
 OpenToolbar="Debug", Floating=0, x=0, y=0
-OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=602, h=160, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=176, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=401, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
-OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=602, h=481, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=465, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1041, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=265, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
-OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=672, h=642, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;66", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="313;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="326;0"
+OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=531, h=642, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="70;20", CodeGraphLegendShown=0, CodeGraphLegendPosition="80;20"
 OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=210, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
 TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;889]
-TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (((yaw_motor)->motor_controller).speed_PID).Ref";" (((yaw_motor)->motor_controller).speed_PID).Measure";" (((yaw_motor)->motor_controller).angle_PID).Ref";" (((yaw_motor)->motor_controller).angle_PID).Measure"], ColWidths=[100;100;100;100;100;100]
+TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (diff_pitch_loop).Measure";" (diff_pitch_loop).Ref"], ColWidths=[100;100;100;100]
-TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;126;110;102;126;100;118;110]
+TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;100;100;100;100;100;107;107]
 TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
 TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
 TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;124;104;117;100]
@@ -48,4 +45,9 @@ WatchedExpression="gimbal_cmd_recv", RefreshRate=2, Window=Watched Data 1
 WatchedExpression="yaw_motor", RefreshRate=2, Window=Watched Data 1
 WatchedExpression="roll_motor", RefreshRate=2, Window=Watched Data 1
 WatchedExpression="(((roll_motor)->motor_controller).speed_PID).Ref", Window=Watched Data 1
-WatchedExpression="roll_spd_filter", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="roll_spd_filter", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="(fk_p).data_", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="(fk_rpy).data_", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="arm_q", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="ik_q.data_", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="pitch_motor", Window=Watched Data 1

modules/robotics/robotics.cpp

@@ -338,3 +338,50 @@ Matrixf<4, 4> robotics::Link::T(float q) {
   }
   return dh_.fkine();
 }
+
+Matrixf<5,2> robotics::my_analytic_ikine(Matrixf<4, 4> T)
+{
+    const float L1=0.151,L3=0.350,L4=0.139;
+    Matrixf<3,1> Pe = T.block<3, 1>(0, 3);  // p=T(1:3,4)
+    Matrixf<3,1> Ze = T.block<3, 1>(0, 2);  // z=T(1:3,3)
+    //求解W点（球形腕）
+    Matrixf<3,1> Pw = Pe-L4*Ze;
+    //求解theta1
+    float theta_1 = atan2(Pw[1][0],Pw[0][0]);
+    //求解theta2
+    //L1^2 + L3^2 - 2*L1*L3*cos_th2 = norm(Pw)^2
+    float cos_th2 = (L1*L1 + L3*L3 - Pw.norm()*Pw.norm())/(2*L1*L3);
+    float theta_2 = acosf(cos_th2) ; //关节2 有PI/2的offset
+    //计算T02
+    DH_t l1_dh = {
+            .theta = theta_1,
+            .d = L1,
+            .a = 0,
+            .alpha = PI/2,
+    };
+    DH_t l2_dh = {
+            .theta = theta_2,
+            .d = 0,
+            .a = 0,
+            .alpha = PI/2,
+    };
+    Matrixf<4,4> T02 = l1_dh.fkine() * l2_dh.fkine();
+
+    Matrixf<4,4> T25 = invT(T02) * T;
+
+
+    //腕关节(theta_5)朝上  >0
+    float theta_30 = atan2(T25[1][2],T25[0][2]);
+    float theta_40 = atan2(sqrtf(T25[0][2]*T25[0][2] + T25[1][2]*T25[1][2]),T25[2][2]);
+    float theta_50 = atan2(T25[2][1],-T25[2][0]);
+    //腕关节(theta_5)朝下  <0
+    float theta_31 = atan2(-T25[1][2],-T25[0][2]);
+    float theta_41 = atan2(-sqrtf(T25[0][2]*T25[0][2] + T25[1][2]*T25[1][2]),T25[2][2]);
+    float theta_51 = atan2(-T25[2][1],T25[2][0]);
+
+    float q_data[10] = {theta_1,theta_2 - PI/2,theta_30,theta_40,theta_50,
+                        theta_1,theta_2 - PI/2,theta_31,theta_41,theta_51};//关节2 有PI/2的offset
+
+    Matrixf<5,2> ik_q(q_data);
+    return ik_q;
+}

modules/robotics/robotics.h

@@ -280,7 +280,7 @@ class Serial_Link {
   }
 
   // (Reserved function) inverse kinematic, analytic solution(geometric method)
-  Matrixf<_n, 1> (*ikine_analytic)(Matrixf<4, 4> T);
+  Matrixf<_n, 2> (*ikine_analytic)(Matrixf<4, 4> T);
 
   // inverse dynamic, Newton-Euler method
   // param[in]  q: joint variable vector
@@ -402,6 +402,8 @@ class Serial_Link {
   Matrixf<4, 4> T_;
   Matrixf<6, _n> J_;
 };
+
+    Matrixf<5, 2> my_analytic_ikine(Matrixf<4, 4> T);
 };  // namespace robotics
 
 #endif  // ROBOTICS_H