ROS2 Stage中差速驱动绘制李萨如曲线失败求助

核心问题 #

你的代码存在三个关键问题，导致机器人无法正确跟踪李萨如曲线：

1. 轨迹导数计算错误 #

你计算的一阶、二阶导数都遗漏了参数a的系数，且二阶导数符号错误：

• 正确的一阶导数：

  double dx = A * a * cos(a * t);  // 原代码漏乘a
  double dy = B * 2 * a * cos(2 * a * t);  // 原代码漏乘a
  

• 正确的二阶导数：

  double dx2 = -A * a * a * sin(a * t);  // 原代码符号错误且漏乘a²
  double dy2 = -B * 4 * a * a * sin(2 * a * t);  // 原代码符号错误且漏乘a²
  

导数错误会直接导致线速度v和角速度w的计算完全偏离预期。

2. 坐标系混淆 #

差速驱动机器人的Twist消息中，linear.x是机器人本体坐标系下的前进速度，而非全局坐标系下的速度大小。你直接将全局轨迹的速度模长赋值给linear.x，忽略了机器人当前朝向与轨迹切线方向的差异，导致机器人运动方向错误。

3. 无反馈的开环控制 #

预先生成所有速度消息再发布的方式属于开环控制，完全没有考虑机器人的实际运动误差。Stage环境中机器人的运动存在延迟、打滑等误差，开环控制会让偏差不断累积，最终完全偏离轨迹。

修正方案 #

改用闭环轨迹跟踪控制，实时根据机器人当前位姿调整速度，步骤如下：

1. 修正导数计算 #

确保轨迹的一阶、二阶导数计算正确，用于生成期望的轨迹位置和切线方向。

2. 实时获取机器人位姿 #

订阅机器人的odom或pose话题，获取当前的位置和朝向。

3. 计算偏差并生成控制指令 #

将全局坐标系下的轨迹偏差转换到机器人本体坐标系，用PID控制器计算速度，修正位置和角度偏差。

修正后的代码示例（闭环版） #

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/utils.h"

using namespace std::chrono_literals;

class LissajousTracker : public rclcpp::Node {
public:
    LissajousTracker() : Node("lissajous_tracker") {
        pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "/robot_pose", 10, std::bind(&LissajousTracker::poseCallback, this, std::placeholders::_1));
        cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
        
        // 轨迹参数
        A_ = 5.0;
        B_ = 4.0;
        a_ = 3.0;
        duration_ = 10.0;  // 轨迹总时长
        current_t_ = 0.0;
        
        // PID参数（可根据实际机器人调整）
        kp_v_ = 0.5;
        kp_w_ = 1.0;
    }

private:
    void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
        if (current_t_ >= duration_) {
            // 发布停止指令
            geometry_msgs::msg::Twist stop_cmd;
            cmd_vel_pub_->publish(stop_cmd);
            return;
        }

        // 计算期望轨迹的位置和切线方向
        double x_d = A_ * sin(a_ * current_t_);
        double y_d = B_ * sin(2 * a_ * current_t_);
        double dx_d = A_ * a_ * cos(a_ * current_t_);
        double dy_d = B_ * 2 * a_ * cos(2 * a_ * current_t_);
        double theta_d = atan2(dy_d, dx_d);

        // 获取当前机器人位姿
        double x = msg->pose.position.x;
        double y = msg->pose.position.y;
        tf2::Quaternion q(
            msg->pose.orientation.x,
            msg->pose.orientation.y,
            msg->pose.orientation.z,
            msg->pose.orientation.w);
        double theta = tf2::getYaw(q);

        // 将全局偏差转换到机器人本体坐标系
        double dx_global = x_d - x;
        double dy_global = y_d - y;
        double dx_body = dx_global * cos(theta) + dy_global * sin(theta);
        double dy_body = -dx_global * sin(theta) + dy_global * cos(theta);

        // 计算角度偏差并归一化到[-π, π]
        double theta_err = theta_d - theta;
        theta_err = atan2(sin(theta_err), cos(theta_err));

        // 生成控制指令
        geometry_msgs::msg::Twist cmd_vel;
        cmd_vel.linear.x = kp_v_ * dx_body;
        // 用角速度修正横向偏差
        cmd_vel.angular.z = kp_w_ * theta_err + kp_v_ * dy_body;

        cmd_vel_pub_->publish(cmd_vel);

        // 更新时间（假设回调频率为10Hz，每次累加0.1秒）
        current_t_ += 0.1;
    }

    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    
    double A_, B_, a_;
    double duration_, current_t_;
    double kp_v_, kp_w_;
};

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<LissajousTracker>());
    rclcpp::shutdown();
    return 0;
}

额外建议 #

• 调整轨迹参数A、B、a，避免线速度v和角速度w超过机器人的运动限制（比如v不要超过0.5m/s，w不要超过1rad/s）。
• 如果使用Stage仿真，确保机器人的odom话题正确发布，或者直接订阅base_pose_ground_truth话题获取准确位姿。

内容的提问来源于stack exchange，提问作者Boban Ljubicic