一种速度自适应的无人车轨迹跟踪PID控制方法技术

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

A speed adaptive PID control method for trajectory tracking of unmanned vehicles

The invention discloses a speed adaptive PID control method for the track tracking of unmanned vehicle. The incremental PID algorithm is used to track the predetermined trajectory accurately and promptly, and the speed adaptive adjustment is introduced to ensure that the vehicle arrives at the predetermined location on time, and the design is adjusted to improve the driving stability of the unmanned vehicle. The invention can be made. It is guaranteed that the unmanned vehicle can accurately track the predetermined trajectory, can achieve the predetermined position on time, and can significantly improve the smoothness of the unmanned vehicle. It has the advantages of high accuracy, high real-time, good stability and good security. It can be widely used in industrial production and military, such as the unmanned vehicle, accurate, on time and safety. An occasion where the predetermined trajectory is tracked steadily.

【技术实现步骤摘要】
一种速度自适应的无人车轨迹跟踪PID控制方法
本专利技术属于导航定位领域，内容具体涉及了一种速度自适应的无人车轨迹跟踪PID控制方法。
技术介绍
随着传感器技术和导航技术的不断进步，无人系统正以井喷态势迅速发展，其中的典型代表当属无人机、无人车和无人艇。相比于无人机和无人艇，无人车与工业生产和日常生活结合更加紧密，更具进一步发展的生机与活力，其在军事领域也起到了重要作用。例如，工业生产中，危险环境下设备维修和无人值守；军事领域中，排雷和排爆，月球车等。因此，无人车在无人系统中的重要性越加显著。无人车轨迹跟踪性能优秀与否，取决于三个评判标准，一是能否准确地按照预定轨迹行驶，二是能否准时到达预定位置，三是能否保证行驶的安全性和平稳性。准确地按照预定轨迹行驶说明无人车控制策略的轨迹跟踪准确性高、实时性好，跟踪预定轨迹的实时调整能力强；准时到达预定位置说明无人车控制策略具备较好的速度调整灵活性；保证行驶的安全性、稳定性和平稳性说明无人车控制策略调整无人车行驶姿态的能力强，准确性和实时性较高。在无人车设计和应用过程中，提高跟踪预定轨迹的准确性，提升到达预定地点的准时性，改善行驶平稳性是无人车控制策略未来重要的发展方向，也是提高无人车自主化和智能化水平、高效化运作的必然途径。无人车轨迹跟踪方法是决定以上三项性能指标的关键，但是，由于现有的无人车轨迹跟踪方法过于关注轨迹跟踪的准确性，对于无人车到达预定地点的准时性以及改善无人车行驶的平稳性的重要性没有引起足够的重视。例如，王振在《无人车运动控制系统设计与实现》一文中提出，采用增量式PID控制方法控制行进速度，并设计了模糊分级式制动控制策略控制制动速度，同时还设计了方向预估算法来控制方向。该无人车轨迹跟踪方法可实现高精度轨迹跟踪，低速下轨迹跟踪误差为0.8m。虽然，该方法均有效地提升了无人车轨迹跟踪性能，但是该无人车轨迹跟踪方法以单段轨迹跟踪为考核标准，没有考虑到多段轨迹下的控制特性，例如准时性差等问题。而且，对于行驶平稳性问题，并没有提出有效手段给予及时干预。虽然无人车能够精确地按照预定轨迹行驶，但其却不能准时地执行预定计划，以及平稳地、安全地行驶至目标位置，从而导致无人车有悖于自主化、智能化、高效化的设计理念和设计初衷。基于此，在保证无人车跟踪预定轨迹的准确性的基础上，为了有效提升无人车执行预定计划的准确性，改善无人车行驶的平稳性和安全性，确保无人车完全具备自主化和智能化的优势，以及拓展无人车的应用领域，研究一种速度自适应的无人车轨迹跟踪PID控制策略具有重要的实际意义。
技术实现思路
本专利技术的目的在于根据现有技术的不足，提供一种轨迹跟踪精度、准时性、平稳性和安全性更优的速度自适应无人车轨迹跟踪方法。本专利技术解决其技术问题所采用的技术方案是：一种速度自适应的无人车轨迹跟踪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，预定计划执行完毕，此时结束无人车轨迹跟踪，无人车到达目标位置。所述的一种速度自适应的无人车轨迹跟踪PID控制方法，在无人车行驶过程中，根据无人车的水平状态实时进行速度平稳性调整，平稳性调整具体表达式为：式中，θ为无人车水平倾角，Vt为无人车平稳性调整后的速度，Vc为无人车平稳性调整前的速度。所述的一种速度自适应的无人车轨迹跟踪PID控制方法，其步骤e)中根据当前位置Pc和目标位置Pt(n)实时解算目标方向，利用当前方向和目标方向求取方向偏差，基于增量式PID算法实时解算方向控制量，方向增量式PID控制器的表达式为：式中，eH(k)为k采样时刻的方向偏差，eH(k-1)为(k-1)采样时刻的方向偏差，eH(k-2)为(k-2)采样时刻的方向偏差，ΔuH(k)为方向控制量增量，uH(k)为方向控制量，AH为方向偏差eH(k)的系数，BH为方向偏差eH(k-1)的系数，CH为方向偏差eH(k-2)的系数，为方向增量式PID控制器的比例系数，为方向增量式PID控制器的积分系数，为方向增量式PID控制器的微分系数，TH为方向采样间隔。所述的一种速度自适应的无人车轨迹跟踪PID控制方法，其步骤e)中根据当前速度Vc和目标速度Vt利用增量式PID算法实时解算速度控制量，通过调整方向增量式PID控制器和速度增量式PID控制器的P参数、I参数和D参数，实现精准地、准时地跟踪预定轨迹，速度增量式PID控制器的表达式为：式中，eV(k)为k采样时刻的速度偏差，eV(k-1)为(k-1)采样时刻的速度偏差，eV(k-2)为(k-2)采样时刻的速度偏差，ΔuV(k)为速度控制量增量，uV(k)为速度控制量，AV为速度偏差eV(k)的系数，BV为速度偏差eV(k-1)的系数，CV为速度偏差eV(k-2)的系数，为速度增量式PID控制器的比例系数，为速度增量式PID控制器的积分系数，为速度增量式PID控制器的微分系数，TV为速度采样间隔。本专利技术具有如下有益效果：(1)引入速度自适应调整方法督促无人车严格执行预定计划，保证其准时到达预定地点，确保了无人车轨迹跟踪的准时性。(2)设计平稳性调整方法有效地改善无人车行驶平稳性，显著提升了无人车行驶的安全性。附图说明图1是本专利技术方法的总体方案图；图2是传统型无人车轨迹跟踪控制策略控制框图；图3是本专利技术方法无人车轨迹跟踪控制方法框图；图4是本专利技术中单段轨迹偏离轨迹误差对比图；图5是本专利技术中多段轨迹偏离轨迹误差对比图。具体实施方式下面结合附图对本专利技术本文档来自技高网...

【技术保护点】
1.一种速度自适应的无人车轨迹跟踪PID控制方法，其特征在于，包括如下步骤：a)，以WGS84坐标作为参考，获取无人车当前位置Pc，并根据预定计划查询当前目标位置Pt(n)，根据如下公式利用球面坐标计算当前位置Pc与当前目标位置Pt(n)之间的距离Dt：

【技术特征摘要】
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，预定计划执行完毕，此时结束无人车轨迹跟踪，无人车到达目标位置。2.根据权利要求1所述的一种速度自适应的无人车轨迹...

【专利技术属性】
技术研发人员：袁宇翔，邹斌，
申请(专利权)人：武汉理工大学，
类型：发明
国别省市：湖北,42