diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 46afe8e..3670422 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -15,7 +15,7 @@ { "label": "download dap", "type": "shell", // 如果希望在下载前编译,可以把command换成下面的命令 - "command":"mingw32-make download_dap", // "mingw32-make -j24 ; mingw32-make download_dap", + "command":"mingw32-make -j24 ; mingw32-make download_dap", // "mingw32-make -j24 ; mingw32-make download_dap", "group": { // 如果没有修改代码,编译任务不会消耗时间,因此推荐使用上面的替换. "kind": "build", "isDefault": false, @@ -24,7 +24,7 @@ { "label": "download jlink", // 要使用此任务,需添加jlink的环境变量 "type": "shell", - "command":"mingw32-make download_jlink", // "mingw32-make -j24 ; mingw32-make download_dap" + "command":"mingw32-make -j24 ; mingw32-make download_jlink", // "mingw32-make -j24 ; mingw32-make download_jlink" "group": { "kind": "build", "isDefault": false, diff --git a/Makefile b/Makefile index 29bbfeb..4473243 100644 --- a/Makefile +++ b/Makefile @@ -347,8 +347,8 @@ clean: # download directl without debugging ####################################### download_dap: - openocd -f openocd_dap.cfg -c init -c halt -c "flash write_image erase $(BUILD_DIR)/$(TARGET).bin 0x08000000" -c reset -c shutdown + openocd -f openocd_dap.cfg -c "program $(BUILD_DIR)/$(TARGET).elf verify reset exit" download_jlink: - JFlash -openprj'stm32.jflash' -open'$(BUILD_DIR)/$(TARGET).hex',0x8000000 -auto -startapp -exit + openocd -f openocd_jlink.cfg -c "program $(BUILD_DIR)/$(TARGET).elf verify reset exit" # *** EOF *** diff --git a/application/chassis/balance.c b/application/chassis/balance.c deleted file mode 100644 index 9e0f13a..0000000 --- a/application/chassis/balance.c +++ /dev/null @@ -1,404 +0,0 @@ -// app -#include "balance.h" -#include "robot_def.h" -#include "general_def.h" -#include "ins_task.h" -#include "HT04.h" -#include "LK9025.h" -#include "controller.h" -#include "can_comm.h" -#include "super_cap.h" -#include "user_lib.h" -#include "remote_control.h" -#include "referee_task.h" -#include "stdint.h" -#include "arm_math.h" // 需要用到较多三角函数 -#include "bsp_dwt.h" -#include "bsp_log.h" -#include "linkNleg.h" -#include "speed_estimation.h" -#include "lqr_calc.h" - - -static uint32_t balance_dwt_cnt; -static float del_t; - -/* 底盘拥有的模块实例 */ -static attitude_t *imu_data; -static RC_ctrl_t *rc_data; // 底盘单独调试用 -static Referee_Interactive_info_t my_ui; -static referee_info_t *referee_data; -static Chassis_Ctrl_Cmd_s chassis_cmd_recv; -static Chassis_Upload_Data_s chassis_feed; -static CANCommInstance *ci; -static SuperCapInstance *cap; - -// 四个关节电机和两个驱动轮电机 -static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试 -static LKMotorInstance *l_driven, *r_driven, *driven[2]; - -// 两个腿的参数,0为左腿,1为右腿 -static LinkNPodParam l_side, r_side; -static ChassisParam chassis; - -// 综合运动补偿的PID控制器 -static PIDInstance steer_p_pid, steer_v_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量 -static PIDInstance anti_crash_pid, phi5_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上 -static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬" -static PIDInstance roll_compensate_pid, rolldot_pid; // roll轴补偿,用于保持机体水平 - -static Robot_Status_e chassis_status; - -void BalanceInit() -{ - rc_data = RemoteControlInit(&huart3); - imu_data = INS_Init(); - - // 双板通信 - CANComm_Init_Config_s commconf = { - .can_config = { - .can_handle = &hcan2, - .tx_id = 0x40, - .rx_id = 0x41}, - .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s), - .send_data_len = sizeof(Chassis_Upload_Data_s)}; - ci = CANCommInit(&commconf); - - // 超级电容 - SuperCap_Init_Config_s cap_conf = { - .can_config = { - .can_handle = &hcan2, - .tx_id = 0x302, // todo 电容id - .rx_id = 0x301}}; - cap = SuperCapInit(&cap_conf); - - // 关节电机 - Motor_Init_Config_s joint_conf = { - // 写一个,剩下的修改方向和id即可 - .can_init_config = { - .can_handle = &hcan1}, - .controller_param_init_config = { - .angle_PID = { - .Kp = 0.3, - .Kd = 0.1, - .Ki = 0, - .DeadBand = 0.0001, - .Improve = PID_DerivativeFilter, - .MaxOut = 4, - .Derivative_LPF_RC = 0.05, - }, // 仅用于复位腿 - }, - .controller_setting_init_config = { - .close_loop_type = ANGLE_LOOP, - .outer_loop_type = OPEN_LOOP, - .motor_reverse_flag = FEEDBACK_DIRECTION_NORMAL, - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, - }, - .motor_type = HT04}; - joint_conf.can_init_config.tx_id = 4; - joint_conf.can_init_config.rx_id = 14; - joint[LF] = lf = HTMotorInit(&joint_conf); - joint_conf.can_init_config.tx_id = 3; - joint_conf.can_init_config.rx_id = 13; - joint[LB] = lb = HTMotorInit(&joint_conf); - joint_conf.can_init_config.tx_id = 2; - joint_conf.can_init_config.rx_id = 12; - joint[RF] = rf = HTMotorInit(&joint_conf); - joint_conf.can_init_config.tx_id = 1; - joint_conf.can_init_config.rx_id = 11; - joint[RB] = rb = HTMotorInit(&joint_conf); - - // 驱动轮电机 - Motor_Init_Config_s driven_conf = { - // 写一个,剩下的修改方向和id即可 - .can_init_config.can_handle = &hcan2, - .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, - .outer_loop_type = OPEN_LOOP, - .close_loop_type = OPEN_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, - }, - .motor_type = LK9025, - }; - driven_conf.can_init_config.tx_id = 1; - driven[LD] = l_driven = LKMotorInit(&driven_conf); - driven_conf.can_init_config.tx_id = 2; - driven[RD] = r_driven = LKMotorInit(&driven_conf); - - // 转向PID - PID_Init_Config_s steer_p_pid_conf = { - .Kp = 2, - .Kd = 1, - .Ki = 0.0f, - .MaxOut = 4, - .DeadBand = 0.01f, - .Improve = PID_DerivativeFilter, - .Derivative_LPF_RC = 0.05, - }; - PIDInit(&steer_p_pid, &steer_p_pid_conf); - PID_Init_Config_s steer_v_pid_conf = { - .Kp = 2, - .Kd = 0.0f, - .Ki = 0.0f, - .MaxOut = 100, - .DeadBand = 0.0f, - .Improve = PID_DerivativeFilter | PID_Integral_Limit, - .Derivative_LPF_RC = 0.05, - .IntegralLimit = 2, - }; - PIDInit(&steer_v_pid, &steer_v_pid_conf); - - // 抗劈叉 - PID_Init_Config_s anti_crash_pid_conf = { - .Kp = 8, - .Kd = 2.5, - .Ki = 0.4, - .MaxOut = 45, - .DeadBand = 0.01f, - .Improve = PID_DerivativeFilter | PID_ChangingIntegrationRate | PID_Integral_Limit, - .Derivative_LPF_RC = 0.05, - .CoefA = 0.05, - .CoefB = 0.05, - .IntegralLimit = 2, - }; - PIDInit(&anti_crash_pid, &anti_crash_pid_conf); - - // 腿长控制 - PID_Init_Config_s leg_length_pid_conf = { - .Kp = 450, - .Kd = 150, - .Ki = 5, - .MaxOut = 60, - .DeadBand = 0.0001f, - .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement, - .CoefA = 0.01, - .CoefB = 0.02, - .Derivative_LPF_RC = 0.08, - }; - PIDInit(&leglen_pid_l, &leg_length_pid_conf); - PIDInit(&leglen_pid_r, &leg_length_pid_conf); - - // 横滚角补偿 - PID_Init_Config_s roll_compensate_pid_conf = { - .Kp = 0.0008f, - .Kd = 0.00065f, - .Ki = 0.0f, - .MaxOut = 0.04, - .DeadBand = 0.005f, - .Improve = PID_DerivativeFilter, - .Derivative_LPF_RC = 0.05, - }; - PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf); - - l_side.target_len = r_side.target_len = 0.23; // 初始腿长 - chassis.vel_cov = 1000; // 初始化速度协方差 - chassis_status = ROBOT_READY; - DWT_GetDeltaT(&balance_dwt_cnt); -} - -static void EnableAllMotor() /* 打开所有电机 */ -{ - for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机 - HTMotorEnable(joint[i]); - for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机 - LKMotorEnable(driven[i]); -} - -/* 切换底盘遥控器控制和云台双板控制 */ -static void ControlSwitch() -{ - // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令 - if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline()) - { - if (rc_data->rc.rocker_l1 < -600) - { - chassis_cmd_recv.chassis_mode = CHASSIS_RESET; - chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s - } - else // 设定值覆盖双板 - { - chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后 - chassis_cmd_recv.vx = 0.02 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s - chassis_cmd_recv.offset_angle = 0.001 * (float)rc_data[TEMP].rc.rocker_r_; // rotate? follow. - chassis_cmd_recv.delta_leglen = -0.0000015f * (float)rc_data[TEMP].rc.dial; - } - } - else if (CANCommIsOnline(ci) && !switch_is_down(rc_data->rc.switch_right)) - chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(ci); // 获取云台板指令 - else - chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停 -} - - -/* 腿缩回复位,只允许驱动轮电机移动 */ -static void ResetChassis() -{ - EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身 - - // 复位时清空距离和腿长积累量,保证顺利站起 - chassis.dist = chassis.target_dist = 0; - l_side.target_len = r_side.target_len = 0.24; - // 撞墙时前后移动保证能重新站立,执行速度输入 - LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2); - LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2); - - // 若关节完成复位,进入ready态 - if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.02 && - abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.02 && - abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.02 && - abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.02) - { - chassis_status = ROBOT_READY; // 底盘已经准备好重新站立 - } - else if (abs(lf->measure.total_angle) <= 0.02 && - abs(lb->measure.total_angle) <= 0.02 && - abs(rf->measure.total_angle) <= 0.02 && - abs(rb->measure.total_angle) <= 0.02) - { // 双阈值保证关节能够复位而不会进入死区 - chassis_status = ROBOT_READY; // 底盘已经准备好重新站立 - for (uint8_t i = 0; i < JOINT_CNT; i++) - HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环 - return; // 退出函数不再执行关节指令 - } - else - chassis_status = ROBOT_STOP; - - // 还在复位中,关节改为位置环,执行复位 - for (uint8_t i = 0; i < JOINT_CNT; i++) - { - HTMotorOuterLoop(joint[i], ANGLE_LOOP); - HTMotorSetRef(joint[i], 0); - } -} - - -/* 工作状态设定 */ -static void WokingStateSet() -{ - if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET) // 复位模式 - { - ResetChassis(); - return; - } - else if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停 - { - for (uint8_t i = 0; i < JOINT_CNT; i++) - HTMotorStop(joint[i]); - for (uint8_t i = 0; i < DRIVEN_CNT; i++) - LKMotorStop(driven[i]); - return; // 关闭所有电机,发送的指令为零 - } - - // 运动模式 - EnableAllMotor(); - // 设置目标速度/腿长/距离 - l_side.target_len += chassis_cmd_recv.delta_leglen; - r_side.target_len += chassis_cmd_recv.delta_leglen; - VAL_LIMIT(l_side.target_len, 0.13, 0.3); // 腿长限幅 - VAL_LIMIT(r_side.target_len, 0.13, 0.3); - // 加速度限幅,防止键盘控制摔倒 - if (abs(chassis_cmd_recv.vx - chassis.target_v) / del_t < MAX_ACC_REF) - chassis.target_v = chassis_cmd_recv.vx; - else - chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t; - // 模型距离参考输入 - chassis.target_dist += chassis.target_v * del_t; - chassis.target_yaw = chassis_cmd_recv.offset_angle; // 云台和底盘对齐时电机编码器的单圈反馈角度 -} - - -/** - * @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体 - * - * @note HT04电机上电的编码器位置为零(校准过),请看Link2Pod()的note,以及HT04.c中的电机解码部分 - * @note 海泰04电机顺时针旋转为正; LK9025电机逆时针旋转为正,此处皆需要转换为模型中给定的正方向 - * - */ -static void ParamAssemble() -{ - // 机体参数,视为平面刚体 - chassis.pitch = (-imu_data->Pitch + BALANCE_GRAVITY_BIAS) * DEGREE_2_RAD; - chassis.pitch_w = -imu_data->Gyro[0]; - chassis.yaw = imu_data->YawTotalAngle * DEGREE_2_RAD; - chassis.wz = imu_data->Gyro[2]; - chassis.roll = imu_data->Roll * DEGREE_2_RAD + ROLL_GRAVITY_BIAS; - chassis.roll_w = imu_data->Gyro[1]; - - // HT04电机的角度是顺时针为正,LK9025电机的角度是逆时针为正 - l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_angle; - l_side.phi4 = -lf->measure.total_angle - LIMIT_LINK_RAD; - l_side.phi1_w = -lb->measure.speed_rads; - l_side.phi4_w = -lf->measure.speed_rads; - l_side.w_ecd = l_driven->measure.speed_rads; - r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_angle; - r_side.phi4 = rf->measure.total_angle - LIMIT_LINK_RAD; - r_side.phi1_w = rb->measure.speed_rads; - r_side.phi4_w = rf->measure.speed_rads; - r_side.w_ecd = -r_driven->measure.speed_rads; -} - - -/* 腿部控制:抗劈叉; 轮子控制:转向 */ -static void SynthesizeMotion() -{ - // 跟随云台yaw - if (chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW || - chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG) // 角度环 - { - float p_ref = PIDCalculate(&steer_p_pid, chassis_cmd_recv.offset_angle, 0); - PIDCalculate(&steer_v_pid, chassis.wz, p_ref); // 双环 - } - else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 速度环 - PIDCalculate(&steer_v_pid, chassis.wz, 4); - - l_side.T_wheel -= steer_v_pid.Output; - r_side.T_wheel += steer_v_pid.Output; - - // 抗劈叉 - volatile static float swerving_speed_ff, ff_coef = 3; - swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈 - PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0); - l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff; - r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff; -} - - -/* 腿长控制和Roll补偿 */ -static void LegControl() -{ - PIDCalculate(&roll_compensate_pid, chassis.roll, 0); - l_side.target_len -= roll_compensate_pid.Output; - r_side.target_len += roll_compensate_pid.Output; - - static float gravity_comp = 0; - static float roll_extra_comp_p = 0; - float roll_comp = roll_extra_comp_p * chassis.roll; - l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp + roll_comp; - r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp - roll_comp; - - // @todo: 还需要加和roll的纯Kp项 -} - - -/* 设定运动模态的输出 */ -static void WattLimitSet() -{ - HTMotorSetRef(lf, 0.2857 * -l_side.T_front); // 根据扭矩常数计算得到的系数 - HTMotorSetRef(lb, 0.2857 * -l_side.T_back); - HTMotorSetRef(rf, 0.2857 * r_side.T_front); - HTMotorSetRef(rb, 0.2857 * r_side.T_back); - LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel); - LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel); -} - - -void BalanceTask() -{ - del_t = DWT_GetDeltaT(&balance_dwt_cnt); - - // 切换遥控器控制or云台板控制 - ControlSwitch(); - -} \ No newline at end of file diff --git a/application/chassis/balance.h b/application/chassis/balance.h index f652056..5703405 100644 --- a/application/chassis/balance.h +++ b/application/chassis/balance.h @@ -42,14 +42,17 @@ typedef struct // joint float phi1_w, phi4_w, phi2_w, phi5_w; // phi2_w used for calc real wheel speed float T_back, T_front; + // link angle, phi1-ph5, phi5 is pod angle float phi1, phi2, phi3, phi4, phi5; + // wheel float w_ecd; // 电机编码器速度 float wheel_dist; // 单侧轮子的位移 float wheel_w; // 单侧轮子的速度 float body_v; // 髋关节速度 float T_wheel; + // pod float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一 float leg_len, legd; @@ -59,22 +62,25 @@ typedef struct float coord[6]; // xb yb xc yc xd yd - float wheel_out[7]; - float hip_out[7]; } LinkNPodParam; typedef struct { + // 速度 float vel, target_v; // 底盘速度 float vel_m; // 底盘速度测量值 float vel_predict; // 底盘速度预测值 float vel_cov; // 速度方差 - float acc, acc_m, acc_last; // 水平方向加速度,用于计算速度预测值 + float acc_m, acc_last; // 水平方向加速度,用于计算速度预测值 + // 位移 float dist, target_dist; // 底盘位移距离 + + // IMU float yaw, wz, target_yaw; // yaw角度和底盘角速度 float pitch, pitch_w; // 底盘俯仰角度和角速度 float roll, roll_w; // 底盘横滚角度和角速度 + } ChassisParam; /** diff --git a/application/chassis/linkNleg.h b/application/chassis/linkNleg.h index 409cbbc..b8c6bb7 100644 --- a/application/chassis/linkNleg.h +++ b/application/chassis/linkNleg.h @@ -74,6 +74,6 @@ void Link2Leg(LinkNPodParam *p, ChassisParam *chassis) p->phi2_w = (phi2_pred - p->phi2) / predict_dt; // 稍后用于修正轮速 p->phi5_w = (phi5_pred - p->phi5) / predict_dt; p->legd = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->leg_len) / predict_dt; - p->theta_w = ((phi5_pred - 0.5 * PI - (chassis->pitch + chassis->pitch_w) - p->theta) / predict_dt); // 可以不考虑机体? -predict_dt*chassis.pitch_w + p->theta_w = ((phi5_pred - 0.5 * PI - (chassis->pitch + chassis->pitch_w * predict_dt) - p->theta) / predict_dt); // 可以不考虑机体? -predict_dt*chassis.pitch_w p->height_v = p->legd * mcos(p->theta) - p->leg_len * msin(p->theta) * p->theta_w; } diff --git a/application/chassis/lqr_calc.h b/application/chassis/lqr_calc.h index b2ca080..e32d620 100644 --- a/application/chassis/lqr_calc.h +++ b/application/chassis/lqr_calc.h @@ -9,8 +9,8 @@ */ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis) { - static float k[12][3] = {}; - float T[2] = {0}; // 0 T_wheel 1 T_hip + float k[12][3] = {0}; + float T[2] = {0}; // 0 T_wheel 1 T_hip float l = p->leg_len; float lsqr = l * l; diff --git a/application/chassis/speed_estimation.h b/application/chassis/speed_estimation.h deleted file mode 100644 index 836c37d..0000000 --- a/application/chassis/speed_estimation.h +++ /dev/null @@ -1,100 +0,0 @@ -#include "balance.h" -#include "user_lib.h" -#include "ins_task.h" -#include "general_def.h" - -#define EST_FINAL_LPF 0.005f // 最终速度的低通滤波系数 -static KalmanFilter_t kf; - -/** - * @brief 底盘为右手系 - * - * ^ y 左轮 右轮 - * | | | 前 - * |_____ > x |------| - * z 轴从屏幕向外 | | 后 - */ - -void SpeedEstInit() -{ - // 使用kf同时估计速度和加速度 - // Kalman_Filter_Init(&kf, 2, 0, 2); - // float F[4] = {1, 0.001, 0, 1}; - // float Q[4] = {VEL_PROCESS_NOISE, 0, 0, ACC_PROCESS_NOISE}; - // float R[4] = {VEL_MEASURE_NOISE, 0, 0, ACC_MEASURE_NOISE}; - // float P[4] = {100000, 0, 0, 100000}; - // float H[4] = {1, 0, 0, 1}; - // memcpy(kf.F_data, F, sizeof(F)); - // memcpy(kf.Q_data, Q, sizeof(Q)); - // memcpy(kf.R_data, R, sizeof(R)); - // memcpy(kf.P_data, P, sizeof(P)); - // memcpy(kf.H_data, H, sizeof(H)); -} - -/** - * @brief 使用卡尔曼滤波估计底盘速度 - * @todo 增加w和dw的滤波,当w和dw均小于一定值时,不考虑dw导致的角加速度 - * - * @param lp 左侧腿 - * @param rp 右侧腿 - * @param cp 底盘 - * @param imu imu数据 - * @param delta_t 更新间隔 - */ -void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS_t *imu, float delta_t) -{ - // 修正轮速和距离 - lp->wheel_w = lp->w_ecd + lp->phi2_w - cp->pitch_w; // 减去和定子固连的phi2_w - rp->wheel_w = rp->w_ecd + rp->phi2_w - cp->pitch_w; - - // 直接使用轮速反馈,不进行速度融合 - // cp->vel = (lp->wheel_w + rp->wheel_w) * WHEEL_RADIUS / 2; - // cp->dist = cp->dist + cp->vel * delta_t; - - // 以轮子为基点,计算机体两侧髋关节处的速度 - lp->body_v = lp->wheel_w * WHEEL_RADIUS + lp->leg_len * lp->theta_w + lp->legd * msin(lp->theta); - rp->body_v = rp->wheel_w * WHEEL_RADIUS + rp->leg_len * rp->theta_w + rp->legd * msin(rp->theta); - cp->vel_m = (lp->body_v + rp->body_v) / 2; // 机体速度(平动)为两侧速度的平均值 - - // 扣除旋转导致的向心加速度和角加速度*R - float *gyro = imu->Gyro, *dgyro = imu->dgyro; - static float yaw_ddwrNwwr, yaw_p_ddwrNwwr, pitch_ddwrNwwr; - static float macc_y, macc_z; // 补偿后的实际平动加速度,机体系前进方向和竖直方向 - yaw_ddwrNwwr = powf(gyro[Z], 2) * CENTER_IMU_W - dgyro[Z] * CENTER_IMU_L; // yaw旋转导致motion_acc[1]的额外加速度(机体前后方向) - yaw_p_ddwrNwwr = powf(gyro[X], 2) * CENTER_IMU_W + dgyro[X] * CENTER_IMU_H; // pitch旋转导致motion_acc[1]的额外加速度(机体前后方向) - pitch_ddwrNwwr = powf(gyro[X], 2) * CENTER_IMU_H - dgyro[X] * CENTER_IMU_W; // pitch旋转导致motion_acc[2]的额外加速度(机体竖直方向) - macc_y = -imu->MotionAccel_b[Y] - yaw_ddwrNwwr - yaw_p_ddwrNwwr; - macc_z = imu->MotionAccel_b[Z] - pitch_ddwrNwwr; - - float pitch = imu->Pitch * DEGREE_2_RAD; - cp->acc_last = cp->acc_m; - cp->acc_m = macc_y * mcos(pitch) - macc_z * msin(pitch); // 绝对系下的平动加速度,即机体系下的加速度投影到绝对系 - - // for debug 对比修正前后的加速度 - static float ry, rz, rawaa; - ry = -imu->MotionAccel_b[Y]; - rz = imu->MotionAccel_b[Z]; - rawaa = ry * mcos(pitch) - rz * msin(pitch); - - // 使用kf同时估计加速度和速度,滤波更新 - // kf.MeasuredVector[0] = cp->vel_m; - // kf.MeasuredVector[1] = cp->acc_m; - // kf.F_data[1] = delta_t; // 更新F矩阵 - // Kalman_Filter_Update(&kf); - // cp->vel = kf.xhat_data[0]; - // cp->acc = kf.xhat_data[1]; - - // 融合加速度计的数据和机体速度 - static float f, k, prior, measure, cov; - f = (cp->acc_m + cp->acc_last) / 2; // 速度梯形积分 - prior = cp->vel + f * delta_t; // x' = Fx,先验估计 - cp->vel_predict = prior; - measure = cp->vel_m; // 测量值 - cov = cp->vel_cov + VEL_PROCESS_NOISE * delta_t; // P' = P + Q ,先验协方差 - cp->vel_cov = cov; - k = cov / (cov + VEL_MEASURE_NOISE); // K = P'/(P'+R),卡尔曼增益 - cp->vel = prior + k * (measure - prior); // x^ = x'+K(z-x'),后验估计 - cp->vel_cov *= (1 - k); // P^ = (1-K)P',后验协方差 - VAL_LIMIT(cp->vel_cov, 0.01, 100); // 协方差限幅 - cp->dist = cp->dist + cp->vel * delta_t; -} \ No newline at end of file diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index ecb1f57..6e5ce71 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -111,7 +111,7 @@ attitude_t *INS_Init(void) // noise of accel is relatively big and of high freq,thus lpf is used INS.AccelLPF = 0.0085; - INS.DGyroLPF = 0.008; + INS.DGyroLPF = 0.009; DWT_GetDeltaT(&INS_DWT_Count); return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复. } diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h index a4e2073..46772a2 100644 --- a/modules/imu/ins_task.h +++ b/modules/imu/ins_task.h @@ -44,7 +44,7 @@ typedef struct float MotionAccel_n[3]; // 绝对系加速度 float AccelLPF; // 加速度低通滤波系数 - float DGyroLPF; + float DGyroLPF; // 角加速度低通滤波系数 // bodyframe在绝对系的向量表示 float xn[3]; @@ -57,7 +57,7 @@ typedef struct // IMU量测值 float Gyro[3]; // 角速度 - float dgyro[3]; + float dgyro[3]; // 角加速度 float Accel[3]; // 加速度 // 位姿 float Roll;