CN109765901A - 基于线激光与双目视觉的动态代价地图导航方法 - Google Patents

Abstract

本发明公开了一种基于线激光与双目视觉的动态代价地图导航方法，包括步骤：1)建立二维栅格地图模型；2)建立移动机器人位姿模型；3)建立激光雷达数据模型；4)标定双目摄像头和获取三维坐标；5)使用步骤4)获取的三维坐标创建动态代价地图；6)利用dwa算法进行路径规划与避障导航。本发明能够通过双目测距创建代价地图，感知二维激光雷达不能探测的障碍物；能够在已知环境二维栅格地图环境下，使用双目摄像头探测环境中高于和低于二维激光雷达平面的障碍物，并且将障碍物映射到环境地图中，通过dwa算法，得到避开障碍物的导航路径线，可以使移动机器人在运动过程中沿着避开障碍的路径到达目的地。

Description

基于线激光与双目视觉的动态代价地图导航方法

技术领域

本发明涉及机器人导航避障的技术领域，尤其是指一种基于线激光与双目视觉的动态代价地图导航方法。

背景技术

随着智能机器人技术蓬勃发展，机器人在生产与生活中有了越来越广泛的应用。根据应用环境的不同，机器人可以被分为多种类别，比如说工业机器人往往有着多个关节和机械臂，其在工厂当中承担着装配、搬运等高效率的工作；而服务机器人产品，包括轮式移动机器人、仿人机器人等可以被用在家用或者商用的环境中，针对不同使用目的开发出不同功能，智能移动小车可以实现无人驾驶，仿人机器人可以实现唱歌跳舞娱乐互动等等；无人机常常用于军事侦察，遥感测绘等；还有水下机器人、纳米机器人、农业机器人等更多类别的特种机器人，都有着其比较专一的用途。其中随着推动移动机器人发展的激光传感器技术的不断发展和突破，激光传感器在移动机器人领域得到了广泛的应用。在机器人对未知环境的探索，创建地图，以及导航避障上，激光有着不可取代的重要地位。目前市面上广泛使用且成本低的主要是二维激光，但是它的平面性使得机器人探测不到低于机器人平面的障碍物和悬空高于激光平面的障碍物。

为了弥补单个二维激光在移动机器人中的不足，越来越多的传感器加到了机器人上，其中就有摄像头。利用双目摄像头可以测距的功能，很自然的可以想到应用双目摄像头来探测二维激光无法探测到的障碍物，从而实现移动机器人可以避开实际场景中激光无法检测到的障碍物。

在机器人实时避障算法中，动态代价地图至关重要，动态代价地图显示的是传感器实时检测到的障碍物，结合建立的静态地图，就可以探测到移动机器人周围的障碍物信息，然后结合路径规划算法，计算出一条能够到达目的地的最短路径。

而对于机器人软件系统，目前主流的机器人系统框架是基于ROS(The RobotOperating System)系统，该系统提供有各种主流传感器的数据接口，也提供对机器人的控制接口，可以在真实环境下完成各类型机器人的实验。并且ROS系统提供了很多基础功能包，使得我们可以在机器人的基础框架上进一步做实验和研究。

发明内容

本发明的目的在于克服二维激光传感器只能探测单一平面障碍物的不足，提出了一种基于线激光与双目视觉的动态代价地图导航方法，在导航避障时不能检测到低矮，或者悬空的障碍物，利用标定好的双目摄像头可以检测到这些障碍物并计算出这些障碍物到摄像头平面的距离，然后我们在导航避障时加上由摄像头生成的三维映射到二维的动态代价地图层，通过该层来管理双目视觉的检测的障碍物，如定期更新视觉的障碍物在地图的位置，删除地图中过期的障碍物。再结合二位激光的动态代价地图，我们就可以获取真是世界中三维障碍物的信息，使得移动机器人可以避开激光雷达检测不到的障碍物。

为实现上述目的，本发明所提供的技术方案为：基于线激光与双目视觉的动态代价地图导航方法，包括以下步骤：

1)建立二维栅格地图模型

建立以二维栅格地图作为环境障碍信息的环境地图模型，建立世界坐标系与地图坐标系的转换关系；其中，模型使用二维栅格地图表示环境障碍信息，二维栅格地图模型在系统中以灰度图片的形式保存，并且在模型中记录了地图图片的高度height，地图图片的宽度width，以及地图左下角像素的对应的世界坐标(X_lowerleft,Y_lowerleft)，定义地图图片中左上角像素为地图坐标系的原点(0,0)，行坐标以地图图片从上到下为正方向，列坐标以地图图片从左到右为正方向，定义世界坐标的原点在地图坐标处，resolution为地图分辨率，世界坐标系的x轴方向对应地图坐标系中列坐标增大的方向，世界坐标系的y轴方向对应地图坐标系中行坐标减小的方向，世界坐标系以现实中的米为单位，每一个像素对应了现实世界中一个长宽都是5cm的正方形二维平面区域，若像素值为白色，则表示该区域不存在障碍物，若像素值为黑色，则代表该区域存在障碍物；使用gmapping建图算法构建基于激光雷达的静态地图；

2)建立移动机器人位姿模型

建立以移动机器人中心为原点的机器人坐标系，建立二维坐标平面上的机器人位姿模型，并以坐标系转换关系的形式表示机器人在环境中的位姿；其中，所述机器人位姿模型以机器人中心为原点，机器人中心到机器人正面方向为x轴，以米为单位建立右手平面直角坐标系，即为机器人坐标系，并且用Pose(x,y,θ)表示机器人的位姿，式中x,y代表机器人坐标系的原点所处的世界坐标系的坐标，θ表示世界坐标系x轴方向到机器人坐标系x轴方向的夹角，以逆时针为角度增大的方向，则机器人坐标系中的坐标对应到世界坐标系中的坐标转换如下：

式中，x,y,θ为机器人位姿Pose(x,y,θ)；w_x,w_y为世界坐标系的坐标；r_x,r_y为机器人坐标系的坐标；

3)建立激光雷达数据模型

根据二维激光雷达的安放位置与朝向，建立激光雷达数据模型，并根据步骤1)建立的地图模型和步骤2)建立的机器人位姿模型，以及激光雷达的数据协议，实现激光雷达对环境距离的测量并将测量到的障碍物数据映射到环境地图当中；其中，所述激光雷达数据模型形式如下：

二维激光雷达能够扫描一个平面内360度范围内的障碍信息，二维激光雷达扫描范围能够达8米，角度分辨率为1度，在二维激光雷达的数据模型中，每次传输数据将传输360个浮点数据，从激光雷达的0度方向开始，逆时针方向传输每1角方向上的障碍距离激光雷达中心的距离，以米为单位，并且记录i度方向上的障碍物距离雷达中心距离为ρ_i，当二维激光雷达中心安装在机器人坐标系中的(0.08，0)处，激光雷达的0度方向与机器人坐标系的y轴方向平行且朝向相同时，当机器人处于位姿状态Pose(x,y,θ)时，雷达数据ρ_i映射到世界坐标系中坐标：

式中，Xworld,Yworld为世界坐标系中的坐标；

并且，将雷达数据ρ_i最终映射到地图坐标系中的坐标：

式中，height代表地图图片行坐标，width代表地图图片列坐标，resolution代表地图分辨率；

4)标定双目摄像头和获取三维坐标

使用标定板拍10组不同两个摄像头的照片，通过找到标定板的角点在图像的位置，来计算两个摄像头的内参和外参，最终通过联合标定得到4×4的双目摄像头的重投影矩阵c_x,c_y为左相机像平面中心坐标，c'_x,c'_y为右相机像平面中心坐标；T_x为两个相机的中心距离，f为相机焦距；然后使用块匹配立体匹配算法得到左右两个摄像头的视差图即深度图，给定视差d和一个像平面二维点(x,y)，通过公式求得摄像机坐标系下的三维坐标P＝(X/W,Y/W,Z/W)；

5)使用步骤4)获取的三维坐标创建动态代价地图

根据双目视觉系统的定义，以双目摄像头中的左摄像头的光心为原点，光轴方向为z轴，基线方向从左向右为x轴，以米为单位，建立右手空间直角坐标系，称为双目坐标系，在获取图像上每个像素点的三维坐标P＝(x',y',z')后，y'表示的就是高度信息，x'对应着机器人二维坐标平面的-y轴，z'对应的是x轴方向，根据y'来筛选低于机器人和高于机器人的障碍物，然后把它压缩到机器人所在的二维平面，记机器人坐标系下的坐标为r(x,y),具体公式有r(x,y)＝(z',-x')，得到压缩到机器人二维平面的世界坐标系坐标(r_x,r_y)后，根据当前机器人的位姿(robot_x,robot_y,θ),由刚体变换矩阵和平面坐标相乘即得到双目坐标系在世界坐标系中的坐标(w_x,w_y)；在创建地图时，机器人初始位姿为世界坐标系的原点，是在地图坐标系的中心，根据世界坐标系和地图坐标系的关系，进一步计算出双目坐标系下的障碍物在地图坐标系中的位姿，然后在栅格地图中添加该障碍物，并添加膨胀半径，这样就能够创建出基于双目视觉的代价地图；同理使用相同的方法能够创建出二维激光的代价地图，然后融合两个代价地图，就能够得到当前环境中激光和视觉所看到的障碍物的代价地图；使用该代价地图机器人就能够避障导航到地图中的目标点；

6)最后就是选择一个目标点，通过dwa局部避障算法进行避障导航：a、在机器人控制的空间上进行速度离散采样；b、对于每一个采样速度，看看使用该采样速度移动一段距离后会发生什么；c、评价前向模拟的每个轨迹，评价标准是：靠近障碍物，靠近目标贴近全局路径和速度；在评价的过程中丢弃非法路径；d、挑出得分最高的轨迹并发送相应的速度给机器人；e、不断重复步骤a-d，直到到达目的地。

本发明与现有技术相比，具有如下优点与有益效果：

本发明方法能够通过双目测距创建代价地图，感知二维激光雷达不能探测的障碍物；能够在已知环境二维栅格地图环境下，使用双目摄像头探测环境中高于和低于二维激光雷达平面的障碍物，并且将障碍物映射到环境地图中，通过dwa算法，得到避开障碍物的导航路径线，可以使移动机器人在运动过程中沿着避开障碍的路径到达目的地。

附图说明

图1为二维栅格地图示意图。

图2为地图对应的真实环境示意图。

图3为移动机器人硬件平台示意图。

图4为机器人坐标系定义以及激光雷达0度方向示意图。

图5为双目标定示例图。

图6为激光雷达与双目摄像头的安装方式示意图。

图7为现实环境障碍示意图。

图8为双目摄像头与激光雷达融合的动态代价地图。

图9为机器人系统中计算得出的导航路径线示意图。

图10为机器人按照导航路径线避开障碍到达目的结果示意图。

具体实施方式

下面结合具体实施例对本发明作进一步说明。

本发明所提供的基于线激光与双目视觉的动态代价地图导航方法，包括以下步骤：

1)建立二维栅格地图模型：建立以二维栅格地图作为环境障碍信息的环境地图模型，建立世界坐标系与地图坐标系的转换关系。其中，模型使用二维栅格地图表示环境障碍信息，二维栅格地图模型在系统中以灰度图片的形式保存，并且在模型中记录了地图图片的高度height，地图图片的宽度width，以及地图左下角像素的对应的世界坐标(X_lowerleft,Y_lowerleft)，定义地图图片中左上角像素为地图坐标系的原点(0,0)，行坐标以地图图片从上到下为正方向，列坐标以地图图片从左到右为正方向，定义世界坐标的原点在地图坐标处，resolution为地图分辨率，世界坐标系的x轴方向对应地图坐标系中列坐标增大的方向，世界坐标系的y轴方向对应地图坐标系中行坐标减小的方向，世界坐标系以现实中的米为单位，每一个像素对应了现实世界中一个长宽都是5cm的正方形二维平面区域，若像素值为白色，则表示该区域不存在障碍物，若像素值为黑色，则代表该区域存在障碍物。使用gmapping建图算法可以构建基于激光雷达的静态地图。

2)建立移动机器人位姿模型：建立以移动机器人中心为原点的机器人坐标系，建立二维坐标平面上的机器人位姿模型，并以坐标系转换关系的形式表示机器人在环境中的位姿。其中，所述机器人位姿模型，以机器人中心为原点，机器人中心到机器人正面方向为x轴，以米为单位建立右手平面直角坐标系，即为机器人坐标系，并且用Pose(x,y,θ)表示机器人的位姿，其中x,y代表机器人坐标系的原点所处的世界坐标系的坐标，θ表示世界坐标系x轴方向到机器人坐标系x轴方向的夹角，以逆时针为角度增大的方向，则机器人坐标系中的坐标对应到世界坐标系中的坐标转换如下：

式中，x,y,θ为机器人位姿Pose(x,y,θ)；w_x,w_y为世界坐标系的坐标；r_x,r_y为机器人坐标系的坐标。

3)建立激光雷达数据模型：根据二维激光雷达的安放位置与朝向，建立激光雷达数据模型，并根据步骤1)建立的地图模型和步骤2)建立的机器人位姿模型，以及激光雷达的数据协议，实现激光雷达对环境距离的测量并将测量到的障碍物数据映射到环境地图当中。其中，所述激光雷达数据模型形式如下：

二维激光雷达可以扫描一个平面内360度范围内的障碍信息，一般的二维激光雷达扫描范围可达8米，角度分辨率为1度，在二维激光雷达的数据模型中，每次传输数据将传输360个浮点数据，从激光雷达的0度方向开始，逆时针方向传输每1角方向上的障碍距离激光雷达中心的距离，以米为单位，并且记录i度方向上的障碍物距离雷达中心距离为ρ_i，当二维激光雷达中心安装在机器人坐标系中的(0.08，0)处，激光雷达的0度方向与机器人坐标系的y轴方向平行且朝向相同时，当机器人处于位姿状态Pose(x,y,θ)时，雷达数据ρ_i映射到世界坐标系中坐标：

式中，Xworld,Yworld为世界坐标系中的坐标；

并且，将雷达数据ρ_i最终映射到地图坐标系中的坐标，

式中，height代表地图图片行坐标，width代表地图图片列坐标，resolution代表地图分辨率。

4)标定双目摄像头和获取三维坐标：使用标定板拍10组不同两个摄像头的照片，通过找到标定板的角点在图像的位置，来计算两个摄像头的内参和外参，最终通过联合标定得到4×4的双目摄像头的重投影矩阵c_x,c_y为左相机像平面中心坐标，c'_x,c'_y为右相机像平面中心坐标；T_x为两个相机的中心距离，f为相机焦距；然后使用块匹配立体匹配算法得到左右两个摄像头的视差图即深度图，给定视差d和一个像平面二维点(x,y)，我们可以通过公式求得摄像机坐标系下的三维坐标P＝(X/W,Y/W,Z/W)。

5)使用步骤4)获取的三维坐标创建动态代价地图：根据双目视觉系统的定义，以双目摄像头中的左摄像头的光心为原点，光轴方向为z轴，基线方向从左向右为x轴，以米为单位，建立右手空间直角坐标系，称为双目坐标系,在获取图像上每个像素点的三维坐标P＝(x',y',z')后，y'表示的就是高度信息，x'对应着机器人二维坐标平面的-y轴，z'对应的是x轴方向，可以根据y'来筛选低于机器人和高于机器人的障碍物，然后把它压缩到机器人所在的二维平面，记机器人坐标系下的坐标为r(x,y),具体公式有r(x,y)＝(z',-x')，得到压缩到机器人二维平面的世界坐标系坐标(r_x,r_y)后，根据当前机器人的位姿(robot_x,robot_y,θ),由刚体变换矩阵和平面坐标相乘即得到双目坐标系在世界坐标系中的坐标(w_x,w_y)，在创建地图时，机器人初始位姿为世界坐标系的原点，是在地图坐标系的中心，根据世界坐标系和地图坐标系的关系，可以进一步计算出双目坐标系下的障碍物在地图坐标系中的位姿，然后我们在栅格地图中添加该障碍物，并添加膨胀半径，这样就可以创建出基于双目视觉的代价地图。同理使用相同的方法可以创建出二维激光的代价地图，然后融合两个代价地图，就可以得到当前环境中激光和视觉所看到的障碍物的代价地图。使用该代价地图机器人就可以避障导航到地图中的目标点。

6)最后就是选择一个目标点，通过dwa局部避障算法进行避障导航：a、在机器人控制的空间上进行速度离散采样。b、对于每一个采样速度，看看使用该采样速度移动一小段后会发生什么。c、评价前向模拟的每个轨迹，评价标准如：靠近障碍物，靠近目标贴近全局路径和速度；在评价的过程中丢弃非法路径。d、挑出得分最高的轨迹并发送相应的速度给机器人。e、不断重复步骤a-d，直到到达目的地。

实施案例：在KOBUKI移动机器人平台上完成基于双目摄像头与二维激光雷达的移动机器人导航。

(1)二维栅格地图模型

二维栅格地图如图1所示，储存信息包括每行像素个数以及地图图片总共的行数，地图中每一个像素代表真实环境中一个长宽5cm的平面区域，使用黑色像素代表该区域存在障碍，白色像素代表该区域不存在障碍。

本实例中，地图图片宽度为50像素，高度为60像素，代表一块宽2.5米，长3米的实验场地，如图2所示。

(2)移动机器人平台模型

在移动机器人硬件平台的选择上，使用了KOBUKI通用移动机器人底座，以及LSLIDAR2D激光雷达和两个普通的30万像素摄像头作为传感器获取距离信息，一个X86平台4GB内存的终端作为机器人服务端，如图3所示。

本实例中对传感器的安装方式与机器人坐标的设置如图4所示，以机器人中心为原点，以机器人正面方向为x轴，建立右手平面坐标系，并且将激光雷达的0度方向摆放与机器人坐标系的y轴平行。

(3)标定双目摄像头和获取三维坐标：用于双目标定的标定板，标定均方误差，标定结果Q如图5所示。

(4)基于双目动态代价地图和激光动态代价地图以及二维栅格静态地图的联合导航定位。

本实施案例中：在机器人平台上，双目摄像头与激光雷达固定的形式如图6所示。真实环境如图7所示。而在机器人系统中使用二维栅格地图表示全局环境的障碍物信息，使用双目摄像头和二维激光雷达同时探测障碍物，并且互不干扰，将局部环境障碍信息映射到环境地图当中，将机器人周边5米范围内的全局障碍信息使用局部障碍信息替换，如图8所示，在图7中白色矮箱子是低于激光雷达扫描平面的障碍，它由双目摄像头探测得到，并且映射到了图8的地图中。然通过dwa算法得到了图9所示的导航路径。按照导航规划的路径，最终到达图10所示的终点。

因此，综上所述，本发明方法能够通过双目测距创建代价地图，感知二维激光雷达不能探测的障碍物；能够在已知环境二维栅格地图环境下，使用双目摄像头探测环境中高于和低于二维激光雷达平面的障碍物，并且将障碍物映射到环境地图中，通过dwa算法，得到避开障碍物的导航路径线，可以使移动机器人在运动过程中沿着避开障碍的路径到达目的地，具有实际推广价值，值得推广。

以上所述实施例只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (1)

1.基于线激光与双目视觉的动态代价地图导航方法，其特征在于，包括以下步骤：

1)建立二维栅格地图模型

建立以二维栅格地图作为环境障碍信息的环境地图模型，建立世界坐标系与地图坐标系的转换关系；其中，模型使用二维栅格地图表示环境障碍信息，二维栅格地图模型在系统中以灰度图片的形式保存，并且在模型中记录了地图图片的高度height，地图图片的宽度width，以及地图左下角像素的对应的世界坐标(X_lowerleft,Y_lowerleft)，定义地图图片中左上角像素为地图坐标系的原点(0,0)，行坐标以地图图片从上到下为正方向，列坐标以地图图片从左到右为正方向，定义世界坐标的原点在地图坐标处，resolution为地图分辨率，世界坐标系的x轴方向对应地图坐标系中列坐标增大的方向，世界坐标系的y轴方向对应地图坐标系中行坐标减小的方向，世界坐标系以现实中的米为单位，每一个像素对应了现实世界中一个长宽都是5cm的正方形二维平面区域，若像素值为白色，则表示该区域不存在障碍物，若像素值为黑色，则代表该区域存在障碍物；使用gmapping建图算法构建基于激光雷达的静态地图；

2)建立移动机器人位姿模型

建立以移动机器人中心为原点的机器人坐标系，建立二维坐标平面上的机器人位姿模型，并以坐标系转换关系的形式表示机器人在环境中的位姿；其中，所述机器人位姿模型以机器人中心为原点，机器人中心到机器人正面方向为x轴，以米为单位建立右手平面直角坐标系，即为机器人坐标系，并且用Pose(x,y,θ)表示机器人的位姿，式中x,y代表机器人坐标系的原点所处的世界坐标系的坐标，θ表示世界坐标系x轴方向到机器人坐标系x轴方向的夹角，以逆时针为角度增大的方向，则机器人坐标系中的坐标对应到世界坐标系中的坐标转换如下：

式中，x,y,θ为机器人位姿Pose(x,y,θ)；w_x,w_y为世界坐标系的坐标；r_x,r_y为机器人坐标系的坐标；

3)建立激光雷达数据模型

根据二维激光雷达的安放位置与朝向，建立激光雷达数据模型，并根据步骤1)建立的地图模型和步骤2)建立的机器人位姿模型，以及激光雷达的数据协议，实现激光雷达对环境距离的测量并将测量到的障碍物数据映射到环境地图当中；其中，所述激光雷达数据模型形式如下：

二维激光雷达能够扫描一个平面内360度范围内的障碍信息，二维激光雷达扫描范围能够达8米，角度分辨率为1度，在二维激光雷达的数据模型中，每次传输数据将传输360个浮点数据，从激光雷达的0度方向开始，逆时针方向传输每1角方向上的障碍距离激光雷达中心的距离，以米为单位，并且记录i度方向上的障碍物距离雷达中心距离为ρ_i，当二维激光雷达中心安装在机器人坐标系中的(0.08，0)处，激光雷达的0度方向与机器人坐标系的y轴方向平行且朝向相同时，当机器人处于位姿状态Pose(x,y,θ)时，雷达数据ρ_i映射到世界坐标系中坐标：

式中，Xworld,Yworld为世界坐标系中的坐标；

并且，将雷达数据ρ_i最终映射到地图坐标系中的坐标：

式中，height代表地图图片行坐标，width代表地图图片列坐标，resolution代表地图分辨率；

4)标定双目摄像头和获取三维坐标

使用标定板拍10组不同两个摄像头的照片，通过找到标定板的角点在图像的位置，来计算两个摄像头的内参和外参，最终通过联合标定得到4×4的双目摄像头的重投影矩阵c_x,c_y为左相机像平面中心坐标，c'_x,c'_y为右相机像平面中心坐标；T_x为两个相机的中心距离，f为相机焦距；然后使用块匹配立体匹配算法得到左右两个摄像头的视差图即深度图，给定视差d和一个像平面二维点(x,y)，通过公式求得摄像机坐标系下的三维坐标P＝(X/W,Y/W,Z/W)；

5)使用步骤4)获取的三维坐标创建动态代价地图

根据双目视觉系统的定义，以双目摄像头中的左摄像头的光心为原点，光轴方向为z轴，基线方向从左向右为x轴，以米为单位，建立右手空间直角坐标系，称为双目坐标系，在获取图像上每个像素点的三维坐标P＝(x',y',z')后，y'表示的就是高度信息，x'对应着机器人二维坐标平面的-y轴，z'对应的是x轴方向，根据y'来筛选低于机器人和高于机器人的障碍物，然后把它压缩到机器人所在的二维平面，记机器人坐标系下的坐标为r(x,y),具体公式有r(x,y)＝(z',-x')，得到压缩到机器人二维平面的世界坐标系坐标(r_x,r_y)后，根据当前机器人的位姿(robot_x,robot_y,θ),由刚体变换矩阵和平面坐标相乘即得到双目坐标系在世界坐标系中的坐标(w_x,w_y)；在创建地图时，机器人初始位姿为世界坐标系的原点，是在地图坐标系的中心，根据世界坐标系和地图坐标系的关系，进一步计算出双目坐标系下的障碍物在地图坐标系中的位姿，然后在栅格地图中添加该障碍物，并添加膨胀半径，这样就能够创建出基于双目视觉的代价地图；同理使用相同的方法能够创建出二维激光的代价地图，然后融合两个代价地图，就能够得到当前环境中激光和视觉所看到的障碍物的代价地图；使用该代价地图机器人就能够避障导航到地图中的目标点；

6)最后就是选择一个目标点，通过dwa局部避障算法进行避障导航：a、在机器人控制的空间上进行速度离散采样；b、对于每一个采样速度，看看使用该采样速度移动一段距离后会发生什么；c、评价前向模拟的每个轨迹，评价标准是：靠近障碍物，靠近目标贴近全局路径和速度；在评价的过程中丢弃非法路径；d、挑出得分最高的轨迹并发送相应的速度给机器人；e、不断重复步骤a-d，直到到达目的地。