)，航向（θ )和速度（V)。2.机动设备目标点状 态。3.机动设备自身的驾驶性能，这个因素可以由机动设备模型模拟，以保证生成的轨迹最 接近机动设备真实的机动能力。4.行动参数中的最大速度和理想加速度。5.路上的静态 /动态障碍物（可以由虚拟世界模型提供）。
[0067] 本方法具体地包括如下步骤：
[0068] 首先区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区域；而后根 据不同的驾驶环境生成不同的运动轨迹。结构化区域可以是有规则的区域，如道路，机动设 备辆的运动要受到区域结构化的限制，如图1所示，具有上下两个车道，机动设备A的运行 要在两个车道中进行。非结构化区域可以是没有规则的区域，如停车场或者操场等，机动设 备可以往任意方向或者路径行进，如图2所示。对于不同的区域，对区域进行区分并生成不 同的运动轨迹，而后在该运动轨迹上完成机动设备的行进，有利于节省机动设备能耗和减 少行进时间。
[0069] 因为在不同行驶环境下，合理可行的轨迹的形状会有很大的不同，所以动作控制 模块用不同的方法区别对待这些环境：
[0070] 1.结构化的区域。即可行驶车道有明确界定的道路。在这种环境下机动设备的 运动受限于车道和道路的形状，因此动作控制模块生成的轨迹和车道在方向和走向上相似 (如图2所示）。在机动设备沿道路行驶的情况下，以下两种动作的组合足以满足正常情况 下的机动要求：a.沿当前车道行驶；b.换车道行驶。
[0071] 2.非结构化的区域。例如没有规划车道的停车场。如图3所示，这类环境开放而 且没有道路结构，因此机动设备的运动轨迹相对自由很多。在这种环境下，行动目标指令仅 仅要求机动设备达到某一位置的同时保持某一朝向，而具体的轨迹灵活性很大。
[0072] 进一步地，所述生成运动轨迹具体包括如下步骤：
[0073] 生成一条平滑的沿车道中线的运动轨迹；
[0074] 则本方法还包括如下步骤：
[0075] 调整不同的机动设备控制参数，根据不同的机动设备控制参数算出多条接近运动 轨迹的备选轨迹；选择最佳的备选轨迹；输出最佳的备选轨迹对应的机动设备控制参数实 现对机动设备控制。
[0076] 因为在沿道路行驶的背景下，行动目标点通常位于某一车道内。这个时候，道路的 走向和是否变道决定了合理的轨迹的形状。如果目标是顺着车道行驶，那么合理的运动轨 迹应该和道路中线基本平行，如图2机动设备A正前方的轨迹曲线。如果目标是变道行驶， 那么合理的运动轨迹会应该一开始和原车道中线基本平行，接着平滑过渡到和目标车道中 线基本平行，如图2机动设备A上面车道的曲线。由此可见，道路中线曲线提供了一条很好 的参照轨迹，这条轨迹可以作为轨迹搜索的起始点。
[0077] 搜索轨迹的最终目的，是要找到能实现这么一条轨迹的电传操控控制参数，控制 参数用于控制机动设备运动。本实施例中采取的办法是，用一组微分方程来模拟机动设备 状态受控制参数变化而变化的过程。同时，将上述中所提到的一些制约因素以约束模型的 形式加入这一运动模型。从而用求微分方程组的边界值问题来解决轨迹搜索的问题。
[0078] 这个微分方程组的两个边界值便是车辆在起始点和目标点的状态矢量（位置（X， y)，航向（Θ)和速度（V))。驱动机动设备起始点状态变化至目标点状态的，是电传操纵模 块的一系列输入参数的连续函数，这也是微分方程组最终要搜寻的解。
[0079] 算法的具体步骤为：
[0080] 1.生成一条平滑的中线轨迹。从起始点起，沿着目标车道的中线，直到目标点，从 道路几何地图（虚拟世界模型中包含）中提取一系列道路中线点。然后从这些点生成一条 平滑的曲线，即中线轨迹（如图4的中线f)。
[0081] 2.以中线轨迹为基础，通过一定的数值计算法，一个一个地寻找满足微分方程组 的解，即不同的机动设备控制参数，获得一系列逐渐侧向平移的轨迹（如图4中线f周围的 轨迹曲线）即是备选轨迹。这种方法能够极大地缩小搜索空间，确保动作控制模块能够以 较高的频率输出电传操纵指令。
[0082] 3.对第2步生成的轨迹进行筛选，过滤掉不可行的轨迹，然后选出最佳轨迹。最佳 轨迹所代表的解，即为电传操纵模块的参数函数，可以直接输出至电传操纵模块，实现对机 动设备e的控制。
[0083] 具体地，可以使用以下这一组微分方程模型，来模拟由机动设备电传控制参数的 改变，而导致机动设备运动状态的改变，使得机动设备由从起始点状态变化到目标点状 态：
【主权项】
1. 一种轨迹搜寻方法，其特征在于，包括如下步骤： 区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区域； 根据不同的驾驶环境生成不同的运动轨迹。
2. 根据权利要求1所述的轨迹搜寻方法，其特征在于：所述结构化区域包括具有车道 的道路。
3. 根据权利要求2所述的轨迹搜寻方法，其特征在于：所述生成运动轨迹具体包括如 下步骤： 生成一条平滑的沿车道中线的运动轨迹； 则本方法还包括如下步骤： 调整不同的机动设备控制参数，根据不同的机动设备控制参数算出多条接近运动轨迹 的备选轨迹； 选择最佳的备选轨迹； 输出最佳的备选轨迹对应的机动设备控制参数实现对机动设备控制。
4. 根据权利要求3所述的轨迹搜寻方法，其特征在于：还包括成本计算步骤：在算出多 条备选轨迹时并计算备选轨迹的成本，所述最佳的备选轨迹为成本最低的备选轨迹。
5. 根据权利要求4所述的轨迹搜寻方法，其特征在于：所述成本计算时包括如下计算 条件： 备选轨迹与障碍物在一定的最小安全距离之外，成本和备选轨迹与障碍物距离呈反 比； 或者备选轨迹与障碍物在一定的最小安全距离之内，成本和备选轨迹与障碍物距离的 倒数呈反比； 或者备选轨迹与障碍物相交，则成本无穷大； 或者成本与备选轨迹和车道中线的平均距离成正比； 或者成本与备选轨迹平滑度成反比； 或者成本与车辆完成轨迹所需要的总时间成正比。
6. 根据权利要求1所述的轨迹搜寻方法，其特征在于：所述根据不同的驾驶环境生成 不同的运动轨迹还包括根据机动设备当前位置、航向、目标点位置或机动设备性能生成运 动轨迹。
7. 根据权利要求1所述的轨迹搜寻方法，其特征在于，还包括如下步骤：间隔固定时间 段或距离后重新生成运动轨迹。
8. 根据权利要求1所述的轨迹搜寻方法，其特征在于，当驾驶环境是非结构化区域时， 所述生成运动轨迹包括使用A星算法对运动轨迹进行搜寻并生成运动轨迹。
9. 根据权利要求8所述的轨迹搜寻方法，其特征在于，还包括对运动轨迹进行成本计 算步骤：使用点阵多维搜索算法对运动轨迹的成本进行计算。
10. 根据权利要求8所述的轨迹搜寻方法，其特征在于，还包括如下步骤：对生成的运 动轨迹进行平滑化操作。
11. 一种轨迹搜寻系统，其特征在于，包括如下模块： 区域区分模块：用于区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区 域； 轨迹生成模块：根据不同的驾驶环境生成不同的运动轨迹。
12. 根据权利要求11所述的轨迹搜寻系统，其特征在于：所述结构化区域包括具有车 道的道路。
13. 根据权利要求12所述的轨迹搜寻系统，其特征在于：所述轨迹生成模块具体包括 如下模块： 中线轨迹生成模块：用于生成一条平滑的沿车道中线的运动轨迹； 则本系统还包括如下模块： 备选轨迹计算模块：用于调整不同的机动设备控制参数，根据不同的机动设备控制参 数算出多条接近运动轨迹的备选轨迹； 备选轨迹选择模块：用于选择最佳的备选轨迹； 控制参数输出模块：用于输出最佳的备选轨迹对应的机动设备控制参数实现对机动设 备控制。
14. 根据权利要求13所述的轨迹搜寻系统，其特征在于：还包括成本计算模块：用于 在算出多条备选轨迹时并计算备选轨迹的成本，所述最佳的备选轨迹为成本最低的备选轨 迹。
15. 根据权利要求14所述的轨迹搜寻系统，其特征在于：所述成本计算模块在成本计 算时包括如下计算条件： 备选轨迹与障碍物在一定的最小安全距离之外，成本和备选轨迹与障碍物距离呈反 比； 或者备选轨迹与障碍物在一定的最小安全距离之内，成本和备选轨迹与障碍物距离的 倒数呈反比； 或者备选轨迹与障碍物相交，则成本无穷大； 或者成本与备选轨迹和车道中线的平均距离成正比； 或者成本与备选轨迹平滑度成反比； 或者成本与车辆完成轨迹所需要的总时间成正比。
16. 根据权利要求11所述的轨迹搜寻系统，其特征在于：所述根据不同的驾驶环境生 成不同的运动轨迹还包括根据机动设备当前位置、航向、目标点位置或机动设备性能生成 运动轨迹。
17. 根据权利要求11所述的轨迹搜寻系统，其特征在于，还包括如下模块：轨迹重生成 模块：用于间隔固定时间段或距离后重新生成运动轨迹。
18. 根据权利要求11所述的轨迹搜寻系统，其特征在于，当驾驶环境是非结构化区域 时，所述轨迹生成模块生成运动轨迹包括使用A星算法对运动轨迹进行搜寻并生成运动轨 迹。
19. 根据权利要求18所述的轨迹搜寻系统，其特征在于，成本计算模块还用于使用点 阵多维搜索算法对运动轨迹的成本进行计算。
20. 根据权利要求18所述的轨迹搜寻系统，其特征在于，还包括如下轨迹平滑化模块： 用于对生成的运动轨迹进行平滑化操作。
【专利摘要】本发明公开一种轨迹搜寻方法和系统，其中方法包括如下步骤：区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区域；根据不同的驾驶环境生成不同的运动轨迹。本发明对轨迹搜寻的效果好，使得机动设备的控制能达到较好的效果。
【IPC分类】B60W30-10, B60W40-00
【公开号】CN104590259
【申请号】CN201510005390
【发明人】潘晨劲, 赵江宜
【申请人】福州华鹰重工机械有限公司
【公开日】2015年5月6日
【申请日】2015年1月7日
【公告号】CN104590259B