添加速度估计，未进行速度融合

application/chassis/balance.c

@@ -17,6 +17,7 @@
 #include "bsp_dwt.h"
 #include "bsp_log.h"
 #include "lqr_calc.h"
+#include "speed_estimation.h"
 
 // 计时变量
 static uint32_t balance_dwt_cnt;
@@ -190,6 +191,8 @@ void BalanceTask()
     // 将五连杆映射成单杆
     Link2Leg(&l_side, &chassis);
     Link2Leg(&r_side, &chassis);
+    // 通过卡尔曼滤波估计机体速度
+    SpeedEstimation(&l_side, &r_side, &chassis, Chassis_IMU_data, del_t);
     // 根据单杆计算处的角度和杆长,计算反馈增益
     CalcLQR(&l_side, &chassis);
     CalcLQR(&r_side, &chassis);

application/chassis/balance.h

@@ -4,7 +4,7 @@
 #define CALF_LEN 0.24f              // 小腿
 #define THIGH_LEN 0.14f             // 大腿
 #define JOINT_DISTANCE 0.11f        // 关节间距
-#define WHEEL_RADIUS 0.06925f       // 轮子半径
+#define WHEEL_RADIUS 0.075f         // 轮子半径
 #define LIMIT_LINK_RAD 0.205467224      // 初始限位角度,见ParamAssemble
 #define BALANCE_GRAVITY_BIAS 0
 #define ROLL_GRAVITY_BIAS 0

application/chassis/speed_estimation.h

@@ -0,0 +1,28 @@
+#include "balance.h"
+#include "user_lib.h"
+#include "ins_task.h"
+#include "general_def.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;
+
+    // 以轮子为基点,计算机体两侧髋关节处的速度
+    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->dist = cp->dist + cp->vel * delta_t;
+}