发布作者：
Viiicee
作品采用：
《
署名-非商业性使用-相同方式共享 4.0 国际 (CC BY-NC-SA 4.0)
》许可协议授权
在ROS中集成PX4位置平滑PositionSmoothing功能
2024年11月14日
270阅读
1评论
0点赞
在ROS中集成PX4位置平滑功能
一、背景
在ROS中，发布目标点的过程通常是直接发布位置数据而未进行平滑处理，这可能引发控制上的不稳定等问题。为了提升平稳性，本项目引入了PX4中的位置平滑功能 PositionSmoothing。在之前的工作中，我们已成功将 PositionSmoothing 应用于PX4，因此希望进一步将其集成到ROS项目中，实现平滑的位置控制。
二、准备工作
在本文中，我们基于上一篇的配置环境进行开发，因而建议先完成上篇文章中的环境配置。然后，按照以下步骤引入所需的PX4库文件。
1. 拷贝所需文件
找到
PX4-Autopilot/src/lib目录下的以下文件夹并拷贝至src/offboard_run/include中：mathlibmatrixmotion_planning
- 前往
PX4-Autopilot/platforms/common/include目录，将该目录下的所有文件拷贝至src/offboard_run/include。
2. 解决编译错误
引入PX4的库文件后，可能会遇到以下编译错误：
<px4_boardconfig.h>文件找不到<matrix/math.hpp>文件路径错误
错误 1： <px4_boardconfig.h> 文件缺失
由于我们不使用此配置文件（在官方的包里我也没有找到这个文件，索性直接注释放弃使用了），可通过注释相关代码来解决。使用批量查找将涉及 <px4_boardconfig.h> 的代码注释掉即可。
错误 2： <matrix/math.hpp> 文件路径错误
该错误是由于路径设置问题引起的（可能是环境不同，原来官方的是matrix/math.hpp，但在这里需要修改）。实际路径应为 matrix/matrix/math.hpp。可以通过批量替换将所有 <matrix/math.hpp> 替换为 <matrix/matrix/math.hpp> 来解决此问题。
3. 项目文件结构
以下是 src 文件夹的目录结构（省略 include 下的子目录文件）：
── src
├── CMakeLists.txt -> /opt/ros/noetic/share/catkin/cmake/toplevel.cmake
└── offboard_run
├── CMakeLists.txt
├── include
│ ├── mathlib
│ ├── matrix
│ ├── motion_planning
│ ├── offboard_run
│ ├── px4_defines.h
│ ├── px4_log.h
│ ├── px4_platform_common
│ ├── px4_random.h
│ └── px4_time.h
├── package.xml
└── src
└── offboard_run_node.cpp三、源码配置
offboard_run_node.cpp 代码：
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h> // 包含速度消息类型
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include "motion_planning/PositionSmoothing.hpp"
#include "motion_planning/VelocitySmoothing.hpp"
// 全局变量
mavros_msgs::State current_state;
geometry_msgs::PoseStamped current_pose;
geometry_msgs::TwistStamped current_velocity; // 速度信息
geometry_msgs::TwistStamped velocity; // 速度消息
PositionSmoothing position_smoothing;
ros::Time last_generation_time;
int publish_count = 0; // 发布次数计数器
int offboard_count = 0; //offboard计数器
// 状态回调函数
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
current_state = *msg;
}
// 位置回调函数
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
current_pose = *msg;
}
// 速度回调函数
void velocity_cb(const geometry_msgs::TwistStamped::ConstPtr& msg) {
current_velocity = *msg;
}
// 设置轨迹平滑器参数
void setupPositionSmoothing() {
// 创建一个 Vector3f 对象用于最大加加速度
Vector3f max_jerk(4.0f, 4.0f, 4.0f); // 假设你希望在所有轴上使用相同的加加速度
position_smoothing.setMaxJerk(max_jerk); // 使用 Vector3f 对象
// position_smoothing.setMaxJerk(4.0f); // 最大加加速度 (m/s³)
position_smoothing.setMaxAccelerationXY(2.0f); // 最大水平加速度 (m/s²)
position_smoothing.setMaxAccelerationZ(3.0f); // 最大垂直加速度 (m/s²)
position_smoothing.setMaxVelocityXY(5.0f); // 最大水平速度 (m/s)
position_smoothing.setMaxVelocityZ(3.0f); // 最大垂直速度 (m/s)
position_smoothing.setHorizontalTrajectoryGain(0.8f); // 水平轨迹增益：
position_smoothing.setMaxAllowedHorizontalError(2.0f); // 最大允许的水平误差 (m)：
position_smoothing.setVerticalAcceptanceRadius(0.5f); // 垂直接受半径 (m)：
position_smoothing.setCruiseSpeed(3.0f); // 巡航速度 (m/s)：
position_smoothing.setTargetAcceptanceRadius(0.5f); // 目标接受半径 (m)：
}
// 在生成轨迹前，初始化轨迹生成器的状态
void initializeTrajectoryGenerator(const geometry_msgs::PoseStamped& current_pose) {
Vector3f current_position(
current_pose.pose.position.x,
current_pose.pose.position.y,
current_pose.pose.position.z
);
Vector3f current_local_velocity(
current_velocity.twist.linear.x,
current_velocity.twist.linear.y,
current_velocity.twist.linear.z
);
Vector3f zero_acceleration(0.0f, 0.0f, 0.0f);
// 重置轨迹生成器状态
position_smoothing.reset(zero_acceleration, current_local_velocity, current_position);
}
// 生成平滑轨迹
void generateSmoothTrajectory(const geometry_msgs::PoseStamped& current_pose,
const Vector3f& target_position,
PositionSmoothing::PositionSmoothingSetpoints& setpoints // 修改为引用类型输出参数
) {
// 计算时间间隔
ros::Time current_time = ros::Time::now();
float delta_time = (current_time - last_generation_time).toSec();
last_generation_time = current_time;
ROS_INFO("Delta time: %.4f", delta_time); // 打印时间差，确认它是否有变化
// 创建当前位置向量
Vector3f current_pos(current_pose.pose.position.x,
current_pose.pose.position.y,
current_pose.pose.position.z);
// ROS_INFO("Smooth Current Position: [%.2f, %.2f, %.2f]", current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
// ROS_INFO("Smooth Target Position: [%.2f, %.2f, %.2f]", target_position(0), target_position(1), target_position(2));
// 创建预馈速度（这里设为0）
Vector3f feedforward_velocity(0.0f, 0.0f, 0.0f);
// 生成平滑轨迹
position_smoothing.generateSetpoints(
current_pos, //当前位置
target_position,// 目标位置
feedforward_velocity, // 目标速度
delta_time, // 时间步长
false, // 不强制零速度
setpoints // 输出的平滑轨迹点
);
//返回平滑后的XYZ位置position，速度velocity，加速度acceleration等信息
// ROS_INFO("New Smoothed Position: [%.2f, %.2f, %.2f]", setpoints.position(0), setpoints.position(1), setpoints.position(2));
}
int main(int argc, char **argv) {
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
// 订阅者
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb); //状态订阅
ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
("mavros/local_position/pose", 10, pose_cb); //位置订阅
ros::Subscriber velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>
("mavros/local_position/velocity_local", 10, velocity_cb); // 速度订阅
// 发布者
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10); // 位置发布
ros::Publisher velocity_pub = nh.advertise<geometry_msgs::TwistStamped>
("mavros/setpoint_velocity/cmd_vel", 10); // 速度发布
// 服务客户端
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
ros::Rate rate(20.0);
// 等待连接
while(ros::ok() && !current_state.connected) {
ros::spinOnce();
rate.sleep();
}
// 初始化位置和轨迹平滑器
geometry_msgs::PoseStamped pose;
// pose.pose.position.x = 0;
// pose.pose.position.y = 0;
// pose.pose.position.z = 0;
// 初始化平滑器设定值
setupPositionSmoothing();
// 设置目标点
float target_x = 20;
float target_y = 20;
float target_z = 20;
// 控制标志
bool z_reached = false;
bool x_reached = false;
bool y_reached = false;
// // 发送一些初始setpoints
// for(int i = 100; ros::ok() && i > 0; --i) {
// local_pos_pub.publish(pose);
// ros::spinOnce();
// rate.sleep();
// }
// // 设置OFFBOARD模式和解锁命令
// mavros_msgs::SetMode offb_set_mode;
// offb_set_mode.request.custom_mode = "OFFBOARD";
// mavros_msgs::CommandBool arm_cmd;
// arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
while(ros::ok()) {
// // OFFBOARD模式检查
// if(current_state.mode != "OFFBOARD" &&
// (ros::Time::now() - last_request > ros::Duration(5.0))) {
// if(set_mode_client.call(offb_set_mode) &&
// offb_set_mode.response.mode_sent) {
// ROS_INFO("Offboard enabled");
// }
// last_request = ros::Time::now();
// } else {
// // 解锁检查
// if(!current_state.armed &&
// (ros::Time::now() - last_request > ros::Duration(5.0))) {
// if(arming_client.call(arm_cmd) &&
// arm_cmd.response.success) {
// ROS_INFO("Vehicle armed");
// }
// last_request = ros::Time::now();
// }
// }
if (current_state.mode == "OFFBOARD" ) {
static bool initialized = false;
if (!initialized) {
initializeTrajectoryGenerator(current_pose);
ROS_INFO("-----------");
ROS_INFO("initialized");
ROS_INFO("-----------");
initialized = true;
}
// if (offboard_count<100){
// // 保持当前位置
// pose.pose.position.x = current_pose.pose.position.x;
// pose.pose.position.y = current_pose.pose.position.y;
// pose.pose.position.z = current_pose.pose.position.z;
// local_pos_pub.publish(pose); // 发送当前位置
// ROS_WARN_THROTTLE(5, "Waiting for OFFBOARD mode...");
// ROS_INFO("offboard count: %d", offboard_count);
// ROS_INFO("Current Position: [%.2f, %.2f, %.2f]", current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
// offboard_count++;
// }
// else{
Vector3f target_position;
// bool force_stop = false;
// 分段飞行逻辑
if (!z_reached) {
target_position = Vector3f(0.0f, 0.0f, target_z);
// pose.pose.position.z = target_z; // 设置目标 z 坐标
if (abs(current_pose.pose.position.z - target_z) < 0.1) {
ROS_INFO("Z axis reached");
z_reached = true;
}
} else if (!x_reached) {
target_position = Vector3f(target_x, 0.0f, target_z);
// pose.pose.position.x = target_x; // 设置目标 x 坐标
if (abs(current_pose.pose.position.x - target_x) < 0.1) {
ROS_INFO("X axis reached");
x_reached = true;
}
} else if (!y_reached) {
target_position = Vector3f(target_x, target_y, target_z);
// pose.pose.position.y = target_y; // 设置目标 y 坐标
if (abs(current_pose.pose.position.y - target_y) < 0.1) {
ROS_INFO("Y axis reached");
y_reached = true;
}
} else {
ROS_INFO("Flight completed");
}
// 创建setpoints对象
PositionSmoothing::PositionSmoothingSetpoints setpoints;
// 生成平滑轨迹
generateSmoothTrajectory(current_pose, target_position, setpoints);
// 平滑后的位置
pose.pose.position.x = setpoints.position(0);
pose.pose.position.y = setpoints.position(1);
pose.pose.position.z = setpoints.position(2);
// 平滑后的速度
// velocity.twist.linear.x = setpoints.velocity(0);
// velocity.twist.linear.y = setpoints.velocity(1);
// velocity.twist.linear.z = setpoints.velocity(2);
// 输出调试信息
ROS_INFO("Publish count: %d", ++publish_count);
ROS_INFO("Target Position: [%.2f, %.2f, %.2f]", target_position(0), target_position(1), target_position(2));
ROS_INFO("Current Position: [%.2f, %.2f, %.2f]", current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
// ROS_INFO("Current Velocity: [%.2f, %.2f, %.2f]", current_velocity.twist.linear.x, current_velocity.twist.linear.y, current_velocity.twist.linear.z); // 输出速度信息
ROS_INFO("Smoothed Position: [%.2f, %.2f, %.2f]", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z);
// ROS_INFO("Smoothed Velocity: [%.2f, %.2f, %.2f]", velocity.twist.linear.x, velocity.twist.linear.y, velocity.twist.linear.z);
// 发布位置和速度命令
local_pos_pub.publish(pose);
// velocity_pub.publish(velocity); // 发布速度命令
// }
} else {
// 保持当前位置
pose.pose.position.x = current_pose.pose.position.x;
pose.pose.position.y = current_pose.pose.position.y;
pose.pose.position.z = current_pose.pose.position.z;
local_pos_pub.publish(pose); // 发送当前位置
// 初始化时间
last_generation_time = ros::Time::now();
ROS_WARN_THROTTLE(5, "Waiting for OFFBOARD mode...");
ROS_WARN_THROTTLE(5, "Current Position: [%.2f, %.2f, %.2f]", current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
}
ros::spinOnce();
rate.sleep();
}
return 0;
}
在 CMakeLists.txt 中添加以下代码以配置项目：
cmake_minimum_required(VERSION 3.0.2)
project(offboard_run)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
mavros_msgs
roscpp
std_msgs
)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS geometry_msgs mavros_msgs roscpp std_msgs
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_executable(${PROJECT_NAME}_node
src/offboard_run_node.cpp
include/motion_planning/PositionSmoothing.cpp
include/motion_planning/VelocitySmoothing.cpp
)
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)四、测试
删除 uav_demo_ws 目录下的 build 文件夹，重新编译项目，完成后进行仿真测试。具体操作可以参考上篇文章。以下是仿真测试结果示意图：
五、结语
本次尝试将PX4中的位置平滑库集成至ROS中，旨在优化位置控制的平稳性。实际项目中可能还会遇到其他问题，需根据具体情况调整代码。希望本文能够为相关的ROS开发提供参考。
void setupPositionSmoothing() {
// 创建一个 Vector3f 对象用于最大加加速度
Vector3f max_jerk(4.0f, 4.0f, 4.0f); // 假设你希望在所有轴上使用相同的加加速度
position_smoothing.setMaxJerk(max_jerk); // 使用 Vector3f 对象
position_smoothing.setMaxAccelerationXY(2.0f); // 最大水平加速度 (m/s²)
position_smoothing.setMaxAccelerationZ(3.0f); // 最大垂直加速度 (m/s²)
position_smoothing.setMaxVelocityXY(5.0f); // 最大水平速度 (m/s)
position_smoothing.setMaxVelocityZ(3.0f); // 最大垂直速度 (m/s)
position_smoothing.setHorizontalTrajectoryGain(0.8f); // 水平轨迹增益
position_smoothing.setMaxAllowedHorizontalError(2.0f); // 最大允许的水平误差 (m)
position_smoothing.setVerticalAcceptanceRadius(0.5f); // 垂直接受半径 (m)
position_smoothing.setCruiseSpeed(3.0f); // 巡航速度 (m/s)
position_smoothing.setTargetAcceptanceRadius(0.5f); // 目标接受半径 (m)
// 初始化时间
last_generation_time = ros::Time::now();
}