[发明专利]一种轮式移动机器人三闭环编队轨迹跟踪控制方法

权利要求书

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分别为中间控制率、速度控制率和角速度控制率中的参数。