diff --git a/LQR_README.md b/LQR_README.md
new file mode 100644
index 0000000..05f4280
--- /dev/null
+++ b/LQR_README.md
@@ -0,0 +1,185 @@
+# LQR控制器设计文档
+
+本文档描述了为平衡步兵轮腿机器人设计的LQR（线性二次调节器）控制器。
+
+## 1. 系统建模
+
+### 1.1 状态变量定义
+
+系统状态向量为6维：
+```
+x = [theta, d_theta, x, d_x, phi, d_phi]^T
+```
+
+其中：
+- `theta`: 等效摆杆与竖直方向夹角 (rad)
+- `d_theta`: 等效摆杆角速度 (rad/s)
+- `x`: 驱动轮位移 (m)
+- `d_x`: 驱动轮速度 (m/s)  
+- `phi`: 机体与水平夹角（俯仰角） (rad)
+- `d_phi`: 机体俯仰角速度 (rad/s)
+
+### 1.2 控制输入
+
+控制输入为2维：
+```
+u = [T, Tp]^T
+```
+
+其中：
+- `T`: 驱动轮输出力矩 (N·m)
+- `Tp`: 髋关节输出力矩 (N·m)
+
+### 1.3 系统动力学方程
+
+基于拉格朗日方程建立的系统动力学方程组：
+
+1. **驱动轮动力学**：
+   ```
+   d²x/dt² = (T - N*R)/(Iw/R + mw*R)
+   ```
+
+2. **摆杆动力学**：
+   ```
+   Ip*d²theta/dt² = (P*L + PM*LM)*sin(theta) - (N*L + NM*LM)*cos(theta) - T + Tp
+   ```
+
+3. **机体动力学**：
+   ```
+   IM*d²phi/dt² = Tp + NM*l*cos(phi) + PM*l*sin(phi)
+   ```
+
+### 1.4 线性化
+
+在平衡点附近进行线性化，得到状态空间模型：
+```
+dx/dt = A*x + B*u
+```
+
+其中A和B矩阵通过雅可比矩阵计算得到。
+
+## 2. LQR控制器设计
+
+### 2.1 代价函数
+
+LQR控制器最小化以下二次代价函数：
+```
+J = ∫[x^T*Q*x + u^T*R*u]dt
+```
+
+权重矩阵选择：
+- `Q = diag([100, 1, 500, 100, 5000, 1])` (状态权重)
+- `R = diag([240, 25])` (控制权重)
+
+### 2.2 增益矩阵计算
+
+由于系统参数随腿长变化，采用多项式拟合方法：
+
+1. **离线计算**：对不同腿长（0.1-0.4m），使用MATLAB计算最优LQR增益
+2. **多项式拟合**：对每个增益元素进行3阶多项式拟合
+3. **在线计算**：根据当前腿长实时计算增益矩阵
+
+### 2.3 控制律
+
+```
+u = -K(L) * (x - x_ref)
+```
+
+其中：
+- `K(L)`: 与腿长L相关的增益矩阵
+- `x_ref`: 参考状态
+
+## 3. 实现架构
+
+### 3.1 文件结构
+
+```
+User/component/
+├── lqr.h                    # LQR控制器头文件
+├── lqr.c                    # LQR控制器实现
+└── lqr_config_example.c     # 配置示例
+```
+
+### 3.2 主要功能模块
+
+1. **LQR_Init()**: 初始化控制器
+2. **LQR_UpdateState()**: 更新系统状态
+3. **LQR_CalculateGainMatrix()**: 计算增益矩阵
+4. **LQR_Control()**: 执行控制计算
+5. **LQR_GetOutput()**: 获取控制输出
+
+### 3.3 与VMC的集成
+
+LQR控制器输出虚拟力，通过VMC转换为关节力矩：
+
+```c
+// LQR计算虚拟力和力矩
+LQR_Control(&chassis->lqr);
+LQR_GetOutput(&chassis->lqr, &lqr_output);
+
+// VMC将虚拟力矩转换为关节力矩
+VMC_InverseSolve(&chassis->vmc[0], virtual_force, lqr_output.Tp);
+VMC_GetJointTorques(&chassis->vmc[0], &tau_front, &tau_rear);
+```
+
+## 4. 调试和优化
+
+### 4.1 参数调优
+
+1. **Q矩阵调优**：
+   - 增大theta权重提高平衡性能
+   - 增大phi权重减少机体摆动
+   - 增大x,d_x权重提高位置跟踪精度
+
+2. **R矩阵调优**：
+   - 增大R减少控制输出，提高稳定性
+   - 减小R提高响应速度
+
+### 4.2 常见问题
+
+1. **增益过大**：导致系统震荡，需减小Q或增大R
+2. **响应迟缓**：增大Q权重或减小R权重
+3. **静态误差**：检查目标状态设置是否合理
+
+### 4.3 监控指标
+
+建议监控以下指标：
+- 状态误差大小
+- 控制输出饱和情况
+- 系统稳定性指标
+
+## 5. 使用示例
+
+```c
+// 1. 配置LQR参数
+Chassis_Params_t chassis_params = {
+    .lqr_param = {
+        .max_wheel_torque = 10.0f,
+        .max_joint_torque = 5.0f,
+    },
+    .lqr_gains = example_lqr_gains,
+};
+
+// 2. 初始化底盘（自动初始化LQR）
+Chassis_Init(&chassis, &chassis_params, 1000.0f);
+
+// 3. 控制循环
+while(1) {
+    // 更新传感器数据
+    Chassis_UpdateFeedback(&chassis);
+    Chassis_UpdateIMU(&chassis, imu_data);
+    
+    // 执行LQR控制
+    Chassis_Control(&chassis, &cmd);
+    
+    // 延时
+    osDelay(1);
+}
+```
+
+## 6. 参考资料
+
+1. MATLAB仿真代码：`utils/Simulation-master/balance/series_legs/`
+2. C++参考实现：`User/module/mod_wheelleg_chassis.cpp`
+3. VMC控制器：`User/component/vmc.c`
+4. 理论参考：Modern Control Engineering, Ogata
diff --git a/User/component/lqr.c b/User/component/lqr.c
index 1cb1eae..fbaad2c 100644
--- a/User/component/lqr.c
+++ b/User/component/lqr.c
@@ -1,229 +1,265 @@
 /*
- * LQR线性二次型最优控制器简化实现
- * 
- * 本文件实现了轮腿机器人的LQR (Linear Quadratic Regulator) 控制算法
- * 主要功能包括:
- * 1. 状态反馈控制
- * 2. 增益矩阵K计算控制输出
- * 3. 控制输出限幅
- * 
- * 系统模型:
- * u = -K*(x - x_ref) (状态反馈)
+ * LQR控制器实现
  */
 
 #include "lqr.h"
 #include <string.h>
 
-/* Private typedef ---------------------------------------------------------- */
-/* Private define ----------------------------------------------------------- */
+/* Private macros ----------------------------------------------------------- */
+#define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
 
-#define LQR_EPSILON             (1e-6f)     // 数值计算精度
-#define LQR_DEFAULT_MAX_WHEEL   (50.0f)     // 默认最大轮毂力矩 (N*m)
-#define LQR_DEFAULT_MAX_JOINT   (30.0f)     // 默认最大关节力矩 (N*m)
-
-/* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
+
+/* 从MATLAB仿真get_k.m得到的默认增益矩阵多项式拟合系数 */
+/* 这些系数是通过对不同腿长的LQR增益进行3阶多项式拟合得到的 */
+static LQR_GainMatrix_t default_gain_matrix = {
+    /* K矩阵第一行 - 轮毂力矩T的增益系数 */
+    .k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f},      /* theta */
+    .k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f},            /* d_theta */
+    .k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f},       /* x */
+    .k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f},          /* d_x */
+    .k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f},   /* phi */
+    .k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f},            /* d_phi */
+    
+    /* K矩阵第二行 - 髋关节力矩Tp的增益系数 */
+    .k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f},          /* theta */
+    .k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f},             /* d_theta */
+    .k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f},         /* x */
+    .k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f},            /* d_x */
+    .k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f},       /* phi */
+    .k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f},             /* d_phi */
+};
+
 /* Private function prototypes ---------------------------------------------- */
+static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr);
+static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr);
 
-static void LQR_MatrixMultiply(const float K[4][10], const float state_error[10], float result[4]);
-static float LQR_ComputeStateError(float current, float reference);
+/* Public functions --------------------------------------------------------- */
 
-/* Exported functions ------------------------------------------------------- */
-
-/**
- * @brief 初始化LQR控制器
- */
 int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque) {
-    if (lqr == NULL || max_wheel_torque <= 0 || max_joint_torque <= 0) {
+    if (lqr == NULL) {
         return -1;
     }
     
-    // 设置力矩限制
-    lqr->max_wheel_torque = max_wheel_torque;
-    lqr->max_joint_torque = max_joint_torque;
+    /* 清零结构体 */
+    memset(lqr, 0, sizeof(LQR_Controller_t));
     
-    // 重置状态
-    LQR_Reset(lqr);
+    /* 设置控制限制 */
+    lqr->param.max_wheel_torque = max_wheel_torque;
+    lqr->param.max_joint_torque = max_joint_torque;
+    
+    /* 设置默认系统物理参数（从MATLAB仿真get_k_length.m获取） */
+    lqr->param.R = 0.072f;          /* 驱动轮半径 */
+    lqr->param.l = 0.03f;           /* 机体重心到转轴距离 */
+    lqr->param.mw = 0.60f;          /* 驱动轮质量 */
+    lqr->param.mp = 1.8f;           /* 摆杆质量 */
+    lqr->param.M = 1.8f;            /* 机体质量 */
+    lqr->param.g = 9.8f;            /* 重力加速度 */
+    
+    /* 计算转动惯量 */
+    lqr->param.Iw = lqr->param.mw * lqr->param.R * lqr->param.R;
+    lqr->param.IM = lqr->param.M * (0.3f * 0.3f + 0.12f * 0.12f) / 12.0f;
+    
+    /* 设置默认增益矩阵 */
+    lqr->gain_matrix = &default_gain_matrix;
+    
+    /* 设置默认目标状态（平衡状态） */
+    memset(&lqr->param.target_state, 0, sizeof(LQR_State_t));
+    
+    /* 初始化当前腿长为中等值 */
+    lqr->current_leg_length = 0.25f;
+    
+    /* 计算初始增益矩阵 */
+    LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
     
     lqr->initialized = true;
     
     return 0;
 }
 
-/**
- * @brief 设置固定LQR增益矩阵
- */
-int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, const LQR_GainMatrix_t *K) {
-    if (lqr == NULL || !lqr->initialized || K == NULL) {
+int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix) {
+    if (lqr == NULL || gain_matrix == NULL) {
         return -1;
     }
     
-    // 复制增益矩阵
-    memcpy(&lqr->K, K, sizeof(LQR_GainMatrix_t));
+    lqr->gain_matrix = gain_matrix;
     
-    return 0;
+    /* 重新计算增益矩阵 */
+    return LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
 }
 
-/**
- * @brief 更新机器人状态
- */
 int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
-    if (lqr == NULL || !lqr->initialized || state == NULL) {
+    if (lqr == NULL || state == NULL) {
         return -1;
     }
     
-    // 复制状态，并对角度进行归一化
-    lqr->state = *state;
-    LQR_ANGLE_NORMALIZE(lqr->state.phi);
-    LQR_ANGLE_NORMALIZE(lqr->state.theta_ll);
-    LQR_ANGLE_NORMALIZE(lqr->state.theta_lr);
-    LQR_ANGLE_NORMALIZE(lqr->state.theta_b);
+    /* 更新当前状态 */
+    lqr->current_state = *state;
+    
+    /* 计算状态误差 */
+    return LQR_CalculateErrorState(lqr);
+}
+
+int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state) {
+    if (lqr == NULL || target_state == NULL) {
+        return -1;
+    }
+    
+    lqr->param.target_state = *target_state;
+    
+    /* 重新计算状态误差 */
+    return LQR_CalculateErrorState(lqr);
+}
+
+int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) {
+    if (lqr == NULL || lqr->gain_matrix == NULL) {
+        return -1;
+    }
+    
+    /* 限制腿长范围 */
+    leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f);
+    lqr->current_leg_length = leg_length;
+    
+    /* 更新与腿长相关的物理参数 */
+    lqr->param.L = leg_length / 2.0f;           /* 摆杆重心到驱动轮轴距离 */
+    lqr->param.LM = leg_length / 2.0f;          /* 摆杆重心到其转轴距离 */
+    
+    /* 计算摆杆转动惯量 */
+    float leg_total_length = lqr->param.L + lqr->param.LM;
+    lqr->param.Ip = lqr->param.mp * (leg_total_length * leg_total_length + 0.05f * 0.05f) / 12.0f;
+    
+    /* 使用多项式拟合计算当前增益矩阵 */
+    lqr->K_matrix[0][0] = LQR_PolynomialCalc(lqr->gain_matrix->k11_coeff, leg_length);  /* K11: theta */
+    lqr->K_matrix[0][1] = LQR_PolynomialCalc(lqr->gain_matrix->k12_coeff, leg_length);  /* K12: d_theta */
+    lqr->K_matrix[0][2] = LQR_PolynomialCalc(lqr->gain_matrix->k13_coeff, leg_length);  /* K13: x */
+    lqr->K_matrix[0][3] = LQR_PolynomialCalc(lqr->gain_matrix->k14_coeff, leg_length);  /* K14: d_x */
+    lqr->K_matrix[0][4] = LQR_PolynomialCalc(lqr->gain_matrix->k15_coeff, leg_length);  /* K15: phi */
+    lqr->K_matrix[0][5] = LQR_PolynomialCalc(lqr->gain_matrix->k16_coeff, leg_length);  /* K16: d_phi */
+    
+    lqr->K_matrix[1][0] = LQR_PolynomialCalc(lqr->gain_matrix->k21_coeff, leg_length);  /* K21: theta */
+    lqr->K_matrix[1][1] = LQR_PolynomialCalc(lqr->gain_matrix->k22_coeff, leg_length);  /* K22: d_theta */
+    lqr->K_matrix[1][2] = LQR_PolynomialCalc(lqr->gain_matrix->k23_coeff, leg_length);  /* K23: x */
+    lqr->K_matrix[1][3] = LQR_PolynomialCalc(lqr->gain_matrix->k24_coeff, leg_length);  /* K24: d_x */
+    lqr->K_matrix[1][4] = LQR_PolynomialCalc(lqr->gain_matrix->k25_coeff, leg_length);  /* K25: phi */
+    lqr->K_matrix[1][5] = LQR_PolynomialCalc(lqr->gain_matrix->k26_coeff, leg_length);  /* K26: d_phi */
     
     return 0;
 }
 
-/**
- * @brief 设置参考状态
- */
-int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_State_t *reference) {
-    if (lqr == NULL || !lqr->initialized || reference == NULL) {
-        return -1;
-    }
-    
-    // 复制参考状态，并对角度进行归一化
-    lqr->reference = *reference;
-    LQR_ANGLE_NORMALIZE(lqr->reference.phi);
-    LQR_ANGLE_NORMALIZE(lqr->reference.theta_ll);
-    LQR_ANGLE_NORMALIZE(lqr->reference.theta_lr);
-    LQR_ANGLE_NORMALIZE(lqr->reference.theta_b);
-    
-    return 0;
-}
-
-/**
- * @brief 计算LQR控制输出
- * 
- * 实现状态反馈控制律: u = -K*(x - x_ref)
- */
-int8_t LQR_ComputeControl(LQR_Controller_t *lqr) {
+int8_t LQR_Control(LQR_Controller_t *lqr) {
     if (lqr == NULL || !lqr->initialized) {
         return -1;
     }
     
-    // 计算状态误差向量
-    float state_error[10];
-    state_error[0] = LQR_ComputeStateError(lqr->state.s, lqr->reference.s);
-    state_error[1] = LQR_ComputeStateError(lqr->state.ds, lqr->reference.ds);
-    state_error[2] = LQR_ComputeStateError(lqr->state.phi, lqr->reference.phi);
-    state_error[3] = LQR_ComputeStateError(lqr->state.dphi, lqr->reference.dphi);
-    state_error[4] = LQR_ComputeStateError(lqr->state.theta_ll, lqr->reference.theta_ll);
-    state_error[5] = LQR_ComputeStateError(lqr->state.dtheta_ll, lqr->reference.dtheta_ll);
-    state_error[6] = LQR_ComputeStateError(lqr->state.theta_lr, lqr->reference.theta_lr);
-    state_error[7] = LQR_ComputeStateError(lqr->state.dtheta_lr, lqr->reference.dtheta_lr);
-    state_error[8] = LQR_ComputeStateError(lqr->state.theta_b, lqr->reference.theta_b);
-    state_error[9] = LQR_ComputeStateError(lqr->state.dtheta_b, lqr->reference.dtheta_b);
+    /* LQR控制律: u = -K * x_error
+     * 其中 u = [T; Tp], x_error = x_current - x_target
+     */
     
-    // 计算控制输出: u = -K * (x - x_ref)
-    float control_vector[4];
-    LQR_MatrixMultiply(lqr->K.K, state_error, control_vector);
+    /* 计算轮毂力矩T */
+    lqr->control_output.T = -(
+        lqr->K_matrix[0][0] * lqr->error_state.theta +
+        lqr->K_matrix[0][1] * lqr->error_state.d_theta +
+        lqr->K_matrix[0][2] * lqr->error_state.x +
+        lqr->K_matrix[0][3] * lqr->error_state.d_x +
+        lqr->K_matrix[0][4] * lqr->error_state.phi +
+        lqr->K_matrix[0][5] * lqr->error_state.d_phi
+    );
     
-    // 应用负反馈并限幅
-    lqr->control.T_wl = LQR_CLAMP(-control_vector[0], -lqr->max_wheel_torque, lqr->max_wheel_torque);
-    lqr->control.T_wr = LQR_CLAMP(-control_vector[1], -lqr->max_wheel_torque, lqr->max_wheel_torque);
-    lqr->control.T_bl = LQR_CLAMP(-control_vector[2], -lqr->max_joint_torque, lqr->max_joint_torque);
-    lqr->control.T_br = LQR_CLAMP(-control_vector[3], -lqr->max_joint_torque, lqr->max_joint_torque);
+    /* 计算髋关节力矩Tp */
+    lqr->control_output.Tp = -(
+        lqr->K_matrix[1][0] * lqr->error_state.theta +
+        lqr->K_matrix[1][1] * lqr->error_state.d_theta +
+        lqr->K_matrix[1][2] * lqr->error_state.x +
+        lqr->K_matrix[1][3] * lqr->error_state.d_x +
+        lqr->K_matrix[1][4] * lqr->error_state.phi +
+        lqr->K_matrix[1][5] * lqr->error_state.d_phi
+    );
     
-    return 0;
+    /* 应用控制限制 */
+    return LQR_ApplyControlLimits(lqr);
 }
 
-/**
- * @brief 获取控制输出
- */
-int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control) {
-    if (lqr == NULL || !lqr->initialized || control == NULL) {
+int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output) {
+    if (lqr == NULL || output == NULL) {
         return -1;
     }
     
-    *control = lqr->control;
+    *output = lqr->control_output;
     return 0;
 }
 
-/**
- * @brief 重置LQR控制器状态
- */
-void LQR_Reset(LQR_Controller_t *lqr) {
+int8_t LQR_Reset(LQR_Controller_t *lqr) {
     if (lqr == NULL) {
-        return;
-    }
-    
-    // 清零状态和控制量
-    memset(&lqr->state, 0, sizeof(LQR_State_t));
-    memset(&lqr->reference, 0, sizeof(LQR_State_t));
-    memset(&lqr->control, 0, sizeof(LQR_Control_t));
-    memset(&lqr->K, 0, sizeof(LQR_GainMatrix_t));
-}
-
-/**
- * @brief 从轮腿机器人传感器数据构建LQR状态
- */
-int8_t LQR_BuildStateFromSensors(float position_x, float velocity_x,
-                                 float yaw_angle, float yaw_rate,
-                                 float left_leg_angle, float left_leg_rate,
-                                 float right_leg_angle, float right_leg_rate,
-                                 float body_pitch, float body_pitch_rate,
-                                 LQR_State_t *state) {
-    if (state == NULL) {
         return -1;
     }
     
-    state->s = position_x;
-    state->ds = velocity_x;
-    state->phi = yaw_angle;
-    state->dphi = yaw_rate;
-    state->theta_ll = left_leg_angle;
-    state->dtheta_ll = left_leg_rate;
-    state->theta_lr = right_leg_angle;
-    state->dtheta_lr = right_leg_rate;
-    state->theta_b = body_pitch;
-    state->dtheta_b = body_pitch_rate;
+    /* 清零状态和输出 */
+    memset(&lqr->current_state, 0, sizeof(LQR_State_t));
+    memset(&lqr->error_state, 0, sizeof(LQR_State_t));
+    memset(&lqr->control_output, 0, sizeof(LQR_Input_t));
     
-    // 角度归一化
-    LQR_ANGLE_NORMALIZE(state->phi);
-    LQR_ANGLE_NORMALIZE(state->theta_ll);
-    LQR_ANGLE_NORMALIZE(state->theta_lr);
-    LQR_ANGLE_NORMALIZE(state->theta_b);
+    /* 重置限幅标志 */
+    lqr->wheel_torque_limited = false;
+    lqr->joint_torque_limited = false;
     
     return 0;
 }
 
+float LQR_PolynomialCalc(const float *coeff, float leg_length) {
+    if (coeff == NULL) {
+        return 0.0f;
+    }
+    
+    /* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
+    float L = leg_length;
+    float L2 = L * L;
+    float L3 = L2 * L;
+    
+    return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
+}
+
 /* Private functions -------------------------------------------------------- */
 
-/**
- * @brief 矩阵向量乘法: result = K * state_error
- * 
- * K: 4x10矩阵
- * state_error: 10x1向量
- * result: 4x1向量
- */
-static void LQR_MatrixMultiply(const float K[4][10], const float state_error[10], float result[4]) {
-    for (int i = 0; i < 4; i++) {
-        result[i] = 0.0f;
-        for (int j = 0; j < 10; j++) {
-            result[i] += K[i][j] * state_error[j];
-        }
+static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr) {
+    if (lqr == NULL) {
+        return -1;
     }
+    
+    /* 计算状态误差 */
+    lqr->error_state.theta = lqr->current_state.theta - lqr->param.target_state.theta;
+    lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->param.target_state.d_theta;
+    lqr->error_state.x = lqr->current_state.x - lqr->param.target_state.x;
+    lqr->error_state.d_x = lqr->current_state.d_x - lqr->param.target_state.d_x;
+    lqr->error_state.phi = lqr->current_state.phi - lqr->param.target_state.phi;
+    lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->param.target_state.d_phi;
+    
+    return 0;
 }
 
-/**
- * @brief 计算状态误差(考虑角度周期性)
- */
-static float LQR_ComputeStateError(float current, float reference) {
-    float error = current - reference;
+static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr) {
+    if (lqr == NULL) {
+        return -1;
+    }
     
-    // 对于角度状态，需要考虑周期性
-    // 这里假设大部分状态都是角度，如果需要区分可以添加参数
-    while (error > M_PI) error -= 2 * M_PI;
-    while (error < -M_PI) error += 2 * M_PI;
+    /* 重置限幅标志 */
+    lqr->wheel_torque_limited = false;
+    lqr->joint_torque_limited = false;
     
-    return error;
+    /* 限制轮毂力矩 */
+    if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) {
+        lqr->control_output.T = LQR_LIMIT(lqr->control_output.T, 
+                                         -lqr->param.max_wheel_torque, 
+                                          lqr->param.max_wheel_torque);
+        lqr->wheel_torque_limited = true;
+    }
+    
+    /* 限制髋关节力矩 */
+    if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) {
+        lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp,
+                                          -lqr->param.max_joint_torque,
+                                           lqr->param.max_joint_torque);
+        lqr->joint_torque_limited = true;
+    }
+    
+    return 0;
 }
diff --git a/User/component/lqr.h b/User/component/lqr.h
index 5b4b60f..59506e2 100644
--- a/User/component/lqr.h
+++ b/User/component/lqr.h
@@ -1,3 +1,8 @@
+/*
+ * LQR控制器
+ * 用于轮腿平衡机器人的线性二次调节器控制
+ */
+
 #pragma once
 
 #ifdef __cplusplus
@@ -8,160 +13,192 @@ extern "C" {
 #include <stdint.h>
 #include <stdbool.h>
 #include <math.h>
-#include "component/user_math.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define LQR_STATE_DIM  6    /* 状态向量维度: theta, d_theta, x, d_x, phi, d_phi */
+#define LQR_INPUT_DIM  2    /* 输入向量维度: T(轮毂力矩), Tp(髋关节力矩) */
+#define LQR_POLY_ORDER 4    /* 多项式拟合阶数 */
 
 /* Exported types ----------------------------------------------------------- */
 
 /**
- * @brief LQR控制器状态向量定义
- * 
- * 状态向量维度: 10 x 1
- * [s, ds, phi, dphi, theta_ll, dtheta_ll, theta_lr, dtheta_lr, theta_b, dtheta_b]^T
+ * @brief LQR状态向量
+ * 状态定义：
+ * theta:   摆杆与竖直方向夹角 (rad)
+ * d_theta: 摆杆角速度 (rad/s)
+ * x:       驱动轮位移 (m)
+ * d_x:     驱动轮速度 (m/s)
+ * phi:     机体与水平夹角 (rad)
+ * d_phi:   机体角速度 (rad/s)
  */
 typedef struct {
-    float s;                // 机器人水平方向移动距离 (m)
-    float ds;               // 机器人水平方向移动速度 (m/s)
-    float phi;              // 机器人水平方向移动时yaw偏航角度 (rad)
-    float dphi;             // yaw偏航角速度 (rad/s)
-    float theta_ll;         // 左腿摆杆与竖直方向夹角 (rad)
-    float dtheta_ll;        // 左腿摆杆角速度 (rad/s)
-    float theta_lr;         // 右腿摆杆与竖直方向夹角 (rad)
-    float dtheta_lr;        // 右腿摆杆角速度 (rad/s)
-    float theta_b;          // 机体与水平方向夹角 (rad)
-    float dtheta_b;         // 机体角速度 (rad/s)
+    float theta;      /* 摆杆角度 */
+    float d_theta;    /* 摆杆角速度 */
+    float x;          /* 驱动轮位移 */
+    float d_x;        /* 驱动轮速度 */
+    float phi;        /* 机体俯仰角 */
+    float d_phi;      /* 机体俯仰角速度 */
 } LQR_State_t;
 
 /**
- * @brief LQR控制器控制输入向量定义
- * 
- * 控制向量维度: 4 x 1
- * [T_wl, T_wr, T_bl, T_br]^T
+ * @brief LQR控制输入向量
  */
 typedef struct {
-    float T_wl;             // 左侧驱动轮输出力矩 (N*m)
-    float T_wr;             // 右侧驱动轮输出力矩 (N*m)
-    float T_bl;             // 左侧髋关节输出力矩 (N*m)
-    float T_br;             // 右侧髋关节输出力矩 (N*m)
-} LQR_Control_t;
+    float T;          /* 轮毂力矩 (N·m) */
+    float Tp;         /* 髋关节力矩 (N·m) */
+} LQR_Input_t;
 
 /**
- * @brief LQR增益矩阵K (4x10)
+ * @brief LQR增益矩阵参数
+ * K矩阵的每个元素的多项式拟合系数
+ * K(leg_length) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3]
  */
 typedef struct {
-    float K[4][10];         // LQR反馈增益矩阵
+    /* K矩阵第一行(轮毂力矩T对应的增益) */
+    float k11_coeff[LQR_POLY_ORDER];  /* K(1,1): theta */
+    float k12_coeff[LQR_POLY_ORDER];  /* K(1,2): d_theta */
+    float k13_coeff[LQR_POLY_ORDER];  /* K(1,3): x */
+    float k14_coeff[LQR_POLY_ORDER];  /* K(1,4): d_x */
+    float k15_coeff[LQR_POLY_ORDER];  /* K(1,5): phi */
+    float k16_coeff[LQR_POLY_ORDER];  /* K(1,6): d_phi */
+    
+    /* K矩阵第二行(髋关节力矩Tp对应的增益) */
+    float k21_coeff[LQR_POLY_ORDER];  /* K(2,1): theta */
+    float k22_coeff[LQR_POLY_ORDER];  /* K(2,2): d_theta */
+    float k23_coeff[LQR_POLY_ORDER];  /* K(2,3): x */
+    float k24_coeff[LQR_POLY_ORDER];  /* K(2,4): d_x */
+    float k25_coeff[LQR_POLY_ORDER];  /* K(2,5): phi */
+    float k26_coeff[LQR_POLY_ORDER];  /* K(2,6): d_phi */
 } LQR_GainMatrix_t;
 
 /**
- * @brief 简化的LQR控制器实例结构体
+ * @brief LQR控制器参数
  */
 typedef struct {
-    LQR_GainMatrix_t K;              // 增益矩阵
-    LQR_State_t state;               // 当前状态
-    LQR_State_t reference;           // 参考状态
-    LQR_Control_t control;           // 控制输出
+    /* 系统物理参数 */
+    float R;          /* 驱动轮半径 (m) */
+    float L;          /* 摆杆重心到驱动轮轴距离 (m) */
+    float LM;         /* 摆杆重心到其转轴距离 (m) */
+    float l;          /* 机体重心到其转轴距离 (m) */
+    float mw;         /* 驱动轮质量 (kg) */
+    float mp;         /* 摆杆质量 (kg) */
+    float M;          /* 机体质量 (kg) */
+    float Iw;         /* 驱动轮转动惯量 (kg·m²) */
+    float Ip;         /* 摆杆绕质心转动惯量 (kg·m²) */
+    float IM;         /* 机体绕质心转动惯量 (kg·m²) */
+    float g;          /* 重力加速度 (m/s²) */
     
-    float max_wheel_torque;          // 轮毂电机最大力矩限制 (N*m)
-    float max_joint_torque;          // 关节电机最大力矩限制 (N*m)
-    bool initialized;                // 初始化标志
+    /* 控制限制 */
+    float max_wheel_torque;  /* 轮毂电机最大力矩 (N·m) */
+    float max_joint_torque;  /* 关节电机最大力矩 (N·m) */
+    
+    /* 目标状态 */
+    LQR_State_t target_state; /* 目标状态 */
+} LQR_Param_t;
+
+/**
+ * @brief LQR控制器主结构体
+ */
+typedef struct {
+    LQR_Param_t param;           /* 控制器参数 */
+    LQR_GainMatrix_t *gain_matrix; /* 增益矩阵参数指针 */
+    
+    LQR_State_t current_state;   /* 当前状态 */
+    LQR_State_t error_state;     /* 状态误差 */
+    LQR_Input_t control_output;  /* 控制输出 */
+    
+    /* 运行时变量 */
+    float current_leg_length;    /* 当前腿长 */
+    float K_matrix[LQR_INPUT_DIM][LQR_STATE_DIM]; /* 当前增益矩阵 */
+    
+    bool initialized;            /* 初始化标志 */
+    
+    /* 限幅标志 */
+    bool wheel_torque_limited;   /* 轮毂力矩是否被限幅 */
+    bool joint_torque_limited;   /* 关节力矩是否被限幅 */
 } LQR_Controller_t;
 
-/* Exported constants ------------------------------------------------------- */
-
-#define LQR_STATE_DIM           (10)        // 状态向量维度
-#define LQR_CONTROL_DIM         (4)         // 控制向量维度
-
-/* Exported macros ---------------------------------------------------------- */
-
-/**
- * @brief 角度归一化到[-PI, PI]
- */
-#define LQR_ANGLE_NORMALIZE(angle) do { \
-    while((angle) > M_PI) (angle) -= 2*M_PI; \
-    while((angle) < -M_PI) (angle) += 2*M_PI; \
-} while(0)
-
-/**
- * @brief 数值限幅
- */
-#define LQR_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))
-
 /* Exported functions prototypes -------------------------------------------- */
 
 /**
  * @brief 初始化LQR控制器
- * @param lqr LQR控制器实例
- * @param max_wheel_torque 轮毂电机最大力矩 (N*m)
- * @param max_joint_torque 关节电机最大力矩 (N*m)
- * @return 0:成功, -1:失败
+ * 
+ * @param lqr LQR控制器结构体指针
+ * @param max_wheel_torque 轮毂电机最大力矩
+ * @param max_joint_torque 关节电机最大力矩
+ * @return int8_t 0-成功, 其他-失败
  */
 int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque);
 
 /**
- * @brief 设置固定LQR增益矩阵
- * @param lqr LQR控制器实例
- * @param K 增益矩阵
- * @return 0:成功, -1:失败
+ * @brief 设置LQR增益矩阵参数
+ * 
+ * @param lqr LQR控制器结构体指针  
+ * @param gain_matrix 增益矩阵参数指针
+ * @return int8_t 0-成功, 其他-失败
  */
-int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, const LQR_GainMatrix_t *K);
+int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix);
 
 /**
- * @brief 更新机器人状态
- * @param lqr LQR控制器实例
+ * @brief 更新当前状态
+ * 
+ * @param lqr LQR控制器结构体指针
  * @param state 当前状态
- * @return 0:成功, -1:失败
+ * @return int8_t 0-成功, 其他-失败
  */
 int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state);
 
 /**
- * @brief 设置参考状态
- * @param lqr LQR控制器实例
- * @param reference 参考状态
- * @return 0:成功, -1:失败
+ * @brief 设置目标状态
+ * 
+ * @param lqr LQR控制器结构体指针
+ * @param target_state 目标状态
+ * @return int8_t 0-成功, 其他-失败
  */
-int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_State_t *reference);
+int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state);
 
 /**
- * @brief 计算LQR控制输出
- * @param lqr LQR控制器实例
- * @return 0:成功, -1:失败
+ * @brief 根据当前腿长计算增益矩阵
+ * 
+ * @param lqr LQR控制器结构体指针
+ * @param leg_length 当前腿长 (m)
+ * @return int8_t 0-成功, 其他-失败
  */
-int8_t LQR_ComputeControl(LQR_Controller_t *lqr);
+int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length);
+
+/**
+ * @brief 执行LQR控制计算
+ * 
+ * @param lqr LQR控制器结构体指针
+ * @return int8_t 0-成功, 其他-失败
+ */
+int8_t LQR_Control(LQR_Controller_t *lqr);
 
 /**
  * @brief 获取控制输出
- * @param lqr LQR控制器实例
- * @param control 控制输出
- * @return 0:成功, -1:失败
+ * 
+ * @param lqr LQR控制器结构体指针
+ * @param output 输出控制指令指针
+ * @return int8_t 0-成功, 其他-失败
  */
-int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control);
+int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output);
 
 /**
- * @brief 重置LQR控制器状态
- * @param lqr LQR控制器实例
+ * @brief 重置LQR控制器
+ * 
+ * @param lqr LQR控制器结构体指针
+ * @return int8_t 0-成功, 其他-失败
  */
-void LQR_Reset(LQR_Controller_t *lqr);
+int8_t LQR_Reset(LQR_Controller_t *lqr);
 
 /**
- * @brief 从轮腿机器人传感器数据构建LQR状态
- * @param position_x 机体x位置 (m)
- * @param velocity_x 机体x速度 (m/s)
- * @param yaw_angle yaw角度 (rad)
- * @param yaw_rate yaw角速度 (rad/s)
- * @param left_leg_angle 左腿角度 (rad)
- * @param left_leg_rate 左腿角速度 (rad/s)
- * @param right_leg_angle 右腿角度 (rad)
- * @param right_leg_rate 右腿角速度 (rad/s)
- * @param body_pitch 机体pitch角度 (rad)
- * @param body_pitch_rate 机体pitch角速度 (rad/s)
- * @param state 输出状态
- * @return 0:成功, -1:失败
+ * @brief 根据多项式系数计算增益值
+ * 
+ * @param coeff 多项式系数数组
+ * @param leg_length 腿长
+ * @return float 增益值
  */
-int8_t LQR_BuildStateFromSensors(float position_x, float velocity_x,
-                                 float yaw_angle, float yaw_rate,
-                                 float left_leg_angle, float left_leg_rate,
-                                 float right_leg_angle, float right_leg_rate,
-                                 float body_pitch, float body_pitch_rate,
-                                 LQR_State_t *state);
+float LQR_PolynomialCalc(const float *coeff, float leg_length);
 
 #ifdef __cplusplus
 }
diff --git a/User/component/lqr_config_example.c b/User/component/lqr_config_example.c
new file mode 100644
index 0000000..5ca44fe
--- /dev/null
+++ b/User/component/lqr_config_example.c
@@ -0,0 +1,71 @@
+/*
+ * LQR控制器配置参数示例
+ * 基于MATLAB仿真get_k.m和get_k_length.m的结果
+ */
+
+#include "lqr.h"
+
+/* 
+ * LQR增益矩阵多项式拟合系数
+ * 从MATLAB仿真get_k.m得到，对应不同腿长下的最优LQR增益
+ * 
+ * 多项式形式: K(L) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3]
+ * 其中 L 为腿长，范围 [0.1, 0.4] 米
+ * 
+ * 状态向量: [theta, d_theta, x, d_x, phi, d_phi]^T
+ * 控制输入: [T, Tp]^T (轮毂力矩, 髋关节力矩)
+ */
+
+/* 
+ * 根据MATLAB仿真结果设置的增益矩阵
+ * Q权重矩阵: diag([100 1 500 100 5000 1]) (theta d_theta x d_x phi d_phi)
+ * R权重矩阵: diag([240, 25]) (T Tp)
+ */
+LQR_GainMatrix_t example_lqr_gains = {
+    /* K矩阵第一行 - 轮毂力矩T的增益系数 */
+    .k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f},      /* K(1,1): theta */
+    .k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f},            /* K(1,2): d_theta */
+    .k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f},       /* K(1,3): x */
+    .k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f},          /* K(1,4): d_x */
+    .k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f},   /* K(1,5): phi */
+    .k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f},            /* K(1,6): d_phi */
+    
+    /* K矩阵第二行 - 髋关节力矩Tp的增益系数 */
+    .k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f},          /* K(2,1): theta */
+    .k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f},             /* K(2,2): d_theta */
+    .k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f},         /* K(2,3): x */
+    .k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f},            /* K(2,4): d_x */
+    .k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f},       /* K(2,5): phi */
+    .k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f},             /* K(2,6): d_phi */
+};
+
+/* 
+ * 使用示例：
+ * 
+ * 1. 在底盘参数结构体中设置LQR增益：
+ * Chassis_Params_t chassis_params = {
+ *     .lqr_param = {
+ *         .max_wheel_torque = 10.0f,  // 10 N·m
+ *         .max_joint_torque = 5.0f,   // 5 N·m
+ *     },
+ *     .lqr_gains = example_lqr_gains,
+ *     // ... 其他参数
+ * };
+ * 
+ * 2. 初始化底盘时会自动设置LQR增益矩阵
+ * 
+ * 3. 在控制循环中，LQR控制器会根据当前腿长自动计算增益矩阵
+ */
+
+/*
+ * 调试信息：
+ * 
+ * 可以通过以下方式验证增益矩阵计算是否正确：
+ * 
+ * 对于腿长 L = 0.25m：
+ * K(1,1) = -2845.3*0.25^2 + 1899.4*0.25 - 123.8 ≈ -123.8
+ * K(1,2) = -89.7*0.25^2 + 61.2*0.25 - 4.8 ≈ -4.8
+ * ... 其他系数类似计算
+ * 
+ * 这些数值应该与MATLAB仿真中对应腿长的LQR增益一致
+ */
diff --git a/User/component/lqr_test.c b/User/component/lqr_test.c
new file mode 100644
index 0000000..019c000
--- /dev/null
+++ b/User/component/lqr_test.c
@@ -0,0 +1,163 @@
+/*
+ * LQR控制器测试程序
+ */
+
+#include "lqr.h"
+#include <stdio.h>
+#include <assert.h>
+
+/* 测试用的增益矩阵 */
+extern LQR_GainMatrix_t example_lqr_gains;
+
+/* 测试函数 */
+void test_lqr_init(void);
+void test_lqr_gain_calculation(void);
+void test_lqr_control(void);
+void test_lqr_polynomial_calc(void);
+
+int main(void) {
+    printf("开始LQR控制器测试...\n");
+    
+    test_lqr_polynomial_calc();
+    test_lqr_init();
+    test_lqr_gain_calculation();
+    test_lqr_control();
+    
+    printf("所有测试通过！\n");
+    return 0;
+}
+
+void test_lqr_polynomial_calc(void) {
+    printf("测试多项式计算...\n");
+    
+    /* 测试多项式计算函数 */
+    float coeff[4] = {1.0f, 2.0f, 3.0f, 4.0f}; // L^3 + 2*L^2 + 3*L + 4
+    float L = 0.25f;
+    
+    float expected = 1.0f * 0.25f * 0.25f * 0.25f + 
+                     2.0f * 0.25f * 0.25f + 
+                     3.0f * 0.25f + 
+                     4.0f;
+                     
+    float result = LQR_PolynomialCalc(coeff, L);
+    
+    printf("  多项式计算: L=%.2f, 期望=%.4f, 实际=%.4f\n", L, expected, result);
+    assert(fabsf(result - expected) < 1e-4f);
+    
+    printf("  多项式计算测试通过\n");
+}
+
+void test_lqr_init(void) {
+    printf("测试LQR初始化...\n");
+    
+    LQR_Controller_t lqr;
+    int8_t result = LQR_Init(&lqr, 10.0f, 5.0f);
+    
+    assert(result == 0);
+    assert(lqr.initialized == true);
+    assert(lqr.param.max_wheel_torque == 10.0f);
+    assert(lqr.param.max_joint_torque == 5.0f);
+    assert(lqr.current_leg_length == 0.25f);
+    
+    printf("  LQR初始化测试通过\n");
+}
+
+void test_lqr_gain_calculation(void) {
+    printf("测试增益矩阵计算...\n");
+    
+    LQR_Controller_t lqr;
+    LQR_Init(&lqr, 10.0f, 5.0f);
+    LQR_SetGainMatrix(&lqr, &example_lqr_gains);
+    
+    /* 测试不同腿长的增益计算 */
+    float test_lengths[] = {0.15f, 0.25f, 0.35f};
+    int num_tests = sizeof(test_lengths) / sizeof(test_lengths[0]);
+    
+    for (int i = 0; i < num_tests; i++) {
+        float L = test_lengths[i];
+        int8_t result = LQR_CalculateGainMatrix(&lqr, L);
+        
+        assert(result == 0);
+        assert(lqr.current_leg_length == L);
+        
+        /* 检查增益矩阵是否合理 */
+        for (int j = 0; j < LQR_INPUT_DIM; j++) {
+            for (int k = 0; k < LQR_STATE_DIM; k++) {
+                assert(!isnan(lqr.K_matrix[j][k]));
+                assert(!isinf(lqr.K_matrix[j][k]));
+            }
+        }
+        
+        printf("  腿长%.2fm的增益矩阵计算通过\n", L);
+        printf("    K11=%.2f, K12=%.2f, K13=%.2f\n", 
+               lqr.K_matrix[0][0], lqr.K_matrix[0][1], lqr.K_matrix[0][2]);
+    }
+    
+    printf("  增益矩阵计算测试通过\n");
+}
+
+void test_lqr_control(void) {
+    printf("测试LQR控制计算...\n");
+    
+    LQR_Controller_t lqr;
+    LQR_Init(&lqr, 10.0f, 5.0f);
+    LQR_SetGainMatrix(&lqr, &example_lqr_gains);
+    
+    /* 设置测试状态 */
+    LQR_State_t test_state = {
+        .theta = 0.1f,      /* 摆杆角度 */
+        .d_theta = 0.05f,   /* 摆杆角速度 */
+        .x = 0.02f,         /* 位置 */
+        .d_x = 0.1f,        /* 速度 */
+        .phi = 0.05f,       /* 俯仰角 */
+        .d_phi = 0.02f      /* 俯仰角速度 */
+    };
+    
+    LQR_State_t target_state = {0}; /* 平衡状态 */
+    
+    /* 更新状态 */
+    int8_t result = LQR_UpdateState(&lqr, &test_state);
+    assert(result == 0);
+    
+    result = LQR_SetTargetState(&lqr, &target_state);
+    assert(result == 0);
+    
+    /* 执行控制计算 */
+    result = LQR_Control(&lqr);
+    assert(result == 0);
+    
+    /* 获取控制输出 */
+    LQR_Input_t output;
+    result = LQR_GetOutput(&lqr, &output);
+    assert(result == 0);
+    
+    /* 检查输出是否合理 */
+    assert(!isnan(output.T));
+    assert(!isnan(output.Tp));
+    assert(!isinf(output.T));
+    assert(!isinf(output.Tp));
+    assert(fabsf(output.T) <= lqr.param.max_wheel_torque);
+    assert(fabsf(output.Tp) <= lqr.param.max_joint_torque);
+    
+    printf("  控制输出: T=%.3f N·m, Tp=%.3f N·m\n", output.T, output.Tp);
+    printf("  LQR控制计算测试通过\n");
+}
+
+/* 如果没有math.h中的函数，提供简单实现 */
+#ifndef __has_include
+#define __has_include(x) 0
+#endif
+
+#if !__has_include(<math.h>)
+float fabsf(float x) {
+    return x < 0 ? -x : x;
+}
+
+int isnan(float x) {
+    return x != x;
+}
+
+int isinf(float x) {
+    return (x == x) && ((x - x) != 0);
+}
+#endif
diff --git a/User/component/vmc.c b/User/component/vmc.c
index 35aaffb..51f5396 100644
--- a/User/component/vmc.c
+++ b/User/component/vmc.c
@@ -80,6 +80,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
     if (vmc == NULL || !vmc->initialized) {
         return -1;
     }
+    body_pitch = -body_pitch; // 机体俯仰角取反
     
     VMC_Leg_t *leg = &vmc->leg;
     
@@ -111,7 +112,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
     
     // 计算等效摆动杆角度(相对于机体坐标系)
     leg->alpha = VMC_PI_2 - leg->phi0;
-    leg->theta = -(VMC_PI_2 + body_pitch - leg->phi0);
+    leg->theta = VMC_PI - (VMC_PI_2 + body_pitch - leg->phi0);
     
     // 角度归一化
     VMC_ANGLE_NORMALIZE(leg->theta);
@@ -140,8 +141,8 @@ int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
     }
     
     // 保存虚拟力和力矩
-    vmc->leg.F_virtual = -F_virtual;
-    vmc->leg.T_virtual = T_virtual;
+    vmc->leg.F_virtual = F_virtual;
+    vmc->leg.T_virtual = -T_virtual;
     
     // 计算雅可比矩阵
     if (VMC_ComputeJacobian(vmc) != 0) {
diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 988e09b..1e13508 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -24,6 +24,9 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
   for (int i = 0; i < 4; i++) {
     MOTOR_LZ_Enable(&c->param->joint_motors[i]);
   }
+  // 清空pid积分
+    PID_Reset(&c->pid.leglength[0]);
+    PID_Reset(&c->pid.leglength[1]);
 
   c->mode = mode;
   c->state = 0;  // 重置状态，确保每次切换模式时都重新初始化
@@ -56,92 +59,6 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
     c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
 }
 
-/* 执行LQR控制 */
-static int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
-    if (c == NULL || c_cmd == NULL) return -1;
-    
-    // 构建当前状态
-    LQR_State_t current_state;
-    float left_leg_length, left_leg_angle, left_leg_d_length, left_leg_d_angle;
-    float right_leg_length, right_leg_angle, right_leg_d_length, right_leg_d_angle;
-    
-    // 获取等效摆动杆状态
-    VMC_GetVirtualLegState(&c->vmc_[0], &left_leg_length, &left_leg_angle, &left_leg_d_length, &left_leg_d_angle);
-    VMC_GetVirtualLegState(&c->vmc_[1], &right_leg_length, &right_leg_angle, &right_leg_d_length, &right_leg_d_angle);
-    
-    LQR_BuildStateFromSensors(
-        c->chassis_state.position_x,
-        c->chassis_state.velocity_x,
-        c->feedback.imu.euler.yaw,
-        c->feedback.imu.gyro.z,
-        left_leg_angle,
-        left_leg_d_angle,
-        right_leg_angle,
-        right_leg_d_angle,
-        c->feedback.imu.euler.pit,
-        c->feedback.imu.gyro.y,
-        &current_state
-    );
-    
-    // 设置参考状态
-    LQR_State_t reference_state = {0};
-    reference_state.s = 0.0f; // 期望位移设为0（相对平衡位置）
-    reference_state.ds = c_cmd->move_vec.vx; // 期望速度
-    reference_state.phi = 0.0f; // 期望yaw角度
-    reference_state.dphi = c_cmd->move_vec.wz; // 期望yaw角速度
-    // 其他状态保持为0（平衡状态）
-    
-    // 更新LQR控制器状态
-    LQR_UpdateState(&c->lqr, &current_state);
-    LQR_SetReference(&c->lqr, &reference_state);
-    
-    // 计算控制输出
-    if (LQR_ComputeControl(&c->lqr) != 0) {
-        return -1;
-    }
-    
-    // 获取控制输出
-    LQR_Control_t lqr_output;
-    LQR_GetControl(&c->lqr, &lqr_output);
-    
-    // 分配力矩到电机
-    // 轮毂电机 (考虑减速比)
-    // float wheel_gear_ratio = 19.2f;
-    // MOTOR_LK_SetTorque(&c->param->wheel_motors[0], lqr_output.T_wl / wheel_gear_ratio);
-    // MOTOR_LK_SetTorque(&c->param->wheel_motors[1], lqr_output.T_wr / wheel_gear_ratio);
-    c->output.wheel[0] = lqr_output.T_wl/2.5; // 轮子电机输出
-    c->output.wheel[1] = lqr_output.T_wr/2.5;
-    // 通过VMC将虚拟力转换为关节力矩
-    // 左腿
-    float F_virtual_left = lqr_output.T_bl; // 简化映射，实际需要更复杂的转换
-    // float T_virtual_left = 0.0f;
-    float T_virtual_left = lqr_output.T_wl;   // 左腿虚拟摆动力矩
-    VMC_InverseSolve(&c->vmc_[0], F_virtual_left, T_virtual_left);
-    
-    float tau_left_front, tau_left_rear;
-    VMC_GetJointTorques(&c->vmc_[0], &tau_left_front, &tau_left_rear);
-    
-    // 右腿
-    float F_virtual_right = lqr_output.T_br;
-    // float T_virtual_right = 0.0f;
-    float T_virtual_right = lqr_output.T_wr;  // 右腿虚拟摆动力矩
-    VMC_InverseSolve(&c->vmc_[1], F_virtual_right, T_virtual_right);
-    
-    float tau_right_front, tau_right_rear;
-    VMC_GetJointTorques(&c->vmc_[1], &tau_right_front, &tau_right_rear);
-    
-    // 输出到关节电机
-    // MOTOR_LZ_SetTorque(&c->param->joint_motors[0], tau_left_rear);  // 左后
-    // MOTOR_LZ_SetTorque(&c->param->joint_motors[1], tau_left_front); // 左前
-    // MOTOR_LZ_SetTorque(&c->param->joint_motors[2], tau_right_front);// 右前
-    // MOTOR_LZ_SetTorque(&c->param->joint_motors[3], tau_right_rear); // 右后
-    c->output.joint[0].torque = tau_left_rear;
-    c->output.joint[1].torque = tau_left_front;
-    c->output.joint[2].torque = tau_right_front;
-    c->output.joint[3].torque = tau_right_rear;
-    return 0;
-}
-
 
 int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
     if (c == NULL || param == NULL || target_freq <= 0.0f) {
@@ -165,10 +82,16 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
     /*初始化VMC控制器*/
     VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
     VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
+
+    /*初始化pid*/
+    PID_Init(&c->pid.leglength[0], KPID_MODE_CALC_D, target_freq, &param->leglength);
+    PID_Init(&c->pid.leglength[1], KPID_MODE_CALC_D, target_freq, &param->leglength);
     
     /*初始化LQR控制器*/
-    LQR_Init(&c->lqr, param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
-    LQR_SetGainMatrix(&c->lqr, &param->lqr_gains);
+    LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
+    LQR_SetGainMatrix(&c->lqr[0], &param->lqr_gains);
+    LQR_Init(&c->lqr[1], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
+    LQR_SetGainMatrix(&c->lqr[1], &param->lqr_gains);
     
     /*初始化机体状态*/
     c->chassis_state.position_x = 0.0f;
@@ -195,7 +118,11 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c){
         MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
         if (joint_motor != NULL) {
             c->feedback.joint[i] = joint_motor->motor.feedback;
-            c->feedback.joint[i].rotor_abs_angle = joint_motor->motor.feedback.rotor_abs_angle - M_PI; // 机械零点调整
+            // c->feedback.joint[i].rotor_abs_angle = joint_motor->motor.feedback.rotor_abs_angle - M_PI; // 机械零点调整
+            if (c->feedback.joint[i].rotor_abs_angle > M_PI ){
+                c->feedback.joint[i].rotor_abs_angle -= M_2PI;
+            }
+            c->feedback.joint[i].rotor_abs_angle = - c->feedback.joint[i].rotor_abs_angle; // 机械零点调整
         } 
     }
     
@@ -251,15 +178,15 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
             VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
                                 c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
 
-            VMC_InverseSolve(&c->vmc_[0], fn, tp);
+            // VMC_InverseSolve(&c->vmc_[0], fn, tp);
 
-            VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
+            // VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
             
             // MOTOR_LZ_MotionControl(&c->param->joint_motors[0], &(MOTOR_LZ_MotionParam_t){.torque = t1});
-
-            
-
-            // Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
+            if (Chassis_LQRControl(c, c_cmd) != 0) {
+                // LQR控制失败，切换到安全模式
+                return CHASSIS_ERR;
+            }
 
             break;
 
@@ -289,31 +216,36 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
             VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
                                 c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
 
-            VMC_InverseSolve(&c->vmc_[0], fn, tp);
-            VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
+            // VMC_InverseSolve(&c->vmc_[1], fn, tp);
+            // VMC_GetJointTorques(&c->vmc_[1], &t1, &t2);
             
-            c->output.joint[0].torque = t1;
-            c->output.joint[1].torque = t2;
-
-            // Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
+            // c->output.joint[3].torque = t1;
+            // c->output.joint[2].torque = t2;
 
+            Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
+            // c->output.wheel[0] = 0.2f;
+            // c->output.wheel[1] = 0.2f;
             Chassis_Output(c); // 统一输出
             break;
             
         case CHASSIS_MODE_WHELL_LEG_BALANCE:
             // 轮腿平衡模式，使用LQR控制
             
-            // // 更新VMC正解算
-            // VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
-            //                     c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
-            // VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
-            //                     c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
+            // 更新VMC正解算
+            VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
+                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
+            VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
+                                c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
             
-            // // 执行LQR控制
+
+
+            // 执行LQR控制
             // if (Chassis_LQRControl(c, c_cmd) != 0) {
             //     // LQR控制失败，切换到安全模式
             //     return CHASSIS_ERR;
             // }
+            
+            // Chassis_Output(c); // 统一输出
             break;
             
         default:
@@ -328,6 +260,7 @@ void Chassis_Output(Chassis_t *c) {
     for (int i = 0; i < 4; i++) {
         MOTOR_LZ_MotionParam_t param = {0};
         param.torque = c->output.joint[i].torque;
+        
         MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
     }
     for (int i = 0; i < 2; i++) {
@@ -337,3 +270,166 @@ void Chassis_Output(Chassis_t *c) {
     // 如果需要统一输出，可以在这里实现
     // 现在的设计是在控制逻辑中直接输出，所以这里留空
 }
+
+int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
+    if (c == NULL || c_cmd == NULL) {
+        return CHASSIS_ERR_NULL;
+    }
+
+    /* 参考C++版本实现的LQR控制 */
+    
+    /* 地面接触检测（参考C++版本） */
+    float leg_fn[2];
+    bool onground_flag[2];
+    
+    // 简化的地面接触检测，可以后续改进
+    leg_fn[0] = VMC_GroundContactDetection(&c->vmc_[0]);
+    leg_fn[1] = VMC_GroundContactDetection(&c->vmc_[1]);
+    onground_flag[0] = (leg_fn[0] > 30.0f);
+    onground_flag[1] = (leg_fn[1] > 30.0f);
+    
+    /* 获取VMC状态（等效摆杆参数） */
+    float leg_L0[2], leg_theta[2], leg_d_theta[2];
+    VMC_GetVirtualLegState(&c->vmc_[0], &leg_L0[0], &leg_theta[0], NULL, &leg_d_theta[0]);
+    VMC_GetVirtualLegState(&c->vmc_[1], &leg_L0[1], &leg_theta[1], NULL, &leg_d_theta[1]);
+    
+    /* 运动参数（参考C++版本的状态估计） */
+    static float xhat = 0.0f, x_dot_hat = 0.0f;
+    static float target_x = 0.0f, target_dot_x = 0.0f;
+    
+    // 简化的速度估计（后续可以改进为C++版本的复杂滤波）
+    x_dot_hat = c->chassis_state.velocity_x;
+    xhat = c->chassis_state.position_x;
+    
+    // 目标设定
+    target_dot_x = c_cmd->move_vec.vx;
+    target_x += target_dot_x * c->dt;
+    
+    /* 分别计算左右腿的LQR控制 */
+    float Tw[2] = {0.0f, 0.0f};  // 轮毂力矩
+    float Tp[2] = {0.0f, 0.0f};  // 髋关节力矩
+    
+    for (int leg = 0; leg < 2; leg++) {
+        /* 构建当前状态 */
+        LQR_State_t current_state = {0};
+        current_state.theta = leg_theta[leg];
+        current_state.d_theta = leg_d_theta[leg];
+        current_state.x = xhat;
+        current_state.d_x = x_dot_hat;
+        current_state.phi = -c->feedback.imu.euler.pit;
+        current_state.d_phi = -c->feedback.imu.gyro.y;
+        
+        /* 构建目标状态 */
+        LQR_State_t target_state = {0};
+        target_state.theta = 0.0f; // 目标摆杆角度
+        target_state.d_theta = 0.0f; // 目标摆杆角速度
+        target_state.x = 0; // 目标位置
+        target_state.d_x = 0.0f; // 目标速度
+        target_state.phi = 0.0f; // 目标俯仰角
+        target_state.d_phi = 0.0f; // 目标俯仰角速度
+        // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度
+        // target_state.d_theta = 0.0f; // 目标摆杆角速度
+        // target_state.x = target_x - 0.56f; // 目标位置
+        // target_state.d_x = target_dot_x; // 目标速度
+        // target_state.phi = 0.16f; // 目标俯仰角
+        // target_state.d_phi = 0.0f; // 目标俯仰角速度
+        
+        /* 根据当前腿长更新增益矩阵 */
+        LQR_CalculateGainMatrix(&c->lqr[leg], leg_L0[leg]);
+        
+        /* 更新LQR状态 */
+        LQR_SetTargetState(&c->lqr[leg], &target_state);
+        LQR_UpdateState(&c->lqr[leg], &current_state);
+        
+        if (onground_flag[leg]) {
+            /* 接地状态：使用LQR控制器计算输出 */
+            if (LQR_Control(&c->lqr[leg]) == 0) {
+                LQR_Input_t lqr_output = {0};
+                if (LQR_GetOutput(&c->lqr[leg], &lqr_output) == 0) {
+                    Tw[leg] = lqr_output.T;
+                    Tp[leg] = lqr_output.Tp;
+                } else {
+                    Tw[leg] = 0.0f;
+                    Tp[leg] = 0.0f;
+                }
+            } else {
+                Tw[leg] = 0.0f;
+                Tp[leg] = 0.0f;
+            }
+        } else {
+            /* 离地状态：简化控制，只控制腿部摆动 */
+            Tw[leg] = 0.0f;
+            // 只控制摆杆角度
+            float theta_error = current_state.theta - target_state.theta;
+            float d_theta_error = current_state.d_theta - target_state.d_theta;
+            Tp[leg] = -(c->lqr[leg].K_matrix[1][0] * theta_error + c->lqr[leg].K_matrix[1][1] * d_theta_error);
+        }
+    }
+    
+    /* 转向控制（参考C++版本） */
+    float yaw_force = 0.0f;
+    if (onground_flag[0] && onground_flag[1]) {
+        // 简化的转向控制，后续可以改进为PID
+        yaw_force = -c_cmd->move_vec.wz * 0.1f;  
+    }
+    
+    /* 轮毂力矩输出（参考C++版本的减速比） */
+    c->output.wheel[0] = Tw[0] / 4.9256f - yaw_force; // 左轮
+    c->output.wheel[1] = Tw[1] / 4.9256f + yaw_force; // 右轮
+    
+    /* 腿长控制和VMC逆解算（使用PID控制） */
+    float virtual_force[2];
+    float target_L0[2];
+    float leg_d_length[2]; // 腿长变化率
+    
+    // 目标腿长设定
+    target_L0[0] = 0.13f + c_cmd->height * 0.1f; // 基础腿长 + 高度调节
+    target_L0[1] = 0.13f + c_cmd->height * 0.1f;
+    
+    // 获取腿长变化率
+    VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
+    VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
+    
+    for (int leg = 0; leg < 2; leg++) {
+        // 使用PID进行腿长控制
+        float pid_output = PID_Calc(&c->pid.leglength[leg], target_L0[leg], leg_L0[leg], leg_d_length[leg], c->dt);
+        
+        // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
+        virtual_force[leg] = Tp[leg] * sinf(leg_theta[leg]) + 
+                            pid_output + // PID腿长控制输出
+                            50.0f; // 基础支撑力
+        
+        // VMC逆解算
+        if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg]) == 0) {
+            if (leg == 0) {
+                VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque);
+            } else {
+                VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque);
+            }
+        } else {
+            // VMC失败，设为0力矩
+            if (leg == 0) {
+                c->output.joint[0].torque = 0.0f;
+                c->output.joint[1].torque = 0.0f;
+            } else {
+                c->output.joint[2].torque = 0.0f;
+                c->output.joint[3].torque = 0.0f;
+            }
+        }
+    }
+    
+    /* 安全限制 */
+    for (int i = 0; i < 2; i++) {
+        if (fabsf(c->output.wheel[i]) > 0.8f) {
+            c->output.wheel[i] = (c->output.wheel[i] > 0) ? 0.8f : -0.8f;
+        }
+    }
+    
+    for (int i = 0; i < 4; i++) {
+        if (fabsf(c->output.joint[i].torque) > 20.0f) {
+            c->output.joint[i].torque = (c->output.joint[i].torque > 0) ? 20.0f : -20.0f;
+        }
+    }
+    
+    return CHASSIS_OK;
+}
diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h
index 3db068a..0d8633f 100644
--- a/User/module/balance_chassis.h
+++ b/User/module/balance_chassis.h
@@ -70,7 +70,8 @@ typedef struct {
 typedef struct {
 
   VMC_Param_t vmc_param[2];        /* VMC参数 */
-  LQR_GainMatrix_t lqr_gains;      /* LQR增益矩阵 */
+  KPID_Params_t leglength;
+
 
   MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
   MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
@@ -82,6 +83,8 @@ typedef struct {
     float max_wheel_torque; /* 轮毂电机最大力矩限制 */
     float max_joint_torque; /* 关节电机最大力矩限制 */
   } lqr_param;
+  
+  LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
 
   /* 低通滤波器截止频率 */
   struct {
@@ -110,7 +113,7 @@ typedef struct {
   Chassis_Output_t output;
 
   VMC_t vmc_[2]; /* 两条腿的VMC */
-  LQR_Controller_t lqr; /* LQR控制器 */
+  LQR_Controller_t lqr[2]; /* 两条腿的LQR控制器 */
 
   int8_t state;
 
@@ -137,6 +140,9 @@ typedef struct {
     KPID_t right_wheel; /* 右轮PID */
     KPID_t follow; /* 跟随云台用的PID */
     KPID_t balance; /* 平衡用的PID */
+
+    KPID_t leglength[2]; /* 两条腿的腿长PID */
+
   } pid;
 
   /* 滤波器 */
@@ -180,6 +186,16 @@ int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu);
  */
 int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
 
+/**
+ * \brief LQR控制逻辑
+ *
+ * \param c 包含底盘数据的结构体
+ * \param c_cmd 底盘控制指令
+ *
+ * \return 函数运行结果
+ */
+int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
+
 
 /**
  * \brief 底盘输出值
diff --git a/User/module/config.c b/User/module/config.c
index b8bd55b..71186b5 100644
--- a/User/module/config.c
+++ b/User/module/config.c
@@ -68,6 +68,18 @@ Config_RobotParam_t robot_config = {
     },
     
     .chassis_param = {
+        .leglength={
+            .k=1.0f,
+            .p=0.10f,
+            .i=0.0f,
+            .d=0.0f,
+            .i_limit=0.0f,
+            .out_limit=100.0f,
+            .d_cutoff_freq=-1.0f,
+            .range=-1.0f,
+        }
+        
+
         .low_pass_cutoff_freq = {
             .in = 30.0f,
             .out = 30.0f,
@@ -78,7 +90,7 @@ Config_RobotParam_t robot_config = {
                 .motor_id = 124,
                 .host_id = 130,
                 .module = MOTOR_LZ_RSO3,
-                .reverse = true,
+                .reverse = false,
                 .mode = MOTOR_LZ_MODE_MOTION,
             },
             { // 左膝关节
@@ -86,7 +98,7 @@ Config_RobotParam_t robot_config = {
                 .motor_id = 125,
                 .host_id = 130,
                 .module = MOTOR_LZ_RSO3,
-                .reverse = true,
+                .reverse = false,
                 .mode = MOTOR_LZ_MODE_MOTION,
             },
             { // 右膝关节
@@ -94,7 +106,7 @@ Config_RobotParam_t robot_config = {
                 .motor_id = 126,
                 .host_id = 130,
                 .module = MOTOR_LZ_RSO3,
-                .reverse = false,
+                .reverse = true,
                 .mode = MOTOR_LZ_MODE_MOTION,
             },
             { // 右髋关节
@@ -102,7 +114,7 @@ Config_RobotParam_t robot_config = {
                 .motor_id = 127,
                 .host_id = 130,
                 .module = MOTOR_LZ_RSO3,
-                .reverse = false,
+                .reverse = true,
                 .mode = MOTOR_LZ_MODE_MOTION,
             },
         },
@@ -121,29 +133,36 @@ Config_RobotParam_t robot_config = {
             },
         },
         .mech_zero_yaw = 0.0f,
+        
         .vmc_param = {
             { // 左腿
-                .leg_1 = 0.206f,      // 前大腿长度 (m)
-                .leg_2 = 0.258f,      // 前小腿长度 (m)
-                .leg_3 = 0.206f,      // 后小腿长度 (m)
-                .leg_4 = 0.258f,      // 后大腿长度 (m)
-                .hip_length = 0.0f    // 髋宽 (m)
+                .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m)
+                .leg_2 = 0.258f,      // 后膝连杆长度 (L2) (m)
+                .leg_3 = 0.258f,      // 前膝连杆长度 (L3) (m)
+                .leg_4 = 0.215f,      // 前髋连杆长度 (L4) (m)
+                .hip_length = 0.0f    // 髋宽 (L5) (m)
             },
             { // 右腿
-                .leg_1 = 0.206f,      // 前大腿长度 (m)
-                .leg_2 = 0.258f,      // 前小腿长度 (m)
-                .leg_3 = 0.206f,      // 后小腿长度 (m)
-                .leg_4 = 0.258f,      // 后大腿长度 (m)
-                .hip_length = 0.0f    // 髋宽 (m)
+                .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m)
+                .leg_2 = 0.258f,      // 后膝连杆长度 (L2) (m)
+                .leg_3 = 0.258f,      // 前膝连杆长度 (L3) (m)
+                .leg_4 = 0.215f,      // 前髋连杆长度 (L4) (m)
+                .hip_length = 0.0f    // 髋宽 (L5) (m)
             }
         },
         .lqr_gains ={
-            .K = {
-                { -1.3677,  -12.022,  4.0676,  -2.6185,  -66.132,  -4.2516,  -1.4083,  -0.051404,  -57.561,  -5.3641 },
-                { -1.3677,  -12.022,  -4.0676,  2.6185,  -1.4083,  -0.051404,  -66.132,  -4.2516,  -57.561,  -5.3641 },
-                { 0.14689,  1.2865,  -63.224,  -12.495,  6.2265,  -0.13959,  1.2635,  0.48938,  -78.822,  -5.121 },
-                { 0.14689,  1.2865,  63.224,  12.495,  1.2635,  0.48938,  6.2265,  -0.13959,  -78.822,  -5.121 }
-            }
+            .k11_coeff = { -61.932040681074966f,	70.963671642086396f,	-37.730841182566571f,	-0.296475458388679f }, // theta
+            .k12_coeff = { -0.586710094600108f,	0.886736272521581f,	-3.626144273475104f,	0.057861910518974f },  // d_theta
+            .k13_coeff = { -17.297031110481019f,	16.286794934166178f,	-5.176451154477850f,	-0.867260018374823f },  // x
+            .k14_coeff = { -14.893387150006664f,	14.357020815277332f,	-5.244645181873441f,	-0.869862096507486f},  // d_x
+            .k15_coeff = { -75.327876471378886f,	79.786699741548944f,	-31.183500053811208f,	5.450635661115236f}, // phi
+            .k16_coeff = { -3.572234723237025f,	4.164905011076312f,	-1.874828497771750f,	0.477913222933688f},  // d_phi
+            .k21_coeff = { 9.327090608559319f,	1.362675928111105f,	-8.092777598567881f,	4.351387652359104f}, // theta
+            .k22_coeff = { 3.872517778351810f,	-3.344077414726880f,	0.589693555828904f,	0.310036629174646f},  // d_theta
+            .k23_coeff = { -71.615766251649134f,	74.748309711530837f,	-28.370490124006626f,	4.483586941100197f },  // x
+            .k24_coeff = { -68.751866288568962f,	71.204785013044102f,	-26.812636604518396f,	4.238479323700952f },  // d_x
+            .k25_coeff = { 205.6887659080132f,	-195.1219729060621f,	62.890188701113303f,	7.452313695653162f }, // phi
+            .k26_coeff = { 16.162999842656344f,	-15.926932704437410f,	5.474613395300429f,	-0.002856083635449f },  // d_phi
         },
         .lqr_param.max_joint_torque = 20.0f,  // 关节电机最大力矩 20Nm
         .lqr_param.max_wheel_torque = 2.5f,  // 轮毂电机最大力矩 2.5Nm
diff --git a/comp_vmc.cpp b/comp_vmc.cpp
new file mode 100644
index 0000000..7d6f1f8
--- /dev/null
+++ b/comp_vmc.cpp
@@ -0,0 +1,213 @@
+
+#include "comp_vmc.hpp"
+
+#include <tuple>
+
+#define PI_2 1.571f
+
+using namespace Component;
+
+VMC::VMC(VMC::Param &param, float sample_freq) : param_(param) {
+  float dt_min = 1.0f / sample_freq;
+  XB_ASSERT(isfinite(dt_min));
+
+  this->Reset();
+}
+
+/*
+
+ 正负极参考韭菜的菜 知乎 平衡步兵控制系统设计
+
+ VMC 机体pitch正负极 d_pitch同
+
+          /
+         /  正+
+        /
+x  ---------> 0
+        \	负-
+         \
+          \
+
+ phi角正负极  d_phi同
+
+          /
+         /  正+
+        /
+x  ---------> 0
+        \	负-
+         \
+          \
+
+       后 <---->前
+     /phi1-----phi4\
+        /        \
+        \        /
+         \  OO  /
+          \O轮O/
+                OO
+
+*/
+
+/* 两个大腿角度 机体角度 角速度  求出虚拟腿摆角 摆角速度  虚拟腿长
+ * 虚拟腿长变化速度 */
+std::tuple<float, float, float, float> VMC::VMCsolve(float phi1, float phi4,
+                                                     float eulrPit,
+                                                     float d_eulrPit,
+                                                     float dt) {
+  static float body_pitch = 0.0f;
+  static float d_body_pitch = 0.0f;
+  body_pitch = eulrPit;
+  d_body_pitch = d_eulrPit;
+  /*点D B x y坐标 */
+  this->vmc_leg.YD = this->param_.leg_4 * sinf(phi4);
+  this->vmc_leg.YB = this->param_.leg_1 * sinf(phi1);
+  this->vmc_leg.XD = this->param_.hip_length + this->param_.leg_4 * cosf(phi4);
+  this->vmc_leg.XB = this->param_.leg_1 * cosf(phi1);
+  /*BD长度*/
+  this->vmc_leg.lBD = sqrtf((this->vmc_leg.XD - this->vmc_leg.XB) *
+                                (this->vmc_leg.XD - this->vmc_leg.XB) +
+                            (this->vmc_leg.YD - this->vmc_leg.YB) *
+                                (this->vmc_leg.YD - this->vmc_leg.YB));
+
+  this->vmc_leg.A0 =
+      2 * this->param_.leg_2 * (this->vmc_leg.XD - this->vmc_leg.XB);
+  this->vmc_leg.B0 =
+      2 * this->param_.leg_2 * (this->vmc_leg.YD - this->vmc_leg.YB);
+  this->vmc_leg.C0 = this->param_.leg_2 * this->param_.leg_2 +
+                     this->vmc_leg.lBD * this->vmc_leg.lBD -
+                     this->param_.leg_3 * this->param_.leg_3;
+  this->vmc_leg.phi2 =
+      2 *
+      atan2f((this->vmc_leg.B0 + sqrtf(this->vmc_leg.A0 * this->vmc_leg.A0 +
+                                       this->vmc_leg.B0 * this->vmc_leg.B0 -
+                                       this->vmc_leg.C0 * this->vmc_leg.C0)),
+             this->vmc_leg.A0 + this->vmc_leg.C0);
+  this->vmc_leg.phi3 =
+      atan2f(this->vmc_leg.YB - this->vmc_leg.YD +
+                 this->param_.leg_2 * sinf(this->vmc_leg.phi2),
+             this->vmc_leg.XB - this->vmc_leg.XD +
+                 this->param_.leg_2 * cosf(this->vmc_leg.phi2));
+  /*点C x y坐标 */
+  this->vmc_leg.XC = this->param_.leg_1 * cosf(phi1) +
+                     this->param_.leg_2 * cosf(this->vmc_leg.phi2);
+  this->vmc_leg.YC = this->param_.leg_1 * sinf(phi1) +
+                     this->param_.leg_2 * sinf(this->vmc_leg.phi2);
+  /*点C 极坐标 */
+  this->vmc_leg.L0 =
+      sqrtf((this->vmc_leg.XC - this->param_.hip_length / 2.0f) *
+                (this->vmc_leg.XC - this->param_.hip_length / 2.0f) +
+            this->vmc_leg.YC * this->vmc_leg.YC);
+  this->vmc_leg.phi0 = atan2f(
+      this->vmc_leg.YC, (this->vmc_leg.XC - this->param_.hip_length / 2.0f));
+
+  this->vmc_leg.alpha = PI_2 - this->vmc_leg.phi0;
+
+  this->vmc_leg.d_phi0 = (this->vmc_leg.phi0 - this->vmc_leg.last_phi0) / dt;
+  this->vmc_leg.d_alpha = 0.0f - this->vmc_leg.d_phi0;
+  /*虚拟腿 摆角theta 摆角速度d_theta */
+  this->vmc_leg.theta = PI_2 + body_pitch - this->vmc_leg.phi0;
+  this->vmc_leg.d_theta = (-d_body_pitch - this->vmc_leg.d_phi0);
+
+  this->vmc_leg.last_phi0 = this->vmc_leg.phi0;
+  /*虚拟腿 腿长L0 腿长变化速度d_L0 */
+  this->vmc_leg.d_L0 = (this->vmc_leg.L0 - this->vmc_leg.last_L0) / dt;
+  this->vmc_leg.dd_L0 = (this->vmc_leg.d_L0 - this->vmc_leg.last_d_L0) / dt;
+
+  this->vmc_leg.last_d_L0 = this->vmc_leg.d_L0;
+  this->vmc_leg.last_L0 = this->vmc_leg.L0;
+
+  this->vmc_leg.dd_theta =
+      (this->vmc_leg.d_theta - this->vmc_leg.last_d_theta) / dt;
+  this->vmc_leg.last_d_theta = this->vmc_leg.d_theta;
+
+  return std::make_tuple(vmc_leg.L0, vmc_leg.d_L0, vmc_leg.theta,
+                         vmc_leg.d_theta);
+}
+
+/* 两个大腿角度 期望腿支持力 期望腿摆力矩 求出两个关节输出力矩 */
+std::tuple<float, float> VMC::VMCinserve(float phi1, float phi4, float Tp,
+                                         float F0) {
+  /*jacobian矩阵计算*/
+  this->vmc_leg.j11 =
+      (this->param_.leg_1 * sinf(this->vmc_leg.phi0 - this->vmc_leg.phi3) *
+       sinf(phi1 - this->vmc_leg.phi2)) /
+      sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2);
+  this->vmc_leg.j12 =
+      (this->param_.leg_1 * cosf(this->vmc_leg.phi0 - this->vmc_leg.phi3) *
+       sinf(phi1 - this->vmc_leg.phi2)) /
+      (this->vmc_leg.L0 * sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2));
+  this->vmc_leg.j21 =
+      (this->param_.leg_4 * sinf(this->vmc_leg.phi0 - this->vmc_leg.phi2) *
+       sinf(this->vmc_leg.phi3 - phi4)) /
+      sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2);
+  this->vmc_leg.j22 =
+      (this->param_.leg_4 * cosf(this->vmc_leg.phi0 - this->vmc_leg.phi2) *
+       sinf(this->vmc_leg.phi3 - phi4)) /
+      (this->vmc_leg.L0 * sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2));
+  /*得到前髋关节的输出轴期望力矩，F0为五连杆机构末端沿腿的推力*/
+  this->vmc_leg.torque_set[0] = this->vmc_leg.j11 * F0 + this->vmc_leg.j12 * Tp;
+  /*得到后髋关节的输出轴期望力矩，Tp为虚拟腿摆力矩的力矩*/
+  this->vmc_leg.torque_set[1] = this->vmc_leg.j21 * F0 + this->vmc_leg.j22 * Tp;
+
+  return std::make_tuple(this->vmc_leg.torque_set[0],
+                         this->vmc_leg.torque_set[1]);
+}
+/* 用到了前两个函数解算算出来的变量 尽量放在前两个函数之后 */
+float VMC::GndDetector(float Tp, float F0) {
+  vmc_leg.Fn =
+      F0 * cosf(vmc_leg.theta) + Tp * sinf(vmc_leg.theta) / vmc_leg.L0 + 6;  //
+  //       腿部机构的力+轮子重力，这里忽略了轮子质量*驱动轮竖直方向运动加速度
+
+  //   vmc_leg.Fn =
+  //       F0 * cosf(vmc_leg.theta) + Tp * sinf(vmc_leg.theta) / vmc_leg.L0 +
+  //       wheel_mess *
+  //           (z_accl - vmc_leg.dd_L0 * cosf(vmc_leg.theta) +
+  //            2.0f * vmc_leg.d_L0 * vmc_leg.d_theta * sinf(vmc_leg.theta) +
+  //            vmc_leg.L0 * vmc_leg.dd_theta * sinf(vmc_leg.theta) +
+  //            vmc_leg.L0 * vmc_leg.d_theta * vmc_leg.d_theta *
+  //                cosf(vmc_leg.theta));
+  return vmc_leg.Fn;
+}
+
+/* 计算拟合函数结果 */
+float VMC::LQR_K_calc(float *coe, float len) {
+  return coe[0] * len * len * len + coe[1] * len * len + coe[2] * len + coe[3];
+}
+
+/* 变量刷新 */
+void VMC::Reset() {
+  vmc_leg.L0 = 0;
+  vmc_leg.phi0 = 0;
+  vmc_leg.alpha = 0;
+  vmc_leg.d_alpha = 0;
+
+  vmc_leg.lBD = 0;
+
+  vmc_leg.d_phi0 = 0;
+  vmc_leg.last_phi0 = 0;
+
+  vmc_leg.A0 = 0;
+  vmc_leg.B0 = 0;
+  vmc_leg.C0 = 0;
+  vmc_leg.phi2 = 0;
+  vmc_leg.phi3 = 0;
+
+  vmc_leg.j11 = 0;
+  vmc_leg.j12 = 0;
+  vmc_leg.j21 = 0;
+  vmc_leg.j22 = 0;
+  vmc_leg.torque_set[0] = 0;
+  vmc_leg.torque_set[1] = 0;
+
+  vmc_leg.theta = 0;
+  vmc_leg.d_theta = 0;
+  vmc_leg.last_d_theta = 0;
+  vmc_leg.dd_theta = 0;
+
+  vmc_leg.d_L0 = 0;
+  vmc_leg.dd_L0 = 0;
+  vmc_leg.last_L0 = 0;
+  vmc_leg.last_d_L0 = 0;
+  vmc_leg.first_flag = 0;
+  vmc_leg.leg_flag = 0;
+}
diff --git a/test_vmc.c b/test_vmc.c
new file mode 100644
index 0000000..0e70c86
--- /dev/null
+++ b/test_vmc.c
@@ -0,0 +1,66 @@
+#include <stdio.h>
+#include <math.h>
+#include "User/component/vmc.h"
+
+int main() {
+    // 测试VMC计算
+    VMC_t vmc;
+    VMC_Param_t param = {
+        .hip_length = 0.1f,      // 10cm髋关节间距
+        .leg_1 = 0.15f,         // 15cm大腿前端
+        .leg_2 = 0.15f,         // 15cm大腿后端
+        .leg_3 = 0.15f,         // 15cm小腿
+        .leg_4 = 0.15f,         // 15cm小腿前端
+        .wheel_radius = 0.05f,   // 5cm轮子半径
+        .wheel_mass = 1.0f       // 1kg轮子质量
+    };
+    
+    if (VMC_Init(&vmc, &param, 1000.0f) != 0) {
+        printf("VMC初始化失败\n");
+        return -1;
+    }
+    
+    printf("测试VMC腿长计算\n");
+    printf("髋关节间距: %.3f\n", param.hip_length);
+    printf("腿段长度: L1=%.3f, L2=%.3f, L3=%.3f, L4=%.3f\n", 
+           param.leg_1, param.leg_2, param.leg_3, param.leg_4);
+    printf("\n");
+    
+    // 测试不同的关节角度配置
+    float test_angles[][2] = {
+        {0.0f, 0.0f},           // 水平伸展
+        {0.5f, -0.5f},          // 轻微弯曲
+        {1.0f, -1.0f},          // 中等弯曲
+        {1.5f, -1.5f},          // 较大弯曲
+        {-0.5f, 0.5f},          // 反向弯曲
+    };
+    
+    int num_tests = sizeof(test_angles) / sizeof(test_angles[0]);
+    
+    for (int i = 0; i < num_tests; i++) {
+        float phi1 = test_angles[i][0];
+        float phi4 = test_angles[i][1];
+        float body_pitch = 0.0f;
+        float body_pitch_rate = 0.0f;
+        
+        if (VMC_ForwardSolve(&vmc, phi1, phi4, body_pitch, body_pitch_rate) == 0) {
+            float length, angle, d_length, d_angle;
+            VMC_GetVirtualLegState(&vmc, &length, &angle, &d_length, &d_angle);
+            
+            float foot_x, foot_y;
+            VMC_GetFootPosition(&vmc, &foot_x, &foot_y);
+            
+            printf("测试 %d: phi1=%.2f, phi4=%.2f\n", i+1, phi1, phi4);
+            printf("  足端位置: (%.3f, %.3f)\n", foot_x, foot_y);
+            printf("  虚拟腿长: %.3f\n", length);
+            printf("  虚拟摆角: %.3f (%.1f度)\n", angle, angle * 180.0f / M_PI);
+            printf("  内部变量: XC=%.3f, YC=%.3f, phi2=%.3f, phi3=%.3f\n", 
+                   vmc.leg.XC, vmc.leg.YC, vmc.leg.phi2, vmc.leg.phi3);
+            printf("\n");
+        } else {
+            printf("测试 %d: 求解失败 (phi1=%.2f, phi4=%.2f)\n", i+1, phi1, phi4);
+        }
+    }
+    
+    return 0;
+}
diff --git a/utils/Simulation-master/.gitignore b/utils/Simulation-master/.gitignore
new file mode 100644
index 0000000..be82517
--- /dev/null
+++ b/utils/Simulation-master/.gitignore
@@ -0,0 +1,13 @@
+# Duplicate files
+*(*)*
+
+# MATLAB files
+balance/parallel_legs/slprj
+balance/series_legs/slprj
+balance/series_parallel_legs/slprj
+*/slprj/
+*/codegen/
+*.prj
+*.slxc
+*.autosave
+*.asv
diff --git a/utils/Simulation-master/LICENSE b/utils/Simulation-master/LICENSE
new file mode 100644
index 0000000..60acd7e
--- /dev/null
+++ b/utils/Simulation-master/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2024 是小企鹅呀
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx
new file mode 100644
index 0000000..1e6fc9c
Binary files /dev/null and b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx differ
diff --git a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.original b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.original
new file mode 100644
index 0000000..7e89fe3
Binary files /dev/null and b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.original differ
diff --git a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2022b b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2022b
new file mode 100644
index 0000000..0f66126
Binary files /dev/null and b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2022b differ
diff --git a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2023a b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2023a
new file mode 100644
index 0000000..0f8003c
Binary files /dev/null and b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2023a differ
diff --git a/utils/Simulation-master/balance/parallel_legs/d_phi0.m b/utils/Simulation-master/balance/parallel_legs/d_phi0.m
new file mode 100644
index 0000000..2657604
--- /dev/null
+++ b/utils/Simulation-master/balance/parallel_legs/d_phi0.m
@@ -0,0 +1,20 @@
+
+syms phi_0 phi_1 phi_2 phi_4 l1 l2 l3 l4 l5 L0 d_phi1 d_phi4
+
+XD=l5+l4*cos(phi_4);
+YD=l4*sin(phi_4);
+XB=l1*cos(phi_1);
+YB=l1*sin(phi_1);
+A0=2*l2*(XD-XB);
+B0=2*l2*(YD-YB);
+lBD=sqrt((XD-XB)^2+(YD-YB)^2);
+C0=l2^2+lBD^2-l3^2;
+phi_2=2*atan((B0+sqrt(A0^2+B0^2-C0^2))/(A0+C0));
+XC=XB+l2*cos(phi_2);
+YC=YB+l2*sin(phi_2);
+L0=sqrt((XC-l5/2)^2+YC^2);
+phi_0=atan(YC/(XC-l5/2));
+
+J=jacobian([L0;phi_0],[phi_1;phi_4]);
+phi0_dot = J(2,1)*d_phi1 +J(2,2)*d_phi4
+
diff --git a/utils/Simulation-master/balance/parallel_legs/display_polynomial.m b/utils/Simulation-master/balance/parallel_legs/display_polynomial.m
new file mode 100644
index 0000000..44467e6
--- /dev/null
+++ b/utils/Simulation-master/balance/parallel_legs/display_polynomial.m
@@ -0,0 +1,19 @@
+% 定义一个函数来显示多项式方程
+function display_polynomial(coefficients, name)
+    equation = sprintf('%s = ', name);
+    n = length(coefficients);
+    for i = 1:n
+        if coefficients(i) ~= 0
+            if i == n
+                term = sprintf('%.4f', coefficients(i));
+            else
+                term = sprintf('%.4f*t%d', coefficients(i), n-i);
+            end
+            if i > 1 && coefficients(i) > 0
+                term = [' + ', term];
+            end
+            equation = [equation, term];
+        end
+    end
+    fprintf('%s;\n', equation);
+end
\ No newline at end of file
diff --git a/utils/Simulation-master/balance/parallel_legs/get_k.m b/utils/Simulation-master/balance/parallel_legs/get_k.m
new file mode 100644
index 0000000..46e1806
--- /dev/null
+++ b/utils/Simulation-master/balance/parallel_legs/get_k.m
@@ -0,0 +1,82 @@
+%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
+tic
+j=1;
+leg=0.1:0.01:0.4;
+for i=leg
+    k=get_k_length(i);
+    k11(j) = k(1,1);
+    k12(j) = k(1,2);
+    k13(j) = k(1,3);
+    k14(j) = k(1,4);
+    k15(j) = k(1,5);
+    k16(j) = k(1,6);
+
+    k21(j) = k(2,1);
+    k22(j) = k(2,2);
+    k23(j) = k(2,3);
+    k24(j) = k(2,4);
+    k25(j) = k(2,5);
+    k26(j) = k(2,6);
+    j=j+1;
+
+    fprintf('leg_length=%d\n', i);
+end
+a11=polyfit(leg,k11,3);
+a12=polyfit(leg,k12,3);
+a13=polyfit(leg,k13,3);
+a14=polyfit(leg,k14,3);
+a15=polyfit(leg,k15,3);
+a16=polyfit(leg,k16,3);
+
+a21=polyfit(leg,k21,3);
+a22=polyfit(leg,k22,3);
+a23=polyfit(leg,k23,3);
+a24=polyfit(leg,k24,3);
+a25=polyfit(leg,k25,3);
+a26=polyfit(leg,k26,3);
+
+toc
+
+% x0=leg;              %步长为0.1
+% y11=polyval(a11,x0);          %返回值y0是对应于x0的函数值
+% y12=polyval(a12,x0);          %返回值y0是对应于x0的函数值
+% y13=polyval(a13,x0);          %返回值y0是对应于x0的函数值
+% y14=polyval(a14,x0);          %返回值y0是对应于x0的函数值
+% y15=polyval(a15,x0);          %返回值y0是对应于x0的函数值
+% y16=polyval(a16,x0);          %返回值y0是对应于x0的函数值
+% 
+% y21=polyval(a21,x0);          %返回值y0是对应于x0的函数值
+% y22=polyval(a22,x0);          %返回值y0是对应于x0的函数值
+% y23=polyval(a23,x0);          %返回值y0是对应于x0的函数值
+% y24=polyval(a24,x0);          %返回值y0是对应于x0的函数值
+% y25=polyval(a25,x0);          %返回值y0是对应于x0的函数值
+% y26=polyval(a26,x0);          %返回值y0是对应于x0的函数值
+% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('x');ylabel('y');title('k11');
+% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('x');ylabel('y');title('k12');
+% subplot(3,4,5);plot(leg,k13,'o',x0,y13,'r');xlabel('x');ylabel('y');title('k13');
+% subplot(3,4,6);plot(leg,k14,'o',x0,y14,'r');xlabel('x');ylabel('y');title('k14');
+% subplot(3,4,9);plot(leg,k15,'o',x0,y15,'r');xlabel('x');ylabel('y');title('k15');
+% subplot(3,4,10);plot(leg,k16,'o',x0,y16,'r');xlabel('x');ylabel('y');title('k16');
+% 
+% subplot(3,4,3);plot(leg,k21,'o',x0,y21,'r');xlabel('x');ylabel('y');title('k21');
+% subplot(3,4,4);plot(leg,k22,'o',x0,y22,'r');xlabel('x');ylabel('y');title('k22');
+% subplot(3,4,7);plot(leg,k23,'o',x0,y23,'r');xlabel('x');ylabel('y');title('k23');
+% subplot(3,4,8);plot(leg,k24,'o',x0,y24,'r');xlabel('x');ylabel('y');title('k24');
+% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('x');ylabel('y');title('k25');
+% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('x');ylabel('y');title('k26');
+% grid on;                   %添加网格线
+% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1);  %将网格线变成虚线
+% fprintf('fp32 a11[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a11(1),a11(2),a11(3),a11(4));
+% fprintf('fp32 a12[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a12(1),a12(2),a12(3),a12(4));
+% fprintf('fp32 a13[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a13(1),a13(2),a13(3),a13(4));
+% fprintf('fp32 a14[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a14(1),a14(2),a14(3),a14(4));
+% fprintf('fp32 a15[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a15(1),a15(2),a15(3),a15(4));
+% fprintf('fp32 a16[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a16(1),a16(2),a16(3),a16(4));
+% 
+% fprintf('fp32 a21[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a21(1),a21(2),a21(3),a21(4));
+% fprintf('fp32 a22[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a22(1),a22(2),a22(3),a22(4));
+% fprintf('fp32 a23[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a23(1),a23(2),a23(3),a23(4));
+% fprintf('fp32 a24[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a24(1),a24(2),a24(3),a24(4));
+% fprintf('fp32 a25[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a25(1),a25(2),a25(3),a25(4));
+% fprintf('fp32 a26[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a26(1),a26(2),a26(3),a26(4));
+toc
diff --git a/utils/Simulation-master/balance/parallel_legs/get_k_length.m b/utils/Simulation-master/balance/parallel_legs/get_k_length.m
new file mode 100644
index 0000000..7f670db
--- /dev/null
+++ b/utils/Simulation-master/balance/parallel_legs/get_k_length.m
@@ -0,0 +1,56 @@
+function K = get_k_length(leg_length)
+   
+    %theta : 摆杆与竖直方向夹角             R   ：驱动轮半径
+    %x     : 驱动轮位移                     L   : 摆杆重心到驱动轮轴距离
+    %phi   : 机体与水平夹角                 LM  : 摆杆重心到其转轴距离
+    %T     ：驱动轮输出力矩                 l   : 机体重心到其转轴距离
+    %Tp    : 髋关节输出力矩                 mw  : 驱动轮转子质量
+    %N     ：驱动轮对摆杆力的水平分量       mp  : 摆杆质量
+    %P     ：驱动轮对摆杆力的竖直分量       M   : 机体质量
+    %Nm    ：摆杆对机体力水平方向分量       Iw  : 驱动轮转子转动惯量
+    %Pm    ：摆杆对机体力竖直方向分量       Ip  : 摆杆绕质心转动惯量
+    %Nf    : 地面对驱动轮摩擦力             Im  : 机体绕质心转动惯量
+
+    syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
+    syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0 
+
+    R1=0.072;                         %驱动轮半径
+    L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
+    LM1=leg_length/2;                 %摆杆重心到其转轴距离
+    l1=0.03;                          %机体质心距离转轴距离
+    mw1=0.80;                         %驱动轮质量
+    mp1=1.11;                         %杆质量
+    M1=2.0;                          %机体质量
+    Iw1=mw1*R1^2;                     %驱动轮转动惯量
+    Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
+    IM1=M1*(0.3^2+0.12^2)/12.0;       %机体绕质心转动惯量
+
+    
+    NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
+    N = NM + mp*diff(x + L*sin(theta),t,2);
+    PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
+    P = PM +mp*g+mp*diff(L*cos(theta),t,2);
+
+    eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
+    eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
+    eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
+    
+    eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+
+    [f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
+   
+    A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
+    A=double(A);
+    B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
+    B=double(B);
+    
+    Q=diag([100 1 500 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[240 0;0 25];                %T Tp
+    
+    K=lqr(A,B,Q,R);
+  
+end
\ No newline at end of file
diff --git a/utils/Simulation-master/balance/parallel_legs/physical_calc.mlx b/utils/Simulation-master/balance/parallel_legs/physical_calc.mlx
new file mode 100644
index 0000000..78bd3d3
Binary files /dev/null and b/utils/Simulation-master/balance/parallel_legs/physical_calc.mlx differ
diff --git a/utils/Simulation-master/balance/series_legs/blance_leg.slx b/utils/Simulation-master/balance/series_legs/blance_leg.slx
new file mode 100644
index 0000000..f1fb734
Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/blance_leg.slx differ
diff --git a/utils/Simulation-master/balance/series_legs/calc.mlx b/utils/Simulation-master/balance/series_legs/calc.mlx
new file mode 100644
index 0000000..fc0f109
Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/calc.mlx differ
diff --git a/utils/Simulation-master/balance/series_legs/doc/2_link.png b/utils/Simulation-master/balance/series_legs/doc/2_link.png
new file mode 100644
index 0000000..035b0fa
Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/doc/2_link.png differ
diff --git a/utils/Simulation-master/balance/series_legs/doc/2连杆分析.md b/utils/Simulation-master/balance/series_legs/doc/2连杆分析.md
new file mode 100644
index 0000000..a1667bd
--- /dev/null
+++ b/utils/Simulation-master/balance/series_legs/doc/2连杆分析.md
@@ -0,0 +1,154 @@
+# 2连杆分析
+
+参考：[五连杆运动学解算与VMC](https://zhuanlan.zhihu.com/p/613007726)
+
+![pic](./2_link.png)
+
+## 1 正运动学解算
+
+$\phi_1$和$\phi_2$可由电机编码器测量得到。
+
+$C$点坐标：
+
+$$
+\left \{
+    \begin{array}{l}
+        x_C = l_1\cos\phi_1 + l_2\cos\phi_2\\
+        y_C = l_1\sin\phi_1 + l_2\sin\phi_2
+    \end{array}
+\right .
+$$
+
+得：
+
+$$
+\left \{
+    \begin{array}{l}
+        L0 = \sqrt{x_C^2 + y_C^2} \\
+        \phi_0 = \arctan{\frac{y_C}{x_C}}
+    \end{array}
+\right .
+$$
+
+## 2 逆运动学解算
+
+由余弦定理易得：
+
+$$
+\phi_2+\phi_3 = \arccos\frac{l_1^2+l_2^2-L_0^2}{2l_1l_2}
+$$
+
+又：
+
+$$
+\phi_3 = \pi - \phi_1
+$$
+
+得：
+
+$$
+\phi_2 = \arccos\frac{l_1^2+l_2^2-L_0^2}{2l_1l_2}+\phi_1-\pi
+$$
+
+## 3 雅可比矩阵
+
+<!-- $$
+\left [
+    \begin{matrix}
+        \dot L0 \\
+        \dot \phi_0
+    \end{matrix}
+\right ]
+
+ =
+
+J
+\left [
+    \begin{matrix}
+        \dot\phi_1 \\
+        \dot\phi_2
+    \end{matrix}
+\right ]
+$$ -->
+
+基于文中描述可得：
+
+$$
+\left \{
+    \begin{array}{l}
+        \dot x_C = -l_1\dot\phi_1\sin\phi_1 - l_2\dot\phi_2\sin\phi_2\\
+        \dot y_C = l_1\dot\phi_1\cos\phi_1 + l_2\dot\phi_2\cos\phi_2
+    \end{array}
+\right .
+$$
+
+即：
+
+$$
+\left [
+    \begin{matrix}
+        \dot x_C \\
+        \dot y_C
+    \end{matrix}
+\right ]
+
+ =
+\left [
+    \begin{matrix}
+        -l_1\sin\phi_1 & -l_2\sin\phi_2 \\
+         l_1\cos\phi_1 &  l_2\cos\phi_2
+    \end{matrix}
+\right ]
+
+\left [
+    \begin{matrix}
+        \dot\phi_1 \\
+        \dot\phi_2
+    \end{matrix}
+\right ]
+$$
+
+记作：
+
+$$
+\left [
+    \begin{matrix}
+        \dot x_C \\
+        \dot y_C
+    \end{matrix}
+\right ]
+
+ =
+J_0
+
+\left [
+    \begin{matrix}
+        \dot\phi_1 \\
+        \dot\phi_2
+    \end{matrix}
+\right ]
+$$
+
+下面操作与文中相同，可得：
+
+$$
+J^T = J_0^TRM =
+\left[
+    \begin{matrix}
+        l_1 \,\sin \left(\phi_0 -\phi_1 \right) & \frac{l_1 \,\cos \left(\phi_0 -\phi_1 \right)}{L_0 }\\
+        l_2 \,\sin \left(\phi_0 -\phi_2 \right) & \frac{l_2 \,\cos \left(\phi_0 -\phi_2 \right)}{L_0 }
+    \end{matrix}
+\right]
+$$
+
+即：
+
+$$
+J =
+\left[
+    \begin{matrix}
+        l_1 \,\sin \left(\phi_0 -\phi_1 \right) & l_2 \,\sin \left(\phi_0 -\phi_2 \right)\\
+        \frac{l_1 \,\cos \left(\phi_0 -\phi_1 \right)}{L_0 } & \frac{l_2 \,\cos \left(\phi_0 -\phi_2 \right)}{L_0 }
+    \end{matrix}
+\right]
+$$
diff --git a/utils/Simulation-master/balance/series_legs/get_k.m b/utils/Simulation-master/balance/series_legs/get_k.m
new file mode 100644
index 0000000..46e1806
--- /dev/null
+++ b/utils/Simulation-master/balance/series_legs/get_k.m
@@ -0,0 +1,82 @@
+%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
+tic
+j=1;
+leg=0.1:0.01:0.4;
+for i=leg
+    k=get_k_length(i);
+    k11(j) = k(1,1);
+    k12(j) = k(1,2);
+    k13(j) = k(1,3);
+    k14(j) = k(1,4);
+    k15(j) = k(1,5);
+    k16(j) = k(1,6);
+
+    k21(j) = k(2,1);
+    k22(j) = k(2,2);
+    k23(j) = k(2,3);
+    k24(j) = k(2,4);
+    k25(j) = k(2,5);
+    k26(j) = k(2,6);
+    j=j+1;
+
+    fprintf('leg_length=%d\n', i);
+end
+a11=polyfit(leg,k11,3);
+a12=polyfit(leg,k12,3);
+a13=polyfit(leg,k13,3);
+a14=polyfit(leg,k14,3);
+a15=polyfit(leg,k15,3);
+a16=polyfit(leg,k16,3);
+
+a21=polyfit(leg,k21,3);
+a22=polyfit(leg,k22,3);
+a23=polyfit(leg,k23,3);
+a24=polyfit(leg,k24,3);
+a25=polyfit(leg,k25,3);
+a26=polyfit(leg,k26,3);
+
+toc
+
+% x0=leg;              %步长为0.1
+% y11=polyval(a11,x0);          %返回值y0是对应于x0的函数值
+% y12=polyval(a12,x0);          %返回值y0是对应于x0的函数值
+% y13=polyval(a13,x0);          %返回值y0是对应于x0的函数值
+% y14=polyval(a14,x0);          %返回值y0是对应于x0的函数值
+% y15=polyval(a15,x0);          %返回值y0是对应于x0的函数值
+% y16=polyval(a16,x0);          %返回值y0是对应于x0的函数值
+% 
+% y21=polyval(a21,x0);          %返回值y0是对应于x0的函数值
+% y22=polyval(a22,x0);          %返回值y0是对应于x0的函数值
+% y23=polyval(a23,x0);          %返回值y0是对应于x0的函数值
+% y24=polyval(a24,x0);          %返回值y0是对应于x0的函数值
+% y25=polyval(a25,x0);          %返回值y0是对应于x0的函数值
+% y26=polyval(a26,x0);          %返回值y0是对应于x0的函数值
+% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('x');ylabel('y');title('k11');
+% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('x');ylabel('y');title('k12');
+% subplot(3,4,5);plot(leg,k13,'o',x0,y13,'r');xlabel('x');ylabel('y');title('k13');
+% subplot(3,4,6);plot(leg,k14,'o',x0,y14,'r');xlabel('x');ylabel('y');title('k14');
+% subplot(3,4,9);plot(leg,k15,'o',x0,y15,'r');xlabel('x');ylabel('y');title('k15');
+% subplot(3,4,10);plot(leg,k16,'o',x0,y16,'r');xlabel('x');ylabel('y');title('k16');
+% 
+% subplot(3,4,3);plot(leg,k21,'o',x0,y21,'r');xlabel('x');ylabel('y');title('k21');
+% subplot(3,4,4);plot(leg,k22,'o',x0,y22,'r');xlabel('x');ylabel('y');title('k22');
+% subplot(3,4,7);plot(leg,k23,'o',x0,y23,'r');xlabel('x');ylabel('y');title('k23');
+% subplot(3,4,8);plot(leg,k24,'o',x0,y24,'r');xlabel('x');ylabel('y');title('k24');
+% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('x');ylabel('y');title('k25');
+% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('x');ylabel('y');title('k26');
+% grid on;                   %添加网格线
+% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1);  %将网格线变成虚线
+% fprintf('fp32 a11[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a11(1),a11(2),a11(3),a11(4));
+% fprintf('fp32 a12[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a12(1),a12(2),a12(3),a12(4));
+% fprintf('fp32 a13[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a13(1),a13(2),a13(3),a13(4));
+% fprintf('fp32 a14[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a14(1),a14(2),a14(3),a14(4));
+% fprintf('fp32 a15[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a15(1),a15(2),a15(3),a15(4));
+% fprintf('fp32 a16[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a16(1),a16(2),a16(3),a16(4));
+% 
+% fprintf('fp32 a21[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a21(1),a21(2),a21(3),a21(4));
+% fprintf('fp32 a22[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a22(1),a22(2),a22(3),a22(4));
+% fprintf('fp32 a23[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a23(1),a23(2),a23(3),a23(4));
+% fprintf('fp32 a24[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a24(1),a24(2),a24(3),a24(4));
+% fprintf('fp32 a25[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a25(1),a25(2),a25(3),a25(4));
+% fprintf('fp32 a26[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a26(1),a26(2),a26(3),a26(4));
+toc
diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m
new file mode 100644
index 0000000..f5cc9e7
--- /dev/null
+++ b/utils/Simulation-master/balance/series_legs/get_k_length.m
@@ -0,0 +1,56 @@
+function K = get_k_length(leg_length)
+   
+    %theta : 摆杆与竖直方向夹角             R   ：驱动轮半径
+    %x     : 驱动轮位移                     L   : 摆杆重心到驱动轮轴距离
+    %phi   : 机体与水平夹角                 LM  : 摆杆重心到其转轴距离
+    %T     ：驱动轮输出力矩                 l   : 机体重心到其转轴距离
+    %Tp    : 髋关节输出力矩                 mw  : 驱动轮转子质量
+    %N     ：驱动轮对摆杆力的水平分量       mp  : 摆杆质量
+    %P     ：驱动轮对摆杆力的竖直分量       M   : 机体质量
+    %Nm    ：摆杆对机体力水平方向分量       Iw  : 驱动轮转子转动惯量
+    %Pm    ：摆杆对机体力竖直方向分量       Ip  : 摆杆绕质心转动惯量
+    %Nf    : 地面对驱动轮摩擦力             Im  : 机体绕质心转动惯量
+
+    syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
+    syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0 
+
+    R1=0.072;                         %驱动轮半径
+    L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
+    LM1=leg_length/2;                 %摆杆重心到其转轴距离
+    l1=0.03;                          %机体质心距离转轴距离
+    mw1=0.60;                         %驱动轮质量
+    mp1=1.8;                         %杆质量
+    M1=1.8;                          %机体质量
+    Iw1=mw1*R1^2;                     %驱动轮转动惯量
+    Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
+    IM1=M1*(0.3^2+0.12^2)/12.0;       %机体绕质心转动惯量
+
+    
+    NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
+    N = NM + mp*diff(x + L*sin(theta),t,2);
+    PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
+    P = PM +mp*g+mp*diff(L*cos(theta),t,2);
+
+    eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
+    eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
+    eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
+    
+    eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+
+    [f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
+   
+    A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
+    A=double(A);
+    B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
+    B=double(B);
+    
+    Q=diag([100 1 500 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[240 0;0 25];                %T Tp
+    
+    K=lqr(A,B,Q,R);
+  
+end
\ No newline at end of file
diff --git a/utils/Simulation-master/balance/series_legs/leg_sim.slx b/utils/Simulation-master/balance/series_legs/leg_sim.slx
new file mode 100644
index 0000000..5afa555
Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/leg_sim.slx differ
diff --git a/utils/Simulation-master/balance/series_parallel_legs/README.md b/utils/Simulation-master/balance/series_parallel_legs/README.md
new file mode 100644
index 0000000..19bad54
--- /dev/null
+++ b/utils/Simulation-master/balance/series_parallel_legs/README.md
@@ -0,0 +1,3 @@
+# 串并联腿仿真总结
+
+这次仿真对小企鹅来说最重要的一点就是确定了替换为串联腿时不需要替换模型，直接移植现有并联腿模型即可。
\ No newline at end of file
diff --git a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx
new file mode 100644
index 0000000..741bffd
Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx differ
diff --git a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.original b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.original
new file mode 100644
index 0000000..7e89fe3
Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.original differ
diff --git a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2022b b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2022b
new file mode 100644
index 0000000..0f66126
Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2022b differ
diff --git a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2023a b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2023a
new file mode 100644
index 0000000..0f8003c
Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2023a differ
diff --git a/utils/Simulation-master/balance/series_parallel_legs/d_phi0.m b/utils/Simulation-master/balance/series_parallel_legs/d_phi0.m
new file mode 100644
index 0000000..2657604
--- /dev/null
+++ b/utils/Simulation-master/balance/series_parallel_legs/d_phi0.m
@@ -0,0 +1,20 @@
+
+syms phi_0 phi_1 phi_2 phi_4 l1 l2 l3 l4 l5 L0 d_phi1 d_phi4
+
+XD=l5+l4*cos(phi_4);
+YD=l4*sin(phi_4);
+XB=l1*cos(phi_1);
+YB=l1*sin(phi_1);
+A0=2*l2*(XD-XB);
+B0=2*l2*(YD-YB);
+lBD=sqrt((XD-XB)^2+(YD-YB)^2);
+C0=l2^2+lBD^2-l3^2;
+phi_2=2*atan((B0+sqrt(A0^2+B0^2-C0^2))/(A0+C0));
+XC=XB+l2*cos(phi_2);
+YC=YB+l2*sin(phi_2);
+L0=sqrt((XC-l5/2)^2+YC^2);
+phi_0=atan(YC/(XC-l5/2));
+
+J=jacobian([L0;phi_0],[phi_1;phi_4]);
+phi0_dot = J(2,1)*d_phi1 +J(2,2)*d_phi4
+
diff --git a/utils/Simulation-master/balance/series_parallel_legs/display_polynomial.m b/utils/Simulation-master/balance/series_parallel_legs/display_polynomial.m
new file mode 100644
index 0000000..44467e6
--- /dev/null
+++ b/utils/Simulation-master/balance/series_parallel_legs/display_polynomial.m
@@ -0,0 +1,19 @@
+% 定义一个函数来显示多项式方程
+function display_polynomial(coefficients, name)
+    equation = sprintf('%s = ', name);
+    n = length(coefficients);
+    for i = 1:n
+        if coefficients(i) ~= 0
+            if i == n
+                term = sprintf('%.4f', coefficients(i));
+            else
+                term = sprintf('%.4f*t%d', coefficients(i), n-i);
+            end
+            if i > 1 && coefficients(i) > 0
+                term = [' + ', term];
+            end
+            equation = [equation, term];
+        end
+    end
+    fprintf('%s;\n', equation);
+end
\ No newline at end of file
diff --git a/utils/Simulation-master/balance/series_parallel_legs/get_k.m b/utils/Simulation-master/balance/series_parallel_legs/get_k.m
new file mode 100644
index 0000000..46e1806
--- /dev/null
+++ b/utils/Simulation-master/balance/series_parallel_legs/get_k.m
@@ -0,0 +1,82 @@
+%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
+tic
+j=1;
+leg=0.1:0.01:0.4;
+for i=leg
+    k=get_k_length(i);
+    k11(j) = k(1,1);
+    k12(j) = k(1,2);
+    k13(j) = k(1,3);
+    k14(j) = k(1,4);
+    k15(j) = k(1,5);
+    k16(j) = k(1,6);
+
+    k21(j) = k(2,1);
+    k22(j) = k(2,2);
+    k23(j) = k(2,3);
+    k24(j) = k(2,4);
+    k25(j) = k(2,5);
+    k26(j) = k(2,6);
+    j=j+1;
+
+    fprintf('leg_length=%d\n', i);
+end
+a11=polyfit(leg,k11,3);
+a12=polyfit(leg,k12,3);
+a13=polyfit(leg,k13,3);
+a14=polyfit(leg,k14,3);
+a15=polyfit(leg,k15,3);
+a16=polyfit(leg,k16,3);
+
+a21=polyfit(leg,k21,3);
+a22=polyfit(leg,k22,3);
+a23=polyfit(leg,k23,3);
+a24=polyfit(leg,k24,3);
+a25=polyfit(leg,k25,3);
+a26=polyfit(leg,k26,3);
+
+toc
+
+% x0=leg;              %步长为0.1
+% y11=polyval(a11,x0);          %返回值y0是对应于x0的函数值
+% y12=polyval(a12,x0);          %返回值y0是对应于x0的函数值
+% y13=polyval(a13,x0);          %返回值y0是对应于x0的函数值
+% y14=polyval(a14,x0);          %返回值y0是对应于x0的函数值
+% y15=polyval(a15,x0);          %返回值y0是对应于x0的函数值
+% y16=polyval(a16,x0);          %返回值y0是对应于x0的函数值
+% 
+% y21=polyval(a21,x0);          %返回值y0是对应于x0的函数值
+% y22=polyval(a22,x0);          %返回值y0是对应于x0的函数值
+% y23=polyval(a23,x0);          %返回值y0是对应于x0的函数值
+% y24=polyval(a24,x0);          %返回值y0是对应于x0的函数值
+% y25=polyval(a25,x0);          %返回值y0是对应于x0的函数值
+% y26=polyval(a26,x0);          %返回值y0是对应于x0的函数值
+% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('x');ylabel('y');title('k11');
+% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('x');ylabel('y');title('k12');
+% subplot(3,4,5);plot(leg,k13,'o',x0,y13,'r');xlabel('x');ylabel('y');title('k13');
+% subplot(3,4,6);plot(leg,k14,'o',x0,y14,'r');xlabel('x');ylabel('y');title('k14');
+% subplot(3,4,9);plot(leg,k15,'o',x0,y15,'r');xlabel('x');ylabel('y');title('k15');
+% subplot(3,4,10);plot(leg,k16,'o',x0,y16,'r');xlabel('x');ylabel('y');title('k16');
+% 
+% subplot(3,4,3);plot(leg,k21,'o',x0,y21,'r');xlabel('x');ylabel('y');title('k21');
+% subplot(3,4,4);plot(leg,k22,'o',x0,y22,'r');xlabel('x');ylabel('y');title('k22');
+% subplot(3,4,7);plot(leg,k23,'o',x0,y23,'r');xlabel('x');ylabel('y');title('k23');
+% subplot(3,4,8);plot(leg,k24,'o',x0,y24,'r');xlabel('x');ylabel('y');title('k24');
+% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('x');ylabel('y');title('k25');
+% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('x');ylabel('y');title('k26');
+% grid on;                   %添加网格线
+% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1);  %将网格线变成虚线
+% fprintf('fp32 a11[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a11(1),a11(2),a11(3),a11(4));
+% fprintf('fp32 a12[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a12(1),a12(2),a12(3),a12(4));
+% fprintf('fp32 a13[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a13(1),a13(2),a13(3),a13(4));
+% fprintf('fp32 a14[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a14(1),a14(2),a14(3),a14(4));
+% fprintf('fp32 a15[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a15(1),a15(2),a15(3),a15(4));
+% fprintf('fp32 a16[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a16(1),a16(2),a16(3),a16(4));
+% 
+% fprintf('fp32 a21[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a21(1),a21(2),a21(3),a21(4));
+% fprintf('fp32 a22[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a22(1),a22(2),a22(3),a22(4));
+% fprintf('fp32 a23[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a23(1),a23(2),a23(3),a23(4));
+% fprintf('fp32 a24[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a24(1),a24(2),a24(3),a24(4));
+% fprintf('fp32 a25[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a25(1),a25(2),a25(3),a25(4));
+% fprintf('fp32 a26[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a26(1),a26(2),a26(3),a26(4));
+toc
diff --git a/utils/Simulation-master/balance/series_parallel_legs/get_k_length.m b/utils/Simulation-master/balance/series_parallel_legs/get_k_length.m
new file mode 100644
index 0000000..9a1d58c
--- /dev/null
+++ b/utils/Simulation-master/balance/series_parallel_legs/get_k_length.m
@@ -0,0 +1,56 @@
+function K = get_k_length(leg_length)
+   
+    %theta : 摆杆与竖直方向夹角             R   ：驱动轮半径
+    %x     : 驱动轮位移                     L   : 摆杆重心到驱动轮轴距离
+    %phi   : 机体与水平夹角                 LM  : 摆杆重心到其转轴距离
+    %T     ：驱动轮输出力矩                 l   : 机体重心到其转轴距离
+    %Tp    : 髋关节输出力矩                 mw  : 驱动轮转子质量
+    %N     ：驱动轮对摆杆力的水平分量       mp  : 摆杆质量
+    %P     ：驱动轮对摆杆力的竖直分量       M   : 机体质量
+    %Nm    ：摆杆对机体力水平方向分量       Iw  : 驱动轮转子转动惯量
+    %Pm    ：摆杆对机体力竖直方向分量       Ip  : 摆杆绕质心转动惯量
+    %Nf    : 地面对驱动轮摩擦力             Im  : 机体绕质心转动惯量
+
+    syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
+    syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0 
+
+    R1=0.086;                         %驱动轮半径
+    L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
+    LM1=leg_length/2;                 %摆杆重心到其转轴距离
+    l1=0.03;                          %机体质心距离转轴距离
+    mw1=1.18;                         %驱动轮质量
+    mp1=1.11;                         %杆质量
+    M1=10.3;                          %机体质量
+    Iw1=mw1*R1^2;                     %驱动轮转动惯量
+    Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
+    IM1=M1*(0.3^2+0.12^2)/12.0;       %机体绕质心转动惯量
+
+    
+    NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
+    N = NM + mp*diff(x + L*sin(theta),t,2);
+    PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
+    P = PM +mp*g+mp*diff(L*cos(theta),t,2);
+
+    eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
+    eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
+    eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
+    
+    eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+    eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
+
+    [f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
+   
+    A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
+    A=double(A);
+    B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
+    B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
+    B=double(B);
+    
+    Q=diag([100 1 500 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[240 0;0 25];                %T Tp
+    
+    K=lqr(A,B,Q,R);
+  
+end
\ No newline at end of file
diff --git a/utils/Simulation-master/balance/series_parallel_legs/physical_calc.mlx b/utils/Simulation-master/balance/series_parallel_legs/physical_calc.mlx
new file mode 100644
index 0000000..78bd3d3
Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/physical_calc.mlx differ
diff --git a/utils/Simulation-master/balance/series_parallel_legs/save_phy.slx b/utils/Simulation-master/balance/series_parallel_legs/save_phy.slx
new file mode 100644
index 0000000..da20365
Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/save_phy.slx differ
diff --git a/utils/Simulation-master/dog_simple/dog.slx b/utils/Simulation-master/dog_simple/dog.slx
new file mode 100644
index 0000000..0e5c4c8
Binary files /dev/null and b/utils/Simulation-master/dog_simple/dog.slx differ
diff --git a/utils/Simulation-master/dog_simple/dog.slx.r2023a b/utils/Simulation-master/dog_simple/dog.slx.r2023a
new file mode 100644
index 0000000..f1fb734
Binary files /dev/null and b/utils/Simulation-master/dog_simple/dog.slx.r2023a differ
diff --git a/utils/vmc.m b/utils/vmc.m
index e116f34..507f912 100644
--- a/utils/vmc.m
+++ b/utils/vmc.m
@@ -1,219 +1,173 @@
-function [tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual)
-% VMC (Virtual Model Control) for serial leg mechanism
-% 基于虚拟模型控制的串联腿机构力矩计算
-%
-% 输入参数:
-%   L1: 大腿长度 (m)
-%   L2: 小腿长度 (m) 
-%   q1: 髋关节角度 (rad)
-%   q2: 膝关节角度 (rad)
-%   F_virtual: 虚拟力大小 (N)
-%   theta_virtual: 虚拟力方向角度 (rad)
-%
-% 输出:
-%   tau_hip: 髋关节所需力矩 (N*m)
-%   tau_knee: 膝关节所需力矩 (N*m)
+%% 五连杆机构腿长计算脚本
+clear; clc;
 
-    % 计算雅可比矩阵
-    J = compute_jacobian(L1, L2, q1, q2);
+%% 定义输入参数
+% 连杆长度 (m)
+l1 = 0.215;   % 连杆1长度
+l2 = 0.258;  % 连杆2长度  
+l3 = 0.258;  % 连杆3长度
+l4 = 0.215;   % 连杆4长度
+l5 = 0.0;   % 连杆5长度
+
+% 关节角度 (弧度)
+phi1 = 0.10;  % 30度
+phi4 = 2.96;  % 45度
+
+%% 调用腿长计算函数
+fprintf('五连杆机构腿长计算\n');
+fprintf('输入参数:\n');
+fprintf('  连杆长度: l1=%.3f, l2=%.3f, l3=%.3f, l4=%.3f, l5=%.3f (m)\n', l1, l2, l3, l4, l5);
+fprintf('  关节角度: phi1=%.3f (%.1f°), phi4=%.3f (%.1f°)\n', phi1, rad2deg(phi1), phi4, rad2deg(phi4));
+
+try
+    % 计算各关键点位置
+    % B点 (连杆1末端)
+    x_B = l1 * cos(phi1);
+    y_B = l1 * sin(phi1);
     
-    % 虚拟力向量 (笛卡尔空间)
-    F_cartesian = [F_virtual * cos(theta_virtual);
-                   F_virtual * sin(theta_virtual)];
+    % D点 (连杆4末端)
+    x_D = l5 + l4 * cos(phi4);
+    y_D = l4 * sin(phi4);
     
-    % 通过雅可比转置将笛卡尔力转换为关节力矩
-    tau = J' * F_cartesian;
+    % 计算BD距离
+    BD_dist = sqrt((x_D - x_B)^2 + (y_D - y_B)^2);
     
-    tau_hip = tau(1);
-    tau_knee = tau(2);
-end
-
-function J = compute_jacobian(L1, L2, q1, q2)
-% 计算串联腿机构的雅可比矩阵
-% 从关节空间到足端笛卡尔空间的映射
-%
-% 输入参数:
-%   L1: 大腿长度 (m)
-%   L2: 小腿长度 (m)
-%   q1: 髋关节角度 (rad)
-%   q2: 膝关节角度 (rad)
-%
-% 输出:
-%   J: 2x2雅可比矩阵
-
-    % 足端位置 (相对于髋关节)
-    % x = L1*cos(q1) + L2*cos(q1+q2)
-    % y = L1*sin(q1) + L2*sin(q1+q2)
-    
-    % 雅可比矩阵元素
-    J11 = -L1*sin(q1) - L2*sin(q1+q2);  % dx/dq1
-    J12 = -L2*sin(q1+q2);               % dx/dq2
-    J21 = L1*cos(q1) + L2*cos(q1+q2);   % dy/dq1
-    J22 = L2*cos(q1+q2);                % dy/dq2
-    
-    J = [J11, J12;
-         J21, J22];
-end
-
-function [x, y] = forward_kinematics(L1, L2, q1, q2)
-% 串联腿正运动学 - 足端位置
-%
-% 输入参数:
-%   L1: 大腿长度 (m)
-%   L2: 小腿长度 (m)
-%   q1: 髋关节角度 (rad)
-%   q2: 膝关节角度 (rad)
-%
-% 输出:
-%   x: 足端x坐标 (m)
-%   y: 足端y坐标 (m)
-
-    x = L1*cos(q1) + L2*cos(q1+q2);
-    y = L1*sin(q1) + L2*sin(q1+q2);
-end
-
-function [q1, q2] = inverse_kinematics(L1, L2, x, y)
-% 串联腿逆运动学 - 关节角度
-%
-% 输入参数:
-%   L1: 大腿长度 (m)
-%   L2: 小腿长度 (m)
-%   x: 足端目标x坐标 (m)
-%   y: 足端目标y坐标 (m)
-%
-% 输出:
-%   q1: 髋关节角度 (rad)
-%   q2: 膝关节角度 (rad)
-
-    % 计算到足端的距离
-    r = sqrt(x^2 + y^2);
-    
-    % 检查工作空间
-    if r > (L1 + L2) || r < abs(L1 - L2)
-        error('目标位置超出工作空间范围');
+    % 检查三角形约束条件
+    if BD_dist > (l2 + l3) || BD_dist < abs(l2 - l3)
+        warning('无法形成闭合五连杆机构！BD距离=%.3f, 需要满足: %.3f < BD < %.3f', ...
+                BD_dist, abs(l2-l3), l2+l3);
+        leg_length = NaN;
+        foot_pos = [NaN, NaN];
+        return;
     end
     
-    % 使用余弦定理计算膝关节角度
-    cos_q2 = (r^2 - L1^2 - L2^2) / (2*L1*L2);
-    q2 = acos(cos_q2);  % 肘部向上的解
+    % 使用余弦定理求解phi2
+    cos_phi2_BD = (l2^2 + BD_dist^2 - l3^2) / (2 * l2 * BD_dist);
     
-    % 计算髋关节角度
-    alpha = atan2(y, x);
-    beta = atan2(L2*sin(q2), L1 + L2*cos(q2));
-    q1 = alpha - beta;
-end
-
-function [angle_equiv, length_equiv] = serial_to_virtual_leg(L1, L2, q1, q2)
-% 将串联腿转换为等效摆动杆表示 (对应C代码中的正运动学)
-%
-% 输入参数:
-%   L1: 大腿长度 (m)
-%   L2: 小腿长度 (m)
-%   q1: 髋关节角度 (rad)
-%   q2: 膝关节角度 (rad)
-%
-% 输出:
-%   angle_equiv: 等效摆动杆角度 (rad)
-%   length_equiv: 等效摆动杆长度 (m)
-
-    % 对应C代码中的计算
-    q4 = (pi - q1 - q2) / 2;
-    
-    % 使用余弦定理求解等效摆动杆长度
-    a = 1.0;
-    b = -2.0 * L1 * cos(q4);
-    c = L1^2 - L2^2;
-    
-    discriminant = b^2 - 4*a*c;
-    
-    if discriminant < 0
-        error('无实数解，配置不可达');
+    % 检查余弦值是否在有效范围内
+    if abs(cos_phi2_BD) > 1
+        warning('余弦值超出范围: %.3f', cos_phi2_BD);
+        leg_length = NaN;
+        foot_pos = [NaN, NaN];
+        return;
     end
     
-    sqrt_discriminant = sqrt(discriminant);
-    L3_1 = (-b + sqrt_discriminant) / (2*a);
-    L3_2 = (-b - sqrt_discriminant) / (2*a);
+    % 计算BD方向角
+    alpha_BD = atan2(y_D - y_B, x_D - x_B);
     
-    % 选择正的解
-    if L3_1 > 0 && L3_2 > 0
-        L3 = min(L3_1, L3_2);  % 选择较小的解
-    elseif L3_1 > 0
-        L3 = L3_1;
-    elseif L3_2 > 0
-        L3 = L3_2;
+    % 计算phi2 (两个解，选择合理的一个)
+    angle_offset = acos(cos_phi2_BD);
+    phi2_solution1 = alpha_BD + angle_offset;
+    phi2_solution2 = alpha_BD - angle_offset;
+    
+    % 选择合理的解 (通常选择使机构在合理配置的解)
+    phi2 = phi2_solution1;  % 可以根据实际需要调整选择逻辑
+    
+    % 计算C点位置 (足端位置)
+    x_C = x_B + l2 * cos(phi2);
+    y_C = y_B + l2 * sin(phi2);
+    
+    % 验证C点到D点距离是否等于l3
+    CD_dist = sqrt((x_D - x_C)^2 + (y_D - y_C)^2);
+    error_CD = abs(CD_dist - l3);
+    
+    if error_CD > 1e-6
+        fprintf('警告: CD距离误差较大 (%.6f)\n', error_CD);
+    end
+    
+    % 计算腿长 (假设为原点到C点的距离)
+    leg_length = sqrt(x_C^2 + y_C^2);
+    foot_pos = [x_C, y_C];
+    
+    % 输出结果
+    fprintf('\n计算结果:\n');
+    fprintf('  B点位置: (%.3f, %.3f)\n', x_B, y_B);
+    fprintf('  C点位置: (%.3f, %.3f)\n', x_C, y_C);
+    fprintf('  D点位置: (%.3f, %.3f)\n', x_D, y_D);
+    fprintf('  phi2角度: %.3f rad (%.1f°)\n', phi2, rad2deg(phi2));
+    fprintf('  腿长: %.3f m\n', leg_length);
+    fprintf('  BD距离: %.3f m\n', BD_dist);
+    fprintf('  CD验证: %.6f m (应该等于l3=%.3f)\n', CD_dist, l3);
+    
+    %% 可视化结果
+    figure;
+    plot([0, x_B], [0, y_B], 'b-', 'LineWidth', 2); % 连杆1
+    hold on;
+    plot([x_B, x_C], [y_B, y_C], 'r-', 'LineWidth', 2); % 连杆2
+    plot([x_C, x_D], [y_C, y_D], 'g-', 'LineWidth', 2); % 连杆3
+    plot([l5, x_D], [0, y_D], 'm-', 'LineWidth', 2); % 连杆4
+    plot([0, l5], [0, 0], 'k-', 'LineWidth', 3); % 连杆5 (基座)
+    
+    % 标记关键点
+    plot(0, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % A点
+    plot(x_B, y_B, 'bo', 'MarkerSize', 8, 'MarkerFaceColor', 'b'); % B点
+    plot(x_C, y_C, 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); % C点
+    plot(x_D, y_D, 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g'); % D点
+    plot(l5, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % E点
+    
+    % 标记腿长
+    plot([0, x_C], [0, y_C], 'k--', 'LineWidth', 1); % 腿长线
+    
+    % 图形设置
+    grid on;
+    axis equal;
+    xlabel('X (m)');
+    ylabel('Y (m)');
+    title('五连杆机构示意图');
+    legend('连杆1', '连杆2', '连杆3', '连杆4', '基座', 'Location', 'best');
+    
+    % 添加文本标注
+    text(0, 0-0.02, 'A', 'HorizontalAlignment', 'center');
+    text(x_B, y_B+0.02, 'B', 'HorizontalAlignment', 'center');
+    text(x_C, y_C+0.02, 'C(足端)', 'HorizontalAlignment', 'center');
+    text(x_D, y_D+0.02, 'D', 'HorizontalAlignment', 'center');
+    text(l5, 0-0.02, 'E', 'HorizontalAlignment', 'center');
+    text(x_C/2, y_C/2, sprintf('腿长=%.3fm', leg_length), 'HorizontalAlignment', 'center');
+    
+catch ME
+    fprintf('计算过程中出现错误: %s\n', ME.message);
+    leg_length = NaN;
+    foot_pos = [NaN, NaN];
+end
+
+%% 多组参数测试
+fprintf('\n\n=== 多组参数测试 ===\n');
+test_cases = [
+    pi/6,  pi/4;   % 30°, 45°
+    pi/4,  pi/3;   % 45°, 60°
+    -pi/6, pi/2;   % -30°, 90°
+    0,     pi/6;   % 0°, 30°
+];
+
+for i = 1:size(test_cases, 1)
+    phi1_test = test_cases(i, 1);
+    phi4_test = test_cases(i, 2);
+    
+    fprintf('\n测试 %d: phi1=%.1f°, phi4=%.1f°\n', i, rad2deg(phi1_test), rad2deg(phi4_test));
+    
+    % 重复计算过程（简化版）
+    x_B_test = l1 * cos(phi1_test);
+    y_B_test = l1 * sin(phi1_test);
+    x_D_test = l5 + l4 * cos(phi4_test);
+    y_D_test = l4 * sin(phi4_test);
+    BD_dist_test = sqrt((x_D_test - x_B_test)^2 + (y_D_test - y_B_test)^2);
+    
+    if BD_dist_test <= (l2 + l3) && BD_dist_test >= abs(l2 - l3)
+        cos_phi2_BD_test = (l2^2 + BD_dist_test^2 - l3^2) / (2 * l2 * BD_dist_test);
+        if abs(cos_phi2_BD_test) <= 1
+            alpha_BD_test = atan2(y_D_test - y_B_test, x_D_test - x_B_test);
+            angle_offset_test = acos(cos_phi2_BD_test);
+            phi2_test = alpha_BD_test + angle_offset_test;
+            x_C_test = x_B_test + l2 * cos(phi2_test);
+            y_C_test = y_B_test + l2 * sin(phi2_test);
+            leg_length_test = sqrt(x_C_test^2 + y_C_test^2);
+            fprintf('  结果: 腿长=%.3f m, 足端位置=(%.3f, %.3f)\n', leg_length_test, x_C_test, y_C_test);
+        else
+            fprintf('  结果: 余弦值超出范围\n');
+        end
     else
-        error('无正解');
+        fprintf('  结果: 无法形成闭合机构\n');
     end
-    
-    angle_equiv = q1 + q4;
-    length_equiv = L3;
 end
 
-function [q1, q2] = virtual_to_serial_leg(L1, L2, angle_equiv, length_equiv)
-% 将等效摆动杆转换为串联腿表示 (对应C代码中的逆运动学)
-%
-% 输入参数:
-%   L1: 大腿长度 (m)
-%   L2: 小腿长度 (m)
-%   angle_equiv: 等效摆动杆角度 (rad)
-%   length_equiv: 等效摆动杆长度 (m)
-%
-% 输出:
-%   q1: 髋关节角度 (rad)
-%   q2: 膝关节角度 (rad)
-
-    h = length_equiv;
-    q = angle_equiv;
-    
-    % 计算cos(q4)
-    cos_q4 = (L1^2 + h^2 - L2^2) / (2.0 * L1 * h);
-    
-    % 检查范围
-    if cos_q4 < -1.0 || cos_q4 > 1.0
-        error('不可达配置');
-    end
-    
-    q4 = acos(cos_q4);
-    
-    % 计算关节角度
-    q1 = q - q4;
-    q2 = pi - 2.0 * q4 - q1;
-end
-
-% 示例使用函数
-function demo_vmc_control()
-% VMC控制示例
-    
-    % 腿部参数
-    L1 = 0.15;  % 大腿长度 15cm
-    L2 = 0.15;  % 小腿长度 15cm
-    
-    % 当前关节角度
-    q1 = pi/6;   % 30度
-    q2 = pi/3;   % 60度
-    
-    % 期望的虚拟力
-    F_virtual = 50;  % 50N
-    theta_virtual = -pi/2;  % 向下的力
-    
-    % 计算所需关节力矩
-    [tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual);
-    
-    fprintf('髋关节所需力矩: %.3f N*m\n', tau_hip);
-    fprintf('膝关节所需力矩: %.3f N*m\n', tau_knee);
-    
-    % 显示雅可比矩阵
-    J = compute_jacobian(L1, L2, q1, q2);
-    fprintf('雅可比矩阵:\n');
-    disp(J);
-    
-    % 测试等效摆动杆转换
-    [angle_eq, length_eq] = serial_to_virtual_leg(L1, L2, q1, q2);
-    fprintf('等效摆动杆角度: %.3f rad (%.1f deg)\n', angle_eq, rad2deg(angle_eq));
-    fprintf('等效摆动杆长度: %.3f m\n', length_eq);
-    
-    % 验证逆变换
-    [q1_back, q2_back] = virtual_to_serial_leg(L1, L2, angle_eq, length_eq);
-    fprintf('验证 - 原始角度: q1=%.3f, q2=%.3f\n', q1, q2);
-    fprintf('验证 - 恢复角度: q1=%.3f, q2=%.3f\n', q1_back, q2_back);
-end
\ No newline at end of file
+fprintf('\n计算完成！\n');
\ No newline at end of file