CN112405531A - 位置域阻抗控制方法、装置、存储介质及作业机器人 - Google Patents

Abstract

本发明提供的一种位置域阻抗控制方法、装置、存储介质及作业机器人，通过建立“位置规划环‑阻抗控制环‑速度控制环”三级级联控制闭环，机器人可以完成预定轨迹的移动，且任何位置轨迹都可以通过位置规划随时叠加，不会引起控制系统的失稳；当机器人碰到固定障碍时，可以立即停止运动，防止碰撞；另外，本发明引入机器人实际运动距离作为系统自变量，机器人停止运动期间，时间的迁移不会导致闭环控制误差的持续积分，不会造成障碍退出后，控制输入的超调，实现了机器人在位置域的位置和力控制。

Description

位置域阻抗控制方法、装置、存储介质及作业机器人

技术领域

本发明涉及机器人技术领域，尤其涉及一种位置域阻抗控制方法、装置、存储介质及作业机器人。

背景技术

目前，机器人作业作为未来的发展趋势，其真实的作业对象往往是非刚体、非固定的，作业环境也是非结构化的。如机器人带电检修架空配电网线路，机器人需要在执行准确、柔顺的作业动作过程中实时应对非确定外部扰动。而现有的作业机器人往往采用位置控制模式，难以适应非结构化、多扰动、强柔性的作业环境。

另一方面，传统的机器人运动跟踪控制系统中参考信号以时间为自变量，应对路径上的障碍往往通过自适应或预观规划，调整轨迹实现避障运动。但是，若轨迹调整不及时，就会出现因为跟踪误差的累计导致反馈输入过大，引起系统振动。

发明内容

本发明的目的旨在至少能解决上述的技术缺陷之一，特别是现有技术中作业机器人采用位置控制模式，应对路径上的障碍通过自适应或预观规划，调整轨迹实现避障运动，难以适应非结构化、多扰动、强柔性的作业环境，以及轨迹调整不及时会导致系统不稳定的技术缺陷。

本发明实施例提供了一种位置域阻抗控制方法，包括：

获取机器人的当前位置、初始位置和目标位置，根据所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息；

获取所述机器人当前的受力信息，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度；

根据所述期望速度和所述当前位置确定所述机器人各个关节的运动速度，并根据所述运动速度驱动各个关节进行周期运动。

可选地，根据所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划速度的步骤，包括：

利用机器人末端位置轨迹函数，并结合所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息；其中，所述规划信息包括规划速度和规划位置。

可选地，所述机器人末端位置轨迹函数采用正弦函数；

利用所述正弦函数，并结合所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息的计算公式如下：

其中，p_d为规划位置，v_d为规划速度，p_i为初始位置，p_f为目标位置，p为机器人末端的当前位置，下标“i”表示起始状态、“f”表示终止状态，T表示轨迹预计执行时间，s表示位置域的单调递增变量，Δs表示无穷小量，由控制系统的频率决定。

可选地，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度的步骤之前，还包括：

在阻抗控制器中构建基于速度的导纳控制模型，通过所述导纳控制模型求解期望速度；

所述导纳控制模型的求解公式如下：

F_spring＝K_springΔp

Δp＝p-p_d

其中，v_c表示期望速度，v_d表示规划速度，F_sensed表示实际采集到的机器人的受力信息，F_spring表示由阻抗控制器中的刚度系数确定的弹性力，K_spring表示刚度系数，Δp表示位置误差，p为机器人末端的当前位置，p_d为规划位置，K_damping表示阻抗控制器中的阻尼系数，ΔF表示无穷小量。

可选地，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度的步骤，包括：

将所述规划信息以及所述机器人当前的受力信息输入到所述导纳控制模型中；

通过导纳控制模型的求解公式，计算所述机器人在柔性坐标系下的期望速度。

可选地，根据所述期望速度和所述当前位置确定所述机器人各个关节的运动速度的步骤，包括：

获取与所述期望速度对应的期望位置，以及与所述当前位置对应的当前速度，将所述期望速度、所述期望位置、所述当前位置和所述当前速度输入到运动控制器中，得到所述运动控制器的输出值；

根据所述当前位置对应的雅可比矩阵和所述运动控制器的输出值计算所述机器人各个关节的运动速度。

可选地，所述运动控制器的输出值的计算公式为：

v_{c_ref}＝v_c+kv(v_c-v)+kp(p_c-p)

其中，v_{c_ref}为运动控制器的输出值，v_c为期望速度，p_c为期望位置，v为当前速度，p为当前位置，k为运动控制器中的运动系数；

根据所述当前位置对应的雅可比矩阵和所述运动控制器的输出值计算所述机器人各个关节的运动速度的计算公式如下：

其中，w为关节角速度，M_jaccobi为雅可比矩阵。

本发明还提供了一种位置域阻抗控制装置，包括：

信息规划单元，用于获取机器人的当前位置、初始位置和目标位置，根据所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息；

导纳控制单元，用于获取所述机器人当前的受力信息，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度；

驱动单元，用于根据所述期望速度和所述当前位置确定所述机器人各个关节的运动速度，并根据所述运动速度驱动各个关节进行周期运动。

本发明还提供了一种存储介质，所述存储介质中存储有计算机可读指令，所述计算机可读指令被一个或多个处理器执行时，使得一个或多个处理器执行如上述实施例中任一项所述位置域阻抗控制方法的步骤。

本发明还提供了一种作业机器人，所述作业机器人包括机械臂和位置域阻抗控制器，所述位置域阻抗控制器在所述机械臂遇到固定障碍时，执行如上述实施例中任一项所述位置域阻抗控制方法的步骤。

从以上技术方案可以看出，本发明实施例具有以下优点：

本发明提供的一种位置域阻抗控制方法、装置、存储介质及作业机器人，包括：获取机器人的当前位置、初始位置和目标位置，根据所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息；获取所述机器人当前的受力信息，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度；根据所述期望速度和所述当前位置确定所述机器人各个关节的运动速度，并根据所述运动速度驱动各个关节进行周期运动。

本发明通过建立“位置规划环-阻抗控制环-速度控制环”三级级联控制闭环，机器人可以完成预定轨迹的移动，且任何位置轨迹都可以通过位置规划随时叠加，不会引起控制系统的失稳；当机器人碰到固定障碍时，可以立即停止运动，防止碰撞；另外，本发明引入机器人实际运动距离作为系统自变量，机器人停止运动期间，时间的迁移不会导致闭环控制误差的持续积分，不会造成障碍退出后，控制输入的超调，实现了机器人在位置域的位置和力控制。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

图1为本发明实施例提供的一种位置域阻抗控制方法的流程示意图；

图2为本发明实施例提供的位置域阻抗控制器的结构示意图；

图3为本发明实施例提供的一种位置域阻抗控制装置的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本申请的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组合。

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)，具有与本申请所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语，应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像本申请实施例中一样被特定定义，否则不会用理想化或过于正式的含义来解释。

请参阅图1，图1为本发明实施例提供的一种位置域阻抗控制方法的流程示意图，本发明实施例提供了一种位置域阻抗控制方法，具体包括如下步骤：

S110：获取机器人的当前位置、初始位置和目标位置，根据所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息。

本申请中，由于现有的作业机器人往往采用位置控制模式，难以适应非结构化、多扰动、强柔性的作业环境；并且，传统的机器人运动跟踪控制系统中参考信号以时间为自变量，应对路径上的障碍往往通过自适应或预观规划来调整轨迹实现避障运动，若轨迹调整不及时，就会出现因为跟踪误差的累计导致反馈输入过大，引起系统振动。

因而，本步骤中，引入机器人实际运动距离作为系统自变量，机器人停止运动期间，时间的迁移不会导致闭环控制误差的持续积分，不会造成障碍退出后，控制输入的超调，实现了机器人在位置域的位置和力控制。

具体地，在机器人作业过程中，实时获取机器人的末端位置，即机器人的当前位置，以及机器人的初始位置和目标位置；当得到机器人的当前位置、初始位置以及目标位置后，即可计算出机器人在刚性坐标系下的规划信息。

进一步地，规划信息的计算过程可通过规划器实现，该规划器为机器人末端位置轨迹函数。

可以理解的是，这里的机器人包括但不限于作业机器人，这里的规划信息包括但不限于规划速度和规划位置。

S120：获取所述机器人当前的受力信息，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度。

本步骤中，对于机器人在运动过程中触碰到障碍物时产生的正压力，或者人为拉扯机器人末端时产生的拉力，均可通过力传感器进行采集，采集到的的机器人当前的受力信息，可与之前计算的机器人的规划信息一同计算机器人在柔性坐标系下的期望速度。

进一步地，该计算过程可通过规划控制器实现，规划控制器中构建有基于速度的导纳控制模型，通过该导纳控制模型即可计算机器人在柔性坐标系下的期望速度。

S130：根据所述期望速度和所述当前位置确定所述机器人各个关节的运动速度，并根据所述运动速度驱动各个关节进行周期运动。

本步骤中，当通过步骤S120得到机器人在柔性坐标系下的期望速度后，可利用机器人当前位置对应的雅可比矩阵与柔性坐标系下的期望速度计算出机器人各个关节的运动速度，该计算过程可通过运动控制器来实现。

需要说明的是，由于步骤S110中得到的规划信息为下一个控制周期的规划信息，步骤S120中得到的期望速度也是下一个控制周期的期望速度，且两个控制周期可以是一致的，也可以是不一致的，因而，最终各个关节所做的运动也是周期性运动。

另外，这里的控制周期是由机器人的硬件采样周期决定的，外部传感器的采样周期一般都高于机器人的采样周期，这样一旦拿到机器人的实时位置就能进行计算，并将控制输出传输给机器人。

上述实施例中，通过建立“位置规划环-阻抗控制环-速度控制环”三级级联控制闭环，机器人可以完成预定轨迹的移动，且任何位置轨迹都可以通过位置规划随时叠加，不会引起控制系统的失稳；当机器人碰到固定障碍时，可以立即停止运动，防止碰撞；另外，本发明引入机器人实际运动距离作为系统自变量，机器人停止运动期间，时间的迁移不会导致闭环控制误差的持续积分，不会造成障碍退出后，控制输入的超调，实现了机器人在位置域的位置和力控制。

在一个实施例中，步骤S110中根据所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划速度的步骤，可以包括：

利用机器人末端位置轨迹函数，并结合所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息；其中，所述规划信息包括规划速度和规划位置。

本实施例中，计算机器人在刚性坐标系下的规划信息可通过规划器来实现，规划器为机器人末端位置轨迹函数，该函数可以采用正弦函数，也可以采用其他任意函数，只要能够根据机器人的当前位置、初始位置以及目标位置规划出机器人在下一个控制周期的运动轨迹即可。

在一个实施例中，如图2所示，图2为本发明实施例提供的位置域阻抗控制器的结构示意图；所述机器人末端位置轨迹函数采用正弦函数；利用所述正弦函数，并结合所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息的计算公式如下：

其中，p_d为规划位置，v_d为规划速度，p_i为初始位置，p_f为目标位置，p为机器人末端的当前位置，下标“i”表示起始状态、“f”表示终止状态，T表示轨迹预计执行时间，s表示位置域的单调递增变量，Δs表示无穷小量，由控制系统的频率决定。

本实施例中，采用正弦的好处是正弦的位置函数，对应余弦的速度函数，初始时刻速度为0，符合实际。

在一个实施例中，如图2所示，步骤S120中根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度的步骤之前，还可以包括：

在阻抗控制器中构建基于速度的导纳控制模型，通过所述导纳控制模型求解期望速度；

所述导纳控制模型的求解公式如下：

F_spring＝K_springΔp

Δp＝p-p_d

其中，v_c表示期望速度，v_d表示规划速度，F_sensed表示实际采集到的机器人的受力信息，F_spring表示由阻抗控制器中的刚度系数确定的弹性力，K_spring表示刚度系数，Δp表示位置误差，p为机器人末端的当前位置，p_d为规划位置，K_damping表示阻抗控制器中的阻尼系数，ΔF表示无穷小量。

在一个实施例中，步骤S120中根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度的步骤，可以包括：

S121：将所述规划信息以及所述机器人当前的受力信息输入到所述导纳控制模型中；

S122：通过导纳控制模型的求解公式，计算所述机器人在柔性坐标系下的期望速度。

本实施例中，由导纳控制模型的求解公式可知，机器人在柔性坐标系下的期望速度与机器人的受力情况以及下一控制周期的规划速度有关，当确定好机器人当前的受力信息以及下一控制周期的规划速度后，即可根据导纳控制模型求解出机器人在柔性坐标系下的虚拟的末端理想值，即期望速度。

在一个实施例中，步骤S130中根据所述期望速度和所述当前位置确定所述机器人各个关节的运动速度的步骤，包括：

S131：获取与所述期望速度对应的期望位置，以及与所述当前位置对应的当前速度，将所述期望速度、所述期望位置、所述当前位置和所述当前速度输入到运动控制器中，得到所述运动控制器的输出值；

S131：根据所述当前位置对应的雅可比矩阵和所述运动控制器的输出值计算所述机器人各个关节的运动速度。

本实施例中，如图2所示，当计算得到机器人在柔性坐标系下的期望速度后，即可根据该期望速度获取相应的期望位置，并且，在获取机器人的当前位置的同时，还可获取其对应的当前速度，这样，运动控制器根据机器人的期望速度、期望位置、当前速度以及当前位置，即可得到对应的输出。

当得到运动控制器的输出后，可结合机器人的当前位置对应的雅可比矩阵计算出机器人的运动速度，从而驱动相应的关节驱动器，即图2中的执行器与环境交互来实现机器人的周期运动。

在一个实施例中，所述运动控制器的输出值的计算公式为：

v_{c_ref}＝v_c+kv(v_c-v)+kp(p_c-p)

其中，v_{c_ref}为运动控制器的输出值，v_c为期望速度，p_c为期望位置，v为当前速度，p为当前位置，k为运动控制器中的运动系数；

根据所述当前位置对应的雅可比矩阵和所述运动控制器的输出值计算所述机器人各个关节的运动速度的计算公式如下：

其中，w_d为关节角速度，M_jaccobi为雅可比矩阵，图2中的q_d为关节角度。

在一个实施例中，如图3所示，图3为本发明实施例提供的一种位置域阻抗控制装置的结构示意图；图3中，本发明还提供了一种位置域阻抗控制装置，包括信息规划单元110、导纳控制单元120、驱动单元130，具体包括如下：

信息规划单元110，用于获取机器人的当前位置、初始位置和目标位置，根据所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息；

导纳控制单元120，用于获取所述机器人当前的受力信息，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度；

驱动单元130，用于根据所述期望速度和所述当前位置确定所述机器人各个关节的运动速度，并根据所述运动速度驱动各个关节进行周期运动。

上述实施例中，通过建立“位置规划环-阻抗控制环-速度控制环”三级级联控制闭环，机器人可以完成预定轨迹的移动，且任何位置轨迹都可以通过位置规划随时叠加，不会引起控制系统的失稳；当机器人碰到固定障碍时，可以立即停止运动，防止碰撞；另外，本发明引入机器人实际运动距离作为系统自变量，机器人停止运动期间，时间的迁移不会导致闭环控制误差的持续积分，不会造成障碍退出后，控制输入的超调，实现了机器人在位置域的位置和力控制。

在一个实施例中，本发明还提供了一种存储介质，所述存储介质中存储有计算机可读指令，所述计算机可读指令被一个或多个处理器执行时，使得一个或多个处理器执行如上述实施例中任一项所述位置域阻抗控制方法的步骤。

在一个实施例中，本发明还提供了一种作业机器人，所述作业机器人包括机械臂和位置域阻抗控制器，所述位置域阻抗控制器在所述机械臂遇到固定障碍时，执行如上述实施例中任一项所述位置域阻抗控制方法的步骤。

以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种位置域阻抗控制方法，其特征在于，包括：

获取机器人的当前位置、初始位置和目标位置，根据所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息；

获取所述机器人当前的受力信息，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度；

根据所述期望速度和所述当前位置确定所述机器人各个关节的运动速度，并根据所述运动速度驱动各个关节进行周期运动。

2.根据权利要求1所述的一种位置域阻抗控制方法，其特征在于，根据所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划速度的步骤，包括：

利用机器人末端位置轨迹函数，并结合所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息；其中，所述规划信息包括规划速度和规划位置。

3.根据权利要求2所述的一种位置域阻抗控制方法，其特征在于，所述机器人末端位置轨迹函数采用正弦函数；

利用所述正弦函数，并结合所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息的计算公式如下：

其中，p_d为规划位置，v_d为规划速度，p_i为初始位置，p_f为目标位置，p为机器人末端的当前位置，下标“i”表示起始状态、“f”表示终止状态，T表示轨迹预计执行时间，s表示位置域的单调递增变量，Δs表示无穷小量，由控制系统的频率决定。

4.根据权利要求1所述的一种位置域阻抗控制方法，其特征在于，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度的步骤之前，还包括：

在阻抗控制器中构建基于速度的导纳控制模型，通过所述导纳控制模型求解期望速度；

所述导纳控制模型的求解公式如下：

F_spring＝K_springΔp

Δp＝p-p_d

其中，v_c表示期望速度，v_d表示规划速度，F_sensed表示实际采集到的机器人的受力信息，F_spring表示由阻抗控制器中的刚度系数确定的弹性力，K_spring表示刚度系数，Δp表示位置误差，p为机器人末端的当前位置，p_d为规划位置，K_damping表示阻抗控制器中的阻尼系数，ΔF表示无穷小量。

5.根据权利要求4所述的一种位置域阻抗控制方法，其特征在于，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度的步骤，包括：

将所述规划信息以及所述机器人当前的受力信息输入到所述导纳控制模型中；

通过导纳控制模型的求解公式，计算所述机器人在柔性坐标系下的期望速度。

6.根据权利要求1所述的一种位置域阻抗控制方法，其特征在于，根据所述期望速度和所述当前位置确定所述机器人各个关节的运动速度的步骤，包括：

获取与所述期望速度对应的期望位置，以及与所述当前位置对应的当前速度，将所述期望速度、所述期望位置、所述当前位置和所述当前速度输入到运动控制器中，得到所述运动控制器的输出值；

根据所述当前位置对应的雅可比矩阵和所述运动控制器的输出值计算所述机器人各个关节的运动速度。

7.根据权利要求6所述的一种位置域阻抗控制方法，其特征在于，所述运动控制器的输出值的计算公式为：

v_{c_ref}＝v_c+kv(v_c-v)+kp(p_c-p)

其中，v_{c_ref}为运动控制器的输出值，v_c为期望速度，p_c为期望位置，v为当前速度，p为当前位置，k为运动控制器中的运动系数；

根据所述当前位置对应的雅可比矩阵和所述运动控制器的输出值计算所述机器人各个关节的运动速度的计算公式如下：

其中，w为关节角速度，M_jaccobi为雅可比矩阵。

8.一种位置域阻抗控制装置，其特征在于，包括：

信息规划单元，用于获取机器人的当前位置、初始位置和目标位置，根据所述当前位置、所述初始位置以及所述目标位置确定所述机器人在刚性坐标系下的规划信息；

导纳控制单元，用于获取所述机器人当前的受力信息，根据所述规划信息以及所述受力信息确定所述机器人在柔性坐标系下的期望速度；

驱动单元，用于根据所述期望速度和所述当前位置确定所述机器人各个关节的运动速度，并根据所述运动速度驱动各个关节进行周期运动。

9.一种存储介质，其特征在于：所述存储介质中存储有计算机可读指令，所述计算机可读指令被一个或多个处理器执行时，使得一个或多个处理器执行如权利要求1至7中任一项所述位置域阻抗控制方法的步骤。

10.一种作业机器人，其特征在于：所述作业机器人包括机械臂和位置域阻抗控制器，所述位置域阻抗控制器在所述机械臂遇到固定障碍时，执行如权利要求1至7中任一项所述位置域阻抗控制方法的步骤。

Images