CN113146617A

CN113146617A - 一种柔性机器人动力学建模及轨迹跟随控制方法及装置

Info

Publication number: CN113146617A
Application number: CN202110228437.0A
Authority: CN
Inventors: 彭键清; 韩瑜; 吴皓轩
Original assignee: Sun Yat Sen University
Current assignee: Sun Yat Sen University
Priority date: 2021-03-02
Filing date: 2021-03-02
Publication date: 2021-07-23

Abstract

本发明公开了一种柔性机器人动力学建模及轨迹跟随控制方法及装置，根据“离散式刚性连杆+联动机构”构型绳索驱动串联机器人的机构几何参数，借助D‑H参数法建立起关节‑末端的运动学模型；再根据关节间绳索传动的关系，建立了“绳索‑关节‑末端”的多重运动学耦合模型。进一步地，提出了分段联动式串‑并联混合的动力学求解方法，利用主从被动式的传动特点，通过对驱动绳索与联动绳索作用力进行求解，推导并建立了绳索驱动串联机器人的串‑并联混合的动力学模型。并且将所得动力学模型作为控制器的前馈控制输入，补偿运动误差，同时设计非线性二次规划模块，对绳索拉力和操作空间精度指标的进行加权优化，实现了较高精度的末端操作空间位姿的跟随运动，并使得绳索拉力和末端位姿的同步优化。

Description

一种柔性机器人动力学建模及轨迹跟随控制方法及装置

技术领域

本发明涉及机器人控制领域，尤其是一种柔性机器人动力学建模及轨迹跟随控制方法及装置。

技术背景

如今对智能机器人的环境适应性以及环境限制的克服能力有着越来越高的要求，由于传统工业机器人工作空间小、运动灵活性能力不足，特别是对一些非结构化的环境适应能力很弱，并且面临自由度数限制以及刚性臂杆难以完成狭小环境中各类障碍物的穿越问题。绳驱超冗余机器人具有自由度数多、臂杆细小、运动灵活性强以及环境适应性好、但刚度差等特点。采用离散式刚性连杆与分段联动构型的绳驱超冗余机器人在保持灵活性的同时大大提升了机械臂的刚度，能满足很多狭小空间的穿越任务，例如在核电站冷却管道的维修、油气管道维护以及核反应堆管道检查等超常规作业区域。

然而，由于该类型机构自由度比较多，主从被动式分段联动机械臂的多层级动力学耦合的存在，使得此类机器人的动力学控制与轨迹规划变得更加复杂。因此需要提出一种能够提高轨迹规划效率的同时兼顾控制精度的柔性机器人轨迹规划方法。

发明内容

鉴于现有技术的缺陷，本发明旨在于提供一种柔性机器人动力学建模及轨迹跟随控制方法及装置，能够更好地研究机器人的运动特性并提高机器人末端轨迹跟随的精确度。

为了实现上述目的，本发明采用的技术方案如下：

一种柔性机器人动力学建模及轨迹跟随的控制装置，具有柔性机器人，以及所述控制装置，所述控制装置包括：

偏差数据获取模块：用于获取相对偏差数据并构建所述柔性机器人空间模型；

计算模块：用于实时计算期望轨迹的笛卡尔空间样条插值、运动学、动力学以及求解非线性二次规划优化模型；

阈值判断模块：用于根据所述相对偏差数据与非线性二次规划优化模型的阈值条件，判断末端位姿、关节角度、绳索拉力是否在设定目标范围内；

执行模块：用于当满足阈值判断条件，将非线性二次规划优化模型的计算结果输入至PD控制器。控制器输出信号到执行模块并作用到所控制柔性机器人；

其中，所述相对偏差数据包括所述柔性机器人末端点和设定的期望点的相对位位移与位姿的偏差、实际绳索长度和期望绳索长度的偏差以及绳索实际拉力与期望拉力的偏差。

本发明还提供一种实现柔性机器人动力学建模及轨迹跟随的控制的方法，所述方法包括以下步骤：

S1建立“绳索-关节-末端”多重传动运动学模型,对目标柔性机器人各关节建立D-H坐标系，推导得柔性机器人末端位姿、速度、加速度与关节角、角速度、角加速度之间的关系，建立起关节空间到末端笛卡尔空间的映射关系；

S2建立了串-并联混合的动力学模型：基于牛顿—欧拉方程对柔性机器人整体惯性力、重力进行求解；求解所述驱动力与摩擦力；求解整个柔性机器人的联动绳索力/力矩；并进一步地推导出柔性机器人串-并联混合动力学模型；

S3建立基于串-并联混合的动力学模型前馈的PD控制器：设计柔性机器人运动的PD控制器，并将步骤S2中的串-并联混合的动力学模型作为控制前馈输入，补偿柔性机器人末端的运动误差；

S4对绳索拉力和末端位姿的同步优化：基于驱动绳索拉力最小条件与位姿误差最小条件的非线性二次规划优化模型，并引入允许绳索拉力范围、位姿误差允许范围和关节角阈值限制条件，实时求解出合适的PD控制器增益及期望驱动绳索拉力；

S5结合多重运动学转换模块、动力学计算模块、PD控制器、二次规划优化模块，偏差数据获取模块获得柔性机器人轨迹跟随。

优选的，所述步骤S1中，所述D-H坐标系根据柔性机器人各关节及关节之间的实际几何参数得到D-H参数表。

优选的，所述步骤S1中，根据节之间绳索分布模型，可以推导出关节角变换量与绳索长度变换量之间的关系，建立起绳索驱动空间到关节空间再到末端笛卡尔空间的映射关系。

优选的，所述PD控制器参数Kp与Kv是动态变化；所述非线性二次规划优化模型获得动态参数Kp、Kv，实现绳索拉力与末端位姿协同优化。

优选的，所述柔性机器为离散式刚性连杆与分段联动构型的机器人。

本发明的有益效果在于，对于很多非结构化环境，需要柔性机器人具备灵巧操作能力，柔性机器人在这类场合具有非常大的优势。然而，柔性机器人的动力学建模与控制方法非常复杂，是一个工程难题。基于此，本发明提出了一种主从被动分段联动式柔性机器人的动力学建模与轨迹跟随控制方法，解决了串-并联混合的动力学求解与轨迹优化难题。首先，建立了柔性机器人的多重耦合运动模型，并推导了绳索-关节-末端的多层级运动学方程。其次，根据主从被动混合驱动的运动特性，推导了串-并联混合建模的动力学方程。最后，基于分段联动式机械臂的动力学前馈控制，对绳索拉力和操作空间精度指标的进行加权优化。数值仿真结果证明所提串并联混合-分段联动式机械臂轨迹跟随控制方法的有效性。

相比于现有技术，本发明能够有效提高柔性机器人轨迹跟随精度，同时还能够优化驱动绳索拉力，节约能源。该方法可广泛应用于柔性机器人末端轨迹控制。

附图说明

图1为本发明的柔性机器人视觉测量系统三维示意图；

图2为本发明柔性机器人混合标定方法流程示意图；

图3为本发明的混合标定装置及柔性机器人之间坐标系转换关系示意图；

图4为本发明的混合标定数学模型计算程序流程图；

图5为本发明的对机器人正运动学参数优化模型的粒子群优化计算程序流程图。

具体实施例

以下将结合附图对本发明作进一步的描述，需要说明的是，本实施例以本技术方案为前提，给出了详细的实施方式和具体的操作过程，但本发明的保护范围并不限于本实施例。

本发明为一种柔性机器人动力学建模及轨迹跟随的控制装置，具有柔性机器人，以及所述控制装置，所述控制装置包括：

实施例一

本实例提供了一种柔性机器人，如图1。该柔性机器人是基于离散式刚性连杆与分段联动构型，主要部分包括臂杆、中心块、联动绳索、驱动绳索。各臂杆之间采用万向节进行连接，若干个连接臂杆组成一个臂段，每个臂段内通过联动绳索的连接将同一方向自由度耦合一起，使得臂段内的各个相邻关节间夹角严格相等，并通过三根绳索对每个臂段进行两个自由度的控制。

本实施例通过引入柔性机器人，解决现有6自由度机器人的运动空间受限、运动灵活性差、不适合于非结构化的环境的问题。该类型柔性机器人具有运动空间大、灵活性强以及环境适应性好等优点，并采用离散式刚性连杆与分段联动构型，改善现有多数柔性机器人刚度不足的通病，特别适合于非结构化的恶劣环境中工作。

实施例二

本发明实施例二提供一种柔性机器人动力学建模及轨迹跟随控制方法，本实施例适用于离散式刚性连杆与分段联动构型的柔性机器人，目的是实现柔性机器人末端较高精度地跟随给定轨迹。图2为本发明实施例提供的一种柔性机器人动力学建模及轨迹跟随控制方法的实现流程图，如图所示，该方法可以包括以下步骤：

S1建立“绳索-关节-末端”多重传动运动学模型：对目标机器人各关节建立D-H坐标系，根据各关节及关节之间的实际几何参数得到D-H参数表，进而推导得机器人末端位姿、速度、加速度与关节角、角速度、角加速度之间的关系，建立起关节空间到末端笛卡尔空间的映射关系。

进一步地，根据图3所示的关节之间绳索分布模型，可以推导出关节角变换量与绳索长度变换量之间的关系，建立起绳索驱动空间到关节空间再到末端笛卡尔空间的映射关系。

S2建立了串-并联混合的动力学模型：基于牛顿—欧拉方程对柔性机器人整体惯性力、重力进行求解。根据图3所示的关节之间绳索分布模型求解所述驱动力与摩擦力。通过同步带原理模型求解整个柔性机器人的联动绳索力/力矩。并进一步地推导出柔性机器人串-并联混合动力学。

S3建立基于串-并联混合的动力学模型前馈的PD控制器：设计柔性机器人运动的PD控制器，并将S2所述串-并联混合的动力学模型作为控制前馈输入，补偿柔性机器人末端的运动误差。

S4对绳索拉力和末端位姿的同步优化：基于驱动绳索拉力最小条件与位姿误差最小条件的非线性二次规划优化模型，并引入允许绳索拉力范围、位姿误差允许范围和关节角阈值限制条件，实时求解出合适的PD控制器增益及期望驱动绳索拉力。

下面详细描述本实施例的计算过程。

如图1所示，为本实施例中柔性机器人的臂段示意图和D-H坐标系分布图。从图中可见，每个运动关节处都是2-DOF(i.e.:俯仰和偏航)的万向节，相邻万向节之间通过联动绳索进行连接，实现段内联动，每段又通过驱动绳索连接，实现关节段的冗余驱动。为了更好地描述分段联动柔性臂的坐标系关系，将基座坐标系记为{O_b-X_bY_bZ_b}。基座的体号记为0,万向节依次编号1,2,i,…,n_J，段号为1,2,m,…, n_m，假设每段由p个小节组成，根据以上描述建立的柔性臂D-H坐标系如图1所示。

从D-H坐标系的分布来看，第m段任意坐标系{i}的+X轴是沿着臂杆的轴线方向，驱动体2i-1的万向节运动可以分解为为绕+Z轴旋转,再绕+Y轴旋转；后面与其相邻万向节2i的运动则是先绕+Y轴旋转,再绕+Z轴旋转，此时第m段任意万向节的关节角可记为

由运动学递推关系，第m段的正运动学的齐次变换矩阵为：

于是，整臂关节到末端的正运动学的齐次变换矩阵为：

根据以上推导，相邻关节的运动递推关系可表示为：

其中，

进一步，第i+1号万向节与第i号万向节的递推关系可写为：

于是，各万向节节点的广义速度可表示为:

这里,J_q＝ΛΓ，

对式(5)进行求导,化简得到各万向节的广义加速度为:

到此完成柔性机器人的关节空间到末端笛卡尔空间的运动学映射。进一步地，对驱动绳索空间到关节空间进行运动学模型建立。

每个万向节之间的绳索分布如图3所示，第m段第j节绳长变化量可表示为:

这里,

进一步地，第m段关节角变化量与绳长变化量之间的关系为:

这里,广义逆矩阵

于是，整个柔性臂绳长到关节的递推关系为:

这里,

以上通过推导，建立起柔性机器人“绳索-关节-末端”的多重传动运动学模型。进一步地，对柔性机器人的动力学模型进行建立。

首先对机器人臂杆惯性力进行求解。第i个刚体的质心位置为:

这里，

为第i个万向节旋转中心的位置矢量，

为第i个万向节旋转中心到第i个刚体质心的位置矢量.

对进行求导可得第i个刚体的质心线速度为:

这里，

与

分别为第i个万向节旋转中心的线速度与角速度.

进一步地，第i个刚体质心的线加速度为:

于是，第i个刚体所受惯性力可表示为:

这里，

整个柔性臂所受的惯性力/力矩可以写成矩阵的形式:

这里,

为O_2i坐标轴的指向。

另一方面,第i个刚体质心所受重力为:

这里，g表示重力加速度,

于是，整个柔性臂所受的重力可表示为:

这里,

进一步地，对绳索作用在机器人段的驱动力、绳索通过圆盘导孔产生的摩擦力进行求解。

驱动绳索的动力学要通过末端向内递推,以其中第m段的驱动绳索为例，绳索

一共穿过2[(m-1)p+j]个孔, 该绳索在作用于第m段第j节的受力图如图4所示。

如图3所示，第m段第j子节中任意两个相邻圆盘之间绳索的拉力方向可表示为:

这里，

表示第c条绳索从O_a孔到O_b孔的位置矢量。

如图4所示,记第2[(m-1)p+j]-1号坐标系上绳索

所受的拉力大小分别为

其对应的绳索拉力和绳孔位置可表示为:

这里,q代表绳索的编号，

与

分别表示2[(m-1)p+j]-1号坐标系在基坐标系下的旋转矩阵与平移向量，

绳索q在2[(m-1)p+j]-1号坐标系下的位置矢量。

进一步地，作用在任意刚体(m-1)p+j上的合成驱动力和驱动力矩分别为:

如图5所示，第q根绳索的摩擦力大小和方向分别为:

这里,

F_pv是粘胶摩擦参数,μ_n是库仑摩擦系数,α_n是包角。

进一步地，作用在任意刚体(m-1)p+j上的合成摩擦力和摩擦力矩分别为:

这里,

根据上述等效关系，式与又可以进一步化简为:

于是,整个柔性臂所受的驱动力和摩擦力可以写成矩阵的形式:

这里,

进一步地，对联动绳索对各臂段的作用力求解。

假设联动绳索施加了足够大的预紧力,使得联动绳索与关节之间不打滑。第m段内(m-1)p+j号万向节与(m-1)p+j+1号万向节通过小“8”字绳索和大“8”字绳索进行联动,实现关节段内4个子关节的同步俯仰与偏航,小、大“8”字联动示意图如图6所示。

如图6(a)所示,通过同步带原理实现相邻两个子关节的以相同的角速度向相反方向运动,假设不与关节接触部分两段绳索长度为 L_os,绳索横截面积为A_s,绳索的杨氏模量为E_s,关节的旋转半径为r_s, 绳索变形量为

两边绳索变形不一致导致的控制力矩为:

这里,

因此,小“8”字联动的等效刚度为:

如图6(b)所示,假设不与关节接触部分两段绳索长度为L_ol,绳索横截面积为A_l,绳索的杨氏模量为E_l,关节的旋转半径为r_l,同理可求得大“8”字联动的等效刚度为:

于是，联动绳索作用在万向节(m-1)p+j上的合力矩为:

这里,

整个柔性臂绳索联动力/力矩可表示为:

这里,

至此，可以推导出柔性机器人的串并联混合动力学模型。

假设作用在万向节(m-1)p+j上广义力为

于是相邻万向节广义力的递推关系可表示为:

将(35)拓展到整个柔性臂上,可以写成如下矩阵的形式:

根据(36)与(6)可求得作用在关节上的广义力可表示为:

F_O＝Λ^T[F_It+(F_dr-F_fr)+F_g] (37)

另一方面,关节处(m-1)p+j的广义力与该处联动绳索的合力矩存在如下关系:

于是,整个柔性臂联动绳索的合力矩满足条件:

τ_s＝Γ^TF_O (39)

这里,

联立(39)与(37)可得如下关系:

将(6),,(28)以及(29)代入(40)可得到分段联动式串- 并联混合驱动的动力学方程为:

这里,

表示绳索的驱动力,

表示施加在第m段上驱动绳索的拉力。

进一步地，对柔性机器人的末端轨迹跟随控制算法进行设计。

为了实现末端的轨迹跟随,等效关节力矩通过位姿偏差的PD控制获得,即有:

其中，

假设由驱动绳索拉力产生的等效关节力矩为

则根据虚功原理有如下关系:

其中,

为第m段驱动绳索产生的等效关节力矩.

根据式,可得关节空间等效力矩与驱动绳索空间拉力之间的关系为:

为了节省能源、保护执行器,柔性臂运动过程中尽可能地使驱动绳索的拉力最小,但驱动绳索的拉力也是有上下界的,因此,驱动绳索的拉力最小化可以作为优化的指标,同时保证各个关节角在其有效区间内。根据非线性二次规划法可建立如下的SQP优化模型,即:

同时为了保证轨迹跟随控制方法的精度,实际理想的 K_p＝diag(k_p1,…,k_p6),K_d＝diag(k_d1,…,k_d6)参数值是动态变化的,既要保证末端的位姿误差最小(记为min f_p),又要使得驱动绳索的拉力尽可能的小(记为f_f),因此设计如下的SQP优化模型,即:

其中，

为第i段第j根绳索的拉力幅值，α∈[0,1]的权值常数， ||δP_m||₂为末端允许的最大位置偏差幅值，

为末端允许的最大姿态偏差幅值，

为关节角k的阈值。

对于式(46),可以通过多目标优化的方法进行求解,如图7所示，为基于动力学前馈控制的绳索拉力与操作空间位姿协同优化框图。主要包含：动力学(例如正动力学和逆动力学)计算模块儿、PD控制模块儿、多重运动学(绳索-关节-末端)转换模块儿及非线性二次规划模块儿(例如绳索拉力和末端位姿)等组成。

实施例三：

本发明实施例三提供一种柔性机器人动力学建模及轨迹跟随控制装置，用于执行如实施例二所述的方法，如图7所示，为本实施例的柔性机器人动力学建模及轨迹跟随控制装置，按作用类型分类，可以分成以下模块：

偏差数据获取模块：用于获取相对偏差数据并构建所述柔性机器人空间模型，所述相对偏差数据包括：所述柔性机器人末端点和设定的期望点的相对位位移与位姿的偏差、实际绳索长度和期望绳索长度的偏差以及绳索实际拉力与期望拉力的偏差。

计算模块：用于实时计算期望轨迹的笛卡尔空间样条插值、运动学、动力学以及求解非线性二次规划优化模型。

阈值判断模块：用于根据所述相对偏差数据与非线性二次规划优化模型的阈值条件，判断末端位姿、关节角度、绳索拉力是否在设定目标范围内。

执行模块：用于当满足阈值判断条件，将非线性二次规划优化模型的计算结果输入至PD控制器。控制器输出信号到执行模块并作用到所控制柔性机器人。

本发明解决了串-并联混合的动力学求解与轨迹优化难题。首先，建立了柔性机器人的多重耦合运动模型，并推导了绳索-关节-末端的多层级运动学方程。其次，根据主从被动混合驱动的运动特性，推导了串-并联混合建模的动力学方程。最后，基于分段联动式机器人的动力学前馈控制，对绳索拉力和操作空间精度指标的进行加权优化。相比于现有技术，本发明所述方法能够有效提高柔性机器人轨迹跟随精度，同时还能够优化驱动绳索拉力，节约能源。该方法可广泛应用于柔性机器人末端轨迹控制。

对于本领域的技术人员来说，可以根据以上的技术方案和构思，给出各种相应的改变，而所有的这些改变，都应该包括在本发明权利要求的保护范围之内。

Claims

1.一种柔性机器人动力学建模及轨迹跟随的控制装置，具有柔性机器人，以及所述控制装置，其特征在于，所述控制装置包括：

2.一种根据权利要求1所述的柔性机器人动力学建模及轨迹跟随的控制装置的实现方法，其特征在于，所述方法包括以下步骤：

3.根据权利要求2所述的柔性机器人动力学建模及轨迹跟随的控制装置的实现方法，其特征在于，所述步骤S1中，所述D-H坐标系根据柔性机器人各关节及关节之间的实际几何参数得到D-H参数表。

4.根据权利要求2所述的柔性机器人动力学建模及轨迹跟随的控制装置的实现方法，其特征在于，所述步骤S1中，根据节之间绳索分布模型，可以推导出关节角变换量与绳索长度变换量之间的关系，建立起绳索驱动空间到关节空间再到末端笛卡尔空间的映射关系。

5.根据权利要求2所述的柔性机器人动力学建模及轨迹跟随的控制装置的实现方法，其特征在于，所述PD控制器参数Kp与Kv是动态变化；所述非线性二次规划优化模型获得动态参数Kp、Kv，实现绳索拉力与末端位姿协同优化。

6.根据权利要求2所述的柔性机器人动力学建模及轨迹跟随的控制装置的实现方法，其特征在于，所述柔性机器为离散式刚性连杆与分段联动构型的机器人。