CN109857108B - 基于内模控制算法的移动机器人轨迹跟踪方法及系统 - Google Patents

基于内模控制算法的移动机器人轨迹跟踪方法及系统 Download PDF

Info

Publication number
CN109857108B
CN109857108B CN201910097059.XA CN201910097059A CN109857108B CN 109857108 B CN109857108 B CN 109857108B CN 201910097059 A CN201910097059 A CN 201910097059A CN 109857108 B CN109857108 B CN 109857108B
Authority
CN
China
Prior art keywords
robot
mobile robot
deviation
state variable
acquiring
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.)
Active
Application number
CN201910097059.XA
Other languages
English (en)
Other versions
CN109857108A (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.)
Zhejiang Mairui Robot Co Ltd
Original Assignee
Zhejiang Mairui Robot 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 Zhejiang Mairui Robot Co Ltd filed Critical Zhejiang Mairui Robot Co Ltd
Priority to CN201910097059.XA priority Critical patent/CN109857108B/zh
Publication of CN109857108A publication Critical patent/CN109857108A/zh
Application granted granted Critical
Publication of CN109857108B publication Critical patent/CN109857108B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Feedback Control In General (AREA)

Abstract

本发明公开一种基于内模控制算法的移动机器人轨迹跟踪方法，包括以下步骤：分别获取移动机器人在某一具体时刻的状态变量，所述状态变量包括运动轨迹的状态变量、里程计的状态变量和移动机器人的状态变量，通过所述运动轨迹、里程计和状态变量获取实际运动轨迹与预设运动轨迹的偏差；将获取到的偏差作为内模控制器的输入，进而得到相应时刻的输出；将所述输出传入移动机器人的电机中，并且获取下一时刻里程计的状态变量，并根据移动机器人的运动学方程获取移动机器人在下一时刻的状态变量的预测值。通过本发明的方法和系统实现一种计算量小、实时性好的改进的内模控制的机器人轨迹跟踪方法。

Description

基于内模控制算法的移动机器人轨迹跟踪方法及系统
技术领域
本发明涉及机器人轨迹跟踪控制技术领域，尤其涉及一种基于内模控制算法的移动机器人轨迹跟踪方法及系统。
背景技术 在现实环境中有很多因素可能导致移动机器人偏离预定轨迹，比如地面的不平整、轮子打滑等等，因此一个效果好的轨迹跟踪方法显得十分重要。在线性系统中，由于内模控制具有结构简单、调节参数少、不需要知道被控对象精确的数学模型、鲁棒性强等特点受到了人们的青睐。内模控制在线性系统中取得的成功吸引了科研人员的注意，许多学者尝试把内模控制运用到移动机器人的轨迹跟踪，但是由于机器人的运动学模型是一个耦合的非线性系统，学者们的尝试并不是那么容易，尽管如此，科研人员还是提出了多种方案，比如基于牛顿迭代法的内模控制和基于神经网络的内模控制，这些算法有一个缺陷是计算量很大、实时性不好，并且计算量很大、实时性不好。
发明内容
本发明针对现有技术中的缺点，提供了一种基于内模控制算法的移动机器人轨迹跟踪方法及系统。
为了解决上述技术问题，本发明通过下述技术方案得以解决：
一种基于内模控制算法的移动机器人轨迹跟踪方法，包括以下步骤：
分别获取移动机器人在某一具体时刻的状态变量，所述状态变量包括运动轨迹的状态变量、里程计的状态变量和移动机器人的状态变量，通过所述运动轨迹、里程计和状态变量获取实际运动轨迹与预设运动轨迹的偏差；
将获取到的偏差作为内模控制器的输入，进而得到相应时刻的输出；
将所述输出传入移动机器人的电机中，并且获取下一时刻里程计的状态变量，并根据移动机器人的运动学方程获取移动机器人在下一时刻的状态变量的预测值。
作为一种可实施方式，所述状态变量包括机器人在世界坐标系中的横坐标、纵坐标、横向速度和纵向速度；
所述运动轨迹的状态变量包括机器人运动轨迹在世界坐标系的横坐标、运动轨迹的纵坐标、运动轨迹的横向速度和运动轨迹的纵向速度，分别记为xref,yref,
Figure RE-GDA0002026172220000011
所述里程计的状态变量包括机器人里程计在世界坐标系中的横坐标、里程计的纵坐标、横向速度和纵向速度，分别记为x,y,
Figure RE-GDA0002026172220000021
移动机器人运动学方程的状态变量记为
Figure RE-GDA0002026172220000022
获取到的状态变量表示分别表示为：zref,z,
Figure RE-GDA0002026172220000023
即
Figure RE-GDA0002026172220000024
Figure RE-GDA0002026172220000025
其中T表示转置。
作为一种可实施方式，将实际运动轨迹与预设运动轨迹的偏差记作为e，则偏差e的具体计算公式为
Figure RE-GDA0002026172220000026
作为一种可实施方式，所述将获取到的偏差作为内模控制器的输入，进而得到相应时刻的输出，是指将获取到的偏差输入至内模控制器中，所述内模控制器输出相应时刻的输出，具体过程为：
将机器人的运动学模型表示为
Figure RE-GDA0002026172220000027
其中，x表示机器人的横坐标，y表示机器人的纵坐标，θ表示机器人的航向角，v表示机器人的线速度，ω表示机器人的角速度；假设
Figure RE-GDA0002026172220000028
η2＝ω,z1＝x,z2＝y,z3＝vcos(θ),z4＝vsin(θ)，则得到
Figure RE-GDA0002026172220000029
写成矩阵形式则有：
Figure RE-GDA00020261722200000210
假设
Figure RE-GDA00020261722200000211
则有
Figure RE-GDA00020261722200000212
进而得到线性化系统，所述线性化系统表示为：
Figure RE-GDA00020261722200000213
其中，A、B和C表示常数，取线性化系统的输出为机器人的横坐标x和纵坐标y，则
Figure RE-GDA0002026172220000031
进而机器人运动学模型的线性化状态空间表达式为
Figure RE-GDA0002026172220000032
其中，
Figure RE-GDA0002026172220000033
进而得到
Figure RE-GDA0002026172220000034
根据系统的相对阶的定义γ＝min(k:CAk-1B≠0)可知该线性化系统的相对阶为γ＝2，则获得该线性化系统的内模控制的控制变量：
Figure RE-GDA0002026172220000035
其中，φ为控制变量，z表示状态变量，αi(i＝0,...,γ-1)表示调节参数。
作为一种可实施方式，所述将所述输出传入移动机器人的电机中，并且获取下一时刻里程计的状态变量，并根据移动机器人的运动学方程获取移动机器人在下一时刻的状态变量的预测值，具体为：
将控制变量φ反馈到电机驱动机器人进行纠正偏差，并且获取下一采样时刻的状态变量，将控制变量φ输入机器人的运动学模型得到下一采样时刻的z～，再和下一采样时刻的轨迹规划位置信息zref获取偏差e，从而实时纠正偏差跟踪预定轨迹。
一种基于内模控制算法的移动机器人轨迹跟踪系统，包括获取模块、计算模块以及预测模块；
所述获取模块，用于分别获取移动机器人在某一具体时刻的状态变量，所述状态变量包括运动轨迹的状态变量、里程计的状态变量和移动机器人的状态变量，通过所述运动轨迹、里程计和状态变量获取实际运动轨迹与预设运动轨迹的偏差；
所述计算模块，用于将获取到的偏差作为内模控制器的输入，进而得到相应时刻的输出；
所述预测模块，用于将所述输出传入移动机器人的电机中，并且获取下一时刻里程计的状态变量，并根据移动机器人的运动学方程获取移动机器人在下一时刻的状态变量的预测值。
作为一种可实施方式，所述获取模块被设置为：
所述状态变量包括机器人在世界坐标系中的横坐标、纵坐标、横向速度和纵向速度；
所述运动轨迹的状态变量包括机器人运动轨迹在世界坐标系的横坐标、运动轨迹的纵坐标、运动轨迹的横向速度和运动轨迹的纵向速度，分别记为xref,yref,
Figure RE-GDA0002026172220000036
所述里程计的状态变量包括机器人里程计在世界坐标系中的横坐标、里程计的纵坐标、横向速度和纵向速度，分别记为x,y,
Figure RE-GDA0002026172220000041
移动机器人运动学方程的状态变量记为
Figure RE-GDA0002026172220000042
获取到的状态变量表示分别表示为：zref,z,
Figure RE-GDA0002026172220000043
即
Figure RE-GDA0002026172220000044
Figure RE-GDA0002026172220000045
其中T表示转置。
作为一种可实施方式，所述获取模块被设置为：将实际运动轨迹与预设运动轨迹的偏差记作为e，则偏差e的具体计算公式为
Figure RE-GDA0002026172220000046
作为一种可实施方式，所述计算模块被设置为：
将机器人的运动学模型表示为
Figure RE-GDA0002026172220000047
其中，x表示机器人的横坐标，y表示机器人的纵坐标，θ表示机器人的航向角，v表示机器人的线速度，ω表示机器人的角速度；假设
Figure RE-GDA0002026172220000048
η2＝ω,z1＝x,z2＝y,z3＝vcos(θ),z4＝vsin(θ)，则得到
Figure RE-GDA0002026172220000049
写成矩阵形式则有：
Figure RE-GDA00020261722200000410
假设
Figure RE-GDA00020261722200000411
则有
Figure RE-GDA00020261722200000412
进而得到线性化系统，所述线性化系统表示为：
Figure RE-GDA00020261722200000413
其中，A、B和C表示常数，取线性化系统的输出为机器人的横坐标x和纵坐标y，则
Figure RE-GDA00020261722200000414
进而机器人运动学模型的线性化状态空间表达式为
Figure RE-GDA00020261722200000415
其中，
Figure RE-GDA00020261722200000416
进而得到
Figure RE-GDA0002026172220000051
根据系统的相对阶的定义γ＝min(k:CAk-1B≠0)可知该线性化系统的相对阶为γ＝2，则获得该线性化系统的内模控制的控制变量：
Figure RE-GDA0002026172220000052
其中，φ为控制变量，z表示状态变量，αi(i＝0,...,γ-1)表示调节参数。
作为一种可实施方式，所述预测模块被设置为：
将控制变量φ反馈到电机驱动机器人进行纠正偏差，并且获取下一采样时刻的状态变量，将控制变量φ输入机器人的运动学模型得到下一采样时刻的z～，再和下一采样时刻的轨迹规划位置信息zref获取偏差e，从而实时纠正偏差跟踪预定轨迹。
本发明由于采用了以上技术方案，具有显著的技术效果：
通过本发明的方法和系统能实现一种计算量小、实时性好的改进的内模控制的机器人轨迹跟踪方法。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明方法的流程示意图；
图2是本发明系统的整体结构示意图；
图3是本发明的移动机器人示意图；
图4是本发明的内模控制算法结构示意图。
具体实施方式
下面结合实施例对本发明做进一步的详细说明，以下实施例是对本发明的解释而本发明并不局限于以下实施例。
实施例1：
一种基于内模控制算法的移动机器人轨迹跟踪方法，如图1所示，包括以下步骤：
S100、分别获取移动机器人在某一具体时刻的状态变量，所述状态变量包括运动轨迹的状态变量、里程计的状态变量和移动机器人的状态变量，通过所述运动轨迹、里程计和状态变量获取实际运动轨迹与预设运动轨迹的偏差；
S200、将获取到的偏差作为内模控制器的输入，进而得到相应时刻的输出；
S300、将所述输出传入移动机器人的电机中，并且获取下一时刻里程计的状态变量，并根据移动机器人的运动学方程获取移动机器人在下一时刻的状态变量的预测值。
通过本发明的方法实现一种计算量小、实时性好的改进的内模控制的机器人轨迹跟踪方法。
在S100中，所述状态变量包括机器人在世界坐标系中的横坐标、纵坐标、横向速度和纵向速度；
所述运动轨迹的状态变量包括机器人运动轨迹在世界坐标系的横坐标、运动轨迹的纵坐标、运动轨迹的横向速度和运动轨迹的纵向速度，分别记为xref,yref,
Figure RE-GDA0002026172220000061
所述里程计的状态变量包括机器人里程计在世界坐标系中的横坐标、里程计的纵坐标、横向速度和纵向速度，分别记为x,y,
Figure RE-GDA0002026172220000062
移动机器人运动学方程的状态变量记为
Figure RE-GDA0002026172220000063
获取到的状态变量表示分别表示为：zref,z,
Figure RE-GDA0002026172220000064
即
Figure RE-GDA0002026172220000065
Figure RE-GDA0002026172220000066
其中T表示转置。
更进一步地，将实际运动轨迹与预设运动轨迹的偏差记作为e，则偏差e的具体计算公式为
Figure RE-GDA0002026172220000067
在步骤S200中，所述将获取到的偏差作为内模控制器的输入，进而得到相应时刻的输出，是指将获取到的偏差输入至内模控制器中，所述内模控制器输出相应时刻的输出，具体过程为：
将机器人的运动学模型表示为
Figure RE-GDA0002026172220000068
其中，x表示机器人的横坐标，y表示机器人的纵坐标，θ表示机器人的航向角，v表示机器人的线速度，ω表示机器人的角速度；假设
Figure RE-GDA0002026172220000069
η2＝ω,z1＝x,z2＝y,z3＝vcos(θ),z4＝vsin(θ)，则得到
Figure RE-GDA00020261722200000610
写成矩阵形式则有：
Figure RE-GDA00020261722200000611
假设
Figure RE-GDA00020261722200000612
则有
Figure RE-GDA00020261722200000613
进而得到线性化系统，所述线性化系统表示为：
Figure RE-GDA0002026172220000071
其中，A、B和C表示常数，取线性化系统的输出为机器人的横坐标x和纵坐标y，则
Figure RE-GDA0002026172220000072
进而机器人运动学模型的线性化状态空间表达式为
Figure RE-GDA0002026172220000073
其中，
Figure RE-GDA0002026172220000074
进而得到
Figure RE-GDA0002026172220000075
根据系统的相对阶的定义γ＝min(k:CAk-1B≠0)可知该线性化系统的相对阶为γ＝2，则获得该线性化系统的内模控制的控制变量：
Figure RE-GDA0002026172220000076
其中，φ为控制变量，z表示状态变量，αi(i＝0,...,γ-1)表示调节参数。
在此具体实施例中，对于一个线性状态空间模型：
Figure RE-GDA0002026172220000077
其中，x是一个n维状态向量，A是一个n×n的矩阵，B是一个n×m的矩阵，C是一个p×n 的矩阵，y是一个p维输出向量，其相对阶的定义如下：
γ＝min(k:CAk-1B≠0)
因此：
y(k)＝CAkx,1≤k≤γ-1
y(γ)＝CAγx+CAγ-1Bu
选取偏差e和y(k),0≤k≤γ-1的线性组合来表示y(γ)：
y(γ)＝α0e-α0y-α1y(1)-...-αγ-1y(γ-1)
其中αk≥0,0≤k≤γ-1是调节参数，对上式两边做Laplace变换，记y的Laplace变换为Y(s)， e的Laplace变换为E(s)，则：
sγY(s)＝α0E(s)-α0Y(s)-α1sY(s)-...-αγ-1sγ-1Y(s)
因此：
Figure RE-GDA0002026172220000081
Figure RE-GDA0002026172220000082
选取合适的αk≥0,0≤k≤γ-1可以把上式写成：
Figure RE-GDA0002026172220000083
如图4，是一幅内模控制示意图，在内模控制中偏差
Figure RE-GDA0002026172220000084
如果模型Model完全匹配系统Plant，则：
Figure RE-GDA0002026172220000085
因此：
e＝yref,E(s)＝Yref(s)
因此：
Figure RE-GDA0002026172220000086
当ε→0时，
Figure RE-GDA0002026172220000087
因此：
y→yref
此时内模控制变量u的选取可以由下面的方程推导出来：
y(γ)＝CAγx+CAγ-1Bu＝α0e-α0y-α1y(1)-...-αγ-1y(γ-1)＝α0e-α0Cx-α1CAx-...-αγ-1CAγ-1x
因此：
Figure RE-GDA0002026172220000088
其中，u为控制变量，x表示状态变量，αi(i＝0,...,γ-1)表示调节参数。
所以，当内模控制变量u如上式选取时，y→yref也就是y能很好的跟踪yref，因此进行预测时还选取了内模控制变量u。
在步骤S300中，所述将所述输出传入移动机器人的电机中，并且获取下一时刻里程计的状态变量，并根据移动机器人的运动学方程获取移动机器人在下一时刻的状态变量的预测值，具体为：
将控制变量u反馈到电机驱动机器人进行纠正偏差，并且获取下一采样时刻的状态变量，将控制变量u输入机器人的运动学模型得到下一采样时刻的z～，再和下一采样时刻的轨迹规划位置信息zref获取偏差e，从而实时纠正偏差跟踪预定轨迹。
更加具体地，根据移动机器人的运动学方程获取移动机器人在tk+1时刻的状态变量的预测值，具体实施步骤如下：根据移动机器人的运动学方程：
Figure RE-GDA0002026172220000091
写出其相应的离散方程：
Figure RE-GDA0002026172220000092
给定采样周期SamplePeriod，在已知移动机器人在tk时刻的状态变量的情况下，就可以根据上面的离散方程计算移动机器人在tk+1时刻的状态变量的预测值。
实施例2：
一种基于内模控制算法的移动机器人轨迹跟踪系统，如图2所示，包括获取模块100、计算模块200以及预测模块300；
所述获取模块100，用于分别获取移动机器人在某一具体时刻的状态变量，所述状态变量包括运动轨迹的状态变量、里程计的状态变量和移动机器人的状态变量，通过所述运动轨迹、里程计和状态变量获取实际运动轨迹与预设运动轨迹的偏差；
所述计算模块200，用于将获取到的偏差作为内模控制器的输入，进而得到相应时刻的输出；
所述预测模块300，用于将所述输出传入移动机器人的电机中，并且获取下一时刻里程计的状态变量，并根据移动机器人的运动学方程获取移动机器人在下一时刻的状态变量的预测值。
更进一步地，所述获取模块100被设置为：所述状态变量包括机器人在世界坐标系中的横坐标、纵坐标、横向速度和纵向速度；
所述运动轨迹的状态变量包括机器人运动轨迹在世界坐标系的横坐标、运动轨迹的纵坐标、运动轨迹的横向速度和运动轨迹的纵向速度，分别记为xref,yref,
Figure RE-GDA0002026172220000093
所述里程计的状态变量包括机器人里程计在世界坐标系中的横坐标、里程计的纵坐标、横向速度和纵向速度，分别记为x,y,
Figure RE-GDA0002026172220000101
移动机器人运动学方程的状态变量记为
Figure RE-GDA0002026172220000102
获取到的状态变量表示分别表示为：zref,z,
Figure RE-GDA0002026172220000103
即
Figure RE-GDA0002026172220000104
Figure RE-GDA0002026172220000105
其中T表示转置。
所述获取模块100还被设置为：将实际运动轨迹与预设运动轨迹的偏差记作为e，则偏差e的具体计算公式为
Figure RE-GDA0002026172220000106
更加具体地，所述计算模块200被设置为：将机器人的运动学模型表示为
Figure RE-GDA0002026172220000107
其中，x表示机器人的横坐标，y表示机器人的纵坐标，θ表示机器人的航向角，v表示机器人的线速度，ω表示机器人的角速度；
假设
Figure RE-GDA0002026172220000108
η2＝ω,z1＝x,z2＝y,z3＝vcos(θ),z4＝vsin(θ)，则得到
Figure RE-GDA0002026172220000109
写成矩阵形式则有：
Figure RE-GDA00020261722200001010
假设
Figure RE-GDA00020261722200001011
则有
Figure RE-GDA00020261722200001012
进而得到线性化系统，所述线性化系统表示为：
Figure RE-GDA00020261722200001013
其中，A、B和C表示常数，取线性化系统的输出为机器人的横坐标x和纵坐标y，则
Figure RE-GDA00020261722200001014
进而机器人运动学模型的线性化状态空间表达式为
Figure RE-GDA00020261722200001015
其中，
Figure RE-GDA00020261722200001016
进而得到
Figure RE-GDA0002026172220000111
根据系统的相对阶的定义γ＝min(k:CAk-1B≠0)可知该线性化系统的相对阶为γ＝2，则获得该线性化系统的内模控制的控制变量：
Figure RE-GDA0002026172220000112
其中，φ为控制变量，z表示状态变量，αi(i＝0,...,γ-1)表示调节参数。
另外，所述预测模块300被设置为：将控制变量φ反馈到电机驱动机器人进行纠正偏差，并且获取下一采样时刻的状态变量，将控制变量φ输入机器人的运动学模型得到下一采样时刻的z～，再和下一采样时刻的轨迹规划位置信息zref获取偏差e，从而实时纠正偏差跟踪预定轨迹。通过本系统，也能实现一种计算量小、实时性好的改进的内模控制的机器人轨迹跟踪方法。
对于装置实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
本领域内的技术人员应明白，本发明的实施例可提供为方法、装置、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明的方法、终端设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
需要说明的是：
说明书中提到的“一个实施例”或“实施例”意指结合实施例描述的特定特征、结构或特性包括在本发明的至少一个实施例中。因此，说明书通篇各个地方出现的短语“一个实施例”或“实施例”并不一定均指同一个实施例。
尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
此外，需要说明的是，本说明书中所描述的具体实施例，其零、部件的形状、所取名称等可以不同。凡依本发明专利构思所述的构造、特征及原理所做的等效或简单变化，均包括于本发明专利的保护范围内。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，只要不偏离本发明的结构或者超越本权利要求书所定义的范围，均应属于本发明的保护范围。

Claims (4)

1.一种基于内模控制算法的移动机器人轨迹跟踪方法，其特征在于包括以下步骤：
分别获取移动机器人在某一具体时刻的状态变量，所述状态变量包括运动轨迹的状态变量、里程计的状态变量和移动机器人的状态变量，通过所述运动轨迹、里程计和状态变量获取实际运动轨迹与预设运动轨迹的偏差；
将获取到的偏差作为内模控制器的输入，进而得到相应时刻的输出，所述输出为内模控制器的控制变量；
将所述输出传入移动机器人的电机中，并且获取下一时刻里程计的状态变量，并根据移动机器人的运动学方程获取移动机器人在下一时刻的状态变量的预测值；
其中，所述状态变量包括机器人在世界坐标系中的横坐标、纵坐标、横向速度和纵向速度；
所述运动轨迹的状态变量包括机器人运动轨迹在世界坐标系的横坐标、运动轨迹的纵坐标、运动轨迹的横向速度和运动轨迹的纵向速度，分别记为xref,yref,
Figure FDA0003391380160000011
所述里程计的状态变量包括机器人里程计在世界坐标系中的横坐标、里程计的纵坐标、横向速度和纵向速度，分别记为x,y,
Figure FDA0003391380160000012
移动机器人运动学方程的状态变量记为
Figure FDA0003391380160000013
获取到的状态变量表示分别表示为：zref,z,
Figure FDA0003391380160000014
即
Figure FDA0003391380160000015
Figure FDA0003391380160000016
其中T表示转置；
其中，将实际运动轨迹与预设运动轨迹的偏差记作为e，则偏差e的具体计算公式为
Figure FDA0003391380160000017
其中，所述将获取到的偏差作为内模控制器的输入，进而得到相应时刻的输出，是指将获取到的偏差输入至内模控制器中，所述内模控制器输出相应时刻的输出，具体过程为：
Figure FDA0003391380160000018
将机器人的运动学模型表示为
Figure FDA0003391380160000019
其中，x表示机器人的横坐标，y表示机器人的纵坐标，θ表示机器人的航向角，v表示机器人的线速度，ω表示机器人的角速度；假设
Figure FDA00033913801600000110
η2＝ω,z1＝x,z2＝y,z3＝vcos(θ),z4＝vsin(θ)，则得到
Figure FDA00033913801600000111
写成矩阵形式则有：
Figure FDA0003391380160000021
假设
Figure FDA0003391380160000022
则有
Figure FDA0003391380160000023
进而得到线性化系统，所述线性化系统表示为：
Figure FDA0003391380160000024
其中，A、B和C表示常数，取线性化系统的输出为机器人的横坐标x和纵坐标y，则
Figure FDA0003391380160000025
进而机器人运动学模型的线性化状态空间表达式为
Figure FDA0003391380160000026
其中，
Figure FDA0003391380160000027
进而得到
Figure FDA0003391380160000028
根据系统的相对阶的定义γ＝min(k:CAk-1B≠0)可知该线性化系统的相对阶为γ＝2，则获得该线性化系统的内模控制的控制变量：
Figure FDA0003391380160000029
其中，φ为控制变量，z表示状态变量，αi(i＝0,...,γ-1)表示调节参数。
2.根据权利要求1基于内模控制算法的移动机器人轨迹跟踪方法，其特征在于，所述将所述输出传入移动机器人的电机中，并且获取下一时刻里程计的状态变量，并根据移动机器人的运动学方程获取移动机器人在下一时刻的状态变量的预测值，具体为：
将控制变量φ反馈到电机驱动机器人进行纠正偏差，并且获取下一采样时刻的状态变量，将控制变量φ输入机器人的运动学模型得到下一采样时刻的z～，再和下一采样时刻的轨迹规划位置信息zref获取偏差e，从而实时纠正偏差跟踪预定轨迹。
3.一种基于内模控制算法的移动机器人轨迹跟踪系统，其特征在于，包括获取模块、计算模块以及预测模块；
所述获取模块，用于分别获取移动机器人在某一具体时刻的状态变量，所述状态变量包括运动轨迹的状态变量、里程计的状态变量和移动机器人的状态变量，通过所述运动轨迹、里程计和状态变量获取实际运动轨迹与预设运动轨迹的偏差；
所述计算模块，用于将获取到的偏差作为内模控制器的输入，进而得到相应时刻的输出；
所述预测模块，用于将所述输出传入移动机器人的电机中，并且获取下一时刻里程计的状态变量，并根据移动机器人的运动学方程获取移动机器人在下一时刻的状态变量的预测值；其中，所述获取模块被设置为：
所述状态变量包括机器人在世界坐标系中的横坐标、纵坐标、横向速度和纵向速度；
所述运动轨迹的状态变量包括机器人运动轨迹在世界坐标系的横坐标、运动轨迹的纵坐标、运动轨迹的横向速度和运动轨迹的纵向速度，分别记为xref,yref,
Figure FDA0003391380160000031
所述里程计的状态变量包括机器人里程计在世界坐标系中的横坐标、里程计的纵坐标、横向速度和纵向速度，分别记为x,y,
Figure FDA0003391380160000032
移动机器人运动学方程的状态变量记为
Figure FDA0003391380160000033
获取到的状态变量表示分别表示为：zref,z,
Figure FDA0003391380160000034
即
Figure FDA0003391380160000035
Figure FDA0003391380160000036
其中T表示转置；其中，所述获取模块被设置为：将实际运动轨迹与预设运动轨迹的偏差记作为e，则偏差e的具体计算公式为
Figure FDA0003391380160000037
其中，所述计算模块被设置为：
将机器人的运动学模型表示为
Figure FDA0003391380160000038
其中，x表示机器人的横坐标，y表示机器人的纵坐标，θ表示机器人的航向角，v表示机器人的线速度，ω表示机器人的角速度；假设
Figure FDA0003391380160000039
η2＝ω,z1＝x,z2＝y,z3＝vcos(θ),z4＝vsin(θ)，则得到
Figure FDA00033913801600000310
写成矩阵形式则有：
Figure FDA00033913801600000311
假设
Figure FDA00033913801600000312
则有
Figure FDA00033913801600000313
进而得到线性化系统，所述线性化系统表示为：
Figure FDA0003391380160000041
其中，A、B和C表示常数，取线性化系统的输出为机器人的横坐标x和纵坐标y，则
Figure FDA0003391380160000042
进而机器人运动学模型的线性化状态空间表达式为
Figure FDA0003391380160000043
其中，
Figure FDA0003391380160000044
进而得到
Figure FDA0003391380160000045
根据系统的相对阶的定义γ＝min(k:CAk-1B≠0)可知该线性化系统的相对阶为γ＝2，则获得该线性化系统的内模控制的控制变量：
Figure FDA0003391380160000046
其中，φ为控制变量，z表示状态变量，αi(i＝0,...,γ-1)表示调节参数。
4.根据权利要求3基于内模控制算法的移动机器人轨迹跟踪系统，其特征在于，所述预测模块被设置为：
将控制变量φ反馈到电机驱动机器人进行纠正偏差，并且获取下一采样时刻的状态变量，将控制变量φ输入机器人的运动学模型得到下一采样时刻的z～，再和下一采样时刻的轨迹规划位置信息zref获取偏差e，从而实时纠正偏差跟踪预定轨迹。
CN201910097059.XA 2019-01-31 2019-01-31 基于内模控制算法的移动机器人轨迹跟踪方法及系统 Active CN109857108B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910097059.XA CN109857108B (zh) 2019-01-31 2019-01-31 基于内模控制算法的移动机器人轨迹跟踪方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910097059.XA CN109857108B (zh) 2019-01-31 2019-01-31 基于内模控制算法的移动机器人轨迹跟踪方法及系统

Publications (2)

Publication Number Publication Date
CN109857108A CN109857108A (zh) 2019-06-07
CN109857108B true CN109857108B (zh) 2022-04-01

Family

ID=66897160

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910097059.XA Active CN109857108B (zh) 2019-01-31 2019-01-31 基于内模控制算法的移动机器人轨迹跟踪方法及系统

Country Status (1)

Country Link
CN (1) CN109857108B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110865405A (zh) * 2019-11-29 2020-03-06 浙江商汤科技开发有限公司 融合定位方法及装置、移动设备控制方法及电子设备
CN111829551B (zh) * 2020-06-19 2022-04-12 浙江华睿科技股份有限公司 一种机器人里程计的获取方法、装置和电子设备
CN113381511B (zh) * 2021-06-02 2022-12-27 深圳市莱达四维信息科技有限公司 变电站在线智能巡视系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103728988A (zh) * 2013-12-24 2014-04-16 江南大学 基于内模的scara机器人轨迹跟踪控制方法
CN105487385A (zh) * 2016-02-01 2016-04-13 金陵科技学院 基于无模型自适应内模控制方法
CN108365787A (zh) * 2018-03-23 2018-08-03 东南大学 一种基于内模控制的永磁同步电机调速系统及其设计方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103728988A (zh) * 2013-12-24 2014-04-16 江南大学 基于内模的scara机器人轨迹跟踪控制方法
CN105487385A (zh) * 2016-02-01 2016-04-13 金陵科技学院 基于无模型自适应内模控制方法
CN108365787A (zh) * 2018-03-23 2018-08-03 东南大学 一种基于内模控制的永磁同步电机调速系统及其设计方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Internal model control for improving the gait tracking of a compliant humanoid robot;Luca Colasanto等;《2012 IEEE/RSJ International Conference on Intelligent Robots and Systems》;IEEE;20121224;第5347-5352页 *
SCARA 机器人内模控制方法;赵登步等;《计算机工程与应用》;华北计算技术研究所;20140815;第52卷(第5期);第22-26页 *
The Controller Design of Quadrotor UAV Based on Internal Model Control;Gao, Qiang等;《36th Chinese Control Conference (CCC)》;IEEE;20170911;第505-510页 *
基于预测控制的非连续路段下移动机器人的轨迹跟踪;吴菁等;《计算机测量与控制》;20161025;第24卷(第10期);第74-78页 *
轮式移动机器人的轨迹跟踪与编队控制研究;骆伟宾;《中国优秀硕士学位论文全文数据库 信息科技辑》;中国学术期刊（光盘版）电子杂志社;20180715(第07期);第1-83页 *

Also Published As

Publication number Publication date
CN109857108A (zh) 2019-06-07

Similar Documents

Publication Publication Date Title
CN109857108B (zh) 基于内模控制算法的移动机器人轨迹跟踪方法及系统
Wang et al. Robust H∞ attitude tracking control of a quadrotor UAV on SO (3) via variation-based linearization and interval matrix approach
CN109017984B (zh) 一种无人车的轨迹跟随控制方法、控制系统及相关装置
CN113428218B (zh) 一种车辆转向控制方法、装置、设备及存储介质
CN110221541B (zh) 伺服系统中前馈控制器的前馈系数获取装置、方法
CN107272677A (zh) 一种移动机器人的变结构自适应轨迹跟踪控制方法
EP2188685A1 (en) Inverse kinematics
CN108829123B (zh) 一种四旋翼飞行器控制方法、系统和装置
US20230415333A1 (en) Center of mass planning method for robot, robot and computer-readable storage medium
CN111553239A (zh) 一种机器人关节视觉伺服控制方法、终端设备及存储介质
CN113110423B (zh) 步态轨迹规划方法、装置、计算机可读存储介质及机器人
CN111597702A (zh) 一种火箭着陆轨迹规划方法及装置
Xia et al. Robust adaptive super-twisting sliding mode formation controller for homing of multi-underactuated AUV recovery system with uncertainties
CN111230860B (zh) 机器人控制方法、装置、计算机设备及存储介质
Rosmann et al. Time-optimal nonlinear model predictive control with minimal control interventions
CN109108964B (zh) 一种基于自适应动态规划Nash博弈的空间机械臂协调控制方法
CN112597612B (zh) 机器人优化方法、装置、终端设备及计算机可读存储介质
CN113064350B (zh) 一种导弹助推段自适应动态面控制方法和装置
CN115981159A (zh) 基于模型辅助和类史密斯预估的广义改进自抗扰控制方法
CN114415698B (zh) 机器人、机器人的定位方法、装置和计算机设备
CN111413995B (zh) 双刚体特征点间相对位置跟踪与姿态同步控制方法及系统
Copot et al. Visual predictive control architecture based on image moments for manipulator robots
Tiefensee et al. Digital implementation of backstepping controllers via input/Lyapunov matching
CN112596377A (zh) 双驱式设备直线运动控制方法、装置及双驱式设备
CN111390905A (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