机械臂运动学正逆解
This commit is contained in:
parent
7d9955f575
commit
98f7aa2272
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
.close_loop_type = OPEN_LOOP,
|
||||
.outer_loop_type = SPEED_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_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 roll_out = PIDCalculate(&diff_roll_spd_loop,diff_roll_spd,roll_angle_out);
|
||||
// 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 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 r_speed_set = pitch_out + roll_out;
|
||||
float l_speed_set = pitch_out - roll_out;
|
||||
float r_speed_set = pitch_angle_out + roll_angle_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
|
||||
|
|
|
|||
|
|
@ -1,35 +1,32 @@
|
|||
|
||||
|
||||
|
||||
Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:259:5, State=BP_STATE_DISABLED
|
||||
GraphedExpression="(((yaw_motor)->motor_controller).speed_PID).Ref", Color=#e56a6f
|
||||
GraphedExpression="(((yaw_motor)->motor_controller).speed_PID).Measure", Color=#35792b
|
||||
GraphedExpression="(((yaw_motor)->motor_controller).angle_PID).Ref", Color=#769dda
|
||||
GraphedExpression="(((yaw_motor)->motor_controller).angle_PID).Measure", Color=#b14f0d
|
||||
OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69
|
||||
Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:265:1, State=BP_STATE_DISABLED
|
||||
GraphedExpression="(diff_pitch_loop).Measure", Color=#e56a6f
|
||||
GraphedExpression="(diff_pitch_loop).Ref", Color=#35792b
|
||||
OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=72
|
||||
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 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 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;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]
|
||||
|
|
@ -49,3 +46,8 @@ 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="(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
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue