一种柔性机械臂振动抑制方法

1.本发明涉及柔性机械臂控制领域，具体的说，涉及了一种柔性机械臂振动抑制方法。


背景技术：

2.传统制造业的机械臂手臂一般为刚性材料，由于刚性材料不会发生弹性变形，可以避免运动期间因机械变形导致的控制不精确问题，但是刚性机械臂的质量很高，不适合直接驱动，由于其具备比较大的惯性，不适合高速移动，无法满足某些需要精细操作领域中的机械臂控制要求。
3.柔性机械臂与刚性机械臂相比，具有质量轻巧、活动灵活、能耗较少以及高精度运行等优点，但其由诸多柔性元件构成(包括柔性关节、柔性臂杆)，柔性元件的柔韧性可在机械臂发生碰撞时起到缓冲和保护作用，但关节的柔性会造成在运动停止后，臂杆仍然在不断振动，加上臂杆本身的柔性，这会严重影响末端执行机构的位置精度，故在实际应用中必须对其进行振动抑制。
4.专利202010753420.2公开了一种柔性机械臂的组合滑模运动控制方法，其将柔性机械臂建模为带有奇异摄动参数的奇异摄动系统，并分解为描述刚体运动和柔性振动的快、慢子系统；慢时间尺度下，考虑外界扰动及参数不确定性对系统的影响，设计带有扰动观测器的自适应滑模控制器，实现对柔性机械臂的轨迹跟踪；快时间尺度下，为消除未建模动态及慢时间尺度下振动状态对控制器设计的影响，设计鲁棒滑模控制器实现对柔性振动的抑制；最后利用奇异摄动理论将慢控制器与快控制器结合。缺点：采用奇异摄动理论的闭环反馈控制方法在面对高阶模态时容易产生谐振，需要对关节的柔性做低柔性假设，与实际情况存在一定差距。
5.专利201811252001.x公开了一种柔性空间机械臂振动控制方法及系统，其是由关节力矩计算得到返回波，返回波中包含系统振动信息，叠加在输入中，对输入进行实时整形，整形后的输入经过关节pd控制机械臂的运动，对返回波进行吸收，达到振动抑制的目的。缺点：上述两种控制方法在进行振动抑制算法时主要控制驱动电机的作动力，需要在电流环路进行控制，对控制系统要求很高，对原系统的改动成本比较大。
6.专利202210592114.4公开了一种用于关节机器人振动抑制的输入整形的方法，其在算法中引入负脉冲，与只有正脉冲的算法相比，显著提升了系统的响应速度，不需要求出脉冲序列参数的解析解，用计算机即可快速获得数值最优解。该方法在进行振动抑制时仅能够减小由输入作动器本身所导致的振动，无法处理柔性臂杆本身已有的、残余的振动。


技术实现要素：

7.本发明的目的是针对现有技术的不足，从而提供一种面对多自由度、多模态的柔性机械臂时不易引起高频振荡，无需对关节的柔性做低柔性假设；在电流环路的外层，即位置环路与速度环路进行控制，对控制系统要求低，对系统改动小、改进成本低；可以使作动
器对柔性关节产生的振动与柔性臂杆产生的振动相抵消，对已有振动进行实时消除，应用更加全面；的柔性机械臂振动抑制方法。
8.为了实现上述目的，本发明所采用的技术方案是：一种柔性机械臂振动抑制方法，所述柔性机械臂包括多根臂杆和连接在臂杆之间的关节，每个关节均由一电机驱动，包括以下步骤：
9.s1、动力学建模：
10.采集每个关节电机的转角θ
1，2,..,n
、角速度角加速度其中下标n代表第n个关节；
11.利用集中质量法，将每根所述臂杆离散为m个由弹簧阻尼相连接的刚性臂，通过弹簧阻尼等效关节柔性和臂杆柔性，测出各个测量点的角速度并利用积分获得角位移q
11,...,nm
，利用微分获得角加速度其中下标nm代表第n节臂杆上的第m个陀螺仪；
12.利用拉格朗日法获取离散后的刚性臂的惯性矩阵m，获得离散为多节刚性臂的柔性机械臂系统模型如下：
[0013][0014]
式中，m(q,θ)为n
×
m行n
×
m列的对称正定矩阵；向量q为n
×
m行的列向量，元素分别为[q
11
,q
12
,...,q
1m
,q
21
,...,q
nm
]
t
，表示各等效刚性臂的转动角度；向量θ为n
×
m行的列向量，元素为[θ1,0,...,0,θ2,...,0,θn,...,0]，其中第k
×
m+1个元素为θ
k+1
，k为整数，0＜＝k＜＝n-1，其它元素均为0，用来表示各个关节电机的输入角度；c表示等效离散模型的阻尼矩阵，为n
×
m行n
×
m列对角正定矩阵，元素分别为各等效关节的阻尼(c
11
,c
12
,...,c
nm
)，下标nm则表示第n节臂杆上的第m个等效刚性臂对应的柔性关节阻尼；k为n
×
m行n
×
m列对角正定矩阵，表示等效离散模型的刚度矩阵，元素分别为各等效关节的刚度(k
11
,k
12
,...,k
nm
)，下标nm则表示第n节臂杆上的第m个等效刚性臂对应的柔性关节刚度；
[0015]
s2、计算模态及主坐标转换：
[0016]
忽略柔性机械臂系统模型中的阻尼矩阵，将式(1)改写为：
[0017][0018]
对式(2)的左侧进行求解，求出柔性机械臂系统的振动模态：
[0019]
(k-ω2m(q,θ))a＝0
ꢀꢀꢀ
(3)
[0020]
求出系统的固有频率ω
(1,...,n
×
m)
以及对应的振动模态φ
(1,...,n
×
m)
，进而在主坐标模式下的振动方程表示如下：
[0021][0022]
式(4)中，主惯量m
p
＝φ
t
m(q,θ)φ，主刚度k
p
＝φ
t
kφ，φ＝[φ
(1)
,...,φ
(n
×
m)
]，主坐标下的模态位移x
p
＝φ-1
x，向量x为q；
[0023]
通过以上步骤，实现各阶模态的解耦与坐标转换；
[0024]
s3、计算振动模态的振动相位:
[0025]
根据x
p
＝φ-1
x，可求出主坐标振动速度为故可计算出在输入下的各阶模态的振动相位：
[0026][0027]
式中，上标(i)表示第i阶模态，表示第i阶模态的振动相位，a
phase(i)
为第i阶模态的振幅，ω(i)表示第i阶模态的固有频率；
[0028]
s4、计算减振加速度：
[0029]
对式(4)求解零输入下系统的振动能量，有
[0030][0031]
式中a
phase
为n
×
m行的列向量，其各元素即为各阶模态的振幅；
[0032]
结合式(4)与(5)，即可获得弹性系统振动能量与输入之间的关系，令可得下式：
[0033][0034]
式中，γ＝k
112
+...+k
nm2
＞0，∏可表示为相位与刚度的函数由此式即可得各输入端的位移加速度的变化量
[0035]
考虑采用该难以使加速度值最终收敛到0，故采取实际的加速度如下：
[0036][0037]
式中，tk为第k次控制周期的值，ka为选取的较小正值，用于保证最终收敛到0，kv则是保证收敛到0；
[0038]
s5、计算定位速度：
[0039]
令减振部分速度如下式：
[0040][0041]
类似的，令最终的目标速度计算如下，即可令θ对期望角度θ
desire
进行跟踪与定位，从而实现令最终的位置误差收敛到0：
[0042][0043]
s6、位置控制输出：
[0044]
最终用下式表示对位置控制器的实际输出：
[0045]
[0046]
式中，θ
output
输出即为对位置控制器的实际输出。
[0047]
基于上述，对惯性矩阵m而言，其受小角度变化时的影响小，当变形量q较小时，可认为其仅受关节转角θ影响；当系统在某一位置进行自由振动时，可令θ为零，即可表示其振动状态；通过式(1)进行辨识获取等效的阻尼矩阵c、刚度矩阵k。
[0048]
基于上述，步骤s1中通过编码器采集每个关节电机的转角θ、角速度角加速度
[0049]
基于上述，步骤s1中，根据臂杆柔性程度，在每根臂杆上间隔设置有m个测量点，测量点的个数可根据臂架软硬程度分别进行改变，每个测量点上安装陀螺仪测量角速度。
[0050]
本发明相对现有技术具有突出的实质性特点和显著的进步，具体的说，本发明具有以下优点：
[0051]
(1)方法最终的实际输出对象为位置控制器θ
output
，减振原理是针对振动模态进行位移补偿，不会引起更高阶模态的振动位移。
[0052]
(2)本方法在电流环路的外层，即位置环路与速度环路进行控制，相比于采用力控的控制方法，本方法对原有系统更改更少，要求更低，适用于包括电机驱动、液压驱动在内的多种驱动方式。
[0053]
(3)本方法的振动抑制本质上是反馈控制的一种，可以使作动器对柔性关节产生的振动与柔性臂杆产生的振动相抵消，对已有振动进行实时消除，适用于有外力下的强迫振动、系统本身的残余振动等不同类型振动的抑制，应用更加全面。
[0054]
(4)采用的传感器除电机(或其他作动器)自带的编码器外，仅需额外添加陀螺仪，成本低。
附图说明
[0055]
图1是本发明中柔性机械臂上传感器的的安装结构示意图。
[0056]
图2是本发明中柔性机械臂振动抑制方法的原理框图。
[0057]
图3是本发明中柔性机械臂被离散为多节刚性臂的原理示意图。
[0058]
图中：1.臂杆；2.关节；3.电机；4.陀螺仪。
具体实施方式
[0059]
下面通过具体实施方式，对本发明的技术方案做进一步的详细描述。
[0060]
如图1-3所示，一种柔性机械臂振动抑制方法，所述柔性机械臂包括多根臂杆1和连接在臂杆1之间的关节2，每个关节2均由一电机3驱动，包括以下步骤：
[0061]
s1、动力学建模：
[0062]
通过编码器采集每个关节2电机3的转角θ
1，2...n
、角速度角加速度其中下标n代表第n个关节；
[0063]
根据臂杆柔性程度，在每节臂杆1上间隔安装m个陀螺仪4，利用集中质量法，将每根所述臂杆离散为m个由弹簧阻尼相连接的刚性臂，通过弹簧阻尼等效关节柔性和臂杆柔性，测出各个测量点的角速度并利用积分获得角位移q
11,...,nm
，利用微分获得角加速度其中下标nm代表第n节臂杆上的第m个陀螺仪；
[0064]
利用拉格朗日法获取离散后的刚性臂的惯性矩阵m，获得离散为多节刚性臂的柔性机械臂系统模型如下：
[0065][0066]
式中，m(q,θ)为n
×
m行n
×
m列的对称正定矩阵；向量q为n
×
m行的列向量，元素分别为[q
11
,q
12
,...,q
1m
,q
21
,...,q
nm
]
t
，表示各等效刚性臂的转动角度；向量θ为n
×
m行的列向量，元素为[θ1,0,...,0,θ2,...,0,θn,...,0]，其中第k
×
m+1个元素为θ
k+1
，k为整数，0＜＝k＜＝n-1，其它元素均为0，用来表示各个关节电机的输入角度；c表示等效离散模型的阻尼矩阵，为n
×
m行n
×
m列对角正定矩阵，元素分别为各等效关节的阻尼(c
11
,c
12
,...,c
nm
)，下标nm则表示第n节臂杆上的第m个等效刚性臂对应的柔性关节阻尼；k为n
×
m行n
×
m列对角正定矩阵，表示等效离散模型的刚度矩阵，元素分别为各等效关节的刚度(k
11
,k
12
,...,k
nm
)，下标nm则表示第n节臂杆上的第m个等效刚性臂对应的柔性关节刚度。
[0067]
对惯性矩阵m(q,θ)而言，其受小角度变化时的影响小，故当变形量q较小时，可认为其仅受关节转角θ影响。当系统在某一位置进行自由振动时，可令θ为零，即可表示其振动状态。
[0068]
可以利用式(1)进行对多自由度柔性机械臂进行参数辨识，即可获取阻尼矩阵c、刚度矩阵k。
[0069]
s2、计算模态与主坐标转换：
[0070]
进行模态计算时，获取阻尼矩阵对各阶模态幅值、相位的影响较为困难，且难以实现各阶模态解耦计算。考虑到一般柔性臂的阻尼矩阵较小，且阻尼对模态相位的计算影响较小，故在进行模态计算时可以进行忽略，将式(1)改写为：
[0071][0072]
由(2)可以看出，当关节处施加一个恒定的加速度时，对系统产生的振动可以认为是当前的振动状态与由加速度变化引起的振动状态的叠加。故通过改变电机端位置输入的加速度大小(即附加一个)，即可实现振动的叠加，以此为基础，即可利用振动的线性叠加原理，令电机3端输入产生的振动与系统原有的振动进行抵消，从而减少振动。
[0073]
对式(2)的左侧进行求解，可求出柔性臂系统的振动模态：
[0074]
(k-ω2m(q,θ))a＝0
ꢀꢀꢀ
(3)
[0075]
即可求出系统的固有频率ω
(1,..,n
×
m)
以及对应的振动模态φ
(1,..,n
×
m)
，共计n
×
m阶，进而在主坐标模式下的振动方程表示如下：
[0076][0077]
式(4)中，主惯量m
p
＝φ
t
m(q,θ)φ，主刚度k
p
＝φ
t
kφ，主坐标下的模态位移x
p
＝φ-1
x，向量x即为向量q，φ＝[φ
(1)
,...,φ
(n
×
m)
]通过以上步骤，即将各阶模态实现了解耦与坐标转换。
[0078]
s3、计算振动模态的振动相位：
[0079]
根据x
p
＝φ-1
x，可求出主坐标振动速度为故可计算出在输入下的各阶模态的振动相位：
[0080][0081]
式中，上标(i)表示第i阶模态，表示第i阶模态的振动相位，a
phase(i)
为第i阶模态的振幅，ω(i)表示第i阶模态的固有频率。
[0082]
s4、减振加速度计算：
[0083]
对式(4)求解零输入下系统的振动能量，有
[0084][0085]
式中，a
phase
为n
×
m行的列向量，其各元素即为各阶模态的振幅；
[0086]
结合式(4)与(5)，即可获得弹性系统振动能量与输入之间的关系，令可得下式：
[0087][0088]
式中，γ＝k
112
+...+k
nm2
＞0，∏可表示为相位与刚度的函数由此式即可得各输入端的位移加速度的变化量
[0089]
考虑采用该难以使加速度值最终收敛到0，故采取实际的加速度如下：
[0090][0091]
式中，tk为第k次控制周期的值，ka为选取的较小正值，用于保证最终收敛到0，kv则是保证收敛到0。
[0092]
s5、计算定位速度：
[0093]
令减振部分速度如下式
[0094][0095]
类似的，令最终的目标速度计算如下，即可令θ对期望角度θ
desire
进行跟踪与定位，从而实现令最终的位置误差收敛到0：
[0096][0097]
s6、位置控制输出：
[0098]
最终对位置控制器的实际输出可以表示为下式
[0099]
[0100]
式中，θ
output
输出即为对位置控制器的实际输出。
[0101]
最后应当说明的是:以上实施例仅用以说明本发明的技术方案而非对其限制；尽管参照较佳实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者对部分技术特征进行等同替换；而不脱离本发明技术方案的精神，其均应涵盖在本发明请求保护的技术方案范围当中。

技术特征：
1.一种柔性机械臂振动抑制方法，所述柔性机械臂包括多根臂杆和连接在臂杆之间的关节，每个关节均由一电机驱动，其特征在于，包括以下步骤：s1、动力学建模：采集每个关节电机的转角θ
1，2,..,n
、角速度角加速度其中下标n代表第n个关节；利用集中质量法，将每根所述臂杆离散为m个由弹簧阻尼相连接的刚性臂，通过弹簧阻尼等效关节柔性和臂杆柔性，测出各个测量点的角速度并利用积分获得角位移q
11,...,nm
，利用微分获得角加速度其中下标nm代表第n节臂杆上的第m个陀螺仪；利用拉格朗日法获取离散后的刚性臂的惯性矩阵m，获得离散为多节刚性臂的柔性机械臂系统模型如下：式中，m(q,θ)为n
×
m行n
×
m列的对称正定矩阵；向量q为n
×
m行的列向量，元素分别为[q
11
,q
12
,...,q
1m
,q
21
,...,q
nm
]
t
，表示各等效刚性臂的转动角度；向量θ为n
×
m行的列向量，元素为[θ1,0,...,0,θ2,...,0,θ
n
,...,0]，其中第k
×
m+1个元素为θ
k+1
，k为整数，0＜＝k＜＝n-1，其它元素均为0，用来表示各个关节电机的输入角度；c表示等效离散模型的阻尼矩阵，为n
×
m行n
×
m列对角正定矩阵，元素分别为各等效关节的阻尼(c
11
,c
12
,...,c
nm
)，下标nm则表示第n节臂杆上的第m个等效刚性臂对应的柔性关节阻尼；k为n
×
m行n
×
m列对角正定矩阵，表示等效离散模型的刚度矩阵，元素分别为各等效关节的刚度(k
11
,k
12
,...,k
nm
)，下标nm则表示第n节臂杆上的第m个等效刚性臂对应的柔性关节刚度；s2、计算模态及主坐标转换：忽略柔性机械臂系统模型中的阻尼矩阵，将式(1)改写为：对式(2)的左侧进行求解，求出柔性机械臂系统的振动模态：(k-ω2m(q，θ))a＝0
ꢀꢀ
(3)求出系统的固有频率ω
(1，...，n
×
m)
以及对应的振动模态φ
(1，...，n
×
m)
，进而在主坐标模式下的振动方程表示如下：式(4)中，主惯量m
p
＝φ
t
m(q，θ)φ，主刚度k
p
＝φ
t
kφ，φ＝[φ
(1)
，...，φ
(n
×
m)
]，主坐标下的模态位移x
p
＝φ-1
x，向量x为q；通过以上步骤，实现各阶模态的解耦与坐标转换；s3、计算振动模态的振动相位：根据x
p
＝φ-1
x，可求出主坐标振动速度为故可计算出在输入下的各阶模态的振动相位：
式中，上标(i)表示第i阶模态，表示第i阶模态的振动相位，a
phase(i)
为第i阶模态的振幅，ω
(i)
表示第i阶模态的固有频率；s4、计算减振加速度：对式(4)求解零输入下系统的振动能量，有式中a
phase
为n
×
m行的列向量，其各元素即为各阶模态的振幅；结合式(4)与(5)，即可获得弹性系统振动能量与输入之间的关系，令可得下式：式中，γ＝k
112
+...+k
nm2
＞0，π可表示为相位与刚度的函数由此式即可得各输入端的位移加速度的变化量考虑采用该难以使加速度值最终收敛到0，故采取实际的加速度如下：式中，t
k
为第k次控制周期的值，k
a
为选取的较小正值，用于保证最终收敛到0，k
v
则是保证收敛到0；s5、计算定位速度：令减振部分速度如下式：类似的，令最终的目标速度计算如下，即可令θ对期望角度θ
desire
进行跟踪与定位，从而实现令最终的位置误差收敛到0：s6、位置控制输出：最终用下式表示对位置控制器的实际输出：式中，θ
output
输出即为对位置控制器的实际输出。2.根据权利要求1所述的柔性机械臂振动抑制方法，其特征在于：对惯性矩阵m而言，其
受小角度变化时的影响小，当变形量q较小时，可认为其仅受关节转角θ影响；当系统在某一位置进行自由振动时，可令θ为零，即可表示其振动状态；通过式(1)进行辨识获取等效的阻尼矩阵c、刚度矩阵k。3.根据权利要求1或2所述的柔性机械臂振动抑制方法，其特征在于：步骤s1中通过编码器采集每个关节电机的转角θ、角速度角加速度4.根据权利要求1或2所述的柔性机械臂振动抑制方法，其特征在于：步骤s1中，根据臂杆柔性程度，在每根臂杆上间隔设置有m个测量点，测量点的个数可根据臂架软硬程度分别进行改变，每个测量点上安装陀螺仪测量角速度。

技术总结
本发明提供了一种柔性机械臂振动抑制方法，所述柔性机械臂包括多根臂杆和连接在臂杆之间的关节，每个关节均由一电机驱动，包括以下步骤：S1、动力学建模；S2、模态计算与主坐标转换；S3、计算振动模态的振动相位；S4、减振加速度计算与积分；S5、计算定位速度；S6、位置控制输出。该柔性机械臂振动抑制方法具有面对多自由度、多模态的柔性机械臂时不易引起高频振荡(高阶模态)，无需对关节的柔性做低柔性假设；在电流环路的外层，即位置环路与速度环路进行控制，对控制系统要求低，对系统改动小、改进成本低；可以使作动器对柔性关节产生的振动与柔性臂杆产生的振动相抵消，对已有振动进行实时消除，应用更加全面的优点。应用更加全面的优点。应用更加全面的优点。


技术研发人员：赵璞 张志铭 王蕊蕊
受保护的技术使用者：河南工业大学
技术研发日：2023.05.29
技术公布日：2023/7/22