zhujinzhao/auto_drive_misson_center_webots_simulation
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

tune lqr control algorithm

Browse Source
This commit is contained in:
liuhaoyan 2026-09-17 16:45:41 +08:00
parent 97dd2859e1
commit 4c0024aa9a
7 changed files with 103 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/auto_drive_misson_center/config/misson_center_params.yaml
View File

@ -26,7 +26,7 @@ path_follow_handler_node:
device_id: "2072868304711761920"
controller_type: "pure_pursuit" # "pure_pursuit", "lqr", "pid"
controller_type: "lqr" # "pure_pursuit", "lqr", "pid"
# logger
logger_logger_name: "path_follow"

57
src/auto_drive_misson_center/include/auto_drive_misson_center/lqr_control.hpp
View File

@ -22,17 +22,18 @@ public:
B.resize(2, 1); // 控制输入矩阵
Q.resize(2, 2); // 状态误差的加权矩阵
R.resize(1, 1); // 控制输入的加权矩阵
B << -1, 1; // 控制输入矩阵
//B << 0, 1; // 控制输入矩阵
// 设计LQR的代价函数矩阵
Q << 50, 0, 0, 15; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ
Q << 100, 0, 0, 10; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ
R << 10; // 控制输入的加权矩阵：惩罚角速度ω
R << 50; // 控制输入的加权矩阵：惩罚角速度ω
};
~LQR_Controller_C() {};
// LQR求解器函数
MatrixXd lqr(const MatrixXd &A, const MatrixXd &B, const MatrixXd &Q, const MatrixXd &R)
{
MatrixXd P = Q; // 初始化P为Q
MatrixXd P_prev = MatrixXd::Zero(A.rows(), A.cols());
for (int i = 0; i < 1000; ++i)
@ -46,12 +47,16 @@ public:
MatrixXd K = (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;
return K;
};
float update(const float alpha, const float linear_val, const float dis)
float update(const float alpha, const float linear_val, const float dis, const float kappa)
{
// std::cout << "LQR update!" << std::endl;
logger_->trace("LQR update!");
double dt = 0.01;
MatrixXd A(2, 2);
A << 0, linear_val, 0, 0;
A << 1.0, linear_val * dt,
0, 1.0;
B << 0.5 * linear_val * dt * dt,
dt;
Eigen::VectorXd x(2);
x << dis, alpha;
@ -61,8 +66,24 @@ public:
// << K << std::endl;
std::stringstream ss;
ss << K;
logger_->trace("LQR Gain Matrix K:\n{}", ss.str());
return -(K * x)(0, 0);
logger_->info("LQR Gain Matrix K:\n{}", ss.str());
double omega_feedback = -(K * x)(0, 0);
// Curvature feedforward
double omega_feedforward = linear_val * kappa;// feedforwward unused temporarily
double omega = omega_feedback;
logger_->info(
"LQR: dis={}, alpha={}, kappa={}, "
"omega_fb={}, omega_ff={}, omega={}",
dis,
alpha,
kappa,
omega_feedback,
omega_feedforward,
omega);
return omega;
};
/*void init_lqr(const MatrixXd B, const MatrixXd Q, const MatrixXd R)
{
@ -70,6 +91,26 @@ public:
this->Q = Q;
this->R = R;
}*/
float update(const float alpha, const float linear_val, const float dis)
{
// std::cout << "LQR update!" << std::endl;
logger_->trace("LQR update!");
double dt = 0.01;
MatrixXd A(2, 2);
A << 1.0, linear_val*dt,0, 1.0;
B << 0.5 * linear_val * dt * dt,dt;
Eigen::VectorXd x(2);
x << dis, alpha;
// 求解LQR增益矩阵K
MatrixXd K = lqr(A, B, Q, R);
// std::cout << "LQR Gain Matrix K:" << std::endl
// << K << std::endl;
std::stringstream ss;
ss << K;
logger_->info("LQR Gain Matrix K:\n{}", ss.str());
return -(K * x)(0, 0);
};
void init(const float max_linear_vel, const float max_angular_vel, const float kp, const float ki,
const float kd, const float look_ahead_dis, const float kv)
{
@ -82,7 +123,7 @@ public:
B.resize(2, 1); // 控制输入矩阵
Q.resize(2, 2); // 状态误差的加权矩阵
R.resize(1, 1); // 控制输入的加权矩阵
B << -1, 1; // 控制输入矩阵
B << 0, 1; // 控制输入矩阵
// 设计LQR的代价函数矩阵
Q << 50, 0, 0, 15; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ

2
src/auto_drive_misson_center/include/auto_drive_misson_center/path_follow_handler.hpp
View File

@ -45,7 +45,7 @@ public:
private:
MainState main_state_{MainState::IDLE};
bool is_resuming_{false};
std::string controller_type_{"pure_pursuit"};
std::string controller_type_{"lqr"};
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;

29
src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp
View File

@ -60,7 +60,36 @@ public:
}
return false;
}
void computePathCurvature()
{
if (path_points_.size() < 3) {
return;
}
for (size_t i = 1; i < path_points_.size() - 1; ++i)
{
const auto& p_prev = path_points_[i - 1];
const auto& p_curr = path_points_[i];
const auto& p_next = path_points_[i + 1];
double ds1 = std::hypot(p_curr.x - p_prev.x,p_curr.y - p_prev.y);
double ds2 = std::hypot(p_next.x - p_curr.x,p_next.y - p_curr.y);
double ds = 0.5 * (ds1 + ds2);
if (ds < 1e-6) {
path_points_[i].kappa= 0.0;
continue;
}
double dyaw = p_next.yaw - p_prev.yaw;
while (dyaw > M_PI)
dyaw -= 2.0 * M_PI;
while (dyaw < -M_PI)
dyaw += 2.0 * M_PI;
path_points_[i].kappa = dyaw / (ds1 + ds2);
logger_->info("path ind:{}, kappa:{}", i, path_points_[i].kappa);
}
// 首尾点处理
path_points_.front().kappa= path_points_[1].kappa;
path_points_.back().kappa= path_points_[path_points_.size() - 2].kappa;
}
// 运行一次控制计算
ControlOutput run(const PathPoint& current_pose) {
if (!is_initialized_) {

58
src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp
View File

@ -29,6 +29,8 @@ protected:
// 这里需要根据需求设置合适的参数值
float max_linear_vel = mission_speed_;
float max_angular_vel = mission_speed_ / turn_radius_; // 根据转弯半径估算最大角速度
float max_angular_vel_theory = max_allowed_speed_/wheel_dis_;
max_angular_vel = std::max(max_angular_vel, max_angular_vel_theory);
float kp = 1.76; // 需要根据实际情况调整
float ki = 0.0;
float kd = 0.8;
@ -38,18 +40,22 @@ protected:
controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv);
return true;
}
bool kappa_cal = false;
ControlOutput onRun(const PathPoint& current_pose) override {
// 从PathPoint计算所需参数
float alpha = 0.0; // 角度误差
float linear_val = 0.0; // 线速度
float dis = 0.0; // 横向误差
computeControlParams(current_pose, alpha, linear_val, dis);
logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}", alpha, linear_val, dis);
float kappa = 0.0; // 路徑曲率
if(!kappa_cal){
computePathCurvature();
kappa_cal = true;
}
computeControlParams(current_pose, alpha, linear_val, dis, kappa);
logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}, kappa:{}", alpha, linear_val, dis,kappa);
// 调用原有控制器的update方法
float angular_velocity = controller_->update(alpha, linear_val, dis);
float angular_velocity = controller_->update(alpha, linear_val, dis, kappa);
logger_->debug("PP output angular={}", angular_velocity);
// 判断是否完成
@ -66,7 +72,7 @@ private:
std::shared_ptr<LQR_Controller_C> controller_;
std::shared_ptr<spdlog::logger> logger_;
void computeControlParams(const PathPoint& current_pose, float& alpha, float& linear_val, float& dis) {
void computeControlParams(const PathPoint& current_pose, float& alpha, float& linear_val, float& dis, float& kappa) {
// 获取当前路径点的索引
int idx = current_point_index_;
if (idx >= static_cast<int>(path_points_.size()) - 1) {
@ -82,13 +88,15 @@ private:
dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
// 角度误差
alpha = static_cast<float>(path_yaw - current_pose.yaw);
alpha = static_cast<float>(current_pose.yaw - path_yaw);
// 归一化到 [-pi, pi]
while (alpha > M_PI) alpha -= 2 * M_PI;
while (alpha < -M_PI) alpha += 2 * M_PI;
// 线速度
linear_val = static_cast<float>(mission_speed_);
kappa = path_points_[idx].kappa;
}
};
@ -186,8 +194,11 @@ protected:
float dis = 0.0;
float lon_dis = 0.0;
float kappa = 0.0;
if(!kappa_cal)
if(!kappa_cal){
computePathCurvature();
kappa_cal = true;
}
// Pure Pursuit使用前视点，计算需要特殊处理
computeControlParamsWithLookAhead(current_pose, alpha, linear_val, dis, lon_dis, kappa);
logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}, lon_dis={},kappa={}", alpha, linear_val, dis,lon_dis,kappa);
@ -204,37 +215,6 @@ protected:
controller_->reset();
}
void computePathCurvature()
{
if (path_points_.size() < 3) {
return;
}
for (size_t i = 1; i < path_points_.size() - 1; ++i)
{
const auto& p_prev = path_points_[i - 1];
const auto& p_curr = path_points_[i];
const auto& p_next = path_points_[i + 1];
double ds1 = std::hypot(p_curr.x - p_prev.x,p_curr.y - p_prev.y);
double ds2 = std::hypot(p_next.x - p_curr.x,p_next.y - p_curr.y);
double ds = 0.5 * (ds1 + ds2);
if (ds < 1e-6) {
path_points_[i].kappa= 0.0;
continue;
}
double dyaw = p_next.yaw - p_prev.yaw;
while (dyaw > M_PI)
dyaw -= 2.0 * M_PI;
while (dyaw < -M_PI)
dyaw += 2.0 * M_PI;
path_points_[i].kappa = dyaw / (ds1 + ds2);
logger_->info("path ind:{}, kappa:{}", i, path_points_[i].kappa);
}
// 首尾点处理
path_points_.front().kappa= path_points_[1].kappa;
path_points_.back().kappa= path_points_[path_points_.size() - 2].kappa;
kappa_cal = true;
}
private:
std::shared_ptr<PurePursuit_Controller_C> controller_;
double adapter_look_ahead_dis_; //保存前视距离

2
src/auto_drive_misson_center/src/path_follow_handler.cpp
View File

@ -79,7 +79,7 @@ PathFollowHandler::PathFollowHandler(const std::string & node_name)
10
);
this->declare_parameter<std::string>("controller_type", "pure_pursuit");
this->declare_parameter<std::string>("controller_type", "lqr");
this->get_parameter("controller_type", controller_type_);
LOG_INFO("Controller type parameter: {}", controller_type_);

4
src/webots_simulation/src/PathVisualizer.cpp
View File

@ -533,7 +533,7 @@ void PathVisualizer::publishHistoryPathRviz(){
history_pose_pub_->publish(history_pose);
}
void PathVisualizer::pathIndCallback(const std_msgs::msg::Int32::SharedPtr msg){
path_track_index_ = msg->data + 2;
path_track_index_ = msg->data;
RCLCPP_INFO(
this->get_logger(),
"Received auto path index: %d",
@ -966,7 +966,7 @@ void PathVisualizer::publishOdomPath(){
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
std::string file_name;
file_name = "1701796317143199744.json";
file_name = "1701895373952102400.json";
// std::cout << "请输入路径文件路径: ";
// std::getline(std::cin, file_name);