lqr test

error_comparsion.py

@@ -8,6 +8,9 @@ import glob
 files = [
     "stanley_*_*_*_*.csv",
 ]
+files = [
+    "lqr_*_*_*_*_*.csv",
+]
 
 data = {}
 for pattern in files:
@@ -86,7 +89,7 @@ plt.grid(True, alpha=0.3)
 plt.legend()
 plt.tight_layout()
 
-lateral_output = "Lateral_error_comparison.png"
+lateral_output = "X_Lateral_error_comparison.png"
 plt.savefig(lateral_output, dpi=200)
 plt.show()
 
@@ -111,7 +114,7 @@ plt.grid(True, alpha=0.3)
 plt.legend()
 plt.tight_layout()
 
-heading_output = "Heading_error_comparison.png"
+heading_output = "X_Heading_error_comparison.png"
 plt.savefig(heading_output, dpi=200)
 plt.show()
 
@@ -137,7 +140,7 @@ plt.grid(True, alpha=0.3)
 plt.legend()
 plt.tight_layout()
 
-yaw_output = "Yaw_command_comparison.png"
+yaw_output = "X_Yaw_command_comparison.png"
 plt.savefig(yaw_output, dpi=200)
 plt.show()
 print("读取完成：")

path_generate_json.py

@@ -10,14 +10,19 @@ import matplotlib.pyplot as plt
 START_LAT = 30.80062100422322
 START_LON = 121.21541599992909
 
-TURN_RADIUS = 1.0
-MISSION_SPEED = 0.64
+TURN_RADIUS = 1.2
+MISSION_SPEED = 0.32
 ARC_SPACING = 0.3
 
 # stanley para
 K_YAW = 2.0
 K_LAT = 4.0
 
+# lqr para
+Q_e = 50
+Q_h = 15
+R_u = 10
+
 # 地球半径（m）
 EARTH_RADIUS = 6378137.0
 
@@ -169,7 +174,10 @@ path_json = {
         "turnRadius": TURN_RADIUS,
         "misionSpeed": MISSION_SPEED,
         "k_yaw": K_YAW,
-        "k_lat": K_LAT
+        "k_lat": K_LAT,
+        "lqr_Q_e": Q_e,
+        "lqr_Q_h": Q_h,
+        "lqr_R_u": R_u
     },
     "features": [
         {

src/webots_simulation/include/sweet_webots_robot/PathVisualizer.hpp

@@ -22,6 +22,8 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <std_msgs/msg/int32.hpp>
+#include <Eigen/Dense>
+
 
 #include <spdlog/spdlog.h>
 #include <spdlog/sinks/stdout_color_sinks.h>
@@ -63,7 +65,9 @@ private:
     void interpolatePoints(double max_interval_distance);
     void publishLocalPose();
     void publishOdomPath();
-
+    void computePathCurvature();
+    // lqr
+    Eigen::MatrixXd lqr(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R);
 private:
     // topic pub
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
@@ -91,6 +95,7 @@ private:
     std::vector<double> path_x_;
     std::vector<double> path_y_;
     std::vector<double> path_yaw_;
+    std::vector<double> path_kappa_;
     // odom history pose
     std::vector<double> history_x_;
     std::vector<double> history_y_;
@@ -121,6 +126,11 @@ private:
     double k_yaw_ = 1.0;
     double k_lat_ = 1.0;
 
+    //lqr parametres
+    double Q_e = 1.0;
+    double Q_h = 1.0;
+    double R_u = 1.0;
+    bool kappa_cal_ = false;
 
     bool origin_set_= false;
     bool path_transed = false;

src/webots_simulation/src/PathVisualizer.cpp

@@ -44,6 +44,7 @@ has_pose_(false)
     // path get
     downloadPathJson(file_path_,json_data_,true);
     loadPathFromJson(json_data_);
+    
 
     // logger初始化
     auto console_sink =
@@ -264,6 +265,9 @@ bool PathVisualizer::loadPathFromJson(
         max_speed_ = data["properties"]["misionSpeed"].get<double>();
         k_yaw_ = data["properties"]["k_yaw"].get<double>();
         k_lat_ = data["properties"]["k_lat"].get<double>();
+        Q_e = data["properties"]["lqr_Q_e"].get<double>();
+        Q_h = data["properties"]["lqr_Q_h"].get<double>();
+        R_u = data["properties"]["lqr_R_u"].get<double>();
         // 检查 features
         if (!data.contains("features") ||
             !data["features"].is_array() ||
@@ -595,6 +599,38 @@ void PathVisualizer::publishPathRviz()
     origin_path_pub_->publish(origin_path);
   
 }
+
+void PathVisualizer::computePathCurvature()
+    {
+        if (path_x_.size() < 3) {
+            return;
+        }
+        path_kappa_.resize(path_x_.size());
+        for (size_t i = 1; i < path_x_.size() - 1; ++i)
+        {
+            int p_prev = i - 1;
+            int p_curr = i;
+            int p_next = i + 1;
+            double ds1 = std::hypot(path_x_[p_curr] - path_x_[p_prev],path_y_[p_curr] - path_y_[p_prev]);
+            double ds2 = std::hypot(path_x_[p_next] - path_x_[p_curr],path_y_[p_next] - path_y_[p_curr]);
+            double ds = 0.5 * (ds1 + ds2);
+            if (ds < 1e-6) {
+                path_kappa_[i] = 0.0;
+                continue;
+            }
+            double dyaw = path_yaw_[p_next] - path_yaw_[p_prev];
+            while (dyaw > M_PI)
+                dyaw -= 2.0 * M_PI;
+
+            while (dyaw < -M_PI)
+                dyaw += 2.0 * M_PI;
+            path_kappa_[i] = dyaw / (ds1 + ds2);
+            logger_->debug("path ind:{}, kappa:{}", i, path_kappa_[i]);
+        }
+        // 首尾点处理
+        path_kappa_[0] = path_kappa_[1];
+        path_kappa_[path_kappa_.size() - 1] = path_kappa_[path_kappa_.size() - 2];
+    }
 void PathVisualizer::publishHistoryPathRviz(){
     history_x_.push_back(x_);
     history_y_.push_back(y_);
@@ -685,7 +721,22 @@ void PathVisualizer::localozation_callback(const geometry_msgs::msg::PoseWithCov
         publishOdomPath();
     }
 }
-
+Eigen::MatrixXd PathVisualizer::lqr(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R)
+{
+    
+    Eigen::MatrixXd P = Q; // 初始化P为Q
+    Eigen::MatrixXd P_prev = Eigen::MatrixXd::Zero(A.rows(), A.cols());
+    for (int i = 0; i < 1000; ++i)
+    {
+        P_prev = P;
+        P = Q + A.transpose() * P * A - A.transpose() * P * B * (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;
+        if ((P - P_prev).norm() < 1e-6)
+            break; // 收敛条件
+    }
+    // LQR增益矩阵K
+    Eigen::MatrixXd K = (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;
+    return K;
+};
 void PathVisualizer::control(){
     if(!has_pose_)
     {
@@ -697,6 +748,11 @@ void PathVisualizer::control(){
     double speed=max_speed_;
     publishVehicleInfoRviz();
     publishLocalPose();
+    if(!kappa_cal_){
+        kappa_cal_=true;
+        computePathCurvature();
+    }
+        
     target_index_= findClosestPoint(forward_index_limit_);
     int current_index = target_index_;
     int target =findLookAheadPoint();
@@ -704,6 +760,7 @@ void PathVisualizer::control(){
     double target_x =path_x_[target];
     double target_y =path_y_[target];
     double target_yaw =path_yaw_[target];
+    double target_kappa =path_kappa_[target];
 
     /*世界坐标转换到车辆坐标系*/
     double dx =target_x-x_;
@@ -716,6 +773,8 @@ void PathVisualizer::control(){
     while (alpha > M_PI) alpha -= 2 * M_PI;
     while (alpha < -M_PI) alpha += 2 * M_PI;
 
+    geometry_msgs::msg::Twist cmd;
+
     /*Pure Pursuit*/
     // double Ld =sqrt(local_x*local_x+local_y*local_y);
     // double w_cmd =2.0*speed*sin(alpha)/(Ld);
@@ -724,6 +783,7 @@ void PathVisualizer::control(){
     double curent_path_x = path_x_[current_index];
     double curent_path_y = path_y_[current_index];
     double current_path_yaw =path_yaw_[current_index];
+    double current_path_kappa = path_kappa_[current_index];
     alpha = current_path_yaw - yaw_;
     while (alpha > M_PI) alpha -= 2 * M_PI;
     while (alpha < -M_PI) alpha += 2 * M_PI;
@@ -733,8 +793,30 @@ void PathVisualizer::control(){
     double error_lat = -dx_path * sin(current_path_yaw) + dy_path * cos(current_path_yaw);
     double w_cmd =k_yaw_ * alpha - k_lat_ * std::atan2(error_lat,speed);
 
-    geometry_msgs::msg::Twist cmd;
+    /*lqr*/
+    double dt = 0.01;
+    Eigen::MatrixXd A(2, 2),B(2, 1),Q(2, 2),R(1, 1);
+    // A << 1.0 , speed * dt,
+    //      0   , 1.0;
+    // B << 0.5 * speed * dt * dt,
+    //      dt;
+    A << 1.0 , speed * dt,
+         0   , 1.0;
+    B << 0,
+         dt;
+    Q << Q_e , 0 , 
+         0   , Q_h;
+    R << R_u;
+    
+    Eigen::VectorXd x(2);
+     x << error_lat, -alpha;
+    Eigen::MatrixXd K = lqr(A, B, Q, R);
 
+    std::stringstream ss;
+    ss << K;
+    logger_->debug("LQR Gain Matrix K:\n{}", ss.str());
+    w_cmd = - 1 * speed * current_path_kappa -(K * x)(0, 0);
+   
     logger_->debug("current_pose_x:{}, current_pose_y:{},current_pose_yaw:{}",
                 x_, y_,yaw_
     );
@@ -745,7 +827,6 @@ void PathVisualizer::control(){
     );
     cmd.linear.x = speed;
     cmd.angular.z = w_cmd;
-    
 
     if(cmd.angular.z >max_yaw_rate_)
     {
@@ -757,6 +838,13 @@ void PathVisualizer::control(){
         cmd.angular.z=-max_yaw_rate_;
     }
 
+    if (current_index == path_x_.size() - 1)
+    {
+        cmd.linear.x = 0.0;
+        cmd.angular.z = 0.0;
+        RCLCPP_INFO(this->get_logger(),"Reached the end of the path.");
+    }
+
     cmd_pub_->publish(cmd);
     logger_->debug("current_cmd_angular_z:{}",cmd.angular.z);
 

test_path.json

@@ -1,10 +1,13 @@
 {
   "type": "FeatureCollection",
   "properties": {
-    "turnRadius": 1.0,
-    "misionSpeed": 0.64,
+    "turnRadius": 1.2,
+    "misionSpeed": 0.32,
     "k_yaw": 2.0,
-    "k_lat": 4.0
+    "k_lat": 4.0,
+    "lqr_Q_e": 100,
+    "lqr_Q_h": 10,
+    "lqr_R_u": 15
   },
   "features": [
     {
@@ -12,7 +15,7 @@
       "properties": {
         "turnInds": [
           1,
-          13
+          15
         ],
         "mission": []
       },
@@ -30,98 +33,113 @@
             0.0
           ],
           [
-            30.800621368104,
-            121.215481695811,
-            0.285599332
+            30.800621317464,
+            121.215481752769,
+            0.241660973
           ],
           [
-            30.800622430267,
-            121.215484403537,
-            0.571198664
+            30.800622238982,
+            121.215484581609,
+            0.483321947
           ],
           [
-            30.800624104662,
-            121.215486653196,
-            0.856797996
+            30.800623715223,
+            121.215487071501,
+            0.72498292
           ],
           [
-            30.80062625564,
-            121.215488262536,
-            1.142397329
+            30.800625660392,
+            121.215489077742,
+            0.966643893
           ],
           [
-            30.80062870894,
-            121.215489101177,
-            1.427996661
+            30.800627961443,
+            121.215490483736,
+            1.208304867
           ],
           [
-            30.800631265812,
-            121.215489101177,
-            1.713595993
+            30.800630484647,
+            121.215491207772,
+            1.44996584
           ],
           [
-            30.800633719113,
-            121.215488262536,
-            1.999195325
+            30.800633083366,
+            121.215491207772,
+            1.691626813
           ],
           [
-            30.80063587009,
-            121.215486653196,
-            2.284794657
+            30.800635606571,
+            121.215490483736,
+            1.933287787
           ],
           [
-            30.800637544485,
-            121.215484403537,
-            2.570393989
+            30.800637907622,
+            121.215489077742,
+            2.17494876
           ],
           [
-            30.800638606648,
-            121.215481695811,
-            2.855993321
+            30.80063985279,
+            121.215487071501,
+            2.416609734
           ],
           [
-            30.800638970529,
+            30.800641329031,
+            121.215484581609,
+            2.658270707
+          ],
+          [
+            30.800642250549,
+            121.215481752769,
+            2.89993168
+          ],
+          [
+            30.80064256379,
             121.215478749384,
             3.141592654
           ],
           [
-            30.800638970529,
+            30.80064256379,
             121.215395083444,
             3.141592654
           ],
           [
-            30.800639276622,
-            121.215392376652,
-            2.879793266
+            30.800642834062,
+            121.215392290831,
+            2.917193178
           ],
           [
-            30.800640174043,
-            121.215389854323,
-            2.617993878
+            30.800643631324,
+            121.215389638251,
+            2.692793703
           ],
           [
-            30.800641601633,
-            121.21538768835,
-            2.35619449
+            30.800644915599,
+            121.215387258715,
+            2.468394228
           ],
           [
-            30.800643462105,
-            121.21538602634,
-            2.094395102
+            30.800646622488,
+            121.215385271544,
+            2.243994753
           ],
           [
-            30.800645628671,
-            121.215384981558,
-            1.832595715
+            30.800648666401,
+            121.215383776383,
+            2.019595277
           ],
           [
-            30.800647953682,
-            121.215384625202,
+            30.800650944846,
+            121.215382848205,
+            1.795195802
+          ],
+          [
+            30.800653343573,
+            121.215382533553,
             1.570796327
           ],
           [
-            30.800692869446,
-            121.215384625202,
+            30.800698259338,
+            121.215382533553,
             1.570796327
           ]
         ]