mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-24 03:27:45 +08:00
增加五连杆解算和速度估计
This commit is contained in:
kai
@@ -15,6 +15,8 @@
|
||||
#include "arm_math.h" // 需要用到较多三角函数
|
||||
#include "bsp_dwt.h"
|
||||
#include "bsp_log.h"
|
||||
#include "linkNleg.h"
|
||||
#include "speed_estimation.h"
|
||||
|
||||
|
||||
static uint32_t balance_dwt_cnt;
|
||||
|
||||
79
application/chassis/linkNleg.h
Normal file
79
application/chassis/linkNleg.h
Normal file
@@ -0,0 +1,79 @@
|
||||
#include "arm_math.h"
|
||||
#include "balance.h"
|
||||
#include "math.h"
|
||||
#include "user_lib.h"
|
||||
|
||||
/* 计算的T_hip和F_Leg映射为关节电机输出 */
|
||||
void VMCProject(LinkNPodParam *p)
|
||||
{
|
||||
float phi12 = p->phi1 - p->phi2;
|
||||
float phi34 = p->phi3 - p->phi4;
|
||||
float phi32 = p->phi3 - p->phi2;
|
||||
float phi53 = p->phi5 - p->phi3;
|
||||
float phi52 = p->phi5 - p->phi2;
|
||||
float F_m_L = p->F_leg * p->leg_len;
|
||||
p->T_back = (THIGH_LEN * msin(phi12) * (F_m_L * msin(phi53) + p->T_hip * mcos(phi53))) / (p->leg_len * msin(phi32));
|
||||
p->T_front = (CALF_LEN * msin(phi34) * (F_m_L * msin(phi52) + p->T_hip * mcos(phi52))) / (p->leg_len * msin(phi32));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 根据关节角度和角速度,计算单杆长度和角度以及变化率
|
||||
*
|
||||
* @note 右侧视图
|
||||
* ___x
|
||||
* | 1 _____ 4
|
||||
* |y / \
|
||||
* 2 \ / 3
|
||||
* \ /
|
||||
* \./
|
||||
* 5
|
||||
* @param p 5连杆和腿的参数
|
||||
*/
|
||||
void Link2Leg(LinkNPodParam *p, ChassisParam *chassis)
|
||||
{
|
||||
float xD, yD, xB, yB, BD, A0, B0, xC, yC;
|
||||
p->coord[4] = xD = JOINT_DISTANCE + THIGH_LEN * mcos(p->phi4);
|
||||
p->coord[5] = yD = THIGH_LEN * msin(p->phi4);
|
||||
p->coord[0] = xB = THIGH_LEN * mcos(p->phi1);
|
||||
p->coord[1] = yB = THIGH_LEN * msin(p->phi1);
|
||||
|
||||
BD = powf(xD - xB, 2) + powf(yD - yB, 2);
|
||||
A0 = 2 * CALF_LEN * (xD - xB);
|
||||
B0 = 2 * CALF_LEN * (yD - yB);
|
||||
p->phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
|
||||
p->coord[2] = xC = xB + CALF_LEN * mcos(p->phi2);
|
||||
p->coord[3] = yC = yB + CALF_LEN * msin(p->phi2);
|
||||
p->phi3 = atan2f(yC - yD, xC - xD); // 稍后用于计算VMC
|
||||
|
||||
// theta and LegLength solve
|
||||
p->phi5 = atan2f(yC, xC - JOINT_DISTANCE / 2);
|
||||
p->leg_len = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
|
||||
p->theta = p->phi5 - 0.5 * PI - chassis->pitch; // 确定方向
|
||||
p->height = p->leg_len * mcos(p->theta);
|
||||
|
||||
// 预测下一个时刻
|
||||
static float predict_dt = 0.0001f;
|
||||
float phi1_pred = p->phi1 + p->phi1_w * predict_dt; // 预测下一时刻的关节角度(利用关节角速度)
|
||||
float phi4_pred = p->phi4 + p->phi4_w * predict_dt;
|
||||
|
||||
// 重新计算腿长和腿角度
|
||||
xD = JOINT_DISTANCE + THIGH_LEN * mcos(phi4_pred);
|
||||
yD = THIGH_LEN * msin(phi4_pred);
|
||||
xB = 0 + THIGH_LEN * mcos(phi1_pred);
|
||||
yB = THIGH_LEN * msin(phi1_pred);
|
||||
|
||||
BD = powf(xD - xB, 2) + powf(yD - yB, 2);
|
||||
A0 = 2 * CALF_LEN * (xD - xB);
|
||||
B0 = 2 * CALF_LEN * (yD - yB);
|
||||
float phi2_pred = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
|
||||
xC = xB + CALF_LEN * mcos(phi2_pred);
|
||||
yC = yB + CALF_LEN * msin(phi2_pred);
|
||||
float phi5_pred = atan2f(yC, xC - JOINT_DISTANCE / 2);
|
||||
|
||||
// 差分计算腿长变化率和腿角速度
|
||||
p->phi2_w = (phi2_pred - p->phi2) / predict_dt; // 稍后用于修正轮速
|
||||
p->phi5_w = (phi5_pred - p->phi5) / predict_dt;
|
||||
p->legd = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->leg_len) / predict_dt;
|
||||
p->theta_w = ((phi5_pred - 0.5 * PI - (chassis->pitch + chassis->pitch_w) - p->theta) / predict_dt); // 可以不考虑机体? -predict_dt*chassis.pitch_w
|
||||
p->height_v = p->legd * mcos(p->theta) - p->leg_len * msin(p->theta) * p->theta_w;
|
||||
}
|
||||
100
application/chassis/speed_estimation.h
Normal file
100
application/chassis/speed_estimation.h
Normal file
@@ -0,0 +1,100 @@
|
||||
#include "balance.h"
|
||||
#include "user_lib.h"
|
||||
#include "ins_task.h"
|
||||
#include "general_def.h"
|
||||
|
||||
#define EST_FINAL_LPF 0.005f // 最终速度的低通滤波系数
|
||||
static KalmanFilter_t kf;
|
||||
|
||||
/**
|
||||
* @brief 底盘为右手系
|
||||
*
|
||||
* ^ y 左轮 右轮
|
||||
* | | | 前
|
||||
* |_____ > x |------|
|
||||
* z 轴从屏幕向外 | | 后
|
||||
*/
|
||||
|
||||
void SpeedEstInit()
|
||||
{
|
||||
// 使用kf同时估计速度和加速度
|
||||
// Kalman_Filter_Init(&kf, 2, 0, 2);
|
||||
// float F[4] = {1, 0.001, 0, 1};
|
||||
// float Q[4] = {VEL_PROCESS_NOISE, 0, 0, ACC_PROCESS_NOISE};
|
||||
// float R[4] = {VEL_MEASURE_NOISE, 0, 0, ACC_MEASURE_NOISE};
|
||||
// float P[4] = {100000, 0, 0, 100000};
|
||||
// float H[4] = {1, 0, 0, 1};
|
||||
// memcpy(kf.F_data, F, sizeof(F));
|
||||
// memcpy(kf.Q_data, Q, sizeof(Q));
|
||||
// memcpy(kf.R_data, R, sizeof(R));
|
||||
// memcpy(kf.P_data, P, sizeof(P));
|
||||
// memcpy(kf.H_data, H, sizeof(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;
|
||||
|
||||
// 直接使用轮速反馈,不进行速度融合
|
||||
// cp->vel = (lp->wheel_w + rp->wheel_w) * WHEEL_RADIUS / 2;
|
||||
// cp->dist = cp->dist + cp->vel * delta_t;
|
||||
|
||||
// 以轮子为基点,计算机体两侧髋关节处的速度
|
||||
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_m = (lp->body_v + rp->body_v) / 2; // 机体速度(平动)为两侧速度的平均值
|
||||
|
||||
// 扣除旋转导致的向心加速度和角加速度*R
|
||||
float *gyro = imu->Gyro, *dgyro = imu->dgyro;
|
||||
static float yaw_ddwrNwwr, yaw_p_ddwrNwwr, pitch_ddwrNwwr;
|
||||
static float macc_y, macc_z; // 补偿后的实际平动加速度,机体系前进方向和竖直方向
|
||||
yaw_ddwrNwwr = powf(gyro[Z], 2) * CENTER_IMU_W - dgyro[Z] * CENTER_IMU_L; // yaw旋转导致motion_acc[1]的额外加速度(机体前后方向)
|
||||
yaw_p_ddwrNwwr = powf(gyro[X], 2) * CENTER_IMU_W + dgyro[X] * CENTER_IMU_H; // pitch旋转导致motion_acc[1]的额外加速度(机体前后方向)
|
||||
pitch_ddwrNwwr = powf(gyro[X], 2) * CENTER_IMU_H - dgyro[X] * CENTER_IMU_W; // pitch旋转导致motion_acc[2]的额外加速度(机体竖直方向)
|
||||
macc_y = -imu->MotionAccel_b[Y] - yaw_ddwrNwwr - yaw_p_ddwrNwwr;
|
||||
macc_z = imu->MotionAccel_b[Z] - pitch_ddwrNwwr;
|
||||
|
||||
float pitch = imu->Pitch * DEGREE_2_RAD;
|
||||
cp->acc_last = cp->acc_m;
|
||||
cp->acc_m = macc_y * mcos(pitch) - macc_z * msin(pitch); // 绝对系下的平动加速度,即机体系下的加速度投影到绝对系
|
||||
|
||||
// for debug 对比修正前后的加速度
|
||||
static float ry, rz, rawaa;
|
||||
ry = -imu->MotionAccel_b[Y];
|
||||
rz = imu->MotionAccel_b[Z];
|
||||
rawaa = ry * mcos(pitch) - rz * msin(pitch);
|
||||
|
||||
// 使用kf同时估计加速度和速度,滤波更新
|
||||
// kf.MeasuredVector[0] = cp->vel_m;
|
||||
// kf.MeasuredVector[1] = cp->acc_m;
|
||||
// kf.F_data[1] = delta_t; // 更新F矩阵
|
||||
// Kalman_Filter_Update(&kf);
|
||||
// cp->vel = kf.xhat_data[0];
|
||||
// cp->acc = kf.xhat_data[1];
|
||||
|
||||
// 融合加速度计的数据和机体速度
|
||||
static float f, k, prior, measure, cov;
|
||||
f = (cp->acc_m + cp->acc_last) / 2; // 速度梯形积分
|
||||
prior = cp->vel + f * delta_t; // x' = Fx,先验估计
|
||||
cp->vel_predict = prior;
|
||||
measure = cp->vel_m; // 测量值
|
||||
cov = cp->vel_cov + VEL_PROCESS_NOISE * delta_t; // P' = P + Q ,先验协方差
|
||||
cp->vel_cov = cov;
|
||||
k = cov / (cov + VEL_MEASURE_NOISE); // K = P'/(P'+R),卡尔曼增益
|
||||
cp->vel = prior + k * (measure - prior); // x^ = x'+K(z-x'),后验估计
|
||||
cp->vel_cov *= (1 - k); // P^ = (1-K)P',后验协方差
|
||||
VAL_LIMIT(cp->vel_cov, 0.01, 100); // 协方差限幅
|
||||
cp->dist = cp->dist + cp->vel * delta_t;
|
||||
}
|
||||
Reference in New Issue
Block a user