一种轨迹跟踪控制方法、装置及存储介质与流程

1.本技术涉及自动化控制技术领域，涉及但不限于一种轨迹跟踪控制方法、装置及存储介质。


背景技术：

2.随着计算机技术的快速发展，机器人技术也在不断突破，逐渐被应用到各个领域。移动机器人凭借其灵活性更被大家喜爱，其应用给人们带来了很大的便利，如扫地机器人，物流机器人，医用机器人等。运动控制是移动机器人完成任务的前提，而轨迹跟踪控制属于运动控制中的一种。
3.在相关技术中，轨迹跟踪控制主要有以下两种方式：一种方式是基于移动机器人的运动学模型进行轨迹跟踪。该种控制方式是基于运动学模型设计一个光滑时变的控制器，控制移动机器人在输入饱和条件的约束下进行轨迹跟踪。该种控制方式虽然考虑输入饱和约束条件，但仅基于运动学模型，不适用复杂工作环境中对移动机器人进行轨迹跟踪控制。
4.另一种方式是基于移动机器人的动力学模型进行轨迹跟踪。该种控制方式是基于动力学模型设计一种鲁棒非线性控制器或力矩控制器，控制移动机器人在有界的外部扰动下进行轨迹跟踪。该种控制方式一方面由于动力学模型中涉及的部分参数在现有条件下还无法测出实际环境值，例如转动惯量参数等，另一方面由于该种控制方式采用理想环境下的参数，假设移动机器人受到的干扰较小，不会影响机器人的位姿状态，存在无法精确地确定控制器参数的缺陷，进而影响移动机器人的轨迹跟踪。并且，该种控制方式没有考虑控制输入饱和约束，仅基于动力学模型，不适用当期望位姿与实际位姿误差较大时对移动机器人进行轨迹跟踪控制。


技术实现要素：

5.有鉴于相关技术中存在的无法对移动机器人进行精确地轨迹跟踪控制的问题，本技术提供一种轨迹跟踪控制方法、装置及存储介质。
6.本技术提供一种轨迹跟踪控制方法，包括：
7.获取进行轨迹跟踪的机器人在当前时刻的实际位姿参数；
8.当确定实际位姿参数不满足预设条件时，对所述实际位姿参数进行饱和速度处理和扰动处理，得到满足预设条件的目标位姿参数；
9.根据所述目标位姿参数控制所述机器人进行轨迹跟踪。
10.本技术提供一种轨迹跟踪控制装置，包括：
11.获取模块，用于获取进行轨迹跟踪的机器人在当前时刻的实际位姿参数；
12.处理模块，用于当确定实际位姿参数不满足预设条件时，对所述实际位姿参数进行饱和速度处理和扰动处理，得到满足预设条件的目标位姿参数；
13.控制模块，用于根据所述目标位姿参数控制所述机器人进行轨迹跟踪。
14.本技术提供一种轨迹跟踪控制设备，包括：
15.存储器，用于存储可执行指令；
16.处理器，用于执行所述存储器中存储的可执行指令时，实现本技术实施例提供的方法。
17.本技术提供一种存储介质，存储有可执行指令，用于引起处理器执行时，实现本技术实施例提供的方法。
18.本技术提供的轨迹跟踪控制方法中，获取进行轨迹跟踪的机器人在当前时刻的实际位姿参数，根据实际位姿参数是否满足预设条件来判断机器人是否出现跟踪误差，当确定实际位姿参数不满足预设条件时，对实际位姿参数进行饱和速度处理和扰动处理，以减小跟踪误差，得到满足预设条件的目标位姿参数，再根据所述目标位姿参数控制所述机器人进行轨迹跟踪，通过考虑速度饱和约束和外界扰动影响，对跟踪过程中的误差进行处理，能够减小跟踪误差，从而提高对机器人的轨迹跟踪控制的精确度。
附图说明
19.图1为本技术实施例提供的轨迹跟踪控制方法的一种实现流程示意图；
20.图2为本技术实施例提供的轨迹跟踪控制方法的另一种实现流程示意图；
21.图3为本技术实施例提供的轨迹跟踪控制方法的又一种实现流程示意图；
22.图4为本技术实施例提供的轨迹跟踪控制系统的结构示意图；
23.图5为本技术实施例提供的移动机器人跟踪过程示意图；
24.图6为本技术实施例提供的轨迹跟踪控制方法的再一种实现流程示意图；
25.图7为本技术实施例提供的轨迹跟踪控制方法的网络架构示意图；
26.图8为本技术实施例提供的轨迹跟踪控制设备的组成结构示意图。
具体实施方式
27.为了使本技术的目的、技术方案和优点更加清楚，下面将结合附图对本技术作进一步地详细描述，所描述的实施例不应视为对本技术的限制，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本技术保护的范围。
28.在以下的描述中，涉及到“一些实施例”，其描述了所有可能实施例的子集，但是可以理解，“一些实施例”可以是所有可能实施例的相同子集或不同子集，并且可以在不冲突的情况下相互结合。
29.在以下的描述中，所涉及的术语“第一\第二\第三”仅仅是区别类似的对象，不代表针对对象的特定排序，可以理解地，“第一\第二\第三”在允许的情况下可以互换特定的顺序或先后次序，以使这里描述的本技术实施例能够以除了在这里图示或描述的以外的顺序实施。
30.除非另有定义，本文所使用的所有的技术和科学术语与属于本技术的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述本技术实施例的目的，不是旨在限制本技术。
31.将结合本技术实施例提供的终端的示例性应用和实施，说明本技术实施例提供的轨迹跟踪控制方法。参见图1，图1为本技术实施例提供的轨迹跟踪控制方法的一种实现流
程示意图，应用于图7所示的轨迹跟踪控制设备，将结合图1示出的步骤进行说明。
32.步骤s101，获取进行轨迹跟踪的机器人在当前时刻的实际位姿参数。
33.机器人在移动过程中，机器人上设置的采集装置可以采集机器人在任意时刻的位姿参数。这里，采集装置可以包括光电编码器、惯性导航系统(ins，inertial navigation system)、定位装置以及其他相关传感器。其中定位装置采集机器人的位置参数，ins采集机器人的运动方向，光电编码器采集机器人左右轮的转速。机器人采集到当前时刻的位置参数、运动方向、左右轮转速后，确定出机器人在当前时刻的实际位姿参数，发送实际位姿参数至轨迹跟踪控制，以使轨迹跟踪控制设备对机器人进行轨迹跟踪控制。
34.轨迹跟踪控制设备在对机器人进行轨迹跟踪的过程中，持续获取机器人所采集的实际位姿参数。这里，持续获取实际位姿参数，可以指周期性的获取实际位姿参数，每个周期内，机器人将采集的实际位姿参数发送至轨迹跟踪控制设备；持续获取实际位姿参数，也可以指在设定时刻点获取实际位姿参数，当到达预先设定的时刻点时，机器人将采集的实际位姿参数发送至轨迹跟踪控制设备；持续获取实际位姿参数，也可以指在设定位置点获取实际位姿参数，当机器人到达预先设定的位置点时，将采集的实际位姿参数发送至轨迹跟踪控制设备；当然，持续获取实际位姿参数，还可以指随机时刻点、或随机位置点、或其它方式获取实际位姿参数，本技术实施例中不做限定。
35.步骤s102，当确定实际位姿参数不满足预设条件时，对所述实际位姿参数进行饱和速度处理和扰动处理，得到满足预设条件的目标位姿参数。
36.轨迹跟踪控制设备获取到实际位姿参数后，根据实际位姿参数判断机器人是否偏离了预设的参考轨迹，即确定实际位姿参数是否满足预设条件。当确定实际位姿参数满足预设条件时，表明机器人当前时刻是按照预设的参考轨迹移动的，未出现偏离；当确定实际位姿参数不满足预设条件时，表明机器人当前时刻偏离了预设的参考轨迹，即出现了跟踪误差，此时，对实际位姿参数进行消除误差处理，以得到满足预设条件的目标位姿参数。
37.步骤s103，根据所述目标位姿参数控制所述机器人进行轨迹跟踪。
38.轨迹跟踪控制设备将经过消除误差处理后得到的目标位姿参数发送至机器人，控制机器人按照目标位姿参数进行移动。本技术实施例通过考虑速度饱和约束和外界扰动影响，对跟踪过程中的误差进行处理，能够减小跟踪误差，从而提高对机器人的轨迹跟踪控制的精确度。
39.本技术实施例提供的轨迹跟踪控制方法，包括：获取进行轨迹跟踪的机器人在当前时刻的实际位姿参数，当确定实际位姿参数不满足预设条件时，对所述实际位姿参数进行饱和速度处理和扰动处理，得到满足预设条件的目标位姿参数，根据所述目标位姿参数控制所述机器人进行轨迹跟踪，通过考虑速度饱和约束和外界扰动影响，对跟踪过程中的误差进行处理，能够减小跟踪误差，从而提高对机器人的轨迹跟踪控制的精确度。
40.在图1所示实施例的基础上，本技术再提供一种轨迹跟踪控制方法，具体涉及图1所示实施例中步骤s102的一种可能的实现方式。参见图2，图2为本技术实施例提供的轨迹跟踪控制方法的另一种实现流程示意图，应用于图7所示的轨迹跟踪控制设备，本技术实施例提供的轨迹跟踪控制方法，包括以下步骤：
41.步骤s201，获取进行轨迹跟踪的机器人在当前时刻的实际位姿参数。
42.本技术实施例中，步骤s201和步骤s209，分别对应图1所示实施例中步骤s101和步
骤s103，实现过程参见上述步骤s101和步骤s103。
43.下述步骤s202至步骤s204是轨迹跟踪控制设备确定实际位姿参数是否满足预设条件的一种实现方式，步骤s205至步骤s208是当轨迹跟踪控制设备确定实际位姿参数不满足预设条件时，如何对实际位姿参数进行处理以得到目标位姿参数的一种实现方式。
44.步骤s202，根据预设参考轨迹确定参考位姿参数。
45.轨迹跟踪控制设备获取到当前时刻的实际位姿参数后，根据预设参考轨迹确定当前时刻的参考位姿参数。这里，预设参考轨迹是在跟踪之前，由用户设定的机器人的预行驶轨迹。在本技术实施例中，所述预设参考轨迹至少包括移动路线、移动时间和移动速度，根据预设参考轨迹，可以确定机器人跟踪过程中任一时刻所在位置和速度信息。
46.步骤s203，根据所述参考位姿参数和所述实际位姿参数计算跟踪误差。
47.这里，计算当前时刻机器人的实际位姿参数与参考位姿参数的差值，将该差值确定为跟踪误差。
48.本技术实施例中，当前时刻机器人的实际位姿参数q＝[x,y,ψ]
t
，(x,y)为机器人的实际位置坐标，ψ为实际偏转角度，参考位姿参数q
r
＝[x
r
,y
r
,ψ
r
]
t
，(x
r
,y
r
)为机器人的参考位置坐标，ψ
r
为参考偏转角度，根据实际位姿参数与参考位姿参数，计算得到的跟踪误差如式(1)所示：
[0049][0050]
步骤s204，当所述跟踪误差大于预设误差阈值时，确定所述实际位姿参数不满足预设条件。
[0051]
判断跟踪误差是否大于预设误差阈值，当跟踪误差大于预设误差阈值时，确定实际位姿参数不满足预设条件，表明机器人当前时刻偏离了预设的参考轨迹，即出现了跟踪误差，进入步骤s205进行误差处理。当跟踪误差不大于预设误差阈值时，确定实际位姿参数满足预设条件，表明机器人当前时刻虽然出现了跟踪误差，但该跟踪误差比较小，无需进行处理，即认为机器人是按照预设的参考轨迹移动的，未出现偏离，此时，进入步骤s201，继续进行轨迹跟踪。
[0052]
步骤s205，对所述跟踪误差进行饱和速度处理，得到期望速度。
[0053]
在机器人轨迹跟踪过程中，考虑到机器人自身属性，如电机性能等因素，以及正常跟踪运动过程中，速度不可能出现大幅度变化，因此，轨迹跟踪控制设备进行跟踪误差处理时，机器人的速度、加速度需要满足一定的约束条件，基于此，本技术实施例中，通过加入饱和速度控制器实现对跟踪误差进行饱和速度处理，得到期望速度。
[0054]
这里，期望速度包括期望线速度和期望角速度。机器人的饱和速度和加速度的约束条件可以由用户预先根据机器人的自身参数进行设定，也可以为机器人出厂设置的默认值。
[0055]
步骤s206，对所述期望速度进行扰动处理，得到处理后的期望速度。
[0056]
在机器人轨迹跟踪过程中，考虑到外界环境扰动，如机器人自身重量、受到的摩擦力等因素发生改变，在进行饱和速度处理后，还需要进一步对期望速度进行扰动处理，得到处理后的期望线速度和期望角速度。
[0057]
步骤s207，根据所述处理后的期望速度计算期望位姿参数。
[0058]
结合处理后的期望线速度和期望角速度和实际位姿参数，计算期望位置参数、期望运动方向、期望左右轮转速，从而确定出期望位姿参数。
[0059]
步骤s208，当所述期望位姿参数满足预设条件时，将所述期望位姿参数确定为目标位姿参数。
[0060]
经过上述步骤s205至步骤s207进行消除误差处理后，得到期望位姿参数，由于外部干扰的不确定性，导致计算得到的期望位姿参数可能还未满足预设条件，需要进一步调整。基于此，本技术实施例在计算得到期望位姿参数后，判断期望位姿参数是否满足预设条件，当期望位姿参数满足预设条件时，将该期望位姿参数确定为目标位姿参数，进入步骤s209；当期望位姿参数不满足预设条件时，表明需要进一步进行消除误差处理，将当前计算得到的期望位姿参数更新为实际位姿参数，进入步骤s203。
[0061]
步骤s209，根据所述目标位姿参数控制所述机器人进行轨迹跟踪。
[0062]
本技术实施例提供的轨迹跟踪控制方法，首先获取进行轨迹跟踪的机器人在当前时刻的实际位姿参数，然后根据预设参考轨迹确定参考位姿参数，根据所述参考位姿参数和所述实际位姿参数计算跟踪误差，当所述跟踪误差大于预设误差阈值时，确定所述实际位姿参数不满足预设条件，再对所述跟踪误差进行饱和速度处理，得到期望速度，对所述期望速度进行扰动处理，得到处理后的期望速度，根据所述处理后的期望速度计算期望位姿参数，当所述期望位姿参数满足预设条件时，将所述期望位姿参数确定为目标位姿参数，最后根据所述目标位姿参数控制所述机器人进行轨迹跟踪，通过考虑速度饱和约束和外界扰动影响，对跟踪过程中的误差进行饱和速度处理和扰动处理，使得机器人移动速度满足约束条件，能够减小外界干扰导致的跟踪误差，从而提高对机器人的轨迹跟踪控制的精确度。
[0063]
在一些实施例中，图2中所述的步骤s205“对所述跟踪误差进行饱和速度处理，得到期望速度”可以通过下述的步骤s2051至步骤s2053实现，以下结合各个步骤进行说明。
[0064]
步骤s2051，根据预设的速度约束条件确定饱和速度控制函数的参数。
[0065]
这里，速度约束条件包括机器人的速度和加速度约束条件。在一种实现方式中，预设的速度约束条件如式(2)所示：
[0066][0067]
其中，ω(t)为机器人在当前时刻t的角速度，υ(t)为机器人在当前时刻t的线速度，当机器人远离初始位置时，线速度为正，当机器人驶近初始位置时，线速度为负。这里，ω
max
和υ
max
是预设设置的最大角速度和最大线速度，ω
max
>0，v
max
>0，t≥0。
[0068]
在实际系统中，机器人移动的速度不可能没有限制，且在正常情况下，不可能出现突变，据此，在处理具有饱和速度约束的移动机器人轨迹跟踪控制问题时，设定速度时连续的，确定连续饱和函数如式(3)所示：
[0069]
θ(x)＝ctanh(kx)
ꢀꢀꢀ
(3)；
[0070]
其中，tanh(kx)是双曲正切函数，c为预先设定的满足c>0的随机参数，用于确定边界范围，k控制机器人在初始位置周围的突变时间，满足k>0，θ(x)为约束条件，满足xθ(x)>0。
[0071]
根据式(2)和式(3)，可以确定得到饱和速度控制函数的参数。
[0072]
步骤s2052，基于所述饱和速度控制函数的参数确定饱和速度控制函数。
[0073]
根据上述确定的饱和速度控制函数的参数，确定饱和速度控制函数，如式(4)所示：
[0074][0075]
其中，ω
r
和υ
r
为根据参考位姿参数确定的参考速度，ω
r
为参考角速度，υ
r
为参考线速度，ω
c
和υ
c
为期望速度，ω
c
和υ
c
均有边界且均匀连续；c1、c2和c3为根据步骤s2051确定的、用于决定期望速度的边界值的参数，k1和k2为根据步骤s2051确定的、用于决定系统速度的突变时间的参数，为关于偏航角度误差ψ
e
的光滑函数。
[0076]
步骤s2053，将所述跟踪误差输入至所述饱和速度控制函数，进行饱和速度处理，得到期望速度。
[0077]
将式(1)代入式(4)，计算得到期望速度v
c
，包括期望角速度ω
c
和期望线速度υ
c
。
[0078]
在一些实施例中，图2中所述的步骤s206“对所述期望速度进行扰动处理，得到处理后的期望速度”可以通过下述的步骤s2061至步骤s2063实现，以下结合各个步骤进行说明。
[0079]
步骤s2061，获取机器人当前时刻的实际速度。
[0080]
这里，实际速度v包括实际角速度ω和实际线速度υ。
[0081]
步骤s2062，根据所述实际速度和所述期望速度，计算速度误差。
[0082]
根据步骤s2061获取到机器人当前时刻的实际速度，根据步骤s2053计算得到期望速度后，计算当前时刻机器人的实际速度和期望速度的差值，将该差值确定为速度误差，如式(5)所示：
[0083][0084]
步骤s2063，对所述速度误差进行扰动处理，得到处理后的期望速度。
[0085]
根据所述实际速度和所述期望速度，计算得到速度误差后，进一步对速度误差进行扰动处理，以消除外界环境的干扰，得到处理后的期望速度。
[0086]
在一些实施例中，步骤s2063：对所述速度误差进行扰动处理，得到处理后的期望速度，可以通过下述的步骤s0631至步骤s0633实现：
[0087]
步骤s0631，获取预先建立的动力控制函数和动力学函数。
[0088]
这里，所述动力控制函数为根据机器人在外界环境中集总扰动建立的数学模型，建立的动力控制函数如式(6)所示：
[0089][0090]
其中，τ为输入控制力矩，v
c
′
为期望速度的导数，v
c
为期望速度，c为离心力和哥氏力，e
v
为速度误差，为集总扰动d(v,τ)的估计量，式(6)中其他参数如式(7)至式
(13)所示：
[0091][0092]
h＝(ms)-1
ꢀꢀꢀ
(8)；
[0093][0094][0095][0096][0097][0098]
其中，m为机器人质量，i为转动惯量，l为左右驱动轮之间的距离，a为左右驱动轮中点m到移动机器人自身运动学质点g的距离。在实际应用中，可以将移动机器人自身运动学质点g看作左右驱动轮中点m，即将a看作0，以便于计算。
[0099]
所述动力学函数为根据跟踪过程中机器人受到的力建立的数学模型，建立的动力学函数如式(14)所示：
[0100][0101]
其中，d(v,τ)为集总扰动。
[0102]
步骤s0632，基于所述速度误差和所述动力控制函数，确定输入控制力矩。
[0103]
在一种实现方式中，步骤s0632，基于所述速度误差和所述动力控制函数，确定输入控制力矩，可以通过下述的步骤s6321至步骤s6324实现：
[0104]
步骤s6321，将所述速度误差输入至所述动力控制函数，得到当前控制力矩。
[0105]
这里，将速度误差输入至动力控制函数，即将e
v
代入式(6)，得到当前控制力矩τ1。
[0106]
步骤s6322，将所述当前控制力矩输入至所述动力学函数，得到处理后的当前期望速度。
[0107]
将当前控制力矩输入至动力学函数，即将τ1代入式(14)，并对式(14)等号两边求积分，得到处理后的当前期望速度v1。
[0108]
步骤s6323，根据所述当前控制力矩、所述当前期望速度，确定跟踪过程中的集总扰动。
[0109]
由于转动惯量i无法直接测量，不能直接求集总扰动d(v,τ)。本技术实施例中，利
用集总扰动d(v,τ)的估计量进行估算，该估计量是根据模糊逼近技术和观测技术得到的。
[0110]
根据系统状态估计值与动力控制函数中v的误差计算扰动观测误差，如式(15)所示：
[0111][0112]
由于只有当扰动观测误差e
o
逼近0时，模糊扰动观测器预估出的扰动值才趋近于真实的扰动d(v,τ)，因此为了使得e
o
→
0，设置权向量的自适应率如式(16)所示：
[0113][0114]
其中，r
+
为正数矩阵，
[0115]
将当前控制力矩τ1和当前期望速度v1代入下式(17)，计算得到集总扰动的估计量将该估计量确定为集总扰动。
[0116][0117]
其中，σ为大于0的常数。
[0118]
步骤s6324，将所述速度误差和所述集总扰动输入至所述动力控制函数，得到输入控制力矩。
[0119]
将速度误差e
v
和集总扰动代入式(6)，计算得到输入控制力矩τ。
[0120]
步骤s0633，将所述输入控制力矩输入至所述动力学函数，得到处理后的期望速度。
[0121]
将计算得到的输入控制力矩τ代入式(14)，并对式(14)等号两边求积分，得到处理后的期望速度v2。
[0122]
在一些实施例中，上述步骤s207“根据所述处理后的期望速度计算期望位姿参数”可以通过下述的步骤s2071至步骤s2073实现，以下结合各个步骤进行说明。
[0123]
步骤s2071，根据实际位姿参数确定运动学函数的参数。
[0124]
获取的实际位姿参数记为q＝[x,y,ψ]
t
，根据实际位姿参数确定运动学函数的参数。
[0125]
步骤s2072，基于所述运动学函数的参数确定运动学函数。
[0126]
这里，运动学函数如式(18)所示：
[0127][0128]
[0129]
其中，s为运动学函数的参数，v为速度，q
′
为实际位姿参数的导数。根据实际位姿参数q＝[x,y,ψ]
t
，可以确定s，从而得到运动学函数。
[0130]
步骤s2073，将所述处理后的期望速度输入至所述运动学函数，得到期望位姿参数。
[0131]
将处理后的期望速度v2代入至运动学函数即式(18)，并对式(18)等号两边求积分，得到期望位姿参数q2。
[0132]
本技术实施例中，轨迹跟踪控制设备根据实际位姿参数确定运动学函数的参数，基于所述运动学函数的参数确定运动学函数，将所述处理后的期望速度输入至所述运动学函数，得到期望位姿参数，能够实现根据处理后的期望速度计算期望位姿参数，从而能够实现对实际位姿参数的调整，以减小跟踪误差，进而能够提高对机器人的轨迹跟踪控制的精确度。
[0133]
基于前述的实施例，本技术实施例再提供一种轨迹跟踪控制方法，应用于图7所示的网络架构，图3为本技术实施例提供的轨迹跟踪控制方法的又一种实现流程示意图，如图3所示，所述方法包括：
[0134]
步骤s301，机器人采集当前时刻的实际位姿参数。
[0135]
这里，所示机器人为轨迹跟踪控制设备进行轨迹跟踪的机器人。
[0136]
步骤s302，轨迹跟踪控制设备获取当前时刻的实际位姿参数。
[0137]
步骤s303，轨迹跟踪控制设备根据预设参考轨迹确定参考位姿参数。
[0138]
这里，预设参考轨迹是在进行轨迹跟踪之前，由用户设定的机器人的预行驶轨迹。在本技术实施例中，所述预设参考轨迹至少包括移动路线、移动时间和移动速度，根据预设参考轨迹，可以确定机器人跟踪过程中任一时刻所在位置和速度信息。
[0139]
步骤s304，轨迹跟踪控制设备根据所述参考位姿参数和所述实际位姿参数计算跟踪误差。
[0140]
步骤s305，轨迹跟踪控制设备判断跟踪误差是否大于预设误差阈值。
[0141]
这里，当所述跟踪误差大于预设误差阈值时，确定所述实际位姿参数不满足预设条件，表明机器人当前运行轨迹出现了误差，需要对该误差进行调整，进入步骤s306；当所述跟踪误差不大于预设误差阈值时，确定所述实际位姿参数满足预设条件，表明机器人当前运行轨迹与预设的参考轨迹一致，未出现跟踪误差，返回步骤s301，机器人采集下一时刻的实际位姿参数，以使轨迹跟踪控制设备继续进行轨迹跟踪。
[0142]
步骤s306，轨迹跟踪控制设备根据预设的速度约束条件确定饱和速度控制函数的参数。
[0143]
步骤s307，轨迹跟踪控制设备基于所述饱和速度控制函数的参数确定饱和速度控制函数。
[0144]
步骤s308，轨迹跟踪控制设备将所述跟踪误差输入至所述饱和速度控制函数，进行饱和速度处理，得到期望速度。
[0145]
这里，期望速度包括期望线速度和期望角速度。机器人的饱和速度和加速度的约束条件可以由用户预先根据机器人的自身参数进行设定，也可以为机器人出厂设置的默认值。
[0146]
步骤s309，轨迹跟踪控制设备获取机器人当前时刻的实际速度。
[0147]
这里，机器人当前时刻的实际速度可以由机器人的速度传感器采集并发送至轨迹跟踪控制设备。
[0148]
步骤s310，轨迹跟踪控制设备根据所述实际速度和所述期望速度，计算速度误差。
[0149]
步骤s311，轨迹跟踪控制设备获取预先建立的动力控制函数和动力学函数。
[0150]
这里，所述动力控制函数为根据机器人在外界环境中集总扰动建立的数学模型，所述动力学函数为根据跟踪过程中机器人受到的力建立的数学模型。
[0151]
步骤s312，轨迹跟踪控制设备将所述速度误差输入至所述动力控制函数，得到当前控制力矩。
[0152]
步骤s313，轨迹跟踪控制设备将所述当前控制力矩输入至所述动力学函数，得到处理后的当前期望速度。
[0153]
步骤s314，轨迹跟踪控制设备根据所述当前控制力矩、所述当前期望速度，确定跟踪过程中的集总扰动。
[0154]
由于转动惯量i无法直接测量，不能直接求集总扰动d(v,τ)。本技术实施例中，利用集总扰动d(v,τ)的估计量进行估算，该估计量是根据模糊逼近技术和观测技术得到的。
[0155]
步骤s315，轨迹跟踪控制设备将所述速度误差和所述集总扰动输入至所述动力控制函数，得到输入控制力矩。
[0156]
步骤s316，轨迹跟踪控制设备将所述输入控制力矩输入至所述动力学函数，得到处理后的期望速度。
[0157]
步骤s317，轨迹跟踪控制设备根据实际位姿参数确定运动学函数的参数。
[0158]
步骤s318，轨迹跟踪控制设备基于所述运动学函数的参数确定运动学函数。
[0159]
步骤s319，轨迹跟踪控制设备将所述处理后的期望速度输入至所述运动学函数，得到期望位姿参数。
[0160]
步骤s320，轨迹跟踪控制设备判断期望位姿参数是否满足预设条件。
[0161]
这里，当所述期望位姿参数满足预设条件时，表明已消除了机器人的跟踪误差，使得机器人的运动轨迹与预设的参考轨迹一致，此时，进入步骤s321；当所述期望位姿参数不满足预设条件时，表明机器人当前还存在跟踪误差，继续进行误差调整，进入步骤s306。
[0162]
步骤s321，轨迹跟踪控制设备将所述期望位姿参数确定为目标位姿参数。
[0163]
步骤s322，轨迹跟踪控制设备发送所述目标位姿参数至机器人。
[0164]
步骤s323，机器人根据所述目标位姿参数进行移动。
[0165]
本技术实施例提供的轨迹跟踪控制方法中，轨迹跟踪控制设备获取进行轨迹跟踪的机器人在当前时刻的实际位姿参数，根据实际位姿参数是否满足预设条件来判断机器人是否出现跟踪误差，当确定实际位姿参数不满足预设条件时，对实际位姿参数进行饱和速度处理和扰动处理，以减小跟踪误差，得到满足预设条件的目标位姿参数，再根据所述目标位姿参数控制所述机器人进行轨迹跟踪，通过考虑速度饱和约束和外界扰动影响，对跟踪过程中的误差进行处理，能够减小跟踪误差，从而提高对机器人的轨迹跟踪控制的精确度。
[0166]
下面，将说明本技术实施例在一个实际的应用场景中的示例性应用。
[0167]
本技术实施例以差分驱动轮式移动机器人为例进行示例性说明，图4为本技术实施例提供的轨迹跟踪控制系统的结构示意图，轨迹跟踪控制系统主要分为两个部分：工控
机和移动机器人，工控机可以设置在移动机器人内部，此时，工控机和移动机器人可以通过串口进行通信；工控机还可以设置在移动机器人外部，此时，工控机和移动机器人可以通过无线网络进行通信。工控机内设置有轨迹跟踪控制设备，移动机器人内设置有惯性导航惯性测量单元(imu，ine rtial measurement unit)、光电编码器、定位装置、微处理器以及电机等。其中定位装置采集移动机器人的位置参数，可以采用全球定位系统(gps，global positioning system)、激光雷达或超宽带(uwb，ultra wide band)基站等方式进行采集；惯性导航imu采集移动机器人的运动方向；光电编码器采集移动机器人左、右轮的转速。采集到当前时刻的数据后，再结合相对定位得到移动机器人当前时刻的实际位姿参数，发送实际位姿参数至工控机进行轨迹跟踪控制。
[0168]
图5为本技术实施例提供的移动机器人跟踪过程示意图，本技术实施例中，为了计算方便，将运动学关注点，即机器人质心(图5中点g)与机器人左右轮，即驱动轮的中点(图5中点m)的距离设置为0，使得点m表示移动机器人的位姿坐标。
[0169]
图6为本技术实施例提供的轨迹跟踪控制方法的再一种实现流程示意图，如图6所示，本技术实施例提供了一种考虑速度饱和约束和外界扰动影响的移动机器人轨迹跟踪控制方法。整体的控制系统是一个双闭环系统，外环系统是基于改进双曲正切函数的饱和速度控制器，内环系统利用模糊逼近技术和观测器技术设计了一种能够迅速逼近系统不确定集总扰动的模糊扰动观测。下面结合图6对本技术实施例进行说明。
[0170]
1)饱和速度控制器
[0171]
移动机器人在轨迹跟踪到某时刻时，工控机接收到采集的当前位姿参数，通过轨迹跟踪控制方法计算出下一时刻机器人运行的左右轮(即驱动轮)角速度，并发送给机器人，使得机器人能够按照预设的参考轨迹移动。在这个过程中，考虑到轮式移动机器人自身电机的性能，以及实际速度不能大幅度变化，工控机计算出的速度及其加速度必须约束在一定范围内，因此加入饱和速度控制器。以下为本技术实施例设计的饱和控制器。
[0172]
首先，建立移动机器人的运动学模型，如式(20)所示：
[0173][0174]
其中：s为运动学函数的参数，v为机器人当前时刻的速度，包括行驶线速度υ和偏航角速度ω，q
′
是当前时刻获取的实际位姿参数q的导数，ψ是偏航角度。
[0175]
轨迹跟踪的目标是找到时变状态反馈控制器，使得实际位姿参数q＝[x,y,ψ]
t
与期望位姿参数q
r
＝[x
r
,y
r
,ψ
r
]
t
之间的误差随着时间的推移，逐渐趋近于0，其形式如式(21)所示：
[0176]
ω＝ω
*
(t,ψ,x,y)，υ＝υ
*
(t,ψ,x,y)
ꢀꢀꢀ
(21)；
[0177]
在实际工作环境中，机器人行驶的速度不可能无限大，因此上式(21)需满足速度饱和约束条件，如式(22)所示：
[0178][0179]
其中，ω(t)为机器人在当前时刻t的角速度，υ(t)为机器人在当前时刻t的线速
度，当机器人远离初始位置时，线速度为正，当机器人驶近初始位置时，线速度为负。这里，ω
max
和υ
max
是预设设置的最大角速度和最大线速度，且满足ω
max
>0，v
max
>0条件，t≥0。
[0180]
在外环系统中，当t
→
∞时，为了获得q
→
q
r
性能，可以转换为跟踪误差趋近于0，跟踪误差的形式如式(23)所示：
[0181][0182]
对式(23)进行微分，得到跟踪误差的微分形式如式(24)所示：
[0183][0184]
其中：υ
r
和ω
r
是期望的线速度和角速度。现有的大多数研究是在不考虑输入饱和约束的情况下进行的，但在实际系统中，机器人移动的速度不可能没有限制，且在正常情况下，机器人移动的速度不可能出现突变，据此，在处理具有饱和速度约束的移动机器人轨迹跟踪控制问题时，设定速度时连续的，确定连续饱和函数如式(25)所示：
[0185]
θ(x)＝c tanh(kx)
ꢀꢀꢀ
(25)；
[0186]
其中，tanh(kx)是双曲正切函数，c为预先设定的满足c>0的随机参数，用于确定边界范围，k控制机器人在初始位置周围的突变时间，满足k>0，θ(x)为约束条件，满足xθ(x)>0。
[0187]
在机器人轨迹跟踪过程中，ω
r
和υ
r
均有边界、且均匀连续，在机器人移动过程中，ω
r
(t)或υ
r
(t)不会收敛到0，那么跟踪误差的微分式在饱和速度控制器下是全局渐进稳定的，饱和速度控制器的控制律如式(26)所示：
[0188][0189]
其中，c1，c2和c3为大于0的常量，决定了速度的边界值，k1,k2>0决定系统速度的突变时间，为关于偏航角度误差ψ
e
的光滑函数。
[0190]
此外，本发明还利用了一个连续可微的近似开关函数tanh()作为h
c
()的特殊函数。与仅使用饱和单元的有界控制器不同，本技术实施例中设计的控制器除了具有更好的连续特性外，还具有边界，更适用于实际应用。
[0191]
2)动力学控制器
[0192]
考虑到机器人在轨迹跟踪的过程中不可避免的会受到外界扰动的影响，导致在某一时刻偏离预先设定的参考轨迹。例如，机器人自身质心的变化会影响机器人的轨迹跟踪。运动学模型是将机器人看作一个运动的质点建立的数学模型，因此不适合用于考虑外部干
扰的研究，基于此，本技术实施例提出动力学模型。动力学模型则是基于移动机器人在运动过程中自身力的变化过程而建立的数学模型，机器人自身的质量对轨迹跟踪控制也会有影响。以下便是基于动力学模型设计的控制器，其中d(v,τ)为集成扰动，该扰动项由本技术实施例下述3)中的模糊扰动观测器观测出来并前馈给动力学控制器。
[0193]
移动机器人在非完整约束下的广义动力学模型如式(27)所示：
[0194][0195]
其中，m为对称正定惯性矩阵，为实际位姿参数的一阶导数，为实际位姿参数的二阶导数，c为离心力和哥氏力，g为机器人所受重力，f摩擦力，τ
d
是机器人所受的外部扰动，τ为输入控制力矩，e为变换矩阵，a是非完整约束关联的矩阵λ为拉格朗日乘子。
[0196]
结合移动机器人滑动，设计非完整约束条件如式(28)所示：
[0197][0198]
其中：r为机器人左右驱动轮的半径，和分别为左右驱动轮的角速度，ζ
l
和ζ
r
分别为左右驱动轮的滑动角速度，两驱动轮中点m到移动机器人自身运动学质点的距离为a，l为两驱动轮之间的距离。这里，在实际应用在，可以将移动机器人自身运动学质点g看作左右驱动轮中点m，即将a看作0，以便于计算。
[0199]
根据运动学模型，动力学模型，以及上述非完整约束条件，推导得到带有集总扰动的动力学模型，如式(29)所示：
[0200][0201]
其中，d(v,τ)为集总扰动，集总扰动可以表达为式(30)：
[0202][0203]
其中：h＝(ms)-1
，p
1v
＝s-1
，p
2v
＝hc，＝hc，
[0204]
在内环部分，控制器的功能主要是实现移动机器人的实际速度v跟踪到期望速度v
c
，因此设实际速度与期望速度之间的误差及其微分项分别如式(31)、式(32)所示：
[0205]
e
v
＝v-v
c
ꢀꢀꢀ
(31)；
[0206][0207]
上式(32)表示除了实际位姿与期望位姿的误差影响速度跟踪以外，还包括外界环境中集总扰动引起的速度跟踪误差。集总扰动可以通过模糊扰动观测器观测出并补偿给动力学控制器，因此动力学控制器的控制律可设计为式(33)：
[0208][0209]
其中，c是正定矩阵。
[0210]
3)模糊扰动观测器
[0211]
上述2)中，在设计动力学控制器时考虑集总扰动，集总扰动可以通过本部分设计的模糊扰动观测器观测系统，得到机器人受到的集总扰动，将其补偿给动力学控制器。本技术实施例中，扰动观测器是基于前馈方法控制，与反馈控制相比，具有很快的响应速度。因此设计模糊扰动观测器能够迅速观测到外界未知扰动，并补偿给动力学控制器，不需要假设外界扰动趋近于0，具有良好的鲁棒性。本技术实施例将扰动影响机器人偏离轨迹的问题转化为控制机器人的左右轮力矩，从而使机器人跟踪到预设的参考轨迹。本技术实施例设计的模糊扰动观测器的过程如下：
[0212]
假设状态向量v属于一个集合z
v
，定义最优参数为：
[0213][0214]
λ
*
属于凸集z
λ
中，遵循以下条件约束：
[0215]
z
λ
＝{λ|||λ||≤z
λ
}
ꢀꢀꢀ
(35)；
[0216]
其中，z
λ
是设计参数。
[0217]
根据上述式(34)、式(35)，得到影响轨迹跟踪控制的集总扰动可以通过模糊系统表示为式(36)：
[0218]
d(v,τ)＝d(v,τ|λ
*
)＝λ
*t
ε(v,τ)+c
ꢀꢀꢀ
(36)；
[0219][0220][0221]
其中，ε(v,τ)为模糊基向量，c的取值范围为[0，∞)。根据式(36)可知，集总扰动依赖于状态向量和最优权向量，而状态向量v的模糊基向量ε(v,τ)是由模糊隶属度函数决定的，当模糊隶属度函数确定后，即可得到一个最优权向量λ
*
逼近系统的集总扰动。本技术实施例中，构建的基于模糊逼近的扰动观测器如式(39)所示：
[0222][0223]
其中，为系统状态的估计值，σ为正常数，可以通过具有万能逼近特性的模糊系统和观测技术得到。
[0224]
模糊扰动观测器中状态估计值与动力学控制模型中v的误差属于扰动观测误差，即：
[0225]
[0226]
只有当扰动观测误差e
o
→
0时，模糊扰动观测器预估出的扰动值才趋近于真实的扰动d(v,τ)。因此为了使得e
o
→
0、观测出任意形式的扰动包括理想情况d(v,τ)＝0。设计权向量的自适应率为：
[0227][0228]
其中：r
+
为正数矩阵，满足
[0229]
4)稳定性分析
[0230]
内环系统稳定性分析：
[0231]
设计李雅普诺夫函数并证明其一阶导数为负：
[0232][0233][0234]
其中：
[0235]
当|e
o
|>c/σ时，李雅普诺夫函数的导数为负，证明了由动力学控制器和模糊扰动观测器组成的内环系统在全局上是一致渐进稳定的。
[0236]
外环系统稳定性分析：
[0237]
设计李雅普诺夫函数并证明其一阶导数为负：
[0238][0239][0240]
由证明了外环系统在全局上是一致渐进稳定的。
[0241]
本技术实施例提供的轨迹跟踪控制方法，轨迹跟踪控制设备获取移动机器人采集的当前时刻的实际位姿参数，根据实际位姿参数确定是否需要进行跟踪误差调整，当需要进行误差调整时，对实际位姿参数进行饱和速度处理和扰动处理，以减小跟踪误差，得到满足预设条件的目标位姿参数，再根据目标位姿参数控制机器人进行轨迹跟踪，通过考虑实际运动中的误差，在跟踪过程中对误差进行处理，能够提高对机器人的轨迹跟踪控制的精确度。
[0242]
下面说明实现本技术实施例的装置的示例性应用，本技术实施例提供的装置可以实施为终端设备。下面，将说明装置实施为终端设备时涵盖终端设备的示例性应用。
[0243]
参见图7，图7为本技术实施例提供的轨迹跟踪控制方法的网络架构示意图，如图7所示，在该网络架构中至少包括轨迹跟踪控制设备100、网络200和机器人300。为实现支撑一个示例性应用，轨迹跟踪控制设备100通过网络200连接机器人300，实现对机器人轨迹的跟踪控制。网络200可以是广域网或者局域网，又或者是二者的组合，使用无线或有线链路实现数据传输。当轨迹跟踪控制设备100设置在机器人300上时，轨迹跟踪控制设备100还可以通过串口通信进行数据传输，以控制机器人进行轨迹跟踪。
[0244]
在对机器人进行轨迹跟踪的过程中，轨迹跟踪控制设备100从机器人300获取机器人当前时刻的实际位姿参数，判断是否出现跟踪误差，在确定出现跟踪误差时，对实际位姿参数进行饱和速度处理和扰动处理，得到目标位姿参数，发送目标位姿参数至机器人300，控制机器人300进行更精确地轨迹跟踪。
[0245]
本技术实施例提供的装置可以实施为硬件或者软硬件结合的方式，下面说明本技术实施例提供的装置的各种示例性实施。
[0246]
根据图8示出的轨迹跟踪控制设备100的示例性结构，可以预见轨迹跟踪控制设备100的其他的示例性结构，因此这里所描述的结构不应视为限制，例如可以省略下文所描述的部分组件，或者，增设下文所未记载的组件以适应某些应用的特殊需求。
[0247]
图8所示的轨迹跟踪控制设备100包括：至少一个处理器110、存储器140、至少一个网络接口120和用户接口130。轨迹跟踪控制设备100中的每个组件通过总线系统150耦合在一起。可理解，总线系统150用于实现这些组件之间的连接通信。总线系统150除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见，在图8中将各种总线都标为总线系统150。
[0248]
用户接口130可以包括显示器、键盘、鼠标、触感板和触摸屏等。
[0249]
存储器140可以是易失性存储器或非易失性存储器，也可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(rom，read only memory)。易失性存储器可以是随机存取存储器(ram，random access memory)。本技术实施例描述的存储器140旨在包括任意适合类型的存储器。
[0250]
本技术实施例中的存储器140能够存储数据以支持轨迹跟踪控制设备100的操作。这些数据的示例包括：用于在轨迹跟踪控制设备100上操作的任何计算机程序，如操作系统和应用程序。其中，操作系统包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序可以包含各种应用程序。
[0251]
作为本技术实施例提供的方法采用软件实施的示例，本技术实施例所提供的方法可以直接体现为由处理器110执行的软件模块组合，软件模块可以位于存储介质中，存储介质位于存储器140，处理器110读取存储器140中软件模块包括的可执行指令，结合必要的硬件(例如，包括处理器110以及连接到总线150的其他组件)完成本技术实施例提供的方法。
[0252]
作为示例，处理器110可以是一种集成电路芯片，具有信号的处理能力，例如通用处理器、数字信号处理器(dsp，digital signal processor)，或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等，其中，通用处理器可以是微处理器或者任何常规的处理器等。
[0253]
下面继续说明本技术实施例提供的轨迹跟踪控制装置90的实施为软件模块的示例性结构，在一些实施例中，如图8所示，存储在存储器140的轨迹跟踪控制装置90中的软件
模块可以包括：
[0254]
获取模块91，用于获取进行轨迹跟踪的机器人在当前时刻的实际位姿参数。
[0255]
确定模块92，用于确定实际位姿参数是否满足预设条件。
[0256]
这里，所述预设条件可以由用户预先定义，或者为机器人出厂时设定的默认条件。
[0257]
处理模块93，用于当确定实际位姿参数不满足预设条件时，对所述实际位姿参数进行饱和速度处理和扰动处理，得到满足预设条件的目标位姿参数；
[0258]
控制模块94，用于根据所述目标位姿参数控制所述机器人进行轨迹跟踪。
[0259]
在一些实施例中，所述确定模块92，进一步包括：
[0260]
第一确定子模块，用于根据预设参考轨迹确定参考位姿参数；
[0261]
第一计算子模块，用于根据所述参考位姿参数和所述实际位姿参数计算跟踪误差；
[0262]
第二确定子模块，用于当所述跟踪误差大于预设误差阈值时，确定所述实际位姿参数不满足预设条件。
[0263]
在一些实施例中，所述处理模块93，进一步包括：
[0264]
第一处理子模块，用于对所述跟踪误差进行饱和速度处理，得到期望速度；
[0265]
第二处理子模块，用于对所述期望速度进行扰动处理，得到处理后的期望速度；
[0266]
第二计算子模块，用于根据所述处理后的期望速度计算期望位姿参数；
[0267]
第三确定子模块，用于当所述期望位姿参数满足预设条件时，将所述期望位姿参数确定为目标位姿参数。
[0268]
在一些实施例中，所述第一处理子模块，进一步包括：
[0269]
第一确定单元，用于根据预设的速度约束条件确定饱和速度控制函数的参数；
[0270]
第二确定单元，用于基于所述饱和速度控制函数的参数确定饱和速度控制函数；
[0271]
第一输入单元，用于将所述跟踪误差输入至所述饱和速度控制函数，进行饱和速度处理，得到期望速度。
[0272]
在一些实施例中，所述第二处理子模块，进一步包括：
[0273]
获取单元，用于获取机器人当前时刻的实际速度；
[0274]
计算单元，用于根据所述实际速度和所述期望速度，计算速度误差；
[0275]
处理单元，用于对所述速度误差进行扰动处理，得到处理后的期望速度。
[0276]
在一些实施例中，所述处理单元，进一步包括：
[0277]
获取子单元，用于获取预先建立的动力控制函数和动力学函数；
[0278]
这里，所述动力控制函数为根据机器人在外界环境中集总扰动建立的数学模型，所述动力学函数为根据跟踪过程中机器人受到的力建立的数学模型；
[0279]
确定子单元，用于基于所述速度误差和所述动力控制函数，确定输入控制力矩；
[0280]
在一些实施例中，所述确定子单元，进一步用于将所述速度误差输入至所述动力控制函数，得到当前控制力矩；将所述当前控制力矩输入至所述动力学函数，得到处理后的当前期望速度；根据所述当前控制力矩、所述当前期望速度，确定跟踪过程中的集总扰动；将所述速度误差和所述集总扰动输入至所述动力控制函数，得到输入控制力矩。
[0281]
输入子单元，用于将所述输入控制力矩输入至所述动力学函数，得到处理后的期望速度。
[0282]
在一些实施例中，所述第二计算子模块，进一步包括：
[0283]
第三确定单元，用于根据实际位姿参数确定运动学函数的参数；
[0284]
第四确定单元，用于基于所述运动学函数的参数确定运动学函数；
[0285]
第二输入单元，用于将所述处理后的期望速度输入至所述运动学函数，得到期望位姿参数。
[0286]
本技术实施例提供一种存储有可执行指令的存储介质，其中存储有可执行指令，当可执行指令被处理器执行时，将引起处理器执行本技术实施例提供的方法，例如，如图1、图2、图3和图6示出的方法。
[0287]
在一些实施例中，存储介质可以是fram、rom、prom、eprom、eeprom、闪存、磁表面存储器、光盘、或cd-rom等存储器；也可以是包括上述存储器之一或任意组合的各种设备。
[0288]
在一些实施例中，可执行指令可以采用程序、软件、软件模块、脚本或代码的形式，按任意形式的编程语言(包括编译或解释语言，或者声明性或过程性语言)来编写，并且其可按任意形式部署，包括被部署为独立的程序或者被部署为模块、组件、子例程或者适合在计算环境中使用的其它单元。
[0289]
作为示例，可执行指令可以但不一定对应于文件系统中的文件，可以可被存储在保存其它程序或数据的文件的一部分，例如，存储在超文本标记语言(html，hyper text markup language)文档中的一个或多个脚本中，存储在专用于所讨论的程序的单个文件中，或者，存储在多个协同文件(例如，存储一个或多个模块、子程序或代码部分的文件)中。
[0290]
作为示例，可执行指令可被部署为在一个计算设备上执行，或者在位于一个地点的多个计算设备上执行，又或者，在分布在多个地点且通过通信网络互连的多个计算设备上执行。
[0291]
以上所述，仅为本技术的实施例而已，并非用于限定本技术的保护范围。凡在本技术的精神和范围之内所作的任何修改、等同替换和改进等，均包含在本技术的保护范围之内。