From 729df0f9880d87ec139daccc6524f6cb48771023 Mon Sep 17 00:00:00 2001
From: kai <1797003616@qq.com>
Date: Sun, 24 Mar 2024 11:45:53 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=80=9F=E5=BA=A6=E4=BC=B0?=
 =?UTF-8?q?=E8=AE=A1=EF=BC=8C=E6=9C=AA=E8=BF=9B=E8=A1=8C=E9=80=9F=E5=BA=A6?=
 =?UTF-8?q?=E8=9E=8D=E5=90=88?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c          |  3 +++
 application/chassis/balance.h          |  2 +-
 application/chassis/speed_estimation.h | 28 ++++++++++++++++++++++++++
 3 files changed, 32 insertions(+), 1 deletion(-)
 create mode 100644 application/chassis/speed_estimation.h

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index b633321..479b49c 100644
--- a/application/chassis/balance.c
+++ b/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);
diff --git a/application/chassis/balance.h b/application/chassis/balance.h
index d23749b..9c4beda 100644
--- a/application/chassis/balance.h
+++ b/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
diff --git a/application/chassis/speed_estimation.h b/application/chassis/speed_estimation.h
new file mode 100644
index 0000000..10d03f7
--- /dev/null
+++ b/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;
+}
\ No newline at end of file