[发明专利]一种基于双目视觉的车辆导航控制系统及控制方法

权利要求书

1.一种基于双目视觉的车辆导航控制系统的导航控制方法，其特征在于，所述基于双目视觉的车辆导航控制系统包括惯导系统、动力系统和视觉导航系统，所述惯导系统包含编码器、陀螺仪，所述动力系统包含电机、驱动器、运动控制器，所述视觉导航系统包括双目相机及图像处理单元，所述动力系统的驱动方式为差速驱动，即前轮为两个驱动轮，后轮为两个万向轮；

所述方法包括以下步骤：

S1，通过惯导系统生成带有控制信息的可行走的路径，根据人为配置的路径关键点，生成一条最优路径，其中路径中的每个点都要包含控制信息，所述控制信息包括：在世界坐标系下的位置(x,y,z)、该点的路径曲率、路径方向；

S2，双目相机通过提取两帧连续左右图像的特征，进行立体匹配，解算出两帧图像的位姿，然后转换到世界坐标中，并利用编码器与陀螺仪实时纠正和补偿视觉里程计的误差，得到比较的位置和欧拉角；

S3，视觉导航系统将车辆3维空间中的运动映射为2维平面内的运动，并进一步通过视线控制法与路缘跟踪控制法相结合的方法控制车辆的行进；

步骤S3中所述视线控制法包括，结合当前位置的航向角与当前速度，根据车辆在2维平面内的运动模型，计算出车辆的角速度，从而分别得到两个前轮的线速度，以保证车辆到达下一个目标点，不仅位置到达，航向角亦达到，从而保证车辆能沿规划的线路行走；

步骤S3中所述路缘跟踪控制法包括，通过深度学习的方法检测出路与非路的交接处，然后利用霍夫变换方法提取出直线的路边缘，通过计算图像中侧向偏差与方向偏差，判断出车辆与路缘的方向关系，然后用模糊控制方法调整车辆方向，使小车辆沿着路缘行走，并利用路径方向矫正车辆的方向角，从而控制车辆沿路行驶；

步骤S3中所述视线控制法与路缘跟踪控制法相结合的控制方法具体为，当车需要躲避障碍物或者曲线行走时，使用视线法进行控制；当规划的路径为直行线路，且找到可以跟踪、并符合行走方向的路缘时，跟随路缘行走；

步骤S3中所述将车辆3维空间中的运动映射为2维平面内的运动，具体包括将原始模板图像的4个顶点A、B、C、D在屏幕的2维平面上投影为坐标(A1、B1、C1、D1)，每次递归运算中，只考虑前一组投影点坐标，即认为现在的状态只依赖于前一个，而不依赖前面所有坐标值的状态，该过程具体包括预测与更新两个阶段，在预测阶段，将前一帧图像顶点投影的2维坐标作为滤波器X_n，在连续运动过程中估算当前图像中4个顶点的2维坐标的预测值X_n+1，由模板图像4个顶点与当前4个估计顶点之间的对应关系估计视觉导航系统位姿，从而获得当前拍摄图像的位姿，经过3维跟踪算法计算出当前视觉导航系统的正确位姿后，使用模板图像4个顶点的投影的观测值更正预测值X_n+1，用于下一次坐标预测，所用公式为：

其中，X_n为n时刻的投影坐标状态变量，A_n+1是转移矩阵，C_n是观测矩阵，W_n、V_n分别为状态和观测矩阵对应的噪声序列，由此即可得到两帧图像的位姿。