软体机器人安全接触控制：基于控制屏障函数的可验证力约束方法

1. 项目概述：当软体机器人需要“温柔”触碰

在机器人领域，让机器人与环境进行物理交互，尤其是与人类或精密物体接触，一直是个棘手的问题。我们常说软体机器人“天生”更安全，因为它们由硅胶、织物等柔性材料制成，碰到东西会“缩”回去，不像金属手臂那样硬邦邦的。这个直觉没错，材料柔顺性确实能被动降低冲击力。但如果你真想让一个软体机器人去执行任务，比如在手术中辅助医生、在仓库里分拣易碎品，或者与老人进行康复互动，仅仅依靠材料特性是远远不够的。想象一下，一个充气软体手臂虽然柔软，但如果控制程序让它以最大功率猛地伸向一个脆弱表面，它依然可能造成损坏或伤害。

这里的关键在于，安全是一个闭环问题。它不仅仅是机器人“是什么”（材料），更是机器人“做什么”（控制）。我们需要一个能主动确保机器人在任何情况下都不会施加超出安全限度的力的“大脑”。这就是我们这次要深入探讨的核心：基于控制屏障函数的安全接触控制方法。简单来说，就是给软体机器人的控制器戴上一个“紧箍咒”，当它即将做出危险动作时，这个“紧箍咒”会自动生效，温和但坚决地把它拉回安全区域。

这套方法的价值在于其形式化与可验证性。它不像一些基于经验或启发式的安全规则（比如“感觉力大了就松一点”），而是通过数学方法严格证明：只要系统初始状态安全，并且控制器按照既定规则运行，那么在未来所有时间里，机器人的接触力都绝对不会超过我们设定的安全阈值。这对于医疗、助老等容错率极低的场景至关重要。本文将带你从零开始，理解如何为软体机器人构建这样一个可验证的安全控制器，涵盖从核心思想、数学建模、控制器设计到软硬件实验的完整链条。

2. 核心思路：从“力安全”到“位置安全”的映射

2.1 重新定义“安全”：从直觉到数学公式

在工程领域，尤其是控制理论中，“安全”需要被精确定义。我们不能只说“别太用力”，而必须明确“多大力算安全”。本文采用了力安全的正式定义：如果机器人末端执行器施加的力F(t)在所有时间t都保持在一个可接受的力集合F_safe内，那么该机器人的轨迹就是力安全的。用数学表示就是：如果初始力在安全集内，那么未来的所有力也都在安全集内。这被称为集合的不变性。

这个定义很清晰，但直接测量和控制力通常比较困难，尤其是对于软体机器人，其本体形变复杂，力传感器可能笨重、昂贵或难以集成。因此，一个巧妙的思路是进行问题转换：既然我们无法直接、完美地控制力，能否通过控制机器人的位置来间接保证力安全？

2.2 关键假设与环境建模

为了实现上述转换，我们需要对环境做一些合理且可处理的假设。本文在一个概念验证场景中进行了阐述：

1. 环境几何已知：机器人工作的环境（比如一个平面、一个曲面）的几何形状是已知的。我们可以用一个多边形（在2D情况下）或多面体（在3D）来定义机器人的无接触空间N。只要末端在这个空间内，就不会碰到环境。
2. 环境可变形且特性已知：环境不是绝对刚体，而是像海绵、橡胶或生物组织一样可以变形。我们假设环境每个表面的力-变形关系ψ(n)是已知的，其中n是沿表面法向的变形量。一个最简单的模型就是线性弹簧：F = k * n，其中k是弹性系数。
3. 最大安全力已知：对于特定任务（如接触人体组织），存在一个最大安全力F_max，超过它就可能造成损伤。

基于这些假设，一个强大的推论产生了：一个最大的安全力F_max，对应一个最大的允许变形n_max（通过ψ的反函数计算）。那么，为了保证施加的力不超过F_max，我们只需要保证机器人的末端在运动时，其侵入环境的变形深度不超过n_max。

2.3 构建安全位置集

接下来就是几何操作。我们有一个初始的“无接触”多边形N。对于N的每一条边（即环境表面），我们沿着其法线方向，向外平移n_max的距离。所有这些平移后的边所围成的新区域，就是安全位置集P。

数学上的精妙之处：这个操作将非线性的力约束（F ≤ F_max）转化为了线性的位置约束（H' * r ≤ h'）。这里r是末端位置向量，H'和h'定义了平移后的新多边形。这意味着，只要机器人的末端位姿r满足这一组线性不等式，我们就从数学上严格保证了接触力不会超标。

注意：这个“平移”方法在顶点处需要特殊处理。因为当末端接近多边形的角点时，它可能同时“推”到两个相邻的面，总变形可能超出n_max。论文中采用了一种保守的近似方法，在顶点处增加额外的约束平面，从而得到一个稍小但绝对安全的子集，确保了计算的简洁性（保持凸性和线性）与安全性。

至此，我们成功地将一个抽象的“力安全”问题，转化为了一个更具体、更易处理的“位置安全”问题：设计一个控制器，确保机器人的末端始终停留在安全位置集P内。

3. 软体机器人动力学建模：为控制提供基础

要对机器人进行控制，首先必须了解它如何运动，即建立其动力学模型。对于像章鱼触手一样连续变形的软体机器人，精确建模非常复杂。本文采用了在软体机器人领域广泛使用且有效的分段常曲率模型来简化问题。

3.1 分段常曲率假设

PCC模型的核心思想是将一根连续弯曲的软体臂，近似为由若干段恒定曲率的圆弧串联而成。对于平面（2D）机器人，每一段只需要一个状态变量q_i（弯曲角度）来描述。整个机器人的构型由所有段的角度组成的向量q表示。

• 运动学：通过几何关系，我们可以轻松地从q计算出机器人末端在任务空间（比如二维平面上的x, y坐标）的位置r(q)。这是通过串联每段的变换矩阵实现的。
• 动力学：为了描述运动（速度、加速度）与力和输入的关系，需要动力学方程。本文采用了“增强刚体”模型。这个模型的巧妙之处在于，它将每一段柔软的、连续弯曲的弧段，等效为一系列由虚拟弹簧和阻尼连接的刚体连杆和转动关节。这样，就能利用成熟的刚体机器人动力学理论（如拉格朗日方程）来推导出软体机器人的运动方程。

最终得到的动力学方程具有以下标准形式：M(q) * q̈ + C(q, q̇) * q̇ + D * q̇ + K * q = τ其中，M是质量矩阵，C包含科氏力和离心力项，D是阻尼矩阵，K是刚度矩阵，τ是作用在关节上的广义力（或力矩）。

3.2 驱动模型与控制仿射形式

对于本文使用的气动软体机器人，其输入是各个气腔的气压u。通过实验标定，可以建立气压u与广义力τ之间的线性映射关系：τ = Λ * u，其中Λ是一个对角矩阵。

将动力学方程进行整理，并定义系统状态x = [q^T, q̇^T]^T，我们可以得到控制仿射形式的系统方程：ẋ = f(x) + g(x) * u这个形式至关重要，因为后续控制屏障函数的理论正是建立在控制仿射系统的基础之上。其中f(x)代表系统的内在动力学���惯性、科氏力、阻尼、刚度等），g(x)代表控制输入如何影响系统状态的变化率。

实操心得：动力学模型的准确性直接影响控制器的性能。虽然PCC模型是简化模型，但在中小变形范围内通常能很好地捕捉软体机器人的主要动力学特性。关键步骤是参数辨识，即通过实验数据（如自由运动时的角度、角速度）来拟合出M,C,D,K,Λ中的未知参数。论文中提到使用了最小二乘法对硬件原型进行“电机游乐”来校准，这是非常标准的做法。

4. 控制屏障函数：安全的“数学守门员”

有了安全集P（由H' * r ≤ h'定义）和系统动力学模型ẋ = f(x) + g(x)u，接下来的任务就是设计一个控制器，使得从安全集内出发的任何轨迹，都永远停留在安全集内。这正是控制屏障函数大显身手的地方。

4.1 CBF的核心思想

你可以把CBF想象成一个“安全能量场”。我们为安全集P的边界定义一个标量函数h(x)，在安全集内部h(x) > 0，在边界上h(x) = 0，在外部h(x) < 0。CBF的目标不是让h(x)变大，而是阻止它变小到零以下。它通过约束控制输入u，使得函数h(x)沿着系统轨迹的导数（李导数）满足一个不等式，从而保证h(x)永远不会低于某个正数（如果初始为正）。

对于我们的问题，安全约束直接来自位置：b_i(x) = h'_i - H'_i * r(q) > 0。这表示末端位置到第i个安全平面边界的“距离”。b_i(x) > 0意味着末端在该平面的安全一侧。

4.2 针对二阶系统的屏障函数设计

一个重要的细节是，我们的约束b_i(x)依赖于位置r(q)，而位置是系统状态q的函数。控制输入u（气压）直接影响的是加速度q̈，进而通过两次积分影响位置。因此，约束b_i(x)相对于输入u的相对阶为2。这意味着我们需要考虑b_i(x)的一阶和二阶导数。

论文采用了针对相对阶为2的系统设计的CBF，其形式如下：B_i(x) = -ln( b_i(x) / (1 + b_i(x)) ) + a_E * (b_E * ḃ_i(x)^2) / (1 + b_E * ḃ_i(x)^2)这个函数看起来复杂，但其性质很好：当b_i(x)趋近于0（接近边界）或ḃ_i(x)很大（高速冲向边界）时，B_i(x)会急剧增大。我们的控制目标就是让B_i(x)保持有界。

基于此CBF，可以导出一个关于控制输入u的约束条件：L_f B_i(x) + L_g B_i(x) * u ≤ γ_i / B_i(x)其中，L_f B和L_g B是CBF函数沿系统动力学f和g方向的李导数，γ_i是一个大于0的调节参数。这个不等式是安全控制的核心。它意味着，我们选择的控制输入u必须足够“克制”，以限制B_i(x)的增长速度，从而确保b_i(x)不会降到零以下。

4.3 二次规划：融合任务与安全

在实际应用中，机器人不仅要保证安全，还要完成某项任务，比如跟踪一条期望轨迹。我们通常已经有一个名义控制器u_nom(x)，它可能性能很好但不考虑安全约束（例如一个简单的PD控制器或计算力矩控制器）。

CBF框架的优雅之处在于，它不直接替换名义控制器，而是作为一个监督器或滤波器。我们将安全约束（上面那个不等式）和最小化控制输入变化的目标结合起来，形成一个二次规划问题：

u* = argmin_u || u - u_nom ||^2 s.t. A(x) * u ≤ b(x)

这里：

• || u - u_nom ||^2是优化目标，意味着我们希望最终施加的控制输入u尽可能接近名义输入u_nom，以保留其性能。
• A(x)u ≤ b(x)是安全约束，其中A(x)矩阵的每一行是L_g B_i(x)，b(x)向量的每个元素是γ_i/B_i(x) - L_f B_i(x)。

这个QP问题在每一个控制周期（例如每秒1000次）实时求解。求解器会找到一个满足所有安全约束的u*，如果名义输入u_nom本身是安全的，那么u*就等于u_nom；如果u_nom会让机器人冲向危险区域，QP求解器就会自动计算出一个经过修正的、安全的u*。

理论保证：论文中的定理1严格证明了，只要初始状态在安全集内，并且每次控制都采用这个QP求解出的u*，那么系统轨迹将永远保持在安全集内，从而满足力安全定义。这是该方法最强大的地方——可验证的安全性。

5. 仿真与硬件实验全流程解析

理论再完美，也需要实验的验证。论文通过仿真和硬件实验两个层面，完整地演示了该框架的有效性。

5.1 仿真环境搭建与参数设定

1. 仿真模型：基于前面推导的PCC动力学方程（公式8-9）进行数值积分（如前向欧拉法）。仿真步长通常很小（如1e-5秒），以保证数值稳定性，而控制器的运行频率可以稍低（如1000Hz）。
2. 名义控制器：为了模拟最坏情况，论文使用了一个开环的正弦波信号作为u_nom(t)。这个信号会让机器人末端在任务空间内大幅度摆动，故意使其可能与环境发生过度接触。这很好地测试了CBF监督器的“纠偏”能力。
3. 环境与安全参数：
   • 设定一个环境平面（H1 = [0.36, 1.0],h1 = 0.15）。
   • 定义最大安全力F_max = 0.16 N。
   • 环境建模为线性弹簧，刚度k = 11.16 N/m。由此可计算出最大允许变形n_max = F_max / k ≈ 1.5 cm。
   • 根据n_max平移环境平面，得到安全位置集P。
4. CBF参数调节：参数a_E,b_E,γ影响着控制器的保守程度。论文测试了三组参数：
   • 无CBF：作为基线，直接使用不安全的开环信号。
   • 低保守度：允许机器人更接近安全边界。
   • 高保守度：让机器人更早、更坚决地远离边界。

5.2 仿真结果分析

仿真结果清晰地验证了理论：

• 无CBF情况：机器人的末端轨迹超出了安全集，计算出的接触力超过了F_max，安全裕度ρ(t)变为负值，表示安全约束被违反。
• 低/高保守度CBF情况：在CBF的监督下，末端轨迹被严格限制在安全集P内。接触力始终低于F_max，安全裕度ρ(t)全程保持为正。
  • 高保守度参数下，机器人甚至完全避免了与环境接触（ρ(t) = 1）。
  • 低保守度参数下，机器人可以与环境发生轻微接触，但力被精确限制在安全范围内。

关键指标——安全裕度：ρ(t) = (F_max - F(t)) / F_max。它直观地显示了当前力距离安全上限还有多少“空间”。ρ > 0安全，ρ = 0临界，ρ < 0危险。所有CBF控制器都保证了ρ(t) > 0。

5.3 硬件实验平台搭建

将算法部署到真实的软体机器人上，是更具挑战性的一步。硬件系统构成一个完整的感知-决策-控制闭环：

1. 被控对象：一个两段式平面气动软体机械臂，每段有两个对抗的气腔，通过调节气压差来驱动弯曲。
2. 状态感知：在机器人每段的基部和末端粘贴AprilTag视觉标记。一个顶置摄像头捕捉这些标记，通过图像处理实时解算出每个段的弯曲角度q和角速度q̇（通过差分得到）。这是系统的“眼睛”。
3. 控制执行：上位机（运行MATLAB/ROS2）根据感知到的状态x和CBF-QP算法，计算出安全的气压指令u*。该指令通过串口/网络发送给微控制器。
4. 底层驱动：微控制器运行PID控制循环，驱动比例阀来快速、精确地调整各个气腔的气压p1, p2, p3, p4，使其跟踪上层给出的指令u*。压力传感器提供反馈。
5. 环境与验证：使用一块贴有柔性ABS板的力传感器作为可变形环境。其刚度k已预先用砝码标定。力传感器的读数用于直接验证接触力是否真的被限制在F_max以下。

5.4 硬件实验结果与挑战

硬件实验成功复现了仿真的核心结论：

• 在CBF控制器作用下，实测接触力被成功限制在F_max以下，安全裕度ρ(t)保持为正。
• 无CBF时，力约束被违反。
• 控制输入u的波形显示，CBF主要在机器人即将发生危险接触的时间段（约15-55秒）对名义输入进行了修正，在其他时间段则基本不干预，体现了其“按需介入”的特性。

然而，硬件与仿真之间存在不可避免的差异：

• 模型误差：PCC模型和“增强刚体”模型是对复杂连续体动力学的近似。未建模的动力学（如材料粘弹性、气动延迟）会带来误差。
• 感知延迟与噪声：视觉处理帧率（12.8 fps）远低于仿真控制频率，且计算q̇的差分会放大噪声。
• 执行器带宽限制：气动阀门的响应速度有限，无法实现理论上的瞬时气压变化。
• 环境假设偏差：实际环境可能并非完美线性弹簧，且可能存在微小的三维运动。

这些因素导致硬件上的轨迹与仿真不完全一致，但最关键的安全属性——力不超限——在所有CBF实验中均得到了保持。这证明了该方法的鲁棒性。

6. 深入探讨：优势、局限与未来方向

6.1 本方法的优势与核心贡献

1. 形式化安全保证：这是首个为软体机器人环境交互提供形式化、可验证安全保证的闭环控制器。它超越了传统的“软材料即安全”的模糊说法，给出了严格的数学承诺。
2. 框架通用性强：虽然论文在2D平面气动机器人上验证，但其核心框架（基于模型的CBF-QP）并不局限于特定驱动方式或维度。任何能写成控制仿射形式的软体机器人动力学模型，理论上都可套用此框架。
3. 与任务控制解耦：CBF作为监督器，可以与任何现有的名义控制器（如轨迹跟踪、阻抗控制）结合，在不牺牲原有任务性能的前提下“注入”安全性。
4. 实时性：QP是凸优化问题，存在高效求解器，能够在毫秒级完成计算，满足实时控制需求。

6.2 当前局限性与挑战

1. 末端安全 vs. 全身安全：本文只保证了末端执行器的力安全。机器人身体的其他部分（如中间段）仍有可能与环境发生不安全接触。解决方案是为机器人的多个关键点分别定义安全集和CBF，但这会增加计算复杂度和约束数量。
2. 环境知识依赖：方法严重依赖于对环境几何和力学特性（ψ(n)）的精确已知。在未知或动态变化的环境中，这成为一个主要限制。
3. 模型依赖性：控制器的有效性建立在相对准确的动力学模型上。对于建模误差大或参数时变的系统，安全性证明可能不再严格成立。
4. 保守性与性能权衡：CBF参数调节是个技术活。过于保守（如论文中的“高”设置）可能导致机器人畏手畏脚，无法完成任务；过于激进则可能削弱安全边界。需要根据具体任务进行权衡。

6.3 未来可能的扩展方向

1. 适应未知环境：结合自适应控制或在线参数估计技术，让机器人能够在线学习环境的力学特性ψ(n)，甚至构建未知环境的几何模型N。
2. 高阶控制屏障函数：本文使用的CBF对相对阶为2的约束进行了处理。HOCBF可以处理更高相对阶的约束，或者提供更好的性能（如减少不必要的保守控制动作）。
3. 时变安全约束：对于移动的环境或变化的安全阈值，可以引入时变CBF，让安全集P(t)能够随时间动态变化。
4. 结合机器学习：使用学习-based的方法（如神经网络）来拟合更复杂的机器人动力学或环境交互模型，然后将CBF作为安全层来约束学习到的策略，兼顾性能与安全。
5. 从理论鲁棒性到实践：更深入地分析在模型不确定性、感知噪声和执行器延迟下的鲁棒性，并量化CBF参数与鲁棒安全裕度之间的关系。

实操心得与避坑指南：

• 参数辨识是关键：动力学模型（M, C, D, K, Λ）的参数必须通过精心设计的实验进行辨识。糟糕的模型会直接导致CBF约束失效或控制器性能低下。
• CBF参数需要仔细调节：不要指望一套参数放之四海而皆准。a_E,b_E,γ需要针对你的具体机器人、任务和环境进行调节。建议从仿真开始，观察不同参数下安全裕度和任务性能的平衡。
• 实时求解器的选择：对于简单的QP问题，可以使用像quadprog（MATLAB）、OSQP、qpOASES这样的高效库。确保求解器的调用时间远小于你的控制周期。
• 状态估计的准确性：q和q̇的估计噪声会直接影响CBF约束的计算。q̇的噪声尤其麻烦，因为它是CBF函数的一部分。考虑使用状态观测器（如卡尔曼滤波器）来获得更平滑、更准确的状态估计。
• 从简单场景开始：像论文一样，先从2D、单接触面、已知线性环境开始验证整个流程。成功后再逐步增加复杂度（3D、多接触、非线性环境）。

这套基于控制屏障函数的方法，为软体机器人走出实验室，进入真正需要安全保证的现实世界应用（如手术、康复、精密装配），提供了一个坚实而优雅的理论与实践框架。它标志着软体机器人控制从“性能优先”向“安全与性能并重”的重要范式转变。