diff --git a/src/robot_control/include/extended_kalman_filter.hpp b/src/robot_control/include/extended_kalman_filter.hpp new file mode 100644 index 0000000..6ac0dcd --- /dev/null +++ b/src/robot_control/include/extended_kalman_filter.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include +#include +#include // 用于保存轨迹数据 + +namespace robot_control { +class EKF +{ +private: + double dt; + + // 卡尔曼滤波参数 + Eigen::Matrix4d Q; // 过程噪声协方差矩阵 + Eigen::MatrixXd R; // 测量噪声协方差矩阵 + + // 状态向量 [x, y, θ, ω] + Eigen::Vector4d X; + // 协方差矩阵 + Eigen::Matrix4d P; + + // 角度归一化到[-π, π] + double normalizeAngle(double angle) { + while (angle > M_PI) angle -= 2.0 * M_PI; + while (angle < -M_PI) angle += 2.0 * M_PI; + return angle; + } + +public: + EKF(double init_x = 0, double init_y = 0, double init_theta = 0, double init_omega = 0); + + // 预测步骤 + void predict(double v, double omega_wheel); + + // 更新步骤 + void update(double gyro_omega); + + // 获取当前状态 + Eigen::Vector4d getState() { return X; } +}; +} diff --git a/src/robot_control/include/robot_control_manager.hpp b/src/robot_control/include/robot_control_manager.hpp index 7d66fea..c7ec83c 100644 --- a/src/robot_control/include/robot_control_manager.hpp +++ b/src/robot_control/include/robot_control_manager.hpp @@ -1,9 +1,11 @@ #pragma once +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/float64_multi_array.hpp" #include "geometry_msgs/msg/twist.hpp" #include "sensor_msgs/msg/joint_state.hpp" +#include "sensor_msgs/msg/imu.hpp" #include "motion_parameters.hpp" #include "arm_control.hpp" #include "leg_control.hpp" @@ -13,8 +15,10 @@ #include "waist_control.hpp" #include "interfaces/msg/motor_pos.hpp" +#include "interfaces/msg/imu_msg.hpp" using MotorPos = interfaces::msg::MotorPos; +using ImuMsg = interfaces::msg::ImuMsg; namespace robot_control { @@ -33,9 +37,9 @@ namespace robot_control void JogAxis(size_t axis, int dir); // 检查参数是否合理 - bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle){return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle);}; - bool SetMoveLegParameters(double moveLegDistance){return legController_->SetMoveLegParametersInternal(moveLegDistance);}; - bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle){return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);}; + bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle); + bool SetMoveLegParameters(double moveLegDistance); + bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle); // 机器人关节指令 std_msgs::msg::Float64MultiArray GetJointCommands(); @@ -43,9 +47,11 @@ namespace robot_control std_msgs::msg::Float64MultiArray GetWheelCommands(); std_msgs::msg::Float64MultiArray GetWheelFeedback(); + // 机器人状态 void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands); void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg); void UpdateWheelStates(const MotorPos::SharedPtr msg); + void UpdateImuMsg(const ImuMsg::SharedPtr msg); MotionParameters GetMotionParameters(); @@ -81,6 +87,12 @@ namespace robot_control std::vector jointInited_; // 机器人是否已经初始化 bool isJointInitValueSet_; + + // 陀螺仪状态 + ImuMsg imuMsg_; + std::vector gyroValues_; // 陀螺仪数据（弧度/秒） + std::vector gyroVelocities_; // 陀螺仪速度（弧度/秒） + std::vector gyroAccelerations_; // 陀螺仪加速度（弧度/秒^2） // 临时变量 std::vector tempWaistCmd_; @@ -94,6 +106,9 @@ namespace robot_control void AssignTempValues(); void UpdateJointCommands(); + + void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw); + void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw); }; } // namespace robot_control_manager diff --git a/src/robot_control/include/robot_control_node.hpp b/src/robot_control/include/robot_control_node.hpp index e565009..bee59e3 100644 --- a/src/robot_control/include/robot_control_node.hpp +++ b/src/robot_control/include/robot_control_node.hpp @@ -17,7 +17,6 @@ #include "interfaces/action/move_wheel.hpp" #include "interfaces/msg/motor_cmd.hpp" - using MoveHome = interfaces::action::MoveHome; using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle; @@ -81,6 +80,7 @@ namespace robot_control rclcpp::Subscription::SharedPtr jointStatesSub_; rclcpp::Publisher::SharedPtr motorCmdPub_; rclcpp::Subscription::SharedPtr wheelStatesSub_; + rclcpp::Subscription::SharedPtr imuMsgSub_; RobotControlManager robotControlManager_; @@ -102,6 +102,7 @@ namespace robot_control void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg); void JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg); void WheelStatesCallback(const MotorPos::SharedPtr cmd_msg); + void ImuMsgCallback(const ImuMsg::SharedPtr msg); }; } // namespace robot_control diff --git a/src/robot_control/src/extended_kalman_filter.cpp b/src/robot_control/src/extended_kalman_filter.cpp new file mode 100644 index 0000000..6989c7d --- /dev/null +++ b/src/robot_control/src/extended_kalman_filter.cpp @@ -0,0 +1,73 @@ +#include "extended_kalman_filter.hpp" + +namespace robot_control { +EKF::EKF(double init_x, double init_y, double init_theta, double init_omega) +{ + this->dt = 0.001; + + // 初始化状态向量 + X << init_x, init_y, init_theta, init_omega; + + // 初始化协方差矩阵 + P = Eigen::Matrix4d::Zero(); + P.diagonal() << 0.1, 0.1, 0.05, 0.01; +} + + // 预测步骤 +void EKF::predict(double v, double omega_wheel) { + // double x = X(0); + // double y = X(1); + double theta = X(2); + // double omega = X(3); + + // 状态转移矩阵F + Eigen::Matrix4d F = Eigen::Matrix4d::Identity(); + F(0, 2) = -v * sin(theta) * dt; + F(1, 2) = v * cos(theta) * dt; + F(2, 3) = dt; + + // 输入矩阵B + Eigen::Matrix B; + B << cos(theta) * dt, 0, + sin(theta) * dt, 0, + 0, dt, + 0, 1; + + // 控制输入 + Eigen::Vector2d u; + u << v, omega_wheel; + + // 预测状态 + X = F * X + B * u; + // 预测协方差 + P = F * P * F.transpose() + Q; + + // 归一化航向角 + X(2) = normalizeAngle(X(2)); +} + +// 更新步骤 +void EKF::update(double gyro_omega) { + // 观测矩阵H + Eigen::Matrix H; + H << 0, 0, 0, 1; + + // 测量残差 + Eigen::VectorXd z(1); + z << gyro_omega; + Eigen::VectorXd y = z - H * X; + + // 计算卡尔曼增益 + Eigen::MatrixXd S = H * P * H.transpose() + R; + Eigen::MatrixXd K = P * H.transpose() * S.inverse(); + + // 更新状态 + X = X + K * y; + // 更新协方差 + Eigen::Matrix4d I = Eigen::Matrix4d::Identity(); + P = (I - K * H) * P; + + // 归一化航向角 + X(2) = normalizeAngle(X(2)); +} +} \ No newline at end of file diff --git a/src/robot_control/src/robot_control_manager.cpp b/src/robot_control/src/robot_control_manager.cpp index 0a3996f..122a4c8 100644 --- a/src/robot_control/src/robot_control_manager.cpp +++ b/src/robot_control/src/robot_control_manager.cpp @@ -21,6 +21,10 @@ void RobotControlManager::init() jointCommands_.data.push_back(0.0); } + gyroValues_.resize(4, 0.0); + gyroVelocities_.resize(3, 0.0); + gyroAccelerations_.resize(3, 0.0); + // Initialize the wheel commands for (size_t i = 0; i < motionParams_.waist_joint_indices_.size(); i++) { @@ -76,6 +80,21 @@ RobotControlManager::~RobotControlManager() delete wheelController_; } +bool RobotControlManager::SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle) +{ + return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle); +} + +bool RobotControlManager::SetMoveLegParameters(double moveLegDistance) +{ + return legController_->SetMoveLegParametersInternal(moveLegDistance); +} + +bool RobotControlManager::SetMoveWaistParameters(double movePitchAngle, double moveYawAngle) +{ + return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle); +} + Float64MultiArray RobotControlManager::GetWheelCommands() { return wheelCommands_; @@ -157,6 +176,79 @@ bool isAllTrueManual(const std::vector& vec) { return true; // 所有元素都是 true } +void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg) +{ + imuMsg_ = *msg; + sensor_msgs::msg::Imu tempMsg = imuMsg_.imu; + + if (tempMsg.orientation.w == 0 && tempMsg.orientation.x == 0 && tempMsg.orientation.y == 0 && tempMsg.orientation.z == 0) + { + return; + } + + if (tempMsg.linear_acceleration.x == 0 && tempMsg.linear_acceleration.y == 0 && tempMsg.linear_acceleration.z == 0) + { + return; + } + + QuaternionToRPYDeg( + tempMsg.orientation.w, + tempMsg.orientation.x, + tempMsg.orientation.y, + tempMsg.orientation.z, + gyroValues_[0], + gyroValues_[1], + gyroValues_[2] + ); + + gyroVelocities_[0] = tempMsg.angular_velocity.x; + gyroVelocities_[1] = tempMsg.angular_velocity.y; + gyroVelocities_[2] = tempMsg.angular_velocity.z; + + gyroAccelerations_[0] = tempMsg.linear_acceleration.x; + gyroAccelerations_[1] = tempMsg.linear_acceleration.y; + gyroAccelerations_[2] = tempMsg.linear_acceleration.z; + + std::cout << "gyroValues_ : " << gyroValues_[0] << " " << gyroValues_[1] << " " << gyroValues_[2] << std::endl; + +} + +void RobotControlManager::QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw) +{ + double norm = sqrt(qw*qw + qx*qx + qy*qy + qz*qz); + if (norm < 1e-12) { // 避免除以零 + roll = 0; + pitch = 0; + yaw = 0; + return; + } + + qw /= norm; + qx /= norm; + qy /= norm; + qz /= norm; + + // 计算Roll角（X轴） + roll = atan2(2 * (qw*qx + qy*qz), 1 - 2 * (qx*qx + qy*qy)); + + // 计算Pitch角（Y轴），限制输入范围避免数值误差（asin输入需在[-1,1]） + double sin_pitch = 2 * (qw*qy - qz*qx); + sin_pitch = std::max(std::min(sin_pitch, 1.0), -1.0); + pitch = asin(sin_pitch); + + // 计算Yaw角（Z轴） + yaw = atan2(2 * (qw*qz + qx*qy), 1 - 2 * (qy*qy + qz*qz)); +} + +void RobotControlManager::QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw) +{ + QuaternionToRPYRad(qw, qx, qy, qz, roll, pitch, yaw); + + roll = roll * 180.0 / M_PI; + pitch = pitch * 180.0 / M_PI; + yaw = yaw * 180.0 / M_PI; +} + void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg) { motorPos_ = *msg; diff --git a/src/robot_control/src/robot_control_node.cpp b/src/robot_control/src/robot_control_node.cpp index 07a3e81..af31875 100644 --- a/src/robot_control/src/robot_control_node.cpp +++ b/src/robot_control/src/robot_control_node.cpp @@ -75,6 +75,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node") motorCmdPub_ = this->create_publisher("/motor_cmd", 10); ethercatSetPub_ = this->create_publisher("/ethercat/set", 10); + imuMsgSub_ = this->create_subscription("/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1)); jointStatesSub_ = this->create_subscription("/ec_joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1)); wheelStatesSub_ = this->create_subscription("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1)); joyCommandSub_ = this->create_subscription("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1)); @@ -686,6 +687,16 @@ void RobotControlNode::JointStatesCallback(const sensor_msgs::msg::JointState::S robotControlManager_.UpdateJointStates(msg); } +void RobotControlNode::ImuMsgCallback(const ImuMsg::SharedPtr cmd_msg) +{ + if (!cmd_msg) { // 检查消息是否有效 + std::cout << "get null imu msg!" << std::endl; + return; + } + + robotControlManager_.UpdateImuMsg(cmd_msg); +} + void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg) { if (!cmd_msg) { // 检查消息是否有效