[发明专利]一种缆索检测机器人的二自由度运动控制方法

权利要求书

1.一种缆索检测机器人的二自由度运动控制方法，其特征在于：包括如下步骤：

步骤1、建立坐标系：缆索检测机器人为四旋翼检测机器人，具有四个旋翼；坐标系包括地球坐标系E(X,Y,Z)和检测机器人坐标系e(x,y,z)；其中，地球坐标系E(X,Y,Z)的原点O为地心，X轴为过地心的水平方向，Y轴为过地心且垂直于X轴的水平方向，Z轴为过地心的竖直方向；检测机器人坐标系e(x,y,z)中的原点o为缆索检测机器人的对称中心，z轴为过原点o且与缆索轴向相平行的轴，x-y平面平行于四个旋翼所在平面；

步骤2、建立转速模型：在检测机器人坐标系中，对每个旋翼电机均建立一个转速模型，转速模型为旋翼电机转速与输入电流之间的函数方程，具体表达式为：

式中，T_m为电机响应时间常数；为当前t时刻时的旋翼电机转速；σ(t)为当前t时刻时的输入电流大小；为稳态转速；σ_c为稳态转速时的输入电流值；C_R为与σ_c的比值；为偏移系数，反映电流零输入条件下电机的偏移转速；

步骤3、求解转速：将四个旋翼所对应的电机输入电流分别代入公式(1)中，从而得到四个旋翼的转速，分别为和

步骤4、建立控制效率模型：在检测机器人坐标系中，根据步骤2建立的转速模型，对缆索检测机器人建立控制效率模型；控制效率模型包括升力模型、以及x、y和z轴上的力矩模型，具体表示为：

式中，f为旋翼升力；c_T、c_M表征了电机转速和各轴上力矩的关系，在具体实验环境下测得；T_x、T_y和T_z分别为缆索检测机器人在x、y和z轴上的旋转力矩；d为旋翼电机与缆索中心轴间的距离；

步骤5、建立姿态模型：在检测机器人坐标系中，根据欧拉方程，对缆索检测机器人建立力矩与角速度的姿态模型为：

式(3)中，J_xx、J_yy和J_zz分别为缆索检测机器人在x、y和z轴上的转动惯量；ω_x、ω_y和ω_z分别为缆索检测机器人分别在x、y和z轴上的姿态角速度；为ω_x的一阶导数；为ω_y的一阶导数；为ω_z的一阶导数；

步骤6、求解姿态角速度：将步骤3求解的四个旋翼的转速，代入公式(2)中，求解得到T_x、T_y和T_z，再将得到的T_x、T_y和T_z代入公式(3)中，进行求解，从而得到ω_x、ω_y和ω_z；

步骤7、坐标转换：将步骤4求解得到的ω_x、ω_y和ω_z，分别转换至地球坐标系，得到地球坐标系下对应的姿态角速度ω_x′、ω_y′和ω_z′；

步骤8、二自由度控制：通过控制四个旋翼电机的转速，实现缆索检测机器人在地球坐标系下姿态角速度ω_x′、ω_y′和ω_z′的调整，进而实现缆索检测机器人在缆索轴向和周向上二自由度的控制。

2.根据权利要求1所述的缆索检测机器人的二自由度运动控制方法，其特征在于：缆索为光滑缆索或螺旋线缆索。

3.根据权利要求1所述的缆索检测机器人的二自由度运动控制方法，其特征在于：步骤7中，坐标转换公式为：

式中，θ为检测机器人坐标系下缆索检测机器人的俯仰角；φ为检测机器人坐标系下缆索检测机器人的横滚角。

4.根据权利要求3所述的缆索检测机器人的二自由度运动控制方法，其特征在于：θ和φ的计算公式分别为：θ＝90°-α；φ＝90°，α为缆索倾斜角度。