CN116901057A - 一种基于变阻抗策略的双机械臂末端位置协同控制方法 - Google Patents

Abstract

本发明公开了一种基于变阻抗策略的双机械臂末端位置协同控制方法，通过机械臂末端力和力矩得到由机械臂末端指向夹取物的虚拟连杆向量估计值；根据所述机械臂末端力和力矩和夹取物的运动状态得到机械臂末端内力偏差；设计变刚度阻抗方程，其中机械臂的刚度自适应变化；根据夹取物的期望轨迹、所述虚拟连杆向量估计值、所述机械臂末端内力偏差和所述变刚度阻抗方程得到机械臂参考轨迹，将所述参考轨迹作为位置控制的输入以实现双机械臂的协同柔顺控制；使用本发明能够提升机械臂的控制精度，以及与环境接触的柔顺性。

Description

一种基于变阻抗策略的双机械臂末端位置协同控制方法

技术领域

本发明涉及机器人技术领域，具体涉及一种基于变阻抗策略的双机械臂末端位置协同控制方法。

背景技术

双臂协作机器人在救援、医疗、学习、娱乐、侦察等领域比单臂机器人具有显著优势。与单臂机器人系统相比，双臂协作机器人操作能力更强、工作空间更广、系统结构和组织模式更灵活，因此双臂机器人常用于执行广泛的任务，例如协作装配、焊接和携带大型物体等。但是双臂机器人在双臂协同工作过程中，由于受外界干扰、末端位置估计偏差等原因，双臂运动偶尔会出现不协调，影响物体位置控制精度等问题。

在双臂协同控制这一问题上，已有研究提出了主从控制法、混合力/位控制法、阻抗/导纳控制法等方法。

主从控制法定义一个机械臂作为主臂，采用纯位置控制，另一个臂作为从臂，采用力控跟踪主臂的运动轨迹；该方法需要从臂的力控跟踪具有快速的响应速度，容易导致系统的不稳定。

混合力/位控制是将任务空间分为位置控制子空间和力控制子空间，两个子空间独立控制，双臂均采用位置/力控制；但是，该方法需要根据实际操作任务选择位置和力控制方向，并且在位置控制和力控制之间进行实时切换，会增加控制器计算量。

阻抗控制建立了机械臂末端速度(位置误差)与末端力之间的动态关系，在位置控制环的基础上，加上力的控制环来实现，末端力的偏差通过阻抗控制转换为位置的修正量，并作为位置环的输入；该方法能够实现双臂的协调运动，但是在存在外界干扰或运动学参数不确定等情况下，也存在位置控制精度低、不能实现柔顺控制等问题。

发明内容

有鉴于此，本发明提供了一种基于变阻抗策略的双机械臂末端位置协同控制方法，能够提升机械臂的控制精度，以及与环境接触的柔顺性。

为了解决上述技术问题，本发明是这样实现的：

一种基于变阻抗策略的双机械臂末端位置协同控制方法，通过机械臂末端力和力矩h_i得到由机械臂末端指向夹取物的虚拟连杆向量估计值根据所述机械臂末端力和力矩h_i和夹取物的运动状态得到机械臂末端内力偏差Δh_i；设计变刚度阻抗方程，其中机械臂的刚度自适应变化；根据夹取物的期望轨迹χ_od、所述虚拟连杆向量估计值/>所述机械臂末端内力偏差Δh_i和所述变刚度阻抗方程得到机械臂参考轨迹，将所述参考轨迹作为位置控制的输入以实现双机械臂的协同柔顺控制。

较佳地，所述变刚度阻抗方程，为：

其中，M、B和K分别表示机械臂的惯性、阻尼和刚度矩阵；ΔK为随时间变化的刚度矩阵K的变化量，用于补偿由所述虚拟连杆向量估计值带来的估计偏差；Δχ_i表示第i个机械臂末端期望位置与实际位置间的机械臂末端位置偏差；/>和/>为机械臂末端位置偏差Δχ_i关于时间的二阶导和一阶导；/>为机械臂末端位置的计算偏差值；ΔK以根据实际情形自适应速率变化，为：

其中，∈为一个满足的中间向量；Γ为一个对称正定矩阵；/>表示所述机械臂末端位置偏差Δχ_i的预先估计值；/>表示机械臂末端位置偏差的预先估计值/>关于时间的一阶导。

较佳地，所述计算偏差值为：

其中，表示由夹取物到机械臂末端的映射向量的估计值；表示由夹取物到机械臂末端的映射向量的理论期望值；r_i为所述虚拟连杆向量估计值/>的理论期望值；/>表示由机械臂末端辅助坐标系到世界坐标系的旋转矩阵；0₃为零向量。

较佳地，所述得到机械臂参考轨迹，为：

对于所述变刚度阻抗方程，以所述机械臂末端内力偏差Δh_i为输入，所述机械臂末端位置偏差Δχ_i为输出；

根据所述虚拟连杆向量估计值和预先给定的所述夹取物的期望轨迹χ_od得到机械臂末端位置估计值/>

根据所述机械臂末端位置偏差Δχ_i和所述机械臂末端位置估计值得到两机械臂的末端位置修正量；

根据所述两机械臂的末端位置修正量，对所述夹取物的期望轨迹χ_od进行修正，得到所述参考轨迹。

较佳地，所述得到机械臂的末端位置估计值为：

将世界坐标系下的所述夹取物的期望轨迹χ_od通过坐标变换转为机械臂末端辅助坐标系下的所述机械臂末端位置估计值为：

其中，表示由夹取物到机械臂末端的映射向量的估计值；表示由机械臂末端辅助坐标系到世界坐标系的旋转矩阵；0₃为零向量。

较佳地，所述得到由机械臂末端指向夹取物的虚拟连杆向量的估计值为：

通过传感器测量得到所述机械臂末端力和力矩h_i；由所述机械臂末端力和力矩h_i得到机械臂末端在平衡条件下的力f_i和机械臂末端在平衡条件下的力矩μ_i；

其中，0₂为2×1的零向量；i为1或2，表示第i个机械臂。

较佳地，所述得到机械臂末端内力偏差Δh_i，包括：

步骤1，通过传感器测量得到所述机械臂末端力和力矩h_i，通过雅可比矩阵将所述机械臂末端力和力矩h_i转换为夹取物质心处的实际力和力矩h_or；

步骤2，根据预先已知的所述夹取物的运动状态，得到夹取物质心处的期望力和力矩h_od；

步骤3，根据所述夹取物质心处的实际力和力矩h_or和所述夹取物质心处的期望力和力矩h_od得到夹取物质心处内力Δh_o＝h_od-h_or；

步骤4，根据所述夹取物质心处内力Δh_o，通过雅可比矩阵得到所述机械臂末端内力偏差Δh_i。

有益效果：

1、本发明通过机械臂刚度参数能够自适应变化的变刚度阻抗方程，将夹取物的期望轨迹χ_od修正为机械臂参考轨迹，并进一步根据此机械臂参考轨迹对双机械臂进行位置控制，减弱的估计偏差影响，提升机械臂的控制精度，以及与环境接触的柔顺性。

2、本发明通过变刚度阻抗方程的设计，以及变刚度阻抗方程中ΔK的具体设计，在阻抗控制的基础上，根据机械臂末端的位置偏差Δχ_i，实时调整阻抗方程的刚度，以对末端位置偏差进行实时补偿；并根据机械臂与物体间的内力偏差Δh_i，实时调整机械臂末端的轨迹，使得机械臂末端在受到外部干扰时，能够根据变刚度阻抗方程，协同调整双机械臂参考轨迹，实现与环境接触的柔顺性。

3、本发明通过将参数加入变刚度阻抗方程的设计，使自适应算法与阻抗控制算法相结合，在进行位置估计补偿的同时提升了机械臂末端控制的柔顺性，提高了计算效率。

4、本发明通过建立机械臂末端辅助坐标系，省去了机械臂末端坐标系到物体坐标系间的复杂姿态变换，进一步提高计算效率。

5、本发明通过虚拟连杆向量的设计建立了力与位置之间的动态关系，将能够实时得到的力组数据转化为位置信息，便于通过变刚度阻抗方程对机械臂进行位置控制和修正。

附图说明

图1为本发明基于实施例的整体流程图；

图2为本发明基于实施例的双臂机器人坐标系及变量定义图；

图3为本发明基于实施例的r_i示意图；

图4为本发明基于实施例的末端辅助参考坐标系示意图；

图5为本发明基于实施例的质心力偏差分解图；

图6为本发明基于实施例的阻抗控制原理图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种基于变阻抗策略的双机械臂末端位置协同控制方法，其核心思想为：

通过机械臂末端的力和力矩h_i得到由机械臂末端指向夹取物的虚拟连杆向量的估计值根据h_i和夹取物的运动状态得到机械臂末端内力偏差Δh_i；设计变刚度阻抗方程，其中机械臂的刚度自适应变化；根据夹取物的期望轨迹χ_od、/>Δh_i和变刚度阻抗方程得到机械臂参考轨迹，将参考轨迹作为位置控制的输入以实现双机械臂的协同柔顺控制。

可见，在机械臂控制过程中，机械臂的末端与被夹取的物体之间的相对位置和作用力关系的描述是机械臂准确、柔顺控制的难点，对于无法准确得到的虚拟连杆向量估计值所带来的机械臂位置控制的低精度和低柔顺性问题，本发明通过机械臂刚度参数能够自适应变化的变刚度阻抗方程，将夹取物的期望轨迹χ_od修正为机械臂参考轨迹，并进一步根据此机械臂参考轨迹对双机械臂进行位置控制，减弱/>的估计偏差影响，提升机械臂的控制精度，以及与环境接触的柔顺性。

下面以一实施例进一步对本发明进行详细说明。

本发明提供了一种基于变阻抗策略的双机械臂末端位置协同控制方法，其流程如图1所示，具体包括：

步骤101、给定物体期望轨迹，具体为：

如图2所示，χ_od表示物体的期望轨迹，预先给定；Σ_w表示世界坐标系；世界坐标系下，表示机械臂末端位姿，包括两部分，平移部分p_i和旋转部分o_i，平移部分由p_i＝[x_i y_i z_i]表示机械臂末端位置，x_i、y_i和z_i分别表示p_i在世界坐标系下的投影，旋转部分由o_i＝[α_i β_i γ_i]表示机械臂末端姿态，α_i、β_i和γ_i分别表示o_i在世界坐标系下的欧拉角。在本发明中，如无特殊说明，以上角标T表示矩阵的转置；以×表示向量的矢积运算；以矩阵A∈R^u表示矩阵A含有u个元素；以/>与/>表示函数A关于时间的二阶导与一阶导；以/>表示物理量A的估计值。

已知旋转部分所表示的机械臂末端姿态为o_i＝[α_i β_i γ_i]，从一个相应坐标系到世界坐标系的旋转矩阵为：

其中，“c”代表“cos”，“s”代表“sin”。

步骤102、估计r_i，具体为：

由物体位姿得到机械臂末端位姿，需要预先估计得到r_i，r_i表示在机械臂末端坐标系下，由机械臂末端指向物体的虚拟连杆向量，如图3所示，记为r_i＝[r_ix r_iy r_iz]^T，r_ix、r_iy和r_iz分别表示r_i的投影。Σ₁和Σ₂分别表示两个机械臂末端坐标系。在每个机械臂末端均安装六维力传感器，六维力传感器可以输出机械臂末端的力和力矩h_i＝[f_ix，f_iy，f_iz，μ_ix，μ_iy，μ_iz]^T∈R⁶，其中力f_i＝[f_ix，f_iy，f_iz]^T∈R³，力矩μ_i＝[μ_ix，μ_iy，μ_iz]^T∈R³，f_ix、f_iy和f_iz分别表示f_i的投影，μ_ix、μ_iy和μ_iz分别表示μ_i的投影。

利用静态条件下的力矩平衡：

其中，0₂为2×1的零向量；^ekr_i表示在坐标系∑_i(i＝1，2)下第i个机械臂的末端到物体质心的向量；f_i∈R³表示第i个末端在平衡条件下的力，μ_i∈R³表示第i个末端在平衡条件下的力矩。

由于力传感器的测量误差、双臂间的内力干扰等因素，测量得到的物理量并不准确，因此基于六维力传感器，以及力平衡方程(II)，计算出的^ekr_i为一个近似值，即为其中/>和/>分别表示/>的投影。

步骤103、优化末端期望轨迹，具体为：

如图4所示，建立辅助末端参考坐标系Σ_i，其与机械臂末端坐标系∑_i具有相同的原点(即机械臂末端点)，与被抓取物体坐标系Σ_o具有相同的坐标方向。通过建立机械臂末端辅助坐标系，省去了机械臂末端坐标系到物体坐标系间的复杂姿态变换，进一步提高计算效率。在进行末端阻抗计算过程中，无法直接获得末端轨迹位置，要由物体质心位置进行计算，以获得末端轨迹位置；对χ_od进行坐标变换，得到坐标系下物体的期望轨迹其中/>表示物体末端到机械臂末端的映射向量，/>表示物体末端坐标系到世界坐标系的旋转矩阵。采用步骤102中得到的估计值进行计算，得到末端位置的估计值/>本发明通过虚拟连杆向量的设计建立了力(h_i)与位置之间的动态关系，将能够实时得到的力组数据转化为位置信息，便于通过变刚度阻抗方程对机械臂进行位置控制和修正。

步骤104、求解内力，具体为：

为保证机械臂运动的协同性，建立机械臂末端基于内力的阻抗控制方程，将机械臂末端与物体的内力和机械臂末端的位置/速度联系起来，根据内力实时修正末端参考轨迹。

进行机械臂末端内力求解，通过雅可比矩阵建立质心处的力和力矩与末端处的力和力矩之间的联系，质心处的力和力矩h_or为：

h_or＝[J_m1 ^T J_m2 ^T … J_mn ^T]h_r＝Θh_r (III)。

其中，J_mk ^T表示末端力到物体质心力的雅可比矩阵，k为从1至n的整数；定义Θ＝[J_m1 ^T J_m2 ^T … J_mn ^T]，是一个行满秩矩阵抓取矩阵；h_r为机械臂实际作用于物体上的力和力矩，h_i代表第i个机械臂末端施加于物体上的力和力矩。本发明中，i为1或2，表示第i个机械臂，i＝0则表示质心处。

已知物体的运动状态，h_od为物体质心处的期望力f_od和期望力矩μ_od的组合，h_od＝[f_od，μ_od]^T，计算物体质心处的期望力f_od和期望力矩μ_od为：

其中，表示物体加速度，ω表示物体角速度，J表示物体转动惯量，M表示物体质量，g为重力加速度，/>表示物体角加速度。

在双臂夹持物体运动时，机械臂实际作用于物体上的力和力矩h_r由六维力传感器测量得到，将机械臂作用于物体的实际力映射到物体的质心，得到物体质心处实际力和力矩h_or＝Θh_r。物体质心处实际力和力矩h_or不完全等于期望的力和力矩h_od，存在一个偏差Δh_o＝h_od-h_or，Δh_o即为质心处内力，如图5所示。质心处内力表示机械臂实际力与期望力不相等，存在大小或者方向上的偏差，需要调整机械臂末端的输出力/轨迹来消除质心处内力。

根据质心处内力计算机械臂末端内力，通过将质心处内力映射到机械臂末端，此时的问题变为求力的映射的逆解：

其中，h_Iri表示物体末端实际内力；为Θ的右伪逆，采用彭罗斯伪逆；为保证质心处的内力和为0，合理选取中间矩阵Φ，使Φ位于抓取矩阵/>的零空间，满足ΘΦ＝0。

由于机械臂与物体的关系为夹持关系，因此将作用于物体质心的期望内力h_Idi设置为0，将质心处内力Δh_o映射到机械臂末端，h_Iri＝ΦΔh_o，则机械臂末端内力偏差Δh_i为：

Δh_i＝h_Idi-h_Iri (VI)。

步骤105、变刚度阻抗控制，具体为：

在阻抗控制的基础上，设计变刚度阻抗控制算法，对r_i的偏差进行分析并进行补偿；具体包括：

先明确基本形式的阻抗方程为：

其中，Δχ_i＝χ_id-χ_ir表示第i个机械臂末端的位置偏差，χ_id表示第i个机械臂末端的期望位置，χ_ir表示第i个机械臂末端的实际位置；Δh_i＝h_id-h_ir表示第i个机械臂末端的期望力组与实际力组的偏差，h_id表示第i个机械臂末端的期望力组，h_i表示第i个机械臂末端的实际力组；M、B和K分别表示惯性、阻尼和刚度矩阵。

如图6所示，之所以建立阻抗方程，是为了通过力传感器得到机器人末端实际力，并与期望力进行比较得到力的偏差值；将力的偏差值带入阻抗方程，得到位置修正量；根据位置修正量对期望位置进行修正，得到物体参考位置；根据物体参考位置，通过位置控制器对机器人进行控制。

为弥补末端估计带来的偏差，在基本阻抗方程(VII)的基础上进一步建立变刚度阻抗控制方程补偿估计误差。首先，根据r_i的估计值，将第i个机械臂位置偏差的理论值Δχ_i与估计值进行比较：

根据理论值Δχ_i与估计值得到末端位置的计算偏差值/>为：

根据可见/>的导数为一常数，0₃为零向量。因此考虑估计偏差设计变刚度阻抗方程为：

其中，ΔK为随时间变化的刚度矩阵K的变化量；为使系统保持稳定，并补偿由于r_i的估计带来的偏差，ΔK以自适应速率变化，具体为：

其中，∈∈R⁶为一个满足的中间向量；Γ∈R^6×6，为对称正定矩阵；∈和Γ的具体选用根据实际场景下的物体轨迹和位置信息等进行调整。本发明中如无特别说明，各估计值通过传感器测量数据计算得到；各理论期望值通过预先已知的物体的运动状态和轨迹得到。本发明通过变刚度阻抗方程的设计，以及变刚度阻抗方程中ΔK的具体设计，在阻抗控制的基础上，根据机械臂末端的位置偏差Δχ_i，实时调整阻抗方程的刚度，以对末端位置偏差进行实时补偿；并根据机械臂与物体间的内力偏差Δh_i，实时调整机械臂末端的轨迹，使得机械臂末端在受到外部干扰时，能够根据变刚度阻抗方程，协同调整双机械臂参考轨迹，实现与环境接触的柔顺性。本发明通过将参数/>加入变刚度阻抗方程的设计，使自适应算法与阻抗控制算法相结合，在进行位置估计补偿的同时提升了机械臂末端控制的柔顺性，提高了计算效率。

步骤106、得到末端参考轨迹以实现协同柔顺控制，具体为：

根据变刚度阻抗方程(X)，将步骤104中的Δh_i作为输入，将Δχ_i作为输出，并根据Δχ_i和得到两机械臂的末端位置修正量/>和/>根据两机械臂的末端位置修正量和/>修正由预先给定的χ_od得到的期望位置轨迹/>和/>以得到参考轨迹和/>作为双臂机器人控制系统的输入，具体为：

根据得到的参考轨迹和/>驱动双臂机器人关节电机进行运动，从而实现机械臂末端的协同柔顺控制。由于实际系统为离散系统，因此需要将系统时域离散化，以T₀为采样周期，一般取T₀＝0.005s，通过采样上一时刻的实际值，并结合期望值作为阻抗控制器的输入，以得到下一时刻的控制参考值。

综上，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于变阻抗策略的双机械臂末端位置协同控制方法，其特征在于，通过机械臂末端力和力矩h_i得到由机械臂末端指向夹取物的虚拟连杆向量估计值根据所述机械臂末端力和力矩h_i和夹取物的运动状态得到机械臂末端内力偏差Δh_i；设计变刚度阻抗方程，其中机械臂的刚度自适应变化；根据夹取物的期望轨迹χ_od、所述虚拟连杆向量估计值/>所述机械臂末端内力偏差Δh_i和所述变刚度阻抗方程得到机械臂参考轨迹，将所述参考轨迹作为位置控制的输入以实现双机械臂的协同柔顺控制。

2.如权利要求1所述的基于变阻抗策略的双机械臂末端位置协同控制方法，其特征在于，所述变刚度阻抗方程，为：

其中，M、B和K分别表示机械臂的惯性、阻尼和刚度矩阵；ΔK为随时间变化的刚度矩阵K的变化量，用于补偿由所述虚拟连杆向量估计值带来的估计偏差；Δχ_i表示第i个机械臂末端期望位置与实际位置间的机械臂末端位置偏差；/>和/>为机械臂末端位置偏差Δχ_i关于时间的二阶导和一阶导；/>为机械臂末端位置的计算偏差值；ΔK以根据实际情形自适应速率变化，为：

其中，∈为一个满足的中间向量；Γ为一个对称正定矩阵；/>表示所述机械臂末端位置偏差Δχ_i的预先估计值；/>表示机械臂末端位置偏差的预先估计值/>关于时间的一阶导。

3.如权利要求2所述的基于变阻抗策略的双机械臂末端位置协同控制方法，其特征在于，所述计算偏差值为：

其中，表示由夹取物到机械臂末端的映射向量的估计值；表示由夹取物到机械臂末端的映射向量的理论期望值；r_i为所述虚拟连杆向量估计值/>的理论期望值；/>表示由机械臂末端辅助坐标系到世界坐标系的旋转矩阵；0₃为零向量。

4.如权利要求2所述的基于变阻抗策略的双机械臂末端位置协同控制方法，其特征在于，所述得到机械臂参考轨迹，为：

对于所述变刚度阻抗方程，以所述机械臂末端内力偏差Δh_i为输入，所述机械臂末端位置偏差Δχ_i为输出；

根据所述虚拟连杆向量估计值和预先给定的所述夹取物的期望轨迹χ_od得到机械臂末端位置估计值/>

根据所述机械臂末端位置偏差Δχ_i和所述机械臂末端位置估计值得到两机械臂的末端位置修正量；

根据所述两机械臂的末端位置修正量，对所述夹取物的期望轨迹χ_od进行修正，得到所述参考轨迹。

5.如权利要求4所述的基于变阻抗策略的双机械臂末端位置协同控制方法，其特征在于，所述得到机械臂的末端位置估计值为：

将世界坐标系下的所述夹取物的期望轨迹χ_od通过坐标变换转为机械臂末端辅助坐标系下的所述机械臂末端位置估计值为：

其中，表示由夹取物到机械臂末端的映射向量的估计值；/>表示由机械臂末端辅助坐标系到世界坐标系的旋转矩阵；0₃为零向量。

6.如权利要求1-5中任意一项所述的基于变阻抗策略的双机械臂末端位置协同控制方法，其特征在于，所述得到由机械臂末端指向夹取物的虚拟连杆向量的估计值为：

通过传感器测量得到所述机械臂末端力和力矩h_i；由所述机械臂末端力和力矩h_i得到机械臂末端在平衡条件下的力f_i和机械臂末端在平衡条件下的力矩μ_i；

其中，0₂为2×1的零向量；i为1或2，表示第i个机械臂。

7.如权利要求1-5中任意一项所述的基于变阻抗策略的双机械臂末端位置协同控制方法，其特征在于，所述得到机械臂末端内力偏差Δh_i，包括：

步骤1，通过传感器测量得到所述机械臂末端力和力矩h_i，通过雅可比矩阵将所述机械臂末端力和力矩h_i转换为夹取物质心处的实际力和力矩h_or；

步骤2，根据预先已知的所述夹取物的运动状态，得到夹取物质心处的期望力和力矩h_od；

步骤3，根据所述夹取物质心处的实际力和力矩h_or和所述夹取物质心处的期望力和力矩h_od得到夹取物质心处内力Δh_o＝h_od-h_or；

步骤4，根据所述夹取物质心处内力Δh_o，通过雅可比矩阵得到所述机械臂末端内力偏差Δh_i。