[发明专利]一种能够消除回程误差的多自由度机械臂回转方法

权利要求书

1.一种能够消除回程误差的多自由度机械臂回转方法，采用的回转驱动系统，包括回转本体、回转驱动组件、驱动油路和信号反馈模块；所述的回转本体包括固定底座和回转平台；所述的回转平台与固定底座构成转动副；回转平台的底部与外齿圈固定；所述的回转驱动组件包括电液伺服马达、行星减速器和驱动齿轮；所述的驱动齿轮支承在固定底座上，且与外齿圈啮合；所述的行星减速器固定在固定底座上；行星减速器的输出轴与驱动齿轮固定；电液伺服马达固定在行星减速器上；电液伺服马达的输出轴与行星减速器的输入口固定；所述的回转驱动组件共有2m个，1≤m≤4；驱动油路包括电液比例阀、第一安全阀和第二安全阀；2m个电液比例阀的第一工作油口与2m个第一安全阀的第一油口分别连通；2m个电液比例阀的第二工作油口与2m个第二安全阀的第一油口分别连通；2m个第一安全阀的第二油口与2m个回转驱动组件内电液伺服马达的第一油口分别连通；2m个第二安全阀的第二油口与2m个回转驱动组件内电液伺服马达的第二油口分别连通；其特征在于：2m个回转驱动组件沿外齿圈的周向均布；所述的驱动油路还包括第一过滤器、第二过滤器、蓄能器、柱塞式液压泵、单向阀、油箱和电液溢流阀；所述柱塞式液压泵的入油口与油箱通过第一过滤器连通，出油口与单向阀的输入口连通；单向阀的输出口与电液溢流阀的入油口及第二过滤器的输入口连通；电液溢流阀的出油口与油箱连通；第二过滤器的输出口与蓄能器的蓄能油口及手动球阀的一个通油口连通；手动球阀的另一个通油口与2m个电液比例阀的进油口连通；2m个电液比例阀的回油口均与油箱连通；

所述的信号反馈模块包括2m+1个旋转编码器和4m个油路压力变送器；旋转编码器均与固定底座固定；2m+1个旋转编码器的输入轴与回转平台、2m个回转驱动组件内的驱动齿轮分别固定；4m个油路压力变送器的检测口与2m个回转驱动组件内电液伺服马达的第一油口、第二油口分别连通；

该能够消除回程误差的多自由度机械臂回转方法具体如下：

步骤一：选取其中m个回转驱动组件作为第一驱动组，另m个回转驱动组件作为第二驱动组；第一驱动组的m个回转驱动组件两两之间互不相邻；以第一驱动组作为行进测试组，第二驱动组作为复位测试组；使用者设定回转平台的角位移θ₀；

步骤二、计算行进测试组内m个驱动齿轮的目标角位移θ₁、θ₂、…、θ_m，复位测试组内m个驱动齿轮的目标角位移θ_m+1、θ_m+2、…、θ_2m；

其中，R为外齿圈的分度圆半径，r为驱动齿轮的分度圆半径；Δθ为驱动齿轮与外齿圈的齿隙在驱动齿轮上对应的角度；

步骤三、将0赋值给t；计算需要输入到2m个驱动齿轮对应的电液比例阀的电压值u_i(t)如式(1)所示，

其中，U_max、q_max分别为电液比例阀的最大控制电压、额定流量；D_m为电液伺服马达的额定排量，Δt是运动算法模块设定的控制周期；Δt＝0.1s；

分别向2m个电液比例阀输入电压值u_i(t)，i＝1,2,…,2m；将t增大Δt；Δt时间后进入步骤四；

步骤四、与驱动齿轮连接的2m个旋转编码器分别检测2m个驱动齿轮的实际角位移信号；

之后计算2m个驱动齿轮的角位移误差e_i(t)＝θ′_i-θ_i，i＝1,2,…,2m；θ′₀～θ′_2m分别为2m个驱动齿轮当前的实际角位移值；

步骤五、计算累加后输出控制值u_i(t)如式(2)所示，

式(2)中，0.6≤K_Pi≤2.0，0.05≤K_Ii≤0.3，0.05≤K_Di≤0.25，i＝1,2,…,2m；e_i(0)＝0；

步骤六、若u_i(t)中有一个或多个的正负符号发生变化，则直接进入步骤七；否则，将t增大Δt，Δt时间后重复执行步骤四和五；

步骤七、若此时第一驱动组为行进测试组，则将此时行进测试组内m个驱动齿轮的角位移误差e₁(t)、e₂(t)、…、e_m(t)，作为第一最终误差组内的m个元素e₁′、e₂′、…、e_m′，之后将第二驱动组作为行进测试组，第一驱动组作为复位测试组；2m个回转驱动组件内的驱动齿轮均复位后，重复执行步骤二至六；

若此时第二驱动组作为行进测试组，则将此时行进测试组内m个驱动齿轮的角位移误差e₁(t)、e₂(t)、…、e_m(t)，作为第二最终误差组内的m个元素e_m+1″、e_m+2″、…、e_2m″；2m个回转驱动组件内的驱动齿轮均复位后，进入步骤八；

步骤八、若则将第一驱动组作为行进驱动组，将第二驱动组作为复位驱动组；否则，将第二驱动组作为行进驱动组，将第一驱动组作为复位驱动组；

步骤九、计算行进驱动组内m个驱动齿轮的目标角位移θ₁、θ₂、…、θ_m，复位驱动组内m个驱动齿轮的目标角位移θ_m+1、θ_m+2、…、θ_2m；

步骤十、将0赋值给t，1赋值给k；计算需要输入到2m个电液比例阀的电压值u_i(t,k)如式(3)所示；

分别向2m个电液比例阀输入电压值u_i(t,k)，i＝1,2,…,2m；将t增大Δt；此时，Δt时间后进入步骤十一；

步骤十一、与驱动齿轮连接的2m个旋转编码器分别检测2m个驱动齿轮的实际角位移信号；

之后计算2m个驱动齿轮的角位移误差e_i(t,k)＝θ′_i-θ_i，i＝1,2,…,2m；θ′₀～θ′_2m分别为2m个驱动齿轮当前的实际角位移值；

步骤十二、4m个油路压力变送器分别检测油压值；以与行进驱动组对应的2m个油路压力变送器中检测到油压值最大的那个油路压力变送器为行进特征变送器；以与复位驱动组对应的2m个油路压力变送器中检测到油压值最大的那个油路压力变送器为复位特征变送器；与行进特征变送器对应的回转驱动组件为行进特征回转驱动组件；与复位特征变送器对应的回转驱动组件为复位特征回转驱动组件；

计算行进驱动组内m个驱动齿轮的同步误差e_synj(i,t,k)＝θ_i′-θ_a′，i＝1,2,…,m；θ_a′为行进特征回转驱动组件内驱动齿轮当前的实际角位移值；计算复位驱动组内m个驱动齿轮的同步误差e_synj(i,t,k)＝θ_i′-θ_b′，i＝m+1,m+2,…,2m；θ_b′为复位特征回转驱动组件内驱动齿轮当前的实际角位移值；

步骤十三、若k＝1，则进入步骤十四；若k＞1，则进入步骤十五；

步骤十四、计算累加后输出控制值u_i(t,k)如式(4)所示，

式(4)中，e_i(0,k)＝0；

直接进入步骤十六；

步骤十五、计算综合误差如式(5)所示，

式(5)中，0.02≤I_d≤0.1；0.05≤I_p≤0.2；e_synj(i,0,k-1)＝0；

计算累加后输出控制值u_i(t,k)如式(6)所示，

式(6)中，

进入步骤十六；

步骤十六、分别向2m个电液比例阀输入电压值u_i(t,k)，i＝1,2,…,2m；

若未接收到复位指令或转动至目标位置指令，则将t增大Δt，Δt时间后重复执行步骤十一至十五；

若接收到复位指令，则令行进驱动组内m个驱动齿轮的目标角位移θ₁＝θ₂＝…＝θ_m＝-Δθ，令复位驱动组内m个驱动齿轮的目标角位移θ_m+1＝θ_m+2＝…＝θ_2m＝0，之后进入步骤十七；

若接收到转动至目标位置指令，则令四个驱动齿轮的目标角位移并进入步骤十七；

步骤十七、将0赋值给Δt，将k增大1；与驱动齿轮连接的2m个旋转编码器分别检测2m个驱动齿轮的实际角位移信号；

之后计算2m个驱动齿轮的角位移误差e_i(t,k)＝θ′_i-θ_i，i＝1,2,…,2m；θ′₀～θ′_2m分别为2m个驱动齿轮当前的实际角位移值；

计算输入到2m个电液比例阀的电压值

分别2m个电液比例阀输入电压值u_i(t,k)，i＝1,2,…,2m；将t增大Δt；Δt时间后重复执行步骤十一至十六。