From 5b7c5cf88fb7cd5f7a65036649d7cd0077cc81d6 Mon Sep 17 00:00:00 2001 From: kai <1797003616@qq.com> Date: Sun, 24 Mar 2024 20:41:40 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=80=9F=E5=BA=A6=E8=9E=8D?= =?UTF-8?q?=E5=90=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/balance.c | 1 + application/chassis/balance.h | 6 ++--- application/chassis/speed_estimation.h | 37 +++++++++++++++++++++++++- 3 files changed, 40 insertions(+), 4 deletions(-) diff --git a/application/chassis/balance.c b/application/chassis/balance.c index c2e63ea..c0d82c6 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -120,6 +120,7 @@ void BalanceInit() // 状态初始化 l_side.target_len = r_side.target_len = 0.12; + chassis.vel_cov = 100; // 速度协方差初始化 chassis_status = ROBOT_READY; DWT_GetDeltaT(&balance_dwt_cnt); } diff --git a/application/chassis/balance.h b/application/chassis/balance.h index 9c4beda..f7d436c 100644 --- a/application/chassis/balance.h +++ b/application/chassis/balance.h @@ -12,10 +12,10 @@ #define MAX_DIST_TRACK 0.1f #define MAX_VEL_TRACK 0.5f -#define CENTER_IMU_R 0.09f // IMU距离中心的距离 +// IMU距离中心的距离 #define CENTER_IMU_W 0 -#define CENTER_IMU_L 0.09f -#define CENTER_IMU_H 0 +#define CENTER_IMU_L 0.1f +#define CENTER_IMU_H -0.055f #define VEL_PROCESS_NOISE 25 // 速度过程噪声 #define VEL_MEASURE_NOISE 800 // 速度测量噪声 diff --git a/application/chassis/speed_estimation.h b/application/chassis/speed_estimation.h index 10d03f7..e50c785 100644 --- a/application/chassis/speed_estimation.h +++ b/application/chassis/speed_estimation.h @@ -23,6 +23,41 @@ void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS // 以轮子为基点,计算机体两侧髋关节处的速度 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 = cp->vel_m = (lp->body_v + rp->body_v) / 2; // 机体速度(平动)为两侧速度的平均值 + 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; + yaw_ddwrNwwr = -powf(gyro[Z], 2) * CENTER_IMU_L + dgyro[Z] * CENTER_IMU_W; // yaw旋转导致motion_acc[1]的额外加速度(机体前后方向) + yaw_p_ddwrNwwr = -powf(gyro[X], 2) * CENTER_IMU_L - dgyro[X] * CENTER_IMU_H; // pitch旋转导致motion_acc[1]的额外加速度(机体前后方向) + pitch_ddwrNwwr = -powf(gyro[X], 2) * CENTER_IMU_H + dgyro[X] * CENTER_IMU_L; // pitch旋转导致motion_acc[2]的额外加速度(机体竖直方向) + + // 补偿后的实际平动加速度,机体系前进方向和竖直方向 + static float macc_y, macc_z; + macc_y = imu->MotionAccel_b[Y] - yaw_ddwrNwwr - yaw_p_ddwrNwwr; + macc_z = imu->MotionAccel_b[Z] - pitch_ddwrNwwr; + + // 机体加速度投影到水平方向上 + static float pitch; + pitch = imu->Pitch * DEGREE_2_RAD; + cp->acc_last = cp->acc_m; + cp->acc_m = macc_y * mcos(pitch) - macc_z * msin(pitch); + + // 融合加速度计的数据和机体速度 + static float u, k; // 输入和卡尔曼增益 + static float vel_prior, vel_measure, vel_cov; // 先验估计、测量、先验协方差 + + // 预测 + u = (cp->acc_m + cp->acc_last) / 2; // 速度梯形积分 + cp->vel_predict = vel_prior = cp->vel + delta_t * u; // 先验估计 + vel_cov = cp->vel_cov + VEL_PROCESS_NOISE * delta_t; // 先验协方差 + + // 校正 + vel_measure = cp->vel_m; + k = vel_cov / (vel_cov + VEL_MEASURE_NOISE); // 卡尔曼增益 + cp->vel = vel_prior + k * (vel_measure - vel_prior); // 后验估计 + cp->vel_cov = (1 - k) * vel_cov; // 后验协方差 + + VAL_LIMIT(cp->vel_cov, 0.01, 100); // 协方差限幅 cp->dist = cp->dist + cp->vel * delta_t; } \ No newline at end of file