From 8d135e741d6560e2a2f723ebac7fc12e2c5db7ea Mon Sep 17 00:00:00 2001 From: liuhaoyan Date: Mon, 14 Sep 2026 18:13:00 +0800 Subject: [PATCH] Optimize control process 1.fix pp algori 2.fix point interpolate yaw --- .../path_tracking_controller.hpp | 2 +- .../path_tracking_controller_adapter.hpp | 95 +++++++++++++++++-- .../pure_pursuit_control.hpp | 17 +++- .../src/mission_center_node.cpp | 25 ++++- .../src/path_follow_handler.cpp | 4 +- src/webots_simulation/src/PathVisualizer.cpp | 2 +- 6 files changed, 125 insertions(+), 20 deletions(-) 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 94df307..3858cea 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 @@ -15,7 +15,7 @@ struct PathPoint { double x; // 经度或UTM X坐标 double y; // 纬度或UTM Y坐标 double yaw; // 朝向角度（弧度） - + double kappa; // 曲率 PathPoint(double x_val = 0.0, double y_val = 0.0, double yaw_val = 0.0) : x(x_val), y(y_val), yaw(yaw_val) {} }; 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 136feb9..a240e27 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 @@ -32,8 +32,8 @@ protected: float kp = 1.76; // 需要根据实际情况调整 float ki = 0.0; float kd = 0.8; - float look_ahead_dis = 0.4; // 需要根据实际情况调整 - float kv = 1.0; // 需要根据实际情况调整 + float look_ahead_dis = 0.6; // 需要根据实际情况调整 + float kv = 0.0; // 需要根据实际情况调整 controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv); return true; @@ -65,7 +65,7 @@ protected: private: std::shared_ptr controller_; std::shared_ptr logger_; - + void computeControlParams(const PathPoint& current_pose, float& alpha, float& linear_val, float& dis) { // 获取当前路径点的索引 int idx = current_point_index_; @@ -179,17 +179,21 @@ protected: controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, adapter_look_ahead_dis_, kv); return true; } - + bool kappa_cal = false; ControlOutput onRun(const PathPoint& current_pose) override { float alpha = 0.0; float linear_val = 0.0; float dis = 0.0; - + float lon_dis = 0.0; + float kappa = 0.0; + if(!kappa_cal) + computePathCurvature(); // Pure Pursuit使用前视点，计算需要特殊处理 - computeControlParamsWithLookAhead(current_pose, alpha, linear_val, dis); - logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}", alpha, linear_val, dis); + 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); - float angular_velocity = controller_->update(alpha, linear_val, dis); + float angular_velocity = controller_->update(alpha, linear_val, dis, lon_dis, kappa); + //float angular_velocity = controller_->update(alpha, linear_val, dis); logger_->debug("PP output angular={}", angular_velocity); bool completed = isPathCompleted(current_pose); @@ -200,13 +204,44 @@ 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_; //保存前视距离 std::shared_ptr logger_; void computeControlParamsWithLookAhead(const PathPoint& current_pose, - float& alpha, float& linear_val, float& dis) { + float& alpha, float& linear_val, float& dis, float& lon_dis, float& kappa) { // 查找前视点 PathPoint lookahead_point; if (!findLookAheadPoint(current_pose, lookahead_point)) { @@ -239,7 +274,46 @@ private: double dx_path = current_pose.x - path_points_[idx].x; double dy_path = current_pose.y - path_points_[idx].y; double path_yaw = path_points_[idx].yaw; - dis = static_cast(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw)); + //dis = static_cast(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw)); + dis = static_cast(-dx * sin(path_yaw) + dy * cos(path_yaw)); + lon_dis = static_cast(sqrt(dx * dx + dy * dy)); + kappa = static_cast(path_points_[idx].kappa); + logger_->info( + "PurePursuit parameters:\n" + " current_x:{}, current_y:{}, current_yaw:{}\n" + " current_path_x:{}, current_path_y:{}, current_path_yaw:{}\n" + " lookahead_x:{}, lookahead_y:{}, lookahead_yaw:{}, lookahead_kappa:{}\n" + " dx:{}, dy:{}, target_yaw:{}, alpha:{}\n" + " linear_val:{}, dis:{}, lon_dis:{}\n" + " current_point_index:{}, idx:{}, path_size:{}", + + current_pose.x, + current_pose.y, + current_pose.yaw, + + path_points_[current_point_index_].x, + path_points_[current_point_index_].y, + path_points_[current_point_index_].yaw, + + lookahead_point.x, + lookahead_point.y, + lookahead_point.yaw, + lookahead_point.kappa, + + dx, + dy, + target_yaw, + alpha, + + linear_val, + dis, + lon_dis, + + current_point_index_, + idx, + path_points_.size() + ); + } bool findLookAheadPoint(const PathPoint& current_pose, PathPoint& lookahead_point) { @@ -269,6 +343,7 @@ private: lookahead_point.x = path_points_[i].x + ratio * dx; lookahead_point.y = path_points_[i].y + ratio * dy; lookahead_point.yaw = path_points_[i].yaw; + lookahead_point.kappa = path_points_[i].kappa; return true; } accumulated_dist += segment_length; diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp index 8ca8817..e3dc44c 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp @@ -27,13 +27,24 @@ public: lookAheadDis = look_ahead_dis; k_v = kv; } - float update(const float alpha, const float linear_val, const float dis) { // std::cout<<"PurePursuit update!"<trace("PurePursuit update!"); - float w_val = (2 * fabs(linear_val) * sin(alpha) / (k_v * fabs(linear_val) + lookAheadDis)); - //float w_val = (2 * fabs(linear_val) * sin(alpha) / (lookAheadDis)); + float w_val = (2 * fabs(linear_val) * sin(alpha) / (lookAheadDis)); + w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val; + // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl; + logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val); + return w_val; + }; + float update(const float alpha, const float linear_val, const float dis, const float lon_dis,const float kappa) + { + // std::cout<<"PurePursuit update!"<trace("PurePursuit update!"); + logger_->info("PurePursuit process! linear_val:{}, dis:{}, lon_dis:{}, kappa:{}", linear_val, dis, lon_dis, kappa); + float w_val = (2 * fabs(linear_val) * sin(alpha) / (lon_dis)) + dis; + + //float w_val = linear_val*kappa; w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val; // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl; logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val); diff --git a/src/auto_drive_misson_center/src/mission_center_node.cpp b/src/auto_drive_misson_center/src/mission_center_node.cpp index 092866b..266490b 100644 --- a/src/auto_drive_misson_center/src/mission_center_node.cpp +++ b/src/auto_drive_misson_center/src/mission_center_node.cpp @@ -920,9 +920,28 @@ std::vector> MissionCenterNode::interpolatePoints( double ratio = static_cast(j) / num_segments; double x = current[0] + dx * ratio; double y = current[1] + dy * ratio; - result.push_back({x, y, current[2]}); - inserted_points++; - } + + // yaw 插值 + double yaw_diff = next[2] - current[2]; + // normalize yaw_diff 到 [-pi, pi] + while (yaw_diff > M_PI) { + yaw_diff -= 2.0 * M_PI; + } + while (yaw_diff < -M_PI) { + yaw_diff += 2.0 * M_PI; + } + double yaw =current[2] + yaw_diff * ratio; + // normalize yaw 到 [-pi, pi] + while (yaw > M_PI) { + yaw -= 2.0 * M_PI; + } + while (yaw < -M_PI) { + yaw += 2.0 * M_PI; + } + + result.push_back({x, y, yaw}); + inserted_points++; + } interpolation_map_.push_back({i + 1, inserted_points}); } 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 0f2c81d..978f926 100644 --- a/src/auto_drive_misson_center/src/path_follow_handler.cpp +++ b/src/auto_drive_misson_center/src/path_follow_handler.cpp @@ -417,7 +417,7 @@ void PathFollowHandler::processRunningState() auto path_ind_msg = std_msgs::msg::Int32(); path_ind_msg.data = path_controller_->getCurrentPointIndex() + path_ind_start_; path_ind_pub_->publish(path_ind_msg); - + LOG_INFO("Published final path index 1: {}", path_ind_msg.data); saveCurrentStateToRedis(); LOG_DEBUG("path controller, current pose point:x:{}, y:{}, yaw:{}; twist_msg pub:linear x:{}, angular z:{}, run is completed:{}, Current Point Index:{}, Path Point Count:{}", @@ -444,7 +444,7 @@ void PathFollowHandler::processRunningState() } final_ind_msg.data = total_points - 1; path_ind_pub_->publish(final_ind_msg); - + LOG_INFO("Published final path index 2: {}", final_ind_msg.data); LOG_INFO("main state change(Running State:Mission completed):{} -> {}", mainStateToString(main_state_), "AUTO_OFF"); main_state_ = MainState::AUTO_OFF; } else { diff --git a/src/webots_simulation/src/PathVisualizer.cpp b/src/webots_simulation/src/PathVisualizer.cpp index c9e240b..bcf728f 100644 --- a/src/webots_simulation/src/PathVisualizer.cpp +++ b/src/webots_simulation/src/PathVisualizer.cpp @@ -966,7 +966,7 @@ void PathVisualizer::publishOdomPath(){ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); std::string file_name; - file_name = "1701895544295370752.json"; + file_name = "1699991293313089536.json"; // std::cout << "请输入路径文件路径: "; // std::getline(std::cin, file_name);