本发明属于回转驱动技术领域，具体涉及一种能够消除回程误差的多自由度机械臂回转方法。
背景技术：
多自由度机械臂是能够模仿人体肢体部分功能并允许对其进行自动控制，使其按照预定要求输送工件或操持工具进行生产操作的自动化产设备。目前，多自由度机械臂系统广泛应用于钢铁、海洋、石油、化工、物流搬运等生产自动化行业，大大减轻了工人劳动强度和劳动条件，提高了生产效率，稳定了产品质量。现有的多自由度机械臂回转机构多为多电机驱动形式，采用多个交流电动机经减速器驱动回转装置，虽具有传动效率高、运行噪声小、可维护性好等特点，但是大功率电机体积较大、安装配合精度要求较高、制动性能较差，当驱动负载发生较大的波动时，执行器容易因刚性过载而发生故障。
液压马达回转驱动形式下的驱动功率密度，比电机驱动形式大，在满足驱动功率的前提下，可以优化执行器体积，合理利用回转系统的内部空间，降低回转系统的制造成本，同时还能在很大程度上消除和抑制负载扰动因素的影响。
目前，多液压马达同步驱动系统有cn94278675，由东北大学设计的一种四液压马达驱动自同步振动锤及其结构参数确定方法，采用液压马达驱动、自同步传动的方式，对振动锤的结构进行创新设计，通过对振动锤结构参数的控制，达到了四液压马达驱动自同步的目的。但其集成程度较差，导致整个设备尺寸较大。
cn203362667u，由中联重科有限公司设计的液压马达同步驱动系统和工程机械，主要应用于混凝土搅拌机，不足之处就在于完全依靠油路调整双马达的同步性能，响应较慢，不适用于多自由度机械臂系统。且上述专利都不涉及回转精度的反馈及补偿，无法实时精确调整回转位移，对其应用于多自由度机械臂系统造成了极大的限制。因此，需发明一种适用于多自由度机械臂的电液伺服回转驱动方法。
技术实现要素：
本发明的目的在于提供一种能够消除回程误差的多自由度机械臂回转方法。
本发明采用的回转驱动系统，包括回转本体、回转驱动组件、驱动油路和信号反馈模块。所述的回转本体包括固定底座和回转平台。所述的回转平台与固定底座构成转动副。回转平台的底部与外齿圈固定。
所述的回转驱动组件包括电液伺服马达、行星减速器和驱动齿轮。所述的驱动齿轮支承在固定底座上，且与外齿圈啮合。所述的行星减速器固定在固定底座上。行星减速器的输出轴与驱动齿轮固定。电液伺服马达固定在行星减速器上。电液伺服马达的输出轴与行星减速器的输入口固定。所述的回转驱动组件共有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个回转驱动组件两两之间互不相邻。以第一驱动组作为行进测试组，第二驱动组作为复位测试组。使用者设定回转平台的角位移θ0。
步骤二、计算行进测试组内m个驱动齿轮的目标角位移θ1、θ2、…、θm，复位测试组内m个驱动齿轮的目标角位移θm+1、θm+2、…、θ2m。
其中，r为外齿圈的分度圆半径，r为驱动齿轮的分度圆半径；δθ为驱动齿轮与外齿圈的齿隙在驱动齿轮上对应的角度。
步骤三、将0赋值给t。计算需要输入到2m个驱动齿轮对应的电液比例阀的电压值ui(t)如式(1)所示，
其中，umax、qmax分别为电液比例阀的最大控制电压、额定流量；dm为电液伺服马达的额定排量，δt是运动算法模块设定的控制周期。δt＝0.1s。
分别向2m个电液比例阀输入电压值ui(t)，i＝1,2,…,2m。将t增大δt。δt时间后进入步骤四。
步骤四、与驱动齿轮连接的2m个旋转编码器分别检测2m个驱动齿轮的实际角位移信号。
之后计算2m个驱动齿轮的角位移误差ei(t)＝θ′i-θi，i＝1,2,…,2m。θ′0～θ′2m分别为2m个驱动齿轮当前的实际角位移值。
步骤五、计算累加后输出控制值ui(t)如式(2)所示，
式(2)中，0.6≤kpi≤2.0，0.05≤kii≤0.3，0.05≤kdi≤0.25，i＝1,2,…,2m；ei(0)＝0。
步骤六、若ui(t)中有一个或多个的正负符号发生变化，则直接进入步骤七。否则，将t增大δt，δt时间后重复执行步骤四和五。
步骤七、若此时第一驱动组为行进测试组，则将此时行进测试组内m个驱动齿轮的角位移误差e1(t)、e2(t)、…、em(t)，作为第一最终误差组内的m个元素e1′、e2′、…、em′，之后将第二驱动组作为行进测试组，第一驱动组作为复位测试组。2m个回转驱动组件内的驱动齿轮均复位后，重复执行步骤二至六。
若此时第二驱动组作为行进测试组，则将此时行进测试组内m个驱动齿轮的角位移误差e1(t)、e2(t)、…、em(t)，作为第二最终误差组内的m个元素em+1″、em+2″、…、e2m″。2m个回转驱动组件内的驱动齿轮均复位后，进入步骤八。
步骤八、若
步骤九、计算行进驱动组内m个驱动齿轮的目标角位移θ1、θ2、…、θm，复位驱动组内m个驱动齿轮的目标角位移θm+1、θm+2、…、θ2m。
步骤十、将0赋值给t，1赋值给k。计算需要输入到2m个电液比例阀的电压值ui(t,k)如式(3)所示。
分别向2m个电液比例阀输入电压值ui(t,k)，i＝1,2,…,2m。将t增大δt。此时，δt时间后进入步骤十一。
步骤十一、与驱动齿轮连接的2m个旋转编码器分别检测2m个驱动齿轮的实际角位移信号。
之后计算2m个驱动齿轮的角位移误差ei(t,k)＝θ′i-θi，i＝1,2,…,2m。θ′0～θ′2m分别为2m个驱动齿轮当前的实际角位移值。
步骤十二、4m个油路压力变送器分别检测油压值。以与行进驱动组对应的2m个油路压力变送器中检测到油压值最大的那个油路压力变送器为行进特征变送器。以与复位驱动组对应的2m个油路压力变送器中检测到油压值最大的那个油路压力变送器为复位特征变送器。与行进特征变送器对应的回转驱动组件为行进特征回转驱动组件。与复位特征变送器对应的回转驱动组件为复位特征回转驱动组件。
计算行进驱动组内m个驱动齿轮的同步误差esynj(i,t,k)＝θi′-θa′，i＝1,2,…,m。θa′为行进特征回转驱动组件内驱动齿轮当前的实际角位移值。计算复位驱动组内m个驱动齿轮的同步误差esynj(i,t,k)＝θi′-θb′，i＝m+1,m+2,…,2m。θb′为复位特征回转驱动组件内驱动齿轮当前的实际角位移值。
步骤十三、若k＝1，则进入步骤十四。若k＞1，则进入步骤十五。
步骤十四、计算累加后输出控制值ui(t,k)如式(4)所示，
式(4)中，ei(0,k)＝0。
直接进入步骤十六。
步骤十五、计算综合误差
式(5)中，0.02≤id≤0.1；0.05≤ip≤0.2；
计算累加后输出控制值ui(t,k)如式(6)所示，
式(6)中，
进入步骤十六。
步骤十六、分别向2m个电液比例阀输入电压值ui(t,k)，i＝1,2,…,2m。
若未接收到复位指令或转动至目标位置指令，则将t增大δt，δt时间后重复执行步骤十一至十五。
若接收到复位指令，则令行进驱动组内m个驱动齿轮的目标角位移θ1＝θ2＝…＝θm＝-δθ，令复位驱动组内m个驱动齿轮的目标角位移θm+1＝θm+2＝…＝θ2m＝0，之后进入步骤十七。
若接收到转动至目标位置指令，则令四个驱动齿轮的目标角位移
步骤十七、将0赋值给δt，将k增大1。与驱动齿轮连接的2m个旋转编码器分别检测2m个驱动齿轮的实际角位移信号。
之后计算2m个驱动齿轮的角位移误差ei(t,k)＝θ′i-θi，i＝1,2,…,2m。θ′0～θ′2m分别为2m个驱动齿轮当前的实际角位移值。
计算输入到2m个电液比例阀的电压值
分别2m个电液比例阀输入电压值ui(t,k)，i＝1,2,…,2m。将t增大δt。δt时间后重复执行步骤十一至十六。
进一步地，所述的信号反馈模块还包括运动算法模块和信号采集模块。运动算法模块采用型号为ipc-610l的工控机。信号采集模块采用s7-300系列中型号为cpu314c的plc。2m+1个旋转编码器、4m个油路压力变送器的信号输出接口均与信号采集模块的6m+1个信号输入接口分别相连。信号采集模块的信号输出接口与运动算法模块的信号输入接口相连。
进一步地，本发明一种多马达同步回转驱动系统还包括误差在线修正系统。所述的误差在线修正系统还包括驱动控制模块和人机界面模块。驱动控制模块采用s7-300系列中型号为cpu314的plc。驱动控制模块的2m个控制输出接口与2m个电液比例阀的控制接口分别相连。人机界面模块采用pc端。人机界面模块的通讯接口与运动算法模块的通讯接口相连。
进一步地，所述的旋转编码器采用增量式旋转编码器。
进一步地，所述的驱动油路还包括电机和液压表。柱塞式液压泵及电机均固定在油箱上。所述电机的输出轴与柱塞式液压泵的输入轴固定。液压表的检测口与电液溢流阀的入油口连通。
进一步地，所述的第一安全阀及第二安全阀均为两位两通换向阀。所述的第二过滤器为精密过滤器。
本发明具有的有益效果是：
1、本发明采用多液压马达同步驱动形式，消除单一马达的不稳定性，优化了执行器的体积，合理利用多自由度电液伺服机械臂的内部空间。
2、本发明集运动控制和运行状态诊断于一体，可有效提升回转驱动系统的多任务适配性，稳定性和驱动效率。
3、本发明采用模块化驱动单元，抗干扰性更强。
4、本发明通过将多个回转驱动系统分为行进驱动组和复位驱动组。行进驱动组作为带动外齿圈的动力，而复位驱动组只是在跟随转动。这就使得在外齿圈复位时，复位驱动组内驱动齿轮能够直接带动外齿圈向复位的方向转动，而不是需要先空转一段齿隙的距离，进而消除了回程误差。
附图说明
图1为本发明采用的回转驱动系统的结构示意图；
图2为本发明采用的回转驱动系统的油路图。
具体实施方式
以下结合附图对本发明作进一步说明。
如图1所示，一种能够消除回程误差的多自由度机械臂回转方法采用的回转驱动系统，包括回转本体、回转驱动组件、驱动油路、信号反馈模块和误差在线修正系统。回转本体包括固定底座1和回转平台2。固定底座1用于支撑整个回转驱动系统。回转平台与固定底座1构成转动副。回转平台2的底部与外齿圈6固定。当外齿圈6啮合受到力矩时，可带动回转平台绕固定底座1中心旋转。
回转驱动组件包括电液伺服马达3、行星减速器4和驱动齿轮5。驱动齿轮5支承在固定底座1上，且与外齿圈6啮合。行星减速器4固定在固定底座1上。行星减速器4的输出轴与驱动齿轮5通过平键配合连接。电液伺服马达3固定在行星减速器4上。电液伺服马达3的输出轴与行星减速器4的输入口通过花键配合连接。实现将电液伺服马达3输出的回转角位移转化为驱动啮合扭矩，驱动回转平台旋转。回转驱动组件共有四个。四个回转驱动组件沿外齿圈6的周向均布。
驱动油路包括第一过滤器8、第二过滤器21、蓄能器13、第一安全阀15、第二安全阀20、柱塞式液压泵9、电机、单向阀10、油箱7、电液比例阀14和电液溢流阀11。第一安全阀15及第二安全阀20均为两位两通换向阀。第二过滤器为精密过滤器。柱塞式液压泵9及电机均固定在油箱7上，电机的输出轴与柱塞式液压泵9的输入轴固定。电机用于驱动柱塞式液压泵9提供高压油。柱塞式液压泵9的入油口与油箱7通过第一过滤器8连通，出油口与单向阀10的输入口连通。单向阀10的输出口与电液溢流阀11的入油口、液压表的检测口及第二过滤器的输入口连通。电液溢流阀11的出油口与油箱连通。第二过滤器的输出口与蓄能器13的蓄能油口及手动球阀12的一个通油口连通。手动球阀12的另一个通油口与四个电液比例阀14的进油口(p口)连通，四个电液比例阀14的回油口(t口)均与油箱7连通。
四个电液比例阀14的第一工作油口(a口)与四个第一安全阀15的第一油口分别连通。四个电液比例阀14的第二工作油口(b口)与四个第二安全阀20的第一油口分别连通。四个第一安全阀15的第二油口与四个回转驱动组件内电液伺服马达3的第一油口分别连通。四个第二安全阀20的第二油口与四个回转驱动组件内电液伺服马达3的第二油口分别连通。
信号反馈模块包括运动算法模块19、信号采集模块、五个旋转编码器和八个油路压力变送器16。运动算法模块19采用型号为ipc-610l的工控机。运动算法模块19通过内部安装的borlandc++软件及相应的运动算法编程，实现角位移跟踪、同步与回程误差数据计算、误差补偿数据计算、运动控制指令计算。信号采集模块采用s7-300系列中型号为cpu314c的plc。五个旋转编码器采用增量式旋转编码器，且均与固定底座1固定。五个旋转编码器的输入轴与回转平台、四个回转驱动组件内的驱动齿轮5分别同轴固定。进而，实时将回转平台和各驱动齿轮5的角位移变化量转变为模拟信号输出。八个油路压力变送器16的检测口与四个回转驱动组件内电液伺服马达3的第一油口、第二油口分别连通。进而实时将油箱7和各电液伺服马达3的工作压力变化量转变为模拟信号输出；五个旋转编码器、八个油路压力变送器16的信号输出接口均与信号采集模块的十三个信号输入接口分别相连。进而将将编码器和油路压力变送器16输出的模拟信号精确地转化为相应的数字信号。信号采集模块能够实现旋转编码器及油压传感器模拟信号的实时转化及存储。信号采集模块的信号输出接口与运动算法模块19的信号输入接口相连。
误差在线修正系统包括运动控制部分和人机界面模块。运动控制部分包括数据采集板卡和驱动控制模块17。驱动控制模块17采用s7-300系列中型号为cpu314的plc。数据采集板卡的型号为pci-1719。驱动控制模块17的输入接口与数据采集板卡的模拟输出接口相连。四个控制输出接口与四个电液比例阀14的控制接口分别相连。驱动控制模块用于将运动算法模块计算得出的控制指令，转化为电液比例阀14的驱动电压，进而控制电液伺服马达3的输出角位移。
人机界面模块18采用pc端。人机界面模块18的通讯接口与运动算法模块19的通讯接口相连。人机界面模块通过mpi通信协议所需的动态链接库和mpi通信电缆与信号采集模块的dp通讯接口相连，实现信号采集模块15中所存储的五个编码器和十个油路压力变送器16的数据读取、存储；采用图形化编程软件，编制状态监控及控制界面，用于实现回转平台角位移的显示，角速度和角加速度显示，电液伺服马达3角位移及工作压力的显示，角速度，角加速度的显示，经由运动算法模块计算得出的角位移跟踪，同步与回程误差数据显示；误差补偿数据显示；回转系统示警信息显示；实现运动控制指令人机交互等。
初始状态下，四个驱动齿轮均与外齿圈接触，且外齿圈上与四个驱动齿轮接触的齿的接触面均位于同一侧。(即四个驱动齿轮与外齿圈的接触状态为外齿圈带动四个驱动齿轮转动时的状态)
四个驱动齿轮与外齿圈之间必然存在齿隙。若四个驱动齿轮同时驱动外齿圈转动，则在外齿圈需要反转时，四个驱动齿轮必然需要经过一段空行程才能与外齿圈重新接触并带动外齿圈反转，这就导致了回程误差。为解决回程误差的问题，本发明中一半的驱动齿轮仅与外齿圈上齿的其中一个侧面接触，另一半齿轮仅与外齿圈上齿的其中一个侧面接触，即一半的驱动齿轮仅用于驱动外齿圈正转，另一半仅用于驱动外齿圈反转。这样，外齿圈需要改变转向时，只需将更换对应驱动齿轮作为动力源，即可大大降低齿隙带来的回程误差。
此外，由于驱动齿轮存在惯性，故不作为动力源的驱动齿轮也不能仅是被外齿圈带动，而是需要在液压马达的动力下跟随外齿圈转动。为此提出本发明，以完成上述目标。
该能够消除回程误差的多自由度机械臂回转方法具体如下：
步骤一：选取其中两个回转驱动组件作为第一驱动组，另两个回转驱动组件作为第二驱动组。第一驱动组的两个回转驱动组件互不相邻。以第一驱动组作为行进测试组，第二驱动组作为复位测试组。使用者设定回转平台的角位移θ0。
步骤二、计算行进测试组内两个驱动齿轮的目标角位移θ1、θ2，复位测试组内两个驱动齿轮的目标角位移θ3、θ4。
其中，r为外齿圈6的分度圆半径，r为驱动齿轮5的分度圆半径；δθ为驱动齿轮与外齿圈的齿隙在驱动齿轮上对应的角度(即以驱动齿轮中心为顶点，齿隙两侧边缘到驱动齿轮中心的连线为两条边所成角的大小)。
步骤三、将0赋值给t。计算需要输入到四个驱动齿轮对应的电液比例阀14的电压值ui(t)如式(1)所示，
其中，umax、qmax分别为电液比例阀14的最大控制电压、额定流量，dm为电液伺服马达3的额定排量，δt是运动算法模块19设定的控制周期。δt＝0.1s。
分别向四个电液比例阀14输入电压值ui(t)，i＝1,2,3,4。将t增大δt。δt时间后进入步骤四。
步骤四、五个旋转编码器分别检测回转平台2、四个驱动齿轮的实际角位移信号，并将其传输给信号采集模块15。信号采集模块15对接收到的实际角位移信号进行处理后传输给运动算法模块19。运动算法模块19计算回转平台2、四个驱动齿轮的角位移误差ei(t)＝θ′i-θi，i＝0,1,2,3,4。θ′0～θ′4分别为回转平台2、行进测试组内两个驱动齿轮的当前的实际角位移值、复位测试组内两个驱动齿轮的当前的实际角位移值。实际角位移值为对应驱动齿轮当前位置与初始位置的角度差(实际角位移值在超过360°后不清零，仅在反向旋转时才能向零靠近)。
步骤五、计算累加后输出控制值ui(t)如式(2)所示，
式(2)中，kpi、kii、kdi分别为四个回转驱动组件的第一pid控制参数、第二pid控制参数、第三pid控制参数，i＝1,2,3,4，其中0.6≤kpi≤2.0，0.05≤kii≤0.3，0.05≤kdi≤0.25；根据系统实际运行结果进行优化选择。ei(0)＝0。
步骤六、若ui(t)中有一个或多个的正负符号发生变化(即ui(t-δt)为负值而ui(t)为正值或ui(t-δt)为正值而ui(t)为负值)，则直接进入步骤七。否则，将t增大δt，δt时间后重复执行步骤四和五。
步骤七、若此时第一驱动组作为行进测试组，则将此时行进测试组内两个驱动齿轮的角位移误差e1(t)、e2(t)，作为第一最终误差组内的两个元素e1′、e2′，之后将第二驱动组作为行进测试组，第一驱动组作为复位测试组。四个回转驱动组件内的驱动齿轮均复位后，重复执行步骤二至六。
若此时第二驱动组作为行进测试组，则将此时行进测试组内两个驱动齿轮的角位移误差e1(t)、e2(t)，作为第二最终误差组内的两个元素e1″、e2″。四个回转驱动组件内的驱动齿轮均复位后，进入步骤八。
步骤八、若e1′+e2′≤e1″+e2″则将第一驱动组作为行进驱动组，将第二驱动组作为复位驱动组。否则，将第二驱动组作为行进驱动组，将第一驱动组作为复位驱动组。
步骤九、开始正式往复转动工作。
计算行进驱动组内两个驱动齿轮的目标角位移θ1、θ2，复位驱动组内两个驱动齿轮的目标角位移θ3、θ4。
步骤十、将0赋值给t，1赋值给k。计算需要输入到四个电液比例阀14的电压值ui(t,k)如式(3)所示。
分别向四个电液比例阀14输入电压值ui(t,k)，i＝1,2,3,4。将t增大δt。此时，δt时间后进入步骤十一。输入电压值ui(t,k)的电液比例阀与目标角位移为θi的驱动齿轮分别对应。在向目标位置行进时，由于复位驱动组内两个驱动齿轮的目标角位移小于行进驱动组内两个驱动齿轮的目标角位移，故仅行进驱动组内两个驱动齿轮作为带动外齿圈的动力，而复位驱动组内两个驱动齿轮只是在跟随转动。这就使得在外齿圈复位时，复位驱动组内两个驱动齿轮能够直接带动外齿圈向复位的方向转动，而不是需要先空转一段齿隙的距离。此外，行进驱动组的驱动齿轮仅与外齿圈上齿的一侧齿面接触，复位驱动组内的驱动齿轮仅与外齿圈上齿的另一侧齿面接触。这就能够将外齿圈夹住，减少外齿圈在齿隙范围内由于惯性产生的颤动。
步骤十一、五个旋转编码器分别检测回转平台2、四个驱动齿轮的实际角位移信号，并将其传输给信号采集模块15。信号采集模块15对接收到的实际角位移信号进行处理后传输给运动算法模块19。运动算法模块19计算回转平台2、四个驱动齿轮的角位移误差ei(t,k)＝θ′i-θi，i＝0,1,2,3,4。θ′0～θ′4分别为回转平台2、行进驱动组内两个驱动齿轮、复位驱动组内两个驱动齿轮当前的实际角位移值。
步骤十二、八个油路压力变送器16分别检测所在油路的油压值。以与行进驱动组对应的四个油路压力变送器16中检测到油压值最大的那个油路压力变送器16为行进特征变送器。以与复位驱动组对应的四个油路压力变送器16中检测到油压值最大的那个油路压力变送器16为复位特征变送器。与行进特征变送器对应的回转驱动组件为行进特征回转驱动组件。与复位特征变送器对应的回转驱动组件为复位特征回转驱动组件。
计算行进驱动组内两个驱动齿轮的同步误差esynj(i,t,k)＝θi′-θa′，i＝1,2。计算复位驱动组内两个驱动齿轮的同步误差esynj(i,t,k)＝θi′-θb′，i＝3,4。θa′为行进特征回转驱动组件内驱动齿轮当前的实际角位移值。θb′为复位特征回转驱动组件内驱动齿轮当前的实际角位移值。
步骤十三、若k＝1，则进入步骤十四。若k＞1，则进入步骤十五。
步骤十四、计算累加后输出控制值ui(t,k)如式(4)所示，
式(4)中，ei(0,k)＝0。
直接进入步骤十六。
步骤十五、计算综合误差
式(5)中，id是微分学习增益，0.02≤id≤0.1；ip是比例微分学习增益，0.05≤id≤0.2，id和ip具体取值根据系统的经验模型确定，以保证当k趋近于∞时，
计算累加后输出控制值ui(t,k)如式(6)所示，
式(6)中，
进入步骤十六。
步骤十六、分别向四个电液比例阀输入电压值ui(t,k)，i＝1,2,3,4。若未接收到复位指令或转动至目标位置指令，则将t增大δt，δt时间后重复执行步骤十一至十五。若接收到复位指令，则令四个驱动齿轮的目标角位移θ1＝θ2＝-δθ；θ3＝θ4＝0，并进入步骤十七。若接收到转动至目标位置指令，则令四个驱动齿轮的目标角位移
步骤十七、将0赋值给δt，将k增大1。五个旋转编码器分别检测回转平台2、四个驱动齿轮的实际角位移信号。计算回转平台2、四个驱动齿轮的角位移误差ei(t,k)＝θ′i-θi，i＝0,1,2,3,4。θ′0～θ′4分别为回转平台2、四个驱动齿轮当前的实际角位移值。
计算需要输入到与四个驱动齿轮分别对应的四个电液比例阀14的电压值
分别向四个电液比例阀输入电压值ui(t,k)，i＝1,2,3,4。将t增大δt。δt时间后重复执行步骤十一至十六。