robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add Kalman filter component for chassis

Browse Source 
Added kalman_filter.c to the project structure for chassis state estimation. Updated Keil MDK project files (.uvprojx, .uvoptx) to include the new component and modified the debug watch window to monitor chassis variables.
This commit is contained in:
Robofish 2026-02-09 20:21:32 +08:00
parent ec22b87dfa
commit 5b89b3c4f4
8 changed files with 8473 additions and 8007 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -185,6 +185,11 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>RF</ItemText> <ItemText>RF</ItemText>
</Ww> </Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>chassis</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -1192,6 +1197,18 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>76</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\component\kalman_filter.c</PathWithFileName>
<FilenameWithoutPath>kalman_filter.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group> </Group>
<Group> <Group>
@ -1202,7 +1219,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>76</FileNumber> <FileNumber>77</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1214,7 +1231,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>77</FileNumber> <FileNumber>78</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1226,7 +1243,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>78</FileNumber> <FileNumber>79</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1238,7 +1255,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>79</FileNumber> <FileNumber>80</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1250,7 +1267,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>80</FileNumber> <FileNumber>81</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1262,7 +1279,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>81</FileNumber> <FileNumber>82</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1274,7 +1291,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>82</FileNumber> <FileNumber>83</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1286,7 +1303,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>83</FileNumber> <FileNumber>84</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1298,7 +1315,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>84</FileNumber> <FileNumber>85</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1310,7 +1327,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>85</FileNumber> <FileNumber>86</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1322,7 +1339,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>86</FileNumber> <FileNumber>87</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1334,7 +1351,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>87</FileNumber> <FileNumber>88</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1354,7 +1371,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>88</FileNumber> <FileNumber>89</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1366,7 +1383,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>89</FileNumber> <FileNumber>90</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1378,7 +1395,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>90</FileNumber> <FileNumber>91</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1390,7 +1407,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>91</FileNumber> <FileNumber>92</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1410,7 +1427,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>92</FileNumber> <FileNumber>93</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1422,7 +1439,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>93</FileNumber> <FileNumber>94</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1434,7 +1451,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>94</FileNumber> <FileNumber>95</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1446,7 +1463,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>95</FileNumber> <FileNumber>96</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1458,7 +1475,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>96</FileNumber> <FileNumber>97</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1470,7 +1487,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>97</FileNumber> <FileNumber>98</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1482,7 +1499,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>98</FileNumber> <FileNumber>99</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1494,7 +1511,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>99</FileNumber> <FileNumber>100</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1506,7 +1523,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>100</FileNumber> <FileNumber>101</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1518,7 +1535,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>101</FileNumber> <FileNumber>102</FileNumber>
<FileType>5</FileType> <FileType>5</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1530,7 +1547,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>102</FileNumber> <FileNumber>103</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1542,7 +1559,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>103</FileNumber> <FileNumber>104</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1554,7 +1571,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>104</FileNumber> <FileNumber>105</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1566,6 +1583,26 @@
</File> </File>
</Group> </Group>
<Group>
<GroupName>ARM_DSP</GroupName>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>106</FileNumber>
<FileType>4</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\Drivers\CMSIS\DSP\Lib\ARM\arm_cortexM7lfdp_math.lib</PathWithFileName>
<FilenameWithoutPath>arm_cortexM7lfdp_math.lib</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group> <Group>
<GroupName>::CMSIS</GroupName> <GroupName>::CMSIS</GroupName>
<tvExp>0</tvExp> <tvExp>0</tvExp>

17
MDK-ARM/CtrBoard-H7_ALL.uvprojx
View File

@ -338,7 +338,7 @@
<v6Rtti>0</v6Rtti> <v6Rtti>0</v6Rtti>
<VariousControls> <VariousControls>
<MiscControls></MiscControls> <MiscControls></MiscControls>
<Define>USE_PWR_LDO_SUPPLY,USE_HAL_DRIVER,STM32H723xx</Define> <Define>USE_PWR_LDO_SUPPLY,USE_HAL_DRIVER,STM32H723xx,FPU_PRESENT=1,FPU_USED=1,ARM_MATH_CM7</Define>
<Undefine></Undefine> <Undefine></Undefine>
<IncludePath>../Core/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include;../User;../Middlewares/Third_Party/ARM/DSP;../Drivers/CMSIS/DSP/Include;../Middlewares/ST/ARM/DSP/Inc</IncludePath> <IncludePath>../Core/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include;../User;../Middlewares/Third_Party/ARM/DSP;../Drivers/CMSIS/DSP/Include;../Middlewares/ST/ARM/DSP/Inc</IncludePath>
</VariousControls> </VariousControls>
@ -840,6 +840,11 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\User\component\limiter.c</FilePath> <FilePath>..\User\component\limiter.c</FilePath>
</File> </File>
<File>
<FileName>kalman_filter.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\kalman_filter.c</FilePath>
</File>
</Files> </Files>
</Group> </Group>
<Group> <Group>
@ -1002,6 +1007,16 @@
</File> </File>
</Files> </Files>
</Group> </Group>
<Group>
<GroupName>ARM_DSP</GroupName>
<Files>
<File>
<FileName>arm_cortexM7lfdp_math.lib</FileName>
<FileType>4</FileType>
<FilePath>..\Drivers\CMSIS\DSP\Lib\ARM\arm_cortexM7lfdp_math.lib</FilePath>
</File>
</Files>
</Group>
<Group> <Group>
<GroupName>::CMSIS</GroupName> <GroupName>::CMSIS</GroupName>
</Group> </Group>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

16179
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

148
User/module/balance_chassis.c
View File

@ -7,6 +7,7 @@
#include "bsp/can.h" #include "bsp/can.h"
#include "bsp/time.h" #include "bsp/time.h"
#include "component/filter.h" #include "component/filter.h"
#include "component/kalman_filter.h"
#include "component/user_math.h" #include "component/user_math.h"
#include "device/motor_lk.h" #include "device/motor_lk.h"
#include "device/motor_lz.h" #include "device/motor_lz.h"
@ -23,7 +24,7 @@
#define BASE_LEG_LENGTH 0.16f /* 基础腿长 (m) */ #define BASE_LEG_LENGTH 0.16f /* 基础腿长 (m) */
#define LEG_LENGTH_RANGE 0.16f /* 腿长调节范围 (m) */ #define LEG_LENGTH_RANGE 0.16f /* 腿长调节范围 (m) */
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */ #define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
#define MAX_LEG_LENGTH 0.34f /* 最大腿长 (m) */ #define MAX_LEG_LENGTH 0.33f /* 最大腿长 (m) */
#define NON_CONTACT_THETA 0.0f /* 离地时的摆角目标 (rad) */ #define NON_CONTACT_THETA 0.0f /* 离地时的摆角目标 (rad) */
#define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */ #define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */
#define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */ #define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */
@ -100,6 +101,9 @@ static void Chassis_ResetControllers(Chassis_t *c) {
LowPassFilter2p_Reset(&c->filter.wheel_out[i], 0.0f); LowPassFilter2p_Reset(&c->filter.wheel_out[i], 0.0f);
} }
/* 重置卡尔曼滤波器 */
KF_Reset(&c->kf_state_est);
/* 清空机体状态 */ /* 清空机体状态 */
c->chassis_state.position_x = 0.0f; c->chassis_state.position_x = 0.0f;
c->chassis_state.velocity_x = 0.0f; c->chassis_state.velocity_x = 0.0f;
@ -168,46 +172,87 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
/* /*
* 轮子角速度补偿算法 * 轮子角速度补偿算法
* 补偿关节运动对轮子速度的影响，并消除机体俯仰运动的干扰 * w = w_wheel - pitch_gyro + joint_speed
* 公式来源：五连杆机构运动学分析
*/ */
float left_w_wheel_compensated = left_wheel_speed + left_joint_speed - pitch_gyro; float left_w_compensated = left_wheel_speed + left_joint_speed - pitch_gyro;
float right_w_wheel_compensated = right_wheel_speed + right_joint_speed - pitch_gyro; float right_w_compensated = right_wheel_speed + right_joint_speed - pitch_gyro;
/* /*
* 机体速度计算算法 * 机体速度计算 (参考港大开源)
* 综合考虑： * v = w*R + L0*dtheta*cos(theta) + dL0*sin(theta)
* 1. 轮子接触点线速度：omega_wheel * r_wheel
* 2. 摆杆运动贡献：L0 * d_theta (摆杆角速度在x方向的分量)
* 3. 腿长变化贡献：d_L0 * sin(theta) (腿长变化在x方向的分量)
*/ */
float left_v_body = left_w_wheel_compensated * WHEEL_RADIUS + float left_v_body = left_w_compensated * WHEEL_RADIUS +
c->vmc_[0].leg.L0 * c->vmc_[0].leg.d_theta + c->vmc_[0].leg.L0 * c->vmc_[0].leg.d_theta * cosf(c->vmc_[0].leg.theta) +
c->vmc_[0].leg.d_L0 * sinf(c->vmc_[0].leg.theta); c->vmc_[0].leg.d_L0 * sinf(c->vmc_[0].leg.theta);
float right_v_body = right_w_wheel_compensated * WHEEL_RADIUS + float right_v_body = right_w_compensated * WHEEL_RADIUS +
c->vmc_[1].leg.L0 * c->vmc_[1].leg.d_theta + c->vmc_[1].leg.L0 * c->vmc_[1].leg.d_theta * cosf(c->vmc_[1].leg.theta) +
c->vmc_[1].leg.d_L0 * sinf(c->vmc_[1].leg.theta); c->vmc_[1].leg.d_L0 * sinf(c->vmc_[1].leg.theta);
/* 保存历史速度 */ /* 保存历史速度 */
c->chassis_state.last_velocity_x = c->chassis_state.velocity_x; c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
/* 计算平均速度 */ /* 计算平均轮子速度 */
float vel_measured = (left_v_body + right_v_body) / 2.0f; float vel_measured = (left_v_body + right_v_body) / 2.0f;
/* /*
* 接地状态检测与速度修正 * 获取IMU前向加速度（世界坐标系x方向）
* 当双腿都离地时，基于轮子的速度估计不可靠，将速度设为0 * 使用旋转矩阵将机体加速度(已去重力)转到世界系
* axE = cos(pitch)*ax_body + sin(roll)*sin(pitch)*ay_body + cos(roll)*sin(pitch)*az_body
*/ */
float pitch = c->feedback.imu.euler.pit;
float roll = c->feedback.imu.euler.rol;
float ax_body = c->feedback.imu.accl.x;
float ay_body = c->feedback.imu.accl.y;
float az_body = c->feedback.imu.accl.z;
/* 去除重力分量（机体系下） */
float gravity_bx = -9.81f * sinf(pitch);
float gravity_by = 9.81f * sinf(roll) * cosf(pitch);
float gravity_bz = 9.81f * cosf(roll) * cosf(pitch);
float motion_ax = ax_body - gravity_bx;
float motion_ay = ay_body - gravity_by;
float motion_az = az_body - gravity_bz;
/* 旋转到世界坐标系x方向 */
float accel_world_x = cosf(pitch) * motion_ax +
sinf(roll) * sinf(pitch) * motion_ay +
cosf(roll) * sinf(pitch) * motion_az;
/* ==================== 卡尔曼滤波器融合 ==================== */
/* 动态更新状态转移矩阵 F（根据实际dt）*/
float dt = c->dt;
if (dt <= 0.0f || dt > 0.1f) dt = 0.001f;
/* F = [1, dt; 0, 1] */
c->kf_state_est.F_data[0] = 1.0f;
c->kf_state_est.F_data[1] = dt;
c->kf_state_est.F_data[2] = 0.0f;
c->kf_state_est.F_data[3] = 1.0f;
/* 量测向量: z = [v_wheel, a_imu]
* 注意：必须写入 measured_vector，KF_Measure 会将其拷贝到 z_data
* 直接写 z_data 会被 KF_Measure 用 measured_vector(全零) 覆盖！ */
c->kf_state_est.measured_vector[0] = vel_measured;
c->kf_state_est.measured_vector[1] = accel_world_x;
/* 动态调整R矩阵：离地时降低轮速信任度 */
if (c->vmc_[0].leg.Fn < 20.0f && c->vmc_[1].leg.Fn < 20.0f) { if (c->vmc_[0].leg.Fn < 20.0f && c->vmc_[1].leg.Fn < 20.0f) {
vel_measured = 0.0f; c->kf_state_est.R_data[0] = 100000.0f; /* 离地：不信任轮速 */
} else {
c->kf_state_est.R_data[0] = 100.0f; /* 接地：信任轮速 */
} }
/* R[1][1] = 1000000.0f 保持不变，始终不信任加速度量测 */
/* 使用二阶低通滤波器平滑速度估计 */ /* 执行卡尔曼滤波 */
c->chassis_state.velocity_x = LowPassFilter2p_Apply(&c->filter.velocity_est, vel_measured); float *kf_result = KF_Update(&c->kf_state_est);
/* 位置积分更新 */ /* KF输出: [velocity, acceleration] */
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt; if (kf_result != NULL) {
c->chassis_state.velocity_x = kf_result[0];
/* 位移 = 滤波后的速度积分（不让加速度误差被二次积分放大） */
c->chassis_state.position_x += c->chassis_state.velocity_x * dt;
}
} }
/** /**
@ -386,6 +431,62 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
} }
LowPassFilter2p_Init(&c->filter.velocity_est, target_freq, 30.0f); /* 速度估计滤波截止频率30Hz */ LowPassFilter2p_Init(&c->filter.velocity_est, target_freq, 30.0f); /* 速度估计滤波截止频率30Hz */
/*
* 初始化卡尔曼滤波器（参考港大开源方案）
* 状态向量 x = [v, a] (2维：速度和加速度)
* 量测向量 z = [v, a] (2维：轮速和IMU加速度)
* 位移在KF外部用滤波后的速度积分，避免加速度误差被二次积分放大
*/
KF_Init(&c->kf_state_est, 2, 0, 2);
float dt_init = 1.0f / target_freq;
/* 初始协方差矩阵 P (2x2) */
static float P_Init[4] = {
1.0f, 0.0f,
0.0f, 1.0f,
};
memcpy(c->kf_state_est.P_data, P_Init, sizeof(P_Init));
/* 状态转移矩阵 F (2x2): v(k) = v(k-1) + a*dt, a(k) = a(k-1) */
float F_Init[4] = {
1.0f, dt_init,
0.0f, 1.0f,
};
memcpy(c->kf_state_est.F_data, F_Init, sizeof(F_Init));
/* 过程噪声协方差矩阵 Q (2x2) */
static float Q_Init[4] = {
0.1f, 0.0f,
0.0f, 0.1f,
};
memcpy(c->kf_state_est.Q_data, Q_Init, sizeof(Q_Init));
/* 状态最小方差（防止过度收敛） */
static float state_min_var[2] = {0.005f, 0.1f};
memcpy(c->kf_state_est.state_min_variance, state_min_var, sizeof(state_min_var));
/* 关闭自动量测调整（手动管理H/R） */
c->kf_state_est.use_auto_adjustment = 0;
/* 量测矩阵 H (2x2): z = H*x, 直接观测速度和加速度 */
static float H_Init[4] = {
1.0f, 0.0f,
0.0f, 1.0f,
};
memcpy(c->kf_state_est.H_data, H_Init, sizeof(H_Init));
/*
* 量测噪声协方差矩阵 R (2x2)
* 关键：加速度噪声设极大(1000000)，几乎不信任加速度量测
* KF主要靠轮速量测修正，加速度仅在预测环节(F矩阵)起辅助作用
*/
static float R_Init[4] = {
100.0f, 0.0f,
0.0f, 1000000.0f,
};
memcpy(c->kf_state_est.R_data, R_Init, sizeof(R_Init));
Chassis_MotorEnable(c); Chassis_MotorEnable(c);
/* 初始化状态变量 */ /* 初始化状态变量 */
@ -427,6 +528,9 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
c->feedback.joint[i].rotor_abs_angle -= M_2PI; c->feedback.joint[i].rotor_abs_angle -= M_2PI;
} }
c->feedback.joint[i].rotor_abs_angle = -c->feedback.joint[i].rotor_abs_angle; c->feedback.joint[i].rotor_abs_angle = -c->feedback.joint[i].rotor_abs_angle;
/* 应用零点偏移 */
c->feedback.joint[i].rotor_abs_angle -= c->param->joint_zero[i];
} }
/* 更新轮子电机反馈 */ /* 更新轮子电机反馈 */

6
User/module/balance_chassis.h
View File

@ -20,6 +20,7 @@ extern "C" {
#include "component/lqr.h" #include "component/lqr.h"
#include "component/ahrs.h" #include "component/ahrs.h"
#include "component/filter.h" #include "component/filter.h"
#include "component/kalman_filter.h"
#include "component/pid.h" #include "component/pid.h"
#include "device/motor.h" #include "device/motor.h"
#include "device/motor_lk.h" #include "device/motor_lk.h"
@ -105,6 +106,8 @@ typedef struct {
float retract_force; /* 收腿前馈力 (N)，负值表示向上收 */ float retract_force; /* 收腿前馈力 (N)，负值表示向上收 */
} jump_params; } jump_params;
float joint_zero[4]; /* 关节电机零点偏移位置 */
float mech_zero_yaw; /* 机械零点 */ float mech_zero_yaw; /* 机械零点 */
float theta; float theta;
@ -189,6 +192,9 @@ typedef struct {
LowPassFilter2p_t velocity_est; /* 速度估计滤波器 */ LowPassFilter2p_t velocity_est; /* 速度估计滤波器 */
} filter; } filter;
/* 卡尔曼滤波器：位移速度融合IMU */
KF_t kf_state_est;
} Chassis_t; } Chassis_t;
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */

10
User/module/config.c
View File

@ -203,7 +203,7 @@ Config_RobotParam_t robot_config = {
.leg_length={ .leg_length={
.k = 40.0f, .k = 40.0f,
.p = 20.0f, .p = 30.0f,
.i = 0.01f, .i = 0.01f,
.d = 2.0f, .d = 2.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
@ -298,7 +298,7 @@ Config_RobotParam_t robot_config = {
}, },
.mech_zero_yaw = 1.78040409f, /* 机械零点 */ .mech_zero_yaw = 1.78040409f, /* 机械零点 */
.joint_zero = {0.0f, 0.0f, 0.0f, 0.0f}, /* 关节电机零点偏移位置 */
.vmc_param = { .vmc_param = {
{ // 左腿 { // 左腿
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m) .leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
@ -316,7 +316,7 @@ Config_RobotParam_t robot_config = {
} }
}, },
.lqr_gains = { .lqr_gains = {
.k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta .k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta
.k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f }, // d_theta .k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f }, // d_theta
.k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f }, // x .k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f }, // x
@ -329,10 +329,10 @@ Config_RobotParam_t robot_config = {
.k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f }, // d_x .k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f }, // d_x
.k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi .k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi
.k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f }, // d_phi .k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f }, // d_phi
}, },
.jump_params = { .jump_params = {
.crouch_time_ms = 300, .crouch_time_ms = 100,
.launch_time_ms = 120, .launch_time_ms = 120,
.retract_time_ms = 80, .retract_time_ms = 80,
.land_time_ms = 300, .land_time_ms = 300,

1
User/task/ctrl_chassis.c
View File

@ -100,6 +100,7 @@ void Task_ctrl_chassis(void *argument) {
osMessageQueueReset(task_runtime.msgq.chassis.vofa); // 重置消息队列，防止阻塞 osMessageQueueReset(task_runtime.msgq.chassis.vofa); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.vofa, &chassis, 0, 0); osMessageQueuePut(task_runtime.msgq.chassis.vofa, &chassis, 0, 0);
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }