CN116000939A - 一种基于定位波动估计的机器人自适应鲁棒控制方法 - Google Patents

一种基于定位波动估计的机器人自适应鲁棒控制方法 Download PDF

Info

Publication number
CN116000939A
CN116000939A CN202310111526.6A CN202310111526A CN116000939A CN 116000939 A CN116000939 A CN 116000939A CN 202310111526 A CN202310111526 A CN 202310111526A CN 116000939 A CN116000939 A CN 116000939A
Authority
CN
China
Prior art keywords
robot
control
fuzzy
positioning
rule
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
CN202310111526.6A
Other languages
English (en)
Other versions
CN116000939B (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.)
Wuhan Suye Technology Co ltd
Original Assignee
Wuhan Suye Technology Co ltd
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 Wuhan Suye Technology Co ltd filed Critical Wuhan Suye Technology Co ltd
Priority to CN202310111526.6A priority Critical patent/CN116000939B/zh
Publication of CN116000939A publication Critical patent/CN116000939A/zh
Application granted granted Critical
Publication of CN116000939B publication Critical patent/CN116000939B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明涉及一种基于定位波动估计的机器人自适应鲁棒控制方法，其包括：根据机器人的运动参数数据建立机器人的一般运动模型；根据所述机器人的一般运动模型计算机器人的跟随误差；基于模糊规则确定粒子集的方差模糊公式和熵模糊公式；对所述方差模糊公式和所述熵模糊公式去模糊化确定定位波动系数；根据所述机器人的跟随误差构建控制过程的成本函数；根据所述控制过程的成本函数确定机器人控制预测的状态方程；建立优化目标函数对所述机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作，提高了波动评估的准确性，便于定位波动下的控制器设计，从而保证了控制系统在动态场景下的稳定性，实现了机器人自适应鲁棒控制。

Description

一种基于定位波动估计的机器人自适应鲁棒控制方法
技术领域
本发明涉及机器人控制技术领域，尤其涉及一种基于定位波动估计的机器人自适应鲁棒控制方法。
背景技术
机器人由于具有优越的灵活性和可操作性，正逐步被运用于无人工厂、物流中心和展览馆等众多场景。对于无人操作来说，自主导航技术对机器人来说是非常重要的。为了遵循给定的轨迹，移动机器人必须能够根据定位结果精确而稳健地控制自己的姿态。然而，定位和控制问题往往是独立研究的，使得机器人的控制性能有待提高。
为保持良好的轨迹跟踪精度，控制系统需要准确的定位结果作为参考。现有技术一般采用传统的控制方法，在传统的控制方法中，运动学或动力学建模或控制理论得到了更多的关注，而定位结果总是被看作是一个绝对的真实值。
但在具体的实践中，移动机器人无论是使用基于视觉的还是基于激光雷达的定位方案，都会受到外部传感器的噪声干扰，这可能会导致定位波动。如果控制器仍将定位结果视为绝对真理，就会导致严重的抖动或较大的跟踪偏差。
发明内容
有鉴于此，有必要提供一种基于定位波动估计的机器人自适应鲁棒控制方法，用以解决现有技术中机器人控制因为外部传感器的噪声干扰产生定位波动，导致抖动或较大的跟踪偏差的问题。
为达到上述技术目的，本发明采取了以下技术方案：
第一方面，本发明提供了一种基于定位波动估计的机器人自适应鲁棒控制方法，包括：
根据机器人的运动参数数据建立机器人的一般运动模型；
根据机器人的一般运动模型计算机器人的跟随误差；
依据马尔科夫假设和贝叶斯规则设置机器人的姿态置信度；
通过蒙特卡洛定位方法以带权重的粒子集优化机器人的姿态置信度；
基于模糊规则确定粒子集的方差模糊公式和熵模糊公式；
对方差模糊公式和熵模糊公式去模糊化确定定位波动系数；
根据机器人的跟随误差和定位波动系数构建控制过程的成本函数；
根据控制过程的成本函数确定机器人控制预测的状态方程；
建立优化目标函数对机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作。
在一些可能的实现方式中，根据机器人的运动参数数据建立机器人的一般运动模型，包括：
建立机器人的如下运动学模型：
Figure BDA0004077066540000021
其中，x和y是机器人中心点在全局固定坐标系中的位置，θ表示机器人的航向角；v和ω分别表示机器人的线速度和角速度；
将上述方程改写为：
χ＝f(χ,u)；
其中，χ＝[x,y,θ]T为状态变量；u＝[v,ω]T代表控制变量；
构建以下参考模型计算机器人轨迹跟踪过程中的控制变量，具体为：
Figure BDA0004077066540000022
其中，χr是参考控制输入值；ur表示计算的参考输入值；
在点(χr,ur)上对χ＝f(χ,u)进行泰勒展开有：
Figure BDA0004077066540000031
上述公式可简写为：
Figure BDA0004077066540000032
其中，
Figure BDA0004077066540000033
和是在点(χr,ur)展开的函数f(·)的系数矩阵；Or是泰勒展开的高阶余数。
在一些可能的实现方式中，根据机器人的一般运动模型计算机器人的跟随误差，包括：
将
Figure BDA0004077066540000034
带入
Figure BDA0004077066540000035
有
Figure BDA0004077066540000036
其中，e＝χ-χr代表移动机器人的跟随误差；
Figure BDA0004077066540000037
是输入控制法的变化；
设采样周期为T，其中
Figure BDA0004077066540000038
其中，k是采样时间；
Figure BDA0004077066540000039
被改写为
Figure BDA00040770665400000310
Figure BDA00040770665400000311
其中：
Figure BDA00040770665400000312
在一些可能的实现方式中，依据马尔科夫假设和贝叶斯规则设置机器人的姿态置信度，包括：
姿态置信度表示为：
Figure BDA00040770665400000313
其中，sk～(xk,yk,θk)是机器人在时间k时在二维平面内的位姿；xk和yk是机器人的位置，θk是机器人的航向；o1:k和u1:k分别代表从初始时间到时间k的传感器观测和运动控制；M是先验图；
根据马尔科夫假设和贝叶斯规则，将
Figure BDA0004077066540000041
改写为：
Figure BDA0004077066540000042
其中，κ表示归一化系数。
在一些可能的实现方式中，通过蒙特卡洛定位方法以带权重的粒子集优化机器人的姿态置信度，包括：
通过蒙特卡洛定位方法计算
Figure BDA0004077066540000043
使用带权重的粒子集表示
Figure BDA0004077066540000044
即：
Figure BDA0004077066540000045
其中，具有权重的粒子集合表示为
Figure BDA0004077066540000046
Figure BDA0004077066540000047
是粒子
Figure BDA0004077066540000048
的权重，Np指粒子的总数；
Figure BDA0004077066540000049
Figure BDA00040770665400000410
和
Figure BDA00040770665400000411
是粒子的位置和方向；δ()是Dirichlet函数。
在一些可能的实现方式中，基于模糊规则确定粒子集的方差模糊公式和熵模糊公式，包括：
粒子集的方差Vi和熵E表示为：
Figure BDA0004077066540000051
Figure BDA0004077066540000052
Figure BDA0004077066540000053
Figure BDA0004077066540000054
其中，Vpi和Ep是来自
Figure BDA0004077066540000055
的方差和熵；
Figure BDA0004077066540000056
表示相关粒子位姿的平均值；
使用模糊逻辑规则将Vpi和Vi整合为一个模糊公式：
rule 1：if 0≤Vpi≤Vpi1 then fVi1＝fVi(Vi) s.t.0≤Vi＜η1VTi
rule 2：if Vpi2≤Vpi≤Vpi3 then fVi2＝fVi(Vi) s.t.η1VTi≤Vi＜η2VTi
rule 3：if Vpi4≤Vpi then fVi3＝fVi(Vi) s.t.η2VTi≤Vi；
其中，Vpi1～Vpi4为Vpi的模糊分界线，fVi1～fVi3为基于方差的波动值；
描述Ep和E之间的映射关系的模糊公式表示为：
rule 1：if 0≤Ep≤Ep1 then fE1＝fE(E) s.t.0≤E＜η3ET
rule 2：if Ep2≤Ep≤Ep3 then fE2＝fE(E) s.t.η3ET≤E＜η4ET
rule 3：if Ep4≤Ep then fE3＝fE(E) s.t.η4ET≤E；
其中，Ep1～Ep4是Ep的模糊分界线；fE1～fE3是基于熵的波动值。
在一些可能的实现方式中，对方差模糊公式和熵模糊公式去模糊化确定定位波动系数，包括：
通过加权平均法去模糊化，波动系数Lf可表示为：
Figure BDA0004077066540000057
其中，r是模糊规则库的数量；χm表示第m条规则的触发强度。
在一些可能的实现方式中，根据机器人的跟随误差构建控制过程的成本函数，包括：
基于
Figure BDA0004077066540000061
的状态空间离散模型，构建控制过程中需要的成本函数：
Figure BDA0004077066540000062
其中，J(·)为成本函数；e(k+i∣k)和
Figure BDA0004077066540000063
分别代表在时间为k时预测的k+1时的误差值和控制波动值，Np和Nc分别为预测期和控制期，且控制期的值不大于预测期；P,R为权重矩阵。
在一些可能的实现方式中，根据控制过程的成本函数确定机器人控制预测的状态方程，包括：
根据成本函数得到机器人控制预测的状态方程如下：
Figure BDA0004077066540000064
Figure BDA0004077066540000065
Figure BDA0004077066540000066
Figure BDA0004077066540000067
系数矩阵表示为：
Figure BDA0004077066540000068
；
Figure BDA0004077066540000069
Figure BDA0004077066540000071
在一些可能的实现方式中，建立优化目标函数对机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作，包括：
结合
Figure BDA0004077066540000072
预测过程以及
Figure BDA0004077066540000073
得到以下优化目标：
Figure BDA0004077066540000074
将
Figure BDA0004077066540000075
引入优化目标
Figure BDA0004077066540000076
因此：
Figure BDA0004077066540000077
服从于：
Figure BDA0004077066540000078
umin≤u(k)≤umax
Figure BDA0004077066540000079
Figure BDA00040770665400000710
；
其中
Figure BDA00040770665400000711
是中间变量。
采用上述实施例的有益效果是：本发明涉及一种基于定位波动估计的机器人自适应鲁棒控制方法，其包括：根据机器人的运动参数数据建立机器人的一般运动模型；根据所述机器人的一般运动模型计算机器人的跟随误差；依据马尔科夫假设和贝叶斯规则设置机器人的姿态置信度；通过蒙特卡洛定位方法以带权重的粒子集优化所述机器人的姿态置信度；基于模糊规则确定粒子集的方差模糊公式和熵模糊公式；；对所述方差模糊公式和所述熵模糊公式去模糊化确定定位波动系数；根据所述机器人的跟随误差和所述定位波动系数构建控制过程的成本函数；根据所述控制过程的成本函数确定机器人控制预测的状态方程；建立优化目标函数对所述机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作。本发明基于模糊逻辑规则的增强型定位波动估计方法，提高了波动评估的准确性；建立了运动学模型，便于定位波动下的控制器设计；提出了一种考虑定位波动的改进的模型预测控制，其预测步长的自适应调整与定位波动有关，从而保证了控制系统在动态场景下的稳定性。
附图说明
图1为本发明提供的基于定位波动估计的机器人自适应鲁棒控制方法的一实施例的流程示意图；
图2为本发明提供的四轮差动机器人的一实施例的模型示意图；
图3为本发明提供的不同方法的跟踪响应的一实施例的效果示意图；
图4为本发明提供的不同方法的距离误差的一实施例的曲线示意图；
图5为本发明提供的不同方法的角度误差的一实施例的曲线示意图；
图6为本发明实施例提供的电子设备的结构示意图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
在本申请的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本发明的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。
本发明的实施例中的机器人为四轮差动机器人，以四轮差动机器人来说明本发明对机器人自适应鲁棒控制方法。
请参阅图1，图1为本发明提供的基于定位波动估计的机器人自适应鲁棒控制方法的一实施例的流程示意图，本发明的一个具体实施例，公开了一种基于定位波动估计的机器人自适应鲁棒控制方法，包括：
S101、根据机器人的运动参数数据建立机器人的一般运动模型；
S102、根据机器人的一般运动模型计算机器人的跟随误差；
S103、依据马尔科夫假设和贝叶斯规则设置机器人的姿态置信度；
S104、通过蒙特卡洛定位方法以带权重的粒子集优化机器人的姿态置信度；
S105、基于模糊规则确定粒子集的方差模糊公式和熵模糊公式；
S106、对方差模糊公式和熵模糊公式去模糊化确定定位波动系数；
S107、根据机器人的跟随误差和定位波动系数构建控制过程的成本函数；
S108、根据控制过程的成本函数确定机器人控制预测的状态方程；
S109、建立优化目标函数对机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作。
与现有技术相比，本实施例提供的一种基于定位波动估计的机器人自适应鲁棒控制方法，其包括：根据机器人的运动参数数据建立机器人的一般运动模型；根据所述机器人的一般运动模型计算机器人的跟随误差；依据马尔科夫假设和贝叶斯规则设置机器人的姿态置信度；通过蒙特卡洛定位方法以带权重的粒子集优化所述机器人的姿态置信度；基于模糊规则确定粒子集的方差模糊公式和熵模糊公式；对所述方差模糊公式和所述熵模糊公式去模糊化确定定位波动系数；根据所述机器人的跟随误差和所述定位波动系数构建控制过程的成本函数；根据所述控制过程的成本函数确定机器人控制预测的状态方程；建立优化目标函数对所述机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作。本发明基于模糊逻辑规则的增强型定位波动估计方法，提高了波动评估的准确性；建立了运动学模型，便于定位波动下的控制器设计；提出了一种考虑定位波动的改进的模型预测控制，其预测步长的自适应调整与定位波动有关，从而保证了控制系统在动态场景下的稳定性。
在本发明的一些实施例中，根据机器人的运动参数数据建立机器人的一般运动模型，包括：
建立机器人的如下运动学模型：
Figure BDA0004077066540000101
其中，x和y是机器人中心点在全局固定坐标系中的位置，θ表示机器人的航向角；v和ω分别表示机器人的线速度和角速度；
将上述方程改写为：
χ＝f(χ,u)；
其中，χ＝[x,y,θ]T为状态变量；u＝[v,ω]T代表控制变量；
构建以下参考模型计算机器人轨迹跟踪过程中的控制变量，具体为：
Figure BDA0004077066540000102
其中，χr是参考控制输入值；ur表示计算的参考输入值；
在点(χr,ur)上对χ＝f(χ,u)进行泰勒展开有：
Figure BDA0004077066540000103
上述公式可简写为：
Figure BDA0004077066540000104
其中，
Figure BDA00040770665400001111
和是在点(χr,ur)展开的函数f(·)的系数矩阵；Or是泰勒展开的高阶余数。
在本发明的一些实施例中，根据机器人的一般运动模型计算机器人的跟随误差，包括：
将
Figure BDA0004077066540000111
带入
Figure BDA0004077066540000112
有
Figure BDA0004077066540000113
其中，e＝χ-χr代表移动机器人的跟随误差；
Figure BDA0004077066540000114
是输入控制法的变化；
设采样周期为T，其中
Figure BDA0004077066540000115
其中，k是采样时间；
Figure BDA0004077066540000116
被改写为
Figure BDA0004077066540000117
Figure BDA0004077066540000118
其中：
Figure BDA0004077066540000119
在上述实施例中，请参阅图2，图2为本发明提供的四轮差动机器人的一实施例的模型示意图，为了提高四轮差动机器人平台的可控性，本发明实现了移动机器人运动学模型的线性建模。这有利于控制器的设计，使移动机器人的控制更加简单。
在本发明的一些实施例中，依据马尔科夫假设和贝叶斯规则设置机器人的姿态置信度，包括：
姿态置信度表示为：
Figure BDA00040770665400001110
其中，sk～(xk,yk,θk)是机器人在时间k时在二维平面内的位姿；xk和yk是机器人的位置，θk是机器人的航向；o1:k和u1:k分别代表从初始时间到时间k的传感器观测和运动控制；M是先验图；
根据马尔科夫假设和贝叶斯规则，将
Figure BDA0004077066540000121
改写为：
Figure BDA0004077066540000122
其中，κ表示归一化系数。
在本发明的一些实施例中，通过蒙特卡洛定位方法以带权重的粒子集优化机器人的姿态置信度，包括：
通过蒙特卡洛定位方法计算
Figure BDA0004077066540000123
使用带权重的粒子集表示
Figure BDA0004077066540000124
即：
Figure BDA0004077066540000125
其中，具有权重的粒子集合表示为
Figure BDA0004077066540000126
Figure BDA0004077066540000127
是粒子
Figure BDA0004077066540000128
的权重，Np指粒子的总数；
Figure BDA0004077066540000129
Figure BDA00040770665400001210
和
Figure BDA00040770665400001211
是粒子的位置和方向；δ()是Dirichlet函数。
在上述实施例中，常情况下，具有最高权重的粒子被选为当前的定位结果。蒙特卡洛定位方法(MCL)由于能够适应非高斯非线性场景而被广泛使用。尽管围绕MCL的鲁棒性有很多研究，但现有的算法在高度动态的情况下并不能避免定位波动的影响。对定位波动的准确描述对后续导航系统的设计具有重要意义。
当最大粒子权重小于权重阈值时，这就表明当前估计的姿势不可靠。尽管如此，只用一个粒子的权重来衡量定位的可靠性是不可靠的。反过来，方差和熵值是估计定位波动的常见有效指标，它们以综合的方式考虑了具有权重的粒子集合。
方差在数学和物理学上都很好理解，方差越大，定位波动越大，但对于具有多峰分布的定位数据来说，方差的描述性很差。信息熵越高表明粒子权重的差异越小，表明定位的不确定性越大。特别是熵对于非凸的数据评估更准确。然而，仅仅依靠数字指标容易出现分类错误。本发明的主要目的是设计一种稳健的能考虑更多的定位波动指标的评估方法。
在本发明的一些实施例中，基于模糊规则确定粒子集的方差模糊公式和熵模糊公式，包括：
粒子集的方差Vi和熵E表示为：
Figure BDA0004077066540000131
Figure BDA0004077066540000132
Figure BDA0004077066540000133
Figure BDA0004077066540000134
其中，Vpi和Ep是来自
Figure BDA0004077066540000135
的方差和熵；
Figure BDA0004077066540000136
表示相关粒子位姿的平均值；
在上述实施例中，定位波动初始状态表示如下：
Lf＝[Lfx,Lfy,Lfθ]T＝[fVx(Vx)+fE(E),fVy(Vy)+fE(E),fVθ(Vθ)+
fE(E)]T；
Figure BDA0004077066540000137
Figure BDA0004077066540000138
其中，Vi和E分别是方差和熵；i＝x,y,θ；fVi(Vi)和fEi(Ei)是基于Vi和E的定位波动因子；αV，βV，λV，αE，βE和λE是波动参数；0＜αV＜βV＜λV和0＜αE＜βE＜λE；η1，η2，η3和η4是权重系数；0＜η1＜η2和0＜η3＜η4；VTi和ET分别是方差和熵的阈值。Lf是一个三维向量[Lfx，Lfy，LFθ]T，它表示x，y和θ的波动。同时，Lf由方差和熵值决定，其中熵值只与粒子的权重有关，所以三个维度都是统一设置的。尽管Vi和E可以在一定时间内通过基于
Figure BDA0004077066540000141
的定位结果反映出来，但Vpi，Vi，Ep和E之间的确切关系仍然难以用分析方法表示。模糊逻辑规则是根据输入变量推断输出的一种有效手段。
使用模糊逻辑规则将Vpi和Vi整合为一个模糊公式：
rule 1：if 0≤Vpi≤Vpi1 then fVi1＝fVi(Vi) s.t.0≤Vi＜η1VTi
rule 2：if Vpi2≤Vpi≤Vpi3 then fVi2＝fVi(Vi) s.t.η1VTi≤Vi＜η2VTi
rule 3：if Vpi4≤Vpi then fVi3＝fVi(Vi) s.t.η2VTi≤Vi；
其中，Vpi1～Vpi4为Vpi的模糊分界线，fVi1～fVi3为基于方差的波动值；
描述Ep和E之间的映射关系的模糊公式表示为：
rule 1：if 0≤Ep≤Ep1 then fE1＝fE(E) s.t.0≤E＜η3ET
rule 2：if Ep2≤Ep≤Ep3 then fE2＝fE(E) s.t.η3ET≤E＜η4ET
rule 3：if Ep4≤Ep then fE3＝fE(E) s.t.η4ET≤E；
其中，Ep1～Ep4是Ep的模糊分界线；fE1～fE3是基于熵的波动值。
在本发明的一些实施例中，对方差模糊公式和熵模糊公式去模糊化确定定位波动系数，包括：
通过加权平均法去模糊化，波动系数Lf可表示为：
Figure BDA0004077066540000142
Figure BDA0004077066540000151
其中，r是模糊规则库的数量；χm表示第m条规则的触发强度。
在上述实施例中，Vpi1～Vpi4和Ep1～Ep4之间的边界可以从不同动态环境中的定位结果的大量方差和熵中学习。作为构建模糊逻辑系统的标准程序的下一步，去模糊化是通过加权平均法实现的。如果Vpi或Ep满足模糊规则，则χm为1，否则，χm设为0。
在本发明的一些实施例中，根据机器人的跟随误差构建控制过程的成本函数，包括：
为了实现四轮差动机器人平台的鲁棒控制，基于
Figure BDA0004077066540000152
Figure BDA0004077066540000153
的状态空间离散模型，构建控制过程中需要的成本函数：
Figure BDA0004077066540000154
其中，J(·)为成本函数；e(k+i|k)和
Figure BDA0004077066540000155
分别代表在时间为k时预测的k+1时的误差值和控制波动值，Np和Nc分别为预测期和控制期，且控制期的值不大于预测期；P，R为权重矩阵。
在上述实施例中，为了保证最优预测的可行性，预测过程的控制变量如下：
emin≤e(k)≤emax
umin≤u(k)≤umax
Figure BDA0004077066540000156
Figure BDA0004077066540000157
Ormin≤Or(k)≤Ormax；
其中，emin，emax分别为最小和最大误差。umin，umax分别为控制法增量的最小和最大值。Omin，Ormax分别为最小和最大扰动。
考虑到局部波动的存在，为了保证运行的稳定性，Np，Nc需要进一步调整如下：
Np＝[k1max(Lf)]+kp；
Nc＝[k2max(Lf)]+kc；
其中，Lf是由Lf＝[Lfx，Lfy，Lfθ]T＝[fVx(Vx)+fE(E)，fVy(Vy)+fE(E)，fVθ(Vθ)+fE(E)]T得到的估计定位波动值，max()是向量的最大值函数，k1，2是调整系数，kc，kp∈N+是最小调整系数，[X]是不大于X的最大整数值。考虑到预测控制方法的约束状态，有Nc≤Np。
在本发明的一些实施例中，根据控制过程的成本函数确定机器人控制预测的状态方程，包括：
根据成本函数得到机器人控制预测的状态方程如下：
Figure BDA0004077066540000161
Figure BDA0004077066540000162
Figure BDA0004077066540000163
Figure BDA0004077066540000164
系数矩阵表示为：
Figure BDA0004077066540000165
；
Figure BDA0004077066540000166
Figure BDA0004077066540000171
在本发明的一些实施例中，建立优化目标函数对机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作，包括：
结合
Figure BDA0004077066540000172
预测过程以及
Figure BDA0004077066540000173
得到以下优化目标：
Figure BDA0004077066540000174
将
Figure BDA0004077066540000175
引入优化目标
Figure BDA0004077066540000176
因此：
Figure BDA0004077066540000177
服从于：
Figure BDA0004077066540000178
umin≤u(k)≤umax
Figure BDA0004077066540000179
Figure BDA00040770665400001710
；
其中
Figure BDA00040770665400001711
是中间变量。
为了验证考虑位置不确定性的控制方法的优越性，本发明选择了以下比较方法：1)具有优化参数的传统PID控制方法；2)本发明所提出的控制方法不考虑Nc＝Np＝5下的定位波动(NMPC)。
比较方法的具体跟踪过程描述如下：
请参阅图3，图3为本发明提供的不同方法的跟踪响应的一实施例的效果示意图，四轮差速移动机器人的初始点是(10，0)。从图3可以看出，初始点的姿态角与曲线的斜率之间存在误差，导致跟踪误差较大。通过时间调整，所有控制方法都可以实现快速收敛。从图3的放大图可以看出，与所提出的方法和NMPC方法相比，PID具有较大的振荡。进一步分析表明，该方法具有较好的跟踪效果。
请参阅图4，图4为本发明提供的不同方法的距离误差的一实施例的曲线示意图，具体而言，图4显示了PID和NMPC的超调量分别为0.5903m和0.5502m，而提出的方法为0.4603m。此外，通过将提出的方法与NMPC进行比较，发现通过自适应步长调整优化了跟踪过程。仔细观察，PID、NMPC和所提出的方法在稳定阶段(10s～60s)的平均误差分别为0.0374m、0.0177m和0.0096m，这表明误差分别降低了74.3％和45.8％。
请参阅图5，图5为本发明提供的不同方法的角度误差的一实施例的曲线示意图，从图5中可以看出，PID方法的角度误差波动很大，而NMPC和所提出的方法将被限制在一个小范围内。PID和NMPC在10-60s稳定期间的平均角误差分别为0.1001rad和0.0062rad，而所提出的方法的误差较小，为0.0047rad，稳定误差分别降低了95.3％和31.9％。通过比较，本发明提供的方法实现了更好的误差抑制。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机可读指令来指令相关的硬件来完成，该计算机可读指令可存储于一计算机可读取存储介质中，该计算机可读指令在执行时，可包括如上述各方法的实施例的流程。其中，前述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)等非易失性存储介质，或随机存储记忆体(Random Access Memory，RAM)等。
应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
请参阅图6，图6为本发明实施例提供的电子设备的结构示意图。基于上述基于定位波动估计的机器人自适应鲁棒控制方法，本发明还相应提供了一种基于定位波动估计的机器人自适应鲁棒控制设备，基于定位波动估计的机器人自适应鲁棒控制设备可以是移动终端、桌上型计算机、笔记本、掌上电脑及服务器等计算设备。该基于定位波动估计的机器人自适应鲁棒控制设备包括处理器610、存储器620及显示器630。图6仅示出了电子设备的部分组件，但是应理解的是，并不要求实施所有示出的组件，可以替代的实施更多或者更少的组件。
存储器620在一些实施例中可以是基于定位波动估计的机器人自适应鲁棒控制设备的内部存储单元，例如基于定位波动估计的机器人自适应鲁棒控制设备的硬盘或内存。存储器620在另一些实施例中也可以是基于定位波动估计的机器人自适应鲁棒控制设备的外部存储设备，例如基于定位波动估计的机器人自适应鲁棒控制设备上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，存储器620还可以既包括基于定位波动估计的机器人自适应鲁棒控制设备的内部存储单元也包括外部存储设备。存储器620用于存储安装于基于定位波动估计的机器人自适应鲁棒控制设备的应用软件及各类数据，例如安装基于定位波动估计的机器人自适应鲁棒控制设备的程序代码等。存储器620还可以用于暂时地存储已经输出或者将要输出的数据。在一实施例中，存储器620上存储有基于定位波动估计的机器人自适应鲁棒控制程序640，该基于定位波动估计的机器人自适应鲁棒控制程序640可被处理器610所执行，从而实现本申请各实施例的基于定位波动估计的机器人自适应鲁棒控制方法。
处理器610在一些实施例中可以是一中央处理器(Central Processing Unit,CPU)，微处理器或其他数据处理芯片，用于运行存储器620中存储的程序代码或处理数据，例如执行基于定位波动估计的机器人自适应鲁棒控制方法等。
显示器630在一些实施例中可以是LED显示器、液晶显示器、触控式液晶显示器以及OLED(Organic Light-Emitting Diode，有机发光二极管)触摸器等。显示器630用于显示在基于定位波动估计的机器人自适应鲁棒控制设备的信息以及用于显示可视化的用户界面。基于定位波动估计的机器人自适应鲁棒控制设备的部件610-630通过系统总线相互通信。
在一实施例中，当处理器610执行存储器620中基于定位波动估计的机器人自适应鲁棒控制程序640时实现如上的基于定位波动估计的机器人自适应鲁棒控制方法中的步骤。
本实施例还提供了一种计算机可读存储介质，其上存储有基于定位波动估计的机器人自适应鲁棒控制程序，该基于定位波动估计的机器人自适应鲁棒控制程序被处理器执行时实现以下步骤：
根据机器人的运动参数数据建立机器人的一般运动模型；
根据机器人的一般运动模型计算机器人的跟随误差；
依据马尔科夫假设和贝叶斯规则设置机器人的姿态置信度；
通过蒙特卡洛定位方法以带权重的粒子集优化机器人的姿态置信度；
基于模糊规则确定粒子集的方差模糊公式和熵模糊公式；
对方差模糊公式和熵模糊公式去模糊化确定定位波动系数；
根据机器人的跟随误差和定位波动系数构建控制过程的成本函数；
根据控制过程的成本函数确定机器人控制预测的状态方程；
建立优化目标函数对机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作。
综上，本实施例提供的一种基于定位波动估计的机器人自适应鲁棒控制方法，其包括：根据机器人的运动参数数据建立机器人的一般运动模型；根据所述机器人的一般运动模型计算机器人的跟随误差；依据马尔科夫假设和贝叶斯规则设置机器人的姿态置信度；通过蒙特卡洛定位方法以带权重的粒子集优化所述机器人的姿态置信度；基于模糊规则确定粒子集的方差模糊公式和熵模糊公式；对所述方差模糊公式和所述熵模糊公式去模糊化确定定位波动系数；根据所述机器人的跟随误差和所述定位波动系数构建控制过程的成本函数；根据所述控制过程的成本函数确定机器人控制预测的状态方程；建立优化目标函数对所述机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作。本发明基于模糊逻辑规则的增强型定位波动估计方法，提高了波动评估的准确性；建立了运动学模型，便于定位波动下的控制器设计；提出了一种考虑定位波动的改进的模型预测控制，其预测步长的自适应调整与定位波动有关，从而保证了控制系统在动态场景下的稳定性。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种基于定位波动估计的机器人自适应鲁棒控制方法，其特征在于，包括：
根据机器人的运动参数数据建立机器人的一般运动模型；
根据所述机器人的一般运动模型计算机器人的跟随误差；
依据马尔科夫假设和贝叶斯规则设置机器人的姿态置信度；
通过蒙特卡洛定位方法以带权重的粒子集优化所述机器人的姿态置信度；
基于模糊规则确定粒子集的方差模糊公式和熵模糊公式；
对所述方差模糊公式和所述熵模糊公式去模糊化确定定位波动系数；
根据所述机器人的跟随误差和所述定位波动系数构建控制过程的成本函数；
根据所述控制过程的成本函数确定机器人控制预测的状态方程；
建立优化目标函数对所述机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作。
2.根据权利要求1所述的基于定位波动估计的机器人自适应鲁棒控制方法，其特征在于，所述根据机器人的运动参数数据建立机器人的一般运动模型，包括：
建立机器人的如下运动学模型：
Figure FDA0004077066520000011
其中，x和y是机器人中心点在全局固定坐标系中的位置，θ表示机器人的航向角；v和ω分别表示机器人的线速度和角速度；
将上述方程改写为：
χ＝f(χ,u)；
其中，χ＝[x,y,θ]T为状态变量；u＝[v,ω]T代表控制变量；
构建以下参考模型计算机器人轨迹跟踪过程中的控制变量，具体为：
Figure FDA0004077066520000021
其中，χr是参考控制输入值；ur表示计算的参考输入值；
在点(χr,ur)上对χ＝f(χ,u)进行泰勒展开有：
Figure FDA0004077066520000022
上述公式可简写为：
Figure FDA0004077066520000023
其中，fχr(·)和是在点(χr,ur)展开的函数f(·)的系数矩阵；Or是泰勒展开的高阶余数。
3.根据权利要求2所述的基于定位波动估计的机器人自适应鲁棒控制方法，其特征在于，所述根据所述机器人的一般运动模型计算机器人的跟随误差，包括：
将
Figure FDA0004077066520000024
带入
Figure FDA0004077066520000025
有
Figure FDA0004077066520000026
其中，e＝χ-χr代表移动机器人的跟随误差；
Figure FDA0004077066520000027
是输入控制法的变化；
设采样周期为T，其中
Figure FDA0004077066520000028
其中，k是采样时间；
Figure FDA0004077066520000029
被改写为
Figure FDA00040770665200000210
Figure FDA00040770665200000211
其中：
Figure FDA00040770665200000212
4.根据权利要求2所述的基于定位波动估计的机器人自适应鲁棒控制方法，其特征在于，所述依据马尔科夫假设和贝叶斯规则设置机器人的姿态置信度，包括：
姿态置信度表示为：
Figure FDA0004077066520000031
其中，sk～(xk,yk,θk)是机器人在时间k时在二维平面内的位姿；xk和yk是机器人的位置，θk是机器人的航向；o1:k和u1:k分别代表从初始时间到时间k的传感器观测和运动控制；M是先验图；
根据马尔科夫假设和贝叶斯规则，将
Figure FDA0004077066520000032
改写为：
Figure FDA0004077066520000033
其中，κ表示归一化系数。
5.根据权利要求4所述的基于定位波动估计的机器人自适应鲁棒控制方法，其特征在于，所述通过蒙特卡洛定位方法以带权重的粒子集优化所述机器人的姿态置信度，包括：
通过蒙特卡洛定位方法计算
Figure FDA0004077066520000034
使用带权重的粒子集表示
Figure FDA0004077066520000035
即：
Figure FDA0004077066520000036
其中，具有权重的粒子集合表示为
Figure FDA0004077066520000037
Figure FDA0004077066520000038
是粒子
Figure FDA0004077066520000039
的权重，Np指粒子的总数；
Figure FDA00040770665200000310
Figure FDA0004077066520000041
和
Figure FDA0004077066520000042
是粒子的位置和方向；δ()是Dirichlet函数。
6.根据权利要求5所述的基于定位波动估计的机器人自适应鲁棒控制方法，其特征在于，所述基于模糊规则确定粒子集的方差模糊公式和熵模糊公式，包括：
粒子集的方差Vi和熵E表示为：
Figure FDA0004077066520000043
Figure FDA0004077066520000044
Figure FDA0004077066520000045
Figure FDA0004077066520000046
其中，Vpi和Ep是来自
Figure FDA0004077066520000047
的方差和熵；
Figure FDA0004077066520000048
表示相关粒子位姿的平均值；
使用模糊逻辑规则将Vpi和Vi整合为一个模糊公式：
rule 1：if 0≤Vpi≤vpi1 then fVi1＝fVi(Vi)s.t.0≤vi<η1VTi
rule 2：if Vpi2≤Vpi≤Vpi3 then fVi2＝fVi(Vi)s.t.η1VTi≤vi<η2VTi
rule 3：if Vpi4≤Vpi then fCi3＝fVi(Vi)s.t.η2VTi≤Vi；
其中，Vpi1～Vpi4为Vpi的模糊分界线，fVi1～fVi3为基于方差的波动值；
描述Ep和E之间的映射关系的模糊公式表示为：
rule 1：if 0≤Ep≤ep1 then fE1＝fE(E)s.t.0≤E<η3ET
rule 2：if Ep2≤Ep≤Ep3 then fE2＝fE(E)s.t.η3ET≤E<η4ET
rule 3：if Ep4≤Ep then fE3＝fE(E)s.t.η4ET≤E；
其中，Ep1～Ep4是Ep的模糊分界线；fE1～fE3是基于熵的波动值。
7.根据权利要求6所述的基于定位波动估计的机器人自适应鲁棒控制方法，其特征在于，所述对所述方差模糊公式和所述熵模糊公式去模糊化确定定位波动系数，包括：
通过加权平均法去模糊化，波动系数Lf可表示为：
Figure FDA0004077066520000051
其中，r是模糊规则库的数量；χm表示第m条规则的触发强度。
8.根据权利要求7所述的基于定位波动估计的机器人自适应鲁棒控制方法，其特征在于，所述根据所述机器人的跟随误差构建控制过程的成本函数，包括：
基于
Figure FDA0004077066520000052
的状态空间离散模型，构建控制过程中需要的成本函数：
Figure FDA0004077066520000053
其中，J(·)为成本函数；e(k+i∣k)和
Figure FDA0004077066520000054
分别代表在时间为k时预测的k+1时的误差值和控制波动值，Np和Nc分别为预测期和控制期，且控制期的值不大于预测期；P,R为权重矩阵。
9.根据权利要求8所述的基于定位波动估计的机器人自适应鲁棒控制方法，其特征在于，所述根据所述控制过程的成本函数确定机器人控制预测的状态方程，包括：
根据所述成本函数得到机器人控制预测的状态方程如下：
Figure FDA0004077066520000055
Figure FDA0004077066520000056
Figure FDA0004077066520000061
Figure FDA0004077066520000062
系数矩阵表示为：
Figure FDA0004077066520000063
；
Figure FDA0004077066520000064
Figure FDA0004077066520000065
10.根据权利要求9所述的基于定位波动估计的机器人自适应鲁棒控制方法，其特征在于，所述建立优化目标函数对所述机器人控制预测的状态方程进行优化，对机器人跟随移动进行控制操作，包括：
结合
Figure FDA0004077066520000066
预测过程以及
Figure FDA0004077066520000067
得到以下优化目标：
Figure FDA0004077066520000068
将
Figure FDA0004077066520000069
引入优化目标
Figure FDA00040770665200000610
因此：
Figure FDA00040770665200000611
服从于：
Figure FDA00040770665200000612
umin≤u(k)≤umax
Figure FDA00040770665200000613
Figure FDA00040770665200000614
；
其中
Figure FDA00040770665200000615
是中间变量。
CN202310111526.6A 2023-02-07 2023-02-07 一种基于定位波动估计的机器人自适应鲁棒控制方法 Active CN116000939B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310111526.6A CN116000939B (zh) 2023-02-07 2023-02-07 一种基于定位波动估计的机器人自适应鲁棒控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310111526.6A CN116000939B (zh) 2023-02-07 2023-02-07 一种基于定位波动估计的机器人自适应鲁棒控制方法

Publications (2)

Publication Number Publication Date
CN116000939A true CN116000939A (zh) 2023-04-25
CN116000939B CN116000939B (zh) 2024-01-26

Family

ID=86035690

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310111526.6A Active CN116000939B (zh) 2023-02-07 2023-02-07 一种基于定位波动估计的机器人自适应鲁棒控制方法

Country Status (1)

Country Link
CN (1) CN116000939B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102289714A (zh) * 2011-06-02 2011-12-21 北京航空航天大学 一种基于行为模型的小型无人旋翼机自主起降控制方法
US20180209815A1 (en) * 2017-01-24 2018-07-26 Korea University Research And Business Foundation Method of estimating reliability of measurement distance of laser rangefinder, and localizating method of mobile robot using laser rangefinder
CN111273551A (zh) * 2020-03-12 2020-06-12 山东大学 双舵轮自动导引车轨迹追踪控制方法及系统
CN112034706A (zh) * 2020-08-17 2020-12-04 华中科技大学 一种基于多模式切换的移动机器人容错控制方法及设备
CN112025697A (zh) * 2020-07-10 2020-12-04 浙江工业大学 一种全向移动机器人的积分模型预测控制方法
CN114459470A (zh) * 2022-01-19 2022-05-10 国网江西省电力有限公司超高压分公司 基于多传感器融合的巡检机器人定位方法
CN114721275A (zh) * 2022-05-13 2022-07-08 北京航空航天大学 一种基于预设性能的视觉伺服机器人自适应跟踪控制方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102289714A (zh) * 2011-06-02 2011-12-21 北京航空航天大学 一种基于行为模型的小型无人旋翼机自主起降控制方法
US20180209815A1 (en) * 2017-01-24 2018-07-26 Korea University Research And Business Foundation Method of estimating reliability of measurement distance of laser rangefinder, and localizating method of mobile robot using laser rangefinder
CN111273551A (zh) * 2020-03-12 2020-06-12 山东大学 双舵轮自动导引车轨迹追踪控制方法及系统
CN112025697A (zh) * 2020-07-10 2020-12-04 浙江工业大学 一种全向移动机器人的积分模型预测控制方法
CN112034706A (zh) * 2020-08-17 2020-12-04 华中科技大学 一种基于多模式切换的移动机器人容错控制方法及设备
CN114459470A (zh) * 2022-01-19 2022-05-10 国网江西省电力有限公司超高压分公司 基于多传感器融合的巡检机器人定位方法
CN114721275A (zh) * 2022-05-13 2022-07-08 北京航空航天大学 一种基于预设性能的视觉伺服机器人自适应跟踪控制方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
GEN LI; JIE MENG; YUANLONG XIE; XIAOLONG ZHANG; LIQUAN JIANG; YU HUANG: "An Improved Observation Model for Monte-Carlo Localization Integrated with Reliable Reflector Prediction", 《2019 IEEE/ASME INTERNATIONAL CONFERENCE ON ADVANCED INTELLIGENT MECHATRONICS (AIM)》, pages 972 - 977 *

Also Published As

Publication number Publication date
CN116000939B (zh) 2024-01-26

Similar Documents

Publication Publication Date Title
Duckett et al. Learning globally consistent maps by relaxation
Thrun et al. Simultaneous mapping and localization with sparse extended information filters: Theory and initial results
US8543241B2 (en) Method, medium, and system estimating pose of mobile robots
Ravichandar et al. Learning Partially Contracting Dynamical Systems from Demonstrations.
Murali et al. Perception-aware trajectory generation for aggressive quadrotor flight using differential flatness
CN111536964A (zh) 机器人定位方法及装置、存储介质
Valiente et al. A comparison of EKF and SGD applied to a view-based SLAM approach with omnidirectional images
CN112444246B (zh) 高精度的数字孪生场景中的激光融合定位方法
CN116661469B (zh) 机器人轨迹误差修正方法及系统
Thrun et al. SLAM updates require constant time
Wang et al. A upf-ukf framework for slam
Zhou et al. Robust Kalman filtering with long short-term memory for image-based visual servo control
CN116416277A (zh) 一种基于运动方程轨迹预测的多目标跟踪方法及装置
Valiente et al. A modified stochastic gradient descent algorithm for view-based SLAM using omnidirectional images
CN116000939B (zh) 一种基于定位波动估计的机器人自适应鲁棒控制方法
de Figueiredo et al. Multi-object detection and pose estimation in 3d point clouds: A fast grid-based bayesian filter
Yuzhen et al. The application of adaptive extended Kalman filter in mobile robot localization
Krämer et al. Improving local trajectory optimization by enhanced initialization and global guidance
CN114600054A (zh) 计算机实施的创建用于运行移动智能体的环境地图的方法
Singh et al. Robot motion planning in a dynamic environment using offset Non-Uniform Rational B-Splines (NURBS)
Kim et al. Active object tracking using context estimation: handling occlusions and detecting missing targets
Wang et al. Combination of the ICP and the PSO for 3D-SLAM
Wang et al. Jacobian estimation with adaptive Kalman filter for uncalibrated visual servoing
Liu et al. Monte Carlo multi-robot localization based on grid cells and characteristic particles
Wang et al. Action selection based on localizability for active global localization of mobile robots

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