[发明专利]一种速度自适应的无人车轨迹跟踪PID控制方法

摘要

本发明公开了一种速度自适应的无人车轨迹跟踪PID控制方法，采用增量式PID算法实现无人车精确地、及时地跟踪预定轨迹，引入速度自适应调整来保证其准时到达预定地点，设计平稳性调整以改善无人车行驶平稳性；该发明可保证无人车能够精确地跟踪预定轨迹，能够按时达到预定位置，并能显著改善无人车行驶的平稳性，具有准确性高、实时性强、平稳性好，安全性优等优点，可广泛应用于工业生产和军事等需要无人车准确地、准时地、安全平稳地跟踪预定轨迹的场合。

主权项

1.一种速度自适应的无人车轨迹跟踪PID控制方法，其特征在于，包括如下步骤：a)，以WGS84坐标作为参考，获取无人车当前位置Pc，并根据预定计划查询当前目标位置Pt(n)，根据如下公式利用球面坐标计算当前位置Pc与当前目标位置Pt(n)之间的距离Dt：式中，当前位置坐标为目标位置坐标为和为经度坐标，λ_A和λ_B为纬度坐标，R为地球半径，R＝6378140m，n为当前目标位置在预定计划目标位置列表中的序号，且n＝1，2，3……，N，N为预定计划中的目标位置总数；b)，根据如下公式利用球面坐标计算当前位置Pc与当前目标位置Pt(n)之间的目标方向夹角Ht：式中，Ht的取值范围为0°～360°，并以正北为0°参考方向；c)，获取当前时间Tc；获取行驶至当前目标位置Pt(n)的目标时间Tt；d)，根据由步骤a)获得的Dt和由步骤c)获得的Tt、Tc，在准时到达目标位置Pt(n)的前提下，根据如下公式计算无人车由当前位置Pc行驶至当前目标位置Pt(n)所需的目标速度Vt：e)，获取无人车当前速度Vc和当前方向Hc；基于当前方向Hc和由步骤b)获得的目标方向Ht，完成无人车轨迹跟踪中的方向控制；基于当前速度Vc和由步骤d)获得的目标速度Vt，完成无人车轨迹跟踪中的速度控制；f)，实时计算无人车当前位置Pc和目标位置Pt(n)之间的距离Dc，并判定预定计划进度；若Dc大于所设定的到达阈值Ds，则判定无人车还没有到达目标位置；若Dc小于或者等于所设定的到达阈值Ds，则判定无人车已经到达目标位置，然后，按照预定计划目标位置列表的顺序，将当前目标位置Pt(n)更新为下一目标位置Pt(n+1)；g)，利用序号n和目标位置总数N判断预定计划是否执行完毕；若n＜N，预定计划未完成，则重新执行步骤a)至步骤f)；若n＝N，预定计划执行完毕，此时结束无人车轨迹跟踪，无人车到达目标位置。