[发明专利]一种机器人在矩形斜坡上行驶的路径导航控制方法

权利要求书

1.一种机器人在矩形斜坡上行驶的路径导航控制方法，其特征在于，包括如下步骤：

步骤S401）设定所述矩形斜坡的右下角为导航起点；

步骤S402）控制所述机器人从所述导航起点向所述矩形斜坡的右上角直线行驶；

步骤S403）检测所述机器人是否行进至所述矩形斜坡的第一角落；若所述机器人未到达所述第一角落，返回步骤S402）；若所述机器人到达所述第一角落，控制所述机器人向左进行U字回转，回转后控制所述机器人直线行驶；

步骤S404）检测所述机器人是否行进至所述矩形斜坡的边缘处，若所述机器人到达所述矩形斜坡的一边缘处；控制所述机器人向右进行U字回转，回转后控制所述机器人直线行驶，执行步骤S405）；以及

步骤S405）检测所述机器人是否行进至所述矩形斜坡的边缘处，若所述机器人到达所述矩形斜坡的一边缘处；控制所述机器人向左进行U字回转，回转后控制所述机器人直线行驶；返回步骤S404）；

在步骤S404）和步骤S405）的执行过程中，实时检测所述机器人是否行进至所述矩形斜坡的第二角落；若所述机器人到达所述第二角落，实时检测所述机器人是否行进至所述矩形斜坡的第三角落；若所述机器人到达所述第三角落，控制所述机器人停止行驶；

其中，控制所述机器人直线行驶的步骤，包括

判断一机器人是否沿着预设的直线路径行驶的步骤，具体包括如下步骤：

步骤S1）在所述机器人上建立三维坐标系，定义所述机器人行进方向为Y轴正方向，定义垂直于斜坡平面的方向为Z轴方向，X轴垂直于所述Y轴及所述Z轴；所述X轴与所述Y轴所处平面与所述斜坡平面平行，所述斜坡平面为所述矩形斜坡所处平面；

步骤S2）定义所述机器人行进方向为Ts时，重力加速度g在所述三维坐标系三个方向上的标准分向量g_xs0、g_ys0、g_zs0；

步骤S3）生成一标准方向参数库；

步骤S4）控制所述机器人在所述斜坡平面上沿着预设的一直线路径向任一方向Tm直线行驶；

步骤S5）从所述标准方向参数库中调取对应该行进方向Tm的标准分向量g_xm0、g_ym0、g_zm0数据；

步骤S6）每隔一定时间间隔t实时采集一组实时方向参数，所述实时方向参数包括重力加速度g在所述三维坐标系三个方向上的实时分向量g_xm1、g_ym1、g_zm1；

步骤S7）计算重力加速度g在所述X轴方向上的实时分向量与标准分向量的分向量差值g_xd =g_xm1 -g_xm0 ；

步骤S8）判定所述机器人是否沿着预设的直线路径行驶；当g_xd等于0时，判定所述机器人沿着预设的直线路径行驶，返回步骤S106）；当g_xd不等于0时，判定所述机器人偏离预设的直线路径。

2.如权利要求1所述的机器人在矩形斜坡上行驶的路径导航控制方法，其特征在于，检测所述机器人是否行进至所述矩形斜坡的一角落或一边缘处，具体包括如下步骤：

步骤S1011）在所述机器人的左前部、右前部、左后部及右后部分别设置一距离传感器，所述距离传感器延伸至所述机器人的外部，所述距离传感器朝向所述矩形斜坡；

步骤S1012）依次为四个距离传感器编号，将所述机器人的左前部、右前部、左后部及右后部设置的距离传感器分别定义为传感器N1、传感器N2、传感器N3及传感器N4；

步骤S1013）所述机器人根据任一时刻同时获取的传感器信号判断所述机器人的位置；当所述机器人同时获取传感器N3信号和传感器N4信号时，判定所述机器人到达所述矩形斜坡的一边缘处；当所述机器人只能获取传感器N4信号时，判定所述机器人到达所述矩形斜坡的第一角落或第二角落；当所述机器人只能获取传感器N3信号时，判定所述机器人到达所述矩形斜坡的第三角落或第四角落。