一种基于灵敏度分析的混联机器人动力学参数辨识方法

1.本发明涉及工业机器人动力学领域，特别涉及一种基于灵敏度分析的混联机器人动力学参数辨识方法。


背景技术：

2.目前，随着航空航天事业的发展，对于大型结构件的需求不断增加。这些结构件具有壁薄、尺寸大、易变形、精度要求高和材料去除量大的特点。针对这类结构件，采用混联机器人加工逐渐成为主流。但采用混联机器人进行高精度加工，需要有较为复杂的控制策略和精确的动力学模型。为了获取混联机器人较为精确的动力学模型，进行混联机器人动力学参数辨识是至关重要的一步。
3.动力学参数主要分为机器人本体的惯性参数以及关节摩擦参数。对于惯性参数，虽然可以利用三维建模软件根据机器人本体的三维模型自动导出，但由于零件的加工误差以及不可避免的装配误差，通过这种方法计算出来的惯性参数会有较大偏差，并且这种方法不能得到摩擦参数。另一种通过设计实验，让机器人沿着预定轨迹运动，利用采集计算得到的关节位置、速度、加速度以及力矩信息进行辨识的方法更加可行，并且辨识出来的惯性参数和摩擦参数可以真实反映机器人的工作状态。
4.这种基于实验的辨识方法虽然能辨识出真实的动力学参数，但是辨识出来的结果往往是许多参数耦合在一起的，并不能获取单个动力学参数的值。除此之外，由于待辨识参数过多以及采集的数据具有噪声，常出现辨识精度不高的问题。


技术实现要素：

5.本发明为解决公知技术中存在的技术问题而提供一种基于灵敏度分析的混联机器人动力学参数辨识方法。
6.本发明为解决公知技术中存在的技术问题所采取的技术方案是：一种基于灵敏度分析的混联机器人动力学参数辨识方法，该混联机器人为五自由度混联机器人，其包括三自由度并联机构和与之串接的具有两个转动自由度的串联机构，三自由度并联机构包括动平台、第一主动臂、第二主动臂、第三主动臂、从动支撑臂、第一转动支架及第二转动支架；其中：动平台周侧铰接有三个由伺服电机驱动伸缩的主动臂，依次为第一主动臂、第二主动臂、第三主动臂；动平台后端固接有从动支撑臂；串联机构包括由伺服电机驱动的a/c轴双摆头，其中双摆头的c轴与动平台转动连接，双摆头的a轴与刀具连接；第一主动臂通过第一转轴与第一转动支架转动连接；第一转动支架通过第二转轴与固定的轴承座转动连接；第二主动臂、第三主动臂、从动支撑臂，对应通过第三转轴、第四转轴、第五转轴与第二转动支架转动连接；第二转动支架通过第六转轴与固定的轴承座转动连接；双摆头的c轴与a轴通过第七转轴转动连接；第一转轴与第二转轴的轴线垂直；第三转轴、第四转轴、第五转轴的轴线相互平行且与第六转轴的轴线垂直；第二转动支架的中心位于第五转轴的轴线与第六转轴的轴线交点上；包括以下步骤：
7.步骤1，对混联机器人依次进行位置逆解、速度逆解、加速度逆解，然后利用虚功原理建立混联机器人的动力学模型，该动力学模型的动力学方程如下：
8.f＝mξa+cξv+g+τf；
9.其中：
10.f＝[f
1 f
2 f
3 τ
4 τ5]
t
；
[0011][0012]
式中：
[0013]
f1为第一主动臂的驱动力；
[0014]
f2为第二主动臂的驱动力；
[0015]
f3为第三主动臂的驱动力；
[0016]
τ4为c轴的驱动力矩；
[0017]
τ5为a轴的驱动力矩；
[0018]
f1为第一主动臂的移动副的摩擦力；
[0019]
f2为第二主动臂的移动副的摩擦力；
[0020]
f3为第三主动臂的移动副的摩擦力；
[0021]
τ
f4
为c轴的转动副的摩擦力矩；
[0022]
τ
f5
为a轴的转动副的摩擦力矩；
[0023]
为混联机器人的惯性矩阵；
[0024]
为混联机器人的离心力和哥式力矩阵；
[0025]
为混联机器人的重力向量；
[0026]
为混联机器人末端刚体刀尖点的广义速度；
[0027]
为混联机器人末端刚体刀尖点的广义加速度；
[0028]
步骤2，将混联机器人各个构件的惯性力和惯性力矩等效到对应的关键点，对五自由度混联机器人的动力学模型进行线性化，将待辨识参数分离出来，得到如下公式：
[0029]
f＝φp；
[0030]
式中：
[0031]
φ为混联机器人的系统观测矩阵；
[0032]
p为混联机器人的待辨识动力学参数；
[0033]
步骤3，设位置、速度、加速度三者初始或结束时数值均为0，运动过程中满足工作空间、位置、速度、加速度四者的参数约束，按照七次多项式规划混联机器人各个关节的辨识轨迹；
[0034]
步骤4，运行混联机器人，运行过程中采集混联机器人的各个关节的位置信息和各个伺服电机的驱动电流，获得ξv、ξa、φ、f；
[0035]
步骤5，采用sobol全局灵敏度分析方法，对线性化的动力学模型进行参数灵敏度分析，将待辨识惯性参数按照灵敏度高低分为三类：高灵敏度参数p
high
、低灵敏度参数p
low
和零灵敏度参数p
zero
；对于零灵敏度参数p
zero
不做处理，对于低灵敏度参数p
low
采用三维软件自动导出数值，而对于高灵敏度参数p
high
采用遗传算法进行辨识。
[0036]
进一步地，步骤1包括如下分步骤：
[0037]
步骤1-1，设：第一主动臂轴线与第一转轴轴线的交点为b1；第二主动臂轴线与第三转轴轴线的交点为b2；第三主动臂轴线与第四转轴轴线的交点为b3；从动支撑臂轴线与第五转轴轴线的交点为b4；第一主动臂与动平台铰接点为a1；第二主动臂与动平台铰接点为a2；第三主动臂与动平台铰接点为a3；a2、a3的连线与双摆头的c轴轴线的交点为a4；基座标系坐标轴包括两两相互垂直的x0轴、y0轴、z0轴；b4为基坐标系原点，b2、b3位于x0轴上；a1相对b1的位移矢量为q
3,1
；a2相对b2的位移矢量为q
3,2
；a3相对b3的位移矢量为q
3,3
；a4相对b4的位移矢量为q
3,4
；双摆头的c轴绕动平台轴线转动的转角为θ
4,4
；双摆头的a轴绕第七转轴转动的转角为θ
5,4
；第七转轴轴线与动平台轴线的交点为p；混联机器人末端刚体刀尖点为g点，其对应基坐标系原点的位置矢量为rg；刀轴绕基坐标系x0轴的转角为α；绕基坐标系y0轴的转角为β；
[0038]
对混联机器人进行位置逆解，由rg、α、β，得到q
3,1
、q
3,2
、q
3,3
、q
3,4
、θ
4,4
、θ
5,4
；
[0039]
步骤1-2，对混联机器人进行速度逆解和加速度逆解，由ξv得到：第一至第三主动臂的伸缩速度及对应的加速度，双摆头的c轴和a轴的转动速度及对应的加速度；
[0040]
步骤1-3，将第一至第三主动臂及从动支撑臂拆分为若干构件；由第一至第三主动臂的伸缩速度及对应的加速度，以及双摆头的c轴和a轴的转动速度及对应的加速度，得到各构件质心的速度和加速度；
[0041]
步骤1-4，分别计算各个构件的惯性力和惯性力矩，再利用虚功原理建立混联机器人的动力学模型。
[0042]
进一步地，步骤1-2包括如下分步骤：
[0043]
步骤1-2-1，通过对下式求导，得到点p的速度v
p
；
[0044]
rg＝r
p
+r
pg
；
[0045]
式中：r
p
为点p的位置矢量；r
pg
为由点p指向点g的位置矢量；
[0046]
步骤1-2-2，以点p为节点，利用如下的闭环约束方程，分别求导得到2，以点p为节点，利用如下的闭环约束方程，分别求导得到
[0047]rp
＝(q
3,4
+e)s
3,4
,r
p
＝bk+q
3,ks3,k-ak+es
3,4
,k＝1～3；
[0048]
式中：e为点a4到点p的长度；s
3,k
，k＝1～3，对应为第一、第二、第三主动臂轴线的单位矢量；bk为基坐标系原点o指向点bk的矢量；ak为点a4指向点ak的矢量；
[0049]
步骤1-2-3，，根据角速度叠加原理得到如下公式，进一步得到和
[0050][0051]
式中：w
5,4
为混联机器人末端刚体刀尖点的角速度；w
3,4
为动平台的角速度；s
4,4
为双摆头的c轴旋转轴线的单位矢量；s
5,4
为双摆头的a轴旋转轴线的单位矢量。
[0052]
进一步地，步骤2包括如下分步骤：
[0053]
步骤2-1，选取各构件的关键节点；
[0054]
步骤2-2，计算求得各个构件所对应关键节点的广义速度，以及将各构件质心处的惯性力和惯性力矩等效到所对应的关键节点，以此将每个构件的惯性参数分离出来；
[0055]
步骤2-3，由f及关键节点的广义速度、惯性力、惯性力矩，利用虚功原理，建立混联机器人的线性化动力学模型。
[0056]
进一步地，步骤3包括如下分步骤：
[0057]
步骤3-1，设置辨识轨迹的约束条件如下式所示：
[0058][0059][0060]
式中：
[0061]
t为机器人运动时间；
[0062]q3,k
(t)为第k个主动臂移动副在t时刻的位移；
[0063]q3,k_0
为第k个主动臂移动副初始位移；
[0064]
θ
u,4
(t)为第u个主动转动副在t时刻的转角；u＝4时，第u个主动转动副为双摆头的c轴转动副；
[0065]
θ
u,4_0
为第u个主动转动副初始转角；u＝5时，第u个主动转动副为双摆头的a轴转动副；
[0066]
步骤3-2，按照七次多项式模型规划混联机器人各个关节的辨识轨迹；其中，七次多项式模型如下式所示：
[0067]
x(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5+a6t6+a7t7；
[0068]
x(t)为关节在t时刻的位移或转角；
[0069]
a0～a7为对应七次多项式的0次至7次的项系数；
[0070]
根据步骤3-1中的约束条件，计算出各个关节辨识轨迹的多项式系数，得到各个关节辨识轨迹。
[0071]
进一步地，第一至第三主动臂均包括套筒、伸缩杆及由伺服电机驱动的丝杠副；伸缩杆由丝杠副带动相对套筒伸缩；第一主动臂的套筒与第一转轴固接；第二主动臂的套筒与第三转轴固接；第三主动臂的套筒与第四转轴固接；步骤4包括如下分步骤：
[0072]
步骤4-1，将获取的各个关节辨识轨迹编写为g代码，然后输入到混联机器人的控制器中，驱动混联机器人的各个关节运动；
[0073]
步骤4-2，在混联机器人按照辨识轨迹运行过程中，同步采集混联机器人伸缩杆与动平台铰接点的位置数据和对应的伺服电机电流数据，生成各个关节的位置曲线和电流曲线；
[0074]
步骤4-3，利用低通滤波器对各个关节的位置曲线进行滤波，然后依次微分得到各个关节的速度和加速度；
[0075]
步骤4-4，利用混联机器人的正运动学，根据各个关节位置、速度和加速度，计算出末端刚体的位姿信息和末端刀尖点的广义速度ξv和广义加速度ξa；
[0076]
步骤4-5，根据采样得到的驱动双摆头的c轴及a轴转动的伺服电机电流，按照下式得到对应的驱动c轴及a轴转动的驱动力矩τd：
[0077]
τd＝rekaim；
[0078]
式中：ka为伺服电机的转矩常数；re为与伺服电机连接的减速机的减速比；im为伺服电机电流；
[0079]
然后再根据采样得到的驱动第一至第三主动臂的伺服电机电流，按照下式得到对应的驱动第一至第三主动臂的驱动力fd：
[0080][0081]
式中：b为丝杠的导程；
[0082]
利用低通滤波器将计算得到的驱动力及驱动力矩进行滤波处理；
[0083]
步骤4-6，将末端刚体的位姿信息以及末端刀尖点的广义速度、广义加速度代入到观测矩阵φ的表达式中，求解得到观测矩阵φ的结果。
[0084]
进一步地，步骤5包括如下分步骤：
[0085]
步骤5-1，将待辨识的惯性参数根据其各自的取值范围映射到单位区间上，然后将线性化出来的动力学模型分解成如下关于待辨识惯性参数的函数形式：
[0086][0087]
p＝[x
1 x
2 ... xn]
t
；
[0088]
式中：
[0089]
f0为常数；
[0090]
n为参数数量；
[0091]
xi为第i个待辨识参数；
[0092]
xj为第j个待辨识参数；
[0093]fi
(xi)为仅包含待辨识参数xi的函数子项；
[0094]fi,j
(xi,xj)为包含待辨识参数xi和xj的函数子项；
[0095]f1,2,
…
,n
(x1,x2,
…
,xn)为包含待辨识参数x1,x2,
…
,xn的函数子项；
[0096]
动力学模型分解出来的所有子项之间都正交，则有：
[0097][0098]in
＝(p|0≤xi≤1；i＝1,2,
…
,n)；
[0099]
式中：
[0100]in
为待辨识的参数映射出来的n维单元体；
[0101]
s为所要正交的函数子项中包含的待辨识参数个数；
[0102]
e1，e2…
，es对应为待辨识参数集合e中的第1,2,
…
,s个参数；
[0103]
h1，h2…
，hs对应为待辨识参数集合h中的第1,2,
…
,s个参数；
[0104]
为包含待辨识参数集合e中的第1,2,
…
,s个参数的函数子项；
[0105]
为包含待辨识参数集合h中的第1,2,
…
,s个参数的函数子项；
[0106]
步骤5-2，对线性化的动力学模型分解之后的表达式取多重积分，得到如下各子项：
[0107][0108]
式中：
[0109]
p-i
为除xi之外的其余参数；
[0110]
p-(ij)
为除xi和xj之外的其余参数；
[0111]fj
(xj)为包含参数xj的函数子项；
[0112]
以此类推，求得其他的子项；
[0113]
步骤5-3，根据步骤5-3求解的各子项，求得线性化动力学模型函数的总方差和各项偏方差：
[0114][0115][0116]
其中，各项方差和总方差之间有如下特性：
[0117][0118]
xs为第s个参数；
[0119]
d为线性化动力学模型函数的总方差；
[0120]di
为参数xi的偏方差；
[0121]dij
为参数xi和xj的偏方差；
[0122]d1,2,
…
,s
为参数x1,x2,
…
,xs的偏方差；
[0123]d1,2,
…
,n
为参数x1,x2,
…
,xn的偏方差；
[0124]
步骤5-4，根据第三步求解的总方差和各项方差，按照下式计算出所有参数的各阶灵敏度
[0125][0126]
通过下式，计算得到参数xi的总灵敏度系数：
[0127][0128][0129]
式中：
[0130]sti
为参数xi的总灵敏度系数；
[0131]
d-i
为除了参数xi外，其他参数的偏方差之和；
[0132]
步骤5-5，对参数变量进行两次独立的抽样，进而得到两个矩阵an×n和bn×n，其中n为抽样的个数，n为参数数量；然后利用蒙特卡洛法，按照如下公式计算出和
[0133][0134]
式中：
[0135]
为样本a中第v列被b中第v列元素替换所得到的新的样本矩阵；
[0136]
为样本b中第v列被a中第v列元素替换所得到的新的样本矩阵；
[0137]
f(a)w为样本a对应的动力学模型函数值；
[0138]
为样本对应的动力学模型函数值；
[0139]
为样本对应的动力学模型函数值；
[0140]
为用蒙特卡洛法对f0的估计值；
[0141]
为用蒙特卡洛法对动力学模型函数总方差的估计值；
[0142]
为用蒙特卡洛法对参数xv偏方差的估计值；
[0143]
为用蒙特卡洛法对除了参数xv外，其他参数的偏方差之和的估计值；
[0144]
步骤5-6，将计算得到的待辨识惯性参数的灵敏度值分为三类；选取灵敏度值区间中的前30％个参数定义为高灵敏度参数p
high
，选取灵敏度值区间中的后20％个参数定义为为零灵敏度参数p
zero
，剩下的参数定义为低灵敏度参数p
low
；
[0145]
步骤5-7，设置待辨识惯性参数p
high
和摩擦参数为遗传算法的种群，目标函数如下式所示：
[0146][0147]
式中：q为采样数；
[0148]
q为采样序号；
[0149]
τ
qa
为第q个实际采样力矩；
[0150]
τ
qt
为对应第q个实际采样力矩的理论计算力矩；
[0151]
步骤5-8，根据适应度值，利用交叉、遗传和变异特性不断迭代更新种群，得到最优适应度下的种群，即为利用遗传算法辨识出来的参数。
[0152]
进一步地，还包括步骤6，根据已辨识的参数，通过实验对动力学模型进行验证及评估，并修正混联机器人动力学模型。
[0153]
进一步地，步骤6包括如下分步骤：
[0154]
步骤6-1，利用辨识出来的动力学参数计算出各关节的驱动力和驱动力矩，绘制理论预测曲线，并与采集的驱动力和驱动力矩数据进行对比；
[0155]
步骤6-2，使用如下均方根误差对得到的动力学模型进行评估：
[0156][0157]
式中：
[0158]
q为采样数；
[0159]
q为采样序号；
[0160]
τ
qa
为第q个实际采样力矩；
[0161]
φ为观测矩阵；
[0162]
为辨识出的动力学参数。
[0163]
进一步地，使其中包括70个机器人本体的惯性参数以及10个关节摩擦参数。
[0164]
本发明具有的优点和积极效果是：本发明利用sobol全局灵敏性分析方法对线性化化后的动力学模型进行了参数灵敏度分析，将待辨识惯性参数分为高灵敏度参数p
high
、低灵敏度参数p
low
以及零灵敏度参数p
zero
，对零灵敏度参数不做处理，低灵敏度参数采用三维软件导出的值，针对高灵敏度参数和摩擦参数采用遗传算法辨识，减少了待辨识参数，提高了辨识效率和辨识精度。
附图说明
[0165]
图1表示本发明中的混联机器人立体结构主视图。
[0166]
图2表示本发明中的混联机器人立体结构后视图。
[0167]
图3表示本发明中的混联机器人的机构简图。
[0168]
图4表示本发明中的混联机器人并联机构各关节辨识轨迹曲线。
[0169]
图5表示本发明中的混联机器人串联机构各关节辨识轨迹曲线。
[0170]
图6表示适应度值变化曲线。
[0171]
图7表示混联机器人的第一主动臂采集的驱动力和计算的驱动力预测曲线。
[0172]
图8表示混联机器人的第二主动臂采集的驱动力和计算的驱动力预测曲线。
[0173]
图9表示混联机器人的第三主动臂采集的驱动力和计算的驱动力预测曲线。
[0174]
图10表示混联机器人的双摆头的c轴采集的驱动力矩和计算的驱动力矩预测曲线。
[0175]
图11表示混联机器人的双摆头的a轴采集地驱动力矩和计算的驱动力矩预测曲线。
[0176]
图1、图2中：
[0177]
1：第二固定轴座；2：第一固定轴座；3：第一伺服电机；4：第二伺服电机；5：第三伺服电机；6：第一主动臂；7：第二主动臂；8：第三主动臂；9：第一球铰；10：第二球铰；11：第三球铰；12：第二转动支架；13：从动支撑臂；14：动平台；15：双摆头的c轴；16：双摆头的a轴；17：主轴；8：刀具；19：第一转动支架。
[0178]
图3中：
[0179]
o(b4)：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
为从动支撑臂轴线和转动支架轴线的交点；
[0180]ai
(i＝1,2,3)：
ꢀꢀꢀꢀꢀ
对应为第一、第二、第三球铰的中心点；
[0181]
a4：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
为动平台上a2点和a3点的中点；
[0182]bi
(i＝1,2,3)：
ꢀꢀꢀꢀꢀ
对应为第一、第二、第三主动臂轴线和转动支架轴线的交点；
[0183]
p：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
为第七转轴轴线与动平台轴线的交点；
[0184]
g：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
为刀具的刀尖点；
[0185]
x0,y0,z0：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
对应为以o点为原点建立的基座标系的x轴、y轴和z轴；
[0186]s3,i
(i＝1～4)：
ꢀꢀꢀꢀ
对应为第一、第二、第三主动臂轴线及从动支撑臂轴线的单位矢量；
[0187]s4,4
：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
为双摆头的c轴旋转轴线的单位矢量；
[0188]s5,4
：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
为双摆头的a轴旋转轴线的单位矢量；
[0189]ai
(i＝1,3)：
ꢀꢀꢀꢀꢀꢀꢀ
对应为a4点到a1、a3点的矢量；
[0190]bi
(i＝1,3)：
ꢀꢀꢀꢀꢀꢀꢀ
对应为o点到b1、b3点的矢量；
[0191]q3,i
(i＝1～4)：对应为第一、第二、第三主动臂轴线及从动支撑臂的移动副位移(bi点到ai点的距离)；
[0192]
θ
4,4
：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
为双摆头的c轴绕动平台轴线转动的转角；
[0193]
θ
5,4
：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
为双摆头的a轴绕第七转轴转动的转角。
具体实施方式
[0194]
下面将参考附图并结合实施例来详细说明本发明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。
[0195]
在本发明的描述中，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明而不是要求本发明必须以特定的方位构造和操作，因此不能理解为对本发明的限制。本发明中使用的术语“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接；可以是直接相连，也可以通过中间部件间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
[0196]
请参见图1至图11，一种基于灵敏度分析的混联机器人动力学参数辨识方法，该混联机器人为五自由度混联机器人，其包括三自由度并联机构和与之串接的具有两个转动自由度的串联机构组成，三自由度并联机构包括动平台14、第一主动臂6、第二主动臂7、第三主动臂8、从动支撑臂13、第一转动支架19及第二转动支架12；其中：
[0197]
动平台14周侧铰接有三个由伺服电机驱动伸缩的主动臂，依次为第一主动臂6、第二主动臂7、第三主动臂8；第一主动臂6通过第一球铰9与动平台14连接；第二主动臂7通过第二球铰10与动平台14连接；第三主动臂8通过第三球铰11与动平台14连接；第一主动臂6由第一伺服电机3驱动伸缩；第二主动臂7由第二伺服电机4驱动伸缩；第三主动臂8由第三伺服电机5驱动伸缩；动平台14后端固接有从动支撑臂13；串联机构包括由伺服电机驱动的a/c轴双摆头，其中双摆头的c轴15与动平台14转动连接，a/c轴双摆头的第一回转轴即双摆头的c轴15，第二回转轴即双摆头的a轴16，双摆头的a轴16与刀具18连接或与安装刀具18的主轴17连接；a/c轴双摆头串接在动平台14末端；第一主动臂6通过第一转轴与第一转动支架19转动连接；第一转动支架19通过第二转轴与固定的轴承座转动连接；第一转动支架19的两侧端分别通过第二转轴旋转地连接于第一固定轴座2，第二主动臂7、第三主动臂8、从动支撑臂13，对应通过第三转轴、第四转轴、第五转轴与第二转动支架12转动连接；即：第二
主动臂7通过第三转轴与第二转动支架12转动连接；第三主动臂8通过第四转轴与第二转动支架12转动连接；从动支撑臂13通过第五转轴与第二转动支架12转动连接；第二转动支架12通过第六转轴与固定的轴承座转动连接；第二转动支架12的两侧端分别通过第六转轴旋转地连接于第二固定轴座1。双摆头的c轴15与双摆头的a轴16通过第七转轴转动连接；第一转轴与第二转轴的轴线垂直；第三转轴、第四转轴、第五转轴的轴线相互平行且与第六转轴的轴线垂直；第二转动支架12的中心位于第五转轴的轴线与第六转轴的轴线交点上。
[0198]
将第一转动支架19、第一主动臂6和第一球铰9记作支链1，用q
3,1
表示第一主动臂6中移动副的位移，该移动副的驱动力用f1表示。将第二转动支架12、第二主动臂7和第二球铰10记作支链2，用q
3,2
表示第二主动臂7中移动副的位移，该移动副的驱动力用f2表示。将第二转动支架12、第三主动臂8和第三球铰11记作支链3，用q
3,3
表示第三主动臂8中移动副的位移，该移动副的驱动力用f3表示。将第二转动支架12、从动支撑臂13、动平台14、a/c轴双摆头记作支链4，用q
3,4
表示从动支撑臂13中移动副的位移，用θ
4,4
表示双摆头的c轴15的转动副绕动平台14轴线转动的角度，该转动副的驱动力矩用τ4表示，用θ
5,4
表示双摆头的a轴16的转动副绕双摆头的c轴15转动的角度，该转动副的驱动力矩用τ5表示。
[0199]
该方法包括以下步骤：
[0200]
步骤1，对混联机器人依次进行位置逆解、速度逆解、加速度逆解，然后利用虚功原理建立混联机器人的动力学模型，该动力学模型的动力学方程如下：
[0201]
f＝mξa+cξv+g+τf；
[0202]
其中：
[0203]
f＝[f
1 f
2 f
3 τ
4 τ5]
t
；
[0204][0205]
式中：
[0206]
f1为第一主动臂6的驱动力；
[0207]
f2为第二主动臂7的驱动力；
[0208]
f3为第三主动臂8的驱动力；
[0209]
τ4为c轴的驱动力矩；
[0210]
τ5为a轴的驱动力矩；
[0211]
f1为第一主动臂6的移动副的摩擦力；
[0212]
f2为第二主动臂7的移动副的摩擦力；
[0213]
f3为第三主动臂8的移动副的摩擦力；
[0214]
τ
f4
为c轴的转动副的摩擦力矩；
[0215]
τ
f5
为a轴的转动副的摩擦力矩；
[0216][0217]
为混联机器人的惯性矩阵；
[0218][0219]
为混联机器人的离心力和哥式力矩阵；
[0220]
为混联机器人的重力向量；
[0221]
为混联机器人末端刚体刀尖点的广义速度；
[0222]
为混联机器人末端刚体刀尖点的广义加速度；
[0223]f1～
f3与对应主动臂的移动副移动速度有关；与对应转动副的转动速度有关。
[0224]
其中，摩擦模型采用的是库伦-粘滞模型，具体公式如下：
[0225][0226]
式中，fc表示库伦摩擦系数；fv表示粘滞摩擦系数；表示关节相对速度。表示摩擦力函数。
[0227]
步骤2，将混联机器人各个构件的惯性力和惯性力矩等效到对应的关键点，对五自由度混联机器人的动力学模型进行线性化，将待辨识参数分离出来，得到如下公式：
[0228]
f＝φp；
[0229]
式中：
[0230]
φ为混联机器人的系统观测矩阵；
[0231]
p为混联机器人的待辨识动力学参数；
[0232]
可使其中包括70个机器人本体的惯性参数以及10个关节摩擦参数。
[0233]
步骤3，设位置、速度、加速度三者初始或结束时数值均为0，运动过程中满足工作空间、位置、速度、加速度四者的参数约束，按照七次多项式规划混联机器人各个关节的辨识轨迹；
[0234]
步骤4，运行混联机器人，运行过程中采集混联机器人的各个关节的位置信息和各个伺服电机的驱动电流，获得ξv、ξa、φ、f；
[0235]
步骤5，采用sobol全局灵敏度分析方法，对线性化的动力学模型进行参数灵敏度分析，将待辨识惯性参数按照灵敏度高低分为三类：高灵敏度参数p
high
、低灵敏度参数p
low
和零灵敏度参数p
zero
；对于零灵敏度参数p
zero
不做处理，对于低灵敏度参数p
low
采用三维软件自动导出数值，而对于高灵敏度参数p
high
采用遗传算法进行辨识。
[0236]
优选地，步骤1可包括如下分步骤：
[0237]
步骤1-1，可设：第一主动臂6轴线与第一转轴轴线的交点为b1；第二主动臂7轴线与第三转轴轴线的交点为b2；第三主动臂8轴线与第四转轴轴线的交点为b3；从动支撑臂13轴线与第五转轴轴线的交点为b4；第一主动臂6与动平台14铰接点为a1；第二主动臂7与动平台14铰接点为a2；第三主动臂8与动平台14铰接点为a3；a2、a3的连线与双摆头的c轴15轴线的交点为a4；基座标系坐标轴包括两两相互垂直的x0轴、y0轴、z0轴；b4为基坐标系原点，b2、b3位于x0轴上；a1相对b1的位移矢量为q
3,1
；a2相对b2的位移矢量为q
3,2
；a3相对b3的位移矢量为q
3,3
；a4相对b4的位移矢量为q
3,4
；双摆头的c轴15绕动平台14轴线转动的转角为θ
4,4
；双摆头的a轴16绕第七转轴转动的转角为θ
5,4
；第七转轴轴线与动平台14轴线的交点为p；混联机器人末端刚体刀尖点为g点，其对应基坐标系原点的位置矢量为rg；刀轴绕基坐标系x0轴的转角为α；绕基坐标系y0轴的转角为β；
[0238]
对混联机器人进行位置逆解，由rg、α、β，得到q
3,1
、q
3,2
、q
3,3
、q
3,4
、θ
4,4
、θ
5,4
；
[0239]
步骤1-2，对混联机器人进行速度逆解和加速度逆解，由ξv得到：第一至第三主动臂8的伸缩速度及对应的加速度，双摆头的c轴和a轴的转动速度及对应的加速度；
[0240]
步骤1-3，将第一主动臂6、第二主动臂7、第三主动臂8及从动支撑臂13拆分为若干构件；第一主动臂6、第二主动臂7、第三主动臂8可以分解为p
1,i
、p
2,i
(i＝1～3)两个构件，i表示第一至第三主动臂序号，其中构件p
1,i
包含伺服电机和套筒，构件p
2,i
包含对应主动臂的伸缩杆和丝杠副；从动支撑臂13可以分解为p
1,4
、p
2,4
两个构件；第一转动支架19、第二转动支架12、c摆头和a摆头可作为独立构件，第一转动支架19记作构件r1；第二转动支架12记作构件r2；其中构件p
1,4
可为从动支撑臂13中内圈部分，构件p
2,4
可包含从动支撑臂13的导向套筒和动平台14。
[0241]
由第一至第三主动臂的伸缩速度及对应的加速度，以及双摆头的c轴和a轴的转动速度及对应的加速度，得到各构件质心的速度和加速度；
[0242]
步骤1-4，分别计算各个构件的惯性力和惯性力矩，再利用虚功原理建立混联机器人的动力学模型。
[0243]
优选地，步骤1-2可包括如下分步骤：
[0244]
步骤1-2-1，通过对下式求导，得到点p的速度v
p
；
[0245]
rg＝r
p
+r
pg
；
[0246]
式中：r
p
为点p的位置矢量；r
pg
为由点p指向点g的位置矢量；
[0247]
步骤1-2-2，以点p为节点，利用如下的闭环约束方程，分别求导得到2，以点p为节点，利用如下的闭环约束方程，分别求导得到
[0248]rp
＝(q
3,4
+e)s
3,4
,r
p
＝bk+q
3,ks3,k-ak+es
3,4
,k＝1～3；
[0249]
式中：e为点a4到点p的长度；s
3,k
，k＝1～3，对应为第一、第二、第三主动臂8轴线的单位矢量，即：s
3,k
为对应q
3,k
的单位位移矢量；bk为基坐标系原点o指向点bk的矢量；ak为点a4指向点ak的矢量；
[0250]
步骤1-2-3，，根据角速度叠加原理得到如下公式，进一步得到和
[0251][0252]
式中：w
5,4
为混联机器人末端刚体刀尖点的角速度；w
3,4
为动平台14的角速度；s
4,4
为双摆头的c轴15旋转轴线的单位矢量；s
5,4
为双摆头的a轴16旋转轴线的单位矢量。
[0253]
优选地，步骤2可包括如下分步骤：
[0254]
步骤2-1，选取各构件的关键节点；如图3所示，可选取点a1～a4、b1～b4为关键点，点o为集成铰链r的关键点，点p为串联部分c摆头的关键点，点c为串联部分a摆头的关键点。
[0255]
步骤2-2，计算求得各个构件所对应关键节点的广义速度，以及将各构件质心处的惯性力和惯性力矩等效到所对应的关键节点，以此将每个构件的惯性参数分离出来；
[0256]
步骤2-3，由f及关键节点的广义速度、惯性力、惯性力矩，利用虚功原理，建立混联机器人的线性化动力学模型：
[0257]
f＝φp
[0258]
式中：φ为混联机器人的系统观测矩阵；p为混联机器人的待辨识动力学参数。可将并联机构第一主动臂6、第二主动臂7、第三主动臂8的惯性参数看作相同。可使其中包括70个机器人本体的惯性参数以及10个关节摩擦参数。
[0259]
优选地，步骤3可包括如下分步骤：
[0260]
步骤3-1，设置辨识轨迹的约束条件如下式所示：
[0261][0262]
具体设置可如下所示：
[0263][0264][0265]
具体设置可如下所示：
[0266][0267]
式中：
[0268]
t为机器人运动时间；
[0269]q3,k
(t)为第k个主动臂移动副在t时刻的位移；
[0270]q3,k_0
为第k个主动臂移动副初始位移；
[0271]
θ
u,4
(t)为第u个主动转动副在t时刻的转角；u＝4时，第u个主动转动副为双摆头的c轴15转动副；
[0272]
θ
u,4_0
为第u个主动转动副初始转角；u＝5时，第u个主动转动副为双摆头的a轴16转动副；
[0273]
步骤3-2，按照七次多项式模型规划混联机器人各个关节的辨识轨迹；其中，七次多项式模型如下式所示：
[0274]
x(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5+a6t6+a7t7；
[0275]
x(t)为关节在t时刻的位移或转角；
[0276]
a0～a7为对应七次多项式的0次至7次的项系数；
[0277]
根据步骤3-1中的约束条件，计算出各个关节辨识轨迹的多项式系数，得到各个关节辨识轨迹。得到的辨识轨迹如图4、图5至所示。
[0278]
优选地，第一至第三主动臂8均可包括套筒、伸缩杆及由伺服电机驱动的丝杠副；伸缩杆由丝杠副带动相对套筒伸缩；第一主动臂6的套筒与第一转轴固接；第二主动臂7的套筒与第三转轴固接；第三主动臂8的套筒与第四转轴固接；步骤4可包括如下分步骤：
[0279]
步骤4-1，将获取的各个关节辨识轨迹编写为g代码，然后输入到混联机器人的控制器中，驱动混联机器人的各个关节运动；
[0280]
步骤4-2，在混联机器人按照辨识轨迹运行过程中，同步采集混联机器人伸缩杆与动平台14铰接点的位置数据和对应的伺服电机电流数据，生成各个关节的位置曲线和电流
曲线；
[0281]
步骤4-3，利用低通滤波器对各个关节的位置曲线进行滤波，然后依次微分得到各个关节的速度和加速度；
[0282]
步骤4-4，利用混联机器人的正运动学，根据各个关节位置、速度和加速度，计算出末端刚体的位姿信息和末端刀尖点的广义速度ξv和广义加速度ξa；
[0283]
步骤4-5，根据采样得到的驱动双摆头的c轴15及双摆头的a轴16转动的伺服电机电流，按照下式得到对应的驱动c轴及a轴转动的驱动力矩τd：
[0284]
τd＝rekaim；
[0285]
式中：ka为伺服电机的转矩常数；re为与伺服电机连接的减速机的减速比；im为伺服电机电流；
[0286]
然后再根据采样得到的驱动第一至第三主动臂8的伺服电机电流，按照下式得到对应的驱动第一至第三主动臂的驱动力fd：
[0287][0288]
式中：b为丝杠的导程；
[0289]
利用低通滤波器将计算得到的驱动力及驱动力矩进行滤波处理。
[0290]
步骤4-6，将末端刚体的位姿信息以及末端刀尖点的广义速度、广义加速度代入到观测矩阵φ的表达式中，求解得到观测矩阵φ的结果。
[0291]
优选地，步骤5可包括如下分步骤：
[0292]
步骤5-1，将待辨识的惯性参数根据其各自的取值范围映射到单位区间上，然后将线性化出来的动力学模型分解成如下关于待辨识惯性参数的函数形式：
[0293][0294]
p＝[x
1 x
2 ... xn]
t
；
[0295]
式中：
[0296]
f0为常数；
[0297]
n为参数数量；
[0298]
xi为第i个待辨识参数；
[0299]
xj为第j个待辨识参数；
[0300]fi
(xi)为仅包含待辨识参数xi的函数子项；
[0301]fi,j
(xi,xj)为包含待辨识参数xi和xj的函数子项；
[0302]f1,2,,n
(x1,x2,
…
,xn)为包含待辨识参数x1,x2,
…
,xn的函数子项；
[0303]
动力学模型分解出来的所有子项之间都正交，则有：
[0304][0305]in
＝(p|0≤xi≤1；i＝1,2,
…
,n)；
[0306]
式中：
[0307]in
为待辨识的参数映射出来的n维单元体；
[0308]
s为所要正交的函数子项中包含的待辨识参数个数；
[0309]
e1，e2…
，es对应为待辨识参数集合e中的第1,2,
…
,s个参数；
[0310]
h1，h2…
，hs对应为待辨识参数集合h中的第1,2,
…
,s个参数；
[0311]
为包含待辨识参数集合e中的第1,2,
…
,s个参数的函数子项；
[0312]
为包含待辨识参数集合h中的第1,2,
…
,s个参数的函数子项；
[0313]
步骤5-2，对线性化的动力学模型分解之后的表达式取多重积分，得到如下各子项：
[0314][0315]
式中：
[0316]
p-i
为除xi之外的其余参数；
[0317]
p-(ij)
为除xi和xj之外的其余参数；
[0318]fj
(xj)为包含参数xj的函数子项；
[0319]
以此类推，求得其他的子项；
[0320]
步骤5-3，根据步骤5-3求解的各子项，求得线性化动力学模型函数的总方差和各项偏方差：
[0321][0322][0323]
其中，各项方差和总方差之间有如下特性：
[0324][0325]
xs为第s个参数；
[0326]
d为线性化动力学模型函数的总方差；
[0327]di
为参数xi的偏方差；
[0328]dij
为参数xi和xj的偏方差；
[0329]d1,2,
…
,s
为参数x1,x2,
…
,xs的偏方差；
[0330]d1,2,...,n
为参数x1,x2,
…
,xn的偏方差；
[0331]
步骤5-4，根据第三步求解的总方差和各项方差，按照下式计算出所有参数的各阶灵敏度
[0332][0333]
通过下式，计算得到参数xi的总灵敏度系数：
[0334]
[0335][0336]
式中：
[0337]sti
为参数xi的总灵敏度系数；
[0338]
d-i
为除了参数xi外，其他参数的偏方差之和；
[0339]
步骤5-5，对参数变量进行两次独立的抽样，进而得到两个矩阵an×n和bn×n，其中n为抽样的个数，n为参数数量；然后利用蒙特卡洛法，按照如下公式计算出和
[0340][0341]
式中：
[0342]
为样本a中第v列被b中第v列元素替换所得到的新的样本矩阵；
[0343]
为样本b中第v列被a中第v列元素替换所得到的新的样本矩阵；
[0344]
f(a)w为样本a对应的动力学模型函数值；
[0345]
为样本对应的动力学模型函数值；
[0346]
为样本对应的动力学模型函数值；
[0347]
为用蒙特卡洛法对f0的估计值；
[0348]
为用蒙特卡洛法对动力学模型函数总方差的估计值；
[0349]
为用蒙特卡洛法对参数xv偏方差的估计值；
[0350]
为用蒙特卡洛法对除了参数xv外，其他参数的偏方差之和的估计值；
[0351]
步骤5-6，将计算得到的待辨识惯性参数的灵敏度值分为三类；选取灵敏度值区间中的前30％个参数定义为高灵敏度参数p
high
，选取灵敏度值区间中的后20％个参数定义为为零灵敏度参数p
zero
，剩下的参数定义为低灵敏度参数p
low
；
[0352]
可将70个待辨识惯性参数的灵敏度值，将其分为三类。选取灵敏度值较大的前20个参数为高灵敏度参数p
high
，灵敏度值为0的14个参数为零灵敏度参数p
zero
，剩下的36个参数为低灵敏度参数p
low
。
[0353]
步骤5-7，设置待辨识惯性参数p
high
和摩擦参数为遗传算法的种群，目标函数如下式所示：
[0354][0355]
式中：q为采样数；
[0356]
q为采样序号；
[0357]
τ
qa
为第q个实际采样力矩；
[0358]
τ
qt
为对应第q个实际采样力矩的理论计算力矩；
[0359]
步骤5-8，根据适应度值，利用交叉、遗传和变异特性不断迭代更新种群，得到最优适应度下的种群，即为利用遗传算法辨识出来的参数。
[0360]
优选地，还可包括步骤6，根据已辨识的参数，通过实验对动力学模型进行验证及评估，并修正混联机器人动力学模型。
[0361]
优选地，步骤6可包括如下分步骤：
[0362]
步骤6-1，利用辨识出来的动力学参数计算出各关节的驱动力和驱动力矩，绘制理论预测曲线，并与采集的驱动力和驱动力矩数据进行对比；
[0363]
步骤6-2，使用如下均方根误差对得到的动力学模型进行评估：
[0364][0365]
式中：
[0366]
q为采样数；
[0367]
q为采样序号；
[0368]
τ
qa
为第q个实际采样力矩；
[0369]
φ为观测矩阵；
[0370]
为辨识出的动力学参数。
[0371]
优选地，可使其中包括70个机器人本体的惯性参数以及10个关节摩擦参数。可将并联机构第一主动臂6、第二主动臂7、第三主动臂8的惯性参数看作相同。据此混联机器人分离出来的惯性参数共有70个，然后还有关节摩擦参数10个。
[0372]
以上所述的实施例仅用于说明本发明的技术思想及特点，其目的在于使本领域内的技术人员能够理解本发明的内容并据以实施，不能仅以本实施例来限定本发明的专利范围，即凡本发明所揭示的精神所作的同等变化或修饰，仍落在本发明的专利范围内。

技术特征：
1.一种基于灵敏度分析的混联机器人动力学参数辨识方法，该混联机器人为五自由度混联机器人，其包括三自由度并联机构和与之串接的具有两个转动自由度的串联机构，三自由度并联机构包括动平台、第一主动臂、第二主动臂、第三主动臂、从动支撑臂、第一转动支架及第二转动支架；其中：动平台周侧铰接有三个由伺服电机驱动伸缩的主动臂，依次为第一主动臂、第二主动臂、第三主动臂；动平台后端固接有从动支撑臂；串联机构包括由伺服电机驱动的a/c轴双摆头，其中双摆头的c轴与动平台转动连接，双摆头的a轴与刀具连接；第一主动臂通过第一转轴与第一转动支架转动连接；第一转动支架通过第二转轴与固定的轴承座转动连接；第二主动臂、第三主动臂、从动支撑臂，对应通过第三转轴、第四转轴、第五转轴与第二转动支架转动连接；第二转动支架通过第六转轴与固定的轴承座转动连接；双摆头的c轴与a轴通过第七转轴转动连接；第一转轴与第二转轴的轴线垂直；第三转轴、第四转轴、第五转轴的轴线相互平行且与第六转轴的轴线垂直；第二转动支架的中心位于第五转轴的轴线与第六转轴的轴线交点上；其特征在于，包括以下步骤：步骤1，对混联机器人依次进行位置逆解、速度逆解、加速度逆解，然后利用虚功原理建立混联机器人的动力学模型，该动力学模型的动力学方程如下：f＝mξ
a
+cξ
v
+g+τ
f
；其中：f＝[f
1 f
2 f
3 τ
4 τ5]
t
；式中：f1为第一主动臂的驱动力；f2为第二主动臂的驱动力；f3为第三主动臂的驱动力；τ4为c轴的驱动力矩；τ5为a轴的驱动力矩；f1为第一主动臂的移动副的摩擦力；f2为第二主动臂的移动副的摩擦力；f3为第三主动臂的移动副的摩擦力；τ
f4
为c轴的转动副的摩擦力矩；τ
f5
为a轴的转动副的摩擦力矩；为混联机器人的惯性矩阵；为混联机器人的离心力和哥式力矩阵；为混联机器人的重力向量；为混联机器人末端刚体刀尖点的广义速度；为混联机器人末端刚体刀尖点的广义加速度；步骤2，将混联机器人各个构件的惯性力和惯性力矩等效到对应的关键点，对五自由度混联机器人的动力学模型进行线性化，将待辨识参数分离出来，得到如下公式：f＝φp；
式中：φ为混联机器人的系统观测矩阵；p为混联机器人的待辨识动力学参数；步骤3，设位置、速度、加速度三者初始或结束时数值均为0，运动过程中满足工作空间、位置、速度、加速度四者的参数约束，按照七次多项式规划混联机器人各个关节的辨识轨迹；步骤4，运行混联机器人，运行过程中采集混联机器人的各个关节的位置信息和各个伺服电机的驱动电流，获得ξ
v
、ξ
a
、φ、f；步骤5，采用sobol全局灵敏度分析方法，对线性化的动力学模型进行参数灵敏度分析，将待辨识惯性参数按照灵敏度高低分为三类：高灵敏度参数p
high
、低灵敏度参数p
low
和零灵敏度参数p
zero
；对于零灵敏度参数p
zero
不做处理，对于低灵敏度参数p
low
采用三维软件自动导出数值，而对于高灵敏度参数p
high
采用遗传算法进行辨识。2.根据权利要求1所述的基于灵敏度分析的混联机器人动力学参数辨识方法，其特征在于，步骤1包括如下分步骤：步骤1-1，设：第一主动臂轴线与第一转轴轴线的交点为b1；第二主动臂轴线与第三转轴轴线的交点为b2；第三主动臂轴线与第四转轴轴线的交点为b3；从动支撑臂轴线与第五转轴轴线的交点为b4；第一主动臂与动平台铰接点为a1；第二主动臂与动平台铰接点为a2；第三主动臂与动平台铰接点为a3；a2、a3的连线与双摆头的c轴轴线的交点为a4；基座标系坐标轴包括两两相互垂直的x0轴、y0轴、z0轴；b4为基坐标系原点，b2、b3位于x0轴上；a1相对b1的位移矢量为q
3,1
；a2相对b2的位移矢量为q
3,2
；a3相对b3的位移矢量为q
3,3
；a4相对b4的位移矢量为q
3,4
；双摆头的c轴绕动平台轴线转动的转角为θ
4,4
；双摆头的a轴绕第七转轴转动的转角为θ
5,4
；第七转轴轴线与动平台轴线的交点为p；混联机器人末端刚体刀尖点为g点，其对应基坐标系原点的位置矢量为r
g
；刀轴绕基坐标系x0轴的转角为α；绕基坐标系y0轴的转角为β；对混联机器人进行位置逆解，由r
g
、α、β，得到q
3,1
、q
3,2
、q
3,3
、q
3,4
、θ
4,4
、θ
5,4
；步骤1-2，对混联机器人进行速度逆解和加速度逆解，由ξ
v
得到：第一至第三主动臂的伸缩速度及对应的加速度，双摆头的c轴和a轴的转动速度及对应的加速度；步骤1-3，将第一至第三主动臂及从动支撑臂拆分为若干构件；由第一至第三主动臂的伸缩速度及对应的加速度，以及双摆头的c轴和a轴的转动速度及对应的加速度，得到各构件质心的速度和加速度；步骤1-4，分别计算各个构件的惯性力和惯性力矩，再利用虚功原理建立混联机器人的动力学模型。3.根据权利要求2所述的基于灵敏度分析的混联机器人动力学参数辨识方法，其特征在于，步骤1-2包括如下分步骤：步骤1-2-1，通过对下式求导，得到点p的速度v
p
；r
g
＝r
p
+r
pg
；式中：r
p
为点p的位置矢量；r
pg
为由点p指向点g的位置矢量；步骤1-2-2，以点p为节点，利用如下的闭环约束方程，分别求导得到分别求导得到r
p
＝(q
3,4
+e)s
3,4
,r
p
＝b
k
+q
3,k
s
3,k-a
k
+es
3,4
,k＝13；式中：e为点a4到点p的长度；s
3,k
，k＝1～3，对应为第一、第二、第三主动臂轴线的单位矢
量；b
k
为基坐标系原点o指向点b
k
的矢量；a
k
为点a4指向点a
k
的矢量；步骤1-2-3，，根据角速度叠加原理得到如下公式，进一步得到和和式中：w
5,4
为混联机器人末端刚体刀尖点的角速度；w
3,4
为动平台的角速度；s
4,4
为双摆头的c轴旋转轴线的单位矢量；s
5,4
为双摆头的a轴旋转轴线的单位矢量。4.根据权利要求2所述的基于灵敏度分析的混联机器人动力学参数辨识方法，其特征在于，步骤2包括如下分步骤：步骤2-1，选取各构件的关键节点；步骤2-2，计算求得各个构件所对应关键节点的广义速度，以及将各构件质心处的惯性力和惯性力矩等效到所对应的关键节点，以此将每个构件的惯性参数分离出来；步骤2-3，由f及关键节点的广义速度、惯性力、惯性力矩，利用虚功原理，建立混联机器人的线性化动力学模型。5.根据权利要求2所述的基于灵敏度分析的混联机器人动力学参数辨识方法，其特征在于，步骤3包括如下分步骤：步骤3-1，设置辨识轨迹的约束条件如下式所示：设置辨识轨迹的约束条件如下式所示：式中：t为机器人运动时间；q
3,k
(t)为第k个主动臂移动副在t时刻的位移；q
3,k_0
为第k个主动臂移动副初始位移；θ
u,4
(t)为第u个主动转动副在t时刻的转角；u＝4时，第u个主动转动副为双摆头的c轴转动副；θ
u,4_0
为第u个主动转动副初始转角；u＝5时，第u个主动转动副为双摆头的a轴转动副；步骤3-2，按照七次多项式模型规划混联机器人各个关节的辨识轨迹；其中，七次多项式模型如下式所示：x(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5+a6t6+a7t7；x(t)为关节在t时刻的位移或转角；a0～a7为对应七次多项式的0次至7次的项系数；根据步骤3-1中的约束条件，计算出各个关节辨识轨迹的多项式系数，得到各个关节辨识轨迹。6.根据权利要求1所述的基于灵敏度分析的混联机器人动力学参数辨识方法，其特征在于，第一至第三主动臂均包括套筒、伸缩杆及由伺服电机驱动的丝杠副；伸缩杆由丝杠副带动相对套筒伸缩；第一主动臂的套筒与第一转轴固接；第二主动臂的套筒与第三转轴固接；第三主动臂的套筒与第四转轴固接；步骤4包括如下分步骤：
步骤4-1，将获取的各个关节辨识轨迹编写为g代码，然后输入到混联机器人的控制器中，驱动混联机器人的各个关节运动；步骤4-2，在混联机器人按照辨识轨迹运行过程中，同步采集混联机器人伸缩杆与动平台铰接点的位置数据和对应的伺服电机电流数据，生成各个关节的位置曲线和电流曲线；步骤4-3，利用低通滤波器对各个关节的位置曲线进行滤波，然后依次微分得到各个关节的速度和加速度；步骤4-4，利用混联机器人的正运动学，根据各个关节位置、速度和加速度，计算出末端刚体的位姿信息和末端刀尖点的广义速度ξ
v
和广义加速度ξ
a
；步骤4-5，根据采样得到的驱动双摆头的c轴及a轴转动的伺服电机电流，按照下式得到对应的驱动c轴及a轴转动的驱动力矩τ
d
：τ
d
＝r
e
k
a
i
m
；式中：k
a
为伺服电机的转矩常数；r
e
为与伺服电机连接的减速机的减速比；i
m
为伺服电机电流；然后再根据采样得到的驱动第一至第三主动臂的伺服电机电流，按照下式得到对应的驱动第一至第三主动臂的驱动力f
d
：式中：b为丝杠的导程；利用低通滤波器将计算得到的驱动力及驱动力矩进行滤波处理；步骤4-6，将末端刚体的位姿信息以及末端刀尖点的广义速度、广义加速度代入到观测矩阵φ的表达式中，求解得到观测矩阵φ的结果。7.根据权利要求1所述的基于灵敏度分析的混联机器人动力学参数辨识方法，其特征在于，步骤5包括如下分步骤：步骤5-1，将待辨识的惯性参数根据其各自的取值范围映射到单位区间上，然后将线性化出来的动力学模型分解成如下关于待辨识惯性参数的函数形式：p＝[x
1 x2…
x
n
]
t
；式中：f0为常数；n为参数数量；x
i
为第i个待辨识参数；x
j
为第j个待辨识参数；f
i
(x
i
)为仅包含待辨识参数x
i
的函数子项；f
i,j
(x
i
,x
j
)为包含待辨识参数x
i
和x
j
的函数子项；f
1,2,
…
,n
(x1,x2,
…
,x
n
)为包含待辨识参数x1,x2,
…
,x
n
的函数子项；动力学模型分解出来的所有子项之间都正交，则有：
i
n
＝(p|0≤x
i
≤1；i＝1,2,
…
,n)；式中：i
n
为待辨识的参数映射出来的n维单元体；s为所要正交的函数子项中包含的待辨识参数个数；e1，e2…
，e
s
对应为待辨识参数集合e中的第1,2,
…
,s个参数；h1，h2…
，h
s
对应为待辨识参数集合h中的第1,2,
…
,s个参数；为包含待辨识参数集合e中的第1,2,
…
,s个参数的函数子项；为包含待辨识参数集合h中的第1,2,
…
,s个参数的函数子项；步骤5-2，对线性化的动力学模型分解之后的表达式取多重积分，得到如下各子项：式中：p-i
为除x
i
之外的其余参数；p-(ij)
为除x
i
和x
j
之外的其余参数；f
j
(x
j
)为包含参数x
j
的函数子项；以此类推，求得其他的子项；步骤5-3，根据步骤5-3求解的各子项，求得线性化动力学模型函数的总方差和各项偏方差：方差：其中，各项方差和总方差之间有如下特性：x
s
为第s个参数；d为线性化动力学模型函数的总方差；d
i
为参数x
i
的偏方差；d
ij
为参数x
i
和x
j
的偏方差；d
1,2,
…
,s
为参数x1,x2,
…
,x
s
的偏方差；d
1,2,
…
,n
为参数x1,x2,
…
,x
n
的偏方差；步骤5-4，根据第三步求解的总方差和各项方差，按照下式计算出所有参数的各阶灵敏度度通过下式，计算得到参数x
i
的总灵敏度系数：
式中：s
ti
为参数x
i
的总灵敏度系数；d-i
为除了参数x
i
外，其他参数的偏方差之和；步骤5-5，对参数变量进行两次独立的抽样，进而得到两个矩阵a
n
×
n
和b
n
×
n
，其中n为抽样的个数，n为参数数量；然后利用蒙特卡洛法，按照如下公式计算出和和式中：为样本a中第v列被b中第v列元素替换所得到的新的样本矩阵；为样本b中第v列被a中第v列元素替换所得到的新的样本矩阵；f(a)
w
为样本a对应的动力学模型函数值；为样本对应的动力学模型函数值；为样本对应的动力学模型函数值；为用蒙特卡洛法对f0的估计值；为用蒙特卡洛法对动力学模型函数总方差的估计值；为用蒙特卡洛法对参数x
v
偏方差的估计值；为用蒙特卡洛法对除了参数x
v
外，其他参数的偏方差之和的估计值；步骤5-6，将计算得到的待辨识惯性参数的灵敏度值分为三类；选取灵敏度值区间中的前30％个参数定义为高灵敏度参数p
high
，选取灵敏度值区间中的后20％个参数定义为为零灵敏度参数p
zero
，剩下的参数定义为低灵敏度参数p
low
；步骤5-7，设置待辨识惯性参数p
high
和摩擦参数为遗传算法的种群，目标函数如下式所示：式中：q为采样数；q为采样序号；τ
qa
为第q个实际采样力矩；τ
qt
为对应第q个实际采样力矩的理论计算力矩；步骤5-8，根据适应度值，利用交叉、遗传和变异特性不断迭代更新种群，得到最优适应
度下的种群，即为利用遗传算法辨识出来的参数。8.根据权利要求1所述的基于灵敏度分析的混联机器人动力学参数辨识方法，其特征在于，还包括步骤6，根据已辨识的参数，通过实验对动力学模型进行验证及评估，并修正混联机器人动力学模型。9.根据权利要求8所述的基于灵敏度分析的混联机器人动力学参数辨识方法，其特征在于，步骤6包括如下分步骤：步骤6-1，利用辨识出来的动力学参数计算出各关节的驱动力和驱动力矩，绘制理论预测曲线，并与采集的驱动力和驱动力矩数据进行对比；步骤6-2，使用如下均方根误差对得到的动力学模型进行评估：式中：q为采样数；q为采样序号；τ
qa
为第q个实际采样力矩；φ为观测矩阵；为辨识出的动力学参数。10.根据权利要求1所述的基于灵敏度分析的混联机器人动力学参数辨识方法，其特征在于，使其中包括70个机器人本体的惯性参数以及10个关节摩擦参数。

技术总结
本发明公开了一种基于灵敏度分析的机器人动力学参数辨识方法，对机器人依次进行位置逆解、速度逆解、加速度逆解，用虚功原理建立机器人动力学模型，将机器人各个构件的惯性力和惯性力矩等效到对应的关键点，对五自由度机器人的动力学模型进行线性化，将待辨识参数分离出来，设位置、速度、加速度三者初始或结束时数值均为0，运动过程中满足工作空间、位置、速度、加速度四者的参数约束，按照七次多项式规划机器人各个关节的辨识轨迹；运行机器人过程中采集各个关节的位置信息和各个伺服电机的驱动电流，采用Sobol全局灵敏度分析方法，对线性化的动力学模型进行参数灵敏度分析，并辨识参数。本发明减少了待辨识参数，提高了辨识效率和精度。和精度。和精度。


技术研发人员：肖聚亮 罗在华 王铭礼 刘思江 赵炜
受保护的技术使用者：天津大学
技术研发日：2023.05.08
技术公布日：2023/7/27