diff --git a/src/auto_drive_misson_center/config/misson_center_params.yaml b/src/auto_drive_misson_center/config/misson_center_params.yaml index 786c0b1..c3456df 100644 --- a/src/auto_drive_misson_center/config/misson_center_params.yaml +++ b/src/auto_drive_misson_center/config/misson_center_params.yaml @@ -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" diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/lqr_control.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/lqr_control.hpp index 6b9c12a..f307449 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/lqr_control.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/lqr_control.hpp @@ -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和角度误差θ diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_follow_handler.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_follow_handler.hpp index d12d201..85562ea 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_follow_handler.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_follow_handler.hpp @@ -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::SharedPtr pose_sub_; rclcpp::Publisher::SharedPtr cmd_vel_pub_; diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp index 3858cea..892ba62 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp @@ -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_) { diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp index a240e27..0bec479 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp @@ -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 controller_; std::shared_ptr 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(path_points_.size()) - 1) { @@ -82,13 +88,15 @@ private: dis = static_cast(-dx * sin(path_yaw) + dy * cos(path_yaw)); // 角度误差 - alpha = static_cast(path_yaw - current_pose.yaw); + alpha = static_cast(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(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 controller_; double adapter_look_ahead_dis_; //保存前视距离 diff --git a/src/auto_drive_misson_center/src/path_follow_handler.cpp b/src/auto_drive_misson_center/src/path_follow_handler.cpp index 978f926..5a73209 100644 --- a/src/auto_drive_misson_center/src/path_follow_handler.cpp +++ b/src/auto_drive_misson_center/src/path_follow_handler.cpp @@ -79,7 +79,7 @@ PathFollowHandler::PathFollowHandler(const std::string & node_name) 10 ); - this->declare_parameter("controller_type", "pure_pursuit"); + this->declare_parameter("controller_type", "lqr"); this->get_parameter("controller_type", controller_type_); LOG_INFO("Controller type parameter: {}", controller_type_); diff --git a/src/webots_simulation/src/PathVisualizer.cpp b/src/webots_simulation/src/PathVisualizer.cpp index 31a3fee..3ffdac1 100644 --- a/src/webots_simulation/src/PathVisualizer.cpp +++ b/src/webots_simulation/src/PathVisualizer.cpp @@ -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);