[发明专利]多关节机器人及其零位标定方法、装置和可读介质

权利要求书

1.一种多关节机器人的零位标定方法，所述多关节机器人包括底座、大臂和小臂，所述大臂位于第二关节和第三关节之间，所述小臂位于第四关节和机器人末端之间，其特征在于，包括：

控制所述小臂的小臂轴线与所述底座的底座安装平面平行；

控制第一关节匀速旋转，所述第一关节位于第一初始位置，通过小臂惯性传感器获取所述小臂的第一动态加速度信息，根据所述第一动态加速度信息、大臂长度和所述小臂惯性传感器的位置计算第一偏转角，控制所述第二关节和所述第三关节旋转所述第一偏转角，使所述第二关节和所述第三关节位于各自的零位；

控制所述第四关节从第四初始位置旋转至目标位置，通过所述小臂惯性传感器获取所述小臂的第一加速度信息，根据所述第一加速度信息计算所述第四关节的第四偏转角，控制所述第四关节从所述第四初始位置旋转所述第四偏转角，使所述第四关节位于其零位；以及

控制所述第二关节和所述第三关节移动以使所述小臂中的电磁线圈位于靠近所述底座的目标位置，控制所述电磁线圈两端的电压使所述电磁线圈分别产生方向相反的第一磁场和第二磁场，读取所述底座中的磁力计的磁力计数据，所述磁力计数据包括所述第一磁场对应的第一矢量和所述第二磁场对应的第二矢量，使所述第一矢量和所述第二矢量相减获得第三矢量，获得所述第三矢量投影在所述底座安装平面上的第三投影矢量，根据所述磁力计的安装方向和所述第三投影矢量的当前方向计算第二偏转角，控制所述第一关节旋转所述第二偏转角，使所述第一关节位于其零位，其中，所述磁力计的安装方向和所述第一关节的零位方向重合。

2.如权利要求1所述的零位标定方法，其特征在于，控制所述小臂的小臂轴线与所述底座的底座安装平面平行的步骤包括：

当所述第一关节静止在所述第一初始位置时，通过所述小臂惯性传感器获取所述小臂的初始加速度信息；

控制所述第一关节匀速旋转，通过所述小臂惯性传感器获取所述小臂的初始动态加速度信息；

根据所述初始加速度信息和所述初始动态加速度信息计算所述小臂与所述底座安装平面之间的第一夹角；以及

控制所述第二关节或所述第三关节旋转所述第一夹角，使所述小臂轴线与所述底座的底座安装平面平行。

3.如权利要求2所述的零位标定方法，其特征在于，采用下面的公式计算所述第一夹角：

其中，θ表示所述第一夹角，(α0_x,α0_y,α0_z)表示所述初始加速度信息，(β0_x,β0_y,β0_z)表示所述初始动态加速度信息，(x,y,z)表示所述小臂惯性传感器的小臂坐标系的每个坐标轴。

4.如权利要求2所述的零位标定方法，其特征在于，还包括：调整所述第一关节的位置到除所述第一初始位置之外的至少一个其他位置，当所述第一关节位于所述其他位置时，重新计算所述第一夹角，将所述第一初始位置对应的第一夹角和所述至少一个其他位置对应的至少一个第一夹角的平均值作为所述第一夹角。

5.如权利要求4所述的零位标定方法，其特征在于，所述至少一个其他位置包括所述第一初始位置沿第一方向转动90度或所述第一初始位置沿第二方向转动90度，所述第一方向和所述第二方向相反。

6.如权利要求1所述的零位标定方法，其特征在于，所述大臂长度是第二关节旋转轴和第三关节旋转轴之间的垂直距离。

7.如权利要求6所述的零位标定方法，其特征在于，根据所述第一动态加速度信息、大臂长度和所述小臂惯性传感器的位置计算第一偏转角的步骤包括，采用下面的公式计算：

其中，γ表示所述第一偏转角，L表示所述小臂惯性传感器的位置到所述第三关节的距离，y表示所述小臂惯性传感器的小臂坐标系中的y轴，β1_y表示所述第一动态加速度信息中的y轴加速度，表示所述第一关节的角速度，D表示所述大臂长度。