CN111208830B - 一种轮式移动机器人三闭环编队轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了一种轮式移动机器人三闭环编队轨迹跟踪控制方法，包括以下步骤：S1.建立单个机器人的运动学模型，求解该机器人的运动学方程；S2.对跟随机器人与虚拟领航者进行相对位姿分析，得相对误差方程；S3.设计轮式机器人的控制***，包括：根据运动学方程和相对误差方程，引入中间控制量，设计中间控制率和速度控制率；根据误差方程设计角速度控制率；S4.通过调节中间控制率、速度控制率和角速度控制率中的参数，使内环收敛速度依次大于中环、外环收敛速度。本发明提供了一种轮式移动机器人三闭环编队轨迹跟踪控制方法，保证了闭环***的稳定性。

Description

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

技术领域

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

背景技术

目前移动机器人编队控制技术广泛应用于远地作业、协助军事行动、震后搜索与营救、清除危险区域、农业机器人、自动化工厂等，吸引了众多学者的关注。多机器人编队是指多个机器人形成一定的几何图形完成指定的目标任务，且在协作运动过程中保持队形。目前常用的控制方法有跟随领航者法、人工势场法、虚拟结构法等。各方法都具有局限性，没有统一的控制方法适用于各种场合。其中，中国专利《一种基于跟随领航者编队的多移动机器人控制***》(申请日：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为机器人航向角，w_i为驱动轮的角速度，v_i为驱动轮的线速度，d为质心与几何中心的距离；

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

S103.将角度θ_i、角速度w_i控制设为内环，线速度v_i控制为中环，轮式机器人的位置坐标(x_i,y_i)调节为外环。

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

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

S202.给定虚拟领航者的参考轨迹为q_d＝[x_id y_id θ_id],u_d＝[v_id w_id]，由步骤S1中的运动学方程，得跟随机器人的位姿为q_i＝[x_i y_i θ_i],u_i＝[v_i w_i]，根据跟随机器人i与虚拟领航者的几何关系，得到跟随机器人i与虚拟领航者相对位姿为p_id＝[p_xid p_yid p_θid p_vidp_wid]^T，其中：

其中，Δx＝x_id-x_i，Δy＝y_id-y_i；

S203.定义δ_id＝[δ_xid δ_yid δ_θid δ_vid δ_wid]^T为参考相对位姿的理想值；

则相对误差方程为：

p_id-δ_id；

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

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

设轮式机器人***为欠驱动***，为克服欠驱动问题，引入中间控制量u′_i＝[v_ixv_iy]^T,则：

其中v_ix表示线速度v_i在X轴上的分量，v_iy表示线速度在Y轴上的分量，进而得到跟随机器人理想角度

θ_id的值域为(-π/2,π/2)，即：

θ_id＝arctan(v_iy/v_ix)；

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

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

其中k_p＞0，

v_id为第i个机器人需要跟踪的理想线速度；

根据

得实际线速度v_i的控制率v_is为：

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

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

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

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

将步骤S202的p_θid＝θ_id-θ_i左右求导，得到

其中，

为p_θid的导数，

为θ_id的导数，w_i为θ_i求导后得到角速度；设计角速度控制率为

取

Lyapunov函数为：

则：

即***稳定，角度θ_i指数收敛于θ_id。

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

进一步地，步骤S4中每一次进行参数调节后，利用Lyapunov理论验证控制***的稳定性，并在控制***不稳定时，重新调整参数k_p、k_v和k_w。

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

附图说明

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

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

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

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

具体实施方式

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

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

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

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

S3.设计轮式机器人的控制***，包括：

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

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

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

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

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

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

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

S103.将角度θ_i、角速度w_i控制设为内环，线速度v_i控制为中环，轮式机器人的位置坐标(x_i,y_i)调节为外环。

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

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

S202.如图3所示，给定虚拟领航者的参考轨迹为q_d＝[x_id y_id θ_id],u_d＝[v_id w_id]，由步骤S1中的运动学方程，得跟随机器人的位姿为q_i＝[x_i y_i θ_i],u_i＝[v_i w_i]，根据跟随机器人i与虚拟领航者的几何关系，得到跟随机器人i与虚拟领航者相对位姿为p_id＝[p_xidp_yid p_θid p_vid p_wid]^T，其中：

其中，Δx＝x_id-x_i，Δy＝y_id-y_i；

S203.定义δ_id＝[δ_xid δ_yid δ_θid δ_vid δ_wid]^T为参考相对位姿的理想值；

则相对误差方程为：

p_id-δ_id；

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

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

设轮式机器人***为欠驱动***，为克服欠驱动问题，引入中间控制量u′_i＝[v_ixv_iy]^T,则：

其中v_ix表示线速度v_i在X轴上的分量，v_iy表示线速度在Y轴上的分量，进而得到跟随机器人理想角度

θ_id的值域为(-π/2,π/2)，即：

θ_id＝arctan(v_iy/v_ix)；

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

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

其中k_p＞0，

v_id为第i个机器人需要跟踪的理想线速度；

根据

得实际线速度v_i的控制率v_is为：

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

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

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

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

将步骤S202的p_θid＝θ_id-θ_i左右求导，得到

其中，

为p_θid的导数，

为θ_id的导数，w_i为θ_i求导后得到角速度；设计角速度控制率为

取

Lyapunov函数为：

则：

即***稳定，角度θ_i指数收敛于θ_id。

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

步骤S4中每一次进行参数调节后，利用Lyapunov理论验证控制***的稳定性，并在控制***不稳定时，重新调整参数k_p、k_v和k_w。

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

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

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

Claims (3)

1.一种轮式移动机器人三闭环编队轨迹跟踪控制方法，其特征在于：包括以下步骤：

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

所述步骤S1包括以下子步骤：

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

设C为机器人两驱动轮连线轴几何中心，2b为机器人两驱动轮之间的距离，r为机器人的驱动轮半径，M为机器人质心，θ_i为机器人航向角，w_i为驱动轮的角速度，v_i为驱动轮的线速度，d为质心与几何中心的距离；

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

S103.将角度θ_i、角速度w_i控制设为内环，线速度v_i控制为中环，轮式机器人的位置坐标(x_i,y_i)调节为外环；

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

所述步骤S2包括以下子步骤：

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

S202.给定虚拟领航者的参考轨迹为q_d＝[x_id y_id θ_id],u_d＝[v_id w_id]，由步骤S1中的运动学方程，得跟随机器人的位姿为q_i＝[x_i y_i θ_i],u_i＝[v_i w_i]，根据跟随机器人i与虚拟领航者的几何关系，得到跟随机器人i与虚拟领航者相对位姿为p_id＝[p_xid p_yid p_θid p_vid p_wid]^T，其中：

其中，Δx＝x_id-x_i，Δy＝y_id-y_i；

S203.定义δ_id＝[δ_xid δ_yid δ_θid δ_vid δ_wid]^T为参考相对位姿的理想值；

则相对误差方程为：

p_id-δ_id；

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

S3.设计轮式机器人的控制***，包括：

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

设轮式机器人***为欠驱动***，为克服欠驱动问题，引入中间控制量u_i′＝[v_ix v_iy]^T,则：

其中v_ix表示线速度v_i在X轴上的分量，v_iy表示线速度在Y轴上的分量，进而得到跟随机器人理想角度

θ_id的值域为(-π/2,π/2)，即：

θ_id＝arctan(v_iy/v_ix)；

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

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

其中k_p＞0，

v_id为第i个机器人需要跟踪的理想线速度；

根据

得实际线速度v_i的控制率v_is为：

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

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

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

S4.通过调节中间控制率、速度控制率和角速度控制率中的参数，使内环收敛速度依次大于中环、外环收敛速度；调节的参数包括k_p、k_v和k_w，通过调节这些参数，使内环收敛速度依次大于中环、外环收敛速度，使各跟随机器人与虚拟领航者保持一定的距离和角度完成编队，k_p、k_v和k_w分别为中间控制率、速度控制率和角速度控制率中的参数。

2.根据权利要求1所述的一种轮式移动机器人三闭环编队轨迹跟踪控制方法，其特征在于：所述步骤S3中，设计角速度控制率的过程包括以下子步骤：

通过设计角速度控制率w_is，实现角度θ_i跟踪随动目标θ_id，角速度控制率w_is为控制和求解w_i的参数，在量上认为w_i和w_is相同，也就是使w_i等于设计的w_is：

将步骤S202的p_θid＝θ_id-θ_i左右求导，得到

其中，

为p_θid的导数，

为θ_id的导数，w_i为θ_i求导后得到角速度；设计角速度控制率为

取

k_w＞0，Lyapunov函数为：

则：

即***稳定，角度θ_i指数收敛于θ_id。

3.根据权利要求2所述的一种轮式移动机器人三闭环编队轨迹跟踪控制方法，其特征在于：步骤S4中每一次进行参数调节后，利用Lyapunov理论验证控制***的稳定性，并在控制***不稳定时，重新调整参数k_p、k_v和k_w。

Images