含柔性运动副欠驱动机构运动学、动力学求解方法

摘要

本发明涉及含有柔性运动副欠驱动机构运动学、动力学求解迭代算法，该算法的步骤是：1)建立机构坐标系；2)定义机构参数，建立欠驱动机构的运动学、动力学模型，得到机构的运动学位置方程、运动学速度方程、运动学加速度方程和动力学方程；3)给定各构件初始条件及原动件运动规律，求解t0时刻机构的运动学和动力学方程，得到此时各构件的运动规律及原动件的驱动力；4)求解tj时刻机构的运动学和动力学方程，得到此时各构件的运动规律及原动件的驱动力；5)将上述步骤4)重复进行n次，即可得到该机构所有构件在整个运动周期内的运动规律及原动件的驱动力，利用MATLAB逐点绘图，得到各构件运动规律曲线图及原动件驱动力变化曲线图。

说明书

技术领域

本发明涉及一种数值迭代算法，具体涉及一种含有柔性运动副欠驱动机构运动学、动力学方程的求解迭代算法。

背景技术

欠驱动机构由于驱动器的减少而具有能耗低、质量轻、成本小等优点，但其运动学和动力学约束不完整，机构运动具有不确定性，而在含有柔性运动副的欠驱动机构中，柔性运动副的弹性反力可以起到驱动力的作用，一定程度上弥补欠驱动机构动力约束的不完整，但柔性运动副的加入也使机构的运动学、动力学问题变得更加复杂而难以求解，但机构的运动学和动力学方程求解问题是机构深入研究开发的基础，因此这一类问题的研究十分必要。

含有柔性运动副的欠驱动机构由于同时具有欠驱动和柔性两方面的特性，因此其动力学模型是高度复杂的非线性微分方程组，要得到其动态响应的解析解是非常困难的。目前，全驱动柔性机构的动力学模型一般采用数值解法，用积分迭代的方法逐步逼近，然而对于含有柔性运动副的欠驱动机构而言，被动关节的加速度存在二阶非完整约束，具有不可积性，因此，传统的数值解法不再适用。目前，针对此问题，主要有两种解决途径：一是从控制的角度出发，利用主被动关节之间的动力学耦合作用，通过恰当的控制策略使机构实现运动，但是，主被动关节之间的内在本质规律并不是固定不变的，它是随着机构位形和驱动器的位置而变化的，因此，该类方法的缺点是求解结果准确度不高[1]；另一种方法是通过模型降阶的思想将欠驱动二阶非完整系统降阶为几个具有完整约束的子系统，然后基于各子系统的积分特性进行求解，该类方法的缺点是需要针对不同的机构设置不同的约束进行降阶，方法不具有通用性[2]。因此，针对含有柔性运动副欠驱动机构的运动学、动力学问题，目前还未见较为完善的数值求解方法。

[1]Arai H,Tachi S.Position control ofmanipulator with passive joints using dynamic coupling[J].IEEE Transactions on Robotics&Automation,1991,7(4):528-534.

[2]Katake A B,Mahindrakar A D,Banavar R N.Study of underactuated mechanisms in the presence ofholonomic constraints:the constrained Acrobot[C]//IEEE International Conference on Industrial Technology.2000:703-706vol.2.

发明内容

针对现有技术的不足，本发明拟解决的技术问题是，提供一种含有柔性运动副欠驱动机构运动学、动力学求解迭代算法，该算法基于时间无限细分时两个时刻区间内速度和加速度不变的原理进行迭代和求解，即直接认为不同时刻区间内速度和加速度是阶跃变化的，不用通过积分方法求解速度和加速度，克服了目前欠驱动机构中由于存在二阶非完整约束而无法进行积分迭代的缺陷。

本发明解决所述技术问题采用的技术方案是：

一种含有柔性运动副欠驱动机构运动学、动力学求解迭代算法，该算法的步骤是：

1)建立机构坐标系，含有柔性运动副欠驱动机构的固定端为A点，与固定端连接的构 件的另一端为B点，柔性运动副连接一端为D点，D点为滑块所在位置，柔性运动副另一端连接固定，以A点为原点，AD连线为x轴，指向D的方向为x轴的正方向，垂直x轴向上为y轴正方向；

2)定义机构参数，建立欠驱动机构的运动学、动力学模型，建模过程中不考虑各运动副之间的摩擦，并设机构处于水平面上，欠驱动机构的杆件数量为e，其中，原动件记为i，其余各杆件记为o，各杆件的杆长记为l_m，各杆件与x轴的夹角记为θ_m，其中m＝1、2、…、e，各构件的重力势能为零，得到机构的运动学位置方程、运动学速度方程、运动学加速度方程和动力学方程，定义迭代次数n，定义迭代步长δt；

3)给定各构件初始条件及原动件运动规律，求解t₀时刻机构的运动学和动力学方程，得到此时各构件的运动规律及原动件的驱动力；所述的求解t₀时刻机构的运动学和动力学方程按如下步骤进行：

(3a)机构原动件的位置输入已知，记为θ_i(t₀)；滑块的初始位置已知，记为x(t₀)，利用运动学位置方程求出θ_o(t₀)；

(3b)初始时刻，原动件角速度滑块速度x以步骤(3a)为基础，利用运动学速度方程求得

(3c)机构原动件的角加速度输入已知，记为在步骤(3a)和步骤(3b)的基础上利用动力学方程求得滑块的加速度，记为同时求得原动件转矩，记为τ_i(t₀)；

(3d)在步骤(3a)-(3c)的基础上将和带入运动学加速度方程，求得

4)设从t_j-1时刻到t_j时刻的时间间隔无限小，记为δt，其中j＝1,2,3...，认为滑块的速度和加速度在某一时间间隔无限小的区间内不变，等于区间内前一时刻的速度和加速度，而不同时刻的区间速度和加速度是阶跃变化的，求解t_j时刻机构的运动学和动力学方程，得到此时各构件的运动规律及原动件的驱动力；所述求解t_j时刻机构的运动学和动力学方程按如下步骤进行：

(4a)在t_j时刻机构原动件的位置输入已知，记为θ_i(t_j)；滑块的位置记为x(t_j)，x(t_j)由如下等式求得，然后利用运动学位置方程求得θ_o(t_j)；

(4b)机构原动件的角速度输入已知，记为滑块的速度记为以t_j-1时刻所求结果为基础进行迭代，然后利用运动学速度方程求出

(4c)机构原动件的角加速度输入已知，记为利用动力学方程求得滑块的加速度及原动件转矩，滑块的加速度记为原动件转矩记为τ_i(t_j)；

(4d)将和带入运动学加速度方程求得

5)将上述步骤4)重复进行n次，即可得到该机构所有构件在整个运动周期内的运动规律及原动件的驱动力，利用MATLAB逐点绘图，得到各构件运动规律曲线图及原动件驱动力变化曲线图。

与现有技术相比，本发明的有益效果是：

本发明构思巧妙，提出一种含有柔性运动副欠驱动机构运动学、动力学求解的数值迭代算法，直接认为滑块的速度和加速度在某一时间间隔无限小的区间内不变，等于区间内前一时刻的速度和加速度，省去积分求解速度和加速度的环节，克服了欠驱动机构中由于加速度不可积而无法得到数值解的缺陷，实现了对含有柔性运动副欠驱动机构的求解。

附图说明

图1为含有柔性运动副欠驱动机构二自由度的机构简图。

图2是滑块位移随时间变化图。

图3是滑块速度随时间变化图。

图4是滑块加速度随时间变化图。

图5是原动件驱动力矩随时间变化图。

图6为含有柔性运动副欠驱动机构三自由度的机构简图。

具体实施方式

下面结合附图和具体的实施例对本发明作进一步说明，本发明的实施方式包括但不限于下列实施例。

本发明含有柔性运动副欠驱动机构(以下简称机构或欠驱动机构)运动学、动力学求解迭代算法，该算法的步骤是：

1)建立机构坐标系，含有柔性运动副欠驱动机构(以下简称机构或欠驱动机构)的固定端为A点，与固定端连接的构件的另一端为B点，柔性运动副连接一端为D点，D点为滑块所在位置，柔性运动副另一端连接固定，以A点为原点，AD连线为x轴，指向D的方向为x轴的正方向，垂直x轴向上为y轴正方向；

2)定义机构参数，建立欠驱动机构的运动学、动力学模型，建模过程中不考虑各运动副之间的摩擦，并设机构处于水平面上，欠驱动机构的杆件数量为e，其中，原动件记为i，其余各杆件记为o，各杆件的杆长记为l_m，各杆件与x轴的夹角记为θ_m，其中m＝1、2、…、e，各构件的重力势能为零，得到机构的运动学位置方程、运动学速度方程、运动学加速度方程和动力学方程，定义迭代次数n，定义迭代步长δt；

3)给定各构件初始条件及原动件运动规律，求解t₀时刻机构的运动学和动力学方程，得到此时各构件的运动规律及原动件的驱动力(力矩)；所述的求解t₀时刻机构的运动学和动力学方程按如下步骤进行：

(3a)机构原动件的位置输入已知，记为θ_i(t₀)；滑块的初始位置已知，记为x(t₀)，利用运动学位置方程求出θ_o(t₀)；

(3b)初始时刻，原动件角速度滑块速度以步骤(3a)为基础，利用运动学速度方程求得

(3c)机构原动件的角加速度输入已知，记为在步骤(3a)和步骤(3b)的基础上利用动力学方程求得滑块的加速度，记为同时求得原动件转矩，记为τ_i(t₀)；

(3d)在步骤(3a)-(3c)的基础上将和带入运动学加速度方程，求得

4)设从t_j-1时刻到t_j时刻的时间间隔无限小，记为δt，认为滑块的速度和加速度在某一时间间隔无限小的区间内不变，等于区间内前一时刻的速度和加速度，而不同时刻的区间速度和加速度是阶跃变化的，求解t_j(j＝1,2,3...)时刻机构的运动学和动力学方程，得到此时各构件的运动规律及原动件的驱动力(力矩)；所述求解t_j时刻机构的运动学和动力学方程按如下步骤进行：

(4a)在t_j(j＝1,2,3...)时刻机构原动件的位置输入已知，记为θ_i(t_j)；滑块的位置记为x(t_j)，x(t_j)由如下等式求得，然后利用运动学位置方程求得θo(t_j)；

(4b)机构原动件的角速度输入已知，记为滑块的速度记为以t_j-1时刻所求结果为基础进行迭代，然后利用运动学速度方程求出

(4c)机构原动件的角加速度输入已知，记为利用动力学方程求得滑块的加速度及原动件转矩，滑块的加速度记为原动件转矩记为τ_i(t_j)；

(4d)将和带入运动学加速度方程求得

5)将上述步骤4)重复进行n次，即可得到该机构所有构件在整个运动周期内的运动规律及原动件的驱动力(力矩)，利用MATLAB逐点绘图，得到各构件运动规律曲线图及原动件驱动力(力矩)变化曲线图。

本发明中的构件是指杆件和滑块。

本发明的进一步特征在于步骤2)中所述的建立欠驱动机构的运动学、动力学模型，以一种含有柔性移动副的平面二自由度欠驱动机构为研究对象，该欠驱动机构(参见图1)有五个构件，固定端为A点，柔性连接端为D点，在D点处的滑块连接线性弹簧的一端，线性弹簧的另一端固定在E点，第一个构件与第二个构件的连接点记为B点，第二个构件与第三个构件的连接点记为C点，其中，杆件1为机构的原动件，杆件2和杆件3为机构的其余杆件，三个杆件的杆长分别记为l₁,l₂,l₃,各杆件与x轴的夹角分别记为θ₁,θ₂,θ₃,A点到E点的距离为l₀，A点到D点的距离(即滑块距原点的距离)为x，线性弹簧的自由长度为a，线性弹簧的刚度系数为k,各杆件质量分别为m₁,m₂,m₃，滑块的质量为m₄，

具体建模步骤如下：

(2a)采用封闭矢量法建立机构的运动学位置方程：

(2b)对步骤(2a)中的运动学位置方程求导，得到机构的运动学速度方程：

(2c)对步骤(2b)中的运动学速度方程求导，得到机构的运动学加速度方程：

(2d)采用拉格朗日方程法得到机构的动力学方程：

其中，q₁,q₂为选定的广义坐标，J₁₁,J₁₂,J₂₂是关于q₁,q₂的函数。

上述步骤3)中所述的“求解t₀时刻机构的运动学和动力学方程”按如下步骤进行：

(3a)机构原动件的位置输入已知，记为θ₁(t₀)；滑块的初始位置已知，记为x(t₀)。利用运动学位置方程求出θ₂(t₀),θ₃(t₀)；

(3b)初始时刻，原动件角速度滑块速度以步骤(3a)为基础，利用运动学速度方程求得参见下式；

(3c)机构原动件的角加速度输入已知，记为在步骤(3a)和步骤(3b)的基础上利用动力学方程求得滑块的加速度，记为参见下式

同时将求得的结果带入动力学方程，求得原动件转矩，记为τ₁(t₀)，参见下式

(3d)在步骤(3a)-(3c)的基础上将和带入运动学加速度方程求得

采用如下等式：

其中

步骤4)中的步骤(4a)～(4d)求解未知数的方法与步骤(3a)～(3d)类似。

本发明以一种含有柔性移动副的平面二自由度欠驱动机构为研究对象进行说明，显然，本发明同样适用于具有该类机构特征的其他含有柔性移动副的欠驱动机构。

实施例1

本实施例以含有柔性移动副的平面二自由度欠驱动机构为研究对象，如图1所示，建立机构的坐标系，取A点为坐标原点，以AD连线为x轴，指向D的方向为x轴的正方向，垂直x轴向上为y轴正方向，机构各构件参数见表1。

表1含柔性运动副欠驱动机构的构件参数

定义机构参数，建立欠驱动机构的运动学、动力学模型，选取迭代时间步长为0.05s，迭代次数为40次。具体建模步骤如下：

(2a)采用封闭矢量法建立机构的运动学位置方程：

(2b)对步骤(2a)中的运动学位置方程求导，得到机构的运动学速度方程：

(2c)对步骤(2b)中的运动学速度方程求导，得到机构的运动学加速度方程：

(2d)采用拉格朗日方程法得到机构的动力学方程：

其中，q₁,q₂为选定的广义坐标，J₁₁,J₁₂,J₂₂是关于q₁,q₂的函数。

对于t₀时刻机构的运动学和动力学方程的求解步骤如下：

(3a)给定原动件的运动规律是初始位置为π/12，初始速度为0rad/s，加速度为πrad/s²，的匀加速圆周运动，滑块的初始位置为400mm，l₀为400mm；将初始位置输入带入运动学位置方程利用加减消元法求得θ₂(t₀),θ₃(t₀)；

(3b)初始时刻，原动件角速度滑块速度将初始速度输入及步骤

(3a)中位置已知量与位置所求量带入方程利用加减消元法求得

(3c)机构原动件的角加速度输入已知，记为将初始加速度输入及步骤(3a)和步骤(3b)中位置已知量与位置所求量，速度已知量与速度所求量带入动力学方程

求得并将其结果带入方程求得原动件转矩，即驱动转矩τ₁(t₀)；

(3d)将以上已知量及所得结果带入运动学加速度方程

利用加减消元法求得

对于t_j时刻机构的运动学和动力学方程的求解步骤如下：

(4a)在t_j(j＝1,2,3...)时刻机构原动件的位置输入已知，记为θ₁(t_j)；滑块的位置记为x(t_j)，以t_j-1时刻所求结果为基础进行迭代，x(t_j)由如下等式求得，然后计算θ₂(t_j),θ₃(t_j)，计算方法与求θ₂(t₀),θ₃(t₀)类似；

(4b)机构原动件的角速度输入已知，记为滑块的速度记为以t_j-1时刻所求结果为基础进行迭代，由如下等式求得，然后计算计算方法与求类似；

(4c)机构原动件的角加速度输入已知，记为将初始加速度输入及步骤(4a)-(4b)中的位置和速度的已知量与所求量带入方程

求得并将其带入方程求得驱动转矩τ₁(t_j)。

(4d)计算计算方法与求类似。

(5)重复执行上述步骤(4a)～(4d)39次，即可得到整个运动周期内的各构件的运动规律及原动件的驱动力矩，利用matlab编写M文件，得到滑块运动规律变化曲线图如图2～4所示，原动件驱动力矩变化曲线图如图5所示。由图可知，滑块的位置、速度、加速度、驱动力矩曲线都较为平缓，显然，本发明解决了含柔性运动副欠驱动机构由于存在非完整约束 而求解困难的难题，且整个求解过程共用时132s，求解速度较快。

实施例2

本实施例以三自由度含有柔性移动副欠驱动机构为研究对象，如图6所示，建立机构的坐标系，取A点为坐标原点，以AD连线为x轴，指向D的方向为x轴的正方向，垂直x轴向上为y轴正方向，AB杆件和BC杆件均为原动件。求解过程同实施例1。

本发明未述及之处适用于现有技术。