CN116968016A - 液压臂末端速度可行空间的构建方法及可视化交互系统 - Google Patents

Abstract

本发明涉及一种液压臂末端速度可行空间的构建方法及可视化交互系统，首先根据关节极限、系统流量和障碍物等约束生成关节空间的约束不等式，并根据约束不等式生成关节空间多面体，然后通过雅可比矩阵将关节空间多面体映射成任务空间多面体，从而实现液压臂在复杂障碍空间下的末端速度可行空间构建；解决了液压机械臂在运行中存在多种相互影响的约束，使得末端的指令速度与系统实际能达到或被允许的速度不匹配，从而制约着液压机械臂的工作效率和安全等级的问题。

Description

液压臂末端速度可行空间的构建方法及可视化交互系统

技术领域

本发明涉及液压机械臂控制技术领域，涉及一种液压臂末端速度可行空间的构建方法及可视化交互系统，尤其涉及一种可广泛应用于在复杂工况或多重约束下的多自由度液压臂辅助操作中的液压臂末端速度可行空间的构建方法及可视化交互系统。

背景技术

液压机械臂因其较大的功率重量比已被应用于建筑、采矿和林业等诸多领域。由于液压机械臂普遍采取直线油缸/旋转马达的驱动方式，与电驱机械臂相比，其非线性、强耦合、大惯量等特点十分明显，加之其工作环境多为复杂、非结构化，给操作人员带来了较差的操作体验和更高的安全风险，且带来了高昂的操作培训成本。同时，由于液压机械臂在运行中存在多种相互影响的约束，包括关节极限、环境碰撞、速度过冲、加速度过大等，使得末端的指令速度与系统实际能达到或被允许的速度不匹配，如由于关节位置极限、系统压力受限等约束导致指令速度不能实现，又如由于环境障碍物约束导致指令速度不被允许，从而制约着液压机械臂的工作效率和安全等级。因此，如何使操作者实时了解机械臂末端的可行速度非常必要，可以帮助其在指令速度不能实现或者不被允许时迅速调整操作策略，对于改善操作体验、提高安全性和操作效率具有十分重要的意义。

发明内容

有鉴于此，本发明为了解决液压机械臂在运行中存在多种相互影响的约束，使得末端的指令速度与系统实际能达到或被允许的速度不匹配，从而制约着液压机械臂的工作效率和安全等级的问题，提供一种液压臂末端速度可行空间的构建方法及可视化交互系统。

为达到上述目的，本发明提供如下技术方案：

一种液压臂末端速度可行空间的构建方法，首先根据关节极限、系统流量和障碍物等约束生成关节空间的约束不等式，并根据约束不等式生成关节空间多面体，然后通过雅可比矩阵将关节空间多面体映射成任务空间多面体，从而实现液压臂在复杂障碍空间下的末端速度可行空间构建。

该液压臂末端速度可行空间的构建方法，包括以下步骤：

Step1、对于n自由度机械臂，记机械臂的末端中心点为TCP，记TCP位姿信息为T(q)，根据机械臂正运动学建立T(q)表达式为：

式(21)中，q＝[q₁…q_n]^T∈R^n×1为获得的机械臂的关节角向量，记操作人员对TCP的线速度控制为建立/>表达式为：

式(22)中，为获得的机械臂的关节角速度向量，J(q)∈R^m×n为该机械臂的雅克比矩阵，机械臂逆运动学公式为：

式(23)中，J⁺为J的伪逆；

Step2、对于n自由度机械臂，建立第i个关节的运动学约束表达式为：

式(24)中，分别关节i的角度、角速度、角加速度的初始设定最小值，分别为关节i的角度、角速度、角加速度的初始设定最大值，关节角度约束考虑了关节极限、避障碍物以及系统最大流量；式(24)构成了关节i角度、角速度、角加速度的上下边界；记式(24)中的角速度约束为第一次约束：

为防止关节在接近极限时因加速度过大而超出角度极限，将角度约束转变为角速度约束，视作对角速度进行第二次约束，记第i个关节的角速度最大值为最小值为/>建立/>和/>的表达式为：

式(26)中，ΔT为采样时间间隔，因此，建立第i个关节的角速度第二次约束表达式为：

根据加速度定义，式(24)中的角加速度约束表达式变为：

式(28)中，为上一个采样时刻的角速度，根据式(28)将角加速度约束转变为角速度约束，视作对角速度进行第三次约束，记第i个关节的角速度最大值为/>最小值为/>建立/>和/>的表达式为：

因此，建立第i个关节的角速度第三次约束表达式为：

结合式(25)、式(27)、式(30)，将式(24)表示为：

式(31)中，

Step3、对于n自由度机械臂，建立第i个关节的流量计算式为：

式(32)中，A_ai和A_bi分别代表第i个关节的驱动液压缸中的无杆腔面积和有杆腔面积，代表第i个关节的运动学微分关系，建立所有关节的总流量表达式为：

对于n自由度机械臂，记关节1与关节2之间为第1节臂，以此类推，建立第j节臂的障碍物约束表达式为：

式(34)中，r_jk＝[x_jk,y_jk,z_jk]^T为第j节臂与第k个障碍物之间的最短向量，J_j为第j节臂上组成最短向量的点的雅克比矩阵，φ_d为安全系数，建立所有机械臂段与第k个障碍物的约束表达式为：

将式(35)简写为：

建立所有障碍物的总约束表达式为：

Step4、结合式(31)、式(33)、式(37)，将运动学、系统流量和障碍物约束条件表示一个由超平面构成的关节空间多面体，记此关节空间多面体为 表达式为：

式(38)中，

A(q)用于计算系统流量，Q_max＝Q_imax·[1,…,1]^T，O(q)＝[O₁,O₂,...,O_m]^T，B＝[b₁,b₂,...,b_m]^T；

将用超平面表示的关节空间多面体转换为用顶点表示的多面体，获得多面体顶点，然后通过式(22)将多面体顶点一一映射至末端速度顶点，记多面体顶点顶点合集为P，记末端速度约束平面顶点合集为V，建立V与P的表达式如下：

根据末端速度约束平面顶点合集形成任务空间多面体，进而得到V的凸集；由V得到的凸多面体构成上述液压臂末端速度的可行空间。

一种液压臂末端速度可行空间的可视化交互系统，包括用于操作人员(1)输入操作信息的输入设备(2)和获得操作实时信息的虚拟现实显示屏(3)、用于计算操作信息和生成末端速度可行空间的控制系统以及用于响应操作输入信息的被控对象，其中控制系统包括控制计算机(4)和人机界面生成程序(5)，被控对象包括实际工作场景(7)、数据采集卡(8)和相机(9)组成，控制系统和被控对象之间的信息交互由信息传输设备(6)实现。

进一步，该可视化交互系统的工作原理包括以下步骤：首先通过信息传输设备(6)将数据采集卡(8)和相机(9)获得的机械臂运动学信息与实际工作场景(7)图像信息传递至控制计算机(4)，然后控制其内置的机械臂模型和建筑信息模型做出相同的动作，通过人机界面生成程序(5)将机械臂模型、建筑物信息模型组合成虚拟建模场景，将输入速度指令与末端速度可行空间组成速度可行空间画面，将虚拟建模场景和速度可行空间画面显示在同一虚拟现实显示屏(3)上，从而实现液压臂末端速度可行空间可视化交互系统。

进一步，输入速度指令，即TCP线速度由操作输入设备决定，记输入速度指令在任务空间的顶点为S，记操作输入设备在三维空间中的输入为[x,y,z]^T，建立S与[x,y,z]^T的关系表达式：

式(40)中，k₁、k₂、k₃分别为x、y、z的缩放比例。

进一步，信息传输设备需实现多种信号并行实时传递，包括但不限于局域网服务器、蓝牙设备、线路信号箱等。

进一步，控制计算机需内置机械臂模型和建筑物信息模型，在运行中依靠运动学信息和图像信息共同控制模型做出运动。

进一步，控制计算机将由V得到的凸多面体与S显示至同一空间中，形成末端速度可行空间画面，信息传输设备将数据采集卡和相机获得的运动学信息与实际工作场景图像信息传递至控制计算机，控制计算机根据信息控制内置的机械臂模型和建筑信息模型做出相同的动作，与实际机械臂动作同步的虚拟样机模型、周围环境障碍物虚拟模型形成虚拟建模画面，末端速度可行空间画面与虚拟建模画面实时同步展示在同一虚拟现实显示屏(3)上供操作人员观看，从而为操作人员提供实时操作辅助，从而实现液压臂末端速度可行空间的可视化交互。

进一步，数据采集卡安装于机械臂上，获取信息包括但不限于关节角度、关节角速度、关节角加速度，各个臂段末端速度、末端加速度等信息，所述相机的视野范围需完整覆盖实际工作场景中的机械臂与工作区域。

本方案的工作原理及有益效果在于：

1、本发明所公开的液压臂末端速度可行空间的可视化交互系统，液压臂末端速度可行空间构建方法根据关节极限、系统流量和障碍物约束等条件生成末端速度约束不等式，将所有约束不等式综合表征为保证末端速度可行性的几何多面体，从而实现液压臂在复杂作业条件下的末端速度可行空间构建。液压臂末端速度可行空间可视化交互系统将机械臂输入速度指令和几何多面体映射至同一空间上，将与实际机械臂动作同步的虚拟样机模型、周围环境障碍物虚拟模型和液压臂末端速度可行空间显示至同一可视化交互系统中，从而为操作人员提供实时操作辅助，从而实现液压臂末端速度可行空间可视化交互系统。

2、本发明所公开的液压臂末端速度可行空间的可视化交互系统，可以快速实时计算出液压机械臂的末端安全速度空间、使操作人员直观地感受液压机械臂的可操作范围，对于改善操作体验、提高安全性、提升操作效率具有十分重要的意义。同时，该液压臂末端速度可行空间的构建方法易于实现、过程清晰，各个约束明确简洁、各个部件容易获取，具有十分可观的工程应用前景。

3、本发明所公开的液压臂末端速度可行空间的可视化交互系统，综合考虑了液压臂因非线性、强耦合、大惯量等自身特点和非结构化作业环境带来的多重操作约束问题，可以更加直观地为操作人员提供辅助参考，降低了复杂工况下操作难度，避免与周围障碍物碰撞等问题，提高了操作效率与安全性。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

图1为本发明液压臂末端速度可行空间的可视化交互系统的整体设计框图；

图2为本发明液压臂末端速度可行空间的构建方法框图；

图3为本发明液压臂末端速度可行空间的可视化交互系统框图；

图4为本发明液压臂末端速度可行空间的可视化交互系统的应用示例图；

图5为本发明液压臂末端速度可行空间的可视化交互系统的试验效果统计图,图5(a)为工作时间对比图，图5(b)为碰撞次数对比图。

附图中标记如下：操作人员1、输入设备2、虚拟现实显示屏3、控制计算机4、人机界面生成程序5、信息传输设备6、实际工作场景7、数据采集卡8、相机9。

具体实施方式

下面通过具体实施方式进一步详细说明：

如图1所示的一种液压臂末端速度可行空间的可视化交互系统，包括用于操作人员1输入操作信息的输入设备2和获得操作实时信息的虚拟现实显示屏3、用于计算操作信息和生成末端速度可行空间的控制系统以及用于响应操作输入信息的被控对象，其中控制系统包括控制计算机4和人机界面生成程序5，被控对象包括实际工作场景7、数据采集卡8和相机9组成，控制系统和被控对象之间的信息交互由信息传输设备6实现。

该可视化交互系统的工作原理包括以下步骤：首先通过信息传输设备6将数据采集卡8和相机9获得的机械臂运动学信息与实际工作场景7图像信息传递至控制计算机4，然后控制其内置的机械臂模型和建筑信息模型做出相同的动作，通过人机界面生成程序5将机械臂模型、建筑物信息模型组合成虚拟建模场景，将输入速度指令与末端速度可行空间组成速度可行空间画面，将虚拟建模场景和速度可行空间画面显示在同一虚拟现实显示屏3上，从而实现液压臂末端速度可行空间可视化交互系统。

实施例

如图2所示，针对一个平面三自由度大型液压柔性机械臂，数据采集卡获得机械臂实时运动学信息后，首先根据下列三个约束式生成运动学约束集，运动学约束计算式如下：

得运动学约束集如下：

式中：

然后根据下列两个约束式生成流量约束集，流量约束计算式如下：

然后根据下列两个约束式生成障碍物约束集，障碍物约束计算式如下：

由三个约束集得一个由超平面构成的关节空间多面体计算式如下：

式中：Q_max＝Q_imax·[1,…,1]^T，O(q)＝[O₁,O₂,...,O_m]^T，B＝[b₁,b₂,...,b_m]^T B＝[b₁ b₂ .. .b_m]^T。

记多面体顶点顶点合集为P，接着通过机械臂雅克比矩阵将P映射至末端速度约束顶点集V，计算式如下：

最后根据末端速度约束平面顶点合集可形成任务空间多面体，通过Graham扫描法得到V的凸集。由V得到的凸多面体构成末端速度可行空间。至此，末端速度可行空间计算完毕。

如图3所示，首先将数据采集卡8和相机9得到的运动学信息和图像信息传递至控制计算机4；其次控制计算机4根据获得的信息，驱动内置的机械臂模型和建筑信息模型做出响应，生成虚拟建模画面；然后控制计算机4根据人机界面生成程序5的末端速度约束顶点集V的凸多面体和输入速度指令S生成末端速度可行空间画面；最后控制计算机4将与实际机械臂动作同步的虚拟样机模型、周围环境障碍物虚拟模型和速度可行空间共同显示至虚拟现实显示屏3中，为操作人员提供辅助，完成可行空间的可视化交互。其中，输入速度指令由操作输入设备决定，记输入速度指令的顶点为S，记操作输入设备在三维空间中的输入为[x,y,z]^T，建立S与[x,y,z]^T的关系表达式：

k₁、k₂、k₃分别为x、y、z的缩放比例。

如图4所示液压臂末端速度可行空间的可视化交互系统的应用示例图，此为根据本方法生成的三节重载液压臂末端速度可行空间构建方法及可视化交互系统示意图，其中左上角为速度可行空间画面，右侧为虚拟建模画面，操作者可根据左上角的画面判断操作速度是否超出安全范围，也可根据右侧虚拟建模画面直观感受施工情况。

如图5所示，此为采用图4所示的三节重载液压臂末端速度可行空间构建方法及可视化交互系统所进行的试验效果图，由如图5(a)和图5(b)图示可知，采用本方法的实验组可减少工作时间并减少碰撞次数，因此将本方法用于液压臂的辅助操作上，可以改善操作人员的操作体验、加快工作效率并提高操作安全性。

以上所述的仅是本发明的实施例，方案中公知的具体结构及特性等常识在此未作过多描述。应当指出，对于本领域的技术人员来说，在不脱离本发明结构的前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些都不会影响本发明实施的效果和本发明的实用性。

Claims (9)

1.一种液压臂末端速度可行空间的构建方法，其特征在于：首先根据关节极限、系统流量和障碍物约束生成关节空间的约束不等式，并根据约束不等式生成关节空间多面体，然后通过雅可比矩阵将关节空间多面体映射成任务空间多面体，从而实现液压臂在复杂障碍空间下的末端速度可行空间构建。

2.根据权利要求1所述液压臂末端速度可行空间的构建方法，其特征在于：包括以下步骤：

Step1、对于n自由度机械臂，记机械臂的末端中心点为TCP，记TCP位姿信息为T(q)，根据机械臂正运动学建立T(q)表达式为：

式(1)中，q＝[q₁…q_n]^T∈R^n×1为获得的机械臂的关节角向量，记操作人员对TCP的线速度控制为建立/>表达式为：

式(2)中，为获得的机械臂的关节角速度向量，J(q)∈R^m×n为该机械臂的雅克比矩阵，机械臂逆运动学公式为：

式(3)中，J⁺为J的伪逆；

Step2、对于n自由度机械臂，建立第i个关节的运动学约束表达式为：

式(4)中，分别关节i的角度、角速度、角加速度的初始设定最小值，/>分别为关节i的角度、角速度、角加速度的初始设定最大值，关节角度约束考虑了关节极限、避障碍物以及系统最大流量；式(4)构成了关节i角度、角速度、角加速度的上下边界；记式(4)中的角速度约束为第一次约束：

为防止关节在接近极限时因加速度过大而超出角度极限，将角度约束转变为角速度约束，视作对角速度进行第二次约束，记第i个关节的角速度最大值为最小值为/>建立和/>的表达式为：

式(6)中，ΔT为采样时间间隔，因此，建立第i个关节的角速度第二次约束表达式为：

根据加速度定义，式(4)中的角加速度约束表达式变为：

式(8)中，为上一个采样时刻的角速度，根据式(8)将角加速度约束转变为角速度约束，视作对角速度进行第三次约束，记第i个关节的角速度最大值为/>最小值为/>建立/>和/>的表达式为：

因此，建立第i个关节的角速度第三次约束表达式为：

结合式(5)、式(7)、式(10)，将式(4)表示为：

式(11)中，

Step3、对于n自由度机械臂，建立第i个关节的流量计算式为：

式(12)中，A_ai和A_bi分别代表第i个关节的驱动液压缸中的无杆腔面积和有杆腔面积，代表第i个关节的运动学微分关系，建立所有关节的总流量表达式为：

对于n自由度机械臂，记关节1与关节2之间为第1节臂，以此类推，建立第j节臂的障碍物约束表达式为：

式(14)中，r_jk＝[x_jk,y_jk,z_jk]^T为第j节臂与第k个障碍物之间的最短向量，J_j为第j节臂上组成最短向量的点的雅克比矩阵，φ_d为安全系数，建立所有机械臂段与第k个障碍物的约束表达式为：

将式(15)简写为：

建立所有障碍物的总约束表达式为：

Step4、结合式(11)、式(13)、式(17)，将运动学、系统流量和障碍物约束条件表示一个由超平面构成的关节空间多面体，记此关节空间多面体为 表达式为：

式(18)中，A(q)用于计算系统流量，Q_max＝Q_imax·[1,…,1]^T，O(q)＝[O₁,O₂,...,O_m]^T，B＝[b₁,b₂,...,b_m]^T；

将用超平面表示的关节空间多面体转换为用顶点表示的多面体，获得多面体顶点，然后通过式(2)将多面体顶点一一映射至末端速度顶点，记多面体顶点顶点合集为P，记末端速度约束平面顶点合集为V，建立V与P的表达式如下：

根据末端速度约束平面顶点合集形成任务空间多面体，进而得到V的凸集；由V得到的凸多面体构成上述液压臂末端速度的可行空间。

3.基于权利要求2所述的一种液压臂末端速度可行空间的构建方法的可视化交互系统，其特征在于：包括用于操作人员(1)输入操作信息的输入设备(2)和获得操作实时信息的虚拟现实显示屏(3)、用于计算操作信息和生成末端速度可行空间的控制系统以及用于响应操作输入信息的被控对象，其中控制系统包括控制计算机(4)和人机界面生成程序(5)，被控对象包括实际工作场景(7)、数据采集卡(8)和相机(9)组成，控制系统和被控对象之间的信息交互由信息传输设备(6)实现。

4.根据权利要求3所述的一种液压臂末端速度可行空间的可视化交互系统，其特征在于：包括以下步骤：首先通过信息传输设备(6)将数据采集卡(8)和相机(9)获得的机械臂运动学信息与实际工作场景(7)图像信息传递至控制计算机(4)，然后控制其内置的机械臂模型和建筑信息模型做出相同的动作，通过人机界面生成程序(5)将机械臂模型、建筑物信息模型组合成虚拟建模场景，将输入速度指令与末端速度可行空间组成速度可行空间画面，将虚拟建模场景和速度可行空间画面显示在同一虚拟现实显示屏(3)上，从而实现液压臂末端速度可行空间可视化交互系统。

5.基于权利要求4所述的一种液压臂末端速度可行空间的可视化交互系统，其特征在于：输入速度指令，即TCP线速度由操作输入设备决定，记输入速度指令在任务空间的顶点为S，记操作输入设备在三维空间中的输入为[x,y,z]^T，建立S与[x,y,z]^T的关系表达式：

式(20)中，k₁、k₂、k₃分别为x、y、z的缩放比例。

6.基于权利要求5所述的一种液压臂末端速度可行空间的可视化交互系统，其特征在于：所述信息传输设备需实现多种信号并行实时传递，包括但不限于局域网服务器、蓝牙设备、线路信号箱。

7.基于权利要求5所述的一种液压臂末端速度可行空间的可视化交互系统，其特征在于：所述控制计算机需内置机械臂模型和建筑物信息模型，在运行中依靠运动学信息和图像信息共同控制模型做出运动。

8.基于权利要求5所述的一种液压臂末端速度可行空间的可视化交互系统，其特征在于：所述控制计算机将由V得到的凸多面体与S显示至同一空间中，形成末端速度可行空间画面，信息传输设备将数据采集卡和相机获得的运动学信息与实际工作场景图像信息传递至控制计算机，控制计算机根据信息控制内置的机械臂模型和建筑信息模型做出相同的动作，与实际机械臂动作同步的虚拟样机模型、周围环境障碍物虚拟模型形成虚拟建模画面，末端速度可行空间画面与虚拟建模画面实时同步展示在同一虚拟现实显示屏(3)上供操作人员观看，从而为操作人员提供实时操作辅助，从而实现液压臂末端速度可行空间的可视化交互。

9.基于权利要求5所述的一种液压臂末端速度可行空间的可视化交互系统，其特征在于：所述数据采集卡安装于机械臂上，获取信息包括但不限于关节角度、关节角速度、关节角加速度以及各个臂段末端速度、末端加速度，所述相机的视野范围需完整覆盖实际工作场景中的机械臂与工作区域。