mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-24 03:27:45 +08:00
28 lines
1.0 KiB
C
28 lines
1.0 KiB
C
|
#include "balance.h"
|
||
|
|
#include "user_lib.h"
|
||
|
|
#include "ins_task.h"
|
||
|
|
#include "general_def.h"
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief 使用卡尔曼滤波估计底盘速度
|
||
|
|
* @todo 增加w和dw的滤波,当w和dw均小于一定值时,不考虑dw导致的角加速度
|
||
|
|
*
|
||
|
|
* @param lp 左侧腿
|
||
|
|
* @param rp 右侧腿
|
||
|
|
* @param cp 底盘
|
||
|
|
* @param imu imu数据
|
||
|
|
* @param delta_t 更新间隔
|
||
|
|
*/
|
||
|
|
void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS_t *imu, float delta_t)
|
||
|
|
{
|
||
|
|
// 修正轮速和距离
|
||
|
|
lp->wheel_w = lp->w_ecd + lp->phi2_w - cp->pitch_w; // 减去和定子固连的phi2_w
|
||
|
|
rp->wheel_w = rp->w_ecd + rp->phi2_w - cp->pitch_w;
|
||
|
|
|
||
|
|
// 以轮子为基点,计算机体两侧髋关节处的速度
|
||
|
|
lp->body_v = lp->wheel_w * WHEEL_RADIUS + lp->leg_len * lp->theta_w + lp->legd * msin(lp->theta);
|
||
|
|
rp->body_v = rp->wheel_w * WHEEL_RADIUS + rp->leg_len * rp->theta_w + rp->legd * msin(rp->theta);
|
||
|
|
cp->vel = cp->vel_m = (lp->body_v + rp->body_v) / 2; // 机体速度(平动)为两侧速度的平均值
|
||
|
|
cp->dist = cp->dist + cp->vel * delta_t;
|
||
|
|
}
|