[发明专利]一种定位的方法及装置

权利要求书

1.一种定位的方法，其特征在于，适用于设有机械臂的机器人，所述机械臂设置在所述机器人的基座上；所述方法包括：

通过所述机器人的摄像装置获取的图像信息，确定所述机械臂的活动端的第一位姿信息；

通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息；

根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息；

所述通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，包括：

通过第一采集器获取所述机械臂的各关节的运动信息，根据所述各关节的运动信息和所述基座的位置信息，确定所述活动端的位置信息；

通过第二采集器获取所述活动端的姿态信息；

根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息。

2.根据权利要求1所述的方法，其特征在于，所述根据所述各关节的运动信息和所述基座的位置信息，确定所述活动端的位置信息，包括：

根据所述各关节的角度信息，确定自所述基座至所述活动端之间的各关节之间的第一变换矩阵；

根据所述各关节的第一变换矩阵和所述基座在世界坐标系下的第一坐标，确定所述活动端在世界坐标系下的第二坐标；

根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息，包括：

根据所述活动端的姿态信息，确定所述活动端的旋转矩阵；

根据所述旋转矩阵和所述第二坐标，确定从所述基座转换至所述活动端的第二变换矩阵；

根据所述第二变换矩阵和所述第二坐标，确定所述活动端的第二位姿信息。

3.根据权利要求2所述的方法，其特征在于，通过公式(1)确定所述活动端在世界坐标系下的第二坐标：

公式(1)

其中[x_a,y_a,z_a,1]为所述基座在世界坐标系下的坐标，为所述机械臂活动端在世界坐标系下的坐标，^n-1T_n为各关节之间的第一变换矩阵；

通过公式(2)确定所述活动端的第二变换矩阵：

公式(2)

其中，R_h＝R(θ_roll)R(θ_pitch)R(θ_yaw)为旋转矩阵，θ_roll为所述机械臂活动端的横滚角，θ_pitch为所述机械臂活动端的俯仰角，θ_yaw为所述机械臂活动端的偏航角；

通过公式(3)确定所述活动端的第二位姿信息：

公式(3)其中为活动端的第二位姿信息，^WT_r为所述活动端的第二变换矩阵，P_r为所述基座在世界坐标系下的位置。

4.根据权利要求1所述的方法，其特征在于，所述通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，包括：

通过第一采集器获取的运动信息，确定所述机械臂的状态转移矩阵；

根据所述运动信息对应的控制信息，确定所述机械臂的输入转移矩阵以及关节输入矩阵；

根据上一时刻所述活动端的第二位姿信息、状态转移矩阵、输入转移矩阵以及关节输入矩阵确定所述活动端在当前的第二位姿信息。

5.根据权利要求4所述的方法，其特征在于，通过公式(4)确定所述活动端在当前的第二位姿信息

公式(4)

其中，A为所述机械臂的状态转移矩阵，B为所述机械臂的输入转移矩阵，u_i-1为第i-1s所述机械臂的关节输入矩阵，P_w(i-1)为所述活动端在第i-1s的位姿信息。

6.根据权利要求1至5任一项所述的方法，其特征在于，所述根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息，包括：

确定所述第一位姿信息的第一误差和所述第二位姿信息的第二误差；

根据所述第一误差、所述第二误差、所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息。