CN116935683A - 一种无人越野车辆自主行驶的安全行为检测方法 - Google Patents
一种无人越野车辆自主行驶的安全行为检测方法 Download PDFInfo
- Publication number
- CN116935683A CN116935683A CN202311088074.0A CN202311088074A CN116935683A CN 116935683 A CN116935683 A CN 116935683A CN 202311088074 A CN202311088074 A CN 202311088074A CN 116935683 A CN116935683 A CN 116935683A
- Authority
- CN
- China
- Prior art keywords
- road vehicle
- unmanned
- constraint
- ground
- track point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 50
- 230000003068 static effect Effects 0.000 claims abstract description 19
- 238000000034 method Methods 0.000 claims description 49
- 230000001133 acceleration Effects 0.000 claims description 26
- 239000011159 matrix material Substances 0.000 claims description 14
- 230000000087 stabilizing effect Effects 0.000 claims description 14
- 238000010586 diagram Methods 0.000 claims description 10
- 230000009194 climbing Effects 0.000 claims description 9
- 230000036544 posture Effects 0.000 claims description 9
- 238000012876 topography Methods 0.000 claims description 8
- 230000007704 transition Effects 0.000 claims description 7
- 230000006399 behavior Effects 0.000 description 35
- 230000008569 process Effects 0.000 description 15
- 230000006872 improvement Effects 0.000 description 9
- 238000012544 monitoring process Methods 0.000 description 8
- 230000005484 gravity Effects 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 6
- 230000009471 action Effects 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 3
- 239000000725 suspension Substances 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000002829 reductive effect Effects 0.000 description 2
- PCTMTFRHKVHKIS-BMFZQQSSSA-N (1s,3r,4e,6e,8e,10e,12e,14e,16e,18s,19r,20r,21s,25r,27r,30r,31r,33s,35r,37s,38r)-3-[(2r,3s,4s,5s,6r)-4-amino-3,5-dihydroxy-6-methyloxan-2-yl]oxy-19,25,27,30,31,33,35,37-octahydroxy-18,20,21-trimethyl-23-oxo-22,39-dioxabicyclo[33.3.1]nonatriaconta-4,6,8,10 Chemical compound C1C=C2C[C@@H](OS(O)(=O)=O)CC[C@]2(C)[C@@H]2[C@@H]1[C@@H]1CC[C@H]([C@H](C)CCCC(C)C)[C@@]1(C)CC2.O[C@H]1[C@@H](N)[C@H](O)[C@@H](C)O[C@H]1O[C@H]1/C=C/C=C/C=C/C=C/C=C/C=C/C=C/[C@H](C)[C@@H](O)[C@@H](C)[C@H](C)OC(=O)C[C@H](O)C[C@H](O)CC[C@@H](O)[C@H](O)C[C@H](O)C[C@](O)(C[C@H](O)[C@H]2C(O)=O)O[C@H]2C1 PCTMTFRHKVHKIS-BMFZQQSSSA-N 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000005065 mining Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/09—Arrangements for giving variable traffic instructions
- G08G1/0962—Arrangements for giving variable traffic instructions having an indicator mounted inside the vehicle, e.g. giving voice messages
- G08G1/0967—Systems involving transmission of highway information, e.g. weather, speed limits
- G08G1/096708—Systems involving transmission of highway information, e.g. weather, speed limits where the received information might be used to generate an automatic action on the vehicle control
- G08G1/096725—Systems involving transmission of highway information, e.g. weather, speed limits where the received information might be used to generate an automatic action on the vehicle control where the received information generates an automatic action on the vehicle control
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/01—Detecting movement of traffic to be counted or controlled
- G08G1/0104—Measuring and analyzing of parameters relative to traffic conditions
- G08G1/0108—Measuring and analyzing of parameters relative to traffic conditions based on the source of data
- G08G1/0112—Measuring and analyzing of parameters relative to traffic conditions based on the source of data from the vehicle, e.g. floating car data [FCD]
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Atmospheric Sciences (AREA)
- Chemical & Material Sciences (AREA)
- Analytical Chemistry (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种无人越野车辆自主行驶的安全行为检测方法，属于车辆安全控制技术领域，解决了现有未考虑复杂地形和路面条件导致安全检测不足的问题。包括：获取规划路线上未检测的轨迹点序列；根据地面模型计算出各轨迹点的位姿和接触点数据；根据位姿依次检测无人越野车辆在各轨迹点是否满足静态位姿约束和动态稳定约束，如果轨迹点在障碍地形内，则根据接触点数据、障碍地形的尺寸和预设的障碍地形通行序列，检测障碍地形内的各轨迹点是否满足通行约束；当任意一个约束不满足，则将已检测的轨迹点标记为危险，否则标记为安全；输出已标记的轨迹点序列，对下一未检测的轨迹点序列进行安全行为检测。实现了多场景安全行为检测。
Description
技术领域
本发明涉及车辆安全控制技术领域，尤其涉及一种无人越野车辆自主行驶的安全行为检测方法。
背景技术
无人驾驶技术起源于上世纪七十年代，经过半个世纪的发展，如今无人驾驶技术框架基本成熟，形成了感知与定位、决策与规划、控制与监测三个关键技术部分。基于无人驾驶技术的无人越野车辆如今广泛应用于城市物流、矿区运输、野外探测及军事等多个领域。在城市环境中的无人驾驶技术考虑的是二维环境中的交互关系，目前相关技术较成熟，应用也较多。除了城市场景的自动驾驶车辆，越野无人驾驶汽车在探险和野外研究领域具有巨大潜力。在极端地形和条件下，使用无人驾驶车辆可以探索和收集数据，而无需将研究人员置于危险环境中。这些车辆可以用于地质勘探、野生动物观察、自然灾害监测等领域。而应用于野外探测越野车辆需要具备在复杂地形和路面条件下的行驶与作业能力，需要具备起伏路面行驶、翻越垂直墙、跨越壕沟等工况下的通过能力，需要综合考虑车辆的通过性与三维环境信息，目前相关的研究还不够充分，对于最终路径生成与任务完成的考虑不够完善。
越野车辆作为在野外实现无人驾驶功能与完成任务的平台，常需要在具有较大风险的复杂路面场景下行驶，此时一些不合理的行为不仅会使无人驾驶越野车辆的机动效率变低，甚至会使其处于违反车辆安全性和稳定性的危险状态，造成侧翻等严重后果，丧失行驶与任务完成能力。这类问题严重危害了无人驾驶越野车辆在复杂路面场景下的作业效率与自身安全性，因此在进行相关行为之前，对于越野车辆在复杂场景下行驶时的安全行为检测是至关重要的。
针对这些技术问题，现有的技术主要是基于传感器数据的实时监测和控制策略。例如，利用惯导、力传感器、悬架位移传感器等获取本体感知数据。同时，利用动力学微分模型对车辆当前行驶状态进行安全性判断，以实现对车辆行驶状态的实时安全检测。然而，在复杂地形和路面条件下，这些方法无法对车辆未来轨迹的安全性进行有效安全性检测，从而无法在危险发生之前进行发出警报而无法及时纠正危险行为。
发明内容
鉴于上述的分析，本发明实施例旨在提供一种无人越野车辆自主行驶的安全行为检测方法，用以解决现有安全检测未考虑复杂地形和路面条件导致安全检测不足的问题。
本发明实施例提供一种无人越野车辆自主行驶的安全行为检测方法，包括以下步骤：
按照固定时间间隔获取规划路线上未检测的轨迹点序列；
根据地面点云拟合的地面模型，计算出无人越野车辆在各轨迹点的位姿和接触点数据；根据位姿依次检测无人越野车辆在各轨迹点是否满足静态位姿约束和动态稳定约束，如果轨迹点在障碍地形内，则继续根据接触点数据、障碍地形的尺寸和预设的障碍地形通行序列，检测障碍地形内的各轨迹点是否满足通行约束；当任意一个约束不满足，则将已检测的轨迹点标记为危险，否则，标记为安全；
各轨迹点均已检测后输出已标记的轨迹点序列，对下一未检测的轨迹点序列进行安全行为检测。
基于上述方法的进一步改进，根据地面点云拟合的地面模型，计算出无人越野车辆在各轨迹点的位姿和接触点数据，包括：
构建由矩形单元组成的栅格地图，根据地面模型中的地面高度，获取无人越野车辆在每个栅格中底盘距离地面的高度，得到底盘高度图；
初始化旋转矩阵，根据地面模型和底盘高度图，获取无人越野车辆与地面的接触栅格作为接触点数据，根据接触点数据得到支撑多边形；
根据支撑多边形对位姿不稳定的轨迹点计算出旋转轴，并围绕旋转轴以固定角度旋转无人越野车辆，更新旋转矩阵和底盘高度图，重新获取接触点数据和支撑多边形，直至轨迹点位姿稳定，得到最终的接触点数据，并根据最终的旋转矩阵得到无人越野车辆在对应轨迹点的位姿。
基于上述方法的进一步改进，检测无人越野车辆在各轨迹点是否满足静态位姿约束，包括：无人越野车辆在各轨迹点的俯仰角是否小于纵向上坡稳定角和纵向下坡稳定角，以及，侧倾角是否小于横向稳定角；如果均小于，则满足静态位姿约束。
基于上述方法的进一步改进，纵向上坡稳定角和纵向下坡稳定角是根据无人越野车辆车体数据计算得到的极限上坡角和极限下坡角，分别与根据地面最大附着系数计算得到的极限纵向下滑角相比，得到的两个最小角；横向稳定角是根据无人越野车辆车体数据和质心偏移距离计算得到的极限横坡角，与根据地面横向附着系数计算的极限横向下滑角相比，得到的最小角。
基于上述方法的进一步改进，动态稳定约束，包括：
根据无人越野车辆纵向摩擦力的区间，计算出加速度的第一约束区间；
当地形因素和预期速度满足不侧滑的判别条件时，根据无人越野车辆的速度和地形因素，计算出加速度的第二约束区间；
根据无人越野车辆临界不侧翻条件，计算出速度的第一约束区间；
根据无人越野车辆与地面接触产生的支持力大于等于0，计算出速度的第二约束区间。
基于上述方法的进一步改进，地形因素是通过简化由无人越野车辆的纵向摩擦力和横向摩擦力的合力位于摩擦圆内而构建的二次不等式方程而得到，地形因素表示如下：
式中，c1、c2和c3表示地形因素；γ＝n*α，α表示无人越野车辆朝向向量，n表示无人越野车辆垂向轴向量；β表示路径主法向量，κ表示曲率，μ表示地面摩擦系数，g表示重力加速度，z表示重力加速度方向向量。
基于上述方法的进一步改进，检测方法还包括：
实时采集无人越野车辆的实际运行轨迹点和实际速度；
根据实际运行轨迹点和实际速度，分别检测是否满足轨迹差异约束和动态稳定约束中速度的约束区间，如果有任意一个约束不满足，则将实际运行轨迹点标记为危险；
当连续标记为危险的实际运行轨迹点达到数量阈值，则判定无人越野车辆行驶异常，发出警告。
基于上述方法的进一步改进，障碍地形包括垂直墙和壕沟；障碍地形的尺寸包括：根据地面点云和无人越野车辆拍摄的图像，估计出障碍地形中垂直墙高度、垂直墙的上下沿区域、壕沟宽度和壕沟的前后沿区域。
基于上述方法的进一步改进，根据接触点数据、障碍地形的尺寸和障碍地形通行序列，检测障碍地形内的各轨迹点是否满足通行约束，包括：
根据无人越野车辆驱动电机的输出扭矩和路面附着系数，计算出无人越野车辆前轮和后轮最大爬坡高度中的最小值，得到无人越野车辆翻越垂直墙的最大高度；根据无人越野车辆前轮和后轮跨越壕沟时驱动电机的输出扭矩以及无人越野车辆车轮直径分别计算出最大跨越宽度中的最小值，得到无人越野车辆跨越壕沟的最大宽度；
当无人越野车辆翻越垂直墙的最大高度大于估计的垂直墙高度，或，无人越野车辆跨越壕沟的最大宽度大于估计的壕沟宽度时，获取无人越野车辆在障碍地形内的各轨迹点的接触点数据，并根据障碍地形通行序列，比对出各轨迹点的接触点数据对应的状态；依次检测每相邻两个轨迹点的状态，记录不同的状态，得到实际通行序列；当实际通行序列存在于障碍地形通行序列中，则满足通行约束。
基于上述方法的进一步改进，障碍地形通行序列是基于翻越垂直墙和跨越壕沟的各阶段，采集有人驾驶模式下无人越野车辆在各阶段各车轮与地面的接触情况及各车轮与垂直墙和壕沟的相对位置而统计得到的车轮状态转移顺序。
与现有技术相比，本发明至少可实现如下有益效果之一：
1、通过静态和动态约束条件，对预期规划路径的轨迹点进行全方面的预期行为安全检测；使用垂直墙/壕沟通行性粗检测和障碍地形通行序列的精检测，实现了对障碍地形的多种安全行为的检测，应用场景广泛；这种精确到点的安全性分级，对错误的规划路线做出提前的预警，有利于无人越野车辆及时调整预期行为；
2、通过对无人越野车辆实时运行情况进行连续帧状态监测，减小系统和环境中扰动和不确定因素的影响，提高报警的准确率；
3、通过对预期行为安全和实时行为安全检测两方面进行安全监测，在复杂地形和路面条件下，可以更好的在危险发生之前发出警报而及时纠正危险行为。
本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件；
图1为本发明实施例中一种无人越野车辆自主行驶的安全行为检测方法流程图；
图2为本发明实施例中无人越野车辆的位姿在世界坐标系的示意图；
图3为本发明实施例中无人越野车辆前轮翻越垂直墙的受力示意图；
图4为本发明实施例中无人越野车辆后轮翻越垂直墙的受力示意图；
图5为本发明实施例中无人越野车辆前轮跨越壕沟状态受力示意图；
图6为本发明实施例中无人越野车辆后轮跨越壕沟状态受力示意图；
图7为本发明实施例中无人越野车辆翻越垂直墙的通行序列示意图；
图8为本发明实施例中无人越野车辆跨越壕沟的通行序列示意图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
本发明的一个具体实施例，公开了一种无人越野车辆自主行驶的安全行为检测方法，如图1所示，包括以下步骤：
S11、按照固定时间间隔获取规划路线上未检测的轨迹点序列；
S12、根据地面点云拟合的地面模型，计算出无人越野车辆在各轨迹点的位姿和接触点数据；根据位姿依次检测无人越野车辆在各轨迹点是否满足静态位姿约束和动态稳定约束，如果轨迹点在障碍地形内，则继续根据接触点数据、障碍地形的尺寸和预设的障碍地形通行序列，检测障碍地形内的各轨迹点是否满足通行约束；当任意一个约束不满足，则将已检测的轨迹点标记为危险，否则，标记为安全；
S13、各轨迹点均已检测后输出已标记的轨迹点序列，对下一未检测的轨迹点序列进行安全行为检测。
实施时，首先利用地面雷达点云与无人越野车辆总体参数实现在不同地形上无人越野车辆的位姿估计；然后同时从静态与动态方面计算各种地形下行驶位姿与速度的安全约束范围；在预期规划轨迹中与垂直墙/壕沟有交集时，通过障碍地形的尺寸和障碍地形通行序列进行通行可行性粗检测和精检测。通过上述三部分对预期行为打上安全或危险的标记，反映规划轨迹的安全状态。当存在危险状态时，将危险轨迹点和其违反的约束类型反馈给路径规划模块作为轨迹调整的参考，保证无人越野车辆的安全行进。
接下来，对步骤S11-S13分别进行说明。
在步骤S11中，按照固定的时间间隔从路径规划模块获取未来一段时间的规划路线，包括：各时刻轨迹点的x轴和y轴坐标及预期速度；根据每相邻两个轨迹点的预期速度，可以获取各轨迹点的预期加速度和朝向角。
将步骤S12细分为步骤S121-S125，其中，在步骤S121中获取到未检测的轨迹点序列中各轨迹点的位姿和接触点数据，步骤S122-S125的安全行为检测是一个迭代的过程，直至轨迹点序列中各轨迹点都被检测。
S121、根据地面点云拟合的地面模型，计算出无人越野车辆在各轨迹点的位姿和接触点数据。
需要说明的是，地面点云是通过对无人越野车辆上激光雷达采集的点云进行分割和提取而得到。本实施例中地面点云包括典型地形和障碍地形的地面点云，其中，典型地形包括平地和坡道，障碍地形包括垂直墙和壕沟。
本实施例不限定识别障碍地形的方法，可以根据实际环境制定识别规则，也可以采集无人越野车辆上安装的摄像头拍摄的图像，利用训练好的神经网络，将垂直墙和壕沟作为目标，识别出图像上的目标，根据目标框得到垂直墙和壕沟的区域范围；进一步地，再结合区域范围内的地面点云，估计出障碍地形中垂直墙高度、垂直墙的上下沿区域、壕沟宽度和壕沟的前后沿区域。
根据地面点云拟合地面模型，包括：采用三次B样条方法，将每个地面点云作为一个控制点，拟合出光滑和连续的曲面；将曲面划分为由矩形单元组成的栅格地图，将每个单元内的地面点云的高度平均值作为该栅格的高度；根据每个栅格的中心坐标和高度拟合出地面的数学模型，得到地面模型。
根据地面模型Mground，在指定位置(x,y)下，地面高度表示为Mground(x,y)。
进一步地，根据地面模型，基于高度迭代的位姿估计算法计算出无人越野车辆在各轨迹点的位姿和接触点数据，包括：
①构建由矩形单元组成的栅格地图，根据地面模型中的地面高度，获取无人越野车辆在每个栅格中底盘距离地面的高度，得到底盘高度图。
底盘高度图是车体坐标系下的栅格地图，根据地面模型中地面高度及无人越野车辆的尺寸，计算出无人越野车辆在每个栅格中底盘距离地面的高度，得到底盘高度图。根据底盘高度图Mvehicle，在指定位置(x,y)下，无人越野车辆距离地面的高度表示为Mvehicle(x,y)。
②初始化旋转矩阵，根据地面模型和底盘高度图，获取无人越野车辆与地面的接触栅格作为接触点数据，根据接触点数据得到支撑多边形。
具体来说，初始化俯仰角和侧倾角为0的旋转矩阵，高度设置为Hmax，Hmax高于任何地面点云的高度，即将无人越野车辆水平悬浮放置于距离地面Hmax高度的位置；随后根据Hmax更新底盘高度图Mvehicle中的高度值，即每个栅格都加Hmax；在指定无人越野车辆的位置和朝向角后，通过底盘高度图中每个栅格的对应的高度和地面模型对应的高度的差值，获取无人越野车辆底盘上每一个点距离地面的间隙hc，从中得到距离地面的最小间隙保持无人越野车辆当前俯仰和朝向角，将无人越野车辆的高度下降/>使无人越野车辆接触地面，并更新底盘高度图。
根据以下公式，获取小于或等于地面模型上对应位置高度与接触增量之和的栅格，作为接触点数据：
ptouch＝{(x,y)|Mvehicle(x,y)≤Mground(x,y)+δh} 公式(1)，
其中，δh表示接触增量，用于表示接触点估计的准确性和由于存在传感器噪声而产生的高度误差的鲁棒性之间的权衡。
支撑多边形是一个能够包围无人越野车辆所有接触点的最小凸包。如果只有一个接触点，则支撑多边形就是这一个接触点；如果有两个接触点，则支撑多边形为这两个点形成的线段；如果有超过两个的接触点，则使用Graham扫描对接触点进行凸包检测，得到支撑多边形。
③根据支撑多边形对位姿不稳定的轨迹点计算出旋转轴，并围绕旋转轴以固定角度旋转无人越野车辆，更新旋转矩阵和底盘高度图，重新获取接触点数据和支撑多边形，直至轨迹点位姿稳定，得到最终的接触点数据，并根据最终的旋转矩阵得到无人越野车辆在对应轨迹点的位姿。
具体来说，如果无人越野车辆质心在底盘高度图上的投影点在支撑多边形内部，则当前轨迹点位姿稳定，否则，位姿不稳定，根据投影点到支撑多边形的距离计算出旋转轴，包括：计算投影点到支撑多边形每个点和每条边的距离，如果最短距离对应的是点，则旋转轴是经过该点且与该点到投影点的直线正交的直线；否则，旋转轴是最短距离对应的边所在直线。为了准确计算出位姿，将无人越野车辆围绕旋转轴旋转的固定角度是一个预置的小角度，比如0.1rad，更新旋转矩阵和底盘高度图，迭代执行步骤②③，直至位姿稳定，得到最终的接触点数据和旋转矩阵。
最后，根据最终的接触点数据解析出无人越野车辆各车轮与地面的接触情况，包括：各车轮对应的接触点及各接触点的坐标；通过X-Y-Z的固定轴姿态矩阵的逆向推算方法，从最终的旋转矩阵中解析出无人越野车辆的俯仰角和侧倾角。结合步骤S11得到的无人越野车辆的位置和朝向角，以及，根据底盘高度图得到无人越野车辆在各轨迹点的高度，得到无人越野车辆在各轨迹点完整的位姿数据。
S122、取出一个未检测的轨迹点，根据位姿检测无人越野车辆在轨迹点是否满足静态位姿约束。
优选地，在本步骤检测之前，如果无人越野车辆在轨迹点处的接触点数据中包含除车轮以外的接触点数据，则说明无人越野车辆在此轨迹点会发生“托底”，判定为不安全行为，直接进入步骤S125，否则，再执行本步骤对无人越野车辆位姿安全进行粗判定。
需要说明的是，检测无人越野车辆在各轨迹点是否满足静态位姿约束，包括：无人越野车辆在各轨迹点的俯仰角是否小于纵向上坡稳定角和纵向下坡稳定角，以及，侧倾角是否小于横向稳定角；如果均小于，则满足静态位姿约束。其中，纵向上坡稳定角和纵向下坡稳定角是根据无人越野车辆车体数据计算得到的极限上坡角和极限下坡角，分别与根据地面最大附着系数计算得到的极限纵向下滑角相比，得到的两个最小角；横向稳定角是根据无人越野车辆车体数据和质心偏移距离计算得到的极限横坡角，与根据地面横向附着系数计算的极限横向下滑角相比，得到的最小角。
具体来说，本步骤考虑无人越野车辆不发生倾覆的纵向稳定性，通过以下公式得到无人越野车辆的极限上坡角α1和极限下坡角α2：
式中，lf为无人越野车辆的前轮着地点到质心的距离；hg为无人越野车辆质心高度；L为无人越野车辆车轮最远的轴距；纵坡长度大于等于L。
考虑无人越野车辆不发生下滑的纵向稳定性，通过以下公式得到极限纵向下滑角α′：
式中，为地面最大附着系数，与地面属性有关。因此无人越野车辆的纵向上坡稳定角/>纵向下坡稳定角/>
进一步地，本步骤考虑无人越野车辆不发生倾覆的横向稳定性，当无人越野车辆在横坡上开始倾覆时，重力作用线通过左侧履带外侧边缘，由此可得无人越野车辆的极限横坡角βh为：
式中，B表示轮距，d表示履带接地宽度或无人越野车辆车轮宽度，e表示质心偏离纵向平面的距离。
考虑无人越野车辆不发生侧滑的横向稳定性，通过以下公式得到极限横向下滑角β′：
式中，为地面横向附着系数，与地面属性有关。
因此，无人越野车辆的横向稳定角为βmax＝min(βh,β′)。
在安全检测前，获取到无人越野车辆车体数据和地面属性后，根据本步骤的公式计算出纵向上坡稳定角、纵向下坡稳定角和横向稳定角，在安全检测进入到本步骤时，直接根据无人越野车辆在轨迹点的位姿即可判断是否满足静态位姿约束。
S123、根据位姿检测无人越野车辆在轨迹点是否满足动态稳定约束。
需要说明的是，复杂地形环境中，在急弯、横纵坡以及起伏不平的路面，无人越野车辆经常会有俯仰、侧倾等姿态改变。因此为保证安全，越野环境中首先还需要考虑的是侧滑与侧翻约束。实际上无人越野车辆在非平坦路面驾车行驶时，车速一旦过快便会脱离地面，持续的脱离地面会产生更为强烈的颠簸，极大的影响乘坐舒适体验，极端情况下由于无人越野车辆腾空产生的失控也会导致翻车。因此还需考虑的是无人越野车辆与地面的接触约束。除此之外，越野环境中由于地面坡度的影响，还需额外考虑驱动力和制动力的限制约束。本步骤的动态稳定约束是基于汽车行驶方程式，从驱动力、侧滑、侧翻和地面接触四个方面计算出无人越野车辆的速度和/或加速度的约束区间，当无人越野车辆在轨迹点的速度和/或加速度均在各约束区间内，则是安全行为。
(1)建立汽车行驶方程式，获取无人越野车辆在车体坐标系下各个坐标轴上的力。
无人越野车辆在行驶过程中，所受重力作用于无人越野车辆质心处，其余所受外力作用在轮胎上，每个轮胎均受到三个坐标轴方向的力。本实施例忽略悬架作用，认为每个轮胎上的力都相同，并将所有力均简化到质心处分析，只在分析无人越野车辆侧倾时将力作用到轮胎上分析。这种方式一方面当车速不高时，悬架对无人越野车辆左右轮胎的作用差异不明显，而在越野环境中由于地形引起的无人越野车辆位姿变化更为明显；另一方面便于提高算法实时性。
无人越野车辆的位姿在世界坐标系的示意图如图2所示，κ表示曲率，α表示无人越野车辆朝向向量(指向与车体x轴相同)，n表示无人越野车辆垂向轴向量(指向与车体z轴相同)，右手系定义得到γ＝n*α(与车体y轴相同)。无人越野车辆沿固定曲线行驶时，其速度方向与α方向一致，向心加速度方向沿曲线主法向量β，重力加速度g方向为z方向。简化的汽车行驶方程式如下所示：
式中，ft代表无人越野车辆纵向摩擦力，fq代表无人越野车辆横向摩擦力，FN为地面提供的支持力，弧长对时间的一阶导和二阶导数/>分别表示无人越野车辆的纵向速度和纵向加速度，m表示无人越野车辆质量，/>表示无风时的空气阻力，Cw为与空气阻力系数、空气密度、迎风面积、无人越野车辆质量有关的常数；等式右边的两项分别为线性加速度力和向心加速度力。
具体来说，通过以下公式表示在车体坐标系下各个坐标轴上的力：
fq＝-mgz·γ+ms2κβ·γ 公式(8)，
(2)从驱动力方面，根据无人越野车辆纵向摩擦力的区间，计算出加速度的第一约束区间。
无人越野车辆受引擎驱动时，与地面的接触点处产生了向前的纵向摩擦力。无人越野车辆受制动力时，与地面接触点处产生了向后的纵向摩擦力，也就是说无人越野车辆所受纵向摩擦力应该在最大驱动力Fbrake_max和最大制动力Fengine_max之间，则由驱动力和制动力引起的加速度第一约束区间为：
需要说明的是，根据经验给最大驱动力和最大制动力一个定值，但这个定值需要比无人越野车辆运行最槽糕情形下能提供的驱动力值还要小。根据无人越野车辆在轨迹点的预期速度，计算出随速度变化的加速度的第一约束区间，再判断根据预期速度计算出来的预期加速度是否在加速度第一约束区间，如果在，则继续下面的判断，否则，判定为不满足动态稳定性约束，进入步骤S125。
(3)从侧滑方面，当地形因素和预期速度满足不侧滑的判别条件时，根据无人越野车辆的速度和地形因素，计算出加速度的第二约束区间。
在简化的汽车行驶方程式下，纵向摩擦力与横向摩擦力的合力位于摩擦圆内即可保证无人越野车辆不会发生侧滑，即：
将公式(7)和公式(8)代入公式(11)，构建出如下的二次不等式方程：
其中，三个地形因素表示如下：
通过解不等式方程(12)，得到为了不发生侧滑而应有的加速度的第二约束区间：
基于上式，三个地形参数和预期速度需要满足以下不侧滑的判别条件，否则意味着式(11)的摩擦圆约束不可能被满足，无人越野车辆必定发生侧滑：
因此，在获取到规划路线后，首先根据式(12)计算地形因素，再根据预期速度和地形参数，检测是否满足不侧滑时的判别条件即公式(14)，如果满足，再将预期速度代入式(13)得到动态变化的加速度的第二约束区间，最后当预期加速度在该区间内时，再继续后续判断，否则，判定为不满足动态稳定性约束，进入步骤S125。
(4)从侧翻方面，根据无人越野车辆临界不侧翻条件，计算出速度的第一约束区间。
需要说明的是，从侧翻临界情形来看，当无人越野车辆刚好要发生侧翻，此时仅有一侧轮胎与地面接触而另一侧轮胎刚好离开地面。支持力与摩擦力作用于无人越野车辆一侧轮胎，二者对无人越野车辆质心产生的扭矩将使无人越野车辆发生旋转。若合力矩产生的旋转与无人越野车辆倾翻方向相反，则能保证无人越野车辆在临界状态时不会发生倾翻。设置扭矩顺时针方向为正，则临界情形下无人越野车辆不侧翻的条件为：
其中，p表示轮距的一半，hg为无人越野车辆质心高度。
由上式看出无论顺时针侧翻还是逆时针侧翻都有相同的临界条件，将式(8)和式(9)代入式(15)中，得到关于速度的临界不侧翻条件：
求解式(16)，得到速度的第一约束区间为：
在获取到规划路线后，根据地形和无人越野车辆尺寸参数计算出速度的第一约束区间。由于这是在临界情形下推导出的速度约束，因此，优选地，在实际应用时，当无人越野车辆在轨迹点的侧倾角大于设定的阈值时，再计算出速度的第一约束区间，当预期速度在该区间内时，再继续后续判断，否则，判定为不满足动态稳定性约束，进入步骤S125。
(5)从地面接触方面，根据无人越野车辆与地面接触产生的支持力大于等于0，计算出速度的第二约束区间。
需要说明的是，无人越野车辆在起伏路面行驶时一般需要保证一直与地面接触，此时地面能够一直为无人越野车辆提供支持力。将式(9)代入FN≥0，得到：
当无人越野车辆在地球表面行驶时，地面法向与重力方向的夹角一定为钝角，即gz·n＜0。因此当二次项系数kβ·n>0时，无论以什么速度行驶均能保证无人越野车辆始终与地面接触。当二次项系数kβ·n<0时，速度的第二约束区间为：
在获取到规划路线后，计算出速度的第二约束区间，确保无人越野车辆持续与地面接触。当无人越野车辆在轨迹点的预期速度在该区间内时，再继续后续判断，否则，判定为不满足动态稳定性约束，进入步骤S125。
S124、如果轨迹点在障碍地形内，根据接触点数据、障碍地形的尺寸和障碍地形通行序列，检测障碍地形内的各轨迹点是否满足通行约束。
需要说明的是，障碍地形包括垂直墙和壕沟；本步骤对通行可行性分为(1)障碍地形的尺寸的粗检测和(2)障碍地形通行序列的精检测。
(1)当无人越野车辆翻越垂直墙的最大高度大于估计的垂直墙高度，或，无人越野车辆跨越壕沟的最大宽度大于估计的壕沟宽度时，进入后续的精检测，否则，判定为不满足通行约束，进入步骤S125。
需要说明的是，翻越垂直墙的最大高度是根据无人越野车辆驱动电机的输出扭矩和路面附着系数计算的无人越野车辆前轮和后轮最大爬坡高度中的最小值。
具体来说，F1c为翻越垂直墙时作用于前轮受力，F2c为翻越垂直墙时后轮受力；根据静力学平衡原理，如图3所示，前轮跨越垂直墙的过程中，无人越野车辆前轮在水平方向以及竖直方向的合力为：
式中，F1c1为翻越垂直墙时作用于左前轮受力；F1c2为翻越垂直墙时作用于右前轮受力；F2c1为翻越垂直墙时左后轮受力；F2c2为翻越垂直墙时右后轮受力；φ为翻越垂直墙时车轮受力与垂直墙之间的夹角；μ为地面摩擦系数，G为无人越野车辆自身重力，m为无人越野车辆自重，g为重力加速度。
根据力矩平衡原理，以无人越野车辆前轮与接地面的接触点为矩心，则无人越野车辆前轮翻越垂直墙时的力矩平衡方程为：
式中，a为前轮和质心的距离，L0为水平方向上前后轮间距，D为车轮直径，根据车轮直径以及垂直墙高度H1，得到如下几何关系：
两前轮在翻越垂直墙过程中，假设路面附着系数足够大以至于不发生滑转，则两前轮能爬上的最大垂直墙高度取决于驱动电机的输出扭矩，公式如下所示：
式中，T1c1和T1c2为翻越垂直墙时左右两前轮电机的输出扭矩；T2c1和T2c2为翻越垂直墙时左右两后轮的输出扭矩。
将几何关系式(22)代入到式(23)中，得到在路面附着系数足够大时前轮翻越高度H1，作为无人越野车辆前轮爬坡高度H1a。
两前轮在翻越垂直墙过程中，如果驱动电机输出扭矩足够大，则两前轮能爬上的最大垂直墙高度取决于路面附着系数。因此，将式(22)代入式(20)与式(21)中计算出的H1作为无人越野车辆前轮能够克服的垂直墙高度H1b。
综上两种情况，在实际行驶过程中，无人越野车辆前轮翻越垂直墙的最大高度受到滑移率和无人越野车辆电机驱动力的共同影响，为了保证安全情况，取上述两种情况中的最小值min(H1a,H1b)作为前轮最大爬坡高度Max1。
后轮跨越垂直墙的过程中，前轮已处于垂直墙的上部，整车车身呈ψ角倾斜，作用在无人越野车辆后轮上的作用力如图4所示。
根据静力学平衡原理，无人越野车辆在水平方向以及竖直方向的合力为：
/>
根据力矩平衡方程，以无人越野车辆后轮与接地面的接触点为矩心，则无人越野车辆后轮翻越垂直墙时的力矩平衡方程为：
F1cL0cosψ+μF2cR＝Gb cosψ+μF1c(H1-R) 公式(25)，
式中，ψ为前后轮轮心连线和水平路面的夹角，即后轮翻越垂直墙时车身的倾斜角；b表示质心到后轮的距离。
两后轮在翻越垂直墙过程中，假设路面附着系数足够大以至于不发生滑转，则两后轮能爬上的最大垂直墙高度取决于驱动电机的输出扭矩。
式中，T1和T2分别为翻越垂直墙时前后轴的输出扭矩。
后轮翻越垂直墙时，具有与前轮翻越垂直墙一样的几何关系，将式(22)带入到式(26)中，得到在路面附着系数足够大时后轮翻越高度H1，作为无人越野车辆后轮爬坡高度H2a。
两后轮在翻越垂直墙过程中，如果驱动电机输出扭矩足够大，则两后轮能爬上的最大垂直墙高度取决于路面附着系数。因此，将式(22)代入式(24)与式(25)计算出的H1作为无人越野车辆后轮能够克服的垂直墙高度H2b。
综上两种情况，在实际行驶过程中，选择上述两种情况中的最小值min(H2a,H2b)作为后轮最大爬坡高度Max2。
最终，翻越垂直墙的最大高度Hmax＝min(Max1,Max2)。
需要说明的是，无人越野车辆前后轮跨越壕沟的阶段和翻越垂直墙的阶段一样，也可划分为三个阶段，第一阶段为无人越野车辆前轮接触到壕沟，在此过程中，由于车体重力作用，若壕沟太宽，则会发生无人越野车辆前端坠落；第二阶段为前后轮间的部分通过壕沟，壕沟宽度小于前后轴车轮的宽度；第三阶段为无人越野车辆后轮接触到壕沟，在此阶段中若壕沟太宽，则会发生无人越野车辆后端坠落，不能安全通过壕沟，导致越壕失败。无人越野车辆前轮、后轮跨越壕沟过程如图5和图6所示。跨越壕沟的最大宽度是根据无人越野车辆前轮和后轮跨越壕沟时驱动电机的输出扭矩以及无人越野车辆车轮直径分别计算的最大跨越宽度中的最小值。
具体来说，根据无人越野车辆尺寸计算得到第一最大跨越宽度W1为车轮直径D：
W1＝D公式(27)。
当假设摩擦系数足够大不发生滑移滑转时，对于前轮跨越壕沟进行分析：
根据静力学平衡原理，无人越野车辆在水平方向以及竖直方向的合力为：
式中，ρ为前轮跨越壕沟时前轮受力与壕沟之间的夹角。
前轮跨越壕沟时存在几何关系如下：
其中，ψ为前轮跨越壕沟时前后轮中心线与水平面之间的夹角。
根据力矩平衡原理，以前轮与壕沟的接触点为矩心，则无人越野车辆前轮跨越壕沟时的力矩平衡方程为：
G(a cosψ+R sinρ)＝F2c(L0cosψ+R sinρ) 公式(30)，
式中，表示车轮半径。
两前轮能跨越的最大壕沟宽度取决于驱动电机的输出扭矩，用以下公式表示：
T1+cos(ρ+ψ)cosψT2＝(G-F2c)R sinρ 公式(31)。
根据式(28)-式(31)中，得到前轮跨越壕沟过程中的W，作为第二最大跨越宽度W2a。
当假设摩擦系数足够大不发生滑移滑转时，对于后轮跨越壕沟进行分析：
根据静力学平衡原理，无人越野车辆在水平方向以及竖直方向的合力为：
式中，ρ为前轮跨越壕沟时前轮受力与壕沟之间的夹角。
后轮跨越壕沟存在与式(29)前轮跨越壕沟一样的几何关系，只是此时ψ为后轮跨越壕沟时前后轮中心线与水平面之间的夹角。
根据力矩平衡原理，以后轮与壕沟的接触点为矩心，则无人越野车辆后轮跨越壕沟时的力矩平衡方程为：
G(b cosψ-R sinρ)＝F1c(L0cosψ-R sinρ) 公式(33)。
则两后轮能越过的最大壕沟宽度取决于驱动电机的输出扭矩：
T2+cos(ρ+ψ)cosψT1＝(G-F1c)R sinρ 公式(34)。
根据式(29)，式(32)-式(34)，得到后轮跨越壕沟过程中的W，作为第三最大跨越宽度W2b。
综上可知，无人越野车辆最终跨越壕沟的宽度受到几何关系与力平衡的共同限制，即最终最大跨越宽度Wmax＝min(W1,W2a,W2b)。
(2)对无人越野车辆可通行的垂直墙和壕沟，检测无人越野车辆在障碍地形内的各轨迹点的接触点数据能否匹配障碍地形通行序列，如果能，则判定障碍地形内的各轨迹点均满足通行约束，否则，均不满足通行约束；进入步骤S125。
需要说明的是，障碍地形通行序列是基于翻越垂直墙和跨越壕沟的各阶段，采集有人驾驶模式下无人越野车辆在各阶段各车轮与地面的接触情况及各车轮与垂直墙和壕沟的相对位置而统计得到的车轮状态转移顺序。
每种车轮状态至少有一种车轮接触情况，每种车轮状态对应的前后轮与垂直墙或壕沟的相对位置不同。进一步地，前后轮与垂直墙或壕沟的相对位置包括：前后轮相对于垂直墙的上下沿区域的位置，及，前后轮相对于壕沟的前后沿区域的位置。
具体来说，图7是本实施例中无人越野车辆翻越垂直墙的障碍地形通行序列示意图，图中黑色表示车轮与地面接触受力，白色表示车轮悬空不接触地面，箭头表示从当前状态可以安全转移到的下一状态。图7中共有5种状态：状态1是无人越野车辆翻越垂直墙之前，四轮接触地面提供向前的驱动力，前轮和后轮的接触点均在垂直墙下沿；状态2是前轮接触垂直墙，开始受到向上抬升的力，前轮接触点在垂直墙上沿和/或下沿，后轮接触点在垂直墙下沿；状态3是前轮与后轮接触地面受力，提供向前的驱动力与向上的支持力，此时无人越野车辆达到最大倾角，前轮在垂直墙上沿，后轮在垂直墙下沿；状态4是无人越野车辆后轮接触垂直墙，前后轮共同提供支持力与驱动力，后轮接触点在垂直墙上沿和/或下沿；状态5是无人越野车辆到达垂直墙上方，后轮接触点在垂直墙上沿，完成翻越垂直墙的工作。在保证安全的状态下翻越垂直墙，其车轮与地面的接触情况一定与从状态1到状态5的一条转移路径相同。
图8是本实施例中无人越野车辆跨越壕沟的障碍地形通行序列示意图，图中标志含义与图7相同。图8共有5种状态：状态1是无人越野车辆跨越壕沟之前，前轮和后轮接触点均在壕沟后沿；状态2是无人越野车辆前轮经过壕沟，前轮接触点在壕沟的前沿和/或后沿，后轮的接触点在壕沟的后沿；状态3与状态4是无人越野车辆后轮经过壕沟，其中状态3中前轮接触点在壕沟前沿，后轮接触点在壕沟后沿，状态4中前轮接触点在壕沟前沿，后轮接触点在壕沟前沿和/或后沿；状态5是整车顺利通过壕沟，前轮和后轮接触点在壕沟前沿。在保证安全的状态下经过壕沟，其车轮与地面的接触情况一定与从状态1到状态5的一条转移路径相同。
将图7和图8中各个状态下车轮与垂直墙和壕沟的相对位置及其与地面接触情况进行编码，按图中的箭头存储安全的状态转移顺序，分别形成翻越垂直墙和跨越壕沟时的多条障碍地形通行序列。
在进行安全行为检测时，落入垂直墙和壕沟地形区域的是多个轨迹点，将这多个轨迹点的接触点数据按相同的方式编码，与障碍地形通行序列进行比对。具体来说，根据障碍地形通行序列中各状态下各车轮与地面的接触情况及各车轮与垂直墙和壕沟的相对位置，比对出各轨迹点的接触点数据对应的状态；依次检测每相邻两个轨迹点的状态，记录不同的状态，得到实际通行序列；当实际通行序列存在于障碍地形通行序列中，则满足通行约束，否则不满足通行约束。
需要注意的是，考虑到轨迹点是按固定时间间隔获取的，落入障碍地形的轨迹点有可能分成两个未检测的轨迹点序列，因此，待检测的轨迹点的接触点数据对应的状态并不要求必须从障碍地形通行序列的状态1开始，也不要求一定要覆盖所有状态。而且，实际情况下，多个轨迹点的接触点数据对应的状态可能相同，因此，本步骤在进行比对时，是依次检测相邻两个轨迹点的状态，记录不同的状态后得到实际通行序列，而且只要存在于障碍地形通行序列中，即判定为满足通行约束。
示例性地，采用2位编码，第1位表示状态编号，第二位表示同一状态下的子状态编号。无人越野车辆在跨越壕沟时，落入壕沟区域的有ABCD四个轨迹点，经比对，在轨迹点A对应状态22，在轨迹点B对应状态32，在轨迹点C仍然对应状态32，在轨迹点D对应状态41，实际通行序列是22—>32—>41，存在于图8中一条障碍地形通行序列中，满足通行约束。如果轨迹点D对应状态51，则实际通行序列是22—>32—>51，状态发生了跳变，不存在于图8的障碍地形通行序列中，不满足通行约束。
优选地，为障碍地形通行序列中不同的车轮状态转移顺序设置不同的安全级别。示例性地，将图7和图8中4个车轮均与地面接触的车轮状态组成的序列的安全级别设置为安全，其它序列的安全级别设置为风险。仍以上述2位编码为例，图7中11—>22—>31—>42—>51序列，及图8中11—>22—>32—>42—>51序列为安全，其它序列为风险。
S125、当任意一个约束不满足，则将已检测的轨迹点标记为危险，否则，标记为安全；返回步骤S122，直至不存在未检测的轨迹点。
需要说明的是，步骤S11获取一个未检测的轨迹点序列，在步骤S122从中取一个未检测的轨迹点i进行安全行为检测，当未检测的轨迹点i不在地面点云中障碍地形内，则执行步骤S122和步骤S123，在步骤S125中根据是否满足约束对轨迹点i标记安全或危险后，返回步骤S122，i＝i+1，即取出未检测的轨迹点i的下一个轨迹点进行安全行为检测；当未检测的轨迹点i在地面点云中障碍地形内时，执行步骤S122到步骤S124，并且在步骤S124的(2)障碍地形通行序列的精检测中，从当前轨迹点i开始，与落入障碍地形内其它多个轨迹点一起进行检测，比如共有m个轨迹点，在步骤S125中根据是否满足约束对m个轨迹点一起标记安全或危险，因此，返回步骤S122，i＝i+m，即跳过障碍地形内的轨迹点，对后续为未检测的轨迹点进行安全行为检测。
当在步骤S12中对步骤S11获取的未检测的轨迹点序列均已检测完，步骤S13根据已标记的轨迹点序列生成规划路线的安全性反馈信息，包括：各个轨迹点的安全行为标记，对标记为危险的轨迹点对应有违反的约束类型和比对数据；反馈给路径规划模块作为轨迹调整的参考，并返回步骤S11，对下一未检测的轨迹点序列进行安全行为检测。
综上所述，在无人驾驶场景中，规划路线反应了无人越野车辆未来的机动过程。为了保证行驶的安全性，通过上述步骤S11-S13的循环检测，对未来轨迹点在发生风险动作之前对错误的规划路线做出提前的预警，这种预期行为检测保障了规划路线的安全性。
进一步地，本实施例还包括步骤S14，采集无人越野车辆的实时轨迹点和速度，根据轨迹差异约束和动态稳定约束获取识别结果，进行安全行为检测。具体来说，包括：
实时采集无人越野车辆的实际运行轨迹点和实际速度；
根据实际运行轨迹点和实际速度，分别检测是否满足轨迹差异约束和动态稳定约束中速度的约束区间，如果有任意一个约束不满足，则将实际运行轨迹点标记为危险；
当连续标记为危险的实际运行轨迹点达到数量阈值，则判定无人越野车辆行驶异常，发出警告。
需要说明的是，实际运行轨迹点和实际速度来源于无人越野车辆自主行驶时传感器采集的数据；轨迹差异约束是检测无人越野车辆当前时刻的实际运行轨迹点与预期轨迹点的横向偏差是否大于宽度阈值；如果大于，则不满足轨迹差异约束。宽度阈值是根据无人越野车辆的宽度和偏差系数计算得到，比如偏差系数为10％，则满足轨迹差异约束时，横向偏差不能大于车宽的十分之一。
检测实际速度是否满足步骤S123中不侧滑的判别条件和两个约束空间，如果有任意一个不满足，则不满足动态稳定约束。
需要说明的是，考虑到系统和环境中存在的扰动以及不确定因素，无人越野车辆的实际状态并不一定满足设定的全部约束，因此，本实施例识别连续标记为危险的实际运行轨迹点是否达到数量阈值，采取的是连续帧状态监测以减小误报警概率。
与现有技术相比，本实施例提供的一种无人越野车辆自主行驶的安全行为检测方法，通过静态和动态约束条件，对预期规划路径的轨迹点进行全方面的预期行为安全检测；使用垂直墙/壕沟通行性粗检测和障碍地形通行序列的精检测，实现了对障碍地形的多种安全行为的检测，应用场景广泛；这种精确到点的安全性分级，对错误的规划路线做出提前的预警，有利于无人越野车辆及时调整预期行为；通过对无人越野车辆实时运行情况进行连续帧状态监测，减小系统和环境中扰动和不确定因素的影响，提高报警的准确率；通过对预期行为安全和实时行为安全检测两方面进行安全监测，在复杂地形和路面条件下，可以更好的在危险发生之前发出警报而及时纠正危险行为。
本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。
Claims (10)
1.一种无人越野车辆自主行驶的安全行为检测方法，其特征在于，包括以下步骤：
按照固定时间间隔获取规划路线上未检测的轨迹点序列；
根据地面点云拟合的地面模型，计算出无人越野车辆在各轨迹点的位姿和接触点数据；根据位姿依次检测无人越野车辆在各轨迹点是否满足静态位姿约束和动态稳定约束，如果轨迹点在障碍地形内，则继续根据接触点数据、障碍地形的尺寸和预设的障碍地形通行序列，检测障碍地形内的各轨迹点是否满足通行约束；当任意一个约束不满足，则将已检测的轨迹点标记为危险，否则，标记为安全；
各轨迹点均已检测后输出已标记的轨迹点序列，对下一未检测的轨迹点序列进行安全行为检测。
2.根据权利要求1所述的无人越野车辆自主行驶的安全行为检测方法，其特征在于，所述根据地面点云拟合的地面模型，计算出无人越野车辆在各轨迹点的位姿和接触点数据，包括：
构建由矩形单元组成的栅格地图，根据地面模型中的地面高度，获取无人越野车辆在每个栅格中底盘距离地面的高度，得到底盘高度图；
初始化旋转矩阵，根据地面模型和底盘高度图，获取无人越野车辆与地面的接触栅格作为接触点数据，根据接触点数据得到支撑多边形；
根据支撑多边形对位姿不稳定的轨迹点计算出旋转轴，并围绕旋转轴以固定角度旋转无人越野车辆，更新旋转矩阵和底盘高度图，重新获取接触点数据和支撑多边形，直至轨迹点位姿稳定，得到最终的接触点数据，并根据最终的旋转矩阵得到无人越野车辆在对应轨迹点的位姿。
3.根据权利要求2所述的无人越野车辆自主行驶的安全行为检测方法，其特征在于，所述检测无人越野车辆在各轨迹点是否满足静态位姿约束，包括：无人越野车辆在各轨迹点的俯仰角是否小于纵向上坡稳定角和纵向下坡稳定角，以及，侧倾角是否小于横向稳定角；如果均小于，则满足静态位姿约束。
4.根据权利要求3所述的无人越野车辆自主行驶的安全行为检测方法，其特征在于，所述纵向上坡稳定角和纵向下坡稳定角是根据无人越野车辆车体数据计算得到的极限上坡角和极限下坡角，分别与根据地面最大附着系数计算得到的极限纵向下滑角相比，得到的两个最小角；所述横向稳定角是根据无人越野车辆车体数据和质心偏移距离计算得到的极限横坡角，与根据地面横向附着系数计算的极限横向下滑角相比，得到的最小角。
5.根据权利要求1所述的无人越野车辆自主行驶的安全行为检测方法，其特征在于，所述动态稳定约束，包括：
根据无人越野车辆纵向摩擦力的区间，计算出加速度的第一约束区间；
当地形因素和预期速度满足不侧滑的判别条件时，根据无人越野车辆的速度和地形因素，计算出加速度的第二约束区间；
根据无人越野车辆临界不侧翻条件，计算出速度的第一约束区间；
根据无人越野车辆与地面接触产生的支持力大于等于0，计算出速度的第二约束区间。
6.根据权利要求5所述的无人越野车辆自主行驶的安全行为检测方法，其特征在于，所述地形因素是通过简化由无人越野车辆的纵向摩擦力和横向摩擦力的合力位于摩擦圆内而构建的二次不等式方程而得到，地形因素表示如下：
式中，c1、c2和c3表示地形因素；γ＝n*α，α表示无人越野车辆朝向向量，n表示无人越野车辆垂向轴向量；β表示路径主法向量，κ表示曲率，μ表示地面摩擦系数，g表示重力加速度，z表示重力加速度方向向量。
7.根据权利要求5所述的无人越野车辆自主行驶的安全行为检测方法，其特征在于，所述检测方法还包括：
实时采集无人越野车辆的实际运行轨迹点和实际速度；
根据实际运行轨迹点和实际速度，分别检测是否满足轨迹差异约束和动态稳定约束中速度的约束区间，如果有任意一个约束不满足，则将实际运行轨迹点标记为危险；
当连续标记为危险的实际运行轨迹点达到数量阈值，则判定无人越野车辆行驶异常，发出警告。
8.根据权利要求1所述的无人越野车辆自主行驶的安全行为检测方法，其特征在于，所述障碍地形包括垂直墙和壕沟；所述障碍地形的尺寸包括：根据地面点云和无人越野车辆拍摄的图像，估计出障碍地形中垂直墙高度、垂直墙的上下沿区域、壕沟宽度和壕沟的前后沿区域。
9.根据权利要求8所述的无人越野车辆自主行驶的安全行为检测方法，其特征在于，所述根据接触点数据、障碍地形的尺寸和障碍地形通行序列，检测障碍地形内的各轨迹点是否满足通行约束，包括：
根据无人越野车辆驱动电机的输出扭矩和路面附着系数，计算出无人越野车辆前轮和后轮最大爬坡高度中的最小值，得到无人越野车辆翻越垂直墙的最大高度；根据无人越野车辆前轮和后轮跨越壕沟时驱动电机的输出扭矩以及无人越野车辆车轮直径分别计算出最大跨越宽度中的最小值，得到无人越野车辆跨越壕沟的最大宽度；
当无人越野车辆翻越垂直墙的最大高度大于估计的垂直墙高度，或，无人越野车辆跨越壕沟的最大宽度大于估计的壕沟宽度时，获取无人越野车辆在障碍地形内的各轨迹点的接触点数据，并根据障碍地形通行序列，比对出各轨迹点的接触点数据对应的状态；依次检测每相邻两个轨迹点的状态，记录不同的状态，得到实际通行序列；当实际通行序列存在于障碍地形通行序列中，则满足通行约束。
10.根据权利要求8所述的无人越野车辆自主行驶的安全行为检测方法，其特征在于，所述障碍地形通行序列是基于翻越垂直墙和跨越壕沟的各阶段，采集有人驾驶模式下无人越野车辆在各阶段各车轮与地面的接触情况及各车轮与垂直墙和壕沟的相对位置而统计得到的车轮状态转移顺序。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311088074.0A CN116935683B (zh) | 2023-08-28 | 2023-08-28 | 一种无人越野车辆自主行驶的安全行为检测方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311088074.0A CN116935683B (zh) | 2023-08-28 | 2023-08-28 | 一种无人越野车辆自主行驶的安全行为检测方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116935683A true CN116935683A (zh) | 2023-10-24 |
| CN116935683B CN116935683B (zh) | 2024-05-14 |
Family
ID=88392578
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311088074.0A Active CN116935683B (zh) | 2023-08-28 | 2023-08-28 | 一种无人越野车辆自主行驶的安全行为检测方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116935683B (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117649060A (zh) * | 2024-01-30 | 2024-03-05 | 航天宏图信息技术股份有限公司 | 野外环境可通过性的分析方法、装置、电子设备和介质 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050151497A1 (en) * | 2003-12-17 | 2005-07-14 | Kenichiro Nagasaka | Robot and motion control method of robot |
| US20060214622A1 (en) * | 2005-03-23 | 2006-09-28 | Harris Corporation | Control system and related method for multi-limbed, multi-legged robot |
| CN103996312A (zh) * | 2014-05-23 | 2014-08-20 | 北京理工大学 | 具有社会行为交互的无人驾驶汽车控制系统 |
| CN109190704A (zh) * | 2018-09-06 | 2019-01-11 | 中国科学院深圳先进技术研究院 | 障碍物检测的方法及机器人 |
| US20190129433A1 (en) * | 2016-12-29 | 2019-05-02 | Amicro Semiconductor Corporation | A path planning method of intelligent robot |
| CN113044029A (zh) * | 2021-03-19 | 2021-06-29 | 北京理工大学 | 一种保证无人车在三维地形上安全行驶的运动规划方法 |
-
2023
- 2023-08-28 CN CN202311088074.0A patent/CN116935683B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050151497A1 (en) * | 2003-12-17 | 2005-07-14 | Kenichiro Nagasaka | Robot and motion control method of robot |
| US20060214622A1 (en) * | 2005-03-23 | 2006-09-28 | Harris Corporation | Control system and related method for multi-limbed, multi-legged robot |
| CN103996312A (zh) * | 2014-05-23 | 2014-08-20 | 北京理工大学 | 具有社会行为交互的无人驾驶汽车控制系统 |
| US20190129433A1 (en) * | 2016-12-29 | 2019-05-02 | Amicro Semiconductor Corporation | A path planning method of intelligent robot |
| CN109190704A (zh) * | 2018-09-06 | 2019-01-11 | 中国科学院深圳先进技术研究院 | 障碍物检测的方法及机器人 |
| CN113044029A (zh) * | 2021-03-19 | 2021-06-29 | 北京理工大学 | 一种保证无人车在三维地形上安全行驶的运动规划方法 |
Non-Patent Citations (2)
| Title |
|---|
| 刘忠泽;陈慧岩;崔星;熊光明;王羽纯;陶溢;: "无人平台越野环境下同步定位与地图创建", 兵工学报, no. 12 * |
| 姜岩;龚建伟;熊光明;陈慧岩;: "基于运动微分约束的无人车辆纵横向协同规划算法的研究", 自动化学报, no. 12 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117649060A (zh) * | 2024-01-30 | 2024-03-05 | 航天宏图信息技术股份有限公司 | 野外环境可通过性的分析方法、装置、电子设备和介质 |
| CN117649060B (zh) * | 2024-01-30 | 2024-05-14 | 航天宏图信息技术股份有限公司 | 野外环境可通过性的分析方法、装置、电子设备和介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116935683B (zh) | 2024-05-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP2023536407A (ja) | 走行可能面識別技術 | |
| CN116935683B (zh) | 一种无人越野车辆自主行驶的安全行为检测方法 | |
| US7272474B1 (en) | Method and system for estimating navigability of terrain | |
| KR101054479B1 (ko) | 방향별 주행성 속도지도를 활용한 무인차량의 지역경로계획장치 및 방법 | |
| Feraco et al. | A local trajectory planning and control method for autonomous vehicles based on the RRT algorithm | |
| CN103718224B (zh) | 三维物体检测装置和三维物体检测方法 | |
| US11975725B2 (en) | Systems and methods for updating the parameters of a model predictive controller with learned external parameters generated using simulations and machine learning | |
| CN109635672A (zh) | 一种无人驾驶的道路特征参数估计方法 | |
| WO2023197535A1 (zh) | 一种深部受限空间下无人有轨电机车坡度及弯道通行方法 | |
| CN110316197A (zh) | 倾斜估算方法、倾斜估算装置以及存储程序的非暂时性计算机可读存储介质 | |
| CN116499486B (zh) | 一种复杂越野环境路径规划方法、系统及电子设备 | |
| US11002539B2 (en) | Method for detecting a slope of a road | |
| Reina et al. | Odometry correction using visual slip angle estimation for planetary exploration rovers | |
| WO2023278152A1 (en) | Estimating angle of a vehicle wheel based on non-steering variables | |
| CN115185266A (zh) | 一种基于地形可通过性的机器人控制方法及系统 | |
| US20220242401A1 (en) | Systems and methods for updating the parameters of a model predictive controller with learned controls parameters generated using simulations and machine learning | |
| WO2022091305A1 (ja) | 挙動推定装置、挙動推定方法、経路生成装置、経路生成方法、及びコンピュータ読み取り可能な記録媒体 | |
| McAllister et al. | Motion planning and stochastic control with experimental validation on a planetary rover | |
| CN117146845B (zh) | 一种无人越野车辆通过障碍地形的安全行为检测方法 | |
| Han et al. | Model predictive control for aggressive driving over uneven terrain | |
| CN117146845A (zh) | 一种无人越野车辆通过障碍地形的安全行为检测方法 | |
| Reina | Methods for wheel slip and sinkage estimation in mobile robots | |
| CN112793566B (zh) | 一种避撞方法及装置 | |
| US20220242441A1 (en) | Systems and methods for updating the parameters of a model predictive controller with learned operational and vehicle parameters generated using simulations and machine learning | |
| Sharma et al. | Ramp: A risk-aware mapping and planning pipeline for fast off-road ground robot navigation |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |