[发明专利]一种抓钢机定位控制方法及系统

权利要求书

1.一种抓钢机定位控制方法，其特征在于，包括：

获取抓钢机的第一关联信息，所述第一关联信息至少包括以下之一：底盘的姿态信息、底盘的位置信息；

建立抓钢机的底盘和抓钢机的末端执行器之间的映射关系，并通过所述第一关联信息和映射关系确定第二关联信息，所述第二关联信息至少包括以下之一：末端执行器的姿态信息、末端执行器的位置信息；

根据所述第一关联信息和第二关联信息对抓钢机进行控制；

建立抓钢机的底盘和抓钢机的末端执行器之间的映射关系，并通过所述第一关联信息和映射关系确定第二关联信息的步骤包括：

根据抓钢机的尺寸，建立抓钢机的底盘和抓钢机的末端执行器之间的映射关系；

通过惯性传感器获取抓钢机各个关节的转角，根据所述第一关联信息、映射关系和转角确定抓钢机的末端执行器相对于所述底盘的位置和姿态；

根据所述第一关联信息、映射关系和转角确定抓钢机的末端执行器相对于所述底盘的位置和姿态的步骤包括：

根据所述第一关联信息和映射关系，在所述抓钢机的每个关节分别建立运动坐标系，并获取相邻所述运动坐标系的齐次变换矩阵；

根据所述齐次变换矩阵，获取所述末端执行器的末端相对于所述底盘的旋转矩阵；

通过惯性传感器获取抓钢机各个关节的转角，所述转角包括：θ_i和α_i，根据所述转角和所述齐次变换矩阵，确定所述末端执行器相对于所述底盘的位置和姿态；

所述旋转矩阵的数学表达为：

其中，为所述末端执行器的末端相对于所述底盘的旋转矩阵，为第1个所述运动坐标系与第0个所述运动坐标系的齐次变换矩阵，为第i个所述运动坐标系与第i-1个所述运动坐标系的齐次变换矩阵，θ_i为绕着z_i轴从x_i-1轴到x_i轴的角度，α_i为沿着x_i轴从z_i轴到z_i+1轴的角度，z_i轴为第i个所述运动坐标系的z轴，x_i-1轴为第i-1个所述运动坐标系的x轴，x_i轴为第i个所述运动坐标系的x轴，z_i+1轴为第i+1个所述运动坐标系的z轴。

2.根据权利要求1所述的抓钢机定位控制方法，其特征在于，所述底盘的姿态信息的获取方式包括：

获取抓钢机的底盘的初始角速度信息和实时角速度信息，根据所述初始角速度信息和实时角速度信息，确定所述底盘的姿态，所述实时角速度信息通过惯性传感器获取。

3.根据权利要求2所述的抓钢机定位控制方法，其特征在于，所述初始角速度信息包括：初始状态下所述底盘在参考坐标系上的角度信息，所述角度信息包括：(θ_x0,θ_y0,θ_z0),其中，θ_x0、θ_y0、θ_z0分别为初始状态下所述底盘与所述参考坐标系的x、y、z轴的夹角；所述实时角速度信息包括：步长采样时间、采样时惯性传感器在所述参考坐标系上的x、y、z轴上的角速度。

4.根据权利要求3所述的抓钢机定位控制方法，其特征在于，根据所述初始角速度信息和实时角速度信息，确定所述底盘的姿态的步骤的数学表达为：

其中，t_n为所述步长采样时间，θ′_xn、θ′_yn、θ′_zn分别为所述采样时惯性传感器在参考坐标系上的x、y、z轴上的角速度，θ_x0、θ_y0、θ_z0分别为初始状态下所述底盘与所述参考坐标系的x、y、z轴的夹角，θ_xk、θ_yk、θ_zk分别为实时状态下所述底盘与所述参考坐标系的x、y、z轴的夹角。