自主导航控制方法和装置与流程

本发明涉及机器人技术领域，特别涉及自主导航控制方法和装置。

背景技术：

基于机器视觉的自主导航技术已经应用于室外移动机器人，其中基于视觉传感器检测与识别路面车道线来控制移动载体保持在车道内行驶的技术目前大量应用于无人驾驶汽车。

但是室外复杂多变的光照条件及路面车道线的清晰程度将极大影响车道线检测与识别结果的准确性。

技术实现要素：

本发明的主要目的是提供一种自主导航控制方法和装置，旨在提高车道线检测与识别结果的准确性。

为实现上述目的，本发明提出的一种自主导航控制方法，用于控制室外移动的机器人沿着道路行驶，所述自主导航控制方法包括如下步骤：

在机器人向前运动时，获得视觉传感器采集的所述机器人航向前方的道路图片；

根据所述道路图片获得机器人的姿态数据；

将当前所述姿态数据和正常的上一姿态数据对比获得数据差值，并且根据所述数据差值和预设的准确性阈值来判断当前所述姿态数据是否正常；

在判定为正常时，根据当前所述姿态数据获得转向控制指令，用以控制机器人沿着道路行驶。

优选的，所述姿态数据包括所述机器人分别与道路的左车道线的左距离D_L、与道路的右车道线的右距离D_R，以及与车道线的角度偏差θ。

优选的，所述准确性阈值包括倾斜跳变夹角偏差阈值Δθ，以及平移跳变距离偏差阈值ΔD_L；

所述根据所述数据差值和预设的准确性阈值来判断当前所述姿态数据是否正常的步骤具体包括：

在θ_n-θ_n-1＞Δθ时，判定当前所述姿态数据为倾斜跳变数据；

在D_L_n-D_L_n-1＞ΔD_L时，判定当前所述姿态数据为平移跳变数据；

否则，判定当前所述姿态数据为正常数据。

优选的，所述根据当前所述姿态数据获得转向控制指令的步骤具体包括：

根据所述左距离D_L、右距离D_R获得道路的车道中线；

定位预瞄点位于所述车道中线，并且根据预设参数而定位所述预瞄点与机器人在沿着道路方向上的距离为瞄点距离dis_goal；

根据所述预瞄点以及角度偏差θ获得指向所述预瞄点的转向角度β；

根据转向角度β生成转向控制指令。

优选的，所述自主导航控制方法还包括如下步骤：

预设左车道线到右侧路牙距离为左限距离L_ROAD，右车道线到右侧路牙距离为右限距离R_ROAD；

通过测距传感器获得所述机器人与道路右侧路牙的距离为测量距离d_road；

当d_road<R_ROAD时，则判定机器人压到了右车道线，并且生成左转控制指令；

当d_road>L_ROAD时，则判定机器人压到了右车道线，并且生成右转控制指令。

本发明还提供了一种自主导航控制装置，用于控制室外移动的机器人沿着道路行驶，所述自主导航控制装置包括：

图片获得模块，用于在机器人向前运动时，获得视觉传感器采集的所述机器人航向前方的道路图片；

视觉处理模块，用于根据所述道路图片获得机器人的姿态数据；

滤波模块，用于将当前所述姿态数据和正常的上一姿态数据对比获得数据差值，并且根据所述数据差值和预设的准确性阈值来判断当前所述姿态数据是否正常；

控制决策模块，用于在所述滤波模块判定当前所述姿态数据为正常时，根据当前所述姿态数据获得转向控制指令，用以控制机器人沿着道路行驶。

优选的，所述姿态数据包括所述机器人分别与道路的左车道线的左距离D_L、与道路的右车道线的右距离D_R，以及与车道线的角度偏差θ。

优选的，所述准确性阈值包括倾斜跳变夹角偏差阈值Δθ，以及平移跳变距离偏差阈值ΔD_L；

所述滤波模块，用于根据所述数据差值和预设的准确性阈值来判断当前所述姿态数据是否正常的步骤具体包括：

在θ_n-θ_n-1＞Δθ时，判定当前所述姿态数据为倾斜跳变数据；

在D_L_n-D_L_n-1＞ΔD_L时，判定当前所述姿态数据为平移跳变数据；

否则，判定当前所述姿态数据为正常数据。

优选的，所述控制决策模块，用于根据当前所述姿态数据获得转向控制指令的步骤具体包括：

根据所述左距离D_L、右距离D_R获得道路的车道中线；

定位预瞄点位于所述车道中线，并且根据预设参数而定位所述预瞄点与机器人在沿着道路方向上的距离为瞄点距离dis_goal；

根据所述预瞄点以及角度偏差θ获得指向所述预瞄点的转向角度β；

根据转向角度β生成转向控制指令。

优选的，所述自主导航控制装置还包括：

预设模块，用于预设左车道线到右侧路牙距离为左限距离L_ROAD，右车道线到右侧路牙距离为右限距离R_ROAD；

测距模块，用于通过测距传感器获得所述机器人与道路右侧路牙的距离为测量距离d_road；

所述控制决策模块还用于，当d_road<R_ROAD时，则判定机器人压到了右车道线，并且生成左转控制指令；当d_road>L_ROAD时，则判定机器人压到了右车道线，并且生成右转控制指令。

优选的，所述测距传感器的数量为一个，安装于所述机器人本体的右侧面，并且位于中间位置；或者

所述测距传感器的数量为两个，安装于所述机器人本体的右侧面，并且分别位于前后位置。

本发明所提供的自主导航控制方法，通过将前后两帧画面获得的姿态数据进行对比，从而将误差较大的数据舍弃，则提高车道线检测与识别结果的准确性，以及当前姿态数据的准确性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

图1为本发明自主导航控制方法第一实施例的流程图；

图2为如图1所示的自主导航控制方法的步骤S103的流程示意图；

图3为如图1所示的自主导航控制方法的步骤S104的流程示意图；

图4为本发明自主导航控制方法第二实施例的流程图；

图5为图4所示自主导航控制方法的坐标系示意图；

图6为图4所示自主导航控制方法的第一种情况的示意图；

图7为图4所示自主导航控制方法的第二种情况的示意图；

图8为图4所示自主导航控制方法的第三种情况的示意图；

图9为图4所示自主导航控制方法的第四种情况的示意图；

图10为图4所示自主导航控制方法的第五种情况的示意图；

图11为图4所示自主导航控制方法的第六种情况的示意图；

图12为图4所示自主导航控制方法测距传感器安装位置的正视示意图；

图13为图4所示自主导航控制方法测距传感器安装位置的俯视示意图；

图14为本发明自主导航控制方法其他实施例中测距传感器安装位置的示意图；

图15为本发明自主导航控制装置一实施例的模块示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

请参看图1，本发明自主导航控制方法第一实施例，用于控制室外移动的机器人沿着道路行驶，所述自主导航控制方法包括如下步骤：

步骤S100，在机器人向前运动时，获得视觉传感器采集的所述机器人航向前方的道路图片。

步骤S101，根据所述道路图片获得机器人的姿态数据。其中，姿态数据可以包括：当前机器人航向、车道线的方向、当前航向与车道线的夹角、预设道路图片位置处航向线与车道线的距离等等。

步骤S102，将当前所述姿态数据和正常的上一姿态数据对比获得数据差值。

步骤S103，根据所述数据差值和预设的准确性阈值来判断当前所述姿态数据是否正常；在判定为正常时，执行步骤S104；否则，执行步骤S105。其中，由于车道线一般为平滑过渡，因此可以通过判断前后两帧画面获得的姿态数据相差是否处于合理的预设阈值以外，来排除错误数据或误差大数据。

步骤S104，根据当前所述姿态数据获得转向控制指令，用以控制机器人沿着道路行驶。

步骤S105，舍弃当前所述姿态数据。

本实施例，通过将前后两帧画面获得的姿态数据进行对比，从而将误差较大的数据舍弃，则提高车道线检测与识别结果的准确性，以及当前姿态数据的准确性。

请结合参看图5和图6，优选的，所述姿态数据包括所述机器人本体1的坐标系原点11分别与道路的左车道线41的左距离D_L、与道路的右车道线42的右距离D_R，以及与车道线的角度偏差θ。

通过左距离D_L和右距离D_R，可以获知机器人当前处于车道内靠右还是靠左。例如，当D_L＞D_R时，判定机器人靠左；当D_L＜D_R，判定机器人靠右。机器人可以通过转向来使得D_L＝D_R，达到靠道路中间行驶的效果。

进一步，通过角度偏差θ可以获知机器人当前航向线与道路方向的偏差程度，因此可以通过角度偏差θ来控制转向控制指令当中的转向角度；例如当角度偏差θ越大，则转向角度越大，从而达到尽快调整航向到位的效果。在本实施例中，设定机器人本体坐标系原点位于所述道路图片中心，Y轴与机器人航向线重合并且指向机器人前进方向为正方向，X轴按照右手法则定义；角度偏差θ定义为X轴与车道线的法线的夹角；当然，在其他实施例中，角度偏差θ也可以是Y轴与车道线的夹角，上述两个夹角的数值相等。

优选的，预设通过道路图片获得的姿态数据划分为三类，分别为正常数据、倾斜跳变数据、平移跳变数据。正常数据的定义为：准确可靠的数据；倾斜跳变数据的定义为：后一帧数据相对于前一帧正常数据的角度偏差θ超过设定阈值；平移跳变数据的定义为：后一帧数据相对于前一帧正常数据的横向距离偏差ΔD_L超过设定阈值。

本实施例中，定义所述准确性阈值包括倾斜跳变夹角偏差阈值Δθ，以及平移跳变距离偏差阈值ΔD_L。

请结合参看图2，所述步骤103，根据所述数据差值和预设的准确性阈值来判断当前所述姿态数据是否正常的步骤具体包括：

步骤1031，判断是否θ_n-θ_n-1＞Δθ，若是，则执行步骤S1032；若否，则执行步骤S1033。

步骤S1032，判定当前所述姿态数据为倾斜跳变数据；

步骤S1033，判断是否D_L_n-D_L_n-1＞ΔD_L，若是，则执行步骤S1034；若否，则执行步骤S1035。

步骤S1034，判定当前所述姿态数据为平移跳变数据；

步骤S1035，判定当前所述姿态数据为正常数据。

其中θ_n为第n帧输出的机器人航向与车道线方向的夹角偏差值，ΔD_L为第n帧输出的机器人与左车道线41或者与右车道线42横向距离，n＝1,2,3…。

本实施例，经过上述处理后，绝大部分跳变的数据都能够被准确的过滤掉，从而保证后续基于该数据进行控制决策结果的可靠性。

请再次参看图5和图6，优选的，所述步骤104，根据当前所述姿态数据获得转向控制指令的步骤具体包括：

步骤1041，根据所述左距离D_L、右距离D_R获得道路的车道中线8；

步骤1042，定位预瞄点12位于所述车道中线8，并且根据预设参数而定位所述预瞄点12与机器人在沿着道路方向上的距离为瞄点距离dis_goal；其中，预设参数，例如根据机器人行驶速度、道路宽度等参数；当速度快、车道宽时，则瞄点距离dis_goal可以较大，否则较小。

步骤1043，根据所述预瞄点12以及角度偏差θ获得指向所述预瞄点12的转向角度β；

步骤1044，根据转向角度β生成转向控制指令。

本实施例，通过计算车道中线8，以及瞄点距离dis_goal可以获得预瞄点12，并且通过预瞄点获得转向角度β，则能够获得较为平滑的转向效果。具体所述角度偏差θ为以车道线的法线起，逆时针旋转至X轴获得，其范围为0°～180°，转向角度β的计算方法可以如下：

如图6所示为第一种情况，机器人在车道中线右侧，机器人车头朝右，机器人需要左转调整行驶方向，具体的左转角度β值的计算如下：

β＝90°-α+θ；

其中，α的计算公式为：α＝arctan(dis_goal/((D_L+D_R)×0.5))

如图7所示为第二种情况，机器人在车道中线右侧，机器人车头朝左，机器人需要左转调整行驶方向，具体的左转角度β值的计算如下：

β＝θ-90°-α；

如图8所示为第三种情况，机器人在车道中线右侧，机器人车头朝左，机器人需要右转调整行驶方向，具体的左转角度β值的计算如下：

β＝90°+α-θ；

如图9所示为第四种情况，机器人在车道中线左侧，机器人车头朝左，机器人需要右转调整行驶方向，具体的左转角度β值的计算如下：

β＝270°-α-θ；

如图10所示为第五种情况，机器人在车道中线左侧，机器人车头朝右，机器人需要右转调整行驶方向，具体的左转角度β值的计算如下：

β＝90°-α-θ；

如图11所示为第六种情况，机器人在车道中线左侧，机器人车头朝右，机器人需要左转调整行驶方向，具体的左转角度β值的计算如下：

β＝α-90°+θ；

上述六种情况根据D_L与D_R的比较来判断机器人位于车道中线的左侧还是右侧：如果D_L＞D_R，则机器人位于车道中线右侧；如果D_L<D_R，则机器人位于车道中线左侧。

上述六种情况根据θ值来判断机器人车头是朝向左还是朝向右：如果θ＞90度，则机器人朝向左；如果θ＜90度，则机器人朝向右。

针对上述六种情况的β大小，控制决策可以通过查表法分别转换为对应大小的转向角度，从而实现了根据车道线与机器人的相对位置关系对机器人运动的转向控制。

请结合参看图4、图12和图13，本发明自主导航控制方法第二实施例，本实施例以第一实施例为基础，新增了步骤。具体如下：

步骤S200，预设左车道线41到右侧路牙2距离为左限距离L_ROAD，右车道线42到右侧路牙2距离为右限距离R_ROAD；

步骤S201，通过测距传感器5获得所述机器人本体1与道路右侧路牙2的距离为测量距离d_road；

步骤S202，判断是否d_road<R_ROAD，若是，则执行步骤S203；若否，则执行步骤S204；

步骤S203，判定机器人压到了右车道线42，并且生成左转控制指令。

步骤S204，判断是否d_road>L_ROAD，若是，则执行步骤S205，若否，则执行步骤S206。

步骤S205，判定机器人压到了右车道线42，并且生成右转控制指令。

步骤S206，与第一实施例的步骤S100相同，在此不再赘述。

步骤S207，与第一实施例的步骤S101相同，在此不再赘述。

步骤S208，与第一实施例的步骤S102相同，在此不再赘述。

步骤S209，与第一实施例的步骤S103相同，在此不再赘述。

步骤S210，与第一实施例的步骤S104相同，在此不再赘述。

步骤S211，与第一实施例的步骤S105相同，在此不再赘述。

本实施例，通过接收来自测距传感器5发送来的数据，据此判断机器人是否跑出自身行驶的车道，并且控制机器人在车道线内行驶，不会撞到路牙。

请参看图15，本发明还提供了一种自主导航控制装置，用于控制室外移动的机器人沿着道路行驶，所述自主导航控制装置1000包括：

图片获得模块1100，用于在机器人向前运动时，获得视觉传感器采集的所述机器人航向前方的道路图片；

视觉处理模块1200，用于根据所述道路图片获得机器人的姿态数据；其中，姿态数据可以包括：当前机器人航向、车道线的方向、当前航向与车道线的夹角、预设道路图片位置处航向线与车道线的距离等等。

滤波模块1300，用于将当前所述姿态数据和正常的上一姿态数据对比获得数据差值，并且根据所述数据差值和预设的准确性阈值来判断当前所述姿态数据是否正常；其中，由于车道线一般为平滑过渡，因此可以通过判断前后两帧画面获得的姿态数据相差是否处于合理的预设阈值以外，来排除错误数据或误差大数据。

控制决策模块1400，用于在所述滤波模块1300判定当前所述姿态数据为正常时，根据当前所述姿态数据获得转向控制指令，用以控制机器人沿着道路行驶。

本实施例，通过将前后两帧画面获得的姿态数据进行对比，从而将误差较大的数据舍弃，则提高车道线检测与识别结果的准确性，以及当前姿态数据的准确性。

请结合参看图5和图6，优选的，所述姿态数据包括所述机器人本体1的坐标系原点11分别与道路的左车道线41的左距离D_L、与道路的右车道线42的右距离D_R，以及与车道线的角度偏差θ。通过左距离D_L和右距离D_R，可以获知机器人当前处于车道内靠右还是靠左。例如，当D_L＞D_R时，判定机器人靠左；当D_L＜D_R，判定机器人靠右。机器人可以通过转向来使得D_L＝D_R，达到靠道路中间行驶的效果。

进一步，通过角度偏差θ可以获知机器人当前航向线与道路方向的偏差程度，因此可以通过角度偏差θ来控制转向控制指令当中的转向角度；例如当角度偏差θ越大，则转向角度越大，从而达到尽快调整航向到位的效果。在本实施例中，设定机器人本体坐标系原点位于所述道路图片中心，Y轴与机器人航向线重合并且指向机器人前进方向为正方向，X轴按照右手法则定义；角度偏差θ定义为X轴与车道线的法线的夹角；当然，在其他实施例中，角度偏差θ也可以是Y轴与车道线的夹角，上述两个夹角的数值相等。

优选的，预设通过道路图片获得的姿态数据划分为三类，分别为正常数据、倾斜跳变数据、平移跳变数据。正常数据的定义为：准确可靠的数据；倾斜跳变数据的定义为：后一帧数据相对于前一帧正常数据的角度偏差θ超过设定阈值；平移跳变数据的定义为：后一帧数据相对于前一帧正常数据的横向距离偏差ΔD_L超过设定阈值。

本实施例中，定义所述准确性阈值包括倾斜跳变夹角偏差阈值Δθ，以及平移跳变距离偏差阈值ΔD_L；

请结合参看图2，所述滤波模块1300，用于根据所述数据差值和预设的准确性阈值来判断当前所述姿态数据是否正常的步骤具体包括：

在θ_n-θ_n-1＞Δθ时，判定当前所述姿态数据为倾斜跳变数据；

在D_L_n-D_L_n-1＞ΔD_L时，判定当前所述姿态数据为平移跳变数据；

否则，判定当前所述姿态数据为正常数据。

其中θ_n为第n帧输出的机器人航向与车道线方向的夹角偏差值，ΔD_L为第n帧输出的机器人与左车道线41或者与右车道线42横向距离，n＝1,2,3…。

本实施例，经过上述处理后，绝大部分跳变的数据都能够被准确的过滤掉，从而保证后续基于该数据进行控制决策结果的可靠性。

请再次参看图5和图6，优选的，所述控制决策模块1400，用于根据当前所述姿态数据获得转向控制指令的步骤具体包括：

根据所述左距离D_L、右距离D_R获得道路的车道中线8；

定位预瞄点12位于所述车道中线8，并且根据预设参数而定位所述预瞄点12与机器人在沿着道路方向上的距离为瞄点距离dis_goal；其中，预设参数，例如根据机器人行驶速度、道路宽度等参数；当速度快、车道宽时，则瞄点距离dis_goal可以较大，否则较小。

根据所述预瞄点12以及角度偏差θ获得指向所述预瞄点12的转向角度β；

根据转向角度β生成转向控制指令。

本实施例，通过计算车道中线8，以及瞄点距离dis_goal可以获得预瞄点12，并且通过预瞄点获得转向角度β，则能够获得较为平滑的转向效果。具体所述角度偏差θ为以车道线的法线起，逆时针旋转至X轴获得，其范围为0°～180°，转向角度β的计算方法可以如下：

如图6所示为第一种情况，机器人在车道中线右侧，机器人车头朝右，机器人需要左转调整行驶方向，具体的左转角度β值的计算如下：

β＝90°-α+θ；

其中，α的计算公式为：α＝arctan(dis_goal/((D_L+D_R)×0.5))

如图7所示为第二种情况，机器人在车道中线右侧，机器人车头朝左，机器人需要左转调整行驶方向，具体的左转角度β值的计算如下：

β＝θ-90°-α；

如图8所示为第三种情况，机器人在车道中线右侧，机器人车头朝左，机器人需要右转调整行驶方向，具体的左转角度β值的计算如下：

β＝90°+α-θ；

如图9所示为第四种情况，机器人在车道中线左侧，机器人车头朝左，机器人需要右转调整行驶方向，具体的左转角度β值的计算如下：

β＝270°-α-θ；

如图10所示为第五种情况，机器人在车道中线左侧，机器人车头朝右，机器人需要右转调整行驶方向，具体的左转角度β值的计算如下：

β＝90°-α-θ；

如图11所示为第六种情况，机器人在车道中线左侧，机器人车头朝右，机器人需要左转调整行驶方向，具体的左转角度β值的计算如下：

β＝α-90°+θ；

上述六种情况根据D_L与D_R的比较来判断机器人位于车道中线的左侧还是右侧：如果D_L＞D_R，则机器人位于车道中线右侧；如果D_L<D_R，则机器人位于车道中线左侧。

上述六种情况根据θ值来判断机器人车头是朝向左还是朝向右：如果θ＞90度，则机器人朝向左；如果θ＜90度，则机器人朝向右。

针对上述六种情况的β大小，控制决策可以通过查表法分别转换为对应大小的转向角度，从而实现了根据车道线与机器人的相对位置关系对机器人运动的转向控制。

请结合参看图4、图12和图13，优选的，所述自主导航控制装置1000还包括：

预设模块1500，用于预设左车道线41到右侧路牙2距离为左限距离L_ROAD，右车道线42到右侧路牙2距离为右限距离R_ROAD；

测距模块1600，用于通过测距传感器5获得所述机器人与道路右侧路牙2的距离为测量距离d_road；

所述控制决策模块1400还用于，当d_road<R_ROAD时，则判定机器人压到了右车道线42，并且生成左转控制指令；当d_road>L_ROAD时，则判定机器人压到了右车道线42，并且生成右转控制指令。

本实施例，通过接收来自测距传感器5发送来的数据，据此判断机器人是否跑出自身行驶的车道，并且控制机器人在车道线内行驶，不会撞到路牙。

请参看图12，优选的，测距传感器5的安装高度及角度要求测距传感器的发射信号能够射到路牙2上，且建议刚好能够射到路牙2的中间位置，且不会射到路面3上。

本实施例中，所述测距传感器5的数量为一个，安装于所述机器人本体1的右侧面，并且位于中间位置。

请参看图14，在其他实施例中，所述测距传感器5的数量可以为两个，测距传感器5和测距传感器6，安装于所述机器人本体1的右侧面，并且分别分别位于前后位置。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是移动终端，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。