CN115127576B - 路径规划方法、装置、芯片、终端、电子设备及存储介质 - Google Patents
路径规划方法、装置、芯片、终端、电子设备及存储介质 Download PDFInfo
- Publication number
- CN115127576B CN115127576B CN202211068224.7A CN202211068224A CN115127576B CN 115127576 B CN115127576 B CN 115127576B CN 202211068224 A CN202211068224 A CN 202211068224A CN 115127576 B CN115127576 B CN 115127576B
- Authority
- CN
- China
- Prior art keywords
- value
- curvature
- point
- unmanned vehicle
- reference line
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种路径规划方法、装置、芯片、终端、电子设备及存储介质，涉及自动化技术领域，主要在于实现自动起步规划，使无人驾驶车辆自行回到预设参考线循迹行驶，无需人工干预。其中方法包括：当检测到无人驾驶车辆与预设参考线之间的综合误差超限时，在预设参考线上确定目标采样点；以无人驾驶车辆的当前位置为起点、目标采样点为终点，采用采样轨迹规划器生成无人驾驶车辆的规划行驶轨迹，控制无人驾驶车辆按照规划行驶轨迹行进；在无人驾驶车辆按照规划行驶轨迹行进的过程中，实时计算无人驾驶车辆的实时位置与预设参考线之间的实时综合误差，以及在计算得到的实时综合误差小于预设误差阈值时，控制无人驾驶车辆按照预设参考线循迹行驶。
Description
技术领域
本发明涉及自动化技术领域，尤其是涉及一种路径规划方法、装置、芯片、终端、电子设备及存储介质。
背景技术
相对于普通公路，矿区道路绝大部分都是非公开道路，没有道路边界、车道线、交通指示灯等信息，因此，无人驾驶矿卡的路由模块中规划有全局路径，正常情况下，无人驾驶矿卡沿着路由模块规划的全局路径行驶。
但是，有时无人驾驶矿卡起步时可能已经偏离全局路径太远，导致综合误差超限使无人驾驶矿卡决策停车，不能回到全局路径继续循迹，需要人工进行起步规划。为了实现人工起步规划，需要为每辆无人驾驶车辆配置安全驾驶员，会导致人工成本增加，可见人工起步规划并不适用于矿区无人驾驶应用场景。因此，亟需一种矿区车辆的行驶路径规划方法，使无人驾驶矿卡在综合误差超限停车时正常起步，并回到路由模块规划的全局路径上正常行驶。
发明内容
本发明提供了一种路径规划方法、装置、芯片、终端、电子设备及存储介质，主要在于实现综合误差超限的自动起步规划，使无人驾驶车辆自行回到预设参考线中循迹行驶，无需人工干预，智能性较好。
根据本发明的第一个方面，提供一种路径规划方法，包括：
当检测到无人驾驶车辆与预设参考线之间的综合误差超限时，在所述预设参考线上确定目标采样点，所述预设参考线是所述无人驾驶车辆中预设的车辆行驶轨迹；
以所述无人驾驶车辆的当前位置为起点、所述目标采样点为终点，采用采样轨迹规划器生成所述无人驾驶车辆的规划行驶轨迹，控制所述无人驾驶车辆按照所述规划行驶轨迹行进；
在所述无人驾驶车辆按照所述规划行驶轨迹行进的过程中，实时计算所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差，以及在计算得到的实时综合误差小于预设误差阈值时，控制所述无人驾驶车辆按照所述预设参考线循迹行驶。
可选地，所述当检测到无人驾驶车辆与预设参考线之间的综合误差超限时，在所述预设参考线上确定目标采样点之前，所述方法还包括：
每隔预设检测周期，获取所述无人驾驶车辆的当前位置的位置姿态信息，以及确定所述无人驾驶车辆中预设的所述预设参考线；
读取所述预设参考线包括的多个路径序列点，分别计算所述多个路径序列点中每个路径序列点与所述无人驾驶车辆的当前位置的距离，在所述多个路径序列点中确定基准序列点，所述基准序列点与所述无人驾驶车辆的当前位置之间的距离小于所述多个路径序列点中除所述基准序列点外的其他路径序列点；
获取所述基准序列点的序列点姿态坐标，计算所述序列点姿态坐标与所述位置姿态信息之间的综合误差；
当所述综合误差大于所述预设误差阈值时，确定检测到所述无人驾驶车辆与预设参考线之间的综合误差超限。
可选地，所述计算所述序列点姿态坐标与所述位置姿态信息之间的综合误差，包括：
读取所述序列点姿态坐标的横坐标、纵坐标，读取所述位置姿态信息的横坐标、纵坐标；
计算所述序列点姿态坐标的横坐标与所述位置姿态信息的横坐标的横坐标差值，计算所述序列点姿态坐标的纵坐标与所述位置姿态信息的纵坐标的纵坐标差值；
读取所述序列点姿态坐标的序列点航向角，读取所述位置姿态信息的车辆航向角，计算所述序列点航向角与所述车辆航向角之间的航向角差值；
计算所述横坐标差值的第一平方值、所述纵坐标差值的第二平方值，计算所述第一平方值和所述第二平方值的和值，以及对所述和值进行平方根计算，得到根值；
计算所述航向角差值的正弦值，获取所述正弦值的绝对值；
计算所述根值与所述绝对值的和值作为所述序列点姿态坐标与所述位置姿态信息之间的综合误差。
可选地，所述获取所述基准序列点的序列点姿态坐标，计算所述序列点姿态坐标与所述位置姿态信息之间的综合误差之后，所述方法还包括：
当所述综合误差小于等于所述预设误差阈值时，控制所述无人驾驶车辆按照所述预设参考线循迹行驶，以及继续每隔所述预设检测周期，重新获取所述无人驾驶车辆的当前位置的位置姿态信息，以及确定所述预设参考线，重新在所述预设参考线包括的多个路径序列点中确定新的基准序列点，获取所述新的基准序列点的序列点姿态坐标，计算所述新的基准序列点的序列点姿态坐标与重新获取的位置姿态信息之间的综合误差，判断所述综合误差是否大于所述预设误差阈值。
可选地，所述在所述预设参考线上确定目标采样点：
获取预设采样距离；
以所述无人驾驶车辆的当前位置为基准，在所述预设参考线上采样一与所述当前位置之间的距离满足所述预设采样距离的点作为所述目标采样点。
可选地，所述采用采样轨迹规划器生成所述无人驾驶车辆的规划行驶轨迹，包括：
根据所述目标采样点对应的横坐标、纵坐标、航向角和曲率值，利用牛顿求根法求解所述采样轨迹规划器中关于曲线弧长的曲率函数对应的曲率系数；
基于所述曲率系数，确定所述无人驾驶车辆的当前位置与所述目标采样点之间满足车辆运动学约束的行驶曲线，以及将所述行驶曲线作为所述无人驾驶车辆的规划行驶轨迹。
可选地，所述根据所述目标采样点对应的横坐标、纵坐标、航向角和曲率值，利用牛顿求根法求解所述采样轨迹规划器中关于曲线弧长的曲率函数对应的曲率系数，包括：
根据所述无人驾驶车辆在所述当前位置的起点曲率值、在所述目标采样点处的终点曲率值，以及所述关于曲线弧长的曲率函数，确定关于起点曲率值、第一曲率、第二曲率、终点曲率值和曲线弧长的曲率系数函数，其中，所述第一曲率和所述第二曲率分别为1/3曲线弧长和2/3曲线弧长处的曲率；
基于所述曲率系数函数和所述采样轨迹规划器，分别确定关于起点曲率值、第一曲率、第二曲率、曲线弧长和终点曲率值的横坐标函数、纵坐标函数和航向角函数；
获取第一曲率、第二曲率和曲线弧长分别对应的第一初始值、第二初始值和第三初始值，并将所述第一初始值、所述第二初始值、所述第三初始值、所述起点曲率值和所述终点曲率值分别代入至所述横坐标函数、所述纵坐标函数、所述航向角函数和所述曲率系数函数，得到所述障碍车辆的当前位置与所述目标采样点之间的目标***点对应的横坐标、纵坐标、航向角和曲率值；
将所述目标采样点对应的横坐标、纵坐标、航向角和曲率值与所述目标***点对应的横坐标、纵坐标、航向角和曲率值对应相减，得到所述目标采样点与所述目标***点之间的横坐标差值、纵坐标差值、航向角差值和曲率差值，并计算所述横坐标差值、所述纵坐标差值、所述航向角差值和所述曲率差值的差值总和；
基于所述差值总和不断对所述第一初始值、所述第二初始值和所述第三初始值进行更新迭代，直至目标***点对应的横坐标、纵坐标、航向角和曲率值分别与所述目标采样点对应的横坐标、纵坐标、航向角和曲率值相同，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长；
根据所述最终迭代更新后的第一曲率值、第二曲率值、曲线弧长和所述曲率系数函数，确定所述曲率系数。
可选地，所述基于所述差值总和不断对所述第一初始值、所述第二初始值和所述第三初始值进行更新迭代，直至目标***点对应的横坐标、纵坐标、航向角和曲率值分别与所述目标采样点对应的横坐标、纵坐标、航向角和曲率值相同，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长，包括：
基于所述横坐标函数、所述纵坐标函数和所述航向角函数，计算所述第一曲率、所述第二曲率和所述曲线弧长分别对应的雅克比矩阵；
计算所述第一曲率、所述第二曲率和所述曲线弧长分别对应的雅克比矩阵的逆矩阵，并将所述第一曲率、所述第二曲率和所述曲线弧长分别对应的逆矩阵与所述差值总和相乘，得到本轮迭代的第一曲率变化量、第二曲率变化量和曲线弧长变化量；
基于所述第一曲率变化量、所述第二曲率变化量和所述曲线弧长变化量，分别对所述第一初始值、所述第二初始值和所述第三初始值进行更新；
重复对所述第一初始值、所述第二初始值和所述第三初始值进行迭代更新的过程，直至所述差值总和为0，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长。
可选地，所述实时计算所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差，包括：
实时对所述预设参考线进行采样，得到多个采样点，分别计算所述多个采样点中每个采样点与所述实时位置之间的欧式距离；
在所述多个采样点中提取欧式距离小于预设距离阈值的采样点作为评估点，实时获取所述评估点的评估点姿态坐标、所述实时位置的实时位置姿态信息；
计算所述评估点姿态坐标与所述实时位置姿态信息之间的综合误差作为所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差。
可选地，所述分别计算所述多个采样点中每个采样点与所述实时位置之间的欧式距离，包括：
对于所述多个采样点中每个采样点，获取所述采样点的横坐标、纵坐标；
读取所述实时位置的横坐标、纵坐标；
计算所述采样点的横坐标与所述实时位置的横坐标的横坐标差值，计算所述横坐标差值的第三平方值；
计算所述采样点的纵坐标与所述实时位置的纵坐标的纵坐标差值，计算所述纵坐标差值的第四平方值；
计算所述第三平方值和所述第四平方值的和值，以及对所述和值进行平方根计算，将得到的根值作为当前计算的采样点与所述实时位置之间的欧式距离。
可选地，所述计算所述评估点姿态坐标与所述实时位置姿态信息之间的综合误差作为所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差，包括：
读取所述评估点姿态坐标的横坐标、纵坐标，读取所述实时位置姿态信息的横坐标、纵坐标；
计算所述评估点姿态坐标的横坐标与所述实时位置姿态信息的横坐标的横坐标差值，计算所述评估点姿态坐标的纵坐标与所述实时位置姿态信息的纵坐标的纵坐标差值；
读取所述评估点姿态坐标的评估点航向角，读取所述实时位置姿态信息的实时航向角，计算所述评估点航向角与所述实时航向角之间的航向角差值；
计算所述横坐标差值的第五平方值、所述纵坐标差值的第六平方值，计算所述第五平方值和所述第六平方值的和值，以及对所述和值进行平方根计算，得到根值；
计算所述航向角差值的正弦值，获取所述正弦值的绝对值；
计算所述根值与所述绝对值的和值作为所述综合误差，以及将所述综合误差作为所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差。
根据本发明的第二个方面，提供一种路径规划装置，包括：
确定单元，用于当检测到无人驾驶车辆与预设参考线之间的综合误差超限时，在所述预设参考线上确定目标采样点，所述预设参考线是所述无人驾驶车辆中预设的车辆行驶轨迹；
生成单元，用于以所述无人驾驶车辆的当前位置为起点、所述目标采样点为终点，采用采样轨迹规划器生成所述无人驾驶车辆的规划行驶轨迹，控制所述无人驾驶车辆按照所述规划行驶轨迹行进；
计算单元，用于在所述无人驾驶车辆按照所述规划行驶轨迹行进的过程中，实时计算所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差，以及在计算得到的实时综合误差小于预设误差阈值时，控制所述无人驾驶车辆按照所述预设参考线循迹行驶。
可选地，所述计算单元，还用于每隔预设检测周期，获取所述无人驾驶车辆的当前位置的位置姿态信息，以及确定所述无人驾驶车辆中预设的所述预设参考线；读取所述预设参考线包括的多个路径序列点，分别计算所述多个路径序列点中每个路径序列点与所述无人驾驶车辆的当前位置的距离，在所述多个路径序列点中确定基准序列点，所述基准序列点与所述无人驾驶车辆的当前位置之间的距离小于所述多个路径序列点中除所述基准序列点外的其他路径序列点；获取所述基准序列点的序列点姿态坐标，计算所述序列点姿态坐标与所述位置姿态信息之间的综合误差；当所述综合误差大于所述预设误差阈值时，确定检测到所述无人驾驶车辆与预设参考线之间的综合误差超限。
可选地，所述计算单元，用于读取所述序列点姿态坐标的横坐标、纵坐标，读取所述位置姿态信息的横坐标、纵坐标；计算所述序列点姿态坐标的横坐标与所述位置姿态信息的横坐标的横坐标差值，计算所述序列点姿态坐标的纵坐标与所述位置姿态信息的纵坐标的纵坐标差值；读取所述序列点姿态坐标的序列点航向角，读取所述位置姿态信息的车辆航向角，计算所述序列点航向角与所述车辆航向角之间的航向角差值；计算所述横坐标差值的第一平方值、所述纵坐标差值的第二平方值，计算所述第一平方值和所述第二平方值的和值，以及对所述和值进行平方根计算，得到根值；计算所述航向角差值的正弦值，获取所述正弦值的绝对值；计算所述根值与所述绝对值的和值作为所述序列点姿态坐标与所述位置姿态信息之间的综合误差。
可选地，所述计算单元，还用于当所述综合误差小于等于所述预设误差阈值时，控制所述无人驾驶车辆按照所述预设参考线循迹行驶，以及继续每隔所述预设检测周期，重新获取所述无人驾驶车辆的当前位置的位置姿态信息，以及确定所述预设参考线，重新在所述预设参考线包括的多个路径序列点中确定新的基准序列点，获取所述新的基准序列点的序列点姿态坐标，计算所述新的基准序列点的序列点姿态坐标与重新获取的位置姿态信息之间的综合误差，判断所述综合误差是否大于所述预设误差阈值。
可选地，所述确定单元，用于获取预设采样距离；以所述无人驾驶车辆的当前位置为基准，在所述预设参考线上采样一与所述当前位置之间的距离满足所述预设采样距离的点作为所述目标采样点。
可选地，所述生成单元，用于根据所述目标采样点对应的横坐标、纵坐标、航向角和曲率值，利用牛顿求根法求解所述采样轨迹规划器中关于曲线弧长的曲率函数对应的曲率系数；基于所述曲率系数，确定所述无人驾驶车辆的当前位置与所述目标采样点之间满足车辆运动学约束的行驶曲线，以及将所述行驶曲线作为所述无人驾驶车辆的规划行驶轨迹。
可选地，所述生成单元，用于根据所述无人驾驶车辆在所述当前位置的起点曲率值、在所述目标采样点处的终点曲率值，以及所述关于曲线弧长的曲率函数，确定关于第一曲率、第二曲率和曲线弧长的曲率系数函数，其中，所述第一曲率和所述第二曲率分别为1/3曲线弧长和2/3曲线弧长处的曲率；基于所述曲率系数函数和所述采样轨迹规划器，分别确定关于起点曲率值、第一曲率、第二曲率、曲线弧长和终点曲率值的横坐标函数、纵坐标函数和航向角函数；获取第一曲率、第二曲率和曲线弧长分别对应的第一初始值、第二初始值和第三初始值，并将所述第一初始值、所述第二初始值、所述第三初始值、所述起点曲率值和所述终点曲率值分别代入至所述横坐标函数、所述纵坐标函数、所述航向角函数和所述曲率系数函数，得到所述障碍车辆的当前位置与所述目标采样点之间的目标***点对应的横坐标、纵坐标、航向角和曲率值；将所述目标采样点对应的横坐标、纵坐标、航向角和曲率值与所述目标***点对应的横坐标、纵坐标、航向角和曲率值对应相减，得到所述目标采样点与所述目标***点之间的横坐标差值、纵坐标差值、航向角差值和曲率差值，并计算所述横坐标差值、所述纵坐标差值、所述航向角差值和所述曲率差值的差值总和；基于所述差值总和不断对所述第一初始值、所述第二初始值和所述第三初始值进行更新迭代，直至目标***点对应的横坐标、纵坐标、航向角和曲率值分别与所述目标采样点对应的横坐标、纵坐标、航向角和曲率值相同，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长；根据所述最终迭代更新后的第一曲率值、第二曲率值、曲线弧长和所述曲率系数函数，确定所述曲率系数。
可选地，所述生成单元，用于基于所述横坐标函数、所述纵坐标函数和所述航向角函数，计算所述第一曲率、所述第二曲率和所述曲线弧长分别对应的雅克比矩阵；计算所述第一曲率、所述第二曲率和所述曲线弧长分别对应的雅克比矩阵的逆矩阵，并将所述第一曲率、所述第二曲率和所述曲线弧长分别对应的逆矩阵与所述差值总和相乘，得到本轮迭代的第一曲率变化量、第二曲率变化量和曲线弧长变化量；基于所述第一曲率变化量、所述第二曲率变化量和所述曲线弧长变化量，分别对所述第一初始值、所述第二初始值和所述第三初始值进行更新；重复对所述第一初始值、所述第二初始值和所述第三初始值进行迭代更新的过程，直至所述差值总和为0，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长。
可选地，所述计算单元，用于实时对所述预设参考线进行采样，得到多个采样点，分别计算所述多个采样点中每个采样点与所述实时位置之间的欧式距离；在所述多个采样点中提取欧式距离小于预设距离阈值的采样点作为评估点，实时获取所述评估点的评估点姿态坐标、所述实时位置的实时位置姿态信息；计算所述评估点姿态坐标与所述实时位置姿态信息之间的综合误差作为所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差。
可选地，所述计算单元，用于对于所述多个采样点中每个采样点，获取所述采样点的横坐标、纵坐标；读取所述实时位置的横坐标、纵坐标；计算所述采样点的横坐标与所述实时位置的横坐标的横坐标差值，计算所述横坐标差值的第三平方值；计算所述采样点的纵坐标与所述实时位置的纵坐标的纵坐标差值，计算所述纵坐标差值的第四平方值；计算所述第三平方值和所述第四平方值的和值，以及对所述和值进行平方根计算，将得到的根值作为当前计算的采样点与所述实时位置之间的欧式距离。
可选地，所述计算单元，用于读取所述评估点姿态坐标的横坐标、纵坐标，读取所述实时位置姿态信息的横坐标、纵坐标；计算所述评估点姿态坐标的横坐标与所述实时位置姿态信息的横坐标的横坐标差值，计算所述评估点姿态坐标的纵坐标与所述实时位置姿态信息的纵坐标的纵坐标差值；读取所述评估点姿态坐标的评估点航向角，读取所述实时位置姿态信息的实时航向角，计算所述评估点航向角与所述实时航向角之间的航向角差值；计算所述横坐标差值的第五平方值、所述纵坐标差值的第六平方值，计算所述第五平方值和所述第六平方值的和值，以及对所述和值进行平方根计算，得到根值；计算所述航向角差值的正弦值，获取所述正弦值的绝对值；计算所述根值与所述绝对值的和值作为所述综合误差，以及将所述综合误差作为所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差。
根据本发明的第三个方面，提供一种芯片，所述芯片包括至少一个芯片处理器和通信接口，所述通信接口和所述至少一个芯片处理器耦合，所述至少一个芯片处理器用于运行计算机程序或指令，以实现如上述第一方面中任一项所述的路径规划方法。
根据本发明的第四个方面，提供一种终端，所述终端包括如第二方面所述的路径规划装置。
根据本发明的第五个方面，提供一种电子设备，包括设备存储器、设备处理器及存储在设备存储器上并可在设备处理器上运行的计算机程序，所述计算机程序被设备处理器执行时实现第一方面中任一项所述的方法的步骤。
根据本发明的第六个方面，提供一种存储介质，其上存储有计算机程序，所述计算机程序被介质处理器执行时实现第一方面中任一项所述的方法的步骤。
本发明提供的一种路径规划方法、装置、芯片、终端、电子设备及存储介质，与目前人工进行起步规划的方式相比，本发明实施例周期性的判断本车与预设参考线之间的综合误差是否超限，如果综合误差超限，则在预设参考线上确定目标采样点作为终点，开始进行起步路径规划，为无人驾驶车辆生成规划行驶轨迹，控制无人驾驶车辆按照规划行驶轨迹行进，并在行进的过程中实时计算无人驾驶车辆与预设参考线之间的综合误差，一旦实时综合误差小于预设误差阈值，则确定无人驾驶车辆已经回归至预设参考线，控制其按照预设参考线循迹行驶，实现综合误差超限的自动起步规划，使无人驾驶车辆自行回到预设参考线中循迹行驶，无需人工干预，智能性较好。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1示出了本发明实施例提供的一种路径规划方法流程图；
图2示出了本发明实施例提供的另一种路径规划方法流程图；
图3示出了本发明实施例提供的又一种路径规划方法的交互图；
图4示出了本发明实施例提供的一种路径规划的结构示意图；
图5示出了本发明实施例提供的存储介质示意图；
图6示出了本发明实施例提供的一种电子设备的实体结构示意图；
图7示出了本发明实施例提供的一种芯片的结构示意图；
图8示出了本发明实施例提供的一种终端的结构示意图。
具体实施方式
下文中将参考附图并结合实施例来详细说明本发明。需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
实施例一
目前，无人驾驶矿卡起步时可能已经偏离全局路径太远，导致综合误差超限使无人驾驶矿卡决策停车，不能回到全局路径继续循迹，需要人工进行起步规划。
为了解决上述问题，本发明实施例提供了一种路径规划方法，如图1所示，所述方法包括：
101、当检测到无人驾驶车辆与预设参考线之间的综合误差超限时，在预设参考线上确定目标采样点。
其中，无人驾驶车辆包括矿用运输车辆，该矿用运输车辆具体包括：矿卡、宽体车和铰接式矿车等。预设参考线是无人驾驶车辆中预设的车辆行驶轨迹，预设参考线包括多个离散的路径序列点，这些离散的路径序列点能够方便后续计算无人驾驶车辆与该预设参考线之间的距离，进而对无人驾驶车辆进行起步规划。
无人驾驶车辆与预设参考线之间的综合误差超限实际上也就是车辆偏离预设参考线较多，距离预设参考线过远，车辆无法正常按照预先规划的路线行驶，导致无人驾驶车辆停车。因此，在本发明实施例中，当检测到无人驾驶车辆与预设参考线之间的综合误差超限时，在预设参考线上确定目标采样点，以便后续将该目标采样点作为终点，为无人驾驶车辆规划路径，使得无人驾驶车辆向目标采样点行驶，从而回归到预设参考线上循迹行驶。其中，目标采样点可以为按照预设采样距离在预设参考线上采样的一路径序列点，以便后续以该目标采样点为无人驾驶车辆规划的路径能够使无人驾驶车辆快速回归至预设参考线行驶。
本发明实施例主要应用于矿区车辆的行驶路径规划的场景。本发明实施例的执行主体为无人驾驶车辆中配备的规划模块，该规划模块能够从无人驾驶车辆的定位模块接收本车的位姿信息，以及从无人驾驶车辆的路由模块接收预设参考线的信息；然后周期性判断本车与预设参考线之间的综合误差是否超限，如果不超限，则控制本车沿着预设参考线循迹行驶，同时该规划模块将循迹路径序列点的信息发送给控制模块，如果综合误差超限，则该规划模块触发起步规划开关，同时开始进行起步路径规划，生成规划行驶轨迹，并将规划行驶轨迹上的起步路径规划序列点发送给控制模块，以使本车按照规划行驶轨迹行进；最后，该规划模块本车与预设参考线之间的实时综合误差，一旦综合误差小于设定的预设误差阈值，则结束起步规划状态，回到循迹状态，控制无人驾驶车辆按照预设参考线循迹行驶。
102、以无人驾驶车辆的当前位置为起点、目标采样点为终点，采用采样轨迹规划器生成无人驾驶车辆的规划行驶轨迹，控制无人驾驶车辆按照规划行驶轨迹行进。
在本发明实施例中，在预设参考线上确定了目标采样点后，规划模块会以无人驾驶车辆的当前位置为起点、目标采样点为终点，采用采样轨迹规划器生成无人驾驶车辆的规划行驶轨迹，控制无人驾驶车辆按照规划行驶轨迹行进，以使无人驾驶车辆回归到预设参考线上行驶。
其中，当前位置可以由无人驾驶车辆中的定位模块对车辆进行定位得到；具体地，当前位置可以采用（x，y，z，θ，k，v）的位置姿态信息描述，其中，（x，y，z）表示无人驾驶车辆当前所处位置坐标，θ为航向角，k为曲率值，v为速度，使得位置姿态信息能够同时体现本车的位置姿态和速度。
103、在无人驾驶车辆按照规划行驶轨迹行进的过程中，实时计算无人驾驶车辆的实时位置与预设参考线之间的实时综合误差，以及在计算得到的实时综合误差小于预设误差阈值时，控制无人驾驶车辆按照预设参考线循迹行驶。
在本发明实施例中，在无人驾驶车辆按照规划行驶轨迹行进的过程中，规划模块会不断的获取本车的实时位置，并实时计算无人驾驶车辆的实时位置与预设参考线之间的实时综合误差，以判断本车是否回到预设参考线行驶。实时综合误差与上述步骤101中描述的综合误差同理，也是用于描述本车当前与预设参考线之间的偏离程度的，因此，在计算得到的实时综合误差小于预设误差阈值时，表示该无人驾驶车辆实际上与预设参考线距离很近，基本上能够认定无人驾驶车辆已经回归至预设参考线，开始控制无人驾驶车辆按照预设参考线循迹行驶。
其中，预设误差阈值可以根据场景、精度或无人驾驶车辆尺寸等的要求进行动态化设置，比如可以为5米、3米、1米等等。以预设误差阈值为5米为例，当计算得到的无人车辆与预设参考线之间的实时综合误差为3米时，确定实时综合误差小于预设误差阈值，可以确定无人车辆已经回到了预设参考线上，结束当前流程即可，否则，需要继续计算无人驾驶车辆与预设参考线之间的实时综合误差值，直至计算得到的实时综合误差值小于5米，才可以结束当前流程。需要说明的是，对于一些尺寸较大的车辆，由于其占地面积较大，因此可以适当增大预设误差阈值，相反，对于一些尺寸较小的车辆，可以适当减小预设误差阈值，本发明对预设误差阈值的具体数值不进行限定。
本发明实施例提供的一种路径规划方法，与目前人工进行起步规划的方式相比，本发明实施例周期性的判断本车与预设参考线之间的综合误差是否超限，如果综合误差超限，则在预设参考线上确定目标采样点作为终点，开始进行起步路径规划，为无人驾驶车辆生成规划行驶轨迹，控制无人驾驶车辆按照规划行驶轨迹行进，并在行进的过程中实时计算无人驾驶车辆与预设参考线之间的综合误差，一旦实时综合误差小于预设误差阈值，则确定无人驾驶车辆已经回归至预设参考线，控制其按照预设参考线循迹行驶，实现综合误差超限的自动起步规划，使无人驾驶车辆自行回到预设参考线中循迹行驶，无需人工干预，智能性较好。
实施例二
进一步的，为了更好的说明上述对矿区车辆的行驶路径进行规划的过程，作为对上述实施例的细化和扩展，本发明实施例提供了另一种路径规划方法，如图2所示，方法包括：
201、每隔预设检测周期，获取无人驾驶车辆的当前位置的位置姿态信息，以及确定无人驾驶车辆中预设的预设参考线。
在本发明实施例中，为了在无人驾驶车辆综合误差超限时能够及时开始起步规划，无人驾驶车辆的规划模块会周期性地循环进行综合误差的统计，而综合误差实际上指示了无人驾驶车辆当前位置与预设参考线之间的偏差程度，因此，规划模块每隔预设检测周期，会获取无人驾驶车辆的当前位置的位置姿态信息，以及确定无人驾驶车辆中预设的预设参考线，以便后续基于位置姿态信息和预设参考线统计本车的综合误差。其中，预设检测周期可以根据实际的应用场景自行定义设置，比如应用场景中无人驾驶车辆较多，车辆偏离后容易发生事故，需要及时对偏离的车辆进行调整，则可以将预设检测周期设置为1分钟、2分钟等等，缩短预设检测周期的时长，保证及时发现车辆的超限，尽快将车辆引导至预设参考线上；而再比如应用场景中只有一辆无人驾驶车辆，且该场景中也不存在其他有人驾驶车辆，该无人驾驶车辆仅用于执行周边的巡逻任务，则可以将预设检测周期设置为10分钟、15分钟等等，因为虽然偏离了预设参考线，但是仍旧满足巡逻任务的条件，所以可以适当增加预设检测周期的时长，避免在计算车辆是否超限上浪费大量的算力，本发明对预设检测周期的具体取值不进行限定。
其中，无人驾驶车辆中配置有定位模块，定位模块可以为规划模块提供本车的位置姿态信息；另外，无人驾驶车辆中还配置有路由模块，路由模块负责与控制无人驾驶车辆的基站进行交互，基站会通过有线或无线网络将预设参考线发送至路由模块，由路由模块将预设参考线提供给规划模块，以便规划模块控制本车按照预设参考线循迹行驶以及起步规划，位置姿态信息和预设参考线的具体内容如上述步骤101和102中的描述，此处不再进行赘述。
202、读取预设参考线包括的多个路径序列点，分别计算多个路径序列点中每个路径序列点与无人驾驶车辆的当前位置的距离，在多个路径序列点中确定基准序列点。
在本发明实施例中，统计本车与预设参考线之间的综合误差时，需要在预设参考线包括的多个路径序列点中找到距离本车最近的一路径序列点，计算本车与该路径序列点之间的偏移量作为综合误差。
其中，本发明实施例将该距离本车最近的一路径序列点作为基准序列点，基准序列点与无人驾驶车辆的当前位置之间的距离小于多个路径序列点中除基准序列点外的其他路径序列点。在一个可选地实施方案中，规划模块会读取预设参考线包括的多个路径序列点，分别计算多个路径序列点中每个路径序列点与无人驾驶车辆的当前位置的距离，在多个路径序列点中确定基准序列点。实际应用的过程中，由于规划模块获取到的无人驾驶车辆的当前位置的位置姿态信息中包括横纵坐标，因此，在计算每个路径序列点与无人驾驶车辆的当前位置的距离时，可以在二维坐标系中计算每个路径序列点的横纵坐标与车辆的横纵坐标之间的距离，并选取出距离最小的一路径序列点作为所述基准序列点。
203、获取基准序列点的序列点姿态坐标，计算序列点姿态坐标与位置姿态信息之间的综合误差，当综合误差小于等于预设误差阈值时，执行下述步骤204；当综合误差大于预设误差阈值时，执行下述步骤205至步骤208。
在本发明实施例中，确定了基准序列点后，开始计算本车的当前位置与该基准序列点之间的综合误差，计算得到的综合误差也即代表了无人驾驶车辆与预设参考线之间的综合误差。在一个可选地实施方案中，计算综合误差的过程如下：
首先，读取序列点姿态坐标的横坐标、纵坐标，读取位置姿态信息的横坐标、纵坐标。随后，计算序列点姿态坐标的横坐标与位置姿态信息的横坐标的横坐标差值，计算序列点姿态坐标的纵坐标与位置姿态信息的纵坐标的纵坐标差值。接着，读取序列点姿态坐标的序列点航向角，读取位置姿态信息的车辆航向角，计算序列点航向角与车辆航向角之间的航向角差值。最后，计算横坐标差值的第一平方值、纵坐标差值的第二平方值，计算第一平方值和第二平方值的和值，以及对和值进行平方根计算，得到根值，并计算航向角差值的正弦值，获取正弦值的绝对值，计算根值与绝对值的和值作为序列点姿态坐标与位置姿态信息之间的综合误差。其中，上述计算综合误差的过程可以通过下述公式1实现：
公式1：
其中，E表示综合误差，xn为序列点姿态坐标的横坐标，x为位置姿态信息的横坐标，yn为序列点姿态坐标的纵坐标，y为位置点姿态信息的纵坐标，θn为序列点航向角，θ为车辆航向角，sin（）表示正弦函数，fabs（）表示绝对值函数。
计算得到综合误差后，规划模块中事先设置有预设误差阈值，规划模块会将计算得到的综合误差与预设误差阈值进行比对，从而判断无人驾驶车辆是否综合误差超限。其中，当综合误差小于等于预设误差阈值时，表示无人驾驶车辆并未发生综合误差超限，是可以正常按照预设参考线循迹前进的，无人驾驶车辆正常行进即可，因此，执行下述步骤204；而当综合误差大于预设误差阈值时，表示无人驾驶车辆与预设参考线之间的偏差较大，车辆距离预设参考线较远，综合误差超限，需要为无人驾驶车辆进行起步规划，因此，执行下述步骤205至步骤208。
204、当综合误差小于等于预设误差阈值时，控制无人驾驶车辆按照预设参考线循迹行驶。
在本发明实施例中，当综合误差小于等于预设误差阈值时，表示无人驾驶车辆并未发生综合误差超限，是可以正常按照预设参考线循迹前进的，无人驾驶车辆正常行进即可。而由于规划模块的综合误差超限检测是实时或者循环进行的，因此，规划模块会继续每隔预设检测周期，重新获取无人驾驶车辆的当前位置的位置姿态信息，以及确定预设参考线，重新在预设参考线包括的多个路径序列点中确定新的基准序列点，获取新的基准序列点的序列点姿态坐标，计算新的基准序列点的序列点姿态坐标与重新获取的位置姿态信息之间的综合误差，判断综合误差是否大于预设误差阈值，以便及时感知无人驾驶车辆综合误差超限停车并及时作出反应。
205、当综合误差大于预设误差阈值时，确定检测到无人驾驶车辆与预设参考线之间的综合误差超限，在预设参考线上确定目标采样点。
在本发明实施例中，当综合误差大于预设误差阈值时，表示无人驾驶车辆与预设参考线之间的偏差较大，车辆距离预设参考线较远，综合误差超限，需要为无人驾驶车辆进行起步规划，因此，规划模块会在预设参考线上确定目标采样点，后续将目标采样点作为选择的终点，为无人驾驶车辆规划一条安全、平滑、满足运动学约束的路径，使无人驾驶车辆朝着目标采样点行驶，最终回到预设参考线上。
其中，为了采样确定该目标采样点，规划模块中设置有预设采样距离。在一个可选地实施方案中，规划模块获取预设采样距离，以无人驾驶车辆的当前位置为基准，在预设参考线上采样一与当前位置之间的距离满足预设采样距离的点作为目标采样点。
206、以无人驾驶车辆的当前位置为起点、目标采样点为终点，采用采样轨迹规划器生成无人驾驶车辆的规划行驶轨迹，控制无人驾驶车辆按照规划行驶轨迹行进。
在本发明实施例中，确定了目标采样点后，规划模块以无人驾驶车辆的当前位置为起点、目标采样点为终点，采用采样轨迹规划器生成无人驾驶车辆的规划行驶轨迹，控制无人驾驶车辆按照规划行驶轨迹行进。在一个可选地实施方案中，生成规划行驶轨迹的过程如下：
首先，根据目标采样点对应的横坐标、纵坐标、航向角和曲率值，利用牛顿求根法求解采样轨迹规划器中关于曲线弧长的曲率函数对应的曲率系数；随后，基于曲率系数，确定无人驾驶车辆的当前位置与目标采样点之间满足车辆运动学约束的行驶曲线，以及将行驶曲线作为无人驾驶车辆的规划行驶轨迹。其中，采样轨迹规划器是采用三次多项式螺旋线来采样生成起步规划路径的，也即规划行驶轨迹实质为三次多项式螺旋线，关于规划行驶轨迹的曲线弧长的曲率函数如下述公式2：
公式2：
其中，k（s）曲率函数是关于曲线弧长的三次多项式函数，s是曲线弧长，a、b、c、d是需要求解的未知曲率系数。因此只要通过目标采样点的横坐标、纵坐标、航向角和曲率值，确定曲率系数a、b、c、d，便能够确以无人驾驶车辆的当前位置为起点、目标采样点为终点构建一条行驶曲线作为规划行驶轨迹。
进一步地，采样轨迹规划器中包括关于曲线弧长的横坐标函数、纵坐标函数、航向角函数和曲率函数，具体如下述公式3所示：
公式3：
其中，x(s)是关于曲线弧长的横坐标函数，y(s)是关于曲线弧长的纵坐标函数，θ(s)是关于曲线弧长的航向角函数。由于已知目标采样点对应的横坐标、纵坐标、航向角和曲率值，以及无人驾驶车辆的当前位置对应的横坐标、纵坐标、航向角和曲率值，将其代入至上述公式3中进行解方程，便能够确定曲率系数a、b、c、d，但是由于上述表达式中包括菲涅尔积分，很难直接解出曲率系数a、b、c、d，因此本发明实施例采用牛顿求根方法求解三次多项式的曲率系数a、b、c、d。
进一步地，针对利用牛顿求根法求解曲率系数的具体过程，作为一种可选实施方式，所述方法包括：根据所述障碍车辆分别在所述当前位置的起点曲率值、在所述目标采样点处的终点曲率值，以及关于曲线弧长的曲率函数，确定关于起点曲率值、第一曲率、第二曲率、终点曲率值和曲线弧长的曲率系数函数，其中，第一曲率和第二曲率分别为1/3曲线弧长和2/3曲线弧长处的曲率；基于所述曲率系数函数和所述采样轨迹规划器，分别确定关于起点曲率值、第一曲率、第二曲率、曲线弧长和终点曲率值的横坐标函数、纵坐标函数和航向角函数；获取第一曲率、第二曲率和曲线弧长分别对应的第一初始值、第二初始值和第三初始值，并将所述第一初始值、所述第二初始值、所述第三初始值、所述起点曲率值和所述终点曲率值分别代入至所述横坐标函数、所述纵坐标函数、所述航向角函数和所述曲率系数函数，得到所述障碍车辆的当前位置与所述目标采样点之间的目标***点对应的横坐标、纵坐标、航向角和曲率值；将目标采样点对应的横坐标、纵坐标、航向角和曲率值与目标***点对应的横坐标、纵坐标、航向角和曲率值对应相减，得到目标采样点与目标***点之间的横坐标差值、纵坐标差值、航向角差值和曲率差值，并计算横坐标差值、纵坐标差值、航向角差值和曲率差值的差值总和；基于差值总和不断对第一初始值、第二初始值和第三初始值进行更新迭代，直至目标***点对应的横坐标、纵坐标、航向角和曲率值分别与目标采样点对应的横坐标、纵坐标、航向角和曲率值相同，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长；根据最终迭代更新后的第一曲率值、第二曲率值、曲线弧长和曲率系数函数，确定曲率系数。
进一步地，所述基于所述差值总和不断对所述第一初始值、所述第二初始值和所述第三初始值进行更新迭代，直至目标***点对应的横坐标、纵坐标、航向角和曲率值分别与所述目标采样点对应的横坐标、纵坐标、航向角和曲率值相同，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长，包括：基于横坐标函数、纵坐标函数和航向角函数，计算第一曲率、第二曲率和曲线弧长分别对应的雅克比矩阵；计算第一曲率、第二曲率和曲线弧长分别对应的雅克比矩阵的逆矩阵，并将第一曲率、第二曲率和曲线弧长分别对应的逆矩阵与差值总和相乘，得到本轮迭代的第一曲率变化量、第二曲率变化量和曲线弧长变化量；基于第一曲率变化量、第二曲率变化量和曲线弧长变化量，分别对第一初始值、第二初始值和第三初始值进行更新；重复对第一初始值、第二初始值和第三初始值进行迭代更新的过程，直至差值总和为0，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长。
具体地，假设曲线弧长总长为S f ，在曲线弧长起始位置0处的曲率为P 0 ，三分之一曲线弧长处的曲率为P 1 ，三分之二曲线弧长处的曲率为P 2 ，在曲线弧长终点位置处的曲率为P3，由此可以得到如公式4所示的表达式：
公式4：
进一步地，通过联立公式2和公式4可得出下述公式5：
公式5：
其中，P0是无人驾驶车辆当前位置处的曲率，也即在曲线弧长起始位置0处的曲率；P3是目标采样点处的曲率，也即在曲线弧长终点位置处的曲率；P0和 P3均是已知的，Sf、P1和P2是未知的，从公式5可知，只要解出Sf、P1和P2，便可以确定曲率系数a、b、c、d。
基于此，本发明实施例将解Sf、P1和P2的问题描述为如公式6所述的一个求根的问题：
公式6：
其中，xdes代表目标采样点对应的横坐标、纵坐标、航向角和曲率值，xp(sf)代表无人驾驶车辆的当前位置与目标采样点之间的目标***点对应的横坐标、纵坐标、航向角和曲率值，在迭代的过程中目标***点对应的横坐标、纵坐标、航向角和曲率值会不断发生变化，当目标***点对应的横坐标、纵坐标、航向角和曲率值与目标采样点对应的横坐标、纵坐标、航向角和曲率值相等时，说明目标***点与目标采样点重合，停止迭代，具体如下述公式7：
公式7：
其中，xpi(sf) 代表第i轮迭代过程中目标***点对应的横坐标、纵坐标、航向角和曲率值，Δx代表目标采样点与目标***点之间的横坐标差值、纵坐标差值、航向角差值和曲率差值的差值总和，Pi代表第i轮迭代过程中Sf、P1和P2的值，Pi+1代表第i+1轮迭代过程中Sf、P1和P2的值，Δp 代表第一曲率变化量（P1变化量）、第二曲率变化量（P2变化量）和曲线弧长变化量（Sf变化量），Jpi(xpi(sf))代表Sf、P1和P2分别对应的雅克比矩阵，Jpi(xpi(sf))-1为代表雅克比矩阵对应的逆矩阵，dxpi(sf)代表关于Sf、P1和P2的横坐标函数、纵坐标函数和航向角函数分别对Sf、P1和P2求偏导，具体可以通过联立公式5和公式3确定关于Sf、P1和P2的横坐标函数、纵坐标函数和航向角函数，dpi为参数，设为0.001。
具体在利用上述公式求解Sf、P1和P2时，首先根据实际经验分别给定Sf、P1和P2的第一初始值、第二初始值和第三初始值，之后将其代入值公式5确定初始曲率系数，接着设定S=0.1，并将初始曲率系数代入值公式3，得到目标***点对应的横坐标、纵坐标、航行角和曲率值，需要说明的是，每轮迭代目标***点对应的曲线弧长S增加0.1。进一步地，将目标***点对应的横坐标、纵坐标、航行角和曲率值，以及目标采样点对应的横坐标、纵坐标、航行角和曲率值，代入至公式7中计算差值总和Δx。
进一步地，根据横坐标函数、纵坐标函数和和航向角函数关于P1的偏导数，以及第一初始值、第二初始值和第三初始值，计算P1对应的雅克比矩阵的逆矩阵，并将P1对应的雅克比矩阵的逆矩阵与差值总和Δx相乘，得到本轮迭代的P1变化量，同理可以得到本轮迭代的P2变化量和Sf变化量。之后基于上述变量分别对Sf、P1和P2的第一初始值、第二初始值和第三初始值进行更新，由此完成本轮对Sf、P1和P2的更新迭代过程。重复上述对Sf、P1和P2的更新迭代过程，直至Δx为0，即目标***点对应的横坐标、纵坐标、航向角和曲率值，与目标采样点对应的横坐标、纵坐标、航向角和曲率值对应相同，停止更新迭代过程，获取最终输出的Sf、P1和P2的值，并将最终输出的Sf、P1和P2的值代入至公式5中，便可以确定无人驾驶车辆的当前位置与目标采样点之间的一条满足车辆运动学约束的行驶曲线作为规划行驶轨迹。
由于本发明实施例生成了满足生成满足车辆运动学约束的行驶轨迹作为规划行驶轨迹，因此能够保证规划行驶轨迹的生成精度，从而能够为后续无人驾驶车辆回归预设参考线提供有力的数据支撑。
207、在无人驾驶车辆按照规划行驶轨迹行进的过程中，实时计算无人驾驶车辆的实时位置与预设参考线之间的实时综合误差。
在本发明实施例中，为无人驾驶车辆规划了规划行驶轨迹后，规划模块会将规划行驶轨迹中的路径序列点依次发送给控制模块，以使控制模块控制无人驾驶车辆按照规划行驶轨迹行进。由于无人驾驶车辆按照规划行驶轨迹行进会使无人驾驶车辆与预设参考线越来越近，因此，为了判断无人驾驶车辆是否已经处于预设参考线上，规划模块会实时计算无人驾驶车辆的实时位置与预设参考线之间的实时综合误差，基于实时综合误差来判断无人驾驶车辆是否已经到达预设参考线。
其中，在计算无人驾驶车辆的实时位置与预设参考线之间的实时综合误差时，首先需要在预设参考线上选择一个采样点作为评估点，计算无人驾驶车辆与该评估点之间的综合误差，将该综合误差作为无人驾驶车辆的实时位置与预设参考线之间的实时综合误差。在一个可选地实施方案中，可以实时对预设参考线进行采样，得到多个采样点，分别计算多个采样点中每个采样点与实时位置之间的欧式距离，确定将哪一个采样点作为评估点，具体计算实时位置与采样点之间欧式距离的过程如下：对于多个采样点中每个采样点，获取采样点的横坐标、纵坐标，读取实时位置的横坐标、纵坐标；计算采样点的横坐标与实时位置的横坐标的横坐标差值，计算横坐标差值的第三平方值；计算采样点的纵坐标与实时位置的纵坐标的纵坐标差值，计算纵坐标差值的第四平方值；计算第三平方值和第四平方值的和值，以及对和值进行平方根计算，将得到的根值作为当前计算的采样点与实时位置之间的欧式距离。实际应用的过程中，计算欧式距离的过程可以通过下述公式8实现：
公式8： 
其中，D表示计算得到的欧式距离，sqrt（）表示平方根计算函数，x1表示采样点的横坐标，x2表示实时位置的横坐标，y1表示采样点的纵坐标，y2表示实时位置的纵坐标。
计算得到每个采样点与实时位置之间的欧式距离之后，规划模块在多个采样点中提取欧式距离小于预设距离阈值的采样点作为评估点，并实时获取评估点的评估点姿态坐标、实时位置的实时位置姿态信息，计算评估点姿态坐标与实时位置姿态信息之间的综合误差作为无人驾驶车辆的实时位置与预设参考线之间的实时综合误差。在一个可选地实施方案中，计算实时综合误差的过程如下：读取评估点姿态坐标的横坐标、纵坐标，读取实时位置姿态信息的横坐标、纵坐标；计算评估点姿态坐标的横坐标与实时位置姿态信息的横坐标的横坐标差值，计算评估点姿态坐标的纵坐标与实时位置姿态信息的纵坐标的纵坐标差值；读取评估点姿态坐标的评估点航向角，读取实时位置姿态信息的实时航向角，计算评估点航向角与实时航向角之间的航向角差值；计算横坐标差值的第五平方值、纵坐标差值的第六平方值，计算第五平方值和第六平方值的和值，以及对和值进行平方根计算，得到根值；计算航向角差值的正弦值，获取正弦值的绝对值；计算根值与绝对值的和值作为综合误差，以及将综合误差作为无人驾驶车辆的实时位置与预设参考线之间的实时综合误差。其中，计算实时综合误差的过程与上述步骤203中描述的计算综合误差的过程同理，也可以基于上述公式1计算得到，此处不再赘述。
208、在计算得到的实时综合误差小于预设误差阈值时，控制无人驾驶车辆按照预设参考线循迹行驶。
在本发明实施例中，计算得到实时综合误差后，规划模块会将实时综合误差将预设误差阈值进行比对，判断无人驾驶车辆是否已经回到预设参考线行驶。其中，如果计算得到的实时综合误差仍旧大于预设误差阈值，则表示无人驾驶车辆还没有回到预设参考线行驶，无人驾驶车辆会继续按照规划行驶轨迹行进，规划模块也会继续实时计算实时综合误差，直至计算得到的实时综合误差小于预设误差阈值；而如果计算得到的实时综合误差小于预设误差阈值，则表示无人驾驶车辆已经回到了预设参考线，无人驾驶车辆可以按照预设参考线正常行驶，因此，控制无人驾驶车辆按照预设参考线循迹行驶。其中，由于路由模块是将接收到的预设参考线发送至规划模块，因此，规划模块可以将预设参考线上的路径序列点依次发送给控制模块，以使控制模块按照路径序列点控制无人驾驶车辆按照预设参考线循迹行驶。
本发明实施例提供的另一种路径规划方法，与目前人工进行起步规划的方式相比，本发明实施例周期性的判断本车与预设参考线之间的综合误差是否超限，如果综合误差超限，则在预设参考线上确定目标采样点作为终点，开始进行起步路径规划，为无人驾驶车辆生成规划行驶轨迹，控制无人驾驶车辆按照规划行驶轨迹行进，并在行进的过程中实时计算无人驾驶车辆与预设参考线之间的综合误差，一旦实时综合误差小于预设误差阈值，则确定无人驾驶车辆已经回归至预设参考线，控制其按照预设参考线循迹行驶，实现综合误差超限的自动起步规划，使无人驾驶车辆自行回到预设参考线中循迹行驶，无需人工干预，智能性较好。
实施例三
为了更好的说明上述路径规划的过程，作为对上述实施例的细化和扩展，本发明实施例提供了一种路径规划方法，如图3所示，由于无人驾驶车辆中设置有定位模块、规划模块、路由模块和控制模块，因此，该方法涉及到上述提及的模块之间的交互，该方法包括：
301、定位模块获取无人驾驶车辆的当前位置的位置姿态信息，将位置姿态信息发送至规划模块。
其中，位置姿态信息的具体内容如上述步骤101和201的描述，此处不再进行赘述。
302、路由模块获取预设参考线，将预设参考线发送至规划模块。
其中，预设参考线的具体内容如上述步骤102和201的描述，此处不再进行赘述。
303、规划模块基于接收到的位置姿态信息和预设参考线，计算综合误差，判断综合误差是否超限，若综合误差未超限，则执行下述步骤304；若综合误差超限，则执行下述步骤305至步骤306。
其中，计算综合误差的过程如上述步骤202至步骤203的描述，此处不再进行赘述。
304、若综合误差未超限，则规划模块沿着预设参考线循迹，将循迹到的路径序列点依次发送至控制模块，以使控制模块控制无人驾驶车辆前进。
其中，综合误差未超限的情况如上述步骤204的描述，此处不再进行赘述。
305、若综合误差超限，则规划模块开始对无人驾驶车辆起步规划，生成规划行驶轨迹，沿着规划行驶轨迹循迹，将循迹到的路径序列点依次发送至控制模块，以使控制模块控制无人驾驶车辆按照规划行驶轨迹行进。
其中，生成规划行驶轨迹的过程如上述步骤205至步骤206中的描述，此处不再进行赘述。
306、规划模块重复执行上述步骤301至步骤303中的过程，继续实时计算无人驾驶车辆的实时位置与预设参考线之间的实时综合误差，以及在计算得到的实时综合误差小于预设误差阈值时，确定无人驾驶车辆回归至预设参考线，规划模块沿着预设参考线循迹，将循迹到的路径序列点依次发送至控制模块，以使控制模块控制无人驾驶车辆前进。
其中，继续实时计算无人驾驶车辆的实时位置与预设参考线之间的实时综合误差以及循迹的过程如上述步骤207至步骤208中的过程，此处不再进行赘述。
本发明实施例提供的一种路径规划方法，与目前人工进行起步规划的方式相比，本发明实施例实现综合误差超限的自动起步规划，使无人驾驶车辆自行回到预设参考线中循迹行驶，无需人工干预，智能性较好。
实施例四
进一步地，作为图1的具体实现，本发明实施例提供了一种路径规划装置，如图4所示，所述装置包括：确定单元401，生成单元402和计算单元403。
该确定单元401，用于当检测到无人驾驶车辆与预设参考线之间的综合误差超限时，在所述预设参考线上确定目标采样点，所述预设参考线是所述无人驾驶车辆中预设的车辆行驶轨迹；
该生成单元402，用于以所述无人驾驶车辆的当前位置为起点、所述目标采样点为终点，采用采样轨迹规划器生成所述无人驾驶车辆的规划行驶轨迹，控制所述无人驾驶车辆按照所述规划行驶轨迹行进；
该计算单元403，用于在所述无人驾驶车辆按照所述规划行驶轨迹行进的过程中，实时计算所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差，以及在计算得到的实时综合误差小于预设误差阈值时，控制所述无人驾驶车辆按照所述预设参考线循迹行驶。
在具体应用场景中，该计算单元403，还用于每隔预设检测周期，获取所述无人驾驶车辆的当前位置的位置姿态信息，以及确定所述无人驾驶车辆中预设的所述预设参考线；读取所述预设参考线包括的多个路径序列点，分别计算所述多个路径序列点中每个路径序列点与所述无人驾驶车辆的当前位置的距离，在所述多个路径序列点中确定基准序列点，所述基准序列点与所述无人驾驶车辆的当前位置之间的距离小于所述多个路径序列点中除所述基准序列点外的其他路径序列点；获取所述基准序列点的序列点姿态坐标，计算所述序列点姿态坐标与所述位置姿态信息之间的综合误差；当所述综合误差大于所述预设误差阈值时，确定检测到所述无人驾驶车辆与预设参考线之间的综合误差超限。
在具体应用场景中，该计算单元403，用于读取所述序列点姿态坐标的横坐标、纵坐标，读取所述位置姿态信息的横坐标、纵坐标；计算所述序列点姿态坐标的横坐标与所述位置姿态信息的横坐标的横坐标差值，计算所述序列点姿态坐标的纵坐标与所述位置姿态信息的纵坐标的纵坐标差值；读取所述序列点姿态坐标的序列点航向角，读取所述位置姿态信息的车辆航向角，计算所述序列点航向角与所述车辆航向角之间的航向角差值；计算所述横坐标差值的第一平方值、所述纵坐标差值的第二平方值，计算所述第一平方值和所述第二平方值的和值，以及对所述和值进行平方根计算，得到根值；计算所述航向角差值的正弦值，获取所述正弦值的绝对值；计算所述根值与所述绝对值的和值作为所述序列点姿态坐标与所述位置姿态信息之间的综合误差。
在具体应用场景中，该计算单元403，还用于当所述综合误差小于等于所述预设误差阈值时，控制所述无人驾驶车辆按照所述预设参考线循迹行驶，以及继续每隔所述预设检测周期，重新获取所述无人驾驶车辆的当前位置的位置姿态信息，以及确定所述预设参考线，重新在所述预设参考线包括的多个路径序列点中确定新的基准序列点，获取所述新的基准序列点的序列点姿态坐标，计算所述新的基准序列点的序列点姿态坐标与重新获取的位置姿态信息之间的综合误差，判断所述综合误差是否大于所述预设误差阈值。
在具体应用场景中，该确定单元401，用于获取预设采样距离；以所述无人驾驶车辆的当前位置为基准，在所述预设参考线上采样一与所述当前位置之间的距离满足所述预设采样距离的点作为所述目标采样点。
在具体应用场景中，该生成单元402，用于根据所述目标采样点对应的横坐标、纵坐标、航向角和曲率值，利用牛顿求根法求解所述采样轨迹规划器中关于曲线弧长的曲率函数对应的曲率系数；基于所述曲率系数，确定所述无人驾驶车辆的当前位置与所述目标采样点之间满足车辆运动学约束的行驶曲线，以及将所述行驶曲线作为所述无人驾驶车辆的规划行驶轨迹。
在具体应用场景中，该生成单元402，用于根据所述无人驾驶车辆在所述当前位置的起点曲率值、在所述目标采样点处的终点曲率值，以及所述关于曲线弧长的曲率函数，确定关于起点曲率值、第一曲率、第二曲率、终点曲率值和曲线弧长的曲率系数函数，其中，所述第一曲率和所述第二曲率分别为1/3曲线弧长和2/3曲线弧长处的曲率；基于所述曲率系数函数和所述采样轨迹规划器，分别确定关于起点曲率值、第一曲率、第二曲率、曲线弧长和终点曲率值的横坐标函数、纵坐标函数和航向角函数；获取第一曲率、第二曲率和曲线弧长分别对应的第一初始值、第二初始值和第三初始值，并将所述第一初始值、所述第二初始值、所述第三初始值、所述起点曲率值和所述终点曲率值分别代入至所述横坐标函数、所述纵坐标函数、所述航向角函数和所述曲率系数函数，得到所述障碍车辆的当前位置与所述目标采样点之间的目标***点对应的横坐标、纵坐标、航向角和曲率值；将所述目标采样点对应的横坐标、纵坐标、航向角和曲率值与所述目标***点对应的横坐标、纵坐标、航向角和曲率值对应相减，得到所述目标采样点与所述目标***点之间的横坐标差值、纵坐标差值、航向角差值和曲率差值，并计算所述横坐标差值、所述纵坐标差值、所述航向角差值和所述曲率差值的差值总和；基于所述差值总和不断对所述第一初始值、所述第二初始值和所述第三初始值进行更新迭代，直至目标***点对应的横坐标、纵坐标、航向角和曲率值分别与所述目标采样点对应的横坐标、纵坐标、航向角和曲率值相同，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长；根据所述最终迭代更新后的第一曲率值、第二曲率值、曲线弧长和所述曲率系数函数，确定所述曲率系数。
在具体应用场景中，该生成单元402，用于基于所述横坐标函数、所述纵坐标函数和所述航向角函数，计算所述第一曲率、所述第二曲率和所述曲线弧长分别对应的雅克比矩阵；计算所述第一曲率、所述第二曲率和所述曲线弧长分别对应的雅克比矩阵的逆矩阵，并将所述第一曲率、所述第二曲率和所述曲线弧长分别对应的逆矩阵与所述差值总和相乘，得到本轮迭代的第一曲率变化量、第二曲率变化量和曲线弧长变化量；基于所述第一曲率变化量、所述第二曲率变化量和所述曲线弧长变化量，分别对所述第一初始值、所述第二初始值和所述第三初始值进行更新；重复对所述第一初始值、所述第二初始值和所述第三初始值进行迭代更新的过程，直至所述差值总和为0，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长。
在具体应用场景中，该计算单元403，用于实时对所述预设参考线进行采样，得到多个采样点，分别计算所述多个采样点中每个采样点与所述实时位置之间的欧式距离；在所述多个采样点中提取欧式距离小于预设距离阈值的采样点作为评估点，实时获取所述评估点的评估点姿态坐标、所述实时位置的实时位置姿态信息；计算所述评估点姿态坐标与所述实时位置姿态信息之间的综合误差作为所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差。
在具体应用场景中，该计算单元403，用于对于所述多个采样点中每个采样点，获取所述采样点的横坐标、纵坐标；读取所述实时位置的横坐标、纵坐标；计算所述采样点的横坐标与所述实时位置的横坐标的横坐标差值，计算所述横坐标差值的第三平方值；计算所述采样点的纵坐标与所述实时位置的纵坐标的纵坐标差值，计算所述纵坐标差值的第四平方值；计算所述第三平方值和所述第四平方值的和值，以及对所述和值进行平方根计算，将得到的根值作为当前计算的采样点与所述实时位置之间的欧式距离。
在具体应用场景中，该计算单元403，用于读取所述评估点姿态坐标的横坐标、纵坐标，读取所述实时位置姿态信息的横坐标、纵坐标；计算所述评估点姿态坐标的横坐标与所述实时位置姿态信息的横坐标的横坐标差值，计算所述评估点姿态坐标的纵坐标与所述实时位置姿态信息的纵坐标的纵坐标差值；读取所述评估点姿态坐标的评估点航向角，读取所述实时位置姿态信息的实时航向角，计算所述评估点航向角与所述实时航向角之间的航向角差值；计算所述横坐标差值的第五平方值、所述纵坐标差值的第六平方值，计算所述第五平方值和所述第六平方值的和值，以及对所述和值进行平方根计算，得到根值；计算所述航向角差值的正弦值，获取所述正弦值的绝对值；计算所述根值与所述绝对值的和值作为所述综合误差，以及将所述综合误差作为所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差。
需要说明的是，本发明实施例提供的一种路径规划装置所涉及各功能模块的其他相应描述，可以参考实施例一至三所示方法的对应描述，在此不再赘述。
基于上述如图1所示方法，相应的，本发明实施例还提供了一种存储介质，如图5所示，介质存储器720上存储有计算机程序，该计算机程序位于程序代码空间730，该程序731被介质处理器710执行时实现实施例一、实施例二、实施例三中任意一种所示的方法步骤。
实施例五
本发明实施例还提供了一种电子设备的实体结构图，如图6所示，该电子设备包括：设备处理器41、设备存储器42、及存储在设备存储器42上并可在设备处理器上运行的计算机程序，其中设备存储器42和设备处理器41均设置在总线43上所述设备处理器41执行所述程序时实现实施例一至实施例三中任意一种方法所示的步骤。
本发明周期性的判断本车与预设参考线之间的综合误差是否超限，如果综合误差超限，则在预设参考线上确定目标采样点作为终点，开始进行起步路径规划，为无人驾驶车辆生成规划行驶轨迹，控制无人驾驶车辆按照规划行驶轨迹行进，并在行进的过程中实时计算无人驾驶车辆与预设参考线之间的综合误差，一旦实时综合误差小于预设误差阈值，则确定无人驾驶车辆已经回归至预设参考线，控制其按照预设参考线循迹行驶，实现综合误差超限的自动起步规划，使无人驾驶车辆自行回到预设参考线中循迹行驶，无需人工干预，智能性较好。
实施例六
图7为本发明实施例提供的一种芯片的结构示意图，如图7所示，芯片500包括一个或两个以上(包括两个) 芯片处理器510和通信接口530。 所述通信接口530和所述至少一个芯片处理器510耦合，所述至少一个芯片处理器510用于运行计算机程序或指令，以实现如实施例一、实施例二、实施例三任意一种方法所述的路径规划方法。
优选地，芯片存储器540存储了如下的元素：可执行模块或者数据结构，或者他们的子集，或者他们的扩展集。
本发明实施例中，芯片存储器540可以包括只读存储器和随机存取存储器，并向芯片处理器510提供指令和数据。芯片存储器540的一部分还可以包括非易失性随机存取存储器(non-volatile random access memory，NVRAM)。
本发明实施例中，芯片存储器540、通信接口530以及芯片处理器510通过总线***520 耦合在一起。其中，总线***520除包括数据总线之外，还可以包括电源总线、控制总线和 状态信号总线等。为了便于描述，在图7中将各种总线都标为总线***520。
上述本发明实施例描述的方法可以应用于芯片处理器510中，或者由芯片处理器510实现。芯片处理器510可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过芯片处理器510中的硬件的集成逻辑电路或者软件形式的指令完成。上述的芯片处理器510可以是通用处理器(例如，微处理器或常规处理器)、数字信号处理器(digital signal processing，DSP)、专用集成电路(application specificintegrated circuit，ASIC)、现成可编程门阵列(field-programmable gate array，FPGA)或者其他可编程逻辑器件、分立门、晶体管逻辑器件或分立硬件组件，芯片处理器510可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。
实施例七
图8为本发明实施例提供的一种终端的结构示意图，如图8所示，终端600包括上述路径规划装置100。
上述终端600可以通过路径规划装置100执行上述任意一种实施例所描述的方法。可以理解，终端600对路径规划装置100进行控制的实现方式，可以根据实际应用场景设定，本发明实施例不作具体限定。
所述终端600包括但不限于：车辆、车载终端、车载控制器、车载模块、车载模组、车载部件、车载芯片、车载单元、车载雷达或车载摄像头等其他传感器，车辆可通过该车载终端、车载控制器、车载模块、车载模组、车载部件、车载芯片、车载单元、车载雷达或摄像头，实施本发明提供的方法。
本发明实施例中的终端作为一种执行非电变量的控制或调整***，对路径进行规划，从而实现综合误差超限的自动起步规划，使无人驾驶车辆自行回到预设参考线中循迹行驶，无需人工干预，智能性较好。
显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包括在本发明的保护范围之内。
Claims (16)
1.一种路径规划方法，其特征在于，包括：
当检测到无人驾驶车辆与预设参考线之间的综合误差超限时，在所述预设参考线上确定目标采样点，所述预设参考线是所述无人驾驶车辆中预设的车辆行驶轨迹，其中，在检测所述无人驾驶车辆与所述预设参考线之间的综合误差是否超限时，在所述预设参考线包括的多个路径序列点中选取距离所述无人驾驶车辆最近的一路径序列点作为基准序列点，计算所述无人驾驶车辆与所述基准序列点之间的偏移量作为综合误差以及判断所述综合误差是否超限，所述目标采样点是在所述预设参考线上采样得到的与所述无人驾驶车辆之间的距离满足预设采样距离的点；
以所述无人驾驶车辆的当前位置为起点、所述目标采样点为终点，采用采样轨迹规划器生成所述无人驾驶车辆的规划行驶轨迹，控制所述无人驾驶车辆按照所述规划行驶轨迹行进；
在所述无人驾驶车辆按照所述规划行驶轨迹行进的过程中，实时计算所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差，以及在计算得到的实时综合误差小于预设误差阈值时，控制所述无人驾驶车辆按照所述预设参考线循迹行驶。
2.根据权利要求1所述的方法，其特征在于，所述当检测到无人驾驶车辆与预设参考线之间的综合误差超限时，在所述预设参考线上确定目标采样点之前，所述方法还包括：
每隔预设检测周期，获取所述无人驾驶车辆的当前位置的位置姿态信息，以及确定所述无人驾驶车辆中预设的所述预设参考线；
读取所述预设参考线包括的多个路径序列点，分别计算所述多个路径序列点中每个路径序列点与所述无人驾驶车辆的当前位置的距离，在所述多个路径序列点中确定所述基准序列点，所述基准序列点与所述无人驾驶车辆的当前位置之间的距离小于所述多个路径序列点中除所述基准序列点外的其他路径序列点；
获取所述基准序列点的序列点姿态坐标，计算所述序列点姿态坐标与所述位置姿态信息之间的综合误差；
当所述综合误差大于所述预设误差阈值时，确定检测到所述无人驾驶车辆与预设参考线之间的综合误差超限。
3.根据权利要求2所述的方法，其特征在于，所述计算所述序列点姿态坐标与所述位置姿态信息之间的综合误差，包括：
读取所述序列点姿态坐标的横坐标、纵坐标，读取所述位置姿态信息的横坐标、纵坐标；
计算所述序列点姿态坐标的横坐标与所述位置姿态信息的横坐标的横坐标差值，计算所述序列点姿态坐标的纵坐标与所述位置姿态信息的纵坐标的纵坐标差值；
读取所述序列点姿态坐标的序列点航向角，读取所述位置姿态信息的车辆航向角，计算所述序列点航向角与所述车辆航向角之间的航向角差值；
计算所述横坐标差值的第一平方值、所述纵坐标差值的第二平方值，计算所述第一平方值和所述第二平方值的和值，以及对所述和值进行平方根计算，得到根值；
计算所述航向角差值的正弦值，获取所述正弦值的绝对值；
计算所述根值与所述绝对值的和值作为所述序列点姿态坐标与所述位置姿态信息之间的综合误差。
4.根据权利要求2所述的方法，其特征在于，所述获取所述基准序列点的序列点姿态坐标，计算所述序列点姿态坐标与所述位置姿态信息之间的综合误差之后，所述方法还包括：
当所述综合误差小于等于所述预设误差阈值时，控制所述无人驾驶车辆按照所述预设参考线循迹行驶，以及继续每隔所述预设检测周期，重新获取所述无人驾驶车辆的当前位置的位置姿态信息，以及确定所述预设参考线，重新在所述预设参考线包括的多个路径序列点中确定新的基准序列点，获取所述新的基准序列点的序列点姿态坐标，计算所述新的基准序列点的序列点姿态坐标与重新获取的位置姿态信息之间的综合误差，判断所述综合误差是否大于所述预设误差阈值。
5.根据权利要求1所述的方法，其特征在于，所述在所述预设参考线上确定目标采样点：
获取所述预设采样距离；
以所述无人驾驶车辆的当前位置为基准，在所述预设参考线上采样一与所述当前位置之间的距离满足所述预设采样距离的点作为所述目标采样点。
6.根据权利要求1所述的方法，其特征在于，所述采用采样轨迹规划器生成所述无人驾驶车辆的规划行驶轨迹，包括：
根据所述目标采样点对应的横坐标、纵坐标、航向角和曲率值，利用牛顿求根法求解所述采样轨迹规划器中关于曲线弧长的曲率函数对应的曲率系数；
基于所述曲率系数，确定所述无人驾驶车辆的当前位置与所述目标采样点之间满足车辆运动学约束的行驶曲线，以及将所述行驶曲线作为所述无人驾驶车辆的规划行驶轨迹。
7.根据权利要求6所述的方法，其特征在于，所述根据所述目标采样点对应的横坐标、纵坐标、航向角和曲率值，利用牛顿求根法求解所述采样轨迹规划器中关于曲线弧长的曲率函数对应的曲率系数，包括：
根据所述无人驾驶车辆在所述当前位置的起点曲率值、在所述目标采样点处的终点曲率值，以及所述关于曲线弧长的曲率函数，确定关于起点曲率值、第一曲率、第二曲率、终点曲率值和曲线弧长的曲率系数函数，其中，所述第一曲率和所述第二曲率分别为1/3曲线弧长和2/3曲线弧长处的曲率；
基于所述曲率系数函数和所述采样轨迹规划器，分别确定关于起点曲率值、第一曲率、第二曲率、曲线弧长和终点曲率值的横坐标函数、纵坐标函数和航向角函数；
获取第一曲率、第二曲率和曲线弧长分别对应的第一初始值、第二初始值和第三初始值，并将所述第一初始值、所述第二初始值、所述第三初始值、所述起点曲率值和所述终点曲率值分别代入至所述横坐标函数、所述纵坐标函数、所述航向角函数和所述曲率系数函数，得到所述无人驾驶车辆的当前位置与所述目标采样点之间的目标***点对应的横坐标、纵坐标、航向角和曲率值；
将所述目标采样点对应的横坐标、纵坐标、航向角和曲率值与所述目标***点对应的横坐标、纵坐标、航向角和曲率值对应相减，得到所述目标采样点与所述目标***点之间的横坐标差值、纵坐标差值、航向角差值和曲率差值，并计算所述横坐标差值、所述纵坐标差值、所述航向角差值和所述曲率差值的差值总和；
基于所述差值总和不断对所述第一初始值、所述第二初始值和所述第三初始值进行更新迭代，直至目标***点对应的横坐标、纵坐标、航向角和曲率值分别与所述目标采样点对应的横坐标、纵坐标、航向角和曲率值相同，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长；
根据所述最终迭代更新后的第一曲率值、第二曲率值、曲线弧长和所述曲率系数函数，确定所述曲率系数。
8.根据权利要求7所述的方法，其特征在于，所述基于所述差值总和不断对所述第一初始值、所述第二初始值和所述第三初始值进行更新迭代，直至目标***点对应的横坐标、纵坐标、航向角和曲率值分别与所述目标采样点对应的横坐标、纵坐标、航向角和曲率值相同，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长，包括：
基于所述横坐标函数、所述纵坐标函数和所述航向角函数，计算所述第一曲率、所述第二曲率和所述曲线弧长分别对应的雅克比矩阵；
计算所述第一曲率、所述第二曲率和所述曲线弧长分别对应的雅克比矩阵的逆矩阵，并将所述第一曲率、所述第二曲率和所述曲线弧长分别对应的逆矩阵与所述差值总和相乘，得到本轮迭代的第一曲率变化量、第二曲率变化量和曲线弧长变化量；
基于所述第一曲率变化量、所述第二曲率变化量和所述曲线弧长变化量，分别对所述第一初始值、所述第二初始值和所述第三初始值进行更新；
重复对所述第一初始值、所述第二初始值和所述第三初始值进行迭代更新的过程，直至所述差值总和为0，输出最终迭代更新后的第一曲率值、第二曲率值和曲线弧长。
9.根据权利要求1所述的方法，其特征在于，所述实时计算所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差，包括：
实时对所述预设参考线进行采样，得到多个采样点，分别计算所述多个采样点中每个采样点与所述实时位置之间的欧式距离；
在所述多个采样点中提取欧式距离小于预设距离阈值的采样点作为评估点，实时获取所述评估点的评估点姿态坐标、所述实时位置的实时位置姿态信息；
计算所述评估点姿态坐标与所述实时位置姿态信息之间的综合误差作为所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差。
10.根据权利要求9所述的方法，其特征在于，所述分别计算所述多个采样点中每个采样点与所述实时位置之间的欧式距离，包括：
对于所述多个采样点中每个采样点，获取所述采样点的横坐标、纵坐标；
读取所述实时位置的横坐标、纵坐标；
计算所述采样点的横坐标与所述实时位置的横坐标的横坐标差值，计算所述横坐标差值的第三平方值；
计算所述采样点的纵坐标与所述实时位置的纵坐标的纵坐标差值，计算所述纵坐标差值的第四平方值；
计算所述第三平方值和所述第四平方值的和值，以及对所述和值进行平方根计算，将得到的根值作为当前计算的采样点与所述实时位置之间的欧式距离。
11.根据权利要求9所述的方法，其特征在于，所述计算所述评估点姿态坐标与所述实时位置姿态信息之间的综合误差作为所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差，包括：
读取所述评估点姿态坐标的横坐标、纵坐标，读取所述实时位置姿态信息的横坐标、纵坐标；
计算所述评估点姿态坐标的横坐标与所述实时位置姿态信息的横坐标的横坐标差值，计算所述评估点姿态坐标的纵坐标与所述实时位置姿态信息的纵坐标的纵坐标差值；
读取所述评估点姿态坐标的评估点航向角，读取所述实时位置姿态信息的实时航向角，计算所述评估点航向角与所述实时航向角之间的航向角差值；
计算所述横坐标差值的第五平方值、所述纵坐标差值的第六平方值，计算所述第五平方值和所述第六平方值的和值，以及对所述和值进行平方根计算，得到根值；
计算所述航向角差值的正弦值，获取所述正弦值的绝对值；
计算所述根值与所述绝对值的和值作为所述综合误差，以及将所述综合误差作为所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差。
12.一种路径规划装置，其特征在于，包括：
确定单元，用于当检测到无人驾驶车辆与预设参考线之间的综合误差超限时，在所述预设参考线上确定目标采样点，所述预设参考线是所述无人驾驶车辆中预设的车辆行驶轨迹，其中，在检测所述无人驾驶车辆与所述预设参考线之间的综合误差是否超限时，在所述预设参考线包括的多个路径序列点中选取距离所述无人驾驶车辆最近的一路径序列点作为基准序列点，计算所述无人驾驶车辆与所述基准序列点之间的偏移量作为综合误差以及判断所述综合误差是否超限，所述目标采样点是在所述预设参考线上采样得到的与所述无人驾驶车辆之间的距离满足预设采样距离的点；
生成单元，用于以所述无人驾驶车辆的当前位置为起点、所述目标采样点为终点，采用采样轨迹规划器生成所述无人驾驶车辆的规划行驶轨迹，控制所述无人驾驶车辆按照所述规划行驶轨迹行进；
计算单元，用于在所述无人驾驶车辆按照所述规划行驶轨迹行进的过程中，实时计算所述无人驾驶车辆的实时位置与所述预设参考线之间的实时综合误差，以及在计算得到的实时综合误差小于预设误差阈值时，控制所述无人驾驶车辆按照所述预设参考线循迹行驶。
13.一种芯片，其特征在于，所述芯片包括至少一个芯片处理器和通信接口，所述通信接口和所述至少一个芯片处理器耦合，所述至少一个芯片处理器用于运行计算机程序或指令，以实现如权利要求1-11中任一项所述的路径规划方法。
14.一种终端，其特征在于，所述终端包括如权利要求12所述的路径规划装置。
15.一种电子设备，包括设备存储器、设备处理器及存储在设备存储器上并可在设备处理器上运行的计算机程序，其特征在于，所述计算机程序被设备处理器执行时实现权利要求1至11中任一项所述的方法的步骤。
16.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被介质处理器执行时实现权利要求1至11中任一项所述的方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211068224.7A CN115127576B (zh) | 2022-09-02 | 2022-09-02 | 路径规划方法、装置、芯片、终端、电子设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211068224.7A CN115127576B (zh) | 2022-09-02 | 2022-09-02 | 路径规划方法、装置、芯片、终端、电子设备及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115127576A CN115127576A (zh) | 2022-09-30 |
| CN115127576B true CN115127576B (zh) | 2022-12-13 |
Family
ID=83387666
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211068224.7A Active CN115127576B (zh) | 2022-09-02 | 2022-09-02 | 路径规划方法、装置、芯片、终端、电子设备及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115127576B (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20240174239A1 (en) * | 2022-11-30 | 2024-05-30 | Zoox, Inc. | Route-relative trajectory generation and optimization computations incorporating vehicle sideslip |
| CN116907412B (zh) * | 2023-09-12 | 2023-11-17 | 农业农村部南京农业机械化研究所 | 农机的行距偏差检测方法、装置及*** |
| CN117572476B (zh) * | 2024-01-15 | 2024-04-09 | 智道网联科技(北京)有限公司 | 一种基于行驶轨迹的自动驾驶车辆定位调整方法及装置 |
| CN117818665B (zh) * | 2024-03-05 | 2024-05-31 | 智道网联科技(北京)有限公司 | 自动驾驶车辆控制方法、装置及电子设备、存储介质 |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1761980A (zh) * | 2003-03-17 | 2006-04-19 | 松下电器产业株式会社 | 探测车***中用于传送行走轨迹的方法和装置 |
| CN107037818A (zh) * | 2017-06-22 | 2017-08-11 | 奇瑞汽车股份有限公司 | 一种无人驾驶汽车的路径跟踪方法 |
| CN111486848A (zh) * | 2020-05-25 | 2020-08-04 | 上海杰销自动化科技有限公司 | Agv视觉导航方法、***、计算机设备以及存储介质 |
| CN111610785A (zh) * | 2020-05-26 | 2020-09-01 | 三一专用汽车有限责任公司 | 无人驾驶车辆的运动规划方法和运动规划装置 |
| CN111750887A (zh) * | 2020-06-11 | 2020-10-09 | 上海交通大学 | 降低事故严重程度的无人驾驶车辆轨迹规划方法及*** |
| CN112146667A (zh) * | 2020-09-29 | 2020-12-29 | 广州小鹏自动驾驶科技有限公司 | 一种车辆过渡轨迹的生成方法和装置 |
| CN112859863A (zh) * | 2021-01-15 | 2021-05-28 | 北京科技大学 | 一种基于预测的路径跟踪控制关键参考点选取方法及*** |
| CN113031583A (zh) * | 2020-03-13 | 2021-06-25 | 青岛慧拓智能机器有限公司 | 一种结构化道路避障方法 |
| CN113985887A (zh) * | 2021-11-04 | 2022-01-28 | 北京京东乾石科技有限公司 | 差速移动机器人运动轨迹生成方法和运动控制装置 |
| CN114115299A (zh) * | 2022-01-25 | 2022-03-01 | 上海仙工智能科技有限公司 | 一种移动机器人平滑回归给定轨迹的路径规划方法及装置 |
| WO2022148441A1 (zh) * | 2021-01-11 | 2022-07-14 | 长沙智能驾驶研究院有限公司 | 车辆泊车规划方法、装置、设备及计算机存储介质 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6643370B2 (ja) * | 2018-01-23 | 2020-02-12 | 本田技研工業株式会社 | 走行基準ライン決定装置及び自動運転装置 |
| CN110749333B (zh) * | 2019-11-07 | 2022-02-22 | 中南大学 | 基于多目标优化的无人驾驶车辆运动规划方法 |
-
2022
- 2022-09-02 CN CN202211068224.7A patent/CN115127576B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1761980A (zh) * | 2003-03-17 | 2006-04-19 | 松下电器产业株式会社 | 探测车***中用于传送行走轨迹的方法和装置 |
| CN107037818A (zh) * | 2017-06-22 | 2017-08-11 | 奇瑞汽车股份有限公司 | 一种无人驾驶汽车的路径跟踪方法 |
| CN113031583A (zh) * | 2020-03-13 | 2021-06-25 | 青岛慧拓智能机器有限公司 | 一种结构化道路避障方法 |
| CN111486848A (zh) * | 2020-05-25 | 2020-08-04 | 上海杰销自动化科技有限公司 | Agv视觉导航方法、***、计算机设备以及存储介质 |
| CN111610785A (zh) * | 2020-05-26 | 2020-09-01 | 三一专用汽车有限责任公司 | 无人驾驶车辆的运动规划方法和运动规划装置 |
| CN111750887A (zh) * | 2020-06-11 | 2020-10-09 | 上海交通大学 | 降低事故严重程度的无人驾驶车辆轨迹规划方法及*** |
| CN112146667A (zh) * | 2020-09-29 | 2020-12-29 | 广州小鹏自动驾驶科技有限公司 | 一种车辆过渡轨迹的生成方法和装置 |
| WO2022148441A1 (zh) * | 2021-01-11 | 2022-07-14 | 长沙智能驾驶研究院有限公司 | 车辆泊车规划方法、装置、设备及计算机存储介质 |
| CN112859863A (zh) * | 2021-01-15 | 2021-05-28 | 北京科技大学 | 一种基于预测的路径跟踪控制关键参考点选取方法及*** |
| CN113985887A (zh) * | 2021-11-04 | 2022-01-28 | 北京京东乾石科技有限公司 | 差速移动机器人运动轨迹生成方法和运动控制装置 |
| CN114115299A (zh) * | 2022-01-25 | 2022-03-01 | 上海仙工智能科技有限公司 | 一种移动机器人平滑回归给定轨迹的路径规划方法及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115127576A (zh) | 2022-09-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115127576B (zh) | 路径规划方法、装置、芯片、终端、电子设备及存储介质 | |
| CN110262508B (zh) | 应用于封闭场地无人驾驶货运车辆上的自动引导***及方法 | |
| CN105292116B (zh) | 自动驾驶车辆的车道变换路径规划算法 | |
| EP3517893B1 (en) | Path and speed optimization fallback mechanism for autonomous vehicles | |
| EP3861291B1 (en) | Spline curve and spiral curve based reference line smoothing method | |
| US10948919B2 (en) | Dynamic programming and gradient descent based decision and planning for autonomous driving vehicles | |
| US10429849B2 (en) | Non-linear reference line optimization method using piecewise quintic polynomial spiral paths for operating autonomous driving vehicles | |
| CN105270410B (zh) | 用于自主驾驶车辆的路径规划的精确曲率估计算法 | |
| US10800427B2 (en) | Systems and methods for a vehicle controller robust to time delays | |
| CN113847920A (zh) | 自主车辆的两级路径规划 | |
| CN110345957A (zh) | 车辆路径识别 | |
| CN110341688B (zh) | 一种前轮转角补偿方法、装置、车载设备及存储介质 | |
| EP3992732A1 (en) | Method and apparatus for predicting motion trajectory | |
| CN112394725B (zh) | 用于自动驾驶的基于预测和反应视场的计划 | |
| US11106212B2 (en) | Path planning for complex scenes with self-adjusting path length for autonomous driving vehicles | |
| CN112477849B (zh) | 自动驾驶卡车的泊车控制方法、装置及自动驾驶卡车 | |
| CN111123947A (zh) | 机器人行进的控制方法、装置、电子设备、介质和机器人 | |
| EP3925844B1 (en) | Relative speed based speed planning for buffer area | |
| CN111813112A (zh) | 一种车辆轨迹点确定方法、装置、车辆及存储介质 | |
| CN112660147B (zh) | 控制车辆调头的方法、装置、设备、存储介质和程序产品 | |
| CN113085868A (zh) | 用于运行自动化车辆的方法、设备和存储介质 | |
| CN115123217B (zh) | 矿区障碍车辆行使轨迹生成方法、装置及计算机设备 | |
| CN115214775B (zh) | 一种方向盘中位的调整方法、装置、设备及介质 | |
| US20230391356A1 (en) | Dynamic scenario parameters for an autonomous driving vehicle | |
| US11724717B2 (en) | Implementation of dynamic cost function of self-driving vehicles |
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 |