230 lines
7.2 KiB
C
230 lines
7.2 KiB
C
/*
|
|
* LQR线性二次型最优控制器简化实现
|
|
*
|
|
* 本文件实现了轮腿机器人的LQR (Linear Quadratic Regulator) 控制算法
|
|
* 主要功能包括:
|
|
* 1. 状态反馈控制
|
|
* 2. 增益矩阵K计算控制输出
|
|
* 3. 控制输出限幅
|
|
*
|
|
* 系统模型:
|
|
* u = -K*(x - x_ref) (状态反馈)
|
|
*/
|
|
|
|
#include "lqr.h"
|
|
#include <string.h>
|
|
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
|
|
#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 -------------------------------------------------------- */
|
|
/* Private function prototypes ---------------------------------------------- */
|
|
|
|
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);
|
|
|
|
/* 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) {
|
|
return -1;
|
|
}
|
|
|
|
// 设置力矩限制
|
|
lqr->max_wheel_torque = max_wheel_torque;
|
|
lqr->max_joint_torque = max_joint_torque;
|
|
|
|
// 重置状态
|
|
LQR_Reset(lqr);
|
|
|
|
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) {
|
|
return -1;
|
|
}
|
|
|
|
// 复制增益矩阵
|
|
memcpy(&lqr->K, K, sizeof(LQR_GainMatrix_t));
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief 更新机器人状态
|
|
*/
|
|
int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
|
|
if (lqr == NULL || !lqr->initialized || 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);
|
|
|
|
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) {
|
|
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);
|
|
|
|
// 计算控制输出: u = -K * (x - x_ref)
|
|
float control_vector[4];
|
|
LQR_MatrixMultiply(lqr->K.K, state_error, control_vector);
|
|
|
|
// 应用负反馈并限幅
|
|
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);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief 获取控制输出
|
|
*/
|
|
int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control) {
|
|
if (lqr == NULL || !lqr->initialized || control == NULL) {
|
|
return -1;
|
|
}
|
|
|
|
*control = lqr->control;
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief 重置LQR控制器状态
|
|
*/
|
|
void 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;
|
|
|
|
// 角度归一化
|
|
LQR_ANGLE_NORMALIZE(state->phi);
|
|
LQR_ANGLE_NORMALIZE(state->theta_ll);
|
|
LQR_ANGLE_NORMALIZE(state->theta_lr);
|
|
LQR_ANGLE_NORMALIZE(state->theta_b);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* 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];
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief 计算状态误差(考虑角度周期性)
|
|
*/
|
|
static float LQR_ComputeStateError(float current, float reference) {
|
|
float error = current - reference;
|
|
|
|
// 对于角度状态，需要考虑周期性
|
|
// 这里假设大部分状态都是角度，如果需要区分可以添加参数
|
|
while (error > M_PI) error -= 2 * M_PI;
|
|
while (error < -M_PI) error += 2 * M_PI;
|
|
|
|
return error;
|
|
}
|