一种轨迹跟踪控制方法、装置、电子设备及存储介质制造方法及图纸

本发明专利技术提供了一种轨迹跟踪控制方法、装置、电子设备及存储介质，涉及轨迹跟踪技术领域。该轨迹跟踪控制方法包括以下步骤：基于差速轮移动机器人的当前位置点，根据预设的第一前瞻距离，在被跟踪轨迹上获取第一轨迹位置点；根据第一前瞻距离和第一轨迹位置点，在被跟踪轨迹上获取目标位置点；基于双环PI控制器，根据目标位置点和当前位置点，计算差速轮移动机器人的第一线速度；根据第一线速度计算差速轮移动机器人的角速度；根据第一线速度和角速度计算驱动轮速度；根据驱动轮速度控制差速轮移动机器人进行轨迹跟踪。本发明专利技术能够在单纯使用PID控制器时对机器人的位置和航向角进行耦合，以此使轨迹跟踪达到更好的效果。以此使轨迹跟踪达到更好的效果。以此使轨迹跟踪达到更好的效果。

【技术实现步骤摘要】
一种轨迹跟踪控制方法、装置、电子设备及存储介质


[0001]本专利技术涉及轨迹跟踪
，具体而言，涉及一种轨迹跟踪控制方法、装置、电子设备及存储介质。

技术介绍

[0002]差速轮移动机器人的轨迹跟踪是影响机器人导航精度的一个重要因素，目前的主流轨迹跟踪算法有比例
‑
积分
‑
微分(PID)、纯跟踪(Pure Pursuit)、线性二次规划调节器(LQR)、模型预测控制器(MPC)。
[0003]对于平面移动机器人而言，共有三个自由度，分别是X方向的平移(位置X坐标)、Y方向的平移(位置Y坐标)和Z方向的旋转(航向角)，机器人的轨迹跟踪输入的是线速度V和角速度W，输出的是机器人的位置(即位置X坐标和位置Y坐标)和航向角，且在机器人轨迹跟踪中，机器人的位置和航向角之间存在耦合关系。
[0004]在传统的PID轨迹跟踪算法中，因为PID控制器只适用于单输入单输出的控制系统，为实现轨迹跟踪，因此往往需要使用两个PID控制器分别单独控制机器人的位置和航向角，然而这种单独控制并没有考虑机器人的位置和航向角之间的耦合关系（即无法处理多输入多输出的耦合关系），以致控制效果并不理想。
[0005]针对上述问题，目前尚未有有效的技术解决方案。

技术实现思路

[0006]本专利技术的目的在于提供一种轨迹跟踪控制方法、装置、电子设备及存储介质，能够在单纯使用PID控制器时对机器人的位置和航向角进行耦合，以此使轨迹跟踪达到更好的效果。
[0007]第一方面，本专利技术提供了一种轨迹跟踪控制方法，应用于差速轮移动机器人的控制系统，包括以下步骤：S1.基于所述差速轮移动机器人的当前位置点，根据预设的第一前瞻距离，在被跟踪轨迹上获取第一轨迹位置点；S2.根据所述第一前瞻距离和所述第一轨迹位置点，在被跟踪轨迹上获取目标位置点；S3.基于双环PI控制器，根据所述目标位置点和所述当前位置点，计算所述差速轮移动机器人的第一线速度；S4.根据所述第一线速度计算所述差速轮移动机器人的角速度；S5.根据所述第一线速度和所述角速度计算驱动轮速度；S6.根据所述驱动轮速度控制所述差速轮移动机器人进行轨迹跟踪。
[0008]本专利技术提供的轨迹跟踪控制方法解决了单纯使用PID控制器无法处理多输入多输出的耦合关系问题，实现对机器人的位置和航向角的耦合，确保轨迹跟踪具有良好的控制效果。
[0009]进一步的，步骤S2中的具体步骤包括：S21.根据所述第一轨迹位置点，获取所述第一轨迹位置点的曲率；S22.根据所述第一前瞻距离和所述第一轨迹位置点的曲率，获取第二前瞻距离；S23.根据所述第二前瞻距离，获取所述目标位置点。
[0010]根据被跟踪轨迹的曲率动态调速，以此适应不同曲率的被跟踪轨迹，进而确保机器人能够顺利沿弯曲轨迹跟踪。
[0011]进一步的，步骤S21中的具体步骤包括：S211.在被跟踪轨迹上获取第二轨迹位置点和第三轨迹位置点，所述第二轨迹位置点、所述第三轨迹位置点和所述第一轨迹位置点均不相同；S212.根据第一轨迹位置点、第二轨迹位置点和第三轨迹位置点，获取曲线参数；S213.根据所述曲线参数计算所述第一轨迹位置点的曲率。
[0012]利用三点法计算出第一轨迹位置点的曲率，计算过程简单，运算量小。
[0013]进一步的，步骤S212中的具体步骤包括：S2121.根据以下公式计算所述曲线参数：；；；；其中，为第一参数，为第二参数，为第三参数，为第四参数，为第五参数，为第六参数，为所述第二轨迹位置点和所述第一轨迹位置点之间的第一距离；为所述第三轨迹位置点和所述第一轨迹位置点之间的第二距离；为所述第二轨迹位置点的x轴坐标值，为所述第一轨迹位置点的x轴坐标值，为所述第三轨迹位置点的x轴坐标值，为所述第二轨迹位置点的y轴坐标值，为所述第一轨迹位置点的y轴坐标值，为所述第三轨迹位置点的y轴坐标值。
[0014]进一步的，步骤S213中的具体步骤包括：S2131.根据以下公式计算所述第一轨迹位置点的曲率：；其中，为所述第一轨迹位置点的曲率。
[0015]进一步的，步骤S22中的具体步骤包括：S221.根据以下公式计算所述第二前瞻距离：
;；;其中，为所述第二前瞻距离，为第一增益系数，为最大曲率，为所述差速轮移动机器人的行驶速度，为第二增益系数，为最小前瞻距离，为所述第一前瞻距离,为所述差速轮移动机器人的最大角速度，为所述差速轮移动机器人的最小线速度。
[0016]进一步的，所述双环PI控制器包括位置环和速度环；步骤S3中的具体步骤包括：S31.根据所述目标位置点和所述当前位置点，通过所述位置环计算出所述差速轮移动机器人的参考线速度；S32.将所述参考线速度输入到所述速度环中，以计算出所述差速轮移动机器人的参考加速度；S33.根据所述参考线速度和所述参考加速度，计算出所述第一线速度；步骤S4中的具体步骤包括：S41.根据以下公式计算所述角速度：；；其中，为所述角速度，为所述第一线速度，为所述第二前瞻距离，为所述当前位置点与所述目标位置点的连线与轨迹切线之间的夹角，为所述当前位置点和所述目标位置点在y轴方向的偏差，为所述当前位置点和所述目标位置点在x轴方向的偏差，为所述差速轮移动机器人的当前航向角。
[0017]速度环能够对机器人加速度进行约束，使得机器人的行驶速度曲线平滑且满足物理加速度约束，进而确保后续计算得到的角速度不会超出物理限制范围，以顺利沿弯曲轨迹进行轨迹跟踪。
[0018]第二方面，本专利技术提供了一种轨迹跟踪控制装置，应用于差速轮移动机器人的控制系统，包括：第一获取模块，用于基于所述差速轮移动机器人的当前位置点，根据预设的第一前瞻距离，在被跟踪轨迹上获取第一轨迹位置点；第二获取模块，用于根据所述第一前瞻距离和所述第一轨迹位置点，在被跟踪轨迹上获取目标位置点；第一计算模块，用于基于双环PI控制器，根据所述目标位置点和所述当前位置点，计算所述差速轮移动机器人的第一线速度；
第二计算模块，用于根据所述第一线速度计算所述差速轮移动机器人的角速度；第三计算模块，用于根据所述第一线速度和所述角速度计算驱动轮速度；控制模块，用于根据所述驱动轮速度控制所述差速轮移动机器人进行轨迹跟踪。
[0019]本专利技术提供的轨迹跟踪控制装置可以使机器人在进行轨迹跟踪时，根据被跟踪轨迹的曲率动态调速，以此适应不同曲率的被跟踪轨迹；且终点的稳态跟踪精度高，计算量小，能够在低性能的硬件上应用。
[0020]第三方面，本专利技术提供一种电子设备，包括处理器以及存储器，所述存储器存储有计算机可读取指令，当所述计算机可读取指令由所述处理器执行时，运行如上述第一方面提供的所述轨迹跟踪控制方法中的步骤。
[0021]第四方面，本专利技术提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如上述第一方面提供的所述轨迹跟踪控制方法中的步骤。
[0022]由上可知，本专利技术提供的轨迹跟踪控制方法，双环PI控制器根据目标位置点和本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种轨迹跟踪控制方法，应用于差速轮移动机器人的控制系统，其特征在于，包括以下步骤：S1.基于所述差速轮移动机器人的当前位置点，根据预设的第一前瞻距离，在被跟踪轨迹上获取第一轨迹位置点；S2.根据所述第一前瞻距离和所述第一轨迹位置点，在被跟踪轨迹上获取目标位置点；S3.基于双环PI控制器，根据所述目标位置点和所述当前位置点，计算所述差速轮移动机器人的第一线速度；S4.根据所述第一线速度计算所述差速轮移动机器人的角速度；S5.根据所述第一线速度和所述角速度计算驱动轮速度；S6.根据所述驱动轮速度控制所述差速轮移动机器人进行轨迹跟踪。2.根据权利要求1所述的轨迹跟踪控制方法，其特征在于，步骤S2中的具体步骤包括：S21.根据所述第一轨迹位置点，获取所述第一轨迹位置点的曲率；S22.根据所述第一前瞻距离和所述第一轨迹位置点的曲率，获取第二前瞻距离；S23.根据所述第二前瞻距离，获取所述目标位置点。3.根据权利要求2所述的轨迹跟踪控制方法，其特征在于，步骤S21中的具体步骤包括：S211.在被跟踪轨迹上获取第二轨迹位置点和第三轨迹位置点，所述第二轨迹位置点、所述第三轨迹位置点和所述第一轨迹位置点均不相同；S212.根据第一轨迹位置点、第二轨迹位置点和第三轨迹位置点，获取曲线参数；S213.根据所述曲线参数计算所述第一轨迹位置点的曲率。4.根据权利要求3所述的轨迹跟踪控制方法，其特征在于，步骤S212中的具体步骤包括：S2121.根据以下公式计算所述曲线参数：；；；；其中，为第一参数，为第二参数，为第三参数，为第四参数，为第五参数，为第六参数，为所述第二轨迹位置点和所述第一轨迹位置点之间的第一距离；为所述第三轨迹位置点和所述第一轨迹位置点之间的第二距离；为所述第二轨迹位置点的x轴坐标值，为所述第一轨迹位置点的x轴坐标值，为所述第三轨迹位置点的x轴坐标值，为所述第二轨迹位置点的y轴坐标值，为所述第一轨迹位置点的y轴坐标值，为所述第三轨迹位置点的y轴坐标值。
5.根据权利要求4所述的轨迹跟踪控制方法，其特征在于，步骤S213中的具体步骤包括：S2131.根据以下公式计算所述第一轨迹位置点的曲率：；其中，为所述第一轨迹位置点的曲率。6.根据权利要求5所述的轨迹跟踪控制方法，其特征在于，步骤S22中的具体步骤包括：S221....

【专利技术属性】
技术研发人员：杨宗泉，吕小戈，温志庆，
申请(专利权)人：季华实验室，
类型：发明
国别省市：