
本文目录
- 一、六轴串联机械臂
- 二、机械臂运动学模型
-
- [2.1 正运动学](#2.1 正运动学)
- [2.2 逆运动学](#2.2 逆运动学)
- [三、机械臂构型模型 D-H 参数法](#三、机械臂构型模型 D-H 参数法)
-
- [3.1 D-H 参数的定义](#3.1 D-H 参数的定义)
- [3.2 连杆坐标系与齐次变换](#3.2 连杆坐标系与齐次变换)
- [3.3 UR20 的连杆参数](#3.3 UR20 的连杆参数)
- [3.4 与 URDF 建模的对应关系](#3.4 与 URDF 建模的对应关系)
- 四、对外接口
-
- [4.1 输入接口](#4.1 输入接口)
- [4.2 输出接口](#4.2 输出接口)
- 五、控制器参数
- 六、配置控制器参数
- 七、编写硬件抽象接口
- 八、控制机械臂运动
前面介绍了用 diff_drive_controller 控制两轮差速机器人,本节介绍用 position_controllers/JointTrajectoryController(位置轨迹控制器)控制一台 六轴串联机械臂。与差速底盘只关心左右轮速度不同,机械臂的控制目标是六个关节的位姿,并进一步通过正/逆运动学让末端执行器到达期望位姿。以 UR20 机械臂为例。
一、六轴串联机械臂
六轴串联机械臂(6R 机械臂)由 6 个转动关节依次串联而成,具有 6 个自由度,理论上可让末端到达工作空间内任意位置与姿态。以下图 UR20 为例,6 个关节自基座到末端依次为:

| 关节 | 关节连接的连杆 | 说明(关系仅参考上图) |
|---|---|---|
| shoulder_pan_joint | shoulder_link 和 base_link | 绕 base_link 的Z轴旋转,决定整个机械臂的水平朝向 |
| shoulder_lift_joint | upper_arm_link 和 shoulder_link | 绕 shoulder_link 的Y轴旋转,控制大臂前后摆动 |
| elbow_joint | upper_arm_link 和 forearm_link | 绕 forearm_link 的X轴旋转,控制小臂俯仰 |
| wrist_1_joint | wrist_1_link 和 forearm_link | 绕 wrist_1_link 的Z轴旋转,控制末端上下摆动 |
| wrist_2_joint | wrist_2_link 和 wrist_1_link | 绕 wrist_2_link 的Z轴旋转,控制末端左右摆动 |
| wrist_3_joint | wrist_3_link 和 wrist_2_link | 绕 wrist_3_link 的Z轴旋转,控制末端轴向旋转 |
前 3 个关节合称"位置关节",主要决定末端的位置;后 3 个关节构成球形手腕,主要决定末端的姿态。这种"位置 + 姿态"的分工是工业机械臂最常见的构型。UR20 的名义可达半径(肩关节到末端)约为 1.75 m,额定负载 20 kg。
在 URDF 中,这 6 个关节通过 transmission_interface/SimpleTransmission 声明为 hardware_interface/PositionJointInterface:
xml
<transmission name="${prefix}elbow_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}elbow_joint">
<hardwareInterface>${hw_interface}</hardwareInterface>
</joint>
<actuator name="${prefix}elbow_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
hw_interface 由 load_ur20.launch 传入,默认即为 hardware_interface/PositionJointInterface。这决定了硬件抽象层必须注册"关节状态接口 + 位置指令接口",控制器也只能下发位置量。
二、机械臂运动学模型
控制器直接操作的是关节角,而任务通常描述的是末端位姿,二者通过运动学联系。
2.1 正运动学
正运动学(Forward Kinematics)在已知各关节角 q = q 1 , ... , q 6 T q=q_1,\\dots,q_6^T q=q1,...,q6T 时求末端位姿。按 URDF 的连杆父子关系,把每个关节的齐次变换依次左乘:
0 T 6 = A 1 ( q 1 ) A 2 ( q 2 ) ⋯ A 6 ( q 6 ) {}^{0}T_{6} = A_1(q_1)A_2(q_2)\cdots A_6(q_6) 0T6=A1(q1)A2(q2)⋯A6(q6)
其中每个 A i A_i Ai 由关节的 <origin>(平移与姿态)和绕关节轴的旋转 R o t ( q i ) \mathrm{Rot}(q_i) Rot(qi) 组成。 0 T 6 {}^{0}T_6 0T6 的平移列即末端位置,旋转矩阵即末端姿态。本项目的 robot_kinematics.cpp 从 /robot_description 解析出连杆链,用四元数转旋转矩阵、齐次变换复合等方法自包含地实现了正运动学,不依赖 MoveIt/KDL。
2.2 逆运动学
逆运动学(Inverse Kinematics)由期望末端位姿反求关节角。6R 机械臂可以有解析解,但换构型就要重推公式;本项目采用通用数值解法------有限差分雅可比 + 阻尼最小二乘(Damped Least Squares):
Δ x = x d e s − x ( q ) Δ q = J T ( q ) J ( q ) J T ( q ) + λ 2 I − 1 Δ x q k + 1 = q k + Δ q \Delta x = x_{des}-x(q)\\ \Delta q = J^{T}(q)\leftJ(q)J\^{T}(q)+\\lambda\^{2}I\\right^{-1}\Delta x\\ q_{k+1} = q_k + \Delta q Δx=xdes−x(q)Δq=JT(q)J(q)JT(q)+λ2I−1Δxqk+1=qk+Δq
其中 J ( q ) = ∂ x / ∂ q J(q)=\partial x/\partial q J(q)=∂x/∂q 是末端位姿误差对关节角的雅可比矩阵,用中心差分近似:
J i j ≈ x i ( q + δ e j ) − x i ( q − δ e j ) 2 δ J_{ij} \approx \frac{x_i(q+\delta e_j)-x_i(q-\delta e_j)}{2\delta} Jij≈2δxi(q+δej)−xi(q−δej)
λ > 0 \lambda>0 λ>0 为阻尼系数(代码中 ik_damping 默认 0.03),保证在奇异位形附近 J J T JJ^T JJT 接近奇异时仍能稳定求逆。迭代直到位置误差小于 ik_epsilon(默认 10 − 4 10^{-4} 10−4 m)或达到最大迭代次数(默认 200)。求解 6 × 6 6\times6 6×6 线性方程组用的是带部分主元的高斯消元,整个求解器文件内自包含。
三、机械臂构型模型 D-H 参数法
串联机械臂由若干连杆经关节依次连接而成。要对它做运动学分析,第一步是用一套统一规则为每根连杆建立坐标系,并用最少的参数把相邻连杆坐标系的关系确定下来。常用的是D-H 参数法(Denavit-Hartenberg Parameterization),它用 4 个参数描述任意相邻两连杆之间的相对位姿。
3.1 D-H 参数的定义
给每个连杆固连一个坐标系 { i } \{i\} {i},相邻坐标系 { i − 1 } \{i-1\} {i−1} 与 { i } \{i\} {i} 之间的相对位姿可由 4 个参数确定:
- 连杆长度 a i a_i ai:沿 x i x_i xi 轴,从 z i − 1 z_{i-1} zi−1 轴到 z i z_i zi 轴的距离;
- 连杆扭角 α i \alpha_i αi:绕 x i x_i xi 轴,从 z i − 1 z_{i-1} zi−1 轴转到 z i z_i zi 轴的角度;
- 连杆偏距 d i d_i di:沿 z i − 1 z_{i-1} zi−1 轴,从 x i − 1 x_{i-1} xi−1 轴到 x i x_i xi 轴的距离;
- 关节角 θ i \theta_i θi:绕 z i − 1 z_{i-1} zi−1 轴,从 x i − 1 x_{i-1} xi−1 轴转到 x i x_i xi 轴的角度。
对于转动关节, θ i \theta_i θi 是关节变量,其余 3 个参数为常量;对于移动关节则相反。UR20 的 6 个关节全部为转动关节,因此每行只有 θ i \theta_i θi 随控制指令变化。
D-H 参数法有两种常见约定:
- 标准 D-H(Classic D-H) :连杆 i i i 的坐标系固连在连杆 i i i 的远端, z i − 1 z_{i-1} zi−1 轴沿关节 i i i 的轴线;
- 改进 D-H(Modified D-H,Craig 表示法) :连杆 i i i 的坐标系固连在连杆 i i i 的近端(即关节 i i i 处), z i z_i zi 轴沿关节 i i i 的轴线, a i − 1 a_{i-1} ai−1、 α i − 1 \alpha_{i-1} αi−1 的下标相应前移。
两者得到的末端位姿完全等价,只是坐标系原点的选取不同。UR 系列机器人官方文档采用改进 D-H 约定,本项目亦沿用该约定。
3.2 连杆坐标系与齐次变换
采用标准 D-H 时,从 { i − 1 } \{i-1\} {i−1} 到 { i } \{i\} {i} 的齐次变换按"绕 z z z 转、沿 z z z 移、沿 x x x 移、绕 x x x 转"的顺序复合:
i − 1 T i = Rot ( z i − 1 , θ i ) Trans ( z i − 1 , d i ) Trans ( x i , a i ) Rot ( x i , α i ) {}^{i-1}T_i = \operatorname{Rot}(z_{i-1},\theta_i)\ \operatorname{Trans}(z_{i-1},d_i)\ \operatorname{Trans}(x_i,a_i)\ \operatorname{Rot}(x_i,\alpha_i) i−1Ti=Rot(zi−1,θi) Trans(zi−1,di) Trans(xi,ai) Rot(xi,αi)
展开为矩阵:
i − 1 T i = cos θ i − sin θ i cos α i sin θ i sin α i a i cos θ i sin θ i cos θ i cos α i − cos θ i sin α i a i sin θ i 0 sin α i cos α i d i 0 0 0 1 {}^{i-1}T_i=\begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i\\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i\\ 0 & \sin\alpha_i & \cos\alpha_i & d_i\\ 0 & 0 & 0 & 1 \end{bmatrix} i−1Ti= cosθisinθi00−sinθicosαicosθicosαisinαi0sinθisinαi−cosθisinαicosαi0aicosθiaisinθidi1
改进 D-H 把平移与旋转的顺序调整为"绕 x x x 转、沿 x x x 移、绕 z z z 转、沿 z z z 移":
i − 1 T i = Rot ( x i − 1 , α i − 1 ) Trans ( x i − 1 , a i − 1 ) Rot ( z i , θ i ) Trans ( z i , d i ) {}^{i-1}T_i = \operatorname{Rot}(x_{i-1},\alpha_{i-1})\ \operatorname{Trans}(x_{i-1},a_{i-1})\ \operatorname{Rot}(z_i,\theta_i)\ \operatorname{Trans}(z_i,d_i) i−1Ti=Rot(xi−1,αi−1) Trans(xi−1,ai−1) Rot(zi,θi) Trans(zi,di)
i − 1 T i = cos θ i − sin θ i 0 a i − 1 sin θ i cos α i − 1 cos θ i cos α i − 1 − sin α i − 1 − d i sin α i − 1 sin θ i sin α i − 1 cos θ i sin α i − 1 cos α i − 1 d i cos α i − 1 0 0 0 1 {}^{i-1}T_i=\begin{bmatrix} \cos\theta_i & -\sin\theta_i & 0 & a_{i-1}\\ \sin\theta_i\cos\alpha_{i-1} & \cos\theta_i\cos\alpha_{i-1} & -\sin\alpha_{i-1} & -d_i\sin\alpha_{i-1}\\ \sin\theta_i\sin\alpha_{i-1} & \cos\theta_i\sin\alpha_{i-1} & \cos\alpha_{i-1} & d_i\cos\alpha_{i-1}\\ 0 & 0 & 0 & 1 \end{bmatrix} i−1Ti= cosθisinθicosαi−1sinθisinαi−10−sinθicosθicosαi−1cosθisinαi−100−sinαi−1cosαi−10ai−1−disinαi−1dicosαi−11
把 6 个连杆变换依次左乘,即得末端坐标系相对基坐标系的位姿:
0 T 6 = 0 T 1 1 T 2 2 T 3 3 T 4 4 T 5 5 T 6 {}^{0}T_{6} = {}^{0}T_1\,{}^{1}T_2\,{}^{2}T_3\,{}^{3}T_4\,{}^{4}T_5\,{}^{5}T_6 0T6=0T11T22T33T44T55T6
这正是正运动学中 A 1 A 2 ⋯ A 6 A_1A_2\cdots A_6 A1A2⋯A6 的具体形式------每个 A i A_i Ai 就是由 D-H 参数构成的连杆变换矩阵。
3.3 UR20 的连杆参数
UR20 的连杆几何定义在 arm_description/config/ur20/default_kinematics.yaml 中。该文件用每根连杆相对父连杆的平移 ( x , y , z ) (x,y,z) (x,y,z) 和旋转 ( r o l l , p i t c h , y a w ) (roll,pitch,yaw) (roll,pitch,yaw) 描述构型,这些量正是 D-H 参数的具体数值。整理成改进 D-H 参数表如下(长度单位 m,角度单位 rad):
| 连杆 i i i | 对应关节 | a i − 1 a_{i-1} ai−1 | α i − 1 \alpha_{i-1} αi−1 | d i d_i di | 关节变量 θ i \theta_i θi |
|---|---|---|---|---|---|
| 1 | shoulder_pan | 0 | 0 | 0.2363 | q 1 q_1 q1 |
| 2 | shoulder_lift | 0 | + π / 2 +\pi/2 +π/2 | 0 | q 2 q_2 q2 |
| 3 | elbow | 0.862 | 0 | 0 | q 3 q_3 q3 |
| 4 | wrist_1 | 0.7287 | 0 | 0.201 | q 4 q_4 q4 |
| 5 | wrist_2 | 0 | + π / 2 +\pi/2 +π/2 | 0.1593 | q 5 q_5 q5 |
| 6 | wrist_3 | 0 | − π / 2 -\pi/2 −π/2 | 0.1543 | q 6 q_6 q6 |
其中 0.2363 是肩部相对底座的高度(yaml 中 shoulder.z),0.862 与 0.7287 分别是大臂和小臂长度(对应 forearm.x、wrist_1.x 的绝对值),0.201/0.1593/0.1543 是腕部三段偏距。大臂与小臂长度之和加上腕部偏距,决定了 UR20 约 1.75 m 的名义可达半径,与业务节点中 max_reach 默认值一致。
需要说明:不同资料对 a a a 的符号和下标约定可能不同(有些资料把大臂、小臂长度写成负值),表中数值依据本项目 URDF 的坐标系方向给出,具体应以 default_kinematics.yaml 和 urdf/inc/ur20_macro.xacro 为准。
3.4 与 URDF 建模的对应关系
D-H 参数法是"手工"建立运动学模型的经典手段,而本项目采用 URDF 描述机械臂,两者本质相通但表达方式不同:
- URDF 用连杆
<origin xyz="..." rpy="..."/>标签直接写出相邻连杆的平移与旋转,相当于把 D-H 的齐次变换矩阵显式参数化,但不受"关节轴必须取作 z z z 轴"等 D-H 规则的约束,因而更通用; robot_kinematics.cpp从/robot_description解析出连杆链,把每个<origin>转成齐次变换(由四元数构造旋转矩阵),再按父子顺序复合得到 0 T 6 {}^{0}T_6 0T6,其效果与 D-H 链式相乘完全一致;- 逆运动学同样建立在这套连杆几何之上:数值解直接对正运动学求雅可比,无需像解析解那样依赖特定构型的 D-H 推导。这正是 选择阻尼最小二乘数值法的原因------换一款机械臂只需换 URDF,无需重写 IK 公式。
四、对外接口
position_controllers/JointTrajectoryController 提供 action 与话题两种下发方式,joint_state_controller 负责发布关节状态。
4.1 输入接口
-
~/follow_joint_trajectory(control_msgs/FollowJointTrajectory)动作接口,接收带时间戳的多点关节轨迹,控制器按时间插值并跟踪。业务节点就是通过它下发轨迹,可对接 MoveIt、
rqt_joint_trajectory_controller或自定义 action client。 -
~/command(trajectory_msgs/JointTrajectory)话题接口,直接发布一条关节轨迹,无需 action 客户端。
作为对比,备选控制器 arm_position_controller(position_controllers/JointGroupPositionController)的输入是 ~/command(std_msgs/Float64MultiArray),一次性下发 6 个关节角。
4.2 输出接口
-
~/state(control_msgs/JointTrajectoryControllerState)控制器跟踪状态,包含期望/实际的关节位置、速度与误差。
-
/joint_states(sensor_msgs/JointState)由
joint_state_controller以 50 Hz 发布,robot_state_publisher据此计算并广播各连杆 TF,供 RViz 显示。
五、控制器参数
JointTrajectoryController 常用参数如下(完整列表见 wiki):
| 参数 | 数据类型 | 说明 |
|---|---|---|
| type | string | 控制器类型,此处为 position_controllers/JointTrajectoryController |
| joints | string\[\] | 控制器管理的关节名列表,必须与 URDF 一致 |
| constraints/goal_time | double | 轨迹结束后的宽限时间(s),在此期间仍可修正终点误差 |
| constraints/stopped_velocity_tolerance | double | 判断关节已停止的速度阈值(rad/s) |
| constraints/<joint>/trajectory | double | 轨迹跟踪误差容差(rad) |
| constraints/<joint>/goal | double | 终点位置误差容差(rad) |
| stop_trajectory_duration | double | 紧急停止时轨迹收敛到停止点的时间(s,默认 0.5) |
| state_publish_rate | double | 状态发布频率(Hz,默认 50) |
| action_monitor_rate | double | action 目标监控频率(Hz,默认 20) |
六、配置控制器参数
arm_control/config/arm_control.yaml 同时给出了硬件接口参数、关节限位和两种控制器配置。
最小配置(仅轨迹控制器):
yaml
arm_trajectory_controller:
type: "position_controllers/JointTrajectoryController"
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
完整配置中,硬件接口参数与关节限位被控制器和硬件抽象层共用:
yaml
hardware_interface:
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
# 上电初始位姿,顺序与 joints 一致
initial_positions: [0.0, -1.5708, 1.5708, -1.5708, 0.0, 0.0]
joint_limits:
shoulder_pan_joint:
has_position_limits: true
min_position: -6.283185307
max_position: 6.283185307
has_velocity_limits: true
max_velocity: 2.094395102
has_effort_limits: true
max_effort: 730.0
# ... 其余 5 个关节类似
joint_state_controller:
type: "joint_state_controller/JointStateController"
publish_rate: 50
arm_trajectory_controller:
type: "position_controllers/JointTrajectoryController"
joints: # 同上 6 个关节
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.20, goal: 0.01}
# ... 其余关节
stop_trajectory_duration: 0.5
state_publish_rate: 50
action_monitor_rate: 20
arm_position_controller:
type: "position_controllers/JointGroupPositionController"
joints: # 同上 6 个关节
注意 initial_positions 让机械臂上电停在"折起来"的姿态([0, -1.5708, 1.5708, -1.5708, 0, 0]),便于在 RViz 中观察;joint_limits 同时供控制器限位和硬件接口 write() 中的位置钳位使用,两者数值保持一致。两个控制器占用同一批关节,不能同时启动,由 launch 文件的 controller 参数二选一。
七、编写硬件抽象接口
与差速底盘使用 VelocityJointInterface 不同,机械臂使用 PositionJointInterface。头文件 arm_hardware_interface.h:
cpp
class ArmHWInterface : public hardware_interface::RobotHW
{
public:
// 单个关节的全部状态与指令,接口句柄直接持有这些成员的地址
struct JointInfo
{
std::string name; // 关节名,必须与 URDF / 配置中的名字一致
double cmd; // 控制器写入的位置指令 (rad)
double pos; // 反馈位置 (rad)
double vel; // 反馈速度 (rad/s)
double eff; // 反馈力矩 (N*m)
double min_pos; // 位置下限 (rad)
double max_pos; // 位置上限 (rad)
};
explicit ArmHWInterface(ros::NodeHandle &nh);
bool init(); // 读取参数并注册硬件接口
void read(const ros::Duration &period); // 从硬件读状态(仿真时为空)
void write(const ros::Duration &period); // 下发指令并推进仿真状态
private:
ros::NodeHandle m_nh;
hardware_interface::JointStateInterface m_jnt_state_interface;
hardware_interface::PositionJointInterface m_jnt_pos_interface;
// 注册句柄时保存了 JointInfo 成员的地址,此后不得再改变 vector 容量
std::vector<JointInfo> m_joints;
};
源文件 arm_hardware_interface.cpp 中,init() 读取参数、注册接口,read()/write() 构成仿真闭环:
cpp
bool ArmHWInterface::init()
{
// 1. 关节列表:优先用参数服务器配置,缺失时回退到 UR20 默认 6 关节
std::vector<std::string> joint_names;
if (!m_nh.getParam("hardware_interface/joints", joint_names) || joint_names.empty())
joint_names.assign(kDefaultJoints, kDefaultJoints + kDefaultJointCount);
// 一次性填满 m_joints:注册句柄之后不能再扩容,否则句柄里的指针会失效
m_joints.clear();
m_joints.reserve(joint_names.size());
for (std::size_t i = 0; i < joint_names.size(); ++i)
m_joints.push_back(JointInfo(joint_names[i]));
// 2. 初始位姿 + 3. 关节限位(write 中做钳位)
std::vector<double> initial_positions;
if (m_nh.getParam("hardware_interface/initial_positions", initial_positions) &&
initial_positions.size() == m_joints.size())
{
for (std::size_t i = 0; i < m_joints.size(); ++i)
{
m_joints[i].pos = initial_positions[i];
m_joints[i].cmd = initial_positions[i];
}
}
for (std::size_t i = 0; i < m_joints.size(); ++i)
{
JointInfo &joint = m_joints[i];
const std::string base = "joint_limits/" + joint.name;
m_nh.getParam(base + "/min_position", joint.min_pos);
m_nh.getParam(base + "/max_position", joint.max_pos);
joint.pos = std::max(joint.min_pos, std::min(joint.pos, joint.max_pos));
joint.cmd = joint.pos;
}
// 4. 注册 ros_control 接口:每个关节既要能读状态,也要能接受位置指令
for (std::size_t i = 0; i < m_joints.size(); ++i)
{
JointInfo &joint = m_joints[i];
hardware_interface::JointStateHandle state_handle(
joint.name, &joint.pos, &joint.vel, &joint.eff);
m_jnt_state_interface.registerHandle(state_handle);
hardware_interface::JointHandle pos_handle(
m_jnt_state_interface.getHandle(joint.name), &joint.cmd);
m_jnt_pos_interface.registerHandle(pos_handle);
}
registerInterface(&m_jnt_state_interface);
registerInterface(&m_jnt_pos_interface);
return true;
}
void ArmHWInterface::read(const ros::Duration & /*period*/)
{
// 接真实机器人时,在这里读编码器/驱动器,刷新 pos、vel、eff。
// 本示例状态由 write() 同步推进,控制器读到的是上一周期的状态,
// 这与真实系统存在一拍采样延迟的情况一致。
}
void ArmHWInterface::write(const ros::Duration &period)
{
const double dt = period.toSec();
for (std::size_t i = 0; i < m_joints.size(); ++i)
{
JointInfo &joint = m_joints[i];
// 位置限幅,模拟驱动器对越界指令的保护
const double target = std::max(joint.min_pos, std::min(joint.cmd, joint.max_pos));
joint.cmd = target;
// 理想位置执行器:指令即反馈,速度由位置差分得到,无负载时力矩为 0
if (dt > 0.0)
joint.vel = (target - joint.pos) / dt;
joint.pos = target;
joint.eff = 0.0;
}
}
控制节点 arm_control_node.cpp 按 read() → cm.update() → write() 的顺序循环,频率可配(默认 100 Hz):
cpp
int main(int argc, char **argv)
{
ros::init(argc, argv, "arm_control_node");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
double control_rate = 100.0;
private_nh.param("control_rate", control_rate, 100.0);
arm_control::ArmHWInterface arm(nh);
if (!arm.init())
{
ROS_FATAL("Failed to initialize the arm hardware interface.");
return 1;
}
controller_manager::ControllerManager cm(&arm, nh);
// 轨迹控制器的 action server、controller_manager 的 service 需要回调线程
ros::AsyncSpinner spinner(2);
spinner.start();
ros::Rate rate(control_rate);
ros::Time last_time = ros::Time::now();
while (ros::ok())
{
const ros::Time now = ros::Time::now();
ros::Duration period = now - last_time;
last_time = now;
// 首帧或使用仿真时间时 period 可能为 0,退回名义周期
if (period.toSec() <= 0.0 || period.toSec() > 1.0)
period = rate.expectedCycleTime();
arm.read(period);
cm.update(now, period);
arm.write(period);
rate.sleep();
}
return 0;
}
这里没有真实驱动器:write() 用"理想位置执行器"把控制器下发的位置指令直接当作编码器反馈,并由位置差分得到速度。接真实机器人时只需把 read() 改为读编码器、write() 改为下发总线指令,控制器侧无需改动。另一个细节是句柄持有 JointInfo 成员的地址,所以 init() 中先 reserve() 一次性填满 m_joints,注册句柄后不能再扩容。
八、控制机械臂运动
启动方式:
bash
# 默认:轨迹控制器(action / 话题接口,可对接 MoveIt)
roslaunch arm_control arm_control.launch
# 备选:一组关节角度控制器(命令行调试最方便)
roslaunch arm_control arm_control.launch controller:=arm_position_controller
rostopic pub -1 /arm_position_controller/command std_msgs/Float64MultiArray \
"data: [0.0, -1.57, 1.57, -1.57, 0.0, 0.0]"
轨迹模式下,最简单的是直接用话题下发一条单点轨迹:
bash
rostopic pub -1 /arm_trajectory_controller/command trajectory_msgs/JointTrajectory \
"{joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint], points: [{positions: [0.5, -1.2, 1.2, -1.0, 0.0, 0.0], time_from_start: {secs: 2, nsecs: 0}}]}"
业务节点 arm_business 演示了完整的"任务空间 → 关节空间"流程:从 /robot_description 构建运动链,以启动时末端位姿为参考,在可达球内随机生成 8 个到肩关节距离互不相同的目标点,逐点数值逆解并检查整条臂是否触地,最后通过 follow_joint_trajectory action 依次下发轨迹,让末端遍历这些点。关键代码如下:
cpp
// 两段式轨迹:起点取当前位形,保证与上一条轨迹连续
control_msgs::FollowJointTrajectoryGoal goal;
goal.trajectory.joint_names = joint_names_;
trajectory_msgs::JointTrajectoryPoint start_point;
start_point.positions = q_prev; // 上一条轨迹的终点
start_point.velocities.assign(dof_, 0.0);
start_point.time_from_start = ros::Duration(0.0);
goal.trajectory.points.push_back(start_point);
trajectory_msgs::JointTrajectoryPoint end_point;
end_point.positions = q_goal; // 逆解得到的关节角
end_point.velocities.assign(dof_, 0.0);
end_point.time_from_start = ros::Duration(traverse_time_);
goal.trajectory.points.push_back(end_point);
随机点与逆解的核心逻辑:
cpp
// 分层半径:第 i 个半径落在互不相交的区间,天然两两不同
const double r = max_radius * (frac_lo + band * (static_cast<double>(i) + unit(rng)));
...
if (!kinematics_.solveInverseKinematics(cur_ref, trial[i], q_trial,
ik_epsilon_, ik_max_iterations_, ik_damping_, ...))
all_ok = false;
// 地面碰撞检查:整条臂各连杆端点必须高于 ground_clearance_
if (kinematics_.armMinZ(q_trial) < ground_clearance_)
all_ok = false;
运行:
bash
roslaunch business_test arm_business.launch
# 常用可调参数
roslaunch business_test arm_business.launch \
max_reach:=1.75 traverse_time:=1.0 hold_time:=0.2 loops:=1 rviz:=false
traverse_time 控制相邻点的运动时间(越小越快),hold_time 为到点停留时间,loops:=0 表示无限循环,orientation_mode 可选保持启动姿态或让末端 Z 轴沿可达球半径指向外侧。RViz 中以黄色圆点标出这 8 个遍历目标点。
