基于控制系统的机器人运动研究

Research on Robot Motion Based on Control System

摘要: 本文针对欠驱动自平衡两轮车的运动学和动力学特征，设计了一种解耦分层滑模模态控制方法，根据被控系统的拉格朗日力学模型，把系统的运动和动力学模型分为两个子系统，速度控制子系统和转向控制子系统，针对速度控制子系统，提出了分层滑模控制策略，针对转向控制子系统，设计了有限时间滑模控制策略，所设计的控制系统具有全局稳定性。可以较好地实现运动控制和转向控制。最后，将所提出的控制算法应用于两轮自平衡车，仿真结果表明了良好的收敛性和性能。

Abstract: According to the kinematics and dynamics characteristics of the under-driven self-balancing two-wheeled vehicle, a decoupling layered sliding mode control method is designed. According to the Lagrange mechanics model of the controlled system, the motion and dynamics model of the system is divided into two subsystems, the speed control subsystem and the steering control sub-system. For the speed control subsystem, a layered sliding mode control strategy is proposed. For the steering control subsystem, a finite-time sliding mode control strategy is designed, and the de-signed control system has global stability. Motion control and steering control can be well realized. Finally, the proposed control algorithm is applied to the two-wheel self-balancing vehicle, and the simulation results show good convergence and performance.

1. 引言

近年来，两轮自平衡机器人已得到广泛应用 [1] 。这种类型的运动体本质上是强耦合、非线性的以及不稳定的系统 [2] [3] 。它一般具有左右两侧的两个车轮，为了完成预定任务，需要施加适当的控制以使机器人保持平衡 [4] 。在干扰影响下，随着前向运动和倾斜角度存在的条件下，欲使机器人保持平衡，必须对机器人进行控制 [5] [6] 。

由于两轮自平衡机器人是内在不稳定的系统，需要对其进行主动控制保持其直立位置 [7] 。为了满足这一要求，其轮子被独立驱动以确保机器人均衡 [8] 。为此，设计了各种线性和非线性控制器。使用的线性控制器主要有基于PID (比例积分微分)、LQR (线性二次调节器)、LQG (线性高斯控制)和极点放置方法及其组合 [9] [10] [11] [12] 。此外，非线性控制器已用于两轮自平衡机器人。常用的非线性控制器包括反推和滑模控制 [13] [14] ，模糊逻辑控制 [15] [16] ，神经网络 [17] ，Lyapunov控制函数 [18] ，非线性最优控制 [19] 和强化学习 [20] 。这些控制器可以确保两轮机器人的跟踪控制与稳定。近年来，滑模控制发展迅速，滑模控制具有控制精度高，鲁棒性强，抗干扰性强等优点，已广泛应用于各种控制系统。本文基于滑模控制理论，针对欠驱动自平衡两轮车的运动学和动力学特征，设计了一种解耦分层滑模模态控制方法，根据被控系统的拉格朗日力学模型，把系统的运动和动力学模型分为两个子系统，速度控制子系统和转向控制子系统，针对速度控制子系统，提出了分层滑模控制策略，针对转向控制子系统，设计了有限时间滑模控制策略。

本文的内容安排如下：第一部分引言介绍了研究对象的应用背景。第二部分建立了被控对象的数学模型，推导出了两轮平衡车的动力学模型。第三部分介绍了控制器的设计策略。第四部分对所建立的模型进行了仿真。第五部分对仿真结果进行了分析和讨论。

2. 控制对象运动数学模型

本文所研究的自平衡车的机械结构由两个轮子由两个步进器驱动的机器人本体电机(SKU955965)。电机由Arduino产生的电信号控制Uno微控制器(PWM信号)，由L298H驱动器(H桥电路)产生控制信号，基于惯性测量获得的测量数据单元(MPU 6050 IMU)IMU提供倾角、当前角速度和加速度读数数据。如图1所示。本文采用文 [16] 中所建立的运动学模型。

Figure 1. Schematic diagram of the actual two-wheeled balancing vehicle and its coordinate system

图1. 两轮平衡车实物和坐标系示意图

平衡车的动力学模型可以由公式(1)表示

$\left(\begin{array}{c}\begin{array}{c}{\stackrel{\dot{}}{x}}_{RM}\\ {\stackrel{\dot{}}{v}}_{RM}\\ {\stackrel{\dot{}}{\theta }}_r\end{array}\\ \begin{array}{c}{\stackrel{\dot{}}{\omega }}_p\\ {\stackrel{\dot{}}{\theta }}_y\\ {\stackrel{\dot{}}{\omega }}_y\end{array}\end{array}\right)=\left(\begin{array}{cc}\begin{array}{ccc}0& 1& 0\\ 0& A_{22}& A_{23}\\ 0& 0& 0\end{array}& \begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\end{array}\\ \begin{array}{ccc}0& A_{42}& A_{43}\\ 0& 0& 0\\ 0& 0& 0\end{array}& \begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& 0& A_{66}\end{array}\end{array}\right)\left(\begin{array}{c}\begin{array}{c}{x}_{RM}\\ v_{RM}\\ {\theta }_r\end{array}\\ \begin{array}{c}{\omega }_p\\ {\theta }_y\\ {\omega }_y\end{array}\end{array}\right)+\left(\begin{array}{c}\begin{array}{c}\begin{array}{cc}0& 0\end{array}\\ \begin{array}{cc}{B}_{21}& 0\end{array}\\ \begin{array}{cc}0& 0\end{array}\end{array}\\ \begin{array}{c}\begin{array}{cc}{B}_{41}& 0\end{array}\\ \begin{array}{cc}0& 0\end{array}\\ \begin{array}{cc}0& B_{62}\end{array}\end{array}\end{array}\right)\left(\begin{array}{c}{U}_L\\ U_R\end{array}\right)+\left(\begin{array}{c}\begin{array}{c}0\\ f_2\\ 0\end{array}\\ \begin{array}{c}{f}_4\\ 0\\ f_6\end{array}\end{array}\right)$ (1)

式中， ${\stackrel{\dot{}}{x}}_{RM}$ 为小车沿 $x_{RM}$ 方向的速度， ${\stackrel{\dot{}}{v}}_{RM}$ 为小车沿 $x_{RM}$ 方向的加速度， ${\omega }_p$ 为小车倾斜角速度， ${\omega }_y$ 为小车转向角速度， $A_{ij}\left(i=1,2,\cdots ,6;j=1,2,\cdots ,6\right)$ ， $B_{ij}\left(i=1,2;j=1,2,\cdots ,6\right)$ 为系统的动力学参数， $f_i\left(i=2,4,6\right)$ 为系统干扰项。各参数由下面表达式给出：

$A_{22}=-2p_1{M}_{P}RL\left(L+R\right)/m-2p_1{J}_{p\theta }R/m$

$A_{23}=-M_P^2{L}^2{R}^{2}g/m$

$A_{42}=-2p_1/\left(J_{p\theta }+L{M}_P\right)$

$A_{43}=L{M}_P/\left(J_{p\theta }+L{M}_P\right)$

$B_{21}=p_2{M}_{P}RL\left(L+R\right)/m+2p_2{J}_{p\theta }R/m=B_{22}$

$B_{41}=p_2/\left(J_{p\theta }+L{M}_P\right)=B_{42}$

又因为

$\begin{array}{l}{U}_L=k_m\left(U_P-\stackrel{\dot{}}{\theta }\right)/r_m\\ U_R=k_m\left(U_y-\stackrel{\dot{}}{\theta }\right)/r_m\end{array}$

其中 $k_m$ 为转矩常数， $r_m$ 为电枢电阻。根据本研究对象的参数值，有

$\left(\begin{array}{c}{U}_L\\ U_R\end{array}\right)=\left(\begin{array}{cc}\frac{1}{2}& \frac{1}{2}\\ \frac{1}{2}& -\frac{1}{2}\end{array}\right)\left(\begin{array}{c}{U}_P\\ U_y\end{array}\right)$ (2)

把(2)式代入(1)式，有

$\left(\begin{array}{c}\begin{array}{c}{\stackrel{\dot{}}{x}}_{RM}\\ {\stackrel{\dot{}}{v}}_{RM}\\ {\stackrel{\dot{}}{\theta }}_r\end{array}\\ \begin{array}{c}{\stackrel{\dot{}}{\omega }}_p\\ {\stackrel{\dot{}}{\theta }}_y\\ {\stackrel{\dot{}}{\omega }}_y\end{array}\end{array}\right)=\left(\begin{array}{cc}\begin{array}{ccc}0& 1& 0\\ 0& A_{22}& A_{23}\\ 0& 0& 0\end{array}& \begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\end{array}\\ \begin{array}{ccc}0& A_{42}& A_{43}\\ 0& 0& 0\\ 0& 0& 0\end{array}& \begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& 0& A_{66}\end{array}\end{array}\right)\left(\begin{array}{c}\begin{array}{c}{x}_{RM}\\ v_{RM}\\ {\theta }_r\end{array}\\ \begin{array}{c}{\omega }_p\\ {\theta }_y\\ {\omega }_y\end{array}\end{array}\right)+\left(\begin{array}{c}\begin{array}{c}\begin{array}{cc}0& 0\end{array}\\ \begin{array}{cc}{B}_{21}& 0\end{array}\\ \begin{array}{cc}0& 0\end{array}\end{array}\\ \begin{array}{c}\begin{array}{cc}{B}_{41}& 0\end{array}\\ \begin{array}{cc}0& 0\end{array}\\ \begin{array}{cc}0& B_{62}\end{array}\end{array}\end{array}\right)\left(\begin{array}{c}{U}_P\\ U_y\end{array}\right)+\left(\begin{array}{c}\begin{array}{c}0\\ f_2\\ 0\end{array}\\ \begin{array}{c}{f}_4\\ 0\\ f_6\end{array}\end{array}\right)$ (3)

在(3)式中，变量 $x_{RM},v_{RM},{\theta }_P,{\omega }_P$ 构成纵向运动控制系统，用于平衡车的速度控制和平衡，变量 ${\theta }_y,{\omega }_y$ 构成转向控制系统。 $U_P$ 和 $U_y$ 可以分别看做是纵向运动和转向运动的控制输入。这样，(3)式可以解耦为

$\left(\begin{array}{c}\begin{array}{c}{\stackrel{\dot{}}{x}}_{RM}\\ {\stackrel{\dot{}}{v}}_{RM}\\ {\stackrel{\dot{}}{\theta }}_r\end{array}\\ {\stackrel{\dot{}}{\omega }}_p\end{array}\right)=\left(\begin{array}{cc}\begin{array}{ccc}0& 1& 0\\ 0& A_{22}& A_{23}\\ 0& 0& 0\end{array}& \begin{array}{c}0\\ 0\\ 1\end{array}\\ \begin{array}{ccc}0& A_{42}& A_{43}\end{array}& 0\end{array}\right)\left(\begin{array}{c}\begin{array}{c}{x}_{RM}\\ v_{RM}\\ {\theta }_r\end{array}\\ {\omega }_p\end{array}\right)+\left(\begin{array}{c}0\\ B_{21}\\ 0\\ B_{41}\end{array}\right)U_P+\left(\begin{array}{c}0\\ f_2\\ 0\\ f_4\end{array}\right)$ (4)

$\left(\begin{array}{c}{\stackrel{\dot{}}{\theta }}_y\\ {\stackrel{\dot{}}{\omega }}_y\end{array}\right)=\left(\begin{array}{cc}0& 1\\ 0& A_{66}\end{array}\right)\left(\begin{array}{c}{\theta }_y\\ {\omega }_y\end{array}\right)U_y+\left(\begin{array}{c}0\\ f_6\end{array}\right)$ (5)

由(4)可以看出，平衡车纵向位移与摆杆倾角构成了四阶非线性系统，其控制问题难度较大。由(5)式可以看出，平衡车转向运动为具有独立输入的双积分系统，其控制问题已经得到较好的解决。

3. 控制器设计策略

滑模控制是一种非线性控制方法，应对系统不确定性和外部干扰非常有效。分层滑模控制策略是在传统滑模控制的基础上发展起来的，分层滑模控制策略主要用来解决多阶耦合欠驱动系统。本文研究对象平衡车的纵向控制系统式(4)就是欠驱动的耦合系统，一个独立输入要控制四个状态变量，这四个状态变量可以分为如下两个系统：

$\{\begin{array}{l}{\stackrel{\dot{}}{x}}_{RM}=v_{RM}\hfill \\ {\stackrel{\dot{}}{v}}_{RM}=A_{22}{v}_{RM}+B_{21}{U}_P+f_2\hfill \end{array}$ (6)

$\{\begin{array}{l}{\stackrel{\dot{}}{\theta }}_r={\omega }_p\hfill \\ {\stackrel{\dot{}}{\omega }}_p=A_{42}{v}_{RM}+A_{43}{\theta }_r+B_{41}{U}_P+f_4\hfill \end{array}$ (7)

下面来设计分层滑模控制器

令 $x_d$ 表示期望位移， $v_d$ 表示期望速度， ${\theta }_d$ 表示期望倾角， ${\omega }_d$ 表示期望倾角速度，定义跟踪误差如下

$\{\begin{array}{l}{e}_x=x_{RM}-x_d\hfill \\ e_v=v_{RM}-v_d\hfill \\ e_{\theta }={\theta }_r-{\theta }_d\hfill \\ e_{\omega }={\omega }_p-{\omega }_d\hfill \end{array}$ (8)

针对系统(6)和(7)，定义如下滑模面

$\{\begin{array}{l}{S}_1=c_1{e}_x+e_v\hfill \\ S_2=c_2{e}_{\theta }+e_{\omega }\hfill \end{array}$ (9)

(9)式中， $c_1$ 和 $c_2$ 为可调节的系统参数，且均为正数，基于这两个滑模面，定义第二层滑模面：

$S=m{S}_1+n{S}_2$ (10)

其中， $m,n$ 为控制增益，可控制系统向滑模面的收敛性能。

沿滑模面 $S_1$ 求导，可得

$\begin{array}{c}{\stackrel{\dot{}}{S}}_1=c_1{\stackrel{\dot{}}{e}}_x+{\stackrel{\dot{}}{e}}_v=c_1{\stackrel{\dot{}}{e}}_x+\left({\stackrel{\dot{}}{v}}_{RM}-{\stackrel{\dot{}}{v}}_d\right)\\ =c_1{\stackrel{\dot{}}{e}}_x+A_{22}{v}_{RM}+A_{23}{\theta }_p+B_{21}{U}_{eq1}-{\stackrel{\dot{}}{v}}_d\end{array}$

令 ${\stackrel{\dot{}}{S}}_1=0$ ，可以得到

$u_{eq1}=\frac{-c_1{\stackrel{\dot{}}{e}}_x-A_{22}{v}_{RM}-A_{23}{\theta }_p-{\stackrel{\dot{}}{v}}_d}{B_{21}}$ (11)

同理可得

$u_{eq2}=\frac{-c_2{\stackrel{\dot{}}{e}}_{\theta }-A_{22}{v}_{RM}-A_{23}{\theta }_p-{\stackrel{\dot{}}{v}}_d}{B_{41}}$ (12)

设 $U_{PH}=u_{eq1}+u_{eq2}+u_{sw}$

其中 $u_{sw}=-\left(m{B}_{21}{u}_{eq1}+n{B}_{41}{u}_{eq2}+\eta fal\left(S,\rho ,\sigma \right)-\eta s\right)$

$fal\left(S,\rho ,\sigma \right)=\{\begin{array}{l}{\left|S\right|}^{\rho }sign\left(S\right),\left|S\ge \sigma \right|\hfill \\ S/{\sigma }^{\rho -1},\left|S<\sigma \right|\hfill \end{array}$ (13)

其中 $0<\rho <1,0<\sigma <1$

为验证所设计的控制器的稳定性，利用李雅普诺夫理论，设计李雅普诺夫函数如下

$V_1=\frac{1}{2}{S}^2$

则

${\stackrel{\dot{}}{V}}_1=S\stackrel{\dot{}}{S}=S\left(m{\stackrel{\dot{}}{S}}_1+n{\stackrel{\dot{}}{S}}_2\right)$

故

$\begin{array}{c}{\stackrel{\dot{}}{V}}_1={\stackrel{\dot{}}{x}}_e{x}_e+{\stackrel{\dot{}}{y}}_e{y}_e\\ =\left({\omega }_d{y}_e-v+v_d\right)x_e-{\omega }_d{x}_e{y}_e\\ =-k_0{\omega }_d{x}_{e}sign\left({\omega }_d\right)x_e-{\omega }_d{x}_e{y}_e\\ =-k_0{x}_e^2{\omega }_{d}sign\left({\omega }_d\right)-k_0{y}_e^2{\omega }_{d}sign\left({\omega }_d\right)\le 0\end{array}$

故系统为渐近稳定的。

4. 仿真结果

假设小车的初始倾斜角度为3度，小车由静止状态起步，在所设计的控制器的作用下，小车的运动轨迹通过matlab simulink进行仿真，仿真结果如下：

Figure 2. The curve of the inclination angle of the balancing vehicle changing with time

图2. 平衡车倾斜角随时间变化曲线

Figure 3. Balance car speed change curve with time

图3. 平衡车速度随时间变化曲线

Figure 4. Voltage variation curve with time

图4. 电压随时间变化曲线

5. 结论

由图2可以看出，小车的倾斜角度较快的收敛到平衡状态，经过约1.8 s的时间，超调量较小。由图3可以看出，小车的速度由10 m/s减至0 m/s，速度变化较为平稳，收敛时间快，至平衡状态误差小于0.01 cm，精度较高。由图4可以看出，电压变化比较平滑，比较容易控制。仿真结果表明，所设计的控制器具有较好的控制性能。