当前位置：首页 > news >正文

实现时间最优轨迹生成/轨迹规划方法(TOTG),不使用moveit，可用于ROS驱动机械臂FollowJointTrajectoryGoal()

news 来源：原创 2025/8/25 6:15:11

前言

在我的这篇文章：https://blog.csdn.net/weixin_45702459/article/details/139293391?spm=1011.2415.3001.5331中，写了不使用moveit来ros驱动机械臂的方法，也就是用FollowJointTrajectoryGoal()来进行一系列点的关节运动，其实这个就是moveit做机械臂控制的内部方法，只是moveit把规划和控制集成到了一起罢了。在我之前的这个代码中，只是传入了路径点，并没有做速度规划，所以导致机械臂每到一个点都会完全停止，之后再运行到下一个点，这个问题在轨迹点间隔较大的情况下其实还不明显，但如果轨迹点之间距离较近，那么卡顿感就会特别明显，卡顿的情况下速度也没法设置地很快，因为会震荡。对FollowJointTrajectoryGoal()进行smooth的相关的教程我全网都没有找到，github和ros网页上也只有人发问，没有什么具体的可行答案，索性自己记录一下以供后人参考。

查看moveit的轨迹运行信息

这里使用gazebo仿真即可，gazebo里就可以看见机械臂运行轨迹的柔顺性。运行以下UR5+moveit+gazebo的roslaunch，这里默认你已经安装好相关的包，还没有的话可以查看我的文章，很快就能装完。 https://blog.csdn.net/weixin_45702459/article/details/139293391?spm=1011.2415.3001.5331

ur5_sim.launch

<launch><!-- 启动UR5仿真 --><include file="$(find ur_gazebo)/launch/ur5_bringup.launch" /><!-- 设置参数 --><rosparam param="/sim">true</rosparam><!-- 启动MoveIt!规划和执行 --><include file="$(find ur5_moveit_config)/launch/moveit_planning_execution.launch"><arg name="sim" value="true"/></include><!-- 启动RViz --><include file="$(find ur5_moveit_config)/launch/moveit_rviz.launch" />
</launch>

这里注意下，<rosparam param="/sim">true</rosparam>这个参数是我自己加的，主要是判断是仿真还是真实机械臂，对程序运行没有影响。

再echo出moveit的FollowJointTrajectoryGoal()信息

rostopic echo /eff_joint_traj_controller/follow_joint_trajectory/goal

在rviz中拖动关节并执行会发现moveit的轨迹是带有速度，加速度，时间信息的。

moveit的轨迹规划方法

这两个网页说明了moveit就是用TOTG系列方法完成的轨迹规划 https://docs.ros.org/en/melodic/api/moveit_tutorials/html/doc/time_parameterization/time_parameterization_tutorial.html https://moveit.picknik.ai/main/doc/concepts/trajectory_processing.html

具体的轨迹规划模块在moveit-noetic-devel\moveit_core\trajectory_processing中，单拎出来是可以编译并跑test的，但是这不是文章的重点，所以跑通过程忽略，后续也不用这个模块。

安装TOTG的python库

使用这个库实现：https://github.com/balakumar-s/trajectory_smoothing

作者在dist中给了.whl的python包，但也可以下载下来直接使用pip install .进行安装：

pip install trajectory_smoothing@git+https://github.com/balakumar-s/trajectory_smoothing.git
pip install .

作者还给了ros1的包，但是个人感觉不如python库灵活性好，一是因为ros包比较老，二是因为如果以后不用ros了也可以用。

在安装完之后不要直接运行作者给出的example，因为会报错

/usr/bin/python3.8 /home/lu/trajectory_smoothing/examples/smooth_example.py 
Traceback (most recent call last):File "/home/lu/trajectory_smoothing/examples/smooth_example.py", line 1, in <module>from trajectory_smoothing import TrajectorySmootherFile "/home/lu/trajectory_smoothing/src/trajectory_smoothing/__init__.py", line 1, in <module>from trajectory import TrajectorySmoother, SmoothResult
ModuleNotFoundError: No module named 'trajectory'

把__init__.py重命名为其他名字比如__init__1.py

发现example可以正常运行。

进行轨迹规划

首先写一个轨迹规划的函数，输入是n*dof的轨迹点，类型是ndarray，输出是一个结构，包含轨迹点，加速度，速度，持续时间。最大速度，最大加速度等信息需要自己设置，具体可以问ai。

traj_plan.py

from trajectory_smoothing import TrajectorySmoother
import numpy as np
from typing import NamedTuple
from ros_config import velocity_limit, acceleration_limit# 定义返回的数据结构
class TrajectoryResult(NamedTuple):positions: np.ndarray  # 位置序列 (n_points x n_dof)velocities: np.ndarray  # 速度序列 (n_points x n_dof)accelerations: np.ndarray  # 加速度序列 (n_points x n_dof)time: np.ndarray  # 时间点序列 (n_points,)total_duration: float  # 总持续时间success: bool  # 是否成功生成轨迹def generate_optimal_trajectory(waypoints: np.ndarray,max_velocity = velocity_limit,max_acceleration = acceleration_limit,max_deviation: float = 0.1,interpolation_dt: float = 0.01,traj_dt: float = 0.001
) -> TrajectoryResult:"""生成时间最优轨迹参数:waypoints: 轨迹点数组 (n_points x n_dof)max_velocity: 各关节最大速度 (标量或数组)max_acceleration: 各关节最大加速度 (标量或数组)max_deviation: 允许的最大轨迹偏差interpolation_dt: 插值时间步长traj_dt: 轨迹计算时间步长返回:TrajectoryResult 包含位置、速度、加速度和时间信息"""# 获取自由度数dof = waypoints.shape[1]# 处理最大速度参数if isinstance(max_velocity, (int, float)):max_velocity = np.full(dof, max_velocity, dtype=np.float32)elif isinstance(max_velocity, (list, np.ndarray)):if len(max_velocity) != dof:raise ValueError(f"max_velocity 长度为 {len(max_velocity)}，但期望是自由度 {dof}")max_velocity = np.array(max_velocity, dtype=np.float32)else:raise TypeError("max_velocity 必须是标量、列表或 ndarray")# 处理最大加速度参数if isinstance(max_acceleration, (int, float)):max_acceleration = np.full(dof, max_acceleration, dtype=np.float32)elif isinstance(max_acceleration, (list, np.ndarray)):if len(max_acceleration) != dof:raise ValueError(f"max_acceleration 长度为 {len(max_acceleration)}，但期望是自由度 {dof}")max_acceleration = np.array(max_acceleration, dtype=np.float32)else:raise TypeError("max_acceleration 必须是标量、列表或 ndarray")# 创建轨迹平滑器trajectory_sm = TrajectorySmoother(dof, max_velocity, max_acceleration, max_deviation)# 生成平滑轨迹result = trajectory_sm.smooth_interpolate(waypoints, interpolation_dt=interpolation_dt, traj_dt=traj_dt)# 计算时间序列time_points = np.arange(result.length) * interpolation_dt# 返回结构化数据return TrajectoryResult(positions=result.position,velocities=result.velocity,accelerations=result.acceleration,time=time_points,total_duration=time_points[-1],success=result.success)# 使用示例
if __name__ == "__main__":# 创建测试轨迹点 (20个点，6自由度)n_points = 20dof = 6waypoints = np.zeros((n_points, dof))# 第一个关节从0到1线性运动waypoints[:, 0] = np.linspace(0, 1, n_points)# 生成轨迹traj_result = generate_optimal_trajectory(waypoints,max_velocity=1.5,max_acceleration=0.1)if traj_result.success:print(f"轨迹生成成功，总持续时间: {traj_result.total_duration:.2f}s")# 可视化第一个关节的结果import matplotlib.pyplot as pltfig, ax = plt.subplots(3, 1, figsize=(8, 8))ax[0].plot(traj_result.time, traj_result.positions[:, 0], label="Position")ax[1].plot(traj_result.time, traj_result.velocities[:, 0], label="Velocity")ax[2].plot(traj_result.time, traj_result.accelerations[:, 0], label="Acceleration")ax[0].set_ylabel("Position (rad)")ax[1].set_ylabel("Velocity (rad/s)")ax[2].set_ylabel("Acceleration (rad/s²)")ax[2].set_xlabel("Time (s)")for a in ax:a.legend()a.grid(True)plt.tight_layout()plt.savefig("trajectory_profile.png")plt.show()else:print("轨迹生成失败")

这是我设置的参数：

velocity_limit = [1.0, 1.0, 1.5, 1.5, 2.5, 2.5]
acceleration_limit = [0.2, 0.2, 0.3, 0.3, 0.8, 0.8]

这是我调用轨迹规划函数的FollowJointTrajectoryGoal()部分

def move(Positions):# goal就是我们向发送的关节运动数据，实例化为FollowJointTrajectoryGoal()类goal = FollowJointTrajectoryGoal()# goal当中的trajectory就是我们要操作的，其余的Header之类的不用管goal.trajectory = JointTrajectory()# goal.trajectory底下一共还有两个成员，分别是joint_names和points，先给joint_names赋值goal.trajectory.joint_names = JOINT_NAMES# # 从joint_state话题上获取当前的关节角度值，因为后续要移动关节时第一个值要为当前的角度值# joint_states = rospy.wait_for_message("joint_states", JointState)# joints_pos = joint_states.position# print("here")## Positions.insert(0, joints_pos)#轨迹规划waypoints = np.array(Positions, dtype=np.float32)traj_result = generate_optimal_trajectory(waypoints)goal.trajectory.points = []for i in range(len(traj_result.positions)):point = JointTrajectoryPoint(positions=traj_result.positions[i].tolist(),velocities=traj_result.velocities[i].tolist(),accelerations=traj_result.accelerations[i].tolist(),time_from_start=rospy.Duration(float(traj_result.time[i])))goal.trajectory.points.append(point)# 没有轨迹规划的版本# # print(Positions)# goal.trajectory.points = [0] * (len(Positions) + 1)  # 填充用的# goal.trajectory.points[0] = JointTrajectoryPoint(positions=joints_pos, velocities=[0] * 6, accelerations=[0] * 6,#                                                  time_from_start=rospy.Duration(0.0))# # goal.trajectory.points=[0]*(len(Positions))# for i in range(len(Positions)):#     goal.trajectory.points[i + 1] = JointTrajectoryPoint(positions=Positions[i], velocities=[0.1] * 6,#                                                          accelerations=[0.] * 6,#                                                          time_from_start=rospy.Duration(float((i + 1) * 0.15)))# 使用partial包装回调函数，传递额外的goal_positionsfeedback_cb_with_args = partial(feedback_callback, goal_positions=Positions)client.send_goal(goal, feedback_cb=feedback_cb_with_args)

运行后可以在gazebo中看到机械臂柔顺地执行轨迹。

前言

查看moveit的轨迹运行信息

moveit的轨迹规划方法

安装TOTG的python库

进行轨迹规划

相关文章：