CN116149340A - 差速轮底盘机器人自适应路径跟随方法及其相关设备 - Google Patents

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

Info

Publication number
CN116149340A
CN116149340A CN202310439698.6A CN202310439698A CN116149340A CN 116149340 A CN116149340 A CN 116149340A CN 202310439698 A CN202310439698 A CN 202310439698A CN 116149340 A CN116149340 A CN 116149340A
Authority
CN
China
Prior art keywords
path
control point
point
control
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310439698.6A
Other languages
English (en)
Other versions
CN116149340B (zh
Inventor
安丽
廖郁夫
陈振翔
彭云龙
李伟
王沛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ji Hua Laboratory
Original Assignee
Ji Hua Laboratory
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ji Hua Laboratory filed Critical Ji Hua Laboratory
Priority to CN202310439698.6A priority Critical patent/CN116149340B/zh
Publication of CN116149340A publication Critical patent/CN116149340A/zh
Application granted granted Critical
Publication of CN116149340B publication Critical patent/CN116149340B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/60—Other road transportation technologies with climate change mitigation effect
    • Y02T10/72—Electric energy management in electromobility

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

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

Description

差速轮底盘机器人自适应路径跟随方法及其相关设备
技术领域
本申请涉及导航技术领域，具体而言，涉及一种差速轮底盘机器人自适应路径跟随方法及其相关设备。
背景技术
复合机器人通常由底盘、执行机构和传感器等组合而成，其中底盘用于安装其他构件并驱动机器人整体空间移动，常见的底盘包括阿克曼底盘、差速轮底盘、腿足式底盘以及麦克纳姆轮底盘等。其中差速轮底盘具有良好的载重能力和越野性能，而且相较于阿克曼底盘，由于四轮差速轮底盘依靠轮子转速差实现转弯，能够实现小半径转弯甚至原地掉头，因此灵活性也更加优秀，更加适用于园区巡检机器人。
为了提高机器人的运动精度，在实际导航过程中，路径规划以及跟随控制的算法需要与底盘的运动学、动力学模型相匹配，来最大的发挥底盘的优势性能。一般地，在规划生成由起始点到目标点的路径后，由路径跟随算法根据路径点的位置和机器人实时位姿生成实时的控制指令来让机器人沿规划的路径运动，实现对规划的路径的跟踪。
目前常用的路径跟随算法包括：纯跟踪（PurePursuit），Stanley Method前轴控制，PID控制，MPC控制等。其中PurePursuit 是基于阿克曼小车模型提出的，只考虑在位置上的跟随，会导致到达跟随点后小车方向与路径切向不一致，同时对于四轮差速机器人而言，PurePursuit的最小转弯半径与前视距离正相关，若前视距离过小，机器人总体沿路径进行S型行进，前视距离过大则不能将四轮差速机器人可以实现0转弯半径的特性进行发挥。PID控制模型结构简单，调整方便，得到了广泛应用，但在实际应用过程中，如果机器人与路径出现了较大的位置偏差或路径存在转弯半径较小、连续转弯等情况，容易出现实际控制过程中位置震荡，同时PID 不能保证在一定距离或时间范围内一定到达路径点上。MPC控制以优化的方式求解在约束下最优行进路径，由于建立的模型与真实系统存在误差，同时真实系统中存在扰动，因此MPC仅执行输出序列的第一个控制输出，由于MPC求解优化问题计算量较大，因此在机器人系统上以较高频率进行计算会占用大量计算资源，实时性不够好。
为此，需要寻求一种适用于差速轮底盘机器人（即具有差速轮底盘的机器人）的自适应路径跟随方法，以提高跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。
发明内容
本申请的目的在于提供一种差速轮底盘机器人自适应路径跟随方法及其相关设备，能够提高路径跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。
第一方面，本申请提供了一种差速轮底盘机器人自适应路径跟随方法，用于对差速轮底盘机器人进行控制；包括步骤：
A1.获取目标路径的路径点在世界坐标系下的路径点位置；
A2.若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻所述差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；
A3.根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；
A4.根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；
A5.根据五个所述控制点在世界坐标系下的控制点全局坐标生成跟随路径的显示函数，并根据所述显示函数生成在当前的所述跟随路径更新周期内的每个控制周期的控制指令，以控制所述差速轮底盘机器人运动。
每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。
优选地，所述四阶贝塞尔曲线为：
Figure SMS_1
；
Figure SMS_2
；
其中，
Figure SMS_5
为归一化时间，/>
Figure SMS_8
为时间，/>
Figure SMS_11
为/>
Figure SMS_4
时刻的路径位置，/>
Figure SMS_7
、/>
Figure SMS_10
、/>
Figure SMS_12
、/>
Figure SMS_3
、/>
Figure SMS_6
为五个所述控制点的控制点坐标，/>
Figure SMS_9
为所述总时间。
优选地，步骤A4包括：
A401.以当前时刻的所述差速轮底盘机器人的位置点作为第一个所述控制点，获取第一个所述控制点在世界坐标系下的控制点全局坐标；
A402.根据当前时刻的所述线速度计算第二个所述控制点在世界坐标系下的控制点全局坐标；
A403.根据当前时刻的所述角速度计算第三个所述控制点在世界坐标系下的控制点全局坐标；
A404.根据第三个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的控制点全局坐标；
A405.根据第四个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的控制点全局坐标。
根据机器人位姿、线速度、角速度计算前三个控制点的位置能够使更新的跟随路径的角速度和线速度连续，通过在规划生成的目标路径上选取后两个控制点，可保证机器人在经过预设的期望前视距离时到达要跟随的路径上，且当后两个控制点都在路径点上时，能保证机器人前进速度方向与目标路径切向方向近似，不会出现S型行进或大幅度震荡。
优选地，步骤A402包括：
根据以下公式计算第二个所述控制点在机器人坐标系下的控制点局部坐标：
Figure SMS_13
；
Figure SMS_14
；
其中，
Figure SMS_15
为第二个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值，/>
Figure SMS_16
为第二个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值，/>
Figure SMS_17
为当前时刻的所述线速度；
通过坐标转换把第二个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。
优选地，步骤A403包括：
根据以下公式计算第三个所述控制点在机器人坐标系下的控制点局部坐标：
Figure SMS_18
；
Figure SMS_19
；
其中，
Figure SMS_20
为第三个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值，/>
Figure SMS_21
为第三个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值，/>
Figure SMS_22
为当前时刻的所述角速度，/>
Figure SMS_23
为所述期望前视距离；
通过坐标转换把第三个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。
通过上述方式计算第三个控制点的控制点全局坐标，有利于使后续生成的指令速度接近期望速度，避免指令速度过大。
优选地，步骤A404包括：
B1.从前到后依次以各所述路径点为第一备选点，循环执行以下步骤：
计算所述第一备选点与第三个所述控制点的第一距离
Figure SMS_24
；
计算所述目标路径在所述第一备选点处的第一曲率
Figure SMS_25
；
若
Figure SMS_26
，则使第一参考距离/>
Figure SMS_27
等于/>
Figure SMS_28
，若/>
Figure SMS_29
，则使第一参考距离
Figure SMS_30
等于/>
Figure SMS_31
；/>
Figure SMS_32
为所述期望前视距离；
如果
Figure SMS_33
，则停止循环，并执行步骤B2；
B2.把最后的所述第一备选点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的所述控制点全局坐标。
优选地，步骤A405包括：
C1.从前到后依次以各所述路径点为第二备选点，循环执行以下步骤：
计算所述第二备选点与第四个所述控制点的第二距离
Figure SMS_34
；
计算所述目标路径在所述第二备选点处的第二曲率
Figure SMS_35
；
若
Figure SMS_36
，则使第二参考距离/>
Figure SMS_37
等于/>
Figure SMS_38
，若/>
Figure SMS_39
，则使第二参考距离
Figure SMS_40
等于/>
Figure SMS_41
；/>
Figure SMS_42
为所述期望前视距离；
如果
Figure SMS_43
，则停止循环，并执行步骤C2；
C2.把最后的所述第二备选点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的所述控制点全局坐标。
第二方面，本申请提供了一种差速轮底盘机器人自适应路径跟随装置，用于对差速轮底盘机器人进行控制；包括：
第一获取模块，用于获取目标路径的路径点在世界坐标系下的路径点位置；
第二获取模块，用于在当前时刻为跟随路径更新周期开始的时刻的时候，获取当前时刻所述差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；
第一计算模块，用于根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；
控制点选取模块，用于根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；
第一控制模块，用于根据五个所述控制点在世界坐标系下的控制点全局坐标生成跟随路径的显示函数，并根据所述显示函数生成在当前的所述跟随路径更新周期内的每个控制周期的控制指令，以控制所述差速轮底盘机器人运动。
每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。
第三方面，本申请提供了一种电子设备，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如前文所述的差速轮底盘机器人自适应路径跟随方法中的步骤。
第四方面，本申请提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如前文所述的差速轮底盘机器人自适应路径跟随方法中的步骤。
有益效果：本申请提供的差速轮底盘机器人自适应路径跟随方法及其相关设备，每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。
附图说明
图1为本申请实施例提供的差速轮底盘机器人自适应路径跟随方法的流程图。
图2为本申请实施例提供的差速轮底盘机器人自适应路径跟随装置的结构示意图。
图3为本申请实施例提供的电子设备的结构示意图。
图4为世界坐标系与机器人坐标系的示意图。
标号说明：1、第一获取模块；2、第二获取模块；3、第一计算模块；4、控制点选取模块；5、第一控制模块；301、处理器；302、存储器；303、通信总线。
具体实施方式
下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
请参照图1，图1是本申请一些实施例中的一种差速轮底盘机器人自适应路径跟随方法，用于对差速轮底盘机器人进行控制；包括步骤：
A1.获取目标路径的路径点在世界坐标系下的路径点位置；
A2.若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；
A3.根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；
A4.根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；
A5.根据五个控制点在世界坐标系下的控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动。
每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。
其中，目标路径是从起始点通往目标点的路径，可通过现有技术的任意路径规划方法规划得到，再对规划得到的目标路径进行离散处理即得到多个路径点在世界坐标系下的路径点位置。
其中，跟随路径更新周期可根据实际需要设置，一般可取为控制周期的n倍，n为正整数，可根据实际需要设置，从而，每根据最新的跟随路径生成n个控制指令控制差速轮底盘机器人运动后则更新一次跟随路径。
其中，当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度可通过设置在差速轮底盘机器人上的定位模块和传感器实时测得。
其中，跟随路径的总时间实际上是使差速轮底盘机器人从当前位置回到目标路径上所需的期望时间，因此，期望线速度和期望前视距离实际上是对该期望时间的两个约束参数，期望线速度和期望前视距离可根据实际需要设置。
具体地，步骤A3包括：
根据以下公式计算总时间：
Figure SMS_44
；
其中，
Figure SMS_45
为总时间，/>
Figure SMS_46
为期望前视距离，/>
Figure SMS_47
为期望线速度。
在本实施例中，四阶贝塞尔曲线为：
Figure SMS_48
（1）；
Figure SMS_49
；
其中，
Figure SMS_66
为归一化时间，/>
Figure SMS_52
为时间（该时间以当前的跟随路径更新周期开始的时刻为零点），/>
Figure SMS_61
为/>
Figure SMS_57
时刻的路径位置（即跟随路径的路径点的位置），/>
Figure SMS_63
、/>
Figure SMS_65
、/>
Figure SMS_68
、/>
Figure SMS_56
、/>
Figure SMS_60
为五个控制点的控制点坐标（/>
Figure SMS_50
为第一个控制点的控制点坐标，/>
Figure SMS_62
为第二个控制点的控制点坐标，/>
Figure SMS_51
为第三个控制点的控制点坐标，/>
Figure SMS_59
为第四个控制点的控制点坐标，/>
Figure SMS_54
为第五个控制点的控制点坐标）；其中，/>
Figure SMS_58
、/>
Figure SMS_53
、/>
Figure SMS_64
、/>
Figure SMS_67
、/>
Figure SMS_69
、/>
Figure SMS_55
均包括两个横坐标值和纵坐标值。
通过对该公式（1）求导，并使
Figure SMS_70
为0，可得到：
Figure SMS_71
（2）；
Figure SMS_72
（3）；
其中，
Figure SMS_73
为/>
Figure SMS_74
时/>
Figure SMS_75
的一阶导数，/>
Figure SMS_76
为/>
Figure SMS_77
时/>
Figure SMS_78
的二阶导数。
在一些优选实施方式中，步骤A4包括：
A401.以当前时刻的差速轮底盘机器人的位置点作为第一个控制点，获取第一个控制点在世界坐标系下的控制点全局坐标；
A402.根据当前时刻的线速度计算第二个控制点在世界坐标系下的控制点全局坐标；
A403.根据当前时刻的角速度计算第三个控制点在世界坐标系下的控制点全局坐标；
A404.根据第三个控制点的控制点全局坐标，确定目标路径的一个路径点作为第四个控制点，获取第四个控制点在世界坐标系下的控制点全局坐标；
A405.根据第四个控制点的控制点全局坐标，确定目标路径的一个路径点作为第五个控制点，获取第五个控制点在世界坐标系下的控制点全局坐标。
根据机器人位姿（包括机器人全局坐标和偏航角度）、线速度、角速度计算前三个控制点的位置能够使更新的跟随路径的角速度和线速度连续，通过在规划生成的目标路径上选取后两个控制点，可保证机器人在经过预设的期望前视距离时到达要跟随的路径上，且当后两个控制点都在路径点上时，能保证机器人前进速度方向与目标路径切向方向近似，不会出现S型行进或大幅度震荡。
参考图4，在跟随路径更新周期开始的时刻，机器人坐标系B相对世界坐标系w具有一个位置偏移量（即两个坐标系原点之间的位置偏差）和一个旋转角度（即两个坐标系的横坐标之间的夹角）；其中机器人坐标系B的原点为差速轮底盘机器人的位置点（也是更新后的跟随路径的起始点），因此该位置偏移量实际上等于差速轮底盘机器人在世界坐标系下的机器人全局坐标；其中机器人坐标系B的横坐标正方向为差速轮底盘机器人的线速度方向，因此该旋转角度等于差速轮底盘机器人的线速度方向与机器人坐标系B的横坐标正方向之间的夹角，该夹角即为在跟随路径更新周期开始的时刻的差速轮底盘机器人在世界坐标系下的偏航角度。
在步骤A401中，当前时刻（此时为跟随路径更新周期开始的时刻）的差速轮底盘机器人的位置点（也是更新后的跟随路径的起始点）即当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标对应的位置点，因此，第一个控制点在世界坐标系下的控制点全局坐标等于当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标。见图4，此时的第一个控制点
Figure SMS_79
为机器人坐标系B的原点。
在本实施例中，步骤A402包括：
根据以下公式计算第二个控制点在机器人坐标系下的控制点局部坐标：
Figure SMS_80
（4）；
Figure SMS_81
（5）；
其中，
Figure SMS_82
为第二个控制点在机器人坐标系下的控制点局部坐标的横坐标值，
Figure SMS_83
为第二个控制点在机器人坐标系下的控制点局部坐标的纵坐标值，/>
Figure SMS_84
为当前时刻的线速度；
通过坐标转换把第二个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标。
根据公式（2）有：
Figure SMS_85
；
其中，
Figure SMS_88
为/>
Figure SMS_92
时/>
Figure SMS_95
的一阶导数，/>
Figure SMS_86
为机器人坐标系下的/>
Figure SMS_93
时刻的路径位置，/>
Figure SMS_97
为第二个控制点的在机器人坐标系下的控制点局部坐标，/>
Figure SMS_100
为/>
Figure SMS_87
的横坐标值，/>
Figure SMS_91
为/>
Figure SMS_96
的纵坐标值，/>
Figure SMS_99
为第一个控制点的在机器人坐标系下的控制点局部坐标，/>
Figure SMS_89
为/>
Figure SMS_90
的横坐标值，/>
Figure SMS_94
为/>
Figure SMS_98
的纵坐标值；
由于第一个控制点
Figure SMS_101
为机器人坐标系的原点，因此/>
Figure SMS_102
和/>
Figure SMS_103
均为零，又由于机器人坐标系的横坐标正方向为差速轮底盘机器人的当前时刻的线速度方向，因此有
Figure SMS_104
，/>
Figure SMS_105
为当前时刻差速轮底盘机器人的线速度，所以上式转换为：
Figure SMS_106
（6）；
根据该公式（6）即可得到公式（4）和（5）；从而，第二个控制点实际上为当前时刻的机器人坐标系横坐标上的点，且该第二个控制点与机器人坐标系原点的距离等于差速轮底盘机器人当前时刻的线速度大小（如图4所示）。
其中，可通过以下公式把第二个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标：
Figure SMS_107
；
其中，
Figure SMS_108
为第二个控制点在世界坐标系下的控制点全局坐标，/>
Figure SMS_109
为第一个控制点在世界坐标系下的控制点全局坐标（等于当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标），/>
Figure SMS_110
为机器人坐标系和世界坐标系之间的转换矩阵，且有：
Figure SMS_111
；/>
Figure SMS_112
为机器人坐标系相对世界坐标系的旋转角度（等于当前时刻差速轮底盘机器人在世界坐标系下的偏航角度）。
在本实施例中，步骤A403包括：
根据以下公式计算第三个控制点在机器人坐标系下的控制点局部坐标：
Figure SMS_113
（7）；
Figure SMS_114
（8）；
其中，
Figure SMS_115
为第三个控制点在机器人坐标系下的控制点局部坐标的横坐标值，
Figure SMS_116
为第三个控制点在机器人坐标系下的控制点局部坐标的纵坐标值，/>
Figure SMS_117
为当前时刻的角速度；
通过坐标转换把第三个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标。
通过上述方式计算第三个控制点的控制点全局坐标，有利于使后续生成的指令速度（即线速度指令对应的速度）接近期望速度，避免指令速度过大。
根据公式（2）、（3）、（6）有：
Figure SMS_118
（9）；
其中，
Figure SMS_121
为/>
Figure SMS_123
时/>
Figure SMS_125
的二阶导数，/>
Figure SMS_120
为/>
Figure SMS_122
的横坐标值，/>
Figure SMS_124
为/>
Figure SMS_126
的纵坐标值，/>
Figure SMS_119
为第三个控制点在机器人坐标系下的控制点局部坐标。
根据曲线曲率计算公式、角速度计算公式和公式（6）、（9）可以得到：
Figure SMS_127
（10）；
其中，
Figure SMS_129
为/>
Figure SMS_132
时机器人坐标系下的角速度，/>
Figure SMS_135
为/>
Figure SMS_130
的横坐标值，/>
Figure SMS_133
为/>
Figure SMS_136
的纵坐标值，/>
Figure SMS_137
为/>
Figure SMS_128
的横坐标值，/>
Figure SMS_131
为/>
Figure SMS_134
的纵坐标值。
根据该公式（10）可以得到公式（7）。
通过试验证明，当采用公式（8）计算第三个控制点在机器人坐标系下的控制点局部坐标的横坐标值时，有利于使后续生成的指令速度接近期望速度。
其中，可通过以下公式把第三个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标：
Figure SMS_138
；
其中，
Figure SMS_139
为第三个控制点在世界坐标系下的控制点全局坐标，/>
Figure SMS_140
为第三个控制点在机器人坐标系下的控制点局部坐标。图4中，/>
Figure SMS_141
点为第三个控制点。
在本实施例中，步骤A404包括：
B1.从前到后依次以各路径点为第一备选点，循环执行以下步骤：
计算第一备选点与第三个控制点的第一距离
Figure SMS_142
（此处为了便于与其它距离做区分，把第一备选点与第三个控制点的距离称为第一距离）；
计算目标路径在第一备选点处的第一曲率
Figure SMS_143
（此处为了便于与其它曲率做区分，把目标路径在第一备选点处的曲率称为第一曲率）；
若
Figure SMS_144
，则使第一参考距离/>
Figure SMS_145
等于/>
Figure SMS_146
，若/>
Figure SMS_147
，则使第一参考距离
Figure SMS_148
等于/>
Figure SMS_149
；
如果
Figure SMS_150
，则停止循环，并执行步骤B2；
B2.把最后的第一备选点作为第四个控制点，获取第四个控制点在世界坐标系下的控制点全局坐标。
其中，两个点之间的距离和路径在某一路径点上的曲率的计算方法为现有技术，此处不对其进行详述。
具体地，先把第一个路径点作为第一备选点，然后计算该第一备选点对应的
Figure SMS_151
和/>
Figure SMS_152
，然后确定/>
Figure SMS_153
值，若满足/>
Figure SMS_154
，则把第一个路径点作为第四个控制点，否则把下一个路径点作为第一备选点，然后重复上述步骤，如此循环，直到/>
Figure SMS_155
，假设在把第k个路径点作为第一备选点时满足该条件，则把第k个路径点作为第四个控制点，并提取该路径点在世界坐标系下的路径点位置作为第四个控制点在世界坐标系下的控制点全局坐标。
在本实施例中，步骤A405包括：
C1.从前到后依次以各路径点为第二备选点，循环执行以下步骤：
计算第二备选点与第四个控制点的第二距离
Figure SMS_156
（此处为了便于与其它距离做区分，把第二备选点与第四个控制点的距离称为第二距离）；
计算目标路径在第二备选点处的第二曲率
Figure SMS_157
（此处为了便于与其它曲率做区分，把目标路径在第二备选点处的曲率称为第二曲率）；
若
Figure SMS_158
，则使第二参考距离/>
Figure SMS_159
等于/>
Figure SMS_160
，若/>
Figure SMS_161
，则使第二参考距离
Figure SMS_162
等于/>
Figure SMS_163
；
如果
Figure SMS_164
，则停止循环，并执行步骤C2；
C2.把最后的第二备选点作为第五个控制点，获取第五个控制点在世界坐标系下的控制点全局坐标。
具体地，先把第一个路径点作为第二备选点，然后计算该第二备选点对应的
Figure SMS_165
和
Figure SMS_166
，然后确定/>
Figure SMS_167
值，若满足/>
Figure SMS_168
，则把第一个路径点作为第五个控制点，否则把下一个路径点作为第二备选点，然后重复上述步骤，如此循环，直到/>
Figure SMS_169
，假设在把第k个路径点作为第二备选点时满足该条件，则把第k个路径点作为第五个控制点，并提取该路径点在世界坐标系下的路径点位置作为第五个控制点在世界坐标系下的控制点全局坐标。
步骤A5中，根据公式（1）生成跟随路径的显示函数，该显示函数为：
Figure SMS_170
；
其中，
Figure SMS_171
为世界坐标系下/>
Figure SMS_172
时刻的路径位置，/>
Figure SMS_173
、/>
Figure SMS_174
、/>
Figure SMS_175
、/>
Figure SMS_176
、/>
Figure SMS_177
为五个控制点在世界坐标系下的控制点全局坐标。
其中，步骤A5中生成的控制指令包括线速度指令和角速度指令。
在生成当前的跟随路径更新周期内的各个控制周期的控制指令时，根据各控制周期的时间
Figure SMS_178
（该时间以当前的跟随路径更新周期开始的时刻为零点）计算归一化时间/>
Figure SMS_179
，其中，计算公式为：
Figure SMS_180
；
然后在各控制周期的归一化时间
Figure SMS_181
处对显示函数进行链式求导和曲线微分计算（具体计算过程为现有技术，此处不对其进行详述），可得到各控制周期的线速度指令和角速度指令。
例如，若每个跟随路径更新周期等于n个控制周期，每个控制周期为△T，则生成t=0、t=△T、t=2△T、...、t=（n-1）△T共n个时刻的控制指令，分别用于在t=0、t=△T、t=2△T、...、t=（n-1）时刻作为控制输入来控制差速轮底盘机器人运动。从而，由于每次更新跟随路径后只根据更新后的跟随路径进行n个控制指令的生成和使用，即再次进行跟随路径更新，如此循环，当工作过程中受到干扰，也能够有效减小该干扰对跟随效果的影响。
实际上，当差速轮底盘机器人移动到接近目标点（即目标路径的终点）的时候容易出现搜索出的第四个控制点和第五个控制点重合或非常接近的情况，此时，若继续依据四阶贝塞尔曲线形的跟随路径生成控制指令以控制差速轮底盘机器人运动，会导致差速轮底盘机器人绕圈接近目标点，从而延迟到达目标点的时间。为了避免差速轮底盘机器人最后绕圈接近目标点，在一些优选实施方式中，步骤A4之后和步骤A5之前还包括步骤：
A6.计算第四个控制点和第五个控制点之间的控制点距离；
A7.若该控制点距离不小于预设的距离阈值（可根据实际需要设置），则执行步骤A5；
A8.若该控制点距离小于预设的距离阈值，则令差速轮底盘机器人原地旋转至正前方对准目标点后控制差速轮底盘机器人沿直线移动至该目标点。
由上可知，该差速轮底盘机器人自适应路径跟随方法，通过获取目标路径的路径点在世界坐标系下的路径点位置，若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度，根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间，根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标，根据五个控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动；从而能够提高路径跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。
参考图2，本申请提供了一种差速轮底盘机器人自适应路径跟随装置，用于对差速轮底盘机器人进行控制；包括：
第一获取模块1，用于获取目标路径的路径点在世界坐标系下的路径点位置；
第二获取模块2，用于在当前时刻为跟随路径更新周期开始的时刻的时候，获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；
第一计算模块3，用于根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；
控制点选取模块4，用于根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；
第一控制模块5，用于根据五个控制点在世界坐标系下的控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动。
每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。
其中，目标路径是从起始点通往目标点的路径，可通过现有技术的任意路径规划方法规划得到，再对规划得到的目标路径进行离散处理即得到多个路径点在世界坐标系下的路径点位置。
其中，跟随路径更新周期可根据实际需要设置，一般可取为控制周期的n倍，n为正整数，可根据实际需要设置，从而，每根据最新的跟随路径生成n个控制指令控制差速轮底盘机器人运动后则更新一次跟随路径。
其中，当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度可通过设置在差速轮底盘机器人上的定位模块和传感器实时测得。
其中，跟随路径的总时间实际上是使差速轮底盘机器人从当前位置回到目标路径上所需的期望时间，因此，期望线速度和期望前视距离实际上是对该期望时间的两个约束参数，期望线速度和期望前视距离可根据实际需要设置。
具体地，第一计算模块3在根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间的时候，执行：
根据以下公式计算总时间：
Figure SMS_182
；
其中，
Figure SMS_183
为总时间，/>
Figure SMS_184
为期望前视距离，/>
Figure SMS_185
为期望线速度。
在本实施例中，四阶贝塞尔曲线为：
Figure SMS_186
（1）；
Figure SMS_187
；
其中，
Figure SMS_194
为归一化时间，/>
Figure SMS_192
为时间（该时间以当前的跟随路径更新周期开始的时刻为零点），/>
Figure SMS_201
为/>
Figure SMS_193
时刻的路径位置（即跟随路径的路径点的位置），/>
Figure SMS_203
、/>
Figure SMS_195
、/>
Figure SMS_197
、/>
Figure SMS_189
、/>
Figure SMS_200
为五个控制点的控制点坐标（/>
Figure SMS_188
为第一个控制点的控制点坐标，/>
Figure SMS_196
为第二个控制点的控制点坐标，/>
Figure SMS_191
为第三个控制点的控制点坐标，/>
Figure SMS_199
为第四个控制点的控制点坐标，/>
Figure SMS_202
为第五个控制点的控制点坐标）；其中，/>
Figure SMS_206
、/>
Figure SMS_198
、/>
Figure SMS_205
、/>
Figure SMS_204
、/>
Figure SMS_207
、/>
Figure SMS_190
均包括两个横坐标值和纵坐标值。
通过对该公式（1）求导，并使
Figure SMS_208
为0，可得到：
Figure SMS_209
（2）；
Figure SMS_210
（3）；/>
其中，
Figure SMS_211
为/>
Figure SMS_212
时/>
Figure SMS_213
的一阶导数，/>
Figure SMS_214
为/>
Figure SMS_215
时/>
Figure SMS_216
的二阶导数。
在一些优选实施方式中，控制点选取模块4在根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标的时候，执行：
以当前时刻的差速轮底盘机器人的位置点作为第一个控制点，获取第一个控制点在世界坐标系下的控制点全局坐标；
根据当前时刻的线速度计算第二个控制点在世界坐标系下的控制点全局坐标；
根据当前时刻的角速度计算第三个控制点在世界坐标系下的控制点全局坐标；
根据第三个控制点的控制点全局坐标，确定目标路径的一个路径点作为第四个控制点，获取第四个控制点在世界坐标系下的控制点全局坐标；
根据第四个控制点的控制点全局坐标，确定目标路径的一个路径点作为第五个控制点，获取第五个控制点在世界坐标系下的控制点全局坐标。
根据机器人位姿（包括机器人全局坐标和偏航角度）、线速度、角速度计算前三个控制点的位置能够使更新的跟随路径的角速度和线速度连续，通过在规划生成的目标路径上选取后两个控制点，可保证机器人在经过预设的期望前视距离时到达要跟随的路径上，且当后两个控制点都在路径点上时，能保证机器人前进速度方向与目标路径切向方向近似，不会出现S型行进或大幅度震荡。
参考图4，在跟随路径更新周期开始的时刻，机器人坐标系B相对世界坐标系w具有一个位置偏移量（即两个坐标系原点之间的位置偏差）和一个旋转角度（即两个坐标系的横坐标之间的夹角）；其中机器人坐标系B的原点为差速轮底盘机器人的位置点（也是更新后的跟随路径的起始点），因此该位置偏移量实际上等于差速轮底盘机器人在世界坐标系下的机器人全局坐标；其中机器人坐标系B的横坐标正方向为差速轮底盘机器人的线速度方向，因此该旋转角度等于差速轮底盘机器人的线速度方向与机器人坐标系B的横坐标正方向之间的夹角，该夹角即为在跟随路径更新周期开始的时刻的差速轮底盘机器人在世界坐标系下的偏航角度。
其中，当前时刻（此时为跟随路径更新周期开始的时刻）的差速轮底盘机器人的位置点（也是更新后的跟随路径的起始点）即当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标对应的位置点，因此，第一个控制点在世界坐标系下的控制点全局坐标等于当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标。见图4，此时的第一个控制点
Figure SMS_217
为机器人坐标系B的原点。
在本实施例中，控制点选取模块4在根据当前时刻的线速度计算第二个控制点在世界坐标系下的控制点全局坐标的时候，执行：
根据以下公式计算第二个控制点在机器人坐标系下的控制点局部坐标：
Figure SMS_218
（4）；
Figure SMS_219
（5）；
其中，
Figure SMS_220
为第二个控制点在机器人坐标系下的控制点局部坐标的横坐标值，/>
Figure SMS_221
为第二个控制点在机器人坐标系下的控制点局部坐标的纵坐标值，/>
Figure SMS_222
为当前时刻的线速度；
通过坐标转换把第二个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标。
根据公式（2）有：
Figure SMS_223
；
其中，
Figure SMS_224
为/>
Figure SMS_231
时/>
Figure SMS_235
的一阶导数，/>
Figure SMS_227
为机器人坐标系下的/>
Figure SMS_229
时刻的路径位置，/>
Figure SMS_233
为第二个控制点的在机器人坐标系下的控制点局部坐标，/>
Figure SMS_237
为/>
Figure SMS_225
的横坐标值，/>
Figure SMS_230
为/>
Figure SMS_234
的纵坐标值，/>
Figure SMS_238
为第一个控制点的在机器人坐标系下的控制点局部坐标，/>
Figure SMS_226
为/>
Figure SMS_228
的横坐标值，/>
Figure SMS_232
为/>
Figure SMS_236
的纵坐标值；
由于第一个控制点
Figure SMS_239
为机器人坐标系的原点，因此/>
Figure SMS_240
和/>
Figure SMS_241
均为零，又由于机器人坐标系的横坐标正方向为差速轮底盘机器人的当前时刻的线速度方向，因此有
Figure SMS_242
，/>
Figure SMS_243
为当前时刻差速轮底盘机器人的线速度，所以上式转换为：
Figure SMS_244
（6）；
根据该公式（6）即可得到公式（4）和（5）；从而，第二个控制点实际上为当前时刻的机器人坐标系横坐标上的点，且该第二个控制点与机器人坐标系原点的距离等于差速轮底盘机器人当前时刻的线速度大小（如图4所示）。
其中，可通过以下公式把第二个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标：
Figure SMS_245
；
其中，
Figure SMS_246
为第二个控制点在世界坐标系下的控制点全局坐标，/>
Figure SMS_247
为第一个控制点在世界坐标系下的控制点全局坐标（等于当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标），/>
Figure SMS_248
为机器人坐标系和世界坐标系之间的转换矩阵，且有：
Figure SMS_249
；/>
Figure SMS_250
为机器人坐标系相对世界坐标系的旋转角度（等于当前时刻差速轮底盘机器人在世界坐标系下的偏航角度）。
在本实施例中，控制点选取模块4在根据当前时刻的角速度计算第三个控制点在世界坐标系下的控制点全局坐标的时候，执行：
根据以下公式计算第三个控制点在机器人坐标系下的控制点局部坐标：
Figure SMS_251
（7）；
Figure SMS_252
（8）；
其中，
Figure SMS_253
为第三个控制点在机器人坐标系下的控制点局部坐标的横坐标值，
Figure SMS_254
为第三个控制点在机器人坐标系下的控制点局部坐标的纵坐标值，/>
Figure SMS_255
为当前时刻的角速度；
通过坐标转换把第三个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标。
通过上述方式计算第三个控制点的控制点全局坐标，有利于使后续生成的指令速度接近期望速度，避免指令速度过大。
根据公式（2）、（3）、（6）有：
Figure SMS_256
（9）；
其中，
Figure SMS_259
为/>
Figure SMS_261
时/>
Figure SMS_263
的二阶导数，/>
Figure SMS_258
为/>
Figure SMS_260
的横坐标值，/>
Figure SMS_262
为/>
Figure SMS_264
的纵坐标值，/>
Figure SMS_257
为第三个控制点在机器人坐标系下的控制点局部坐标。
根据曲线曲率计算公式、角速度计算公式和公式（6）、（9）可以得到：
Figure SMS_265
（10）；
其中，
Figure SMS_267
为/>
Figure SMS_271
时机器人坐标系下的角速度，/>
Figure SMS_274
为/>
Figure SMS_268
的横坐标值，/>
Figure SMS_270
为/>
Figure SMS_273
的纵坐标值，/>
Figure SMS_275
为/>
Figure SMS_266
的横坐标值，/>
Figure SMS_269
为/>
Figure SMS_272
的纵坐标值。
根据该公式（10）可以得到公式（7）。
通过试验证明，当采用公式（8）计算第三个控制点在机器人坐标系下的控制点局部坐标的横坐标值时，有利于使后续生成的指令速度接近期望速度。
其中，可通过以下公式把第三个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标：
Figure SMS_276
；
其中，
Figure SMS_277
为第三个控制点在世界坐标系下的控制点全局坐标，/>
Figure SMS_278
为第三个控制点在机器人坐标系下的控制点局部坐标。图4中，/>
Figure SMS_279
点为第三个控制点。/>
在本实施例中，控制点选取模块4在根据第三个控制点的控制点全局坐标，确定目标路径的一个路径点作为第四个控制点，获取第四个控制点在世界坐标系下的控制点全局坐标的时候，执行：
B1.从前到后依次以各路径点为第一备选点，循环执行以下步骤：
计算第一备选点与第三个控制点的第一距离
Figure SMS_280
（此处为了便于与其它距离做区分，把第一备选点与第三个控制点的距离称为第一距离）；
计算目标路径在第一备选点处的第一曲率
Figure SMS_281
（此处为了便于与其它曲率做区分，把目标路径在第一备选点处的曲率称为第一曲率）；
若
Figure SMS_282
，则使第一参考距离/>
Figure SMS_283
等于/>
Figure SMS_284
，若/>
Figure SMS_285
，则使第一参考距离
Figure SMS_286
等于/>
Figure SMS_287
；
如果
Figure SMS_288
，则停止循环，并执行步骤B2；
B2.把最后的第一备选点作为第四个控制点，获取第四个控制点在世界坐标系下的控制点全局坐标。
其中，两个点之间的距离和路径在某一路径点上的曲率的计算方法为现有技术，此处不对其进行详述。
具体地，先把第一个路径点作为第一备选点，然后计算该第一备选点对应的
Figure SMS_289
和/>
Figure SMS_290
，然后确定/>
Figure SMS_291
值，若满足/>
Figure SMS_292
，则把第一个路径点作为第四个控制点，否则把下一个路径点作为第一备选点，然后重复上述步骤，如此循环，直到/>
Figure SMS_293
，假设在把第k个路径点作为第一备选点时满足该条件，则把第k个路径点作为第四个控制点，并提取该路径点在世界坐标系下的路径点位置作为第四个控制点在世界坐标系下的控制点全局坐标。
在本实施例中，控制点选取模块4在根据第四个控制点的控制点全局坐标，确定目标路径的一个路径点作为第五个控制点，获取第五个控制点在世界坐标系下的控制点全局坐标的时候，执行：
C1.从前到后依次以各路径点为第二备选点，循环执行以下步骤：
计算第二备选点与第四个控制点的第二距离
Figure SMS_294
（此处为了便于与其它距离做区分，把第二备选点与第四个控制点的距离称为第二距离）；
计算目标路径在第二备选点处的第二曲率
Figure SMS_295
（此处为了便于与其它曲率做区分，把目标路径在第二备选点处的曲率称为第二曲率）；
若
Figure SMS_296
，则使第二参考距离/>
Figure SMS_297
等于/>
Figure SMS_298
，若/>
Figure SMS_299
，则使第二参考距离
Figure SMS_300
等于/>
Figure SMS_301
；
如果
Figure SMS_302
，则停止循环，并执行步骤C2；
C2.把最后的第二备选点作为第五个控制点，获取第五个控制点在世界坐标系下的控制点全局坐标。
具体地，先把第一个路径点作为第二备选点，然后计算该第二备选点对应的
Figure SMS_303
和/>
Figure SMS_304
，然后确定/>
Figure SMS_305
值，若满足/>
Figure SMS_306
，则把第一个路径点作为第五个控制点，否则把下一个路径点作为第二备选点，然后重复上述步骤，如此循环，直到/>
Figure SMS_307
，假设在把第k个路径点作为第二备选点时满足该条件，则把第k个路径点作为第五个控制点，并提取该路径点在世界坐标系下的路径点位置作为第五个控制点在世界坐标系下的控制点全局坐标。
第一控制模块5根据公式（1）生成跟随路径的显示函数，该显示函数为：
Figure SMS_308
；
其中，
Figure SMS_309
为世界坐标系下/>
Figure SMS_310
时刻的路径位置，/>
Figure SMS_311
、/>
Figure SMS_312
、/>
Figure SMS_313
、/>
Figure SMS_314
、/>
Figure SMS_315
为五个控制点在世界坐标系下的控制点全局坐标。
其中，第一控制模块5生成的控制指令包括线速度指令和角速度指令。
在生成当前的跟随路径更新周期内的各个控制周期的控制指令时，根据各控制周期的时间
Figure SMS_316
（该时间以当前的跟随路径更新周期开始的时刻为零点）计算归一化时间/>
Figure SMS_317
，其中，计算公式为：
Figure SMS_318
；
然后在各控制周期的归一化时间
Figure SMS_319
处对显示函数进行链式求导和曲线微分计算（具体计算过程为现有技术，此处不对其进行详述），可得到各控制周期的线速度指令和角速度指令。
例如，若每个跟随路径更新周期等于n个控制周期，每个控制周期为△T，则生成t=0、t=△T、t=2△T、...、t=（n-1）△T共n个时刻的控制指令，分别用于在t=0、t=△T、t=2△T、...、t=（n-1）时刻作为控制输入来控制差速轮底盘机器人运动。从而，由于每次更新跟随路径后只根据更新后的跟随路径进行n个控制指令的生成和使用，即再次进行跟随路径更新，如此循环，当工作过程中受到干扰，也能够有效减小该干扰对跟随效果的影响。
实际上，当差速轮底盘机器人移动到接近目标点（即目标路径的终点）的时候容易出现搜索出的第四个控制点和第五个控制点重合或非常接近的情况，此时，若继续依据四阶贝塞尔曲线形的跟随路径生成控制指令以控制差速轮底盘机器人运动，会导致差速轮底盘机器人绕圈接近目标点，从而延迟到达目标点的时间。为了避免差速轮底盘机器人最后绕圈接近目标点，在一些优选实施方式中，该差速轮底盘机器人自适应路径跟随装置还包括：
第二计算模块，用于计算第四个控制点和第五个控制点之间的控制点距离；
第一执行模块，用于在该控制点距离不小于预设的距离阈值（可根据实际需要设置）时，令第一控制模块执行其功能；
第二执行模块，用于在该控制点距离小于预设的距离阈值时，令差速轮底盘机器人原地旋转至正前方对准目标点后控制差速轮底盘机器人沿直线移动至该目标点。
由上可知，该差速轮底盘机器人自适应路径跟随装置，通过获取目标路径的路径点在世界坐标系下的路径点位置，若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度，根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间，根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标，根据五个控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动；从而能够提高路径跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。
请参照图3，图3为本申请实施例提供的一种电子设备的结构示意图，本申请提供一种电子设备，包括：处理器301和存储器302，处理器301和存储器302通过通信总线303和/或其他形式的连接机构（未标出）互连并相互通讯，存储器302存储有处理器301可执行的计算机程序，当电子设备运行时，处理器301执行该计算机程序，以执行上述实施例的任一可选的实现方式中的差速轮底盘机器人自适应路径跟随方法，以实现以下功能：获取目标路径的路径点在世界坐标系下的路径点位置，若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度，根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间，根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标，根据五个控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动。
本申请实施例提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时，执行上述实施例的任一可选的实现方式中的差速轮底盘机器人自适应路径跟随方法，以实现以下功能：获取目标路径的路径点在世界坐标系下的路径点位置，若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度，根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间，根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标，根据五个控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动。其中，计算机可读存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（Static Random Access Memory, 简称SRAM），电可擦除可编程只读存储器（Electrically Erasable Programmable Read-Only Memory,简称EEPROM），可擦除可编程只读存储器（Erasable Programmable Read Only Memory, 简称EPROM），可编程只读存储器（Programmable Red-Only Memory, 简称PROM），只读存储器（Read-Only Memory, 简称ROM），磁存储器，快闪存储器，磁盘或光盘。
在本申请所提供的实施例中，应该理解到，所揭露装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
另外，作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，既可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
再者，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。
在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。
以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (10)

1.一种差速轮底盘机器人自适应路径跟随方法，用于对差速轮底盘机器人进行控制；其特征在于，包括步骤：
A1.获取目标路径的路径点在世界坐标系下的路径点位置；
A2.若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻所述差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；
A3.根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；
A4.根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；
A5.根据五个所述控制点在世界坐标系下的所述控制点全局坐标生成跟随路径的显示函数，并根据所述显示函数生成在当前的所述跟随路径更新周期内的每个控制周期的控制指令，以控制所述差速轮底盘机器人运动。
2.根据权利要求1所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，所述四阶贝塞尔曲线为：
Figure QLYQS_1
；
Figure QLYQS_2
；
其中，
Figure QLYQS_3
为归一化时间，/>
Figure QLYQS_6
为时间，/>
Figure QLYQS_9
为/>
Figure QLYQS_4
时刻的路径位置，/>
Figure QLYQS_7
、/>
Figure QLYQS_10
、/>
Figure QLYQS_12
、/>
Figure QLYQS_5
、/>
Figure QLYQS_8
为五个所述控制点的控制点坐标，/>
Figure QLYQS_11
为所述总时间。
3.根据权利要求2所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A4包括：
A401.以当前时刻的所述差速轮底盘机器人的位置点作为第一个所述控制点，获取第一个所述控制点在世界坐标系下的控制点全局坐标；
A402.根据当前时刻的所述线速度计算第二个所述控制点在世界坐标系下的控制点全局坐标；
A403.根据当前时刻的所述角速度计算第三个所述控制点在世界坐标系下的控制点全局坐标；
A404.根据第三个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的控制点全局坐标；
A405.根据第四个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的控制点全局坐标。
4.根据权利要求3所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A402包括：
根据以下公式计算第二个所述控制点在机器人坐标系下的控制点局部坐标：
Figure QLYQS_13
；/>
Figure QLYQS_14
；
其中，
Figure QLYQS_15
为第二个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值，
Figure QLYQS_16
为第二个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值，/>
Figure QLYQS_17
为当前时刻的所述线速度；
通过坐标转换把第二个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。
5.根据权利要求3所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A403包括：
根据以下公式计算第三个所述控制点在机器人坐标系下的控制点局部坐标：
Figure QLYQS_18
；
Figure QLYQS_19
；
其中，
Figure QLYQS_20
为第三个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值，
Figure QLYQS_21
为第三个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值，/>
Figure QLYQS_22
为当前时刻的所述角速度，/>
Figure QLYQS_23
为所述期望前视距离；
通过坐标转换把第三个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。
6.根据权利要求3所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A404包括：
B1.从前到后依次以各所述路径点为第一备选点，循环执行以下步骤：
计算所述第一备选点与第三个所述控制点的第一距离
Figure QLYQS_24
；
计算所述目标路径在所述第一备选点处的第一曲率
Figure QLYQS_25
；
若
Figure QLYQS_26
，则使第一参考距离/>
Figure QLYQS_27
等于/>
Figure QLYQS_28
，若/>
Figure QLYQS_29
，则使第一参考距离/>
Figure QLYQS_30
等于/>
Figure QLYQS_31
；/>
Figure QLYQS_32
为所述期望前视距离；
如果
Figure QLYQS_33
，则停止循环，并执行步骤B2；
B2.把最后的所述第一备选点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的所述控制点全局坐标。
7.根据权利要求3所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A405包括：
C1.从前到后依次以各所述路径点为第二备选点，循环执行以下步骤：
计算所述第二备选点与第四个所述控制点的第二距离
Figure QLYQS_34
；
计算所述目标路径在所述第二备选点处的第二曲率
Figure QLYQS_35
；
若
Figure QLYQS_36
，则使第二参考距离/>
Figure QLYQS_37
等于/>
Figure QLYQS_38
，若/>
Figure QLYQS_39
，则使第二参考距离/>
Figure QLYQS_40
等于/>
Figure QLYQS_41
；/>
Figure QLYQS_42
为所述期望前视距离；
如果
Figure QLYQS_43
，则停止循环，并执行步骤C2；
C2.把最后的所述第二备选点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的所述控制点全局坐标。
8.一种差速轮底盘机器人自适应路径跟随装置，用于对差速轮底盘机器人进行控制；其特征在于，包括：
第一获取模块，用于获取目标路径的路径点在世界坐标系下的路径点位置；
第二获取模块，用于在当前时刻为跟随路径更新周期开始的时刻的时候，获取当前时刻所述差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；
第一计算模块，用于根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；
控制点选取模块，用于根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；
第一控制模块，用于根据五个所述控制点在世界坐标系下的所述控制点全局坐标生成跟随路径的显示函数，并根据所述显示函数生成在当前的所述跟随路径更新周期内的每个控制周期的控制指令，以控制所述差速轮底盘机器人运动。
9.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如权利要求1-7任一项所述的差速轮底盘机器人自适应路径跟随方法中的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时运行如权利要求1-7任一项所述的差速轮底盘机器人自适应路径跟随方法中的步骤。
CN202310439698.6A 2023-04-23 2023-04-23 差速轮底盘机器人自适应路径跟随方法及其相关设备 Active CN116149340B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310439698.6A CN116149340B (zh) 2023-04-23 2023-04-23 差速轮底盘机器人自适应路径跟随方法及其相关设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310439698.6A CN116149340B (zh) 2023-04-23 2023-04-23 差速轮底盘机器人自适应路径跟随方法及其相关设备

Publications (2)

Publication Number Publication Date
CN116149340A true CN116149340A (zh) 2023-05-23
CN116149340B CN116149340B (zh) 2023-06-30

Family

ID=86354789

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310439698.6A Active CN116149340B (zh) 2023-04-23 2023-04-23 差速轮底盘机器人自适应路径跟随方法及其相关设备

Country Status (1)

Country Link
CN (1) CN116149340B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2002073150A2 (en) * 2001-03-13 2002-09-19 Ab Skf System and method for analyzing vibration signals
US20060052901A1 (en) * 2004-09-02 2006-03-09 Fanuc Ltd Robot interference prevention control device
CN113720345A (zh) * 2021-08-31 2021-11-30 北京轻舟智航科技有限公司 基于三阶贝塞尔曲线的变道路径规划方法
CN114092907A (zh) * 2021-11-09 2022-02-25 清华大学苏州汽车研究院（吴江） 跟随路径的确定方法、装置及存储介质
CN114740863A (zh) * 2022-04-26 2022-07-12 轮趣科技(东莞)有限公司 一种基于领航跟随的多机编队控制方法及装置
CN115639814A (zh) * 2022-09-27 2023-01-24 广州市智能软件产业研究院 一种两轮差速移动机器人的路径跟随方法及系统
CN115705054A (zh) * 2021-08-11 2023-02-17 灵动科技（北京）有限公司 用于移动机器人的路径规划方法及程序产品
WO2023036044A1 (zh) * 2021-09-13 2023-03-16 灵动科技（北京）有限公司 全局路径规划方法、运动控制方法及计算机程序产品

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2002073150A2 (en) * 2001-03-13 2002-09-19 Ab Skf System and method for analyzing vibration signals
US20060052901A1 (en) * 2004-09-02 2006-03-09 Fanuc Ltd Robot interference prevention control device
CN115705054A (zh) * 2021-08-11 2023-02-17 灵动科技（北京）有限公司 用于移动机器人的路径规划方法及程序产品
CN113720345A (zh) * 2021-08-31 2021-11-30 北京轻舟智航科技有限公司 基于三阶贝塞尔曲线的变道路径规划方法
WO2023036044A1 (zh) * 2021-09-13 2023-03-16 灵动科技（北京）有限公司 全局路径规划方法、运动控制方法及计算机程序产品
CN114092907A (zh) * 2021-11-09 2022-02-25 清华大学苏州汽车研究院（吴江） 跟随路径的确定方法、装置及存储介质
CN114740863A (zh) * 2022-04-26 2022-07-12 轮趣科技(东莞)有限公司 一种基于领航跟随的多机编队控制方法及装置
CN115639814A (zh) * 2022-09-27 2023-01-24 广州市智能软件产业研究院 一种两轮差速移动机器人的路径跟随方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
陈成等: "基于四阶贝塞尔曲线的无人车可行轨迹规划", 自动化学报, vol. 41, no. 3, pages 486 - 496 *
高嵩等: "基于贝塞尔曲线的无人车局部避障应用", 现代电子技术, vol. 42, no. 9, pages 163 - 166 *

Also Published As

Publication number Publication date
CN116149340B (zh) 2023-06-30

Similar Documents

Publication Publication Date Title
CN107618503B (zh) 一种自动泊车控制方法及系统
WO2018209864A1 (zh) 移动控制方法、装置、机器人及存储介质
US8606498B2 (en) Path approximation for planar motion of a ground vehicle
CN107289938B (zh) 一种地面无人平台局部路径规划方法
KR20150104484A (ko) 자율 이동 차량의 경로 생성 방법 및 경로 생성 장치
WO2007105077A2 (en) Trajectory tracking control system and method for mobile unit
CN110789530B (zh) 一种四轮独立转向-独立驱动车辆轨迹跟踪方法和系统
JP2019207678A (ja) 低速特徴からのメトリック表現の教師なし学習
CN111121777A (zh) 无人驾驶设备轨迹规划方法、装置、电子设备和存储介质
CN113618742B (zh) 一种机器人避障方法、装置和机器人
Wu et al. Picking up speed: Continuous-time lidar-only odometry using doppler velocity measurements
Wang et al. Path tracking control for autonomous harvesting robots based on improved double arc path planning algorithm
CN116149340B (zh) 差速轮底盘机器人自适应路径跟随方法及其相关设备
CN116661469B (zh) 机器人轨迹误差修正方法及系统
CN112067007B (zh) 地图生成方法、计算机存储介质及电子设备
CN115993089B (zh) 基于pl-icp的在线四舵轮agv内外参标定方法
Chen et al. LIO-PPF: Fast LiDAR-Inertial Odometry via Incremental Plane Pre-Fitting and Skeleton Tracking
Zhang et al. A Robust Lidar SLAM System Based on Multi-Sensor Fusion
CN113763434B (zh) 一种基于卡尔曼滤波多运动模型切换的目标轨迹预测方法
CN114323007A (zh) 一种载体运动状态估计方法及装置
CN117705123B (zh) 一种轨迹规划方法、装置、设备及存储介质
Dai et al. Grey Wolf Resampling-Based Rao-Blackwellized Particle Filter for Mobile Robot Simultaneous Localization and Mapping
Guo et al. Trajectory tracking of unicycle-type robots with constraints
CN112762923A (zh) 一种3d点云地图更新方法和系统
CN116300409B (zh) 一种轨迹跟踪控制方法、装置、电子设备及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant