一种二维运动移动机器人的迭代学轨迹跟踪控制及其鲁棒优化方法

摘要

本发明公开了一种二维运动移动机器人的迭代学轨迹跟踪控制及其鲁棒优化方法。首先建立二维运动移动机器人离散非线性运动系统模型的动力学方程；并建立离散非线性状态空间表达式；构建基于迭代学控制技术的P型开闭环迭代学控制器；然后对构建的离散非线性控制系统的鲁棒收敛性进行理论分析；进而对P型控制器的控制增益进行参数拆项，同时设计一种基于控制器参数的二次性能指标函数，目的对控制参数进行优化；最后分析优化控制算法作用于被控系统时输出误差的单调收敛特性及参数选择条件，实现二维运动移动机器人快速、高精度跟踪上期望运动轨迹。其优点是：鲁棒优化迭代学控制器不仅适用于理想状态下的跟踪控制，而且适用于外界存在干扰情况下的轨迹跟踪任务；设计的迭代算法简单高效，不需要引入大量附加参数变量，易于工程实现。

主权项

一种二维运动移动机器人的迭代学习轨迹跟踪控制及其鲁棒优化方法，其特征包括：构建移动机器人离散非线性系统动力学方程；设计常规P型开闭环迭代学习控制器；鲁棒迭代学习轨迹跟踪控制算法的收敛性分析；进一步优化鲁棒迭代学习轨迹跟踪控制器；系统输出误差的鲁棒单调性分析；给出鲁棒优化迭代学习控制方案的具体实施；第一步：构建移动机器人离散非线性系统动力学方程二维运动移动机器人实际物理模型如式(1)所示，机器人当前位置z(t)点在广义坐标中定义为[s_z(t),p_z(t),θ_z(t)]，s_z(t)和p_z(t)为直角坐标系下z(t)的坐标，θ_z(t)为机器人的方位角；当机器人的标定方向为地理坐标系的横轴正半轴时，θ_z(t)定义为0；移动机器人受不完全约束的影响而只能在驱动轮轴的方向运动，点z(t)的线速度和角速度定义为和$\left[\begin{array}{c}{s}_z(t+1)\\ p_z(t+1)\\ {\theta }_z(t+1)\end{array}\right]=\left[\begin{array}{c}{s}_z\\ p_z\\ {\theta }_z\end{array}\right]+\Delta T\left[\begin{array}{cc}{\mathrm{cos\theta }}_z\left(t\right)& 0\\ {\mathrm{sin\theta }}_z\left(t\right)& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\bar{\upsilon }}_z\left(t\right)\\ {\bar{\omega }}_z\left(t\right)\end{array}\right]---\left(1\right)$其中：ΔT为采样时间；定义状态向量x(t)＝[s_z(t),p_z(t),θ_z(t)]^T表示移动机器人在采样点处的s坐标量，p坐标量和角度量；速度向量移动机器人的输入量在点z(t)处的线速度和角速度定义为和定义变量矩阵$B(t,x(t\left)\right)=\Delta T\left[\begin{array}{cc}{\mathrm{cos\theta }}_z\left(t\right)& 0\\ {\mathrm{sin\theta }}_z\left(t\right)& 0\\ 0& 1\end{array}\right];$考虑到二维运动移动机器人受外界状态干扰因素和输出受扰因素的影响，且系统初态与期望初值不严格一致情况，将式(1)表示为如(2)形式的状态空间方程：$\left\{\begin{array}{c}{x}_k(t+1)=f(t,x_k(t))+B(t,x_k(t))u_k\left(t\right)+{\omega }_k\left(t\right)\\ y_k\left(t\right)=g(t,x_k(t))+v_k\left(t\right)\end{array}\right.---\left(2\right)$式中0≤t≤T，T为系统运动周期,x_k(t)∈Rⁿ,u_k(t)∈R^r,y_k(t)∈R^m分别表示移动机器人第k次运行时的状态量,控制输入量和输出量，即移动机器人在坐标系中的实际s坐标、p坐标及角度坐标运行轨迹；ω_k(t)∈Rⁿ，v_k(t)∈R^m分别为系统第k次运行时的状态和输出干扰量，且对于任意的k＞0，t∈{0,1,…,T}，必然||ω_k(t)||≤b_ω，||v_k(t)||≤b_v；f(t,x_k(t))和B(t,x_k(t))为系统第k次运行时的非线性系统矩阵函数，f(·)、B(·)关于x满足一致全局Lipschitz条件，函数g(t,x_k(t))存在_x的偏导数，满足上确界要求：$g_{x_k\left(t\right)}^{\prime }(t,x_k(t\left)\right)=\frac{\partial g(t,x_k(t))}{\partial x_k\left(t\right)},b_{g_x}=sup\left|\right|g_{x_k\left(t\right)}^{\prime }(t,x_k(t\left)\right)\left|\right|,$且在不注明情况下，均简写为系统的初态满足条件第二步：设计常规P型开闭环迭代学习控制器针对非线性移动机器人系统(2)设计P型开闭环迭代学习控制律：u_k+1(t)＝u_k(t)+L_k+1(t)e_k(t+1)+Γ_k+1(t)e_k+1(t)   (3)式中其中L_k+1(t)，Γ_k+1(t)为迭代学习的增益矩阵，由于实际控制器增益有界，因此$\underset{t\in [0,T]}{sup}\left|\right|L_{k+1}\left(t\right)\left|\right|\le b_L,\underset{t\in [0,T]}{sup}\left|\right|{\Gamma }_{k+1}\left(t\right)\left|\right|\le b_{\Gamma };$第三步：鲁棒迭代学习轨迹跟踪控制器收敛性分析定义输出跟踪误差：e_k(t)＝y_d(t)‑y_k(t)，则：$\begin{array}{c}\left|\right|{\mathrm{\delta u}}_{k+1}\left(t\right)\left|\right|\le \left|\right|I-L_{k+1}\left(t\right)g_{{\tilde{x}}_k(t+1)}^{\prime }B(t,x_k(t\left)\right)\left|\right|\left|\right|{\mathrm{\delta u}}_k\left(t\right)\left|\right|+\left|\right|L_{k+1}\left(t\right)\left|\right|\left|\right|g_{{\tilde{x}}_k(t+1)}^{\prime }\left|\right|\left(\right|\left|f\right(t,{\mathrm{\delta x}}_k\left(t\right)\left)\right||\\ +\left|\right|B(t,{\mathrm{\delta x}}_k(t)\left)\right|\left|\right|\left|u_d\left(t\right)\right|\left|\right)+\left|\right|L_{k+1}\left(t\right)\left|\right|\left|\right|g_{{\tilde{x}}_k(t+1)}^{\prime }\left|\right|\left|\right|{\omega }_k\left(t\right)\left|\right|+\left|\right|L_{k+1}\left(t\right)\left|\right|\left|\right|v_k(t+1)\left|\right|\\ +\left|\right|{\Gamma }_{k+1}\left(t\right)\left|\right|\left|\right|v_{k+1}(t+1)\left|\right|+\left|\right|{\Gamma }_{k+1}\left(t\right)\left|\right|\left|\right|g_{{\tilde{x}}_{k+1}\left(t\right)}^{\prime }\left|\right|{\mathrm{\delta x}}_{k+1}\left(t\right)\left|\right|\end{array}---\left(4\right)$||δx_k(t+1)||≤||f(t,δx_k(t))||+||B(t,δx_k(t))||||u_d(t)||+||B(t,x_k(t))||||δu_k(t)||+||ω_k(t)||   (5)其中为x_k(t+1)与x_d(t+1)之间的某个取值，为x_k+1(t)与x_d(t)之间的某个取值；令$\underset{t\in [0,T]}{sup}\left|\right|u_d\left(t\right)\left|\right|=b_{u_d},\underset{t\in [0,T]}{sup}\left|\right|B(t,x_k(t)\left)\right||=b_B,c_1=k_f+k_B{b}_{u_d},$进而可得：$\left|\right|{\mathrm{\delta x}}_k\left(t\right)\left|\right|\le c_1^t\left|\right|{\mathrm{\delta x}}_k\left(0\right)\left|\right|+\underset{j=0}{\overset{t-1}{\Sigma }}{c}_1^{t-1-j}\left(b_B\right|\left|\right({\mathrm{\delta u}}_k\left(j\right)\left|\right|+b_{\omega })---\left(6\right)$令：$\rho =sup\left|\right|I-L_{k+1}\left(t\right)g_{{\tilde{x}}_k(t+1)}^{\prime }B(t,x_k(t\left)\right)\left|\right|,b_{g_{x_1}}=sup\left|\right|g_{{\tilde{x}}_k(t+1)}^{\prime }\left|\right|,b_{g_{x_2}}=sup\left|\right|g_{{\tilde{x}}_{k+1}\left(t\right)}^{\prime }\left|\right|,$得到：$\left|\right|{\mathrm{\delta u}}_{k+1}\left(t\right)\left|\right|\le \rho \left|\right|{\mathrm{\delta u}}_k\left(t\right)\left|\right|+b_{\Gamma }{b}_{g_{x_2}}\left|\right|{\mathrm{\delta x}}_{k+1}\left(t\right)\left|\right|+b_L{b}_{g_{x_1}}(k_f+k_B{b}_{u_d})\left|\right|{\mathrm{\delta x}}_k\left(t\right)\left|\right|+b_L{b}_{g_{x_1}}{b}_{\omega }+b_L{b}_v+b_{\Gamma }{b}_v---\left(7\right)$接下来，将式(6)代入式(7)中,并令$c_3=b_{\Gamma }{b}_{g_{x2}},c_2=b_L{b}_{g_{x1}}(k_f+k_B{b}_{u_d}),$两端同时乘以λ^t，并取λ范数立即可得:$\left|\right|{\mathrm{\delta u}}_{k+1}\left(t\right)|{|}_{\lambda }\le \bar{\rho }|\left|{\mathrm{\delta u}}_k\left(t\right)\right|{|}_{\lambda }+\theta ---\left(8\right)$其中：$\bar{\rho }=\frac{(\rho +c_2{b}_B\lambda \frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1})}{(1-c_3{b}_B\lambda \frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1})},\theta =\frac{c_2{b}_{x_0}+c_3{b}_{x_0}+c_4}{1-c_3{b}_B\lambda \frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1}}+\frac{c_2{b}_{\omega }\lambda \frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1}+c_3{b}_{\omega }\lambda \frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1}}{1-c_3{b}_B\lambda \frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1}}$即经k次迭代后：$\left|\right|{\mathrm{\delta u}}_{k+1}\left(t\right)|{|}_{\lambda }\le {\bar{\rho }}^{k+1}|\left|{\mathrm{\delta u}}_0\left(t\right)\right|{|}_{\lambda }+\theta \frac{1-{\left(\bar{\rho }\right)}^{k+1}}{1-\bar{\rho }}---\left(9\right)$当λ取足够小，时：$\underset{k\to \infty }{\lim }\left|\right|{\mathrm{\delta u}}_k|{|}_{\lambda }\le \frac{\theta }{1-\bar{\rho }}---\left(10\right)$则：$\left|\right|e_k\left(t\right)|{|}_{\lambda }\le b_{g_x}{b}_{x_0}+b_v+b_{\omega }\lambda \frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1}+{\mathrm{\lambda b}}_B|\left|{\mathrm{\delta u}}_k\right|{|}_{\lambda }\frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1}---\left(11\right)$而且将式(10)代入式(11)中，进一步可得：$\underset{k\to \infty }{\lim }\left|\right|e_k|{|}_{\lambda }\le b_{g_x}{b}_{x_0}+b_v+b_{\omega }\lambda \frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1}+\left({\mathrm{\lambda b}}_B\right)\frac{\theta }{1-\bar{\rho }}\frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1}---\left(12\right)$由式(12)可知，通过k次迭代后，最终跟踪误差会收敛到一定范围以内，即_：$0\le \underset{k\to \infty }{\lim }\left|\right|e_k|{|}_{\lambda }\le ϵ,$其中：$ϵ=b_{g_x}{b}_{x_0}+b_v+b_{\omega }\lambda \frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1}+\left({\mathrm{\lambda b}}_B\right)\frac{\theta }{1-\bar{\rho }}\frac{1-{\left({\mathrm{\lambda c}}_1\right)}^T}{1-{\mathrm{\lambda c}}_1};$特别地，当非线性系统不存在外界因素的干扰且系统初始值与给定期望初态严格一致时_，$\underset{k\to \infty }{\lim }\left|\right|e_k|{|}_{\lambda }=0;$第四步：优化鲁棒迭代学习轨迹跟踪控制器虽然在第三步中鲁棒迭代控制器能够使得移动机器人系统满足稳定性条件，但稳定参数的选择范围较大，稳定参数的人为随意选择将使得系统即使满足稳定性条件通常也会降低最终控制性能；因此需要进一步考虑存在干扰情形下，非线性机器人离散系统(2)的鲁棒优化控制算法，其中t∈{0,1,…,T}为采样时刻，输出表示为：y_k(1)＝H(x_k(0),u_k(0),ω_k(0)+v_k(1))···y_k(T)＝H(x_k(0),u_k(0),…,u_k(T‑1),ω_k(0),…,ω_k(T‑1)+v_k(T))则输出误差可表示为：e_k+1＝e_k‑(y_k+1‑y_k)   (13)由微分中值定理及非线性系统可得：$y_{k+1}-y_k={\tilde{H}}_{k+1}(u_{k+1}-u_k)+\tilde{d}---\left(14\right)$其中表示H(·)关于u_k的Jacobain矩阵在u_k+1(t)和u_k(t)之间的取值，为含初值不严格一致的扰动总量；为方便求解，将增益矩阵改写成一种特殊形式：L_k+1＝α_k+1W，Γ_k+1＝β_k+1M,其中W，M分别为与L_k+1和Γ_k+1相同结构和维数的构造矩阵，α_k+1、β_k+1分别为参量，则迭代学习控制律可改写成：u_k+1‑u_k＝α_k+1We_k+β_k+1Me_k+1   (15)得到e_k+1，即$e_{k+1}=e_k-{\tilde{H}}_{k+1}({\alpha }_{k+1}{\mathrm{We}}_k+{\beta }_{k+1}{\mathrm{Me}}_{k+1})-\tilde{d};$进一步整理得到：$e_{k+1}={(I+{\tilde{H}}_{k+1}{\beta }_{k+1}M)}^{-1}\left(\right(I-{\tilde{H}}_{k+1}{\alpha }_{k+1}W)-\tilde{d})---\left(16\right)$考虑性能指标：其中调节量ψ＞0_，Q＝I_；利用最优控制原理中的极值原理可以得到：$\left\{\begin{array}{c}{\alpha }_{k+1}^{*}=\arg \min \left[J_{k+1}({\alpha }_{k+1},{\beta }_{k+1})\right]\\ {\beta }_{k+1}^{*}=\mathrm{argmin}\left[J_{k+1}({\alpha }_{k+1},{\beta }_{k+1})\right]\end{array}\right.---\left(18\right)$注意到这里求出的与将分别含有ψ和说明ψ与对α_k+1取最优解具有调节作用，同时对β_k+1也具有调节作用，因此需对ψ和求偏导求其相应的最优解，即：综合以上优化分析条件建立多目标优化函数，得到最优化参数代入迭代学习律中即可得到鲁棒优化迭代学习的P型开闭环控制器；第五步：系统输出跟踪误差的鲁棒单调性分析考虑性能指标函数式(17)：当取非最优化解α_k+1＝0，β_k+1＝0，则：因此，||e_k+1||≤||e_k||，即误差单调收敛，||e_k+1||的极限必然存在；根据可知误差会收敛到优化后的有界误差范围内；实际上，对于确定性系统，由于$\underset{k\to \infty }{\lim }(u_{k+1}-u_k)=0,\underset{k\to \infty }{\lim }\left|\right|e_{k+1}|{|}^2=\underset{k\to \infty }{\lim }|\left|e_k\right|{|}^2=0,$则误差将收敛到零；综合式(20)和式(21)可得：经k次迭代后有：实际上，在理想状态下系统不存在干扰，且初值严格一致的条件下，对于P型开环的参数优化式(27)中e₀＝0，最终而对于非线性系统采用P型开闭环控制律，由于随机干扰的存在，且初值不严格一致，则将会无限逼近于零，但无法达到，而对于由式(23)移项整理可知其存在一定的上限；第六步：具体鲁棒单调优化迭代学习控制方案实施迭代学习轨迹跟踪算法具体的鲁棒优化方案如下：1)针对被控移动机器人系统(2)，设定系统期望初始状态x_k(0)，初始控制u₀，期望轨迹y_d，采样周期ΔT；2)给定最大跟踪误差精度ε_max；3)设定系统批次运行时的初值x_k(0)，在干扰量存在条件下运行系统并记录时刻对应的输出误差和干扰量值的大小，给定适当的W和M，带入多目标优化函数性能指标得到得到最优化控制律；4)对第k+1批次控制量作用于被控系统，产生跟踪误差若在允许最大跟踪误差精度范围以内，结束迭代过程，否则回到3)中重新设定相应初值，继续迭代，直到达到要求误差精度范围；将上述鲁棒单调优化迭代学习控制器在FPGA芯片EP1C6T144C8上实现，输入量为移动机器人的线速度和角速度分别经由扭矩传感器和旋转角度传感器检测得到的信号，输入信号通过调理电路由FPGA核心中央处理芯片按性能指标进行程序计算得出优化参数，并构建鲁棒单调优化控制器，CPU程序计算得到的输出信号为FPGA经迭代信号处理后得到最优化控制器控制信号再经RS232通信模块作用于机器人控制系统，不断修正二维运动移动机器人的跟踪轨迹，直到达到设定要求，误差允许在一定范围内保持。