简介这是一份面向机械工程、自动化、计算机方向本科生毕业设计的完整源码与答辩材料包聚焦混合力位控制策略在机械臂末端精细装配、人机交互等场景下的力与速度规划、模型不确定性补偿与仿真验证。资源共69个文件以C/C源文件为主h/cpp/hpp共57个并配有CoppeliaSim客户端控制代码、算法说明、实验录屏、答辩PPT与README文档压缩包约19.83MB目录按论文研究概述、算法设计、控制执行、仿真实验四层组织便于对照学习目前已有451人学习使用。内含机械臂力和速度规划、混合力位控制执行、仿真实验与总结的完整思路可支撑毕业设计课题复现。对于研究力控机械臂或机器人柔顺控制的读者这份资源也能提供清晰的代码级参考。1. 混合力位控制与机械臂末端精细操作毕业设计要攻克的核心关卡“机械臂末端精细操作”这类题目最常被答辩老师问的一句话是位置控制都做得挺好了为什么还要加力答案就藏在“接触”这两个字里。插孔、打磨、贴附、拧螺丝末端一旦和环境发生接触位置控制的误差就会被工件几何当场放大z轴设定偏差0.1毫米接触力就可能从0跳到几十牛顿轻则划伤工件重则报警停机。混合力位控制的做法是把笛卡尔空间的任务自由度拆成两组——该控位置的方向继续由位置环管该控力的方向交给力闭环用一个选择矩阵把两条控制律合到同一个末端输出上。下面顺着数学模型、C/C工程骨架、仿真验证、参数调优到答辩PPT组织这条路径讲下去。2. 先弄懂混合力位控制的数学模型与力觉反馈闭环2.1 为什么纯位置控制解决不了接触问题任务空间的约束拆解一个末端执行器贴着平面做磨抛在表面的法线方向工件是一个高度约束的曲面在切平面方向末端又能自由滑动。用纯位置控制六个自由度全被当成位置环那接触力只能靠机器人本体刚度和工件形变被动决定完全没有办法给定一个“期望力”。反过来六个自由度全做力控末端在切平面方向又因为没有位置基准而漂移轨迹就丢了。混合力位控制最底层的思路是把任务空间一分为二法线方向做力闭环切平面方向做位置闭环两者在不同维度上并行互不冲突。这个“拆自由度”的数学表达就是选择矩阵 S。S 是一个 6×6 对角阵对角线为 1 表示该自由度采用位置控制为 0 表示该自由度采用力控制。注意这里的维度是笛卡尔空间的 6 个自由度3 个平移 3 个旋转不是关节空间的六个轴。关节空间的角度和力是经过运动学映射之后的另一层故事混合控制的选择矩阵必须定义在任务空间才有意义。2.2 雅可比矩阵与静力映射末端力到关节力矩的换算混合控制策略算出来的是作用在末端笛卡尔坐标系上的广义力 F_task包括三个力分量和三个力矩分量。关节执行器要输出的是关节力矩两者之间靠雅可比矩阵的转置建立关系tau J^T * F_task这个式子物理上叫静力映射代表“如果末端顶着环境产生 F_task每个关节需要出多少力矩”。因为毕业设计用的机械臂一般是六轴串联结构雅可比矩阵解析式逐列写出来很长我习惯在工程里边递推做正运动学边填列// computeJacobian.cpp —— 基于DH递推构造几何雅可比矩阵 Eigen::MatrixXd computeJacobian(const Eigen::VectorXd q, const std::vectorEigen::Matrix4d jointTransforms, int dof) { Eigen::MatrixXd J(6, dof); Eigen::Matrix4d T Eigen::Matrix4d::Identity(); Eigen::Vector3d p_end fk(q).topRightCorner3, 1(); // 末端位置 for (int i 0; i dof; i) { Eigen::Vector3d z_i T.block3, 1(0, 2); // 关节转轴方向 Eigen::Vector3d p_i T.topRightCorner3, 1(); // 关节坐标系原点 J.block3, 1(0, i) z_i.cross(p_end - p_i); // 平动列z_i×(p_end-p_i) J.block3, 1(3, i) z_i; // 转动列就是z_i T T * jointTransforms[i]; // 递推到下一关节 } return J; }平动部分每一列是当前关节转轴对末端位置的叉乘转动部分直接就是关节转轴方向在基坐标系下的投影。这些量查 DH 表乘一遍就能得到不复杂但写错符号会导致力控发散。调试时先用单位关节速度驱动机器人对照末端速度实测值校一遍雅可比再进力控循环。2.3 选择矩阵与任务空间分解自由度怎么分才不打架选择矩阵的构造要跟任务坐标系的定义一起做。S 为 1 的是位置自由度S 为 0 的是力自由度。以沿 z 轴做力控磨抛为例// 6x6 选择矩阵, 1: 位置控制, 0: 力控制 Eigen::Matrix6d S Eigen::Matrix6d::Identity(); S(2, 2) 0.0; // z 方向: 力控制, 让末端按恒定压力压住工件 S(5, 5) 1.0; // 绕 z 轴仍做位置控制, 防止磨头转动漂移这里最容易犯错的是“坐标系的朝向”。如果末端姿态在工作过程中发生变化基坐标系下的 z 轴和工件表面的法线方向不再重合选择矩阵里你写的那个 z 还是基坐标系的 z控制对象就错了。所以工程里一般先在末端工具坐标系或工件坐标系底下算混合控制律再通过旋转矩阵把结果变换到基坐标系。如果你的任务里末端姿态变化不大可以适当简化但需要在论文里写明假设边界。2.4 混合力位控制 vs 阻抗控制毕设选型的判断依据答辩时老师经常会追问“你为什么不直接用阻抗控制”。阻抗控制不是直接控力而是调控末端位置偏差和接触力之间的关系把力误差变成位置修正。这两种方案各有定位差异直接对比一张表说清楚对比维度混合力位控制阻抗控制控制目标在指定方向直接控制接触力调节力与位置之间的动态阻抗关系力传感器依赖强需要实时六维力反馈可用关节力矩估算不强依赖末端力传感器与环境刚度适配对环境刚度变化敏感需要标定天然在阻抗模型内吸收刚度变化毕业设计展示动作插孔、贴面、拧螺丝直观好讲适合柔顺拖拽、人机交互场景推导和实现难度中高需要对坐标变换谨慎中等调参数维度也不少混合力位控制在教学上讲得通验证起来有明确的“力误差曲线”指标论文图表好看所以本科毕设里选它的人多。阻抗控制则适合强调“柔顺性”而不是恒定力的任务。3. 用 C/C 实现混合力位控制的完整工程框架3.1 工程目录与构建配置从 vscode 配置 c/c 环境到 CMake 落地拿到源码压缩包以后第一步不是急着读控制算法而是把工程构建起来。常见做法是用 CMake 组织依赖源码目录大概长这样hybrid_pos_force/ ├── CMakeLists.txt ├── include/ │ ├── controller/ │ │ ├── hybrid_controller.h │ │ ├── force_filter.h │ │ └── trajectory.h │ └── robot/ │ ├── kinematics.h │ └── robot_model.h ├── src/ │ ├── main.cpp │ ├── controller/ │ │ ├── hybrid_controller.cpp │ │ └── force_filter.cpp │ └── robot/ │ ├── kinematics.cpp │ └── robot_model.cpp ├── config/ │ ├── controller_params.yaml │ └── robot_params.yaml └── logs/CMakeLists.txt 里最少的配置这么写cmake_minimum_required(VERSION 3.16) project(hybrid_pos_force CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Eigen 做矩阵运算, 是机械臂控制代码的标准配置 find_package(Eigen3 REQUIRED) add_executable(hybrid_controller src/main.cpp src/controller/hybrid_controller.cpp src/controller/force_filter.cpp src/robot/kinematics.cpp ) target_link_libraries(hybrid_controller Eigen3::Eigen)如果你在 Windows 上用 VS Code 开发vscode 配置 c/c 环境时需要注意两点一是 include 路径要加上 Eigen 的安装目录否则智能提示和编译全飘红二是 launch.json 里把工作目录指到 logs 目录这样日志文件路径不会到处乱跑。3.2 主控制循环力位混合控制的 C 实现控制周期一般固定为 1kHz 到 250Hz 之间。仿真里开 1kHz实机六自由度机械臂根据驱动器通信周期常见 500Hz 到 1kHz 都能跑。主循环的骨架如下这里把数学上的两条控制律都写出来了// main_control_loop.cpp —— 单周期混合力位控制 while (running) { // 1. 读关节状态 readJointState(q_meas, dq_meas); // 2. 读末端六维力传感器原始数据 readForceSensor(F_raw); // 3. 力信号滤波, 去掉传感器噪声 F_filtered filter.update(F_raw); // 4. 正运动学, 求当前末端位姿和雅可比 T_cur forwardKinematics(q_meas); J computeJacobian(q_meas, joint_transforms, dof); // 末端笛卡尔速度 雅可比 * 关节速度 dT_cur J * dq_meas; // 5. 轨迹规划器给出期望位姿 xd trajectory.getDesiredPose(cycle_time); // 6. 混合控制律: 位置自由度 PD, 力自由度 PI Eigen::Vector6d F_task Eigen::Vector6d::Zero(); for (int i 0; i 6; i) { if (S(i, i) 1.0) { // 位置控制方向: PD double pos_err xd(i) - T_cur(i); double vel_err -dT_cur(i); F_task(i) Kp(i) * pos_err Kd(i) * vel_err; } else { // 力控制方向: 期望力 PI 修正 double f_err Fd(i) - F_filtered(i); force_integral(i) f_err * dt; F_task(i) Fd(i) Kfp(i) * f_err Kfi(i) * force_integral(i); } } // 7. 由静力映射得到关节力矩 Eigen::Vector6d tau J.transpose() * F_task; // 8. 下发关节力矩指令 sendJointTorque(tau); waitForNextCycle(cycle_time); }位置方向用的是 PD力方向用 PI力误差含有积分项是因为接触力系统里经常存在摩擦和重力偏置纯比例控制在恒定外载作用下会留一个静差。需要说明的是第 6 步算出的 F_task是作用在末端坐标系原点、由任务坐标系表达的广义力。如果你把 S 定义在工具坐标系下那么 F_task 也要先在工具坐标系里合成再在使用基坐标系的 J 转置之前做一次坐标旋转。实机上如果伺服驱动器不开放力矩模式常见的替换做法是把 tau 的一部分作为前馈力矩另一部分折算成关节位置修正量交给底层位置环效果接近而工程上更稳妥。3.3 力传感器数据读取与滤波参数怎么给六维力传感器直接串口或者 EtherCAT 读出来的数据静态时噪声可能在满量程的 0.1% 到 0.5% 之间动态时还可能带振动谐波。力控对噪声相当敏感噪声一大部分直接进了控制律末端就会抖。我一般先做滑动平均代码非常短// force_filter.cpp —— 窗口滑动平均滤波 template int N Eigen::Matrixdouble, 6, 1 movingAverage( const Eigen::Matrixdouble, 6, 1 raw, std::dequeEigen::Matrixdouble, 6, 1 window) { window.push_back(raw); if (static_castint(window.size()) N) { window.pop_front(); } Eigen::Matrixdouble, 6, 1 sum Eigen::Matrixdouble, 6, 1::Zero(); for (auto v : window) sum v; return sum / window.size(); }注意窗口大小会直接带来相位延迟。窗口越大噪声压得越死、但力信号滞后越明显力阶梯响应会变得迟钝。毕业设计里调的时候按这组经验值起步滤波方式推荐参数为什么滑动平均N 5 ~ 15简单直观适合采样率 1kHz 的力信号一阶低通截止频率 20 ~ 100 Hz相位滞后更小力控响应更快高阶巴特沃斯不推荐一上来就用群延迟大旁瓣会把力信号拉出冲击调用滤波之后记得在“空载、不接触”的状态下做一次零偏校验把空载读数存进标定文件。很多毕设实机最后力控不准首先死在零漂上。3.4 机械臂轨迹规划算法在混合控制里的角色混合力位控制里位置方向需要一个参考轨迹力方向虽然没有位置轨迹但力期望值本身也要按工况做斜坡规划。最常用的还是梯形速度规划把目标位移拆成加速段、匀速段、减速段。控制循环里每个节拍调用 getDesiredPose(t) 时如果当前时间点落在什么速度段返回对应的位置、速度和姿态。姿态部分用到四元数球面插值slerp比欧拉角平滑也容易转成旋转矩阵去驱动雅可比。有一点要特别提醒轨迹规划里的期望速度也必须参与位置方向的 PD 控制律。如果不把期望速度写进前馈只靠 Kd 阻尼项机械臂在高速段会有明显跟随滞后末端轨迹会偏力控方向还好说位置方向的表现答辩演示时很难看。4. 机械臂选型、仿真环境与 ROS 机械臂开发接线4.1 机械臂选型预算和力反馈来源怎么平衡毕设题目里如果只是验证控制算法强烈建议从 Gazebo 仿真出发。仿真环境里力传感器是现成的不需要花钱买硬件还能随意改变工件刚度、接触面摩擦系数数据重复性远比实机好。实机制作一般用带总线舵机的六自由度机械臂例如常见套件舵机电流可以直接映射扭矩力反馈精度虽然一般但做“沿平面保持恒定压力”这类展示足够了。方案典型硬件力反馈来源预算量级适合展示的任务纯仿真Ubuntu ROS Noetic GazeboUR10/Panda 模型Gazebo 接触力插件0插孔、平面恒力追踪、轨迹 力曲线低成本实机总线舵机 6 轴机械臂 主轴舵机电流折算关节力矩1000 ~ 3000沿木板打磨求恒力展示末端力数据中阶实机六轴机械臂 国产六维力传感器末端六维力采集卡数万高精度插孔、表面跟踪选型时的关键原则优先保证力反馈数据可记录、可画图其次才追求结构精度。答辩需要的是“我记录了力误差曲线、验证了算法”不是“这块板磨得多光”。4.2 在 Gazebo 中把机械臂模型和接触环境搭起来常见做法是用 ROS 里现成的描述包。以 panda 机械臂为例启动命令一般是这样# 启动 Gazebo 仿真环境 roslaunch panda_gazebo panda_world.launch # 加载控制器 roslaunch panda_control panda_control.launch # 发布一组关节角度目标, 验证整条链路通不通 rostopic pub /panda/joint_command sensor_msgs/JointState \ {position: [0, -0.785, 0, -2.356, 0, 1.571, 0.785]} --rate 10发布之前先rostopic echo -n1 /panda/force_torque看一眼接触力话题是不是在输出然后再让它动。这里关键的坑是混合力位控制需要的是末端力/力矩不是关节力矩。话题名字里带 force_torque 的就是目标数据里指的是基坐标系还是工具坐标系要查描述文件搞反了力控方向会立刻正反馈。4.3 把 C/C 控制算法封装成 ROS 节点仿真环境里跑算法最规范的方式是把第 3 章写的那套 controller 类包进一个 ROS 节点力从订阅话题取算完的关节指令再发到控制器话题// hybrid_ros_node.cpp —— 控制算法的 ROS 封装骨架 class HybridControlNode { public: HybridControlNode(ros::NodeHandle nh) { // 订阅末端六维力 force_sub_ nh.subscribe(/panda/force_torque, 1, HybridControlNode::forceCallback, this); // 发布关节力矩指令 torque_pub_ nh.advertisesensor_msgs::JointState( /panda/joint_command, 1); // 固定周期跑控制循环 timer_ nh.createTimer(ros::Duration(0.001), HybridControlNode::controlLoop, this); } private: void forceCallback(const geometry_msgs::WrenchStamped::ConstPtr msg) { F_raw_ msg-wrench.force.x, msg-wrench.force.y, msg-wrench.force.z, msg-wrench.torque.x, msg-wrench.torque.y, msg-wrench.torque.z; } void controlLoop(const ros::TimerEvent) { // 读关节状态 - 算 tau - 发布 sensor_msgs::JointState cmd; cmd.effort tau_vec_; torque_pub_.publish(cmd); } };这套结构里控制器类完全不知道自己在 ROS 里还是在实机里跑接口只管输入状态和力、输出关节指令之后换电机驱动或者换通信协议都只要改最外层的数据适配层。这就是整份源码工程里最值得抄的部分。5. 调参与排错混合力位控制的参数标定与常见失败现场5.1 力控增益和位置刚度怎么配比混合力位控制最大的调参难题是两个方向的控制参数互相影响。位置刚度过大末端的微小位置误差会转换成大力矩干扰力方向力控增益过大系统容易“软塌”和抖动。所以调参顺序要先调位置再调力不要一上来两个一起改。按自由度分开调位置自由度调 Kp、Kd力自由度调 Kfp、Kfi。起始值参考这个表参数含义建议起始值调参方向Kp位置刚度80 ~ 200位置跟踪误差大就加大振荡就减小Kd位置阻尼2*sqrt(Kp) 附近振荡主要靠它压太大响应变慢Kfp力误差比例0.05 ~ 0.3力收敛慢就加大力噪声大就减小Kfi力误差积分0.01 ~ 0.1稳态力误差大就加大出现过冲就减小实际从仿真看Kp 与 Kfp 相差几十倍一点也不奇怪因为位置误差单位是米、力误差单位是牛顿量纲不同。调参时观察六条曲线位置误差曲线和力误差曲线哪条响就调哪组互不联动地独立收敛。5.2 力传感器零漂与噪声上电先做三个检查实机力控发散一半以上的原因不在算法在传感器数据。没有六维力传感器的场景先用舵机电流折算关节力矩的话电流零漂更要注意。我每次通电后按三个步骤检查机械臂空载悬停记录一分钟静态数据均值偏离零的程度就是零漂写入配置文件。用砝码或已知重物挂在末端指定位置对比读数与理论重力力矩差检查力/力矩量程方向和符号。手动低速晃动末端观察滤波后数据有没有明显滞后滞后超过 50 ms 就要把滤波窗口调小。做完这三条再进力控后面折腾的时间能砍掉大半。5.3 接触瞬间的震荡抑制力期望值斜坡怎么做末端从“未接触”切换到“已接触”的瞬间接触力会从零突变到期望值控制律来不及反应就会产生一个冲击力甚至回弹。解法是在接触发生后再把力期望值从零斜坡上升到目标值Fd(t) Fd_target * min(1.0, (t - t_contact) / T_ramp)T_ramp 一般给 0.5 到 2 秒根据工件刚度和机械臂响应速度调整。工程上判断接触时刻不靠猜测看力值是否连续几个采样点超过阈值触发接触标志位。位置方向同时从“自由运动”切到“位置保持”切换瞬间避免直接给出阶跃位置误差把当前位置保存为期望位置起点。5.4 用日志回放定位发散现场控制循环里每个周期只写几行关键量别贪多// 日志格式: 时间戳 位置误差x/y/z 力误差x/y/z 关节力矩0..5 log_stream cycle_count , pos_err.transpose() , f_err.transpose() , tau.transpose() \n;跑完之后用 Python 或者简单绘图工具把时间序列画出来第一时间就能判断是不是某一轴向的发散。常见的发散形态就三种一是力噪声高频抖动问题在滤波二是单一方向等幅振荡问题在 Kd 或 Kfp 太大三是缓慢漂移不回中问题在积分项饱和。日志能直接区分这些比看机械臂“抖成什么样”直观得多。注意实机上出现发散时第一件事不是改参数而是先做限幅和急停。所有关节力矩都要做饱和限制力控方向尤其注意不然电机直接过热报警。6. 把研究成果落进答辩 PPT图表、对比实验与演示视频的验收技巧6.1 控制框图和实验曲线怎么画才“像”答辩 PPT 里最值钱的是两页一页是混合力位控制框图一页是力误差与位置误差同步曲线。控制框图不要画成一大堆箭头乱飞严格按“期望力/期望位姿 - 选择矩阵 - 控制律 - 机械臂 - 测量”的单回路结构画。曲线图要同时叠上期望值和实测值比如力控方向画出期望力直线和实测力曲线之间的带状差异位置方向画出轨迹偏差。曲线不一定要完美但横轴时间单位、纵轴力单位必须标清楚这一页往往是老师停留最久的地方。6.2 必做的三组对比实验毕业设计答辩最少要有三组数据第一组纯位置控制沿工件表面运动展示接触力波动巨大第二组混合力位控制力误差曲线收敛并保持在一个小范围内第三组分别用不同速度或不同期望力重复说明算法稳定性。三组实验一摆工作量立竿见影。数据表结构可以这样组织实验组控制方式力误差均值位置偏差结论实验一纯位置控制正负 2.5N0.12mm接触力不可控实验二混合力位控制正负 0.4N0.18mm力跟踪有效实验三混合力位控制速度加倍正负 0.7N0.25mm稳定性可接受上表中的具体数字只是示例最终用自己实验跑出来的数据替换。答辩老师不会纠结绝对值但会盯着趋势问问题所以实验二和实验三的对比条件速度、进给量一定要在 PPT 里写明。6.3 演示视频的拍摄顺序如果有实机演示视频别剪太碎按“安装工件 - 初始化 - 接触之前 - 接触瞬间 - 稳定磨抛 - 撤出”的顺序一镜到底。重点拍接触瞬间那一刻力曲线从零跳到期望值的过程最能说明算法有效。如果只有仿真动画配上力误差曲线画在同一画面里同步播放效果会比单放动画好很多。源码包里的那份答辩 PPT常见目录结构是“问题背景 - 方案设计 - 仿真建模 - 实验验证 - 总结展望”配一张完整的混合力位控制实验曲线图就能打开场。本文还有配套的精品资源点击获取
机械臂混合力位控制：原理、C++实现与仿真验证
简介这是一份面向机械工程、自动化、计算机方向本科生毕业设计的完整源码与答辩材料包聚焦混合力位控制策略在机械臂末端精细装配、人机交互等场景下的力与速度规划、模型不确定性补偿与仿真验证。资源共69个文件以C/C源文件为主h/cpp/hpp共57个并配有CoppeliaSim客户端控制代码、算法说明、实验录屏、答辩PPT与README文档压缩包约19.83MB目录按论文研究概述、算法设计、控制执行、仿真实验四层组织便于对照学习目前已有451人学习使用。内含机械臂力和速度规划、混合力位控制执行、仿真实验与总结的完整思路可支撑毕业设计课题复现。对于研究力控机械臂或机器人柔顺控制的读者这份资源也能提供清晰的代码级参考。1. 混合力位控制与机械臂末端精细操作毕业设计要攻克的核心关卡“机械臂末端精细操作”这类题目最常被答辩老师问的一句话是位置控制都做得挺好了为什么还要加力答案就藏在“接触”这两个字里。插孔、打磨、贴附、拧螺丝末端一旦和环境发生接触位置控制的误差就会被工件几何当场放大z轴设定偏差0.1毫米接触力就可能从0跳到几十牛顿轻则划伤工件重则报警停机。混合力位控制的做法是把笛卡尔空间的任务自由度拆成两组——该控位置的方向继续由位置环管该控力的方向交给力闭环用一个选择矩阵把两条控制律合到同一个末端输出上。下面顺着数学模型、C/C工程骨架、仿真验证、参数调优到答辩PPT组织这条路径讲下去。2. 先弄懂混合力位控制的数学模型与力觉反馈闭环2.1 为什么纯位置控制解决不了接触问题任务空间的约束拆解一个末端执行器贴着平面做磨抛在表面的法线方向工件是一个高度约束的曲面在切平面方向末端又能自由滑动。用纯位置控制六个自由度全被当成位置环那接触力只能靠机器人本体刚度和工件形变被动决定完全没有办法给定一个“期望力”。反过来六个自由度全做力控末端在切平面方向又因为没有位置基准而漂移轨迹就丢了。混合力位控制最底层的思路是把任务空间一分为二法线方向做力闭环切平面方向做位置闭环两者在不同维度上并行互不冲突。这个“拆自由度”的数学表达就是选择矩阵 S。S 是一个 6×6 对角阵对角线为 1 表示该自由度采用位置控制为 0 表示该自由度采用力控制。注意这里的维度是笛卡尔空间的 6 个自由度3 个平移 3 个旋转不是关节空间的六个轴。关节空间的角度和力是经过运动学映射之后的另一层故事混合控制的选择矩阵必须定义在任务空间才有意义。2.2 雅可比矩阵与静力映射末端力到关节力矩的换算混合控制策略算出来的是作用在末端笛卡尔坐标系上的广义力 F_task包括三个力分量和三个力矩分量。关节执行器要输出的是关节力矩两者之间靠雅可比矩阵的转置建立关系tau J^T * F_task这个式子物理上叫静力映射代表“如果末端顶着环境产生 F_task每个关节需要出多少力矩”。因为毕业设计用的机械臂一般是六轴串联结构雅可比矩阵解析式逐列写出来很长我习惯在工程里边递推做正运动学边填列// computeJacobian.cpp —— 基于DH递推构造几何雅可比矩阵 Eigen::MatrixXd computeJacobian(const Eigen::VectorXd q, const std::vectorEigen::Matrix4d jointTransforms, int dof) { Eigen::MatrixXd J(6, dof); Eigen::Matrix4d T Eigen::Matrix4d::Identity(); Eigen::Vector3d p_end fk(q).topRightCorner3, 1(); // 末端位置 for (int i 0; i dof; i) { Eigen::Vector3d z_i T.block3, 1(0, 2); // 关节转轴方向 Eigen::Vector3d p_i T.topRightCorner3, 1(); // 关节坐标系原点 J.block3, 1(0, i) z_i.cross(p_end - p_i); // 平动列z_i×(p_end-p_i) J.block3, 1(3, i) z_i; // 转动列就是z_i T T * jointTransforms[i]; // 递推到下一关节 } return J; }平动部分每一列是当前关节转轴对末端位置的叉乘转动部分直接就是关节转轴方向在基坐标系下的投影。这些量查 DH 表乘一遍就能得到不复杂但写错符号会导致力控发散。调试时先用单位关节速度驱动机器人对照末端速度实测值校一遍雅可比再进力控循环。2.3 选择矩阵与任务空间分解自由度怎么分才不打架选择矩阵的构造要跟任务坐标系的定义一起做。S 为 1 的是位置自由度S 为 0 的是力自由度。以沿 z 轴做力控磨抛为例// 6x6 选择矩阵, 1: 位置控制, 0: 力控制 Eigen::Matrix6d S Eigen::Matrix6d::Identity(); S(2, 2) 0.0; // z 方向: 力控制, 让末端按恒定压力压住工件 S(5, 5) 1.0; // 绕 z 轴仍做位置控制, 防止磨头转动漂移这里最容易犯错的是“坐标系的朝向”。如果末端姿态在工作过程中发生变化基坐标系下的 z 轴和工件表面的法线方向不再重合选择矩阵里你写的那个 z 还是基坐标系的 z控制对象就错了。所以工程里一般先在末端工具坐标系或工件坐标系底下算混合控制律再通过旋转矩阵把结果变换到基坐标系。如果你的任务里末端姿态变化不大可以适当简化但需要在论文里写明假设边界。2.4 混合力位控制 vs 阻抗控制毕设选型的判断依据答辩时老师经常会追问“你为什么不直接用阻抗控制”。阻抗控制不是直接控力而是调控末端位置偏差和接触力之间的关系把力误差变成位置修正。这两种方案各有定位差异直接对比一张表说清楚对比维度混合力位控制阻抗控制控制目标在指定方向直接控制接触力调节力与位置之间的动态阻抗关系力传感器依赖强需要实时六维力反馈可用关节力矩估算不强依赖末端力传感器与环境刚度适配对环境刚度变化敏感需要标定天然在阻抗模型内吸收刚度变化毕业设计展示动作插孔、贴面、拧螺丝直观好讲适合柔顺拖拽、人机交互场景推导和实现难度中高需要对坐标变换谨慎中等调参数维度也不少混合力位控制在教学上讲得通验证起来有明确的“力误差曲线”指标论文图表好看所以本科毕设里选它的人多。阻抗控制则适合强调“柔顺性”而不是恒定力的任务。3. 用 C/C 实现混合力位控制的完整工程框架3.1 工程目录与构建配置从 vscode 配置 c/c 环境到 CMake 落地拿到源码压缩包以后第一步不是急着读控制算法而是把工程构建起来。常见做法是用 CMake 组织依赖源码目录大概长这样hybrid_pos_force/ ├── CMakeLists.txt ├── include/ │ ├── controller/ │ │ ├── hybrid_controller.h │ │ ├── force_filter.h │ │ └── trajectory.h │ └── robot/ │ ├── kinematics.h │ └── robot_model.h ├── src/ │ ├── main.cpp │ ├── controller/ │ │ ├── hybrid_controller.cpp │ │ └── force_filter.cpp │ └── robot/ │ ├── kinematics.cpp │ └── robot_model.cpp ├── config/ │ ├── controller_params.yaml │ └── robot_params.yaml └── logs/CMakeLists.txt 里最少的配置这么写cmake_minimum_required(VERSION 3.16) project(hybrid_pos_force CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Eigen 做矩阵运算, 是机械臂控制代码的标准配置 find_package(Eigen3 REQUIRED) add_executable(hybrid_controller src/main.cpp src/controller/hybrid_controller.cpp src/controller/force_filter.cpp src/robot/kinematics.cpp ) target_link_libraries(hybrid_controller Eigen3::Eigen)如果你在 Windows 上用 VS Code 开发vscode 配置 c/c 环境时需要注意两点一是 include 路径要加上 Eigen 的安装目录否则智能提示和编译全飘红二是 launch.json 里把工作目录指到 logs 目录这样日志文件路径不会到处乱跑。3.2 主控制循环力位混合控制的 C 实现控制周期一般固定为 1kHz 到 250Hz 之间。仿真里开 1kHz实机六自由度机械臂根据驱动器通信周期常见 500Hz 到 1kHz 都能跑。主循环的骨架如下这里把数学上的两条控制律都写出来了// main_control_loop.cpp —— 单周期混合力位控制 while (running) { // 1. 读关节状态 readJointState(q_meas, dq_meas); // 2. 读末端六维力传感器原始数据 readForceSensor(F_raw); // 3. 力信号滤波, 去掉传感器噪声 F_filtered filter.update(F_raw); // 4. 正运动学, 求当前末端位姿和雅可比 T_cur forwardKinematics(q_meas); J computeJacobian(q_meas, joint_transforms, dof); // 末端笛卡尔速度 雅可比 * 关节速度 dT_cur J * dq_meas; // 5. 轨迹规划器给出期望位姿 xd trajectory.getDesiredPose(cycle_time); // 6. 混合控制律: 位置自由度 PD, 力自由度 PI Eigen::Vector6d F_task Eigen::Vector6d::Zero(); for (int i 0; i 6; i) { if (S(i, i) 1.0) { // 位置控制方向: PD double pos_err xd(i) - T_cur(i); double vel_err -dT_cur(i); F_task(i) Kp(i) * pos_err Kd(i) * vel_err; } else { // 力控制方向: 期望力 PI 修正 double f_err Fd(i) - F_filtered(i); force_integral(i) f_err * dt; F_task(i) Fd(i) Kfp(i) * f_err Kfi(i) * force_integral(i); } } // 7. 由静力映射得到关节力矩 Eigen::Vector6d tau J.transpose() * F_task; // 8. 下发关节力矩指令 sendJointTorque(tau); waitForNextCycle(cycle_time); }位置方向用的是 PD力方向用 PI力误差含有积分项是因为接触力系统里经常存在摩擦和重力偏置纯比例控制在恒定外载作用下会留一个静差。需要说明的是第 6 步算出的 F_task是作用在末端坐标系原点、由任务坐标系表达的广义力。如果你把 S 定义在工具坐标系下那么 F_task 也要先在工具坐标系里合成再在使用基坐标系的 J 转置之前做一次坐标旋转。实机上如果伺服驱动器不开放力矩模式常见的替换做法是把 tau 的一部分作为前馈力矩另一部分折算成关节位置修正量交给底层位置环效果接近而工程上更稳妥。3.3 力传感器数据读取与滤波参数怎么给六维力传感器直接串口或者 EtherCAT 读出来的数据静态时噪声可能在满量程的 0.1% 到 0.5% 之间动态时还可能带振动谐波。力控对噪声相当敏感噪声一大部分直接进了控制律末端就会抖。我一般先做滑动平均代码非常短// force_filter.cpp —— 窗口滑动平均滤波 template int N Eigen::Matrixdouble, 6, 1 movingAverage( const Eigen::Matrixdouble, 6, 1 raw, std::dequeEigen::Matrixdouble, 6, 1 window) { window.push_back(raw); if (static_castint(window.size()) N) { window.pop_front(); } Eigen::Matrixdouble, 6, 1 sum Eigen::Matrixdouble, 6, 1::Zero(); for (auto v : window) sum v; return sum / window.size(); }注意窗口大小会直接带来相位延迟。窗口越大噪声压得越死、但力信号滞后越明显力阶梯响应会变得迟钝。毕业设计里调的时候按这组经验值起步滤波方式推荐参数为什么滑动平均N 5 ~ 15简单直观适合采样率 1kHz 的力信号一阶低通截止频率 20 ~ 100 Hz相位滞后更小力控响应更快高阶巴特沃斯不推荐一上来就用群延迟大旁瓣会把力信号拉出冲击调用滤波之后记得在“空载、不接触”的状态下做一次零偏校验把空载读数存进标定文件。很多毕设实机最后力控不准首先死在零漂上。3.4 机械臂轨迹规划算法在混合控制里的角色混合力位控制里位置方向需要一个参考轨迹力方向虽然没有位置轨迹但力期望值本身也要按工况做斜坡规划。最常用的还是梯形速度规划把目标位移拆成加速段、匀速段、减速段。控制循环里每个节拍调用 getDesiredPose(t) 时如果当前时间点落在什么速度段返回对应的位置、速度和姿态。姿态部分用到四元数球面插值slerp比欧拉角平滑也容易转成旋转矩阵去驱动雅可比。有一点要特别提醒轨迹规划里的期望速度也必须参与位置方向的 PD 控制律。如果不把期望速度写进前馈只靠 Kd 阻尼项机械臂在高速段会有明显跟随滞后末端轨迹会偏力控方向还好说位置方向的表现答辩演示时很难看。4. 机械臂选型、仿真环境与 ROS 机械臂开发接线4.1 机械臂选型预算和力反馈来源怎么平衡毕设题目里如果只是验证控制算法强烈建议从 Gazebo 仿真出发。仿真环境里力传感器是现成的不需要花钱买硬件还能随意改变工件刚度、接触面摩擦系数数据重复性远比实机好。实机制作一般用带总线舵机的六自由度机械臂例如常见套件舵机电流可以直接映射扭矩力反馈精度虽然一般但做“沿平面保持恒定压力”这类展示足够了。方案典型硬件力反馈来源预算量级适合展示的任务纯仿真Ubuntu ROS Noetic GazeboUR10/Panda 模型Gazebo 接触力插件0插孔、平面恒力追踪、轨迹 力曲线低成本实机总线舵机 6 轴机械臂 主轴舵机电流折算关节力矩1000 ~ 3000沿木板打磨求恒力展示末端力数据中阶实机六轴机械臂 国产六维力传感器末端六维力采集卡数万高精度插孔、表面跟踪选型时的关键原则优先保证力反馈数据可记录、可画图其次才追求结构精度。答辩需要的是“我记录了力误差曲线、验证了算法”不是“这块板磨得多光”。4.2 在 Gazebo 中把机械臂模型和接触环境搭起来常见做法是用 ROS 里现成的描述包。以 panda 机械臂为例启动命令一般是这样# 启动 Gazebo 仿真环境 roslaunch panda_gazebo panda_world.launch # 加载控制器 roslaunch panda_control panda_control.launch # 发布一组关节角度目标, 验证整条链路通不通 rostopic pub /panda/joint_command sensor_msgs/JointState \ {position: [0, -0.785, 0, -2.356, 0, 1.571, 0.785]} --rate 10发布之前先rostopic echo -n1 /panda/force_torque看一眼接触力话题是不是在输出然后再让它动。这里关键的坑是混合力位控制需要的是末端力/力矩不是关节力矩。话题名字里带 force_torque 的就是目标数据里指的是基坐标系还是工具坐标系要查描述文件搞反了力控方向会立刻正反馈。4.3 把 C/C 控制算法封装成 ROS 节点仿真环境里跑算法最规范的方式是把第 3 章写的那套 controller 类包进一个 ROS 节点力从订阅话题取算完的关节指令再发到控制器话题// hybrid_ros_node.cpp —— 控制算法的 ROS 封装骨架 class HybridControlNode { public: HybridControlNode(ros::NodeHandle nh) { // 订阅末端六维力 force_sub_ nh.subscribe(/panda/force_torque, 1, HybridControlNode::forceCallback, this); // 发布关节力矩指令 torque_pub_ nh.advertisesensor_msgs::JointState( /panda/joint_command, 1); // 固定周期跑控制循环 timer_ nh.createTimer(ros::Duration(0.001), HybridControlNode::controlLoop, this); } private: void forceCallback(const geometry_msgs::WrenchStamped::ConstPtr msg) { F_raw_ msg-wrench.force.x, msg-wrench.force.y, msg-wrench.force.z, msg-wrench.torque.x, msg-wrench.torque.y, msg-wrench.torque.z; } void controlLoop(const ros::TimerEvent) { // 读关节状态 - 算 tau - 发布 sensor_msgs::JointState cmd; cmd.effort tau_vec_; torque_pub_.publish(cmd); } };这套结构里控制器类完全不知道自己在 ROS 里还是在实机里跑接口只管输入状态和力、输出关节指令之后换电机驱动或者换通信协议都只要改最外层的数据适配层。这就是整份源码工程里最值得抄的部分。5. 调参与排错混合力位控制的参数标定与常见失败现场5.1 力控增益和位置刚度怎么配比混合力位控制最大的调参难题是两个方向的控制参数互相影响。位置刚度过大末端的微小位置误差会转换成大力矩干扰力方向力控增益过大系统容易“软塌”和抖动。所以调参顺序要先调位置再调力不要一上来两个一起改。按自由度分开调位置自由度调 Kp、Kd力自由度调 Kfp、Kfi。起始值参考这个表参数含义建议起始值调参方向Kp位置刚度80 ~ 200位置跟踪误差大就加大振荡就减小Kd位置阻尼2*sqrt(Kp) 附近振荡主要靠它压太大响应变慢Kfp力误差比例0.05 ~ 0.3力收敛慢就加大力噪声大就减小Kfi力误差积分0.01 ~ 0.1稳态力误差大就加大出现过冲就减小实际从仿真看Kp 与 Kfp 相差几十倍一点也不奇怪因为位置误差单位是米、力误差单位是牛顿量纲不同。调参时观察六条曲线位置误差曲线和力误差曲线哪条响就调哪组互不联动地独立收敛。5.2 力传感器零漂与噪声上电先做三个检查实机力控发散一半以上的原因不在算法在传感器数据。没有六维力传感器的场景先用舵机电流折算关节力矩的话电流零漂更要注意。我每次通电后按三个步骤检查机械臂空载悬停记录一分钟静态数据均值偏离零的程度就是零漂写入配置文件。用砝码或已知重物挂在末端指定位置对比读数与理论重力力矩差检查力/力矩量程方向和符号。手动低速晃动末端观察滤波后数据有没有明显滞后滞后超过 50 ms 就要把滤波窗口调小。做完这三条再进力控后面折腾的时间能砍掉大半。5.3 接触瞬间的震荡抑制力期望值斜坡怎么做末端从“未接触”切换到“已接触”的瞬间接触力会从零突变到期望值控制律来不及反应就会产生一个冲击力甚至回弹。解法是在接触发生后再把力期望值从零斜坡上升到目标值Fd(t) Fd_target * min(1.0, (t - t_contact) / T_ramp)T_ramp 一般给 0.5 到 2 秒根据工件刚度和机械臂响应速度调整。工程上判断接触时刻不靠猜测看力值是否连续几个采样点超过阈值触发接触标志位。位置方向同时从“自由运动”切到“位置保持”切换瞬间避免直接给出阶跃位置误差把当前位置保存为期望位置起点。5.4 用日志回放定位发散现场控制循环里每个周期只写几行关键量别贪多// 日志格式: 时间戳 位置误差x/y/z 力误差x/y/z 关节力矩0..5 log_stream cycle_count , pos_err.transpose() , f_err.transpose() , tau.transpose() \n;跑完之后用 Python 或者简单绘图工具把时间序列画出来第一时间就能判断是不是某一轴向的发散。常见的发散形态就三种一是力噪声高频抖动问题在滤波二是单一方向等幅振荡问题在 Kd 或 Kfp 太大三是缓慢漂移不回中问题在积分项饱和。日志能直接区分这些比看机械臂“抖成什么样”直观得多。注意实机上出现发散时第一件事不是改参数而是先做限幅和急停。所有关节力矩都要做饱和限制力控方向尤其注意不然电机直接过热报警。6. 把研究成果落进答辩 PPT图表、对比实验与演示视频的验收技巧6.1 控制框图和实验曲线怎么画才“像”答辩 PPT 里最值钱的是两页一页是混合力位控制框图一页是力误差与位置误差同步曲线。控制框图不要画成一大堆箭头乱飞严格按“期望力/期望位姿 - 选择矩阵 - 控制律 - 机械臂 - 测量”的单回路结构画。曲线图要同时叠上期望值和实测值比如力控方向画出期望力直线和实测力曲线之间的带状差异位置方向画出轨迹偏差。曲线不一定要完美但横轴时间单位、纵轴力单位必须标清楚这一页往往是老师停留最久的地方。6.2 必做的三组对比实验毕业设计答辩最少要有三组数据第一组纯位置控制沿工件表面运动展示接触力波动巨大第二组混合力位控制力误差曲线收敛并保持在一个小范围内第三组分别用不同速度或不同期望力重复说明算法稳定性。三组实验一摆工作量立竿见影。数据表结构可以这样组织实验组控制方式力误差均值位置偏差结论实验一纯位置控制正负 2.5N0.12mm接触力不可控实验二混合力位控制正负 0.4N0.18mm力跟踪有效实验三混合力位控制速度加倍正负 0.7N0.25mm稳定性可接受上表中的具体数字只是示例最终用自己实验跑出来的数据替换。答辩老师不会纠结绝对值但会盯着趋势问问题所以实验二和实验三的对比条件速度、进给量一定要在 PPT 里写明。6.3 演示视频的拍摄顺序如果有实机演示视频别剪太碎按“安装工件 - 初始化 - 接触之前 - 接触瞬间 - 稳定磨抛 - 撤出”的顺序一镜到底。重点拍接触瞬间那一刻力曲线从零跳到期望值的过程最能说明算法有效。如果只有仿真动画配上力误差曲线画在同一画面里同步播放效果会比单放动画好很多。源码包里的那份答辩 PPT常见目录结构是“问题背景 - 方案设计 - 仿真建模 - 实验验证 - 总结展望”配一张完整的混合力位控制实验曲线图就能打开场。本文还有配套的精品资源点击获取
