深海集矿机轨迹跟踪算法

王随平，桂卫华，张飞

(中南大学 信息科学与工程学院，湖南 长沙，410083)

摘要：针对深海集矿机控制器难以设计的问题，研究了一种有效的轨迹跟踪算法，使工作于深海底、易受到随机大干扰的集矿机能以小于1 m的误差跟踪规划路径行走。通过对集矿机行走特点和驱动系统分析，建立了集矿机简化的运动学模型。运用双环控制方式实现了集矿机的轨迹跟踪，其中外环采用滑模变结构控制方式实现集矿机位姿误差跟踪，内环运用PID控制器实现了集矿机的速度跟踪。为减弱变结构系统的抖振，在设计趋近律时，采用连续函数代替符号函数。运用MATLAB对设计的轨迹跟踪算法进行仿真实验研究。仿真结果表明，采用等速趋近律的滑模变结构控制的轨迹跟踪算法能够使机器人获得较好的跟踪效果，在较好地消除系统误差的同时，具有较小的抖振。

关键词：深海集矿机；轨迹跟踪；变结构控制

中图分类号：TP391.9          文献标志码：A         文章编号：1672-7207(2011)S2-0319-05

Trace tracking algorithm of deep sea mining vehicle

WANG Sui-ping, GUI Wei-hua, ZHANG Fei

(School of Information Science and Engineering, Central South University, Changsha 410083, China)

Abstract: In consideration of the design problem in deep sea mining vehicle controller, the purpose of researching is to find a tracking control algorithm to make sure the vehicle tracking error less than 1 m. The mining vehicle dynamics model was established upon the movement of the robot and differential steering. The variable pump and motor model were erected. The sliding mode variable structure algorithm was used to eliminate the vehicle pose error. The PID controller was chosen to track velocity. Using the variable structure control theory and the backstep-method, based on the mining vehicle dynamic model, the variable function and sliding controller were designed. Taking into account of the mining vehicle dynamic constraints, the speed and acceleration were limited to ensure that the vehicle smoothly moved. For abating the quiver, a successive function was designed as the reaching law. The mining vehicle control system was simulated and the variable structure control algorithm was testified effectively. This tracking approach enables the vehicle to obtain high tracking precision with a small quiver.

Key words: deep sea mining vehicle; trace tracking; variable structure control (VSC)

深海集矿机工作于约6 km深的海底，海底环境复杂多变，表现为复杂的地表特征及海底洋流等多种形式的未知扰动，实现位置和姿态控制(即轨迹跟踪)，使其按预定轨迹行走作业，在行走过程中实现有效避障是对深海底集矿机的最根本要求。受海底环境的影响，深海采矿普遍采用的是为特殊设计的大功率电机带动的液压驱动低速履带车辆，由于液压系统固有的非线性、海底稀软环境的特殊性、履带车辆动力学表现的不完整约束，轨迹跟踪控制器难以设计^[1]。本文作者在建立集矿机简化的运动学模型的基础上，提出了一种滑模变结构控制轨迹跟踪算法，并通过仿真验证了该算法的有效性。

1  集矿机的运动学模型

集矿机的运动学模型采用地面坐标系O_ex_ey_ez_e和机器人坐标系Ox₁y₁z₁，如图1所示。

图1  集矿机行走轨迹示意图

Fig.1  Schamic of mining vehicle walking track

地面坐标系的原点O_e选在地面某处，例如机器车初始位置；O_ez_e轴垂直于地面并指向上方，称为铅垂轴；O_ex_e轴在水平面可指向任意方向，此处选择为正东方向；O_ey_e选择为正北方向，使该坐标系成为右手系。机器人坐标系原点选在机器车的重心；Ox₁轴沿机器车运动方向并指向前方；Oz₁轴垂直于Ox₁轴并指向上方；Oy₁轴垂直于Ox₁轴和Oz₁轴，其方向使坐标系Ox₁y₁z₁成为右手系。图1中，θ为机器人坐标系ox₁轴与地面坐标系O_ex_e轴的夹角，表示机器人的方向角。

为简化模型分析，本文作者仅考虑集矿机在水平面转向时的运动模型，该模型是二维模型。当不考虑履带的滑移时，在地面坐标系O_ex_ey_ez_e中，外侧履带的速度v_o和内侧履带的速度v_i分别为：

                  (1)

                  (2)

集矿机的前进速度为

                (3)

集矿机航向角速度可表述为

                (4)

其中：r表示主动轮半径；ω_o和ω_i分别是外侧和内侧主动轮的角速度；B为履带中心距。

由式(3)和(4)可得集矿机内/外侧履带速度公式：

              (5)

              (6)

将车体速度v按O_ex_e和O_ey_e轴分解，令，，其中(x，y)为集矿机的位置，θ为集矿机前进方向和X轴的夹角，v和ω分别为集矿机的线速度和航向角速度，在运动学模型中它们是控制输入。可得集矿机的运动学方程为：

            (7)

深海集矿机从位姿移动到位姿，令，集矿机在新坐标系中的坐标为：

             (8)

根据坐标变换公式，可得描述集矿机位姿的误差方程为：

     (9)

由位姿误差方程可得，集矿机位姿误差微分方程为：

       (10)

集矿机运动学模型的轨迹跟踪即寻找控制输入，使对于任意的初始误差，式(10)能在该控制输入作用下，使得有界，且

。

2  变结构控制器的设计

2.1  变结构控制器的设计方法

通常来说，变结构控制^[2]的设计可以分为两个步骤：首先是设计适当的切换函数或切换流形使得系统进入滑动模运动后具有良好的动态特性；其次是设计变结构控制律使得系统在有限时间内到达切换流形并保持在它上面运动。

步骤1  切换函数的设计。

考虑形如式的仿射非线性控制系统。设选取的切换函数为，则可以求出其滑动模态方程为

     (11)

进一步将方程(11)分解成如下形式：

              (12)

其中，状态变量分解为，，。

假定所设计的切换函数S关于x₂的偏导数为m，则由隐函数定理知，在滑动流形S=0上，x₂可唯一地由x₁来表示，记作。因此，滑动模态方程(12)可降阶为

              (13)

它显然可视为状态变量为，输入为x₂的n-m维子系统，从而可以用通常的线性或非线性反馈设计思想确定适当的S₀(x₁)，使得滑动模具有良好的动态品质。

步骤2  变结构控制律的设计。

从等效控制的观点来看，可以把变结构控制律分成两部分，一部分为等效控制作用，该部分作用代表系统滑动运动时所需的控制作用；另一部分为切换控制作用，该部分作用与等效控制作用一起保证滑模到达条件的实现。变结构控制律的形式可描述如下：

               (14)

其中：u_eq为等效控制作用，u_sw为切换控制作用。

仍然考虑系统。对步骤 1 中的切换函数，等效控制作用可由直接确定出来，即：

          (15)

切换控制作用的一般形式为

            (16)

其中：P为待定的常数或状态函数。设期望的到达律为等速趋近律，则有

    (17)

把u_eq代入式(17)即可得到相应的切换控制作用u_sw：

          (18)

则变结构控制律u为

       (19)

若所期望的到达律为指数趋近律，则有

  (20)

同理，可得到相应的变结构控制律为

    (21)

由上述过程可以看出，变结构控制系统的设计问题按系统状态运动的两个阶段被分解为相应的两个设计步骤。滑动模的设计转化为一降阶子系统的反馈设计问题，从一定程度上说，比原高阶系统反馈设计问题要容易，可以通过通常的反馈设计方法解决。而变结构控制律的确定则非常容易，它由通常的线性或非线性连续反馈的等效控制作用和不连续的切换控制作用两部分组成。

2.2  滑模变结构控制系统的抖振问题

控制结构切换具有理想开关特性的变结构系统，能在切换面上生成滑动模态，滑动模态是降维光滑运动，且渐进趋向原点。但是当具体实现理想变结构系统时，理想的开关特性不可能实现，由于时间延迟和空间滞后等，使得滑动模态出现抖振现象。

2.3  深海集矿机轨迹跟踪控制

深海集矿机为特种履带车辆，集矿机的行走误差由车辆外部误差和内部误差共同构成。所谓外部误 差，是指位姿误差，即集矿机的实际位姿与期望位姿直接的误差。所谓内部误差，是指由于地面情况的不均匀，导致车辆-地面作用力变化，以及液压驱动系统本身的非线性，使左右履带速度不能严格跟踪给定速度。针对集矿机精确数学模型过于复杂和模型参数不易准确测量的特点，提出了采用滑模变结构控制器对集矿机的行走位姿误差进行控制，实现集矿机的位姿跟踪，采用PID控制器对集矿机的驱动系统^[3]进行控制，实现集矿机履带的速度跟踪，以最终实现集矿机的轨迹跟踪控制。

集矿机轨迹跟踪控制结构如图2所示。系统的输入为参考位姿p_r，系统的输出为集矿机当前时刻的位姿p_c。变结构控制器根据系统位姿误差，得到集矿机的控制速度q_c=[v_c  ω_c]，经过速度受限判断和速度映射环节，得到集矿机的左右驱动轮的控制转速，再通过速度跟踪环节，得到集矿机左右履带速度，最后获得集矿机的当前位姿，与参考位姿比较后得到位姿误差，再经过控制器调节，得到集矿机的控制速度。如此反复循环控制，最终使得系统位姿误差等于零。

履带速度跟踪系统控制结构如图3所示。给定集矿机左右驱动轮转速ω_l和ω_r，经过PID控制器调节，经过液压驱动环节，最终获得实际履带速度v_l和v_r，与给定速度进行比较，获得速度误差，继续通过控制器调节，使得集矿机履带的实际速度跟随给定速度变化。

综上所述，切换函数为

      (22)

参照我国已故变结构控制专家高为炳院士提出的趋近律设计方法，采用等速趋近律，令

             (23)

设为正小数，为了减弱抖振，采用连续函数取代符合函数：

 (i=1, 2)          (24)

图2  集矿机轨迹跟踪控制系统结构

Fig.2  Mining vehicle tracking control system structure

图3  履带速度跟踪系统控制结构

Fig.3  Track speed tracking system control structure

令，由式(10)和(24)可得

   (25)

经整理，得控制律为

    (26)

其中：；。

考虑到集矿机运动过程的动力学特性以及采矿速度要求，如果系统误差比较大，控制律所产生的控制量有可能超过系统控制量的最大值或系统的加速度超出系统允许范围，并且系统的速度或加速度太大将会引起集矿机运动打滑。因此，在控制量中引入如下控制受限策略：

         (27)

        (28)

从而保证集矿机运动平滑且减小运动打滑。其中，分别为集矿机经过运动约束控制后的车体线速度及角速度。

3  数值仿真与分析

以MATLAB为平台，采用S函数模块，对设计的集矿机轨迹跟踪算法进行仿真^[4]。在位置信号中引入正弦信号干扰、速度限制和速度跟踪控制环节，当线速度和角速度均为匀速运动，集矿机的圆轨迹跟踪仿真结果如图4~5所示。取v_r=1.0 m/s，ω_r=0.1 rad/s，初始位姿误差为[-5 m  5 m  1 rad]，半径r=10 m，位姿指令p_r=(x_r  y_r  θ_r)^T为x_r=rcos(ω_rt)=10cos0.1t，y_r=rsin(ω_rt)=10sin0.1t，θ_r=ω_rt=0.1t。

取控制律中的参数δ₁=δ₂=0.2，k₁=k₂=6.0，v_max=2.0 m/s，ω_max=0.2 rad/s，正弦波动幅值为20%，波动周期为6 s。仿真结果如图4和图5所示。

图4  轨迹跟踪图

Fig.4  Tracking plot

图5  轨迹跟踪误差图

Fig.5  Track of following error

从图4和图5可以看出：在位置信号中引入正弦信号干扰后，集矿机的行走误差虽然不能很好地收敛到零，然而是一致有界的，误差最大不超过0.2 m，能满足集矿机的控制要求；同时，系统的抖振也得到了一定程度的削弱，这体现了用连续函数取代符号函数的有效性。

4  结论

(1) 通过对简化的集矿机运动学模型的分析，提出了一种新的滑模变结构控制轨迹跟踪算法，在算法中加入了速度受限策略，并用连续函数取代符号函数。

(2) 仿真结果证明，基于变结构控制的轨迹跟踪算法能够使机器人获得较好的跟踪效果，在较好地消除系统误差的同时，具有较小的抖振。

参考文献：

[1] 陈峰, 桂卫华, 王随平. 深海底采矿机器车的研究现状[J]. 矿山机械, 2005, 33(9): 6-8.
CHEN Feng, GUI Wei-hua, WANG Sui-ping. The research of deep sea mining vehicle[J]. Mining Machinery, 2005, 33(9): 6-8.

[2] 程俊林, 赵东风. 轮式移动机器人滑模轨迹跟踪控制[J]. 云南大学学报, 2007, 29(s1): 64-67.
CHENG Jun-lin, ZHAO Dong-feng. Research on sliding-model tracking control of wheeled mobile robots[J]. Yunnan　University Journal, 2007, 29(s1): 64-67.

[3] 桂卫华, 冯磊华, 王随平. 深海集矿机模型车行走驱动系统[J]. 中南大学学报: 自然科学版, 2005, 36(3): 470-474.
GUI Wei-hua, FENG Lei-hua, WANG Sui-ping. Drive system in mould of ocean mining machine[J]. Journal of Central South University: Science and Technology, 2005, 36(3): 470-474.

[4] 冯磊华. 深海集矿机模型车行走控制系统研究与虚拟仿真[D]. 长沙: 中南大学信息科学与工程学院, 2005.
FENG Lei-hua. Deep-sea mining machine model car set running control system and virtual simulation[D]. Changsha: Central South University. School of Information Science and Engineering, 2005.

(编辑 李向群)

收稿日期：2011-06-15；修回日期：2011-07-15

基金项目：国际海底区域研究开发“十五”资助项目(DY105-03-02-06)；国家自然科学基金资助项目(60505018)

通信作者：王随平(1956-)，男，河南焦作人，博士，教授，从事人工智能、深海机器人、现场总线和计算机控制系统等研究；电话：13975818874；E-mail: wangsp@csu.edu.cn