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

Optimize control process

Browse Source 
1.fix pp algori

2.fix point interpolate yaw
This commit is contained in:
liuhaoyan 2026-09-14 18:13:00 +08:00
parent fe65324a48
commit 8d135e741d
6 changed files with 125 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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) {}
};

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

@ -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<LQR_Controller_C> controller_;
std::shared_ptr<spdlog::logger> 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<PurePursuit_Controller_C> controller_;
double adapter_look_ahead_dis_; //保存前视距离
std::shared_ptr<spdlog::logger> 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<float>(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw));
//dis = static_cast<float>(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw));
dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
lon_dis = static_cast<float>(sqrt(dx * dx + dy * dy));
kappa = static_cast<float>(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;

17
src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp
View File

@ -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!"<<std::endl;
logger_->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!"<<std::endl;
logger_->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);

25
src/auto_drive_misson_center/src/mission_center_node.cpp
View File

@ -920,9 +920,28 @@ std::vector<std::vector<double>> MissionCenterNode::interpolatePoints(
double ratio = static_cast<double>(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});
}

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

@ -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 {

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

@ -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);