164 lines
4.4 KiB
C
164 lines
4.4 KiB
C
/*
|
||
* 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
|