[发明专利]一种机器人双臂自平衡方法及机器人

权利要求书

1.一种机器人双臂自平衡方法，其特征在于，包括：

机器人，所述机器人的手臂包括可旋转的手掌、手肘和肩膀转动关节；

所述手掌设置有第一陀螺仪，所述机器人底部设置有第二陀螺仪；

方法步骤包括：通过控制所述机器人的手臂的电机转动角度及根据第一陀螺仪的倾斜角转动所述手掌调整所述手掌处于平衡状态；

以所述肩膀转动关节为原点，设置所述手掌、所述手肘的坐标，并根据所述手掌、所述手肘和所述肩膀转动关节计算所述手臂的前臂与水平轴的夹角a1及所述手臂的上臂与水平轴的夹角a2；

具体的，以A表示机器人的手掌，以B表示机器人的手肘，以O表示机器人的肩膀转动关节，若以肩膀转动关节为原点建立直角坐标系，则A、B和O的坐标分别为A(x3，y3)、B(x2，y2)和O(0,0)，且AB＝L1，BO＝L2，x轴与OB的夹角为a2，若以B为原点建立直角坐标系，则其x轴与AB的夹角为a1，则有：

x2＝L2*cos(a2)；

y2＝L2*sin(a2)；

x3＝L2*cos(a2)+L1*cos(a1)；

y3＝L2*sin(a2)+L1*sin(a1)；

根据所述夹角a1及所述夹角a2控制所述机器人的手臂的电机转动角度，并控制所述手掌的关节的电机根据所述第一陀螺仪的前后倾斜角大小转动，调整所述手掌处于平衡状态；

具体的，当机器人手臂垂直向下移动L3的距离时，以A1(x31，y31)和B1(x21，y21)表示移动后的手掌和手肘的坐标，此时手臂各关节的数据具体如下：

x31＝L2*cos(a2)+L1*cos(a1)；

y31＝L2*sin(a2)+L1*sin(a1)-L3；

为使得机器人手臂能重新回到移动之前的位置，即将机器人的手臂垂直向上运动L3，此时：

则肩膀转动关节O需要转动的角度为：a21-a2；

手肘关节B要转动的角度为：a11-a1；手掌关节A的转动由第一陀螺仪控制；

根据检测到的所述第一陀螺仪和所述第二陀螺仪的输出数据获取所述机器人的机体和两个手臂的姿态信息，并判断所述机器人的机体倾斜角是否有变化；

若所述机器人的机体倾斜角有变化，计算所述机器人的机体的左右倾斜角和前后倾斜角，并根据所述左右倾斜角上下移动机器人的两支所述手臂，以使两支所述手臂位于同一高度，根据所述前后倾斜角转动所述手掌；

根据预设的所述机器人的承载物的重量范围控制所述机器人的两个手臂的输出力矩。