【技术实现步骤摘要】
本专利技术属于井下车辆跟踪，具体为一种井下铰接驾驶车辆路径跟踪方法。
技术介绍
1、目前，井下铰接驾驶车辆路径跟踪方法主要采用基于传感器数据融合和控制策略的技术方案，首先，利用激光雷达、摄像头、惯性导航等多种传感器获取车辆当前位置、姿态、环境信息等数据，通过传感器数据融合算法实时处理、融合和校正数据，以提高位置和姿态的精度和准确性，接着，采用基于模型预测控制mpc或经典的pid控制等路径跟踪控制策略，根据车辆当前状态和预先规划的路径，计算出最优的转向角度和车速指令，使车辆能够沿着预定路径安全、稳定地行驶，此外，针对井下环境的特殊性，考虑到地质情况、障碍物等因素，会采用特定的路径规划算法，如遗传算法、a*算法等，以确保车辆能够避开障碍物、适应复杂地形，整个系统还需要具备实时性强、抗干扰能力强的特点，以应对井下环境变化快、复杂多变的特点，从而实现井下铰接驾驶车辆路径跟踪的高效、稳定和安全运行；
2、以上轨迹跟踪方法在应用于铰接式车辆时存在一些缺点，首先，铰接式车辆由于车身分为前后两部分，在转弯时存在转弯半径变化的特点，这会导致传统的路径跟踪控制方法难以精确预测车辆的行驶轨迹，可能导致路径偏差或不稳定的行驶，其次，井下环境的复杂性和不确定性对传感器数据的采集和处理提出了更高的要求，而传感器数据的精度和稳定性直接影响路径跟踪的效果，因此需要更加可靠和精确的传感器数据融合算法，另外，铰接式车辆在特定情况下可能会遇到更为复杂的地形和障碍物，传统的路径规划算法可能无法有效应对，对此，我们提出了一种井下铰接驾驶车辆路径跟踪方法。p>
技术实现思路
1、针对现有技术的不足，本专利技术提供了一种井下铰接驾驶车辆路径跟踪方法，以解决井下巷道铰接车辆的路径跟踪问题。
2、为实现上述目的，本专利技术提供如下技术方案：一种井下铰接驾驶车辆路径跟踪方法，跟踪步骤为：
3、s1、前进状态下，获取铰接式车辆的前部位姿作为整体的位姿进行轨迹跟踪，通过车载传感器获取车辆前部的位置和姿态信息，并将其作为整体车辆的位置和姿态；
4、s2、根据前部位姿获得前瞻点，通过将车辆当前位置沿着当前航向方向前推一定距离，来确定前瞻点的位置，这个前推距离根据车辆的速度和控制响应时间来确定，选择距离，让前瞻点保持在车辆前方，并且在车辆到达前瞻点时进行及时的调整；
5、s3、后退状态下，获取铰接式车辆的后部位姿作为整体的位姿进行轨迹跟踪，通过车载传感器获取车辆前部的位置和姿态信息，将其作为整体车辆的位置和姿态；
6、s4、将车辆当前位置沿着当前航向方向即后退方向后推一定距离，来确定前瞻点的位置，这个后退距离根据车辆的速度和控制响应时间来确定，选择距离，使得前瞻点能够保持在车辆后方，并且在车辆到达前瞻点时进行及时的调整。
7、优选地，根据车辆是否处于前进还是后退来确认主位姿，若前进为主位姿，则后退为副位姿，若后退为主位姿，则前进为副位姿。
8、优选地，s2步骤中以前进为主位姿，后退为副位姿，通过主位姿与锚点距离lanchor获得锚点panchor，车辆的坐标位置计算公式为：
9、xanchor＝xfront+lanchor*cos(θfront)
10、yanchor＝yfront+lanchor*sin(θfront)
11、其中xanchor表示为锚点的x坐标，xfront表示为主位姿的x坐标，lanchor表示为锚点距离，θfront表示为铰接车前部的航向角，yanchor表示为锚点的y坐标，yfront表示为主位姿的y坐标；
12、根据锚点与前瞻距离lahead在规划路径上获得前瞻点pahead。
13、优选地，获取前瞻点pahead步骤为以前锚点panchor为圆心，以前瞻距离lahead为半径绘制一个圆与规划路径相交于点pintersection，判断向量pfrontpanchor与向量panchorpintersection交角的绝对值是否小于90°，判断步骤为：
14、假设点pfront坐标为(xf,yf)，panchor坐标为(xa,ya)，pintersectio坐标为(xi,yi)，则向量pfrontpanchor与向量panchorpintersection的夹角可由下式得出：
15、
16、若是则判断该交点为正确的前瞻点pahead，由此获得前瞻角度θhead，将获得的锚点距离lanchor、前瞻距离lahead和前瞻角度θhead代入到横向控制器中获得铰接处的转角为δf,在弯道情况下，仅以δf作为最终的转角输出即δ＝δf。
17、优选地，在非弯道情况下，从车辆运行开始就计算出后车点prear与轨迹上最近点pnear的距离lnear,随着车辆的运动，将每次得到的距离累加起来并得到平均值lagerage,累加一定次数n之后，此时转角输出变为为δ＝k*δf；
18、其中k＝(1-(lnear/lagerage))为惩罚系数，lnear为后车当前最近距离，lagerage为后车最近距离的累加均值。
19、优选地，s2步骤中根据主位姿pfront获得离目标点ptarget的距离d，计算出整条轨迹的长度l，先确定速度最大值vmax，确定加速度a，当前速度vcurrent＝a*t，直至到最大速度vmax，其中t为从车辆启动到当前的时间间隔。
20、优选地，当前瞻点距离弯道点为lcircle时，开始减速，直到减速为vcircle，保证慢速通过弯道，计算出完成度u＝d/l；当u＞95％时，开始减速，保证速度公式为：
21、vcurrent＝(1-d/l)vmax。
22、优选地，通过获取铰接式车辆的前部和后部位姿分别作为整体的位姿用于轨迹跟踪。
23、优选地，s1步骤中获取姿态信息的传感器为姿态传感器，用以记录车辆的位置与方向信息，姿态传感器包括三轴陀螺仪、三轴加速度计与三轴电子罗盘构成。
24、优选地，姿态传感器通过内嵌的低功耗arm处理器，结合基于四元数的三维算法与数据融合技术，实时输出以四元数与欧拉角表示的零漂移三维姿态方位数据。
25、与现有技术相比，本专利技术的有益效果如下：
26、本申请通过获取车辆前后两段的位姿信息可以更好地进行轨迹跟踪，通过分别获取车辆前后两段的位姿，可以更准确地捕捉到车辆整体的运动状态和姿态变化，从而提高路径跟踪的精度和稳定性，同时，可以利用前后两段位姿之间的关系，设计更为复杂和灵活的控制策略，以适应铰接式车辆在转弯和曲线行驶时的特殊需求，此外，通过对车辆前后两段位姿的实时监测和分析，可以及时发现和纠正路径偏差，确保车辆能够按照预定轨迹安全、稳定地行驶，因此，通过获取车辆前后两段的位姿信息进行轨迹跟踪，能够更好地充分利用铰接式车辆的结构特点，提高路径跟踪的效果和性能。
本文档来自技高网...【技术保护点】
1.一种井下铰接驾驶车辆路径跟踪方法，其特征在于，跟踪步骤为：
2.根据权利要求1所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：根据车辆是否处于前进还是后退来确认主位姿，若前进为主位姿，则后退为副位姿，若后退为主位姿，则前进为副位姿。
3.根据权利要求1所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：S2步骤中以前进为主位姿，后退为副位姿，通过主位姿与锚点距离Lanchor获得锚点Panchor，车辆的坐标位置计算公式为：
4.根据权利要求3所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：获取前瞻点Pahead步骤为以前锚点Panchor为圆心，以前瞻距离Lahead为半径绘制一个圆与规划路径相交于点Pintersection，判断向量PfrontPanchor与向量PanchorPintersection交角的绝对值是否小于90°，判断步骤为：
5.根据权利要求4所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：在非弯道情况下，从车辆运行开始就计算出后车点Prear与轨迹上最近点Pnear的距离Lnear,随着车辆的运动，将每次得
6.根据权利要求1所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：S2步骤中根据主位姿Pfront获得离目标点Ptarget的距离D，计算出整条轨迹的长度L，先确定速度最大值Vmax，确定加速度a，当前速度Vcurrent＝a*t，直至到最大速度Vmax，其中t为从车辆启动到当前的时间间隔。
7.根据权利要求6所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：当前瞻点距离弯道点为Lcircle时，开始减速，直到减速为Vcircle，保证慢速通过弯道，计算出完成度u＝D/L；当u＞95％时，开始减速，保证速度公式为：
8.根据权利要求1所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：通过获取铰接式车辆的前部和后部位姿分别作为整体的位姿用于轨迹跟踪。
9.根据权利要求1所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：S1步骤中获取姿态信息的传感器为姿态传感器，用以记录车辆的位置与方向信息，姿态传感器包括三轴陀螺仪、三轴加速度计与三轴电子罗盘构成。
10.根据权利要求9所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：姿态传感器通过内嵌的低功耗ARM处理器，结合基于四元数的三维算法与数据融合技术，实时输出以四元数与欧拉角表示的零漂移三维姿态方位数据。
...【技术特征摘要】
1.一种井下铰接驾驶车辆路径跟踪方法，其特征在于，跟踪步骤为：
2.根据权利要求1所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：根据车辆是否处于前进还是后退来确认主位姿，若前进为主位姿，则后退为副位姿，若后退为主位姿，则前进为副位姿。
3.根据权利要求1所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：s2步骤中以前进为主位姿，后退为副位姿，通过主位姿与锚点距离lanchor获得锚点panchor，车辆的坐标位置计算公式为：
4.根据权利要求3所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：获取前瞻点pahead步骤为以前锚点panchor为圆心，以前瞻距离lahead为半径绘制一个圆与规划路径相交于点pintersection，判断向量pfrontpanchor与向量panchorpintersection交角的绝对值是否小于90°，判断步骤为：
5.根据权利要求4所述的井下铰接驾驶车辆路径跟踪方法，其特征在于：在非弯道情况下，从车辆运行开始就计算出后车点prear与轨迹上最近点pnear的距离lnear,随着车辆的运动，将每次得到的距离累加起来并得到平均值lagerage,累加一定次数n之后，此时转角输出变为为δ＝k*δf；
...
【专利技术属性】
技术研发人员：吴建鹏，李达，高正，
申请(专利权)人：青岛中鸿重型机械有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。