7.2 力控相关传感力控相关传感是人形机器人实现精确操作、柔性抓取及平衡控制的重要基础。通过测量关节力矩、关节扭矩以及末端执行器受力情况机器人可以实时感知自身运动状态与外部环境作用力从而实现力—运动闭环控制。力控传感不仅保证了操作安全性还为动态平衡调节、碰撞防护和复杂操作提供关键数据支持是高层规划与低层控制之间的桥梁。7.2.1 关节力矩估计关节力矩是人形机器人运动控制和力控制系统中的核心物理量描述了每个关节在运动过程中所承受的力矩大小与方向。精确的关节力矩估计不仅能够反映关节本体的受力状态还能作为外部接触力感知、碰撞检测和柔性控制的重要依据。1. 理论原理关节力矩由机器人自身动力学与外部负载共同决定。对于一个n自由度的机器人关节系统来说其关节力矩τ可以通过动力学模型表示为τM(q)qC(q,q)qG(q)Fext其中q,q,q分别为关节角度、角速度与角加速度M(q)为关节惯性矩阵C(q,q)表示科氏力与离心力矩阵G(q)为重力矩Fext为外部作用力引起的力矩如碰撞或负载。通过上述方程可以将传感器测得的关节状态映射为理论力矩为力控和姿态调节提供参考。2. 常用关节力矩估计方法1直接测量法在关节轴或驱动器上集成六轴力/力矩传感器可直接测量关节的受力状态。优点精度高响应快可直接获得真实力矩。缺点增加硬件成本传感器易受噪声干扰和机械安装误差影响。2电流/电压观测法利用伺服电机的电流或电压与输出力矩之间的关系通过电机力矩常数Kt进行反算τ≈Kt⋅I-τfriction其中I为电机电流τfriction为摩擦力矩。优点不增加额外传感器适合轻量化设计缺点需要精确摩擦模型和电机参数对动态响应可能有一定滞后。3动力学模型推算法结合关节角度、速度、加速度及机器人动力学参数使用牛顿-欧拉或拉格朗日方法计算理论关节力矩。可以结合外部力传感器和观测反馈修正模型误差实现精确估计。优点适用于高自由度机器人无需每个关节安装传感器缺点对模型参数精度要求高对摩擦和柔性连接敏感。4观测器/滤波器法使用卡尔曼滤波器、扩展卡尔曼滤波器EKF或自适应观测器对关节力矩进行估计可融合传感器数据和动力学模型提高鲁棒性。在动态运动或外部扰动情况下可以有效的滤除噪声提高估计精度。3. 工程实现与应用姿态与运动控制通过实时关节力矩估计控制器能够调整驱动关节输出力矩实现平衡调节和运动轨迹跟踪。碰撞检测异常力矩信号可用于检测外部碰撞或异常负载提高机器人操作安全性。柔性抓取与力控制在机械手或足部支撑中关节力矩估计提供了闭环反馈使机器人能够根据负载变化调节抓握或支撑力实现柔性控制。能耗优化基于力矩估计控制器可以优化关节驱动力分布降低功率消耗提高运动效率。总之关节力矩估计是连接机器人动力学建模与力控算法的桥梁。通过直接测量、电机观测、动力学推算及观测器融合等方法可以实时获取每个关节的力矩信息为姿态稳定、力控制、碰撞保护及柔性操作提供精确依据是现代人形机器人力觉感知系统的核心组成部分。7.2.2 接触力与抓取稳定性接触力感知是机器人实现安全抓取和精确操作的关键技术通过测量末端执行器如机械手、手指或足部与环境或物体接触时的力可以判断抓取或支撑的稳定性并指导力—运动闭环控制。1. 接触力的定义与测量接触力包括法向力和切向摩擦力两部分法向力Fn垂直于接触面的力决定抓取或支撑的基本稳定性。切向摩擦力Ft平行于接触面方向的力与摩擦系数共同决定物体是否会滑动。常用的测量方法如下所示力/力矩传感器在机械手腕或关节安装六轴力传感器可直接测量法向力和切向力矩。触觉阵列传感器在手指或掌心布置压力/触觉传感器获取局部压力分布用于估算局部接触力和滑移趋势。电机电流推算通过电机电流变化估算末端受力适用于没有直接接触力传感器的系统。2. 抓取稳定性分析抓取稳定性通常基于摩擦锥理论Friction Cone分析∥Ft∥≤μFn其中μ为接触面的摩擦系数。若接触力向量落在摩擦锥内部物体不会滑动超出摩擦锥则可能失稳。对于多点抓取或多指手通常使用力闭环矩阵Grasp Wrench Space, GWS进行分析将每个接触点的法向力和摩擦力矩映射到末端物体坐标系。通过计算所有接触力的组合空间判断能否平衡物体重力和外部扰动。抓取裕度越大说明抓取稳定性越高机器人对扰动的鲁棒性更强。3. 接触力控制策略机器人在抓取或支撑过程中需要实时调整末端力以保证稳定性常用的控制策略如下所示力闭环控制根据力传感器反馈调整关节力矩实现期望接触力或压力分布。滑移检测与预防通过触觉阵列或微分法检测微小滑动趋势提前调节抓握力防止物体滑落。多模态融合结合视觉、触觉和力传感器数据提高抓取鲁棒性和适应性。柔性抓取在力反馈基础上实现可调柔性抓握避免损伤易碎物体或感知环境结构。4. 工程应用示例机械手抓取物体通过测量手指压力分布和手腕力矩判断抓取是否稳固动态调整抓力。足底支撑在机器人行走时足底接触力反馈用于调整关节力矩实现平衡控制和步态稳定。柔性操作搬运易碎物体或软体材料时根据接触力反馈调节抓握力防止损伤。总之接触力感知与抓取稳定性分析是机器人力控系统的核心环节通过法向力、切向摩擦力测量及摩擦锥理论分析结合力闭环控制和多模态传感融合机器人能够在复杂环境中实现安全、稳定且柔性的抓取与操作。这不仅提高了任务成功率也保障了操作的安全性和效率。7.2.3 实战演练人形机器人关节力矩与抓取稳定性的可视化本实例展示了人形机器人力控感知的两个核心应用一是关节力矩估计通过电流观测法、动力学模型推算及卡尔曼滤波融合实现对单关节运动中力矩的实时估计与对比可视化二是接触力与抓取稳定性分析基于摩擦锥理论模拟三种抓取场景判断抓取是否稳定并用图形直观展示法向力、切向力及抓取裕度实现机器人抓取安全性与力控策略的直观演示。实例7-1人形机器人关节力矩与抓取稳定性的可视化源码路径codes\7\Li.py实例文件Li.py的主要实现流程如下所示。1下面代码的功能是定义了关节力矩估计模块 JointTorqueEstimator通过模拟人形机器人单关节的运动生成角度、角速度和角加速度数据结合动力学模型、电机电流观测以及外部扰动计算真实关节力矩并提供电流观测法、动力学推算法及卡尔曼滤波融合三种方法对关节力矩进行估计。# 1. 关节力矩估计模块 class JointTorqueEstimator: 单关节力矩估计器实现电流观测法、动力学推算法、卡尔曼滤波融合 def __init__(self): # 单关节动力学参数人形机器人小腿关节典型值 self.M 1.2 # 转动惯量 (kg·m²) self.C 0.8 # 科氏力/离心力系数 (N·m·s/rad) self.G 9.8 # 重力矩 (N·m) self.Kt 0.5 # 电机力矩常数 (N·m/A) self.tau_friction 0.3 # 摩擦力矩 (N·m) self.noise_std 0.2 # 传感噪声标准差 def generate_joint_motion(self, time_steps100): 生成关节运动数据角度、角速度、角加速度 t np.linspace(0, 5, time_steps) # 5秒运动周期 q np.sin(2 * np.pi * 0.5 * t) # 关节角度 (rad) dq 2 * np.pi * 0.5 * np.cos(2 * np.pi * 0.5 * t) # 角速度 (rad/s) ddq - (2 * np.pi * 0.5) **2 * np.sin(2 * np.pi * 0.5 * t) # 角加速度 (rad/s²) return t, q, dq, ddq def calculate_true_torque(self, dq, ddq, F_ext0.5): 计算真实关节力矩含外部扰动 tau_true self.M * ddq self.C * dq self.G F_ext * np.sin(dq) return tau_true def current_based_estimate(self, tau_true): 电流观测法τ ≈ Kt·I - τ_friction反推电流并加噪声 # 反推理想电机电流 I_ideal (tau_true self.tau_friction) / self.Kt # 添加电流噪声 I_noisy I_ideal np.random.normal(0, self.noise_std, len(I_ideal)) # 估计力矩 tau_current self.Kt * I_noisy - self.tau_friction return tau_current def dynamics_based_estimate(self, dq, ddq): 动力学模型推算法τ M·ddq C·dq G tau_dynamics self.M * ddq self.C * dq self.G # 添加模型噪声 tau_dynamics np.random.normal(0, self.noise_std*1.5, len(tau_dynamics)) return tau_dynamics def kalman_fusion_estimate(self, tau_current, tau_dynamics, tau_true): 卡尔曼滤波融合融合电流法和动力学法结果 # 卡尔曼滤波参数 n len(tau_current) tau_fusion np.zeros(n) # 初始化滤波状态 x_est tau_true[0] # 初始估计值 P_est 1.0 # 初始协方差 Q 0.1 # 过程噪声协方差 R 0.2 # 观测噪声协方差 for i in range(n): # 预测步 x_pred x_est P_pred P_est Q # 观测步融合两个估计值 z (tau_current[i] tau_dynamics[i]) / 2 K P_pred / (P_pred R) # 卡尔曼增益 # 更新步 x_est x_pred K * (z - x_pred) P_est (1 - K) * P_pred tau_fusion[i] x_est return tau_fusion def run_estimation(self): 运行所有估计方法并返回结果 t, q, dq, ddq self.generate_joint_motion() tau_true self.calculate_true_torque(dq, ddq) tau_current self.current_based_estimate(tau_true) tau_dynamics self.dynamics_based_estimate(dq, ddq) tau_fusion self.kalman_fusion_estimate(tau_current, tau_dynamics, tau_true) return t, tau_true, tau_current, tau_dynamics, tau_fusion2下面代码的功能是定义了关节力矩可视化函数 plot_joint_torque_results将真实力矩、各估计方法计算得到的力矩随时间变化绘制在同一图表中通过不同颜色和线型区分各方法同时显示图例、网格和坐标轴标签用于直观对比关节力矩估计效果。def plot_joint_torque_results(t, tau_true, tau_current, tau_dynamics, tau_fusion): 可视化关节力矩估计结果 fig, ax plt.subplots(figsize(12, 6)) ax.plot(t, tau_true, k-, linewidth3, label真实力矩, alpha0.8) ax.plot(t, tau_current, r--, linewidth2, label电流观测法, alpha0.7) ax.plot(t, tau_dynamics, g--, linewidth2, label动力学推算法, alpha0.7) ax.plot(t, tau_fusion, b-, linewidth2, label卡尔曼滤波融合, alpha0.9) ax.set_xlabel(时间 (s), fontsize12) ax.set_ylabel(关节力矩 (N·m), fontsize12) ax.set_title(人形机器人单关节力矩估计结果对比, fontsize14) ax.grid(True, alpha0.3) ax.legend(locupper right) plt.tight_layout() plt.show()3下面代码的功能是定义了接触力与抓取稳定性分析模块 GraspStabilityAnalyzer基于摩擦锥理论计算法向力与切向力的稳定性提供函数模拟三种抓取场景稳定、临界、失稳并返回抓取状态和抓取裕度为机器人抓取动作的力控和安全性评估提供数据支持。# 2. 接触力与抓取稳定性模块 class GraspStabilityAnalyzer: 抓取稳定性分析器基于摩擦锥理论判断接触力稳定性 def __init__(self, mu0.6): self.mu mu # 摩擦系数橡胶-塑料接触面典型值 def calculate_friction_cone(self, F_n, num_points100): 计算摩擦锥边界F_t ±μF_n theta np.linspace(-np.arctan(self.mu), np.arctan(self.mu), num_points) F_t_cone F_n * np.tan(theta) F_n_cone np.ones(num_points) * F_n return F_n_cone, F_t_cone def judge_stability(self, F_n, F_t): 判断抓取稳定性基于摩擦锥理论 max_F_t self.mu * F_n if abs(F_t) max_F_t: state 稳定抓取 margin (max_F_t - abs(F_t)) / max_F_t # 抓取裕度0~1 elif abs(F_t) max_F_t: state 临界状态 margin 0.0 else: state 滑动失稳 margin -abs(F_t - max_F_t) / max_F_t # 负裕度表示失稳程度 return state, margin def simulate_grasp_scenarios(self): 模拟三种抓取场景 # 场景1稳定抓取法向力大切向力小 F_n1, F_t1 10.0, 2.0 state1, margin1 self.judge_stability(F_n1, F_t1) # 场景2临界状态切向力μF_n F_n2, F_t2 8.0, self.mu*8.0 state2, margin2 self.judge_stability(F_n2, F_t2) # 场景3滑动失稳切向力μF_n F_n3, F_t3 6.0, 5.0 state3, margin3 self.judge_stability(F_n3, F_t3) return [ (F_n1, F_t1, state1, margin1, 场景1), (F_n2, F_t2, state2, margin2, 场景2), (F_n3, F_t3, state3, margin3, 场景3) ]4下面代码的功能是定义了抓取稳定性可视化函数 plot_grasp_stability通过绘制摩擦锥边界、接触力向量和标注抓取状态及裕度将三种抓取场景直观呈现在图表中实现对机器人抓取稳定性的形象化展示便于理解和分析力控效果。def plot_grasp_stability(analyzer, scenarios): 可视化接触力与抓取稳定性摩擦锥接触力向量 fig, axes plt.subplots(1, 3, figsize(18, 6)) colors [green, orange, red] for i, (F_n, F_t, state, margin, title) in enumerate(scenarios): ax axes[i] # 绘制摩擦锥 F_n_cone, F_t_cone analyzer.calculate_friction_cone(F_n) ax.fill_between(F_t_cone, 0, F_n_cone, alpha0.2, colorcolors[i], label摩擦锥) ax.plot(F_t_cone, F_n_cone, k--, linewidth1.5, label摩擦锥边界) ax.plot(-F_t_cone, F_n_cone, k--, linewidth1.5) # 绘制接触力向量 ax.arrow(0, 0, F_t, F_n, head_width0.3, head_length0.5, fccolors[i], eccolors[i], linewidth2, label接触力向量) # 标注信息 ax.text(F_t0.5, F_n0.5, f{title}\n状态{state}\n裕度{margin:.2f}\nF_n{F_n}N, F_t{F_t}N, bboxdict(boxstyleround, facecolorwhite, alpha0.8)) # 图表配置 ax.set_xlabel(切向力 F_t (N), fontsize12) ax.set_ylabel(法向力 F_n (N), fontsize12) ax.set_title(f抓取稳定性分析 - {state}, fontsize13) ax.grid(True, alpha0.3) ax.legend(loclower right) ax.set_xlim(-8, 8) ax.set_ylim(0, 12) plt.suptitle(基于摩擦锥理论的抓取稳定性分析, fontsize15) plt.tight_layout() plt.show()5下面代码的功能是主函数运行示例依次调用关节力矩估计模块和抓取稳定性分析模块生成并绘制关节力矩估计结果和抓取稳定性场景图实现整个力控感知示例的端到端演示展示人形机器人在运动与抓取中的力觉应用效果。# 主函数运行示例 if __name__ __main__: # 安装依赖若未安装 # !pip install numpy matplotlib scipy # ---------------------- 1. 关节力矩估计演示 ---------------------- print( 关节力矩估计演示 ) torque_estimator JointTorqueEstimator() t, tau_true, tau_current, tau_dynamics, tau_fusion torque_estimator.run_estimation() plot_joint_torque_results(t, tau_true, tau_current, tau_dynamics, tau_fusion) # ---------------------- 2. 接触力与抓取稳定性演示 ---------------------- print(\n 接触力与抓取稳定性演示 ) grasp_analyzer GraspStabilityAnalyzer(mu0.6) scenarios grasp_analyzer.simulate_grasp_scenarios() plot_grasp_stability(grasp_analyzer, scenarios)执行后绘制如下可视化图关节力矩估计图如图7-3所示展示了真实关节力矩与电流观测法、动力学推算法及卡尔曼滤波融合结果随时间的变化对比。抓取稳定性图如图7-4所示分别绘制了三种抓取场景下摩擦锥边界与接触力向量直观显示抓取状态和抓取裕度。图7-3 关节力矩估计图图7-4 抓取稳定性图