CN104268862A - 一种自主车三维地形可通行性分析方法 - Google Patents

Abstract

一种自主车三维地形可通行性分析方法，步骤为：(1)信息感知：获取当前车辆运动的状态信息及环境信息；(2)轨迹搜索与生成：根据当前车辆位置与最终目标位置进行轨迹初步搜索，生成局部车辆可能运动的轨迹集合；(3)地形分析：根据得到的地形高程数据绘制出当前地形的等高线；根据车速v找到候选轨迹与等高线的交点；分析在等高线下交点与自主车当前位置形成的轨迹曲线段的地形特性，计算基本地形因子；(4)可通行性分析：结合车辆的动力学约束和运动学约束，分别计算车辆通行各轨迹曲线段的代价，以此构造代价图；(5)路径规划：以代价图作为启发式信息，寻找最优路径。本发明具有原理简单、操作简便、判别精度高、计算高效等优点。

Description

一种自主车三维地形可通行性分析方法

技术领域

本发明主要涉及到自主车的地形分析与路径规划领域，特指一种适用于自主车的对三维地形进行可通行性分析的方法。

背景技术

自主车的导航和规划是自主车系统的一个不可分割的部分，而导航的核心是如何根据车载传感器得到的环境数据和定位数据，分析与提取这些数据中的重要信息，辅助自主车能以这些信息规划出一条到达目标点的最优路径。就局部路径规划而言，三维地形可以由传感器得到地形数字高程图(Digital Elevation Maps，简称DEM)来描述，从DEM分析并提取出的这些重要信息主要是能反映地形特征的基本地形因子，包括坡度、粗糙度和起伏度等，然后根据这些信息结合自主车本身的运动学约束和动力学约束，评价自主车通过这片地形区域所需的代价，由此可知哪些区域是不可通行的或哪些区域可通行但通行代价太高，进而找到代价相对低的区域，完成局部路径规划任务。

由此归纳可知，自主车的可通行性分析解决两个主要问题，一是如何根据车载传感器获得的环境数据进行合理地分析地形；二是经过地形分析后，如何结合自主车本身固有的运动学约束和动力学约束条件，对自主车将通行的轨迹地形进行代价评估，为规划器构造代价图。

传统的地形分析方法，主要是先将传感器获得的DEM数据进行栅格化，得到当前地形的栅格图，然后在车辆运行轨迹上找到车辆的下一个位置，然后在该点上构造分析窗口并获得该窗口上各个坐标点的高程值，再使用统计学工具计算该分析窗口区域的基本地形因子，由此得到该片区域的特征信息，最后由代价函数对这些特征信息进行评估，进而给出所分析的地形区域的代价值。类似地，依次滑动式地构造车辆运行轨迹上的未来所在的各个位置的分析窗口并分析相应窗口下的地形区域，最终得到该条轨迹上一系列位置点的代价值，汇总后成为自主车通过该条轨迹代价。若自主车有多条候选轨迹，则经过以上分析过程，可获得自主车到达目标点各条轨迹的代价值，组织这些代价值可形成自主车通过当前整个地形的代价图。最后，路径规划器可将这个代价图作为其中一个启发式信息进行路径搜索和规划。

综上所述，这种基于滑动分析窗口须先在轨迹上找到下一个位置点，然后再在该点分析地形，所以如何找到下一个合理位置点是一个非常重要的问题。若下一个位置点离当前点较远，则中间的轨迹区因没被分析而成为空白区域，对自主车来说是地形未知区。因此，比较安全的方法自然是以一个相对较小的步长来滑动分析窗口，这必然导致分析效率低下以及存在冗余计算问题，成为其中一个制约系统效率的瓶颈问题，无法满足实时要求。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种根据信息感知得到的当前车辆运动和地形环境信息，能实时快速地判断当前地形环境的方法，为自主车的路径规划、导航及控制提供可靠的参考信息，进而很好地协调自主车各个模块的工作的自主车三维地形可通行性分析方法。

为解决上述技术问题，本发明采用以下技术方案：

一种自主车三维地形可通行性分析方法，其步骤为：

(1)信息感知：通过车载传感器提供当前车辆运动的状态信息及环境信息；其中，所述环境信息主要是传感器采集到的地形高程数据集D＝{(x_i,y_i,z_i)^T|x_i,y_i,z_i∈R,i＝1,...,n}，所述车辆运动状态信息为conf＝(x_v,y_v,yaw)，(x_v,y_v)为当前时刻的车辆的水平面位置、yaw为航向角，是自主车的主方向，并根据信息感知得到的地形高程数据，快速绘制出当前地形的等高线；

(2)轨迹搜索与生成：根据当前车辆位置与最终目标位置进行轨迹初步搜索，生成局部车辆可能运动的轨迹集合，包括车体运动轨迹和轮胎运动轨迹；

(3)地形分析：由步骤(2)产生轨迹，并根据车速v找到这些轨迹与等高线的交点，然后分析在等高线下这些交点与车当前位置形成的轨迹曲线段的地形特性，计算其基本地形因子；

(4)可通行性分析：经过步骤(3)得到各条轨迹曲线段的基本地形因子后，结合车辆的动力学约束和运动学约束，分别计算车辆通行各轨迹曲线段的代价，以此构造代价图；

(5)路径规划：以步骤(4)中得到的代价图作为启发式信息，寻找一条通向下一个目标点的代价最小长度最短的最优路径。

作为本发明的进一步改进：所述步骤(3)的具体流程为：

(3.1)读取由步骤(1)得到DEM数据保存为矩阵变量GROUNDMAP,该变量包含大小都是m×n的子变量集分别由D中的所有点的(x,y,z)值构成，其中m,n分别为地形的长和宽，然后从GROUNDMAP中提取高程值z，快速绘制该地形的等高线图CONTOURMAP；

(3.2)由步骤(2)生成的车体轨迹集和四个轮胎的轨迹集 ${\mathrm{traset}}_{\mathrm{fl}}=({\mathrm{tra}}_l^{\mathrm{fl}},...,{\mathrm{tra}}_t^{\mathrm{fl}}),{\mathrm{traset}}_{\mathrm{fr}}=({\mathrm{tra}}_l^{\mathrm{fr}},...,{\mathrm{tra}}_t^{\mathrm{fr}}),{\mathrm{traset}}_{\mathrm{bl}}=({\mathrm{tra}}_l^{\mathrm{bl}},...,{\mathrm{tra}}_t^{\mathrm{bl}}),{\mathrm{traset}}_{\mathrm{br}}=({\mathrm{tra}}_l^{\mathrm{br}},...,{\mathrm{tra}}_t^{\mathrm{br}})$ 选择任一条轨迹分部求出该轨迹曲线与等高线图CONTOURMAP的第一个交点进而分别得到车体和轮胎到相应点的轨迹段 ${\mathrm{tra}}_k^{v^{\′}},{\mathrm{tra}}_k^{{\mathrm{fl}}^{\′}},{\mathrm{tra}}_k^{{\mathrm{fr}}^{\′}},{\mathrm{tra}}_k^{{\mathrm{bl}}^{\′}},{\mathrm{tra}}_k^{{\mathrm{br}}^{\′}}({\mathrm{tra}}_k^{v^{\′}}\⊆{\mathrm{tra}}_k^v,{\mathrm{tra}}_k^{{\mathrm{fl}}^{\′}}\⊆{\mathrm{tra}}_k^{\mathrm{fl}},{\mathrm{tra}}_k^{{\mathrm{fr}}^{\′}}\⊆{\mathrm{tra}}_k^{\mathrm{fr}},{\mathrm{tra}}_k^{{\mathrm{bl}}^{\′}}\⊆{\mathrm{tra}}_k^{\mathrm{bl}},{\mathrm{tra}}_k^{{\mathrm{br}}^{\′}}\⊆{\mathrm{tra}}_k^{\mathrm{br}});$

(3.3)计算车体轨迹段的基本地形因子同理可相应得到轮胎轨迹段的基本地形因子

作为本发明的进一步改进：所述步骤(3.1)的具体流程为：

(3.1.1)从GROUNDMAP的子变量中找出最小的z值zmin和最大的zmax值，设置步长△d，l＝1，创建临时矩阵变量zimage，该矩阵的大小与GROUNDMAP中子变量集大小一致，即 $\mathrm{zimage}=\left\{{\mathrm{zimg}}_{\mathrm{ij}}\right|{\mathrm{zimg}}_{\mathrm{ij}}\∈\stackrel{\→}{z},i=1,...,m,j=1,...,n\};$

(3.1.2)选择区间[zmin+△d×(l-1),zmin+△d×l)，且zmin+△d×l<zmax，遍历GROUNDMAP，找出落在该区间的高程点z_i,j(i＝1,...,m,j＝1,...,n)，将zimage中的zimg_i,j置为l，并保存到集合zset_i，l＝l+1；

(3.1.3)循环(3.1.2)当zmin+△d×l>zmax时，转至(3.1.4)；

(3.1.4)遍历zimage，提取其元素值都为vl(vl＝1,...,l)的元素索引集根据从GROUNDMAP的子变量集中找到相应的坐标点集,构成第vl条等高线的初始坐标点集 ${\mathrm{contset}}_{\mathrm{vl}}=\left\{(x_{\mathrm{ij}},y_{\mathrm{ij}})\right|x_{\mathrm{ij}}\&Element;\stackrel{\&RightArrow;}{x},y_{\mathrm{ij}}\&Element;\stackrel{\&RightArrow;}{y},i\&Element;\stackrel{\&RightArrow;}{i},j\&Element;\stackrel{\&RightArrow;}{j}\},{\mathrm{contset}}_{\mathrm{vl}}\&SubsetEqual;\mathrm{constset},$ 并对该条等高线内的初始坐标点进行升序排序；

(3.1.5)设contset_vl中有m个坐标点，使用链码组织contset_vl中的点，用光滑的曲线拟合这些点，形成相应的等高线，由此叠加l条等高线，得到最终该地形的等高线图CONTOURMAP。

作为本发明的进一步改进：所述步骤(3.2)的具体流程为：

(3.2.1)假设从轨迹集中任一条轨迹包含n(n<<m)个轨迹点，并且这些点的坐标是已知的，即(x_i,y_i)∈tra_k,i＝1,...,n。又由步骤(1)中车辆的运动状态信息conf＝(x_v,y_v,yaw)得到的车辆当前X-Y平面坐标(x_v,y_v)及其航向角，计算出车辆的左前轮、右前轮、左后轮和右后轮的X-Y平面重心坐标，分别为(x_fl,y_fl),(x_fr,y_fr),(x_bl,y_bl),(x_br,y_br)，并且轮胎的前进方向与车体的一致，即都由车体航向角yaw表示；

(3.2.2)将车体重心坐标(x_v,y_v)与地形等高线图CONTOURMAP中各等高线的坐标集内与车体同一水平线上的坐标比较，找出其欧式距离最小的坐标及其所属的等高线确定出离车辆最近的等高线。同理，找出离四个轮胎最近的等高线 ${\mathrm{contset}}_l^{\mathrm{fl}},{\mathrm{contset}}_l^{\mathrm{fr}},{\mathrm{contset}}_l^{\mathrm{bl}},{\mathrm{contset}}_l^{\mathrm{br}}:$

$\left\{\begin{array}{c}\underset{{\mathrm{conset}}_l^v\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^v,y_{\mathrm{cont}}^v)\&Element;{\mathrm{contset}}_l^v}{\arg \min }}\sqrt{{(x_v-x_{\mathrm{cont}}^v)}^2+{(y_v-y_{\mathrm{cont}}^v)}^2}\\ \underset{{\mathrm{conset}}_l^{\mathrm{fl}}\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{fl}},y_{\mathrm{cont}}^{\mathrm{fl}})\&Element;{\mathrm{contset}}_l^{\mathrm{fl}}}{\arg \min }}\sqrt{{(x_{\mathrm{fl}}-x_{\mathrm{cont}}^{\mathrm{fl}})}^2+{(y_{\mathrm{fl}}-y_{\mathrm{cont}}^{\mathrm{fl}})}^2}\\ \underset{{\mathrm{conset}}_l^{\mathrm{fr}}\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{fr}},y_{\mathrm{cont}}^{\mathrm{fr}})\&Element;{\mathrm{contset}}_l^{\mathrm{fr}}}{\arg \min }}\sqrt{{(x_{\mathrm{fr}}-x_{\mathrm{cont}}^{\mathrm{fr}})}^2+{(y_{\mathrm{fr}}-y_{\mathrm{cont}}^{\mathrm{fr}})}^2}\\ \underset{{\mathrm{conset}}_l^3\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{bl}},y_{\mathrm{cont}}^{\mathrm{bl}})\&Element;{\mathrm{contset}}_l^{\mathrm{bl}}}{\arg \min }}\sqrt{{(x_{\mathrm{bl}}-x_{\mathrm{cont}}^{\mathrm{bl}})}^2+{(y_{\mathrm{bl}}-y_{\mathrm{cont}}^{\mathrm{bl}})}^2}\\ \underset{{\mathrm{conset}}_l^{\mathrm{br}}\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{br}},y_{\mathrm{cont}}^{\mathrm{br}})\&Element;{\mathrm{contset}}_l^{\mathrm{br}}}{\arg \min }}\sqrt{{(x_{\mathrm{br}}-x_{\mathrm{cont}}^{\mathrm{br}})}^2+{(y_{\mathrm{br}}-y_{\mathrm{cont}}^{\mathrm{br}})}^2}\end{array}\right.$

(3.2.3)计算车体轨迹上各个轨迹点与等高线中各点的欧式距离，找出距离最小的点，作为该轨迹曲线与等高线图CONTOURMAP的第一个交点同理，分别计算出四个轮胎的轨迹线与等高线的第一个交点由此可得到当前车体到的轨迹线以及四个轮胎到的轨迹线

$\left\{\begin{array}{c}\underset{(x_{\mathrm{trai}}^v,y_{{\mathrm{tra}}_i}^v)\&Element;{\mathrm{tra}}_k^v}{\underset{(x_{{\mathrm{cont}}_i}^v,y_{{\mathrm{cont}}_i}^v)\&Element;{\mathrm{contset}}_l^v}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^v-x_{{\mathrm{cont}}_i}^v)}^2+{(y_{{\mathrm{tra}}_i}^v-y_{{\mathrm{cont}}_i}^v)}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{fl}},y_{{\mathrm{tra}}_i}^{\mathrm{fl}})\&Element;{\mathrm{tra}}_k^{\mathrm{fl}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{fl}},y_{{\mathrm{cont}}_i}^{\mathrm{fl}})\&Element;{\mathrm{contset}}_l^{\mathrm{fl}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{fl}}-x_{{\mathrm{cont}}_i}^{\mathrm{fl}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{fl}}-y_{{\mathrm{cont}}_i}^{\mathrm{fl}})}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{fr}},y_{{\mathrm{tra}}_i}^{\mathrm{fr}})\&Element;{\mathrm{tra}}_k^{\mathrm{fr}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{fr}},y_{{\mathrm{cont}}_i}^{\mathrm{fr}})\&Element;{\mathrm{contset}}_l^{\mathrm{fr}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{fr}}-x_{{\mathrm{cont}}_i}^{\mathrm{fr}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{fr}}-y_{{\mathrm{cont}}_i}^{\mathrm{fr}})}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{bl}},y_{{\mathrm{tra}}_i}^{\mathrm{bl}})\&Element;{\mathrm{tra}}_k^{\mathrm{bl}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{bl}},y_{{\mathrm{cont}}_i}^{\mathrm{bl}})\&Element;{\mathrm{contset}}_l^{\mathrm{bl}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{bl}}-x_{{\mathrm{cont}}_i}^{\mathrm{bl}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{bl}}-y_{{\mathrm{cont}}_i}^{\mathrm{bl}})}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{br}},y_{{\mathrm{tra}}_i}^{\mathrm{br}})\&Element;{\mathrm{tra}}_k^{\mathrm{br}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{br}},y_{{\mathrm{cont}}_i}^{\mathrm{br}})\&Element;{\mathrm{contset}}_l^{\mathrm{br}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{br}}-x_{{\mathrm{cont}}_i}^{\mathrm{br}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{br}}-y_{{\mathrm{cont}}_i}^{\mathrm{br}})}^2},i=1,...,n\end{array}\right.$

作为本发明的进一步改进：所述步骤(3.3)的具体流程为：

假设车辆的下一条运行轨迹为根据车辆速度v找出该轨迹与等高线的交点(a_v,b_v,...)，这些交点的坐标值为同理，可以分别得到左前轮、右前轮、左后轮和右后轮轨迹与等高线的交点(a_fl,b_fl,...),(a_fr,b_fr,...),(a_bl,b_bl,...),(a_br,b_br,...)，相应的交点坐标值分别为 $\{(x_1^{\mathrm{fl}},y_1^{\mathrm{fl}},z_1^{\mathrm{fl}}),...,(x_{n_{\mathrm{fl}}}^{\mathrm{fl}},y_{n_{\mathrm{fl}}}^{\mathrm{fl}},z_{n_{\mathrm{fl}}}^{\mathrm{fl}})\},\{(x_1^{\mathrm{fr}},y_1^{\mathrm{fr}},z_1^{\mathrm{fr}}),...,(x_{n_{\mathrm{fr}}}^{\mathrm{fr}},y_{n_{\mathrm{fr}}}^{\mathrm{fr}},z_{n_{\mathrm{fr}}}^{\mathrm{fr}})\},\{(x_1^{\mathrm{bl}},y_1^{\mathrm{bl}},z_1^{\mathrm{bl}}),...,(x_{n_{\mathrm{bl}}}^{\mathrm{bl}},y_{n_{\mathrm{bl}}}^{\mathrm{bl}},z_{n_{\mathrm{bl}}}^{\mathrm{bl}})\},\{(x_1^{\mathrm{br}},y_1^{\mathrm{br}},z_1^{\mathrm{br}}),...,(x_{n_{\mathrm{br}}}^{\mathrm{br}},y_{n_{\mathrm{br}}}^{\mathrm{br}},z_{n_{\mathrm{br}}}^{\mathrm{br}})\}.$

(3.3.1)计算车辆运行轨迹的坡度：由车体轨迹各个分片段的坡度值表示：

$\left\{\begin{array}{c}{\mathrm{slope}}_1^v=\arccos \left(\frac{\sqrt{{(x_2^v-x_1^v)}^2+{(y_2^v-y_1^v)}^2}}{\sqrt{{(x_2^v-x_1^v)}^2+{(y_2^v-y_1^v)}^2+{(z_2^v-z_1^v)}^2}}\right),\\ ...\\ {\mathrm{slope}}_{n_v-1}^v=\arccos \left(\frac{\sqrt{{(x_{n_v}^v-x_{n_v-1}^v)}^2+{(y_{n_v}^v-y_{n_v-1}^v)}^2}}{\sqrt{{(x_{n_v}^v-x_{n_v-1}^v)}^2+{(y_{n_v}^v-y_{n_v-1}^v)}^2+{(z_{n_v}^v-z_{n_v-1}^v)}^2}}\right),\end{array}\right.$

(3.3.2)计算车辆运行轨迹的粗糙度，主要考虑到轮胎所走轨迹的地形粗糙度，此外左前轮和左后轮、右前轮和右后轮所走的轨迹是一样的，因此只需分别计算左前轮和右前轮轨迹的粗糙度作为左轮和右轮的粗糙度

$\left\{\begin{array}{c}{\mathrm{rough}}_k^{\mathrm{wl}}=\frac{\underset{i=1}{\overset{n_{\mathrm{fl}}}{\mathrm{\&Sigma;}}}{(z_i^{\mathrm{fl}}-\stackrel{\&OverBar;}{z_{\mathrm{fl}}})}^2}{n_{\mathrm{fl}}-1},\stackrel{\&OverBar;}{z_{\mathrm{fl}}}=\frac{\underset{i=1}{\overset{n_{\mathrm{fl}}}{\mathrm{\&Sigma;}}}{z}_i^{\mathrm{fl}}}{n_{\mathrm{fl}}}\\ {\mathrm{rough}}_k^{\mathrm{wr}}=\frac{\underset{i=1}{\overset{n_{\mathrm{wr}}}{\mathrm{\&Sigma;}}}{(z_i^{\mathrm{fr}}-\stackrel{\&OverBar;}{z_{\mathrm{fr}}})}^2}{n_{\mathrm{fr}}-1},\stackrel{\&OverBar;}{z_{\mathrm{fr}}}=\frac{\underset{i=1}{\overset{n_{\mathrm{fr}}}{\mathrm{\&Sigma;}}}{z}_i^{\mathrm{fr}}}{n_{\mathrm{fr}}}\end{array}\right.$

(3.3.3)计算车辆运行轨迹的起伏度：计算车体运行轨迹的起伏度左轮和右轮的起伏度同步骤(3.4.2)中分析，只需分别计算左前轮和右前轮的起伏度作为左轮和右轮的起伏度：

$\left\{\begin{array}{c}{\mathrm{waviness}}_k^v=z_{\max }^v-z_{\min }^v\\ {\mathrm{waviness}}_k^{\mathrm{wl}}=z_{\max }^{\mathrm{wl}}-z_{\min }^{\mathrm{wl}}\\ {\mathrm{waviness}}_k^{\mathrm{wr}}=z_{\max }^{\mathrm{wr}}-z_{\min }^{\mathrm{wr}}\end{array}\right.$

作为本发明的进一步改进：所述步骤(4)的具体流程为：

假设车辆当前位置左前轮、右前轮、左后轮和右后轮的高程值分别为z_fl,z_fr,z_bl,z_br

(4.1)计算当前车辆的爬坡度pitch，设自主车的左(右)前轮与左(右)后轮的间距为L

$\mathrm{pitch}=\arcsin \left(\frac{|z_{\mathrm{fl}}-z_{\mathrm{bl}}|+|z_{\mathrm{fr}}-z_{\mathrm{br}}|}{2\&times;L}\right)$

(4.2)计算当前车辆的侧翻角roll，设自主车的左前(后)轮与右前(后)轮的间距为W：

$\mathrm{roll}=\arcsin \left(\frac{|z_{\mathrm{fl}}-z_{\mathrm{fr}}|+|z_{\mathrm{bl}}-z_{\mathrm{br}}|}{2\&times;W}\right)$

(4.3)计算车辆运行轨迹代价，设自主车的最大爬坡度和最大侧翻角是PITCH和ROLL，车辆在轨迹某一条轨迹tra_k上运行的代价为cost(tra_k)，其中tra_k包含车体轨迹和四个轮胎的轨迹再将cost(tra_k)存于最终代价图COSTMAP中：

$\cos t\left({\mathrm{tra}}_k\right)={\cos t}_{\mathrm{slope}}^k+{\cos t}_{\mathrm{rough}}^k+{\cos t}_{\mathrm{waviness}}^k+{\cos t}_{\mathrm{pitch}}^k+{\cos t}_{\mathrm{roll}}^k+{\cos t}_{\mathrm{path}}^k$

其中表示车体通过该条轨迹坡度代价，坡度值由步骤(3.4.1)得到。表示由左轮和右轮通过粗糙地形的代价。表示由左轮和右轮通过起伏不定的地形的代价，起伏度由步骤(3.4.3)得到。和表示车辆通过该轨迹的所有位置点的爬坡代价和侧翻代价。表示车辆通过的路径长度代价：

$\left\{\begin{array}{c}{\cos t}_{\mathrm{slope}}^k=\underset{i=1}{\overset{n_i^v-1}{\mathrm{\&Sigma;}}}{\mathrm{slope}}_i^v\&times;W_{\mathrm{slope}}\\ {\cos t}_{\mathrm{rough}}^k=\underset{i=1}{\overset{n_i^{\mathrm{fl}}}{\mathrm{\&Sigma;}}}{\mathrm{rough}}_i^{\mathrm{wl}}\&times;W_{\mathrm{rough}}+\underset{i=1}{\overset{n_i^{\mathrm{fr}}}{\mathrm{\&Sigma;}}}{\mathrm{rough}}_i^{\mathrm{wr}}\&times;W_{\mathrm{rough}}\\ {\cos t}_{\mathrm{waviness}}^k={\mathrm{wavniess}}_k^{\mathrm{wl}}\&times;W_{\mathrm{waviness}}+{\mathrm{wavniess}}_k^{\mathrm{wr}}\&times;W_{\mathrm{waviness}}\\ {\cos t}_{\mathrm{pitch}}^k=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{\arcsin \left(\frac{|z_i^{\mathrm{fl}}-z_i^{\mathrm{bl}}|+|z_i^{\mathrm{fr}}-z_i^{\mathrm{br}}|}{2\&times;L}\right)}{\mathrm{PITCH}}\&times;W_{\mathrm{pitch}}\\ {\cos t}_{\mathrm{roll}}^k=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{\arcsin \left(\frac{|z_i^{\mathrm{fl}}-z_i^{\mathrm{fr}}|+|z_i^{\mathrm{bl}}-z_i^{\mathrm{br}}|}{2\&times;W}\right)}{\mathrm{ROLL}}\&times;W_{\mathrm{roll}}\\ {\cos t}_{\mathrm{path}}^k={\cos t}_{\mathrm{lpath}}^k+{\cos t}_{\mathrm{rpath}}^k\\ {\cos t}_{\mathrm{lpath}}^k=\underset{i=1}{\overset{n_{i-1}^{\mathrm{fl}}}{\mathrm{\&Sigma;}}}\sqrt{{(x_{i+1}^{\mathrm{fl}}-x_i^{\mathrm{fl}})}^2+{(y_{i+1}^{\mathrm{fl}}-y_i^{\mathrm{fl}})}^2+{(z_{i+1}^{\mathrm{fl}}-z_i^{\mathrm{fl}})}^2}\&times;W_{\mathrm{path}}\\ {\cos t}_{\mathrm{rpath}}^k=\underset{i=1}{\overset{n_{i-1}^{\mathrm{fr}}}{\mathrm{\&Sigma;}}}\sqrt{{(x_{i+1}^{\mathrm{fr}}-x_i^{\mathrm{fr}})}^2+{(y_{i+1}^{\mathrm{fr}}-y_i^{\mathrm{fr}})}^2+{(z_{i+1}^{\mathrm{fr}}-z_i^{\mathrm{fr}})}^2}\&times;W_{\mathrm{path}}\end{array}\right.$

与现有技术相比，本发明的优点在于：

1、本发明的自主车三维地形可通行性分析方法，由于不再使用以网格形式的分析窗口，而是通过在DEM数据的基础上快速绘制等高线图，计算车辆运行轨迹在等高线图中的地形基本因子，因此与传统的基于滑动的分析窗口相比，所提出的方法能快速地计算出将通行的地形特征，非常适合于自主车的实际应用。

2、本发明的自主车三维地形可通行性分析方法，是以轨迹线段结合等高线进行地形分析，是基于线的分析模式，直接将候选轨迹作为分析对象，大大降低了自主车系统的分析复杂度，减少了不必要的分析步骤。

附图说明

图1是本发明自主车三维地形可通行性分析方法的流程示意图。

图2是本发明在具体应用实例中进行绘制等高线的流程示意图。

图3是本发明在具体应用实例中进行地形分析的流程示意图。

图4是本发明本发明在具体应用实例中进行代价估计的流程示意图。

具体实施方式

以下将结合说明书附图和具体实施例对本发明做进一步详细说明。

如图1～图4所示，本发明的自主车三维地形可通行性分析方法，包含以下步骤：

第一步，信息感知步骤。通过车载传感器提供环境信息及当前车辆运动的状态信息。其中，环境信息主要是传感器采集到的地形高程数据集D＝{(x_i,y_i,z_i)^T|x_i,y_i,z_i∈R,i＝1,...,n}，所述车辆运动状态信息为conf＝(x_v,y_v,yaw)，(x_v,y_v)为当前时刻的车辆的水平面位置、yaw为航向角，并根据信息感知得到的地形高程数据，快速绘制出当前地形的等高线。

第二步，轨迹搜索与生成：根据当前车辆位置与最终目标位置进行轨迹初步搜索，生成局部车辆可能运动的轨迹集合，包括车体运动轨迹和轮胎运动轨迹。

第三步，地形分析：由轨迹搜索与生成产生轨迹，并根据车速v找到这些轨迹与等高线的交点，然后分析在等高线下这些交点与车辆当前位置形成的轨迹曲线段的地形特性，计算其基本地形因子。

1、读取信息感知模块得到DEM数据并保存为矩阵变量GROUNDMAP,该变量包含大小都是m×n的子变量集分别由D中的所有点的(x,y,z)值构成，其中m,n分别为地形的长和宽，然后从GROUNDMAP中提取高程值z，快速绘制该地形的等高线图CONTOURMAP。

具体实现过程如下：

(1)从GROUNDMAP的子变量中找出最小的z值zmin和最大的zmax值，设置步长△d，l＝1，创建临时矩阵变量zimage，该矩阵的大小与GROUNDMAP中子变量集大小一致，即 $\mathrm{zimage}=\left\{{\mathrm{zimg}}_{\mathrm{ij}}\right|{\mathrm{zimg}}_{\mathrm{ij}}\&Element;\stackrel{\&RightArrow;}{z},i=1,...,m,j=1,...,n\};$

(2)选择区间[zmin+△d×(l-1),zmin+△d×l)，且zmin+△d×l<zmax，遍历GROUNDMAP，找出落在该区间的高程点z_i,j(i＝1,...,m,j＝1,...,n)，将zimage中的zimg_i,j置为l，并保存到集合zset_i，l＝l+1；

(3)循环(2)当zmin+△d×l>zmax时，转至(4)；

(4)遍历zimage，提取其元素值都为vl(vl＝1,...,l)的元素索引集根据从GROUNDMAP的子变量集中找到相应的坐标点集,构成第vl条等高线的初始坐标点集 ${\mathrm{contset}}_{\mathrm{vl}}=\left\{(x_{\mathrm{ij}},y_{\mathrm{ij}})\right|x_{\mathrm{ij}}\&Element;\stackrel{\&RightArrow;}{x},y_{\mathrm{ij}}\&Element;\stackrel{\&RightArrow;}{y},i\&Element;\stackrel{\&RightArrow;}{i},j\&Element;\stackrel{\&RightArrow;}{j}\},{\mathrm{contset}}_{\mathrm{vl}}\&SubsetEqual;\mathrm{constset},$ 并对该条等高线内的初始坐标点进行升序排序；

(5)设contset_vl中有m个坐标点，使用链码组织contset_vl中的点，用光滑的曲线拟合这些点，形成相应的等高线，由此叠加l条等高线，得到最终该地形的等高线图CONTOURMAP。

2、由轨迹搜索与生成模块生成的车体轨迹集和四个轮胎的轨迹集 ${\mathrm{traset}}_{\mathrm{fl}}=({\mathrm{tra}}_l^{\mathrm{fl}},...,{\mathrm{tra}}_t^{\mathrm{fl}}),{\mathrm{traset}}_{\mathrm{fr}}=({\mathrm{tra}}_l^{\mathrm{fr}},...,{\mathrm{tra}}_t^{\mathrm{fr}}),{\mathrm{traset}}_{\mathrm{bl}}=({\mathrm{tra}}_l^{\mathrm{bl}},...,{\mathrm{tra}}_t^{\mathrm{bl}}),{\mathrm{traset}}_{\mathrm{br}}=({\mathrm{tra}}_l^{\mathrm{br}},...,{\mathrm{tra}}_t^{\mathrm{br}})$ 选择任一条轨迹分部求出该轨迹曲线与等高线图CONTOURMAP的第一个交点进而分别得到车体和轮胎到相应点的轨迹段 ${\mathrm{tra}}_k^{v^{\&prime;}},{\mathrm{tra}}_k^{{\mathrm{fl}}^{\&prime;}},{\mathrm{tra}}_k^{{\mathrm{fr}}^{\&prime;}},{\mathrm{tra}}_k^{{\mathrm{bl}}^{\&prime;}},{\mathrm{tra}}_k^{{\mathrm{br}}^{\&prime;}}({\mathrm{tra}}_k^{v^{\&prime;}}\&SubsetEqual;{\mathrm{tra}}_k^v,{\mathrm{tra}}_k^{{\mathrm{fl}}^{\&prime;}}\&SubsetEqual;{\mathrm{tra}}_k^{\mathrm{fl}},{\mathrm{tra}}_k^{{\mathrm{fr}}^{\&prime;}}\&SubsetEqual;{\mathrm{tra}}_k^{\mathrm{fr}},{\mathrm{tra}}_k^{{\mathrm{bl}}^{\&prime;}}\&SubsetEqual;{\mathrm{tra}}_k^{\mathrm{bl}},{\mathrm{tra}}_k^{{\mathrm{br}}^{\&prime;}}\&SubsetEqual;{\mathrm{tra}}_k^{\mathrm{br}}).$

具体实现过程如下：

(1)假设从轨迹集中任一条轨迹包含n(n<<m)个轨迹点，并且这些点的坐标是已知的，即(x_i,y_i)∈tra_k,i＝1,...,n。又由车辆的运动状态信息conf＝(x_v,y_v,yaw)得到的车辆当前X-Y平面坐标(x_v,y_v)及其航向角，计算出车辆的左前轮、右前轮、左后轮和右后轮的X-Y平面重心坐标，分别为(x_fl,y_fl),(x_fr,y_fr),(x_bl,y_bl),(x_br,y_br)，并且轮胎的前进方向与车体的一致，即都由车体航向角yaw表示；

(2)将车体重心坐标(x_v,y_v)与地形等高线图CONTOURMAP中各等高线的坐标集内的与车体同一水平线上的坐标比较，找出其欧式距离最小的坐标及其所属的等高线，确定出离车辆最近的等高线，假设找到的等高线是同理，找出离四个轮胎最近的等高线 ${\mathrm{contset}}_l^{\mathrm{fl}},{\mathrm{contset}}_l^{\mathrm{fr}},{\mathrm{contset}}_l^{\mathrm{bl}},{\mathrm{contset}}_l^{\mathrm{br}},$ 具体计算公式如下：

$\left\{\begin{array}{c}\underset{{\mathrm{conset}}_l^v\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^v,y_{\mathrm{cont}}^v)\&Element;{\mathrm{contset}}_l^v}{\arg \min }}\sqrt{{(x_v-x_{\mathrm{cont}}^v)}^2+{(y_v-y_{\mathrm{cont}}^v)}^2}\\ \underset{{\mathrm{conset}}_l^{\mathrm{fl}}\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{fl}},y_{\mathrm{cont}}^{\mathrm{fl}})\&Element;{\mathrm{contset}}_l^{\mathrm{fl}}}{\arg \min }}\sqrt{{(x_{\mathrm{fl}}-x_{\mathrm{cont}}^{\mathrm{fl}})}^2+{(y_{\mathrm{fl}}-y_{\mathrm{cont}}^{\mathrm{fl}})}^2}\\ \underset{{\mathrm{conset}}_l^{\mathrm{fr}}\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{fr}},y_{\mathrm{cont}}^{\mathrm{fr}})\&Element;{\mathrm{contset}}_l^{\mathrm{fr}}}{\arg \min }}\sqrt{{(x_{\mathrm{fr}}-x_{\mathrm{cont}}^{\mathrm{fr}})}^2+{(y_{\mathrm{fr}}-y_{\mathrm{cont}}^{\mathrm{fr}})}^2}\\ \underset{{\mathrm{conset}}_l^3\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{bl}},y_{\mathrm{cont}}^{\mathrm{bl}})\&Element;{\mathrm{contset}}_l^{\mathrm{bl}}}{\arg \min }}\sqrt{{(x_{\mathrm{bl}}-x_{\mathrm{cont}}^{\mathrm{bl}})}^2+{(y_{\mathrm{bl}}-y_{\mathrm{cont}}^{\mathrm{bl}})}^2}\\ \underset{{\mathrm{conset}}_l^{\mathrm{br}}\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{br}},y_{\mathrm{cont}}^{\mathrm{br}})\&Element;{\mathrm{contset}}_l^{\mathrm{br}}}{\arg \min }}\sqrt{{(x_{\mathrm{br}}-x_{\mathrm{cont}}^{\mathrm{br}})}^2+{(y_{\mathrm{br}}-y_{\mathrm{cont}}^{\mathrm{br}})}^2}\end{array}\right.$

(3)计算车体轨迹上各个轨迹点与等高线中各点的欧式距离，找出距离最小的点，作为该轨迹曲线与等高线图CONTOURMAP的第一个交点同理，分别计算出四个轮胎的轨迹线与等高线的第一个交点由此可得到当前车体到的轨迹线以及四个轮胎到的轨迹线具体计算公式如下：

$\left\{\begin{array}{c}\underset{(x_{\mathrm{trai}}^v,y_{{\mathrm{tra}}_i}^v)\&Element;{\mathrm{tra}}_k^v}{\underset{(x_{{\mathrm{cont}}_i}^v,y_{{\mathrm{cont}}_i}^v)\&Element;{\mathrm{contset}}_l^v}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^v-x_{{\mathrm{cont}}_i}^v)}^2+{(y_{{\mathrm{tra}}_i}^v-y_{{\mathrm{cont}}_i}^v)}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{fl}},y_{{\mathrm{tra}}_i}^{\mathrm{fl}})\&Element;{\mathrm{tra}}_k^{\mathrm{fl}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{fl}},y_{{\mathrm{cont}}_i}^{\mathrm{fl}})\&Element;{\mathrm{contset}}_l^{\mathrm{fl}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{fl}}-x_{{\mathrm{cont}}_i}^{\mathrm{fl}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{fl}}-y_{{\mathrm{cont}}_i}^{\mathrm{fl}})}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{fr}},y_{{\mathrm{tra}}_i}^{\mathrm{fr}})\&Element;{\mathrm{tra}}_k^{\mathrm{fr}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{fr}},y_{{\mathrm{cont}}_i}^{\mathrm{fr}})\&Element;{\mathrm{contset}}_l^{\mathrm{fr}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{fr}}-x_{{\mathrm{cont}}_i}^{\mathrm{fr}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{fr}}-y_{{\mathrm{cont}}_i}^{\mathrm{fr}})}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{bl}},y_{{\mathrm{tra}}_i}^{\mathrm{bl}})\&Element;{\mathrm{tra}}_k^{\mathrm{bl}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{bl}},y_{{\mathrm{cont}}_i}^{\mathrm{bl}})\&Element;{\mathrm{contset}}_l^{\mathrm{bl}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{bl}}-x_{{\mathrm{cont}}_i}^{\mathrm{bl}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{bl}}-y_{{\mathrm{cont}}_i}^{\mathrm{bl}})}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{br}},y_{{\mathrm{tra}}_i}^{\mathrm{br}})\&Element;{\mathrm{tra}}_k^{\mathrm{br}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{br}},y_{{\mathrm{cont}}_i}^{\mathrm{br}})\&Element;{\mathrm{contset}}_l^{\mathrm{br}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{br}}-x_{{\mathrm{cont}}_i}^{\mathrm{br}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{br}}-y_{{\mathrm{cont}}_i}^{\mathrm{br}})}^2},i=1,...,n\end{array}\right.$

3、假设待分析的车体轨迹是计算车体轨迹段的基本地形因子基本地形因子包括粗糙度、坡度和起伏度，同理可相应得到轮胎轨迹段的基本地形因子

(1)计算车辆运行轨迹的坡度：由车体轨迹各个分片段的坡度值表示：

$\left\{\begin{array}{c}{\mathrm{slope}}_1^v=\arccos \left(\frac{\sqrt{{(x_2^v-x_1^v)}^2+{(y_2^v-y_1^v)}^2}}{\sqrt{{(x_2^v-x_1^v)}^2+{(y_2^v-y_1^v)}^2+{(z_2^v-z_1^v)}^2}}\right),\\ ...\\ {\mathrm{slope}}_{n_v-1}^v=\arccos \left(\frac{\sqrt{{(x_{n_v}^v-x_{n_v-1}^v)}^2+{(y_{n_v}^v-y_{n_v-1}^v)}^2}}{\sqrt{{(x_{n_v}^v-x_{n_v-1}^v)}^2+{(y_{n_v}^v-y_{n_v-1}^v)}^2+{(z_{n_v}^v-z_{n_v-1}^v)}^2}}\right),\end{array}\right.$

其中为车辆轨迹与等高线的交点坐标值。

(2)计算轮胎所走轨迹的地形粗糙度

$\left\{\begin{array}{c}{\mathrm{rough}}_k^{\mathrm{wl}}=\frac{\underset{i=1}{\overset{n_{\mathrm{fl}}}{\mathrm{\&Sigma;}}}{(z_i^{\mathrm{fl}}-\stackrel{\&OverBar;}{z_{\mathrm{fl}}})}^2}{n_{\mathrm{fl}}-1},\stackrel{\&OverBar;}{z_{\mathrm{fl}}}=\frac{\underset{i=1}{\overset{n_{\mathrm{fl}}}{\mathrm{\&Sigma;}}}{z}_i^{\mathrm{fl}}}{n_{\mathrm{fl}}}\\ {\mathrm{rough}}_k^{\mathrm{wr}}=\frac{\underset{i=1}{\overset{n_{\mathrm{wr}}}{\mathrm{\&Sigma;}}}{(z_i^{\mathrm{fr}}-\stackrel{\&OverBar;}{z_{\mathrm{fr}}})}^2}{n_{\mathrm{fr}}-1},\stackrel{\&OverBar;}{z_{\mathrm{fr}}}=\frac{\underset{i=1}{\overset{n_{\mathrm{fr}}}{\mathrm{\&Sigma;}}}{z}_i^{\mathrm{fr}}}{n_{\mathrm{fr}}}\end{array}\right.$

(3)计算车辆运行轨迹的起伏度，包括车体运行轨迹的起伏度左轮和右轮的起伏度

$\left\{\begin{array}{c}{\mathrm{waviness}}_k^v=z_{\max }^v-z_{\min }^v\\ {\mathrm{waviness}}_k^{\mathrm{wl}}=z_{\max }^{\mathrm{wl}}-z_{\min }^{\mathrm{wl}}\\ {\mathrm{waviness}}_k^{\mathrm{wr}}=z_{\max }^{\mathrm{wr}}-z_{\min }^{\mathrm{wr}}\end{array}\right.$

4、可通行性分析：得到各条轨迹曲线段的基本地形因子后，结合车辆的动力学约束和运动学约束，分别计算车辆通行各轨迹曲线段的代价，以此构造代价图。假设车辆的最大爬坡度和最大侧翻角是PITCH和ROLL，车辆在轨迹某一条轨迹tra_k上运行的代价为cost(tra_k)，其中tra_k包含车体轨迹和四个轮胎的轨迹再将cost(tra_k)存于最终代价图COSTMAP中：

$\cos t\left({\mathrm{tra}}_k\right)={\cos t}_{\mathrm{slope}}^k+{\cos t}_{\mathrm{rough}}^k+{\cos t}_{\mathrm{waviness}}^k+{\cos t}_{\mathrm{pitch}}^k+{\cos t}_{\mathrm{roll}}^k+{\cos t}_{\mathrm{path}}^k$

其中表示车体通过该条轨迹坡度代价，坡度值由公式三得到。表示由左轮和右轮通过粗糙地形的轨迹代价。表示由左轮和右轮通过起伏不定的地形的轨迹代价，起伏度由公式五得到。和表示车辆通过该轨迹的所有位置点的爬坡代价和侧翻代价。表示车辆通过的路径长度代价：

$\left\{\begin{array}{c}{\cos t}_{\mathrm{slope}}^k=\underset{i=1}{\overset{n_i^v-1}{\mathrm{\&Sigma;}}}{\mathrm{slope}}_i^v\&times;W_{\mathrm{slope}}\\ {\cos t}_{\mathrm{rough}}^k=\underset{i=1}{\overset{n_i^{\mathrm{fl}}}{\mathrm{\&Sigma;}}}{\mathrm{rough}}_i^{\mathrm{wl}}\&times;W_{\mathrm{rough}}+\underset{i=1}{\overset{n_i^{\mathrm{fr}}}{\mathrm{\&Sigma;}}}{\mathrm{rough}}_i^{\mathrm{wr}}\&times;W_{\mathrm{rough}}\\ {\cos t}_{\mathrm{waviness}}^k={\mathrm{wavniess}}_k^{\mathrm{wl}}\&times;W_{\mathrm{waviness}}+{\mathrm{wavniess}}_k^{\mathrm{wr}}\&times;W_{\mathrm{waviness}}\\ {\cos t}_{\mathrm{pitch}}^k=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{\arcsin \left(\frac{|z_i^{\mathrm{fl}}-z_i^{\mathrm{bl}}|+|z_i^{\mathrm{fr}}-z_i^{\mathrm{br}}|}{2\&times;L}\right)}{\mathrm{PITCH}}\&times;W_{\mathrm{pitch}}\\ {\cos t}_{\mathrm{roll}}^k=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{\arcsin \left(\frac{|z_i^{\mathrm{fl}}-z_i^{\mathrm{fr}}|+|z_i^{\mathrm{bl}}-z_i^{\mathrm{br}}|}{2\&times;W}\right)}{\mathrm{ROLL}}\&times;W_{\mathrm{roll}}\\ {\cos t}_{\mathrm{path}}^k={\cos t}_{\mathrm{lpath}}^k+{\cos t}_{\mathrm{rpath}}^k\\ {\cos t}_{\mathrm{lpath}}^k=\underset{i=1}{\overset{n_{i-1}^{\mathrm{fl}}}{\mathrm{\&Sigma;}}}\sqrt{{(x_{i+1}^{\mathrm{fl}}-x_i^{\mathrm{fl}})}^2+{(y_{i+1}^{\mathrm{fl}}-y_i^{\mathrm{fl}})}^2+{(z_{i+1}^{\mathrm{fl}}-z_i^{\mathrm{fl}})}^2}\&times;W_{\mathrm{path}}\\ {\cos t}_{\mathrm{rpath}}^k=\underset{i=1}{\overset{n_{i-1}^{\mathrm{fr}}}{\mathrm{\&Sigma;}}}\sqrt{{(x_{i+1}^{\mathrm{fr}}-x_i^{\mathrm{fr}})}^2+{(y_{i+1}^{\mathrm{fr}}-y_i^{\mathrm{fr}})}^2+{(z_{i+1}^{\mathrm{fr}}-z_i^{\mathrm{fr}})}^2}\&times;W_{\mathrm{path}}\end{array}\right..$

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (6)

1.一种自主车三维地形可通行性分析方法，其特征在于，步骤为：

(1)信息感知：获取当前车辆运动的状态信息及环境信息；所述环境信息是采集到的地形高程数据集D＝{(x_i,y_i,z_i)^T|x_i,y_i,z_i∈R,i＝1,...,n}，所述车辆运动的状态信息为conf＝(x_v,y_v,yaw)，其中(x_v,y_v)为当前时刻的车辆的水平面位置、yaw为航向角，并根据信息感知得到的地形高程数据，绘制出当前地形的等高线；

(2)轨迹搜索与生成：根据当前车辆位置与最终目标位置进行轨迹初步搜索，生成局部车辆可能运动的轨迹集合，包括车体运动轨迹和轮胎运动轨迹；

(3)地形分析：由步骤(2)产生轨迹，并根据车速v找到这些轨迹与等高线的交点，然后分析在等高线下所述交点与车当前位置形成的轨迹曲线段的地形特性，计算基本地形因子；

(4)可通行性分析：经过步骤(3)得到各条轨迹曲线段的基本地形因子后，结合车辆的动力学约束和运动学约束，分别计算车辆通行各轨迹曲线段的代价，以此构造代价图；

(5)路径规划：以步骤(4)中得到的代价图作为启发式信息，寻找一条通向下一个目标点的代价最小、长度最短的最优路径。

2.根据权利要求1所述的自主车三维地形可通行性分析方法，其特征在于，所述步骤(3)的具体流程为：

(3.1)读取由步骤(1)得到DEM数据保存为矩阵变量GROUNDMAP，所述矩形变量GROUNDMAP包含大小都是m×n的子变量集分别由地形高程数据集D中的所有点的(x,y,z)值构成，其中m,n分别为地形图的长和宽，然后从矩阵变量GROUNDMAP中提取高程值z，绘制地形的等高线图CONTOURMAP；

(3.2)由步骤(2)生成的车体运动轨迹的集合和四个轮胎运动的轨迹集合：

${\mathrm{traset}}_{\mathrm{fl}}=({\mathrm{tra}}_l^{\mathrm{fl}},...,{\mathrm{tra}}_t^{\mathrm{fl}}),{\mathrm{traset}}_{\mathrm{fr}}=({\mathrm{tra}}_l^{\mathrm{fr}},...,{\mathrm{tra}}_t^{\mathrm{fr}}),{\mathrm{traset}}_{\mathrm{bl}}=({\mathrm{tra}}_l^{\mathrm{bl}},...,{\mathrm{tra}}_t^{\mathrm{bl}}),{\mathrm{traset}}_{\mathrm{br}}=({\mathrm{tra}}_l^{\mathrm{br}},...,{\mathrm{tra}}_t^{\mathrm{br}})$ 选择任一条轨迹分部求出轨迹曲线与等高线图CONTOURMAP的第一个交点进而分别得到车体和轮胎到相应点的轨迹段 ${\mathrm{tra}}_k^{v^{\&prime;}},{\mathrm{tra}}_k^{{\mathrm{fl}}^{\&prime;}},{\mathrm{tra}}_k^{{\mathrm{fr}}^{\&prime;}},{\mathrm{tra}}_k^{{\mathrm{bl}}^{\&prime;}},{\mathrm{tra}}_k^{{\mathrm{br}}^{\&prime;}}({\mathrm{tra}}_k^{v^{\&prime;}}\&SubsetEqual;{\mathrm{tra}}_k^v,{\mathrm{tra}}_k^{{\mathrm{fl}}^{\&prime;}}\&SubsetEqual;{\mathrm{tra}}_k^{\mathrm{fl}},{\mathrm{tra}}_k^{{\mathrm{fr}}^{\&prime;}}\&SubsetEqual;{\mathrm{tra}}_k^{\mathrm{fr}},{\mathrm{tra}}_k^{{\mathrm{bl}}^{\&prime;}}\&SubsetEqual;{\mathrm{tra}}_k^{\mathrm{bl}},{\mathrm{tra}}_k^{{\mathrm{br}}^{\&prime;}}\&SubsetEqual;{\mathrm{tra}}_k^{\mathrm{br}});$

(3.3)计算车体轨迹段的基本地形因子同理可相应得到轮胎轨迹段的基本地形因子

3.根据权利要求2所述的自主车三维地形可通行性分析方法，其特征在于，所述步骤(3.1)的具体流程为：

(3.1.1)从所述等高线图GROUNDMAP的子变量中找出最小的z值zmin和最大的zmax值，设置步长△d，l＝1，创建临时矩阵变量zimage，该矩阵的大小与所述等高线图GROUNDMAP中子变量集大小一致，即 $\mathrm{zimage}=\left\{{\mathrm{zimg}}_{\mathrm{ij}}\right|{\mathrm{zimg}}_{\mathrm{ij}}\&Element;\stackrel{\&RightArrow;}{z},i=1,...,m,j=1,...,n\};$

(3.1.2)选择区间[zmin+△d×(l-1),zmin+△d×l)，且zmin+△d×l<zmax，遍历所述等高线图GROUNDMAP，找出落在该区间的高程点z_i,j(i＝1,...,m,j＝1,...,n)，将zimage中的zimg_i,j置为l，并保存到集合zset_i，l＝l+1；

(3.1.3)循环步骤(3.1.2)，当zmin+△d×l>zmax时，转至(3.1.4)；

(3.1.4)遍历zimage，提取其元素值都为vl(vl＝1,...,l)的元素索引集根据从所述等高线图GROUNDMAP的子变量集中找到相应的坐标点集，构成第vl条等高线的初始坐标点集 ${\mathrm{contset}}_{\mathrm{vl}}=\left\{(x_{\mathrm{ij}},y_{\mathrm{ij}})\right|x_{\mathrm{ij}}\&Element;\stackrel{\&RightArrow;}{x},y_{\mathrm{ij}}\&Element;\stackrel{\&RightArrow;}{y},i\&Element;\stackrel{\&RightArrow;}{i},j\&Element;\stackrel{\&RightArrow;}{j}\},{\mathrm{contset}}_{\mathrm{vl}}\&SubsetEqual;\mathrm{constset},$ 并对该条等高线内的初始坐标点进行升序排序；

(3.1.5)设contset_vl中有m个坐标点，使用链码组织contset_vl中的点，用光滑的曲线拟合这些点，形成相应的等高线，叠加l条等高线，得到最终该地形的等高线图CONTOURMAP。

4.根据权利要求2所述的自主车三维地形可通行性分析方法，其特征在于，所述步骤(3.2)的具体流程为：

(3.2.1)假设从轨迹集中任一条轨迹包含n(n<<m)个轨迹点，并且这些点的坐标是已知的，即(x_i,y_i)∈tra_k,i＝1,...,n；又由步骤(1)中车辆的运动状态信息conf＝(x_v,y_v,yaw)得到当前车辆的X-Y平面坐标(x_v,y_v)及其航向角，计算出车辆的左前轮、右前轮、左后轮和右后轮的X-Y平面重心坐标，分别为(x_fl,y_fl),(x_fr,y_fr),(x_bl,y_bl),(x_br,y_br)，并且轮胎的前进方向与车体的一致，将车体航向角yaw作为自主车的主方向；

(3.2.2)将车体重心坐标(x_v,y_v)与地形等高线图CONTOURMAP中各等高线的坐标集内与车体同一水平线上的坐标比较，找出其欧式距离最小的坐标及其所属的等高线确定出离车辆最近的等高线；同理，找出离四个轮胎最近的等高线 ${\mathrm{contset}}_l^{\mathrm{fl}},{\mathrm{contset}}_l^{\mathrm{fr}},{\mathrm{contset}}_l^{\mathrm{bl}},{\mathrm{contset}}_l^{\mathrm{br}}:$

$\left\{\begin{array}{c}\underset{{\mathrm{conset}}_l^v\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^v,y_{\mathrm{cont}}^v)\&Element;{\mathrm{contset}}_l^v}{\arg \min }}\sqrt{{(x_v-x_{\mathrm{cont}}^v)}^2+{(y_v-y_{\mathrm{cont}}^v)}^2}\\ \underset{{\mathrm{conset}}_l^{\mathrm{fl}}\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{fl}},y_{\mathrm{cont}}^{\mathrm{fl}})\&Element;{\mathrm{contset}}_l^{\mathrm{fl}}}{\arg \min }}\sqrt{{(x_{\mathrm{fl}}-x_{\mathrm{cont}}^{\mathrm{fl}})}^2+{(y_{\mathrm{fl}}-y_{\mathrm{cont}}^{\mathrm{fl}})}^2}\\ \underset{{\mathrm{conset}}_l^{\mathrm{fr}}\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{fr}},y_{\mathrm{cont}}^{\mathrm{fr}})\&Element;{\mathrm{contset}}_l^{\mathrm{fr}}}{\arg \min }}\sqrt{{(x_{\mathrm{fr}}-x_{\mathrm{cont}}^{\mathrm{fr}})}^2+{(y_{\mathrm{fr}}-y_{\mathrm{cont}}^{\mathrm{fr}})}^2}\\ \underset{{\mathrm{conset}}_l^3\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{bl}},y_{\mathrm{cont}}^{\mathrm{bl}})\&Element;{\mathrm{contset}}_l^{\mathrm{bl}}}{\arg \min }}\sqrt{{(x_{\mathrm{bl}}-x_{\mathrm{cont}}^{\mathrm{bl}})}^2+{(y_{\mathrm{bl}}-y_{\mathrm{cont}}^{\mathrm{bl}})}^2}\\ \underset{{\mathrm{conset}}_l^{\mathrm{br}}\&Subset;\mathrm{conset}}{\underset{(x_{\mathrm{cont}}^{\mathrm{br}},y_{\mathrm{cont}}^{\mathrm{br}})\&Element;{\mathrm{contset}}_l^{\mathrm{br}}}{\arg \min }}\sqrt{{(x_{\mathrm{br}}-x_{\mathrm{cont}}^{\mathrm{br}})}^2+{(y_{\mathrm{br}}-y_{\mathrm{cont}}^{\mathrm{br}})}^2}\end{array}\right.$

(3.2.3)计算车体轨迹上各个轨迹点与等高线中各点的欧式距离，找出距离最小的点，作为该轨迹曲线与等高线图CONTOURMAP的第一个交点同理，分别计算出四个轮胎的轨迹线与等高线的第一个交点得到当前车体到的轨迹线以及四个轮胎到的轨迹线

$\left\{\begin{array}{c}\underset{(x_{\mathrm{trai}}^v,y_{{\mathrm{tra}}_i}^v)\&Element;{\mathrm{tra}}_k^v}{\underset{(x_{{\mathrm{cont}}_i}^v,y_{{\mathrm{cont}}_i}^v)\&Element;{\mathrm{contset}}_l^v}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^v-x_{{\mathrm{cont}}_i}^v)}^2+{(y_{{\mathrm{tra}}_i}^v-y_{{\mathrm{cont}}_i}^v)}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{fl}},y_{{\mathrm{tra}}_i}^{\mathrm{fl}})\&Element;{\mathrm{tra}}_k^{\mathrm{fl}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{fl}},y_{{\mathrm{cont}}_i}^{\mathrm{fl}})\&Element;{\mathrm{contset}}_l^{\mathrm{fl}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{fl}}-x_{{\mathrm{cont}}_i}^{\mathrm{fl}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{fl}}-y_{{\mathrm{cont}}_i}^{\mathrm{fl}})}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{fr}},y_{{\mathrm{tra}}_i}^{\mathrm{fr}})\&Element;{\mathrm{tra}}_k^{\mathrm{fr}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{fr}},y_{{\mathrm{cont}}_i}^{\mathrm{fr}})\&Element;{\mathrm{contset}}_l^{\mathrm{fr}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{fr}}-x_{{\mathrm{cont}}_i}^{\mathrm{fr}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{fr}}-y_{{\mathrm{cont}}_i}^{\mathrm{fr}})}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{bl}},y_{{\mathrm{tra}}_i}^{\mathrm{bl}})\&Element;{\mathrm{tra}}_k^{\mathrm{bl}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{bl}},y_{{\mathrm{cont}}_i}^{\mathrm{bl}})\&Element;{\mathrm{contset}}_l^{\mathrm{bl}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{bl}}-x_{{\mathrm{cont}}_i}^{\mathrm{bl}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{bl}}-y_{{\mathrm{cont}}_i}^{\mathrm{bl}})}^2},i=1,...,n\\ \underset{(x_{\mathrm{trai}}^{\mathrm{br}},y_{{\mathrm{tra}}_i}^{\mathrm{br}})\&Element;{\mathrm{tra}}_k^{\mathrm{br}}}{\underset{(x_{{\mathrm{cont}}_i}^{\mathrm{br}},y_{{\mathrm{cont}}_i}^{\mathrm{br}})\&Element;{\mathrm{contset}}_l^{\mathrm{br}}}{\arg \min }}\sqrt{{(x_{{\mathrm{tra}}_i}^{\mathrm{br}}-x_{{\mathrm{cont}}_i}^{\mathrm{br}})}^2+{(y_{{\mathrm{tra}}_i}^{\mathrm{br}}-y_{{\mathrm{cont}}_i}^{\mathrm{br}})}^2},i=1,...,n\end{array}\right..$

5.根据权利要求2所述的自主车三维地形可通行性分析方法，其特征在于，所述步骤(3.3)的具体流程为：

假设车辆的下一条运行轨迹为根据车辆速度v找出该轨迹与等高线的交点(a_v,b_v,...)，这些交点的坐标值为同理，分别得到左前轮、右前轮、左后轮和右后轮轨迹与等高线的交点(a_fl,b_fl,...),(a_fr,b_fr,...),(a_bl,b_bl,...),(a_br,b_br,...)，相应的交点坐标值分别为 $\{(x_1^{\mathrm{fl}},y_1^{\mathrm{fl}},z_1^{\mathrm{fl}}),...,(x_{n_{\mathrm{fl}}}^{\mathrm{fl}},y_{n_{\mathrm{fl}}}^{\mathrm{fl}},z_{n_{\mathrm{fl}}}^{\mathrm{fl}})\},\{(x_1^{\mathrm{fr}},y_1^{\mathrm{fr}},z_1^{\mathrm{fr}}),...,(x_{n_{\mathrm{fr}}}^{\mathrm{fr}},y_{n_{\mathrm{fr}}}^{\mathrm{fr}},z_{n_{\mathrm{fr}}}^{\mathrm{fr}})\},\{(x_1^{\mathrm{bl}},y_1^{\mathrm{bl}},z_1^{\mathrm{bl}}),...,(x_{n_{\mathrm{bl}}}^{\mathrm{bl}},y_{n_{\mathrm{bl}}}^{\mathrm{bl}},z_{n_{\mathrm{bl}}}^{\mathrm{bl}})\},\{(x_1^{\mathrm{br}},y_1^{\mathrm{br}},z_1^{\mathrm{br}}),...,(x_{n_{\mathrm{br}}}^{\mathrm{br}},y_{n_{\mathrm{br}}}^{\mathrm{br}},z_{n_{\mathrm{br}}}^{\mathrm{br}})\};$

(3.3.1)计算车辆运行轨迹的坡度：由车体轨迹各个分片段的坡度值表示：

$\left\{\begin{array}{c}{\mathrm{slope}}_1^v=\arccos \left(\frac{\sqrt{{(x_2^v-x_1^v)}^2+{(y_2^v-y_1^v)}^2}}{\sqrt{{(x_2^v-x_1^v)}^2+{(y_2^v-y_1^v)}^2+{(z_2^v-z_1^v)}^2}}\right),\\ ...\\ {\mathrm{slope}}_{n_v-1}^v=\arccos \left(\frac{\sqrt{{(x_{n_v}^v-x_{n_v-1}^v)}^2+{(y_{n_v}^v-y_{n_v-1}^v)}^2}}{\sqrt{{(x_{n_v}^v-x_{n_v-1}^v)}^2+{(y_{n_v}^v-y_{n_v-1}^v)}^2+{(z_{n_v}^v-z_{n_v-1}^v)}^2}}\right),\end{array}\right.$

(3.3.2)计算车辆运行轨迹的粗糙度，主要考虑到轮胎所走轨迹的地形粗糙度，此外左前轮和左后轮、右前轮和右后轮所走的轨迹是一样的，只需分别计算左前轮和右前轮轨迹的粗糙度作为左轮和右轮的粗糙度

$\left\{\begin{array}{c}{\mathrm{rough}}_k^{\mathrm{wl}}=\frac{\underset{i=1}{\overset{n_{\mathrm{fl}}}{\mathrm{\&Sigma;}}}{(z_i^{\mathrm{fl}}-\stackrel{\&OverBar;}{z_{\mathrm{fl}}})}^2}{n_{\mathrm{fl}}-1},\stackrel{\&OverBar;}{z_{\mathrm{fl}}}=\frac{\underset{i=1}{\overset{n_{\mathrm{fl}}}{\mathrm{\&Sigma;}}}{z}_i^{\mathrm{fl}}}{n_{\mathrm{fl}}}\\ {\mathrm{rough}}_k^{\mathrm{wr}}=\frac{\underset{i=1}{\overset{n_{\mathrm{wr}}}{\mathrm{\&Sigma;}}}{(z_i^{\mathrm{fr}}-\stackrel{\&OverBar;}{z_{\mathrm{fr}}})}^2}{n_{\mathrm{fr}}-1},\stackrel{\&OverBar;}{z_{\mathrm{fr}}}=\frac{\underset{i=1}{\overset{n_{\mathrm{fr}}}{\mathrm{\&Sigma;}}}{z}_i^{\mathrm{fr}}}{n_{\mathrm{fr}}}\end{array}\right.$

(3.3.3)计算车辆运行轨迹的起伏度：计算车体运行轨迹的起伏度左轮和右轮的起伏度同步骤(3.3.2)中分析，只需分别计算左前轮和右前轮的起伏度作为左轮和右轮的起伏度：

$\left\{\begin{array}{c}{\mathrm{waviness}}_k^v=z_{\max }^v-z_{\min }^v\\ {\mathrm{waviness}}_k^{\mathrm{wl}}=z_{\max }^{\mathrm{wl}}-z_{\min }^{\mathrm{wl}}\\ {\mathrm{waviness}}_k^{\mathrm{wr}}=z_{\max }^{\mathrm{wr}}-z_{\min }^{\mathrm{wr}}\end{array}\right..$

6.根据权利要求1～5中任意一项所述的自主车三维地形可通行性分析方法，其特征在于，假设车辆当前位置左前轮、右前轮、左后轮和右后轮的高程值分别为z_fl,z_fr,z_bl,z_br，所述步骤(4)的具体流程为：

(4.1)计算当前车辆的爬坡度pitch，设自主车的左前轮与左后轮、或右前轮与右后轮的间距为L：

$\mathrm{pitch}=\arcsin \left(\frac{|z_{\mathrm{fl}}-z_{\mathrm{bl}}|+|z_{\mathrm{fr}}-z_{\mathrm{br}}|}{2\&times;L}\right)$

(4.2)计算当前车辆的侧翻角roll，设自主车的设自主车的左前轮与右前轮、右前轮与左后轮的间距为的间距为W：

$\mathrm{roll}=\arcsin \left(\frac{|z_{\mathrm{fl}}-z_{\mathrm{fr}}|+|z_{\mathrm{bl}}-z_{\mathrm{br}}|}{2\&times;W}\right)$

(4.3)计算车辆运行轨迹代价，设自主车的最大爬坡度和最大侧翻角是PITCH和ROLL，车辆在轨迹某一条轨迹tra_k上运行的代价为cost(tra_k)，其中tra_k包含车体轨迹和四个轮胎的轨迹再将cost(tra_k)存于最终代价图COSTMAP中：

$\cos t\left({\mathrm{tra}}_k\right)={\cos t}_{\mathrm{slope}}^k+{\cos t}_{\mathrm{rough}}^k+{\cos t}_{\mathrm{waviness}}^k+{\cos t}_{\mathrm{pitch}}^k+{\cos t}_{\mathrm{roll}}^k+{\cos t}_{\mathrm{path}}^k$

其中，表示车体通过该条轨迹坡度代价；表示由左轮和右轮通过粗糙地形的代价；表示由左轮和右轮通过起伏不定的地形的代价；和表示车辆通过该轨迹的所有位置点的爬坡代价和侧翻代价；表示车辆通过的路径长度代价：

$\left\{\begin{array}{c}{\cos t}_{\mathrm{slope}}^k=\underset{i=1}{\overset{n_i^v-1}{\mathrm{\&Sigma;}}}{\mathrm{slope}}_i^v\&times;W_{\mathrm{slope}}\\ {\cos t}_{\mathrm{rough}}^k=\underset{i=1}{\overset{n_i^{\mathrm{fl}}}{\mathrm{\&Sigma;}}}{\mathrm{rough}}_i^{\mathrm{wl}}\&times;W_{\mathrm{rough}}+\underset{i=1}{\overset{n_i^{\mathrm{fr}}}{\mathrm{\&Sigma;}}}{\mathrm{rough}}_i^{\mathrm{wr}}\&times;W_{\mathrm{rough}}\\ {\cos t}_{\mathrm{waviness}}^k={\mathrm{wavniess}}_k^{\mathrm{wl}}\&times;W_{\mathrm{waviness}}+{\mathrm{wavniess}}_k^{\mathrm{wr}}\&times;W_{\mathrm{waviness}}\\ {\cos t}_{\mathrm{pitch}}^k=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{\arcsin \left(\frac{|z_i^{\mathrm{fl}}-z_i^{\mathrm{bl}}|+|z_i^{\mathrm{fr}}-z_i^{\mathrm{br}}|}{2\&times;L}\right)}{\mathrm{PITCH}}\&times;W_{\mathrm{pitch}}\\ {\cos t}_{\mathrm{roll}}^k=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{\arcsin \left(\frac{|z_i^{\mathrm{fl}}-z_i^{\mathrm{fr}}|+|z_i^{\mathrm{bl}}-z_i^{\mathrm{br}}|}{2\&times;W}\right)}{\mathrm{ROLL}}\&times;W_{\mathrm{roll}}\\ {\cos t}_{\mathrm{path}}^k={\cos t}_{\mathrm{lpath}}^k+{\cos t}_{\mathrm{rpath}}^k\\ {\cos t}_{\mathrm{lpath}}^k=\underset{i=1}{\overset{n_{i-1}^{\mathrm{fl}}}{\mathrm{\&Sigma;}}}\sqrt{{(x_{i+1}^{\mathrm{fl}}-x_i^{\mathrm{fl}})}^2+{(y_{i+1}^{\mathrm{fl}}-y_i^{\mathrm{fl}})}^2+{(z_{i+1}^{\mathrm{fl}}-z_i^{\mathrm{fl}})}^2}\&times;W_{\mathrm{path}}\\ {\cos t}_{\mathrm{rpath}}^k=\underset{i=1}{\overset{n_{i-1}^{\mathrm{fr}}}{\mathrm{\&Sigma;}}}\sqrt{{(x_{i+1}^{\mathrm{fr}}-x_i^{\mathrm{fr}})}^2+{(y_{i+1}^{\mathrm{fr}}-y_i^{\mathrm{fr}})}^2+{(z_{i+1}^{\mathrm{fr}}-z_i^{\mathrm{fr}})}^2}\&times;W_{\mathrm{path}}\end{array}\right..$