基于道路结构的无定位车辆出库转向控制方法

熊 莹，周亚琪，毛雪松

(1.武汉科技大学计算机科学与技术学院，湖北 武汉 430065； 2.智能信息处理与实时工业系统湖北省重点实验室，湖北 武汉 430065； 3.武汉科技大学信息科学与工程学院/人工智能学院，湖北 武汉 430081)

0 引 言

无人驾驶车辆安装有多种传感器，包括雷达、激光雷达、立体视觉等，用于感知道路信息完成局部路径规划，以及安装GPS接收器获得位置信息，结合地图信息，完成全局路径规划[1-2]。无论是静态环境还是动态环境，只要能够获得位置信息，就可以方便地建立全局坐标系，规划全局路径控制车辆的运动[3-5]。然而，在很多情况下，定位精度不能满足实际需求，或者定位信号很弱，无法完成定位。例如地下停车场内可能完全没有定位信号。在这些场景下，由于车辆无法获得自身的位置，因此无法建立参考路径引导车辆控制行驶方向。一种解决方法为，通过同步定位与地图创建(Simultaneous Location And Mapping, SLAM)来确定车辆的位置和环境地图[6-8]，但SLAM算法确定的位置为相对于起始位置的局部位置，该位置并不能映射到全局坐标系内。为了完成到车库出口的路径规划，SLAM要求车辆环绕车库获得车库的粗略地图以及出口位置后，才能计算出一条参考路径。

除SLAM外，解决无定位智能驾驶问题的方法还有空地协作、路车通信网络以及惯性导航等方法。其中空地协作[9-11]为无人驾驶车辆分配一个或多个无人飞行器，每个飞行器均携带立体相机，用于建立场景的3D地图。这种方法适用于农业耕作、搜救、道路维护以及军方特殊场景等，但是对于公路交通场景并不合适。作为无人飞行器的一种替代，在无定位信号的道路两侧安装指向性天线，构建车路通信网络是一种道路智能化解决方案[12-13]，可以较好地解决无定位信号下的无人驾驶车辆定位问题。大部分利用通信实现定位的方式都使用测量信号强度或者飞行时间等方式估计路边天线和车辆之间的距离，并在车辆上安装射频识别标签来估计路边天线信号的到达方位角，进而估计车辆的当前位置。这种方式在静态网络中能够获得很高的定位精度，可以完成类似引导一辆车驶出地下车库的任务，但是很难普及到所有停车场。

根据惯性导航推算航位是无定位信号情况下智能驾驶车辆定位的一种比较实用的方法[14-15]，车辆利用惯性元件测量得到车辆加速度，经过积分运算推算车辆的行驶轨迹。这种方法具有不需要增加道路基础设施，不受周围障碍物的干扰，也不向外界辐射能量的优势，是一种自主式导航系统，具有代替定位技术的潜力。但是，从目前已开发的技术来看，惯性导航在测量精度上还不能满足无人驾驶车辆定位的需求。由于在位置推算的运算中使用积分技术，惯性导航的测量误差不断累积且没有校正机制，定位精度随行驶时间的增加而变差。

在不增加道路基础设施的前提下，为了实现无人驾驶车辆的无定位自主驾驶，本文提出一种基于立体相机测量道路边沿，并利用道路边沿数据自动校正车辆行驶方向角的方法，该方法不需要建立全局坐标系与车辆的位置信息。为验证方法的可行性，本文以车辆驶出地下车库为场景，仿真从车库出口到可接收到定位信号位置之间的路径跟随问题。本文的主要工作在于，在不使用位置信息的情况下，从理论上推导根据视角范围有限的立体相机测量路沿，完成直行路段方向角的自适应调整的方法以及直角转弯处方向盘转向角的设置范围，并利用计算机仿真验证在由直行路段和直角转弯构成的简单路段上自适应驾驶的可行性。

1 驾驶场景与车辆模型

本文选取武汉科技大学教1楼西侧地下车库出口路段为场景，研究在该路段上基于立体视觉的无人驾驶转向角控制问题，如图1所示。假设无人驾驶车辆驶出车库后，左转弯行驶一段路程，再左转弯进入树木遮蔽的小路内，行驶路径如图中的白色粗线所示。一般情况下，车辆初始状态的方向角与道路方向存在偏差，另外行驶过程中存在2个左转弯，因此需要研究直线路段方向角的自动校正机制和转弯路段方向角设定方法。与车库出口垂直的路段右侧有停车位，行驶时按照左侧通行规则，其它路段按照中心线通行规则。车辆驶出该路段后，即可以接收定位信号，因此研究范围仅限定在有限路段上，对于更复杂的场景，一般需要配合车辆的位置信息，才能完成行驶路径的规划。

图1 武汉科技大学教1楼西侧车库出口

在机器人运动规划、简单的车辆分析以及推导车辆的控制法则中，通常将车辆系统模型简化为自行车运动学模型[16-17]

(1)

在车辆的正前方安装一个立体相机，用于感知外部道路环境，如图2所示，立体相机可以在视角范围内读取路沿上一点到车辆的距离l和该点视线与车辆正前方的夹角γ。假设车辆行驶速度恒定，和人类驾驶一样，唯一需要控制的参数为车辆的转向角。根据每一对(l,γ)值，设计车辆方向角控制算法，使车辆沿着路沿方向行走，而不会驶出路面。

图2 车辆模型

2 直线路段的车辆相对位姿估算

当车辆行驶于直线路段时，控制方向盘转向角的关键是关于车辆自身相对位姿的推算，如图3所示，黑色粗线为立体相机的正前方中心线。假设在行驶过程中，立体相机的中心方向与道路边沿成一角度θ，且车辆位置偏离道路中心Δx，车辆相对位姿(θ,Δx)的推算就是根据立体相机的测量数据(l,γ)求出这2个未知量。定义车辆方向偏向左侧为正、偏向右侧为负；车辆位置偏离道路中心右侧为正、左侧为负。在中心线的两侧分别任意选取2个视角方向，所测得的各个方向上车辆到道路边沿的距离分别为l11、l12、l21和l22，对应视角方向与中心方向之间的夹角分别为γ11、γ12、γ21和γ22。

图3 直线路段的车辆方向角控制方法

再根据左侧道路测量数据，可以得出：

(2)

其中，D表示路宽，将式(2)展开得：

l11[sinγ11cosθ+cosγ11sinθ]=l12[sinγ12cosθ+cosγ12sinθ]

(3)

因此：

(4)

(5)

也可以根据右侧数据建立相似的关系式。

如果用于推算位姿的2组数据分别来自于左侧和右侧，则得出的估算车辆方位角的方程式：

D=(l11sinγ11-l21sinγ21)cosθ+(l11cosγ11-l21cosγ21)sinθ

为非线性方程，不能给出关于θ的简单方程式，因此在程序实现时，本文选择使用单侧测量数据。

3 交叉路口的转向曲率半径

当车辆行驶至交叉路口，首先通过路口识别算法判断到达路口，然后按照规划的路径或者通过识别左转弯标记执行向左转弯。如果转向半径过大，则车辆就会越过前方路沿驶出路面。反之如果转向半径过小，则车辆会剐蹭到左下方的路角。如图4所示，假设车辆行驶到车头与前方道路下方路沿平行开始转弯，则转向曲率半径必须在R1和R2之间，否则均会发生车辆驶出路面的情况。图4中用三角形标记了车辆可能驶出路面的位置。

图4 转向位置处的转向半径

图5 最大转向半径的计算

其中，R为转向曲率半径，根据图5中的几何关系，可以得出：

(6)

其中，d表示车头中心到前方路沿的距离，将上式展开，得到：

(7)

定义：

则：

R2sin2θ+RP-Q≤0

(9)

因此：

(10)

通常，经过一段直行后，车辆在到达转弯前方向已趋近于和当前行驶道路的边沿平行，因此在式(6)中取θ为90°，化简得到：

(11)

可以得出：

(12)

式(12)右边的参量均为道路和车辆的常数，不随车辆的运动状态而改变。

图6 最小转向半径的计算

(13)

求解该不等式得出：

(14)

根据图6中的几何关系，车的转向曲率半径为：

R=l-x

(15)

因此：

(16)

大部分转弯路口不是直角转弯，而是一段圆弧。这种情况下，车辆的转向半径可以比式(16)确定的值更小。如果从车的后轴到达圆弧起点位置时开始转向，则转向半径可设定为圆弧半径。可以根据地图数据读取转向位置处的圆弧半径，如果道路感知器测量数据准确，也可以根据测量数据推算圆弧半径。

在任意坐标系内，假设测量装置获得圆弧上3个点的坐标分别为(x1,y1)、(x2,y2)和(x3,y3)，圆弧的中心坐标为(x0,y0)，则：

(17)

求解该方程组得出：

(18)

(19)

立体相机测量数据返回距离和方位角，设中心视角方位角为0，逆时针角度为正，则：

(xi,yi)=(-lisinγi,licosγi),i=1,2,3

其中，li表示距离，γi表示相应的方位角，代入式(18)和式(19)即可推算出路口圆弧半径。

狭窄路段转弯时，交通法则要求在路口停车确认安全后继续行驶，因此在转弯处允许出现车辆行驶路径曲率不连续。只要设定车辆的转向半径在由式(12)限定的半径上限和式(16)或式(19)所限定的半径下限范围内，且转向半径在车辆可执行的范围内，车辆就可以在路面内实现转向。

4 车辆转向角控制

Stanley方法是斯坦福大学在DAPRA无人车比赛中使用的路径跟踪方法[23]。如图7所示，设车前轮方向与后轮方向之间的夹角为δ，路径上到前轮的最近点为(cx,cy)，该点与前轮之间的距离为efa，称之为横向跟踪误差。车当前的方位与路径上点(cx,cy)方向之间的差值为：

θe=θ-θp

(20)

其中，θ为车的方位角，θp是点(cx,cy)处路径的方向。Stanley方法为横向跟踪误差efa的非线性反馈函数，该方法具有指数收敛性。当在路径上找到了与前轮最近的点后，可以将方向盘的转向角设置为式(20)给出的θe的值，使得车辆行驶方向与路径一致。

图7 Stanley路径跟踪方法

当efa不为0时，还需要高阶项使车能收敛到路径上。设车的当前行驶速度为v，则增加高阶项后，设置方向盘的转向角为：

(21)

其中，常数k用于调节收敛速率。

当车辆行驶于直线路段时，根据立体相机测量路沿的距离和方位角数据，利用式(5)和式(4)可以分别推算出车与路沿偏差的距离和角度。将推算出的距离Δx减去车正常行驶时需要保持到路沿的距离作为efa，将Δθ作为θe代入式(21)，即可得出在直线路段需要设置的方向盘转向角。

当车辆行驶于转弯路段时，根据式(12)和式(16)求出转弯半径，再利用转弯半径与方向盘转向角之间的关系，可以得出转弯处的转向角为：

(22)

转弯路段上，用Stanley方法控制车辆前行时，可以设置efa为0。

5 仿真结果

为验证所提出的方法，本文根据武汉科技大学黄家湖校区教1楼西侧车库出口附近实际道路场景创建仿真环境，场景参数如图8所示。

仿真车辆考虑普通家用小型轿车，车体的具体参数如表1所示。

表1 车体参数

起始时，车辆可以在偏离中心线任意位置处，且车头方向与路沿方向有偏差角。立体相机测量视角范围内100个点，最大测量距离设定为20 m。图9给出了假定立体相机没有距离测量误差情况下，按照本文所述方法，车辆在无定位情况下的行驶轨迹。在起始位置处，车头方向与道路方向有11.5°的偏差。行驶约10 m后，车头方向已调整到与道路边沿一致，行驶轨迹满足二次连续条件，为车辆可实际行驶路径。当行驶到图9(c)后，受限于立体相机的视野范围，没有道路边沿数据可供参考，车辆进入惯性行驶模式。当检测到与前方水平道路上边沿(或停车位下边沿)距离达到4.2 m时进入转向模式，按照式(22)设定转向角，转向过程如图9(d)所示。图9(h)中，车驶过第二个转向，因立体相机视角的限制，可以看出转向过程中出现了过冲。到达图9(i)位置处时，车已根据路沿自动修正了行驶的方向。

图8 仿真道路场景

图9 无测量误差时车辆的行驶轨迹

图10给出车行驶过程中，通过测量得到的方向角、方向盘的转向角以及方向盘的转向速率，横轴为时间单位。观察车方位角曲线，可以看出起始位置处车头与路沿(90°)有偏差，经约10 s行驶后，车头方向缓慢趋近与道路平行。转向过程中，方向盘的转向角近似为恒定，方向盘的转向速率在机械可执行范围内。在第二个转向处，因车头出现过冲，方向盘的转向上明显可以看出回盘。水平路段上，还出现一段方向盘抖动的现象，这是因为立体相机视野窄，可视道路边沿变短造成的。随后又进入稳定行驶，这是因为已没有道路边沿作为参考，车进入惯性行驶模式。

图10 无测量误差时车的方位角、方向盘转向角和方向盘转向速率

根据图9和图10的结果，在立体相机测量道路边沿准确的前提下，本文的方法可以实现在无定位情况下车辆的行驶方向自动校正与转向。水平道路上出现的方向盘抖动现象仅靠视野范围有限的单个立体相机不可能消除，在后续工作中，将考虑在车上使用多个测量装置，通过对多个装置获取测量数据的数据融合，得到行驶环境的360°全景，实现更安全、舒适的驾驶。实际上，立体相机测量的距离不可能达到精确，20 m测距范围内误差可在±5 cm～±10 cm。为了消除误差的影响，可以考虑采用最小二乘法拟合立体相机的测量数据。在执行拟合之前，要从测量数据中识别出道路边沿与非道路边沿数据，避免非道路边沿数据参与拟合。另外，在道路边沿数据量过少时，应停止拟合而使车进入惯性行驶模式。通过拟合道路边沿数据得到立体相机视角范围内拟合道路边沿的线段，然后将线段起点和终点与立体相机之间所构成的距离和角度代入式(4)和式(5)，并用Stanley方法仿真车辆行驶过程中的方向盘转向调节。

不考虑角度测量误差，并假定距离测量误差服从均匀分布，图11给出距离测量误差在±0 cm～±25 cm范围内车的行驶轨迹。从图中可以看出，随着测量误差的增大，车辆进入惯性行驶状态之前，车与道路方向的偏差角度增大。为使车辆能够在路面内行驶，惯性行驶距离不可过长。当距离测量误差增大到±25 cm时，车经过第二个转向后，出现道路边沿数据识别错误，车驶出路面。出现这一现象，一方面因为距离测量误差过大，另一方面因为立体相机的视野过窄，当行驶到接近转弯处，几乎没有道路边沿数据作为车辆行驶参考。

图11 存在测量误差时车辆的行驶轨迹，图(a)～图(f)中测量误差分别为±0 cm、±5 cm、±10 cm、±15 cm、±20 cm和±25 cm

图12给出车行驶过程中，通过测量得到的方向角、方向盘的转向角以及方向盘的转向速率，横轴为时间单位。观察车方位角曲线，可以看出起始位置处车头与路沿(90°)有偏差，经十多秒行驶后，车头方向趋近与道路平行。转向过程中，方向盘的转向角几乎不发生变化，方向盘的转向速率在机械可执行范围内。与距离测量精确情况类似，在第二个转向处，因车头出现过冲，方向盘的转向上明显可以看出回盘。虽然行驶方向接近稳定，道路边沿距离数据误差引起方向盘抖动，以便及时修正行驶方向，后续工作可考虑设计滤波算法减缓这种方向盘抖动。前2段路上车行驶一段路程后进入稳定行驶，这是因为没有使用道路边沿作为参考，车进入惯性行驶模式。对距离测量误差为±10 cm情况做1000次行驶仿真，未观察到车辆驶出路面情况，表明在立体相机可实现的距离测量精度范围内，本文所提方法可以实现在无定位情况下的直线行驶和转向，确保车辆从车库安全行驶到可以接收定位信号位置处。

图12 测量误差为±15 cm时车的方位角、

6 问题讨论

本文所提的方法对上述结构简单的道路存在理论上的数学解析式，计算量小，实时性高。但对于一般的地下停车场，如图13的例子所示，存在多个出口，这种情况下需要使用相机识别出口指示牌以及路面箭头指示，制定更高层软件来确定行驶方向。一种方法为设置虚拟障碍物，引导车辆按照规定的方向行驶，例如到达目的地需要从A出口驶出，相机识别到2个出口并知道A出口位于左侧，则算法上修改右侧路沿数据，使右侧出现虚拟路沿从而将车辆引导向左侧出口处。

图13 地下停车场示例

另外，图13中还存在环路，其路沿曲率动态变化，由于视角有限的立体相机不能够获取足够的信息，推导方向盘转向控制的数学公式非常困难，甚至不存在严格的数学解。为此，在下一步工作中，将考虑使用多个传感器构成传感器网络，获取汽车行驶的全景环境。根据全景路沿数据，计算一定时间内的安全速度曲线，并根据速度曲线控制方向盘的转角。传感器每隔一定时间获取外部路沿数据，根据路沿数据及时修正速度曲线。为保证车辆在路面的安全行驶，修正路沿数据的时间间隔与车辆当前速度大小、相对路沿的方向等有关信息，相关评估将在下一步工作中完成。

7 结束语

本文以直线路段和直角转弯路段构成的简单场景为研究背景，根据单个视野范围有限的立体相机获取路沿数据，推导了方向盘控制的数学解析式，并根据解析式实现智能驾驶车辆在无定位条件下的自主驾驶，具有计算量小，实时性高的优点。仅依靠传感器当前数据控制方向盘转角的方法对于在无法接收定位信号的场景中完成智能安全驾驶具有很重要的现实意义。虽然该方法在简单驾驶场景中能够实现智能驾驶车辆的无定位行驶，但不足以处理更复杂的情形，需要进一步提高算法的智能性，为此文中简单概述了下一步的工作思路。

针对立体相机存在测量误差的问题，本文使用最小二乘拟合道路边沿，使误差最小化，并假定测量误差在±10 cm情况下执行1000次仿真，结果表明车辆可以在路面内安全行驶。测量误差进一步加大的情况下，会出现参考路沿识别错误，车辆驶出路面的现象。这一问题也期待通过传感器测量的视角范围获得解决。