[发明专利]一种爬壁机器人防倾覆行进控制方法

权利要求书

1.一种爬壁机器人防倾覆行进控制方法，其特征在于，它包括如下步骤：

步骤1：定义爬壁机器人固连坐标系，其中坐标系原点位于系统中心，爬壁机器人直行前进方向为+x方向，垂直于爬壁机器人底盘平面向上方向为+z方向，+y方向根据右手螺旋法确定；

步骤2：爬壁机器人应装配1个三轴倾角仪或3个单轴倾角仪，测量轴分别与爬壁机器人坐标系x轴、y轴和z轴平行，并依次根据测量轴的安装方向初始化测量轴方向标识d_x，d_y和d_z：

若x轴对应的测量轴指向机器人坐标轴+x方向，则令d_x＝0，反之则令d_x＝1；若y轴对应的测量轴指向机器人坐标轴+y方向，则令d_y＝0，反之则令d_y＝1；若z轴对应的测量轴指向机器人坐标轴+z方向，则令d_z＝0，反之则令d_z＝1；

步骤3：将爬壁机器人水平或垂直放置在检测壳体上，然后执行爬壁机器人初始化步骤；

步骤4：在人为放置爬壁机器人的基础上，利用测量数据对爬壁机器人的初始行进方向进行调整，在完成爬壁机器人的初始调向后，爬壁机器人静止T秒钟，其中T为倾角仪校准时间，读取爬壁机器人z轴方向倾角仪测量轴的经卡尔曼滤波后的倾角测量值θ_z0并记录；

步骤5：在爬壁机器人完成初始行进方向对准的基础上，启动爬壁机器人的行进，且在行进过程中保持行进方向，同时完成爬壁机器人的倾覆检测；

步骤6：若需调整爬壁机器人行进方向，则执行本步骤。

2.如权利要求1所述的爬壁机器人防倾覆行进控制方法，其特征在于，爬壁机器人初始化包括如下步骤：

(1)检测爬壁机器人x轴向对应倾角仪测量轴与重力方向夹角α，y轴向对应倾角仪测量轴与重力方向夹角β；

(2)分别判断α和β数值，确定机器人初始朝向；

若|α|＜e，则机器人前进方向应为沿壳体向下；

若|α-π|＜e，则机器人前进方向应为沿壳体向上；

若|β|＜e或|β-π|＜e，则机器人前进方向应为沿壳体水平前进；

其中，e为初始对齐判定阈值，其取值范围为e∈(0，π/4)。

3.如权利要求2所述的爬壁机器人防倾覆行进控制方法，其特征在于，对爬壁机器人的初始行进方向进行调整包括如下步骤：

若|α|＜e，则调整机器人行进方向，使得|α|＜ε；

若|α-π|＜e，则调整机器人行进方向，使得|α-π|＜ε；

若|β|＜e，则调整机器人行进方向，使得|β|＜ε；

若|β-π|＜e，则调整机器人行进方向，使得|β-π|＜ε；

其中，ε为倾角仪测量分辨率。

4.如权利要求1所述的爬壁机器人防倾覆行进控制方法，其特征在于，若爬壁机器人行进方向为沿壳体向上或向下，则爬壁机器人的倾覆检测通道为沿爬壁机器人坐标系y轴的测量轴，执行以下步骤：

(a)令初始时刻t＝0，读取机器人y轴的测量轴的倾角测量数据θ_y(t)并记录；

(b)令t＝t+1；

(c)控制机器人行进，在此过程中，读取机器人y轴的测量轴的倾角测量数据θ_y(t)并记录，同时读取机器人z轴的测量轴的倾角测量数据θ_z(t)，行进过程中，控制机器人的行进方向使得|θ_z(t)-θ_z0|＜ε保持行进方向；

(d)若|θ_y(t)-θ_y(t-1)|＞θ_trig，则认为机器人发生倾覆，启动防倾覆装置，在防倾覆装置的作用下，重新将机器人恢复或将机器人安全停止在当前位置；若|θ_y(t)-θ_y(t-1)|＜θ_trig，则认为机器人未发生倾覆，返回(c)，其中θ_trig为倾角触发值。