[发明专利]一种移动机器人控制系统故障诊断方法

权利要求书

1.一种移动机器人控制系统故障诊断方法，所述移动机器人为前轮转向、后轮驱动的系统；故障模式包括正常、前轮转向系统故障、后轮驱动系统故障、前轮转向和后轮驱动系统同时故障等四种，利用机载二维激光雷达对移动机器人实际运行参数进行估计，通过构建残差特征以及阈值逻辑进行故障诊断；具体实现步骤如下：

输入：从1时刻到T时刻的前轮转向角控制量，后轮驱动速度控制量，激光雷达扫描，其中，前轮转向和后轮驱动控制量的频率是激光雷达扫描频率的a倍，,是机器人开始运动前的激光雷达扫描；

输出：1时刻到Ka-b时刻的故障状态，其中b为根据移动机器人动力学模型确定的移动机器人控制量延时的时间片数目；

步骤1 ：初始化，设置参数k=1, L, ，a，b，,,其中L是移动机器人前轮到后轮之间的距离，单位为米，为前后两个时刻的时间间隔，该时间间隔也称时间片，单位为秒，a为前轮转向和后轮驱动控制量的频率与激光雷达扫描频率的比值，b表示移动机器人控制量延时的时间片数目, 为转向系统故障阈值，为驱动系统故障阈值；

步骤2：设表示ka时刻机器人的位姿，其中，,,，根据移动机器人运动学模型以及，预测ka时刻移动机器人的位姿，具体做法如下：

t=max((k-1)a-b,1);

,,，=0，=0；

while(tka-b)

{

；

；

；

；

；

；

t=t+1；

}

此时，即为ka时刻移动机器人的位姿的估计；

步骤3：根据所求ka时刻位姿的估计计算ka时刻移动机器人相对于(k-1)a时刻移动机器人的位姿变换初值，其中

；

；

；

步骤4： 根据步骤3所得计算(k-1)a时刻到ka时刻时间段期望的平均驱动速度和平均转向角；

步骤5：对以初值表示的为初始估计获得的平移向量和旋转角度进行坐标变换得到点集，利用迭代最近点方法对和进行配准，配准结果为；

步骤6：根据配准结果计算实际的平均驱动速度和平均转向角；

步骤7：故障诊断，如果且，则为正常；如果且，则为前轮转向系统故障；如果且，则为后轮驱动系统故障；如果且，则为前轮转向和后轮驱动系统同时故障；其中；

步骤8：根据配准结果校正ka时刻的位姿估计，

其中，，

；

步骤9：k=k+1,如果k=,转步骤2，否则结束。