[发明专利]一种转臂式清罐装置万向驱动机构控制方法

摘要

本发明专利公开了一种转臂式清罐装置万向驱动机构控制方法，该方法所使用的装置包括控制器、两个角度传感器、第一回转臂驱动装置，转向电机，万向驱动机构。控制器依据角度传感器采样的第一回转臂的关节转角θ1′，第二回转臂的关节转角θ2′，第一回转臂的关节速度第二回转臂的关节速度计算万向驱动机构转向调整角度θ，控制转向电机调整万向驱动机构使其主动驱动轮的运动方向与其安装处第二回转臂B点的合成线速度vB方向一致。本发明采用主动转向的方法使转向运动与驱动运动同步进行，实时调整驱动机构运动方向与第二回转臂B点处的合成线速度vB方向一致，保证转臂式清罐装置回转顺畅，降低换热管定位时间，正确定位清洗的换热管。

主权项

一种转臂式清罐装置万向驱动机构控制方法，该方法所使用的装置包括控制器(1)、第一角度传感器(2)、第二角度传感器(4)、第一回转臂驱动装置(5)，转向电机(7)，万向驱动机构(8)；第一角度传感器(2)安装在第一回转臂(3)的轴线1上，传感器中心与轴线1重合，用于检测第一回转臂(3)的关节转角θ′1，第二角度传感器(4)安装在第二回转臂(6)的轴线2上，传感器中心与轴线2重合，用于检测第二回转臂(6)的关节转角θ′2，转向电机(7)安装在第二回转臂(6)轴线3处驱动万向机构(8)进行转向；其特征在于，转向电机(7)依据第一回转臂(3)的关节转角θ′1，第二回转臂(6)的关节转角θ′2实时调整万向驱动机构(8)使其主动驱动轮的运动方向与第二回转臂(6)B点处的合成线速度vB方向一致；所述的一种转臂式清罐装置万向机构控制方法的具体过程为：控制器(1)根据给定的第一回转臂(3)的目标关节转角θ1，即以第一回转臂(3)回转轴线1与回转平面的交点o为原点建立直角坐标系，第二回转臂(6)的目标关节转角θ2，第一回转臂(3)的关节速度第二回转臂(6)的关节速度分别给第一回转臂驱动装置(5)，万向驱动机构(8)发送指令脉冲，第一回转臂驱动装置(5)根据所述指令脉冲驱动第一回转臂(3)绕轴线1回转运动，第一回转臂(3)的关节转角θ′1由第一角度传感器(2)检测并反馈给控制器(1)；同时万向驱动机构(8)根据所述指令脉冲驱动第二回转臂(6)绕轴线2回转运动，第二回转臂(6)的关节转角θ′2由第二角度传感器(4)检测并反馈给控制器(1)；控制器(1)按照角度控制算法，依据第一回转臂(3)的关节转角θ′1，第二回转臂(6)的关节转角θ′2，第一回转臂(3)的关节速度第二回转臂(6)的关节速度计算万向驱动机构(8)转向调整角度θ，控制器(1)根据调整角度θ控制转向电机(7)调整驱动机构(8)使其主动驱动轮的运动方向与第二回转臂(6)B点处的合成线速度vB方向一致；所述的角度控制算法，包括以下步骤：(1)控制器(1)参数初始化操作：所述的参数包括第一回转臂杆长L1、第二回转臂杆长L2，插补加速度，插补减速度，插补速度；(2)控制器(1)接收目标位置参数及关节速度：所述的目标位置参数包括第一回转臂(3)的目标关节转角θ1，第二回转臂(6)的目标关节转角θ2；所述的关节速度包括第一回转臂(3)的关节速度第二回转臂(6)的关节速度(3)判断定位运行是否完成：所述的定位运行完成的判断条件是：第一回转臂(3)的关节转角θ1′＝目标关节转角θ1且第二回转臂(6)的关节转角θ2′＝目标关节转角θ2；当判断条件成立时，第一回转臂(3)、第二回转臂(6)定位到目标位置，角度调整结束；当判断条件不成立时，则执行下面步骤(4)到步骤(7)；(4)采样插补点处关节转角参数，所述的关节转角参数包括第一回转臂(3)的关节转角θ1′，第二回转臂(6)的关节转角θ2′；(5)计算插补点位置参数：依据关节转角参数θ1′，θ2′计算插补点处第一回转臂(3)关节转角位置θ1i，第二回转臂(6)关节转角位置θ2i；(6)依据第一回转臂(3)关节转角位置θ1i，第二回转臂(6)关节转角位置θ2i按公式(1)计算雅克比矩阵J；按公式(2)计算B点处的x方向速度分量vBx，y方向速度分量vBy；按公式(3)式计算B点处速度vB的方向θvB，按公式(4)计算调整角度θ；J=-L1sinθ1i-L2sin(θ1i+θ2i)-L2sin(θ1i+θ2i)L1cosθ1i+L2cos(θ1i+θ2i)L2cos(θ1i+θ2i)---(1)]]>vBxvBy=Jθ·1θ·2---(2)]]>θvB=arctanvByvBx---(3)]]>θ＝θvB‑θB0  (4)其中：θB0为万向驱动机构(8)的初始方向角；(7)转向电机(7)根据调整角度θ控制调整驱动机构(8)使其主动驱动轮的运动方向与第二回转臂(6)B点处的合成线速度vB方向一致，返回到步骤(3)。