[发明专利]一种多连杆自检协作机械臂及控制方法

权利要求书

1.一种多连杆自检协作机械臂,包括底座（1）、设于底座上拥有六自由度的机械臂、设于机械臂末端的执行舵机（8），所述的机械臂包括依次连接的第一驱动箱（2）、四连杆机构（3）、第二驱动箱（4）、套轴机臂（5）、末端传动头（6）和末端执行头（7），底座、第一驱动箱和第二驱动箱内均设有电机，且电机连接增量式光电编码器；其特征在于：所述底座上设有相对其自身转动的第一转轴（104），所述第一驱动箱与第一转轴的上端连接固定，所述底座上设有蜗轮蜗杆驱动组件，蜗轮蜗杆驱动组件连接第一转轴的下端并同步传动；所述第一驱动箱内设有第二转轴（203）和第三转轴（204）并分别由第二电机（201）和第三电机（202）驱动，所述四连杆机构围成平行四边形且与第二转轴和第三转轴配合传动；所述第二驱动箱固定在四连杆机构的上端，第二驱动箱包括第二箱体，第二箱体的后表面设置有两个直流有刷电机（401），第二箱体内设有分别与两个直流有刷电机连接的减速传动机构，所述套轴机臂包括分别与减速传动机构连接传动的内层输出轴（502）和外层输出轴（501），外层输出轴连接末端传动头并带动其同轴转动，内层输出轴伸入末端传动头内部并通过一级齿轮副与末端执行头连接传动，使末端执行头与末端传动头相对转动；

所述的第二转轴和第三转轴分别通过不同的两级齿轮副与第二电机和第三电机连接传动；

所述的减速传动机构包括两组传动齿轮组，每组所述传动齿轮组包括一个主动轮和一个从动轮，第二箱体内设有第四转轴（403）和第五转轴（402），两个主动轮分别与两个直流有刷电机连接，两个从动轮分别设置在第四转轴和第五转轴上；第四转轴与内层输出轴同轴转动，第五转轴与外层输出轴同轴转动；

所述多连杆自检协作机械臂采用自适应方法控制，该自适应方法控制包括调零步骤和碰撞检测步骤，在机械臂启动时率先执行调零步骤，在机械臂运作过程中执行碰撞检测步骤；

调零步骤：

S11：将第一转轴慢速旋转直至机械限位处，根据机械限位角度得到第一转轴的角度Q1，并将其设置为对应增量式光电编码器初始值；

S12：将第二转轴和第三转轴慢速向机械臂前方旋转直至四连杆结构缩减到最小，确定第二转轴和第三转轴上增量式光电编码器的差值δ；

S13：将第二转轴与第三转轴同时同向匀速向机械臂后方旋转，直至到达机械限位处，得到第二转轴的角度Q2，以及第三转轴角度Q2+δ，分别将这两个值设置为第二转轴和第三转轴对应的增量式光电编码器初始值；

S14：将第四转轴慢速旋转至机械限位处，并同时对第五转轴进行反向旋转补偿，得到第四转轴初始角度Q4，并将其设置为对应增量式光电编码器初始值；

S15：将第五转轴旋转至机械限位处，得到第五转轴的角度Q5并将其设置为对应增量式光电编码器初始值；

碰撞检测步骤：

S21：末端执行头不装载任何执行夹具的情况下，对机械臂完成启动并调零，对每一个扭矩传感器的读数进行测量；

S22：装上夹具，对夹具进行物理标定；在机械臂调零后，读出与S21中同样姿态下的扭矩传感器读数，并计算与S21中对应读数的差值；

S23：根据S22中读数差值对夹具的质量与质心位置进行估算；

S24：对夹具进行物理标定之后，记录夹具在各个角度下执行舵机（8）的电流值，机械臂实际运转过程中，当夹具发生碰撞，或者拖动示教时人为搬动夹具时，执行舵机（8）的电流值会偏离当前角度的基线电流值，若偏移超过一定阈值，则检测到了碰撞或者拖动示教的信号，且偏移的正负值就代表了外力的方向。

2.根据权利要求1所述的一种多连杆自检协作机械臂,其特征在于：所述的四连杆机构包括第一主动杆（301）、第二主动杆（303）、第一从动杆（302）和第二从动杆（304），所述第一主动杆与第二转轴垂直连接并随轴转动，所述第二主动杆与第三转轴垂直连接并随轴转动，所述第一从动杆与第一主动杆铰接且固定到第二箱体，所述第二从动杆一端与第二主动杆铰接，另一端与第一从动杆铰接。

3.根据权利要求1所述的一种多连杆自检协作机械臂,其特征在于：所述的两级齿轮副为直齿轮副，传动比大于等于三。

4.根据权利要求1所述的一种多连杆自检协作机械臂,其特征在于：所述的第一转轴、第二转轴、第三转轴、第四转轴和第五转轴上均设置有扭矩传感器（9）。