差速轮底盘机器人自适应路径跟随方法及其相关设备技术

本申请属于导航技术领域，公开了一种差速轮底盘机器人自适应路径跟随方法及其相关设备，方法包括获取目标路径的路径点的路径点位置，在跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人的机器人全局坐标、偏航角度、线速度和角速度，根据预设的期望线速度和预设的期望前视距离计算跟随路径的总时间，根据前面获得的数据确定四阶贝塞尔曲线形的跟随路径的五个控制点的控制点全局坐标，根据五个控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动；从而能够提高路径跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。转弯性能优势。转弯性能优势。

【技术实现步骤摘要】
差速轮底盘机器人自适应路径跟随方法及其相关设备


[0001]本申请涉及导航
，具体而言，涉及一种差速轮底盘机器人自适应路径跟随方法及其相关设备。

技术介绍

[0002]复合机器人通常由底盘、执行机构和传感器等组合而成，其中底盘用于安装其他构件并驱动机器人整体空间移动，常见的底盘包括阿克曼底盘、差速轮底盘、腿足式底盘以及麦克纳姆轮底盘等。其中差速轮底盘具有良好的载重能力和越野性能，而且相较于阿克曼底盘，由于四轮差速轮底盘依靠轮子转速差实现转弯，能够实现小半径转弯甚至原地掉头，因此灵活性也更加优秀，更加适用于园区巡检机器人。
[0003]为了提高机器人的运动精度，在实际导航过程中，路径规划以及跟随控制的算法需要与底盘的运动学、动力学模型相匹配，来最大的发挥底盘的优势性能。一般地，在规划生成由起始点到目标点的路径后，由路径跟随算法根据路径点的位置和机器人实时位姿生成实时的控制指令来让机器人沿规划的路径运动，实现对规划的路径的跟踪。
[0004]目前常用的路径跟随算法包括：纯跟踪（PurePursuit），Stanley Method前轴控制，PID控制，MPC控制等。其中PurePursuit 是基于阿克曼小车模型提出的，只考虑在位置上的跟随，会导致到达跟随点后小车方向与路径切向不一致，同时对于四轮差速机器人而言，PurePursuit的最小转弯半径与前视距离正相关，若前视距离过小，机器人总体沿路径进行S型行进，前视距离过大则不能将四轮差速机器人可以实现0转弯半径的特性进行发挥。PID控制模型结构简单，调整方便，得到了广泛应用，但在实际应用过程中，如果机器人与路径出现了较大的位置偏差或路径存在转弯半径较小、连续转弯等情况，容易出现实际控制过程中位置震荡，同时PID 不能保证在一定距离或时间范围内一定到达路径点上。MPC控制以优化的方式求解在约束下最优行进路径，由于建立的模型与真实系统存在误差，同时真实系统中存在扰动，因此MPC仅执行输出序列的第一个控制输出，由于MPC求解优化问题计算量较大，因此在机器人系统上以较高频率进行计算会占用大量计算资源，实时性不够好。
[0005]为此，需要寻求一种适用于差速轮底盘机器人（即具有差速轮底盘的机器人）的自适应路径跟随方法，以提高跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。

技术实现思路

[0006]本申请的目的在于提供一种差速轮底盘机器人自适应路径跟随方法及其相关设备，能够提高路径跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。
[0007]第一方面，本申请提供了一种差速轮底盘机器人自适应路径跟随方法，用于对差速轮底盘机器人进行控制；包括步骤：A1.获取目标路径的路径点在世界坐标系下的路径点位置；
A2.若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻所述差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；A3.根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；A4.根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；A5.根据五个所述控制点在世界坐标系下的控制点全局坐标生成跟随路径的显示函数，并根据所述显示函数生成在当前的所述跟随路径更新周期内的每个控制周期的控制指令，以控制所述差速轮底盘机器人运动。
[0008]每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。
[0009]优选地，所述四阶贝塞尔曲线为：；；其中，为归一化时间，为时间，为时刻的路径位置，、、、、为五个所述控制点的控制点坐标，为所述总时间。
[0010]优选地，步骤A4包括：A401.以当前时刻的所述差速轮底盘机器人的位置点作为第一个所述控制点，获取第一个所述控制点在世界坐标系下的控制点全局坐标；A402.根据当前时刻的所述线速度计算第二个所述控制点在世界坐标系下的控制点全局坐标；A403.根据当前时刻的所述角速度计算第三个所述控制点在世界坐标系下的控制点全局坐标；A404.根据第三个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的控制点全局坐标；A405.根据第四个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的控制点全局坐标。
[0011]根据机器人位姿、线速度、角速度计算前三个控制点的位置能够使更新的跟随路径的角速度和线速度连续，通过在规划生成的目标路径上选取后两个控制点，可保证机器人在经过预设的期望前视距离时到达要跟随的路径上，且当后两个控制点都在路径点上时，能保证机器人前进速度方向与目标路径切向方向近似，不会出现S型行进或大幅度震
荡。
[0012]优选地，步骤A402包括：根据以下公式计算第二个所述控制点在机器人坐标系下的控制点局部坐标：；；其中，为第二个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值，为第二个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值，为当前时刻的所述线速度；通过坐标转换把第二个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。
[0013]优选地，步骤A403包括：根据以下公式计算第三个所述控制点在机器人坐标系下的控制点局部坐标：；；其中，为第三个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值，为第三个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值，为当前时刻的所述角速度，为所述期望前视距离；通过坐标转换把第三个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。
[0014]通过上述方式计算第三个控制点的控制点全局坐标，有利于使后续生成的指令速度接近期望速度，避免指令速度过大。
[0015]优选地，步骤A404包括：B1.从前到后依次以各所述路径点为第一备选点，循环执行以下步骤：计算所述第一备选点与第三个所述控制点的第一距离；计算所述目标路径在所述第一备选点处的第一曲率；若，则使第一参考距离等于，若，则使第一参考距离等于；为所述期望前视距离；如果，则停止循环，并执行步骤B2；B2.把最后的所述第一备选点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的所述控制点全局坐标。
[0016]优选地，步骤A405包括：C1.从前到后依次以各所述路径点为第二备选点，循环执行以下步骤：
计算所述第二备选点与第四个所述控制点的第二距离；计算所述目标路径在所述第二备选点处的第二曲率；若，则本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种差速轮底盘机器人自适应路径跟随方法，用于对差速轮底盘机器人进行控制；其特征在于，包括步骤：A1.获取目标路径的路径点在世界坐标系下的路径点位置；A2.若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻所述差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；A3.根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；A4.根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；A5.根据五个所述控制点在世界坐标系下的所述控制点全局坐标生成跟随路径的显示函数，并根据所述显示函数生成在当前的所述跟随路径更新周期内的每个控制周期的控制指令，以控制所述差速轮底盘机器人运动。2.根据权利要求1所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，所述四阶贝塞尔曲线为：；；其中，为归一化时间，为时间，为时刻的路径位置，、、、、为五个所述控制点的控制点坐标，为所述总时间。3.根据权利要求2所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A4包括：A401.以当前时刻的所述差速轮底盘机器人的位置点作为第一个所述控制点，获取第一个所述控制点在世界坐标系下的控制点全局坐标；A402.根据当前时刻的所述线速度计算第二个所述控制点在世界坐标系下的控制点全局坐标；A403.根据当前时刻的所述角速度计算第三个所述控制点在世界坐标系下的控制点全局坐标；A404.根据第三个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的控制点全局坐标；A405.根据第四个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的控制点全局坐标。4.根据权利要求3所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A402包括：根据以下公式计算第二个所述控制点在机器人坐标系下的控制点局部坐标：；
；其中，为第二个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值，为第二个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值，为当前时刻的所述线速度；通过坐标转换把第二个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。5.根据权利要求3所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A403包括：根据以下公式计算第三个所述控制点在机器人坐标系下的控制点局部坐标：；；其中，为第三个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值，为第三个所述控制点在机器人坐标系下的控制点局部坐标...

【专利技术属性】
技术研发人员：安丽，廖郁夫，陈振翔，彭云龙，李伟，王沛，
申请(专利权)人：季华实验室，
类型：发明
国别省市：