一种轮式移动机器人三闭环编队轨迹跟踪控制方法与流程

本发明涉及机器人技术领域，具体涉及一种轮式移动机器人三闭环编队轨迹跟踪控制方法。

背景技术：

目前移动机器人编队控制技术广泛应用于远地作业、协助军事行动、震后搜索与营救、清除危险区域、农业机器人、自动化工厂等，吸引了众多学者的关注。多机器人编队是指多个机器人形成一定的几何图形完成指定的目标任务，且在协作运动过程中保持队形。目前常用的控制方法有跟随领航者法、人工势场法、虚拟结构法等。各方法都具有局限性，没有统一的控制方法适用于各种场合。其中，中国专利《一种基于跟随领航者编队的多移动机器人控制系统》(申请日：2018.7.25；申请号：cn201810828028.2；公开日：2018.12.21；专利号：cn109062204a)公开了一种基于跟随领航者编队的多移动机器人控制系统，提高了控制系统可靠性和实时性，但控制系统依赖于领航机器人，容错性较差；中国专利《基于观测器的多移动机器人自适应编队跟踪控制方法》(申请日：2018.11.06；申请号：cn201811314441.3；公开日：2019-03-19；专利号：cn109491381a)公开了一种基于观测器的多移动机器人自适应编队跟踪控制方法，缓解当前多机器人系统运行中机器人系统状态信息不完全可测,以及领航机器人控制输入信息未知的情况，但由于观测器的观测误差导致整个控制系统的误差增量上升；中国专利《机器人轨迹跟踪方法及装置》公开了一种机器人轨迹跟踪方法及装置，应用双闭环滑模控制器实现了快速轨迹跟踪，但抖振问题仍然存在，且在实际跟踪的初始阶段机器人的方向无法做到与目标方向一致造成闭环系统不稳定。

技术实现要素：

本发明的目的在于克服现有技术的不足，提供一种轮式移动机器人三闭环编队轨迹跟踪控制方法，保证了闭环系统的稳定性。

本发明的目的是通过以下技术方案来实现的：一种轮式移动机器人三闭环编队轨迹跟踪控制方法，包括以下步骤：

s1.建立单个机器人的运动学模型，求解该机器人的运动学方程；

s2.对跟随机器人与虚拟领航者进行相对位姿分析，得相对误差方程；

s3.设计轮式机器人的控制系统，包括：

根据运动学方程和相对误差方程，引入中间控制量，设计中间控制率和线速度控制率；

根据误差方程设计角速度控制率；

s4.通过调节中间控制率、速度控制率和角速度控制率中的参数，使内环收敛速度依次大于中环、外环收敛速度。

进一步地，所述步骤s1包括以下子步骤：

s101.建立单个机器人的运动学模型：

设c为机器人两驱动轮连线轴几何中心，2b为机器人两驱动轮之间的距离，r为机器人的驱动轮半径，m为机器人质心，θi为机器人航向角，wi为驱动轮的角速度，vi为驱动轮的线速度，d为质心与几何中心的距离；

s102.根据运动学模型，求解该机器人的运动学方程：

s103.将角度θi、角速度wi控制设为内环，线速度vi控制为中环，轮式机器人的位置坐标(xi,yi)调节为外环。

进一步地，所述步骤s2包括以下子步骤：

s201.设多个轮式移动机器人编队采用虚拟领航者法，参与编队的i个机器人为跟随者，以将要形成固定队形的几何中心为参考点，i＝1，2，3，…,n，其中n表示参与编队的机器人总数，所述参考点即为各机器人的虚拟领航者；

s202.给定虚拟领航者的参考轨迹为qd＝[xidyidθid],ud＝[vidwid]，由步骤s1中的运动学方程，得跟随机器人的位姿为qi＝[xiyiθi],ui＝[viwi]，根据跟随机器人i与虚拟领航者的几何关系，得到跟随机器人i与虚拟领航者相对位姿为pid＝[pxidpyidpθidpvidpwid]^t，其中：

其中，δx＝xid-xi，δy＝yid-yi；

s203.定义δid＝[δxidδyidδθidδvidδwid]^t为参考相对位姿的理想值；

则相对误差方程为：

pid-δid；

控制目标为通过设计控制率使pid-δid→0，实现跟随机器人快速准确的跟踪领航者。

进一步地，所述步骤s3中，设计中间控制率和线速度控制率的过程包括以下子步骤：

设轮式机器人系统为欠驱动系统，为克服欠驱动问题，引入中间控制量u′i＝[vixviy]^t,则：

其中vix表示线速度vi在x轴上的分量，viy表示线速度在y轴上的分量，进而得到跟随机器人理想角度θid的值域为(-π/2,π/2)，即：

θid＝arctan(viy/vix)；

为了调整系统的开环增益，提高系统的稳态精度，降低系统的惰性，加快响应速度，采用p控制，针对第i个机器人设计中间控制率u′is为:

中间控制率u′is为控制和求解u′i的参数，在量上认为u′i和u′is相同，也就是使u′i等于设计的u′is；

其中kp＞0，vid为第i个机器人需要跟踪的理想线速度；

根据得实际线速度vi的控制率vis为：

vis为控制和求解vi的参数，在量上认为vi和vis相同，也就是使vi等于得到的vis；

其中kv＞0，根据上述vi、θid、u′is的计算公式，得到实际线速度控制率、理想角度以及位置(x，y)控制率；位置(x，y)控制率即pixy的控制率,在数值上与pixy相等，通过u′is求解得到；利用李雅普诺夫稳定性定理，能够证明当t→∞时，pixy-δixy→0，vi→vid且指数收敛。

进一步地，所述步骤s3中，设计角速度控制率的过程包括以下子步骤：

通过设计角速度控制率wis，实现角度θi跟踪随动目标θid，角速度控制率wis为控制和求解wi的参数，在量上认为wi和wis相同，也就是使wi等于设计的wis；

将步骤s202的pθid＝θid-θi左右求导，得到其中，为pθid的导数，为θid的导数，wi为θi求导后得到角速度；设计角速度控制率为

取lyapunov函数为：则：

即系统稳定，角度θi指数收敛于θid。

其中，所述步骤s4中调节的参数包括kp、kv和kw，通过调节这些参数，使内环收敛速度依次大于中环、外环收敛速度，使各跟随机器人与虚拟领航者保持一定的距离和角度完成编队。

进一步地，步骤s4中每一次进行参数调节后，利用lyapunov理论验证控制系统的稳定性，并在控制系统不稳定时，重新调整参数kp、kv和kw。

本发明的有益效果是：本发明在虚拟领航者系统结构下，将角度(θi)、角速度(wi)控制设为内环，线速度(vi)控制为中环，位置坐标(xi,yi)调节为外环。为了实现稳定的三闭环控制，采用内环收敛速度>中环收敛速度>外环收敛速度的方法，通过θi超快速跟踪θid，vi快速跟踪vid，达到内环收敛速度依次大于中环、外环收敛速度，从而保证闭环系统的稳定性。

附图说明

图1为本发明的方法流程图；

图2为轮式移动机器人运动学模型示意图；

图3为跟随机器人与虚拟领航者位姿误差示意图；

图4为实施例中的三闭环控制原理示意图。

具体实施方式

下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于以下所述。

如图1所示，一种轮式移动机器人三闭环编队轨迹跟踪控制方法，包括以下步骤：

s1.建立单个机器人的运动学模型，求解该机器人的运动学方程；

s2.对跟随机器人与虚拟领航者进行相对位姿分析，得相对误差方程；

s3.设计轮式机器人的控制系统，包括：

根据运动学方程和相对误差方程，引入中间控制量，设计中间控制率和线速度控制率；

根据误差方程设计角速度控制率；

s4.通过调节中间控制率、速度控制率和角速度控制率中的参数，使内环收敛速度依次大于中环、外环收敛速度。

其中，所述步骤s1包括以下子步骤：

s101.建立单个机器人的运动学模型：

如图2所示，设c为机器人两驱动轮连线轴几何中心，2b为机器人两驱动轮之间的距离，r为机器人的驱动轮半径，m为机器人质心，θi为机器人航向角，wi为驱动轮的角速度，vi为驱动轮的线速度，d为质心与几何中心的距离；

s102.根据运动学模型，求解该机器人的运动学方程：

s103.将角度θi、角速度wi控制设为内环，线速度vi控制为中环，轮式机器人的位置坐标(xi,yi)调节为外环。

其中，所述步骤s2包括以下子步骤：

s201.设多个轮式移动机器人编队采用虚拟领航者法，参与编队的i个机器人为跟随者，以将要形成固定队形的几何中心为参考点，i＝1，2，3，…,n，其中n表示参与编队的机器人总数，所述参考点即为各机器人的虚拟领航者；

s202.如图3所示，给定虚拟领航者的参考轨迹为qd＝[xidyidθid],ud＝[vidwid]，由步骤s1中的运动学方程，得跟随机器人的位姿为qi＝[xiyiθi],ui＝[viwi]，根据跟随机器人i与虚拟领航者的几何关系，得到跟随机器人i与虚拟领航者相对位姿为pid＝[pxidpyidpθidpvidpwid]^t，其中：

其中，δx＝xid-xi，δy＝yid-yi；

s203.定义δid＝[δxidδyidδθidδvidδwid]^t为参考相对位姿的理想值；

则相对误差方程为：

pid-δid；

控制目标为通过设计控制率使pid-δid→0，实现跟随机器人快速准确的跟踪领航者。

所述步骤s3中，设计中间控制率和线速度控制率的过程包括以下子步骤：

设轮式机器人系统为欠驱动系统，为克服欠驱动问题，引入中间控制量u′i＝[vixviy]^t,则：

其中vix表示线速度vi在x轴上的分量，viy表示线速度在y轴上的分量，进而得到跟随机器人理想角度θid的值域为(-π/2,π/2)，即：

θid＝arctan(viy/vix)；

为了调整系统的开环增益，提高系统的稳态精度，降低系统的惰性，加快响应速度，采用p控制，针对第i个机器人设计中间控制率u′is为:

中间控制率u′is为控制和求解u′i的参数，在量上认为u′i和u′is相同，也就是使u′i等于设计的u′is；

其中kp＞0，vid为第i个机器人需要跟踪的理想线速度；

根据得实际线速度vi的控制率vis为：

vis为控制和求解vi的参数，在量上认为vi和vis相同，也就是使vi等于得到的vis；

其中kv＞0，根据上述vi、θid、u′is的计算公式，得到实际线速度控制率、理想角度以及位置(x，y)控制率；位置(x，y)控制率即pixy的控制率,在数值上与pixy相等，通过u′is求解得到；利用李雅普诺夫稳定性定理，能够证明当t→∞时，pixy-δixy→0，vi→vid且指数收敛。

所述步骤s3中，设计角速度控制率的过程包括以下子步骤：

通过设计角速度控制率wis，实现角度θi跟踪随动目标θid，角速度控制率wis为控制和求解wi的参数，在量上认为wi和wis相同，也就是使wi等于设计的wis；

将步骤s202的pθid＝θid-θi左右求导，得到其中，为pθid的导数，为θid的导数，wi为θi求导后得到角速度；设计角速度控制率为

取lyapunov函数为：则：

即系统稳定，角度θi指数收敛于θid。

其中，所述步骤s4中调节的参数包括kp、kv和kw，通过调节这些参数，使内环收敛速度依次大于中环、外环收敛速度，使各跟随机器人与虚拟领航者保持一定的距离和角度完成编队。

步骤s4中每一次进行参数调节后，利用lyapunov理论验证控制系统的稳定性，并在控制系统不稳定时，重新调整参数kp、kv和kw。

在本申请的实施例中，基于本申请中的步骤s3设计的实际线速度控制率、位置控制率和姿态控制率，构建相应的位置子系统控制器，线速度子系统控制器和角速度子系统控制器，对轮式机器人的位置子系统，线速度子系统和角速度子系统实现三闭环控制，控制原理如图4所示。

综上，本发明在虚拟领航者系统结构下，将角度(θi)、角速度(wi)控制设为内环，线速度(vi)控制为中环，位置坐标(xi,yi)调节为外环。为了实现稳定的三闭环控制，采用内环收敛速度>中环收敛速度>外环收敛速度的方法，通过θi超快速跟踪θid，vi快速跟踪vid，达到内环收敛速度依次大于中环、外环收敛速度，从而保证闭环系统的稳定性。

以上所述是本发明的优选实施方式，应当理解本发明并非局限于本文所披露的形式，不应该看作是对其他实施例的排除，而可用于其他组合、修改和环境，并能够在本文所述构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。