[发明专利]一种实时标定机器人传感器姿态的方法、装置及终端设备

权利要求书

1.一种实时标定机器人传感器姿态的方法，其特征在于，所述机器人上设置有所述传感器的绑定位，所述传感器设置于所述绑定位，所述实时标定机器人传感器姿态的方法包括：

建立机器人运动坐标系和传感器坐标系；

控制所述传感器定向运动，获取六维位姿数据，计算所述传感器坐标系相对于所述机器人运动坐标系的初始姿态关系；

计算所述绑定位相对于所述机器人的姿态变换量；

根据所述绑定位相对于所述机器人的姿态变换量和所述传感器坐标系相对于所述机器人运动坐标系的初始姿态关系，计算所述传感器相对于所述机器人的姿态关系。

2.如权利要求1所述的实时标定机器人传感器姿态的方法，其特征在于，所述控制所述传感器定向运动，获取六维位姿数据，计算所述传感器坐标系相对于所述机器人运动坐标系的初始姿态关系包括：

控制所述传感器沿所述机器人运动坐标系中的坐标轴方向运动，获取所述传感器在所述传感器坐标系中的初始数据；

根据所述传感器在所述传感器坐标系中的初始数据，计算所述机器人运动坐标系的坐标轴在所述传感器坐标系下的单位向量；

根据所述单位向量，计算所述机器人运动坐标系相对于所述传感器坐标系的姿态关系；

根据所述机器人运动坐标系相对于所述传感器坐标系的初始姿态关系，计算所述传感器坐标系相对于所述机器人运动坐标系的初始姿态关系。

3.如权利要求2所述的实时标定机器人传感器姿态的方法，其特征在于，所述获取所述传感器在所述传感器坐标系中的初始数据包括：

所述机器人静止时，获取所述传感器的初始值，公式为：

P₀＝[x₀ y₀ z₀]^T，

其中，x₀，y₀，z₀表示所述传感器在所述传感器坐标系中的初始位置；

控制所述传感器沿所述机器人运动的X方向运动时，获取第一初始数据，公式为：

P_x＝[x_x y_x z_x]^T，

其中，x_x为沿所述机器人运动的X方向推动所述传感器的力在所述传感器坐标系中的X轴上的分量，y_x为沿所述机器人运动的X方向推动所述传感器的力在所述传感器坐标系中的Y轴上的分量，z_x为沿所述机器人运动的X方向推动所述传感器的力在所述传感器坐标系中的Z轴上的分量；

控制所述传感器沿所述机器人运动的Y方向运动时，获取第二初始数据，公式为：

P_y＝[x_y y_y z_y]^T，

其中，x_y为沿所述机器人运动的Y方向推动所述传感器的力在所述传感器坐标系中的X轴上的分量，y_y为沿所述机器人运动的Y方向推动所述传感器的力在所述传感器坐标系中的Y轴上的分量，z_x为沿所述机器人运动的Y方向推动所述传感器的力在所述传感器坐标系中的Z轴上的分量。

4.如权利要求2所述的实时标定机器人传感器姿态的方法，其特征在于，所述根据所述传感器初始数据，计算所述机器人运动坐标系的坐标轴在所述传感器坐标系下的单位向量包括：

计算得到所述机器人运动坐标系的X轴在传感器坐标系下的单位向量，公式为：

计算得到所述机器人运动坐标系的Y轴在传感器坐标系下的单位向量，公式为：

计算得到所述机器人运动坐标系的Z轴在传感器坐标系下的单位向量，公式为：

根据所述单位向量，计算所述机器人运动坐标系相对于所述传感器坐标系的初始姿态关系，公式为：

根据所述机器人运动坐标系相对于所述传感器坐标系的初始姿态关系，计算所述传感器坐标系相对于所述机器人运动坐标系的初始姿态关系，公式为：

其中A表示机器人运动坐标系，B表示传感器坐标系。