CN108279563B - 一种速度自适应的无人车轨迹跟踪pid控制方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于导航定位领域，内容具体涉及了一种速度自适应的无人车轨迹跟踪PID控制方法。

背景技术

随着传感器技术和导航技术的不断进步，无人系统正以井喷态势迅速发展，其中的典型代表当属无人机、无人车和无人艇。相比于无人机和无人艇，无人车与工业生产和日常生活结合更加紧密，更具进一步发展的生机与活力，其在军事领域也起到了重要作用。例如，工业生产中，危险环境下设备维修和无人值守；军事领域中，排雷和排爆，月球车等。因此，无人车在无人系统中的重要性越加显著。

无人车轨迹跟踪性能优秀与否，取决于三个评判标准，一是能否准确地按照预定轨迹行驶，二是能否准时到达预定位置，三是能否保证行驶的安全性和平稳性。准确地按照预定轨迹行驶说明无人车控制策略的轨迹跟踪准确性高、实时性好，跟踪预定轨迹的实时调整能力强；准时到达预定位置说明无人车控制策略具备较好的速度调整灵活性；保证行驶的安全性、稳定性和平稳性说明无人车控制策略调整无人车行驶姿态的能力强，准确性和实时性较高。在无人车设计和应用过程中，提高跟踪预定轨迹的准确性，提升到达预定地点的准时性，改善行驶平稳性是无人车控制策略未来重要的发展方向，也是提高无人车自主化和智能化水平、高效化运作的必然途径。

无人车轨迹跟踪方法是决定以上三项性能指标的关键，但是，由于现有的无人车轨迹跟踪方法过于关注轨迹跟踪的准确性，对于无人车到达预定地点的准时性以及改善无人车行驶的平稳性的重要性没有引起足够的重视。

例如，王振在《无人车运动控制系统设计与实现》一文中提出，采用增量式PID控制方法控制行进速度，并设计了模糊分级式制动控制策略控制制动速度，同时还设计了方向预估算法来控制方向。该无人车轨迹跟踪方法可实现高精度轨迹跟踪，低速下轨迹跟踪误差为0.8m。虽然，该方法均有效地提升了无人车轨迹跟踪性能，但是该无人车轨迹跟踪方法以单段轨迹跟踪为考核标准，没有考虑到多段轨迹下的控制特性，例如准时性差等问题。而且，对于行驶平稳性问题，并没有提出有效手段给予及时干预。虽然无人车能够精确地按照预定轨迹行驶，但其却不能准时地执行预定计划，以及平稳地、安全地行驶至目标位置，从而导致无人车有悖于自主化、智能化、高效化的设计理念和设计初衷。

基于此，在保证无人车跟踪预定轨迹的准确性的基础上，为了有效提升无人车执行预定计划的准确性，改善无人车行驶的平稳性和安全性，确保无人车完全具备自主化和智能化的优势，以及拓展无人车的应用领域，研究一种速度自适应的无人车轨迹跟踪PID控制策略具有重要的实际意义。

发明内容

本发明的目的在于根据现有技术的不足，提供一种轨迹跟踪精度、准时性、平稳性和安全性更优的速度自适应无人车轨迹跟踪方法。

本发明解决其技术问题所采用的技术方案是：一种速度自适应的无人车轨迹跟踪PID控制方法，包括如下步骤：

a)，以WGS84坐标作为参考，获取无人车当前位置P_c，并根据预定计划查询当前目标位置P_t(n)，根据如下公式利用球面坐标计算当前位置P_c与当前目标位置P_t(n)之间的距离D_t：

式中，当前位置坐标为目标位置坐标为 和为经度坐标，λ_A和λ_B为纬度坐标，R为地球半径，R＝6378140m，n为当前目标位置在预定计划目标位置列表中的序号，且n＝1，2，3……，N，N为预定计划中的目标位置总数；

b)，根据如下公式利用球面坐标计算当前位置P_c与当前目标位置P_t(n)之间的目标方向夹角H_t：

式中，H_t的取值范围为0°～360°，并以正北为0°参考方向；

c)，以北京时间作为参考，获取当前时间T_c；然后，查询事先制定的预定计划，获取行驶至当前目标位置P_t(n)的目标时间T_t；

d)，根据由步骤a)获得的D_t和由步骤c)获得的T_t、T_c，在准时到达目标位置P_t(n)的前提下，根据如下公式计算无人车由当前位置P_c行驶至当前目标位置P_t(n)所需的目标速度V_t，由此完成速度自适应调整，具体如下：

e)，获取无人车当前速度V_c和当前方向H_c；基于当前方向H_c和由步骤b)获得的目标方向H_t，完成无人车轨迹跟踪中的方向控制；基于当前速度V_c和由步骤d)获得的目标速度V_t，完成无人车轨迹跟踪中的速度控制；

f)，实时计算无人车当前位置P_c和目标位置P_t(n)之间的距离D_c，并判定预定计划是否全部完成；若D_c大于所设定的到达阈值D_s，则判定无人车还没有到达目标位置；若D_c小于或者等于所设定的到达阈值D_s，则判定无人车已经到达目标位置，然后，按照预定计划目标位置列表的顺序，将当前目标位置P_t(n)更新为下一目标位置P_t(n+1)；

g)，利用序号n和目标位置总数N判断预定计划是否执行完毕；若n＜N，预定计划未完成，则重新执行步骤a)至步骤f)；若n＝N，预定计划执行完毕，此时结束无人车轨迹跟踪，无人车到达目标位置。

所述的一种速度自适应的无人车轨迹跟踪PID控制方法，基于步骤d)获得的速度，在无人车行驶过程中，根据无人车的水平状态实时进行速度平稳性调整，平稳性调整具体表达式为：

式中，θ为无人车水平倾角，V_t为无人车平稳性调整后的速度，V_c为无人车平稳性调整前的速度。

所述的一种速度自适应的无人车轨迹跟踪PID控制方法，其步骤e)中根据当前位置P_c和目标位置P_t(n)实时解算目标方向，利用当前方向和目标方向求取方向偏差，基于增量式PID算法实时解算方向控制量，方向增量式PID控制器的表达式为：

式中，e_H(k)为k采样时刻的方向偏差，e_H(k-1)为(k-1)采样时刻的方向偏差，e_H(k-2)为(k-2)采样时刻的方向偏差，Δu_H(k)为方向控制量增量，u_H(k)为方向控制量，A_H为方向偏差e_H(k)的系数，B_H为方向偏差e_H(k-1)的系数，C_H为方向偏差e_H(k-2)的系数，为方向增量式PID控制器的比例系数，T_i ^H为方向增量式PID控制器的积分系数，为方向增量式PID控制器的微分系数，T^H为方向采样间隔。

所述的一种速度自适应的无人车轨迹跟踪PID控制方法，其步骤e)中根据当前速度V_c和目标速度V_t利用增量式PID算法实时解算速度控制量，通过调整方向增量式PID控制器和速度增量式PID控制器的P参数、I参数和D参数，实现精准地、准时地跟踪预定轨迹，速度增量式PID控制器的表达式为：

式中，e_V(k)为k采样时刻的速度偏差，e_V(k-1)为(k-1)采样时刻的速度偏差，e_V(k-2)为(k-2)采样时刻的速度偏差，Δu_V(k)为速度控制量增量，u_V(k)为速度控制量，A_V为速度偏差e_V(k)的系数，B_V为速度偏差e_V(k-1)的系数，C_V为速度偏差e_V(k-2)的系数，为速度增量式PID控制器的比例系数，T_i ^V为速度增量式PID控制器的积分系数，为速度增量式PID控制器的微分系数，T^V为速度采样间隔。

本发明具有如下有益效果：

(1)引入速度自适应调整方法督促无人车严格执行预定计划，保证其准时到达预定地点，确保了无人车轨迹跟踪的准时性。

(2)设计平稳性调整方法有效地改善无人车行驶平稳性，显著提升了无人车行驶的安全性。

附图说明

图1是本发明方法的总体方案图；

图2是传统型无人车轨迹跟踪控制策略控制框图；

图3是本发明方法无人车轨迹跟踪控制方法框图；

图4是本发明中单段轨迹偏离轨迹误差对比图；

图5是本发明中多段轨迹偏离轨迹误差对比图。

具体实施方式

下面结合附图对本发明作进一步详细说明。

本发明公开了一种速度自适应的无人车轨迹跟踪PID控制方法，采用增量式PID算法实现无人车精确地、及时地跟踪预定轨迹，引入速度自适应调整来保证其准时到达预定地点，设计平稳性调整以改善无人车行驶平稳性。在无人车行驶过程中，根据无人车的当前位置P_c与当前目标位置P_t(n)之间的距离D_t，以及当前时间T_c和目标时间T_t实时更新目标速度，实现速度自适应调整，具体计算过程如下。

步骤a)：

无人车轨迹跟踪方法的目的是，按照预定计划的要求，在按时到达预定地点的前提下，确保无人车精准地跟踪预定轨迹，实际路线与预定路线的偏差应小于预设轨迹偏差值。预定计划以信息列表的行驶详细规划了无人车需要在什么时间到达什么地点，信息列表主要包含的信息为，目标位置序号、目标位置和预定时间。预定计划根据国际惯例和无人车应用地域的要求，以北京时间为时间信息参考，以WGS84坐标为位置信息参考。

由图2可知，方向环和速度环需要目标输入，而目标输入由当前位置和预定计划提供的目标位置实时更新得到。根据当前位置P_c和目标位置P_t(n)，利用球面坐标计算当前位置P_c与目标位置P_t(n)之间的距离D_t。由于地球可近似为球体，因此可采用球体相关计算公式来计算目标距离D_t，具体如下所示：

式中，当前位置坐标为目标位置坐标为 和为经度坐标，λ_A和λ_B为纬度坐标，R为地球半径，R＝6378140m，D_t为当前位置P_c与当前目标位置P_t(n)之间的距离，n为当前目标位置在预定计划目标位置列表中的序号，且n＝1，2，3……，N，N为预定计划中的目标位置总数。

步骤b)：

根据当前位置P_c和目标位置P_t(n)，利用球面坐标计算当前位置与目标位置之间的目标方向H_t。

通常情况下，方向参考基准有两种表达方式，一种是0°～360°，以正北为0°参考方向，顺时针方向为方向角增加方向；另外一种是-180°～180°，以正北为0°参考方向，顺时针方向增加至180°，逆时针方向减小至-180°。为了便于计算，选取方式一作为方向参考基准。因此，根据球面坐标相关计算公式，根据当前位置P_c的GPS坐标和目标位置P_t(n)的GPS坐标计算目标方向H_t，具体如下：

式中，H_t为目标方向，取值范围为0°～360°，并以正北为0°参考方向。

步骤c)：

以北京时间作为参考，首先，获取当前时间T_c；然后，查询事先制定的预定计划，获取行驶至当前目标位置P_t(n)的目标时间T_t。

步骤d)：

在无人车行驶过程中，由于行驶偏差是不可避免的，随着行驶时间的增加，行驶偏差在不断累积，最终，实际行驶轨迹要大于预定行驶轨迹，伴随而来的是到达预定地点的延迟。如果此时还按照预定速度V_t继续行驶，势必不能按时到达预定地点。

为了解决以上问题，需要根据当前位置与目标位置之间的距离，实时调整目标速度，即速度自适应控制方法。根据由步骤1获得的D_t和由步骤3获得的T_t、T_c，在准时到达目标位置P_t(n)的前提下，计算无人车由当前位置P_c行驶至当前目标位置P_t(n)所需的目标速度V_t，由此完成速度自适应调整，具体如下：

步骤e)：

无人车精准跟踪预定轨迹主要通过精准地、实时地控制行驶方向和行驶速度实现，因此，主要由两个控制闭环构成，分别是用于控制行驶方向的方向环和用于控制行驶速度的速度环。由于方向环的准确性和实时性是保证无人车精准地按照预定轨迹行驶的关键，方向控制及时，跟踪轨迹才能更加精确，所以，方向环须在速度环内，即先进行行驶方向精确控制，然后再进行行驶速度精确控制，传统型无人车轨迹跟踪控制框图如图2所示。

此时，获取无人车当前速度V_c和当前方向角H_c。根据当前速度V_c和由步骤d)获得的目标速度V_t，完成速度控制；根据当前方向角H_c和步骤d)获得的目标方向角H_t，完成方向控制。根据当前位置和目标位置实时解算目标方向，利用当前方向和目标方向求取方向偏差，基于增量式PID算法实时解算方向控制量；根据当前速度和目标速度利用增量式PID算法实时解算速度控制量。通过调整方向增量式PID控制器和速度增量式PID控制器的P参数、I参数和D参数，实现精准地、准时地跟踪预定轨迹。

方向增量式PID控制器的表达式如下所示：

式中，e_H(k)为k采样时刻的方向偏差，e_H(k-1)为(k-1)采样时刻的方向偏差，e_H(k-2)为(k-2)采样时刻的方向偏差，Δu_H(k)为方向控制量增量，u_H(k)为方向控制量，A_H为方向偏差e_H(k)的系数，B_H为方向偏差e_H(k-1)的系数，C_H为方向偏差e_H(k-2)的系数，为方向增量式PID控制器的比例系数，T_i ^H为方向增量式PID控制器的积分系数，为方向增量式PID控制器的微分系数，T^H为方向采样间隔。

速度增量式PID控制器的表达式如下所示：

式中，e_V(k)为k采样时刻的速度偏差，e_V(k-1)为(k-1)采样时刻的速度偏差，e_V(k-2)为(k-2)采样时刻的速度偏差，Δu_V(k)为速度控制量增量，u_V(k)为速度控制量，A_V为速度偏差e_V(k)的系数，B_V为速度偏差e_V(k-1)的系数，C_V为速度偏差e_V(k-2)的系数，为速度增量式PID控制器的比例系数，T_i ^V为速度增量式PID控制器的积分系数，为速度增量式PID控制器的微分系数，T^V为速度采样间隔。

当无人车经过弯路或者路况较差地带时，如果速度较快或者弯路较急，这会存在侧滑甚至侧翻的危险。为了保证无人车行驶的安全性和平稳性，需要大幅度限制可能导致侧滑或者侧翻的危险速度，车体的倾斜程度是判断是否会发生侧翻的最直接判据。基于大量的统计数据可知，汽车倾斜角度的极限值为30°，超过极限角度就会发生翻车事故。

为了避免出现危险，及时降低车速是保证无人车行驶的安全性和平稳性的首要选择。通过调查驾驶过各种车型且经验丰富的老司机的实际驾驶经验，经过总结可知，时速20km/h足以应付绝大多数危险路况。为了杜绝所有危险的发生，根据无人车水平倾角决定平稳性调整方法是否参与速度控制过程。

当无人车水平倾角超过极限值30°时，平稳性调整方法会及时干预速度环，并对其进行最小值修正，保证无人车行驶的安全性，即将目标速度降低到20km/h；当无人车水平倾角恢复并小于极限值30°后，自适应速度调整方法会根据步骤d)重新获取目标速度V_t，保证无人车能够按时达到目标位置P_t(n)。平稳性调整方法具体如下：

式中，θ为无人车水平倾角，V_t为无人车平稳性调整后的速度，V_c为无人车平稳性调整前的速度。相比于传统型无人车轨迹跟踪控制方法，本发明方法不仅能够根据无人车当前运行状态实时修正其速度，确保速度根据目标距离和所需时间实时变化，避免了因目标速度不变而导致无法按时到达预定位置的情况发生，而且还能够保证无人车安全地、平稳地行驶至目标位置，有效地避免了因弯路以及路况较差等外部因素而导致的安全性和平稳性较差的风险。改进型无人车轨迹跟踪控制策略为保证无人车准确地且准时到达预定地点提供了有效保障。

步骤f)：

基于式(1)，计算当前位置P_c和目标位置P_t(n)之间的距离D_c，并判定预定计划是否全部完成。若D_c大于所设定的到达阈值D_s，则判定无人车还没有到达目标位置；若D_c小于或者等于所设定的到达阈值D_s，则判定无人车已经到达目标位置，然后，按照预定计划目标位置列表的顺序，将当前目标位置P_t(n)更新为下一目标位置P_t(n+1)。

步骤g)：

利用序号n和目标位置总数N判断预定计划是否执行完毕。若n＜N，预定计划未完成，则重新执行步骤1至步骤7；若n＝N，预定计划执行完毕，此时结束无人车轨迹跟踪，无人车到达目标位置。

实验验证分析

(1)实验条件设置

基于无人车轨迹跟踪性能的评判标准，为了说明无人车轨迹跟踪综合性能的提升，以传统型无人车轨迹跟踪控制方法为参考，与本发明方法的综合性能进行对比。基于全面考核速度自适应的无人车轨迹跟踪方法的综合性能考虑，设计了轨迹跟踪实验，即单段轨迹跟踪实验和多段轨迹跟踪实验。单段轨迹跟踪实验用于考核轨迹跟踪的准确性，多段轨迹跟踪实验用于考核轨迹跟踪的准时性和平稳性。

①单段轨迹跟踪实验

为了增强实验的可信度，预定计划以实际地理坐标为参考，并且随机选取起点和终点位置。例如，以哈尔滨为例，令无人车从A点(126.695116，45.782667)自主行驶到B点(126.694083，45.784783)。图4给出了单段轨迹偏离轨迹误差对比图，表1给出了预定计划和实际到达时间。

②多段轨迹跟踪实验

利用多段轨迹考核无人车轨迹跟踪控制策略的准时性和平稳性。例如，以哈尔滨为例，令无人车从A点(126.695116，45.782667)自主行驶经过B点(126.694083，45.784783)、C点(126.691720，45.785295)、D点(126.692654，45.781271)，最后达到E点(126.695484，45.780988)。图5给出了多段轨迹偏离轨迹误差对比图，表2给出了预定计划和实际到达时间。

(2)实验结果分析

表1预定计划和实际到达时间

由表1可知，在单段轨迹跟踪实验中，本发明方法基本能够按时到达预定地点，传统型无人车轨迹跟踪方法比本发明方法迟到了44s。由图4可知，虽然，传统型无人车轨迹跟踪方法和本发明方法到达目标位置的最大轨迹跟踪偏差均约为5m，但本发明方法在行驶过程中的轨迹跟踪偏差明显小于传统型无人车轨迹跟踪方法的轨迹跟踪偏差。相比于传统型无人车轨迹跟踪方法，本发明方法跟踪精度提高了20％，这说明本发明方法更加准确有效。

表2预定计划和实际到达时间

由表2可知，在多段轨迹跟踪实验中，相比于本发明方法，传统型无人车轨迹跟踪方法到达预定位置比预定时间稍晚，随着行驶路程的增加，到达时间延迟越大。由图5可知，传统型无人车轨迹跟踪方法和本发明方法到达目标位置的最大轨迹跟踪偏差均约为5m，本发明方法在行驶过程中的轨迹跟踪偏差明显小于传统型无人车轨迹跟踪方法的轨迹跟踪偏差。而且，当无人车行驶至拐点时，相比于传统型无人车轨迹跟踪方法，本发明方法的轨迹跟踪偏差为0，这说明无人车降速精准地通过目标位置，进而确保了无人车行驶的平稳性和安全性。因此，本发明方法具有更为优秀的轨迹跟踪准确性、准时性和平稳性。

基于以上结论可知，一种速度自适应的无人车轨迹跟踪方法不仅速度自适应调整方法实现了准确地跟踪预定轨迹，有效保证了无人车能够按时达到目标位置，并且对不同行驶环境下的平稳性问题给出了调整，保证无人车具有更为优秀的轨迹跟踪的精确性、平稳性、稳定性、和安全性。对于需要综合性能较高的无人车轨迹跟踪控制方法的各领域来说，速度自适应的无人车轨迹跟踪方法具有重要的实际意义。

Claims (4)

1.一种速度自适应的无人车轨迹跟踪PID控制方法，其特征在于，包括如下步骤：

a)，以WGS84坐标作为参考，获取无人车当前位置P_c，并根据预定计划查询当前目标位置P_t(n)，根据如下公式利用球面坐标计算当前位置P_c与当前目标位置P_t(n)之间的距离D_t：

式中，当前位置坐标为目标位置坐标为 和为经度坐标，λ_A和λ_B为纬度坐标，R为地球半径，R＝6378140m，n为当前目标位置在预定计划目标位置列表中的序号，且n＝1，2，3……，N，N为预定计划中的目标位置总数；

b)，根据如下公式利用球面坐标计算当前位置P_c与当前目标位置P_t(n)之间的目标方向夹角H_t：

式中，H_t的取值范围为0°～360°，并以正北为0°参考方向；

c)，获取当前时间T_c；获取行驶至当前目标位置P_t(n)的目标时间T_t；

d)，根据由步骤a)获得的D_t和由步骤c)获得的T_t、T_c，在准时到达目标位置P_t(n)的前提下，根据如下公式计算无人车由当前位置P_c行驶至当前目标位置P_t(n)所需的目标速度V_t：

e)，获取无人车当前速度V_c和当前方向H_c；基于当前方向H_c和由步骤b)获得的目标方向H_t，完成无人车轨迹跟踪中的方向控制；基于当前速度V_c和由步骤d)获得的目标速度V_t，完成无人车轨迹跟踪中的速度控制；

f)，实时计算无人车当前位置P_c和目标位置P_t(n)之间的距离D_c，并判定预定计划进度；若D_c大于所设定的到达阈值D_s，则判定无人车还没有到达目标位置；若D_c小于或者等于所设定的到达阈值D_s，则判定无人车已经到达目标位置，然后，按照预定计划目标位置列表的顺序，将当前目标位置P_t(n)更新为下一目标位置P_t(n+1)；

g)，利用序号n和目标位置总数N判断预定计划是否执行完毕；若n＜N，预定计划未完成，则重新执行步骤a)至步骤f)；若n＝N，预定计划执行完毕，此时结束无人车轨迹跟踪，无人车到达目标位置。

2.根据权利要求1所述的一种速度自适应的无人车轨迹跟踪PID控制方法，其特征在于，基于步骤d)获得的速度，在无人车行驶过程中，根据无人车的水平状态实时进行速度平稳性调整，平稳性调整具体表达式为：

式中，θ为无人车水平倾角，V_t为无人车平稳性调整后的速度，V_c为无人车平稳性调整前的速度。

3.根据权利要求1所述的一种速度自适应的无人车轨迹跟踪PID控制方法，其特征在于，所述的步骤e)中根据当前位置P_c和目标位置P_t(n)实时解算目标方向，利用当前方向和目标方向求取方向偏差，基于增量式PID算法实时解算方向控制量，方向增量式PID控制器的表达式为：

式中，e_H(k)为k采样时刻的方向偏差，e_H(k-1)为(k-1)采样时刻的方向偏差，e_H(k-2)为(k-2)采样时刻的方向偏差，Δu_H(k)为方向控制量增量，u_H(k)为方向控制量，A_H为方向偏差e_H(k)的系数，B_H为方向偏差e_H(k-1)的系数，C_H为方向偏差e_H(k-2)的系数，为方向增量式PID控制器的比例系数，T_i ^H为方向增量式PID控制器的积分系数，为方向增量式PID控制器的微分系数，T^H为方向采样间隔。

4.根据权利要求1所述的一种速度自适应的无人车轨迹跟踪PID控制方法，其特征在于，所述的步骤e)中根据当前速度V_c和目标速度V_t利用增量式PID算法实时解算速度控制量，通过调整方向增量式PID控制器和速度增量式PID控制器的P参数、I参数和D参数，实现精准地、准时地跟踪预定轨迹，速度增量式PID控制器的表达式为：

式中，e_V(k)为k采样时刻的速度偏差，e_V(k-1)为(k-1)采样时刻的速度偏差，e_V(k-2)为(k-2)采样时刻的速度偏差，Δu_V(k)为速度控制量增量，u_V(k)为速度控制量，A_V为速度偏差e_V(k)的系数，B_V为速度偏差e_V(k-1)的系数，C_V为速度偏差e_V(k-2)的系数，为速度增量式PID控制器的比例系数，T_i ^V为速度增量式PID控制器的积分系数，为速度增量式PID控制器的微分系数，T^V为速度采样间隔。