-
摘要: 针对工作于复杂环境下的大型工装,本文提出了一种基于非均匀环境建模与三阶Bezier曲线的平滑路径规划算法,以指导工装的运动.在环境建模方面,利用四叉树建立环境的非均匀模型,能够有效压缩环境信息,提高搜索效率;在路径搜索方面,以非均匀环境模型为基础,提出一种距离启发搜索和信息素混合更新的蚁群算法,能够得到工装的安全可行路径点;在路径平滑方面,基于三阶Bezier曲线,提出能够连接任意位置和任意方向两点的转弯单元的设计方法,利用转弯单元连接路径搜索算法得到的路径点,能够获得满足工装非完整性约束的平滑路径.最后,以大型激光驱动器的靶场环境为对象,对本文算法的有效性和可靠性进行验证,并利用DELMIA平台进一步验证了规划路径的运动平滑性和安全性.
-
关键词:
- 平滑路径规划 /
- 非均匀建模 /
- 蚁群算法 /
- Minkowski和 /
- Bezier曲线
Abstract: In this paper, a path planning algorithm is proposed to search feasible paths for an auxiliary robot working in complex environment. The movement of the robot could thus be guided by the planned result of the algorithm in assembly practice. In the process of environmental modeling, a quad tree method is employed to construct the non-uniform environmental model, which decreases the amount of environmental information to a minimum as needed. As a result, the searching efficiency is improved. In the process of path searching, on the basis of the predefined non-uniform environmental model, an improved ant colony algorithm is presented to search the path points for the robot, which adopts distance-based heuristic search method and fusion of two different pheromone updating mechanisms. In the process of path smoothing, Bezier turn is designed based on cubic Bezier curves to connect two arbitrary configurations. Bezier turns are used to fit a sequence of objective points offered by the improved ant colony algorithm, so that a smooth path satisfying the robot's nonholonomic constraints can be obtained. Finally, the method is experimentally demonstrated in target area with large laser facility. Furthermore, the feasibility and security of the planned path is verified based on the platform of DELMIA. -
串联弹性驱动器(Series elastic actuator, SEA)[1]是将弹性组件串联于传统驱动源(伺服电机、液压等)与负载间的驱动装置.弹性组件的引入为SEA带来了本质安全、低阻抗、力/力矩输出精度高等优点, 使得SEA作为理想力/力矩源被广泛应用于人-机交互系统中, 提高了人-机交互的安全性和舒适性[2-5].
SEA最先于1995年由MIT的研究者Pratt G. A.提出并实现, 近20年来, 国内外相关研究人员针对SEA的力/力矩控制问题展开了大量研究.具体而言, Pratt等[1]提出了比例-积分-微分(Proportion-integration-differentiation, PID)结合前馈补偿的控制策略; 随后, Pratt等[6]又提出了级联PID控制策略, 其中力矩控制环作为外环, 电机位置环作为内环; 之后, 许多研究人员对该方法进行了改进和拓展, Wyeth[7]提出了用电机速度环代替电机位置环作为控制内环的控制策略, 并进行了相关的理论分析和实验验证; Vallery等[8]结合无源控制理论对级联PID控制方法的参数选取进行了更深入的研究; Tagliamonte等[9]将阻抗控制理论引入到级联PID控制方法中, 来优化控制系统在交互应用中的性能.由于级联PID控制器结构简单, 便于应用, 使得其被广泛地应用于SEA系统中[10-13].为了进一步提高控制系统的鲁棒性, Kong等[14-15]提出了一系列基于线性扰动观测器(Linear disturbance observer, LDOB)的控制方法.为了能够在线适应人-机交互中人的动态特性, Calanca等[16]提出了一种基于参考模型的自适应控制方法.除此之外, 一系列控制方法, 如滑模控制方法[17]、基于内部补偿器的控制方法[18]、$H_2$鲁棒控制方法[19]、基于神经网络的控制方法[20]、基于快速收敛的李雅普诺夫函数的控制方法[21-22]等都被用来解决SEA的力/力矩控制问题, 并取得了不错的效果.
值得指出的是, 包括上述列举的文献在内, 已有的绝大多数控制方法都是针对线性SEA系统设计的(SEA输出力矩与电机端和负载端的相对转角呈线性关系).然而, 随着SEA的发展, 为了追求更高的能量密度和更好的仿生特性, 其机械结构变得日趋复杂, 促使非线性SEA的出现(力/力矩输出与电机和负载的相对转角呈现非线性关系)[23-25].显然, 非线性SEA的力/力矩控制问题更具有挑战性, 目前该方面的论文相对较少.文献[26-27]应用级联PID控制方法来控制非线性SEA, 对于复杂模型来说, PID控制是一种简单有效的控制方法, 但该方法在理论上难以做进一步的分析; 文献[28-29]提出了一种线性化的方法, 将描述SEA力矩输出特性的非线性函数进行局部线性化后, 利用线性系统理论分别设计LDOB和控制器使系统稳定, 相应的, 该控制方法最大的不足在于只能确保系统的局部稳定性; 文献[30-31]分别针对投掷和敲击任务, 提出了基于最优控制理论的控制方法, 但是该类方法在分析过程中假定负载端的动力学模型已知, 无法应对在交互应用中负载端动力学模型剧烈变化的情况; 文献[32-33]提出了一类基于延时估计的控制方法, 但该方法只能保证控制误差全局一致最终有界(Globally, uniformly, ultimately bounded, GUUB).在我们之前的研究中[34], 提出过一种基于非线性扰动观测器(Nonlinear disturbance observer, NDOB)的滑模控制方法, 但该方法对SEA动力学建模中可能存在的内外扰动缺乏分析.值得一提的是, 除最优控制外, 以上控制方法在控制器设计时均未考虑输入饱和的情况, 而该情形在实际应用中是无法避免的.
基于上述问题, 本文提出了一种自适应滑模控制方法.具体而言, 首先构建了更一般的单关节SEA机器人系统动力学模型, 并对其进行了分析和变换; 在此基础上, 设计了运动观测器来观测负载端的运动信号; 其次, 结合负载运动观测器设计了一种自适应滑模控制律, 并引入了抗饱和环节, 进而形成了一种抗饱和控制律.本文通过Lyapunov方法对控制器的稳定性及闭环系统中信号的有界性进行了严格的数学分析.最后, 进行了仿真和实验验证, 并与已有控制方法进行了对比.结果表明本文设计的自适应滑模控制器能够取得更好的控制效果, 且具有很强的鲁棒性.
本文的其他部分组织如下:第1节通过对SEA动力学模型进行分析和变换, 对其控制问题进行了详尽的描述; 第2节设计了负载运动观测器和带有抗饱和环节的自适应滑模控制器, 并对闭环系统的稳定性和信号有界性进行了理论分析; 第3节给出了仿真和实验结果; 最后一节是对本文工作的总结和展望.
1. 问题描述
本文主要的研究对象是如图 1所示的非线性旋转型SEA.该SEA主要由伺服电机(配有谐波减速器)和弹性组件两部分组成, 弹性组件的机械结构参考了文献[23]的设计.该串联弹性驱动器的柔性主要来源为三个模具压簧, 其力矩输出过程分析如下:当电机与负载发生相对转动时, 滚轮会沿圆弧轨道运动, 进而压缩三个压簧.压簧的压缩会对滚轮产生沿电机轴向且指向电机方向的压力, 与此同时, 圆弧轨道会对滚轮提供一定的支持力, 该支持力可以沿电机轴向和垂直于电机轴向两个方向分解.其中, 在忽略滚轮重力的情况下, 沿电机轴向方向上的分力与弹簧的压缩力大小相等, 方向反向; 垂直于电机轴向方向的分力提供该SEA的输出力矩.根据图 1, 该SEA系统的力矩输出推导过程如下:
$\sin \theta = \frac{{c\varphi }}{{R - r}}$
(1) $\Delta y = (R - r)(1 - \cos \theta )$
(2) $F = 3{k_s}\Delta y$
(3) ${\tau _{{\rm{SEA}}}} = 3{k_s}c(R - r)(\tan \theta - \sin \theta )$
(4) 式中, $\varphi$代表电机和负载的相对转角; $\theta$表示滚轮在圆弧轨道上滚动的角度; R, r分别表示圆弧轨道和滚轮的半径; $\Delta y$表示压簧的压缩量; F表示三个压簧共产生的合力; $\tau_{\rm SEA}$为SEA的输出力矩.该SEA的相关机械参数如表 1所示. SEA的机械参数辨识参见文献[35], 利用辨识所得的参数进行模型验证实验, 其结果如图 2所示, 实际测量值利用力矩传感器直接测得.通过实验结果可以看出, 虽然由于阻尼、传感器延时等不确定因素导致了迟滞现象的出现, 但理论值与实际测量值的偏差在$\pm0.3$ Nm之内, 说明了式(1) ~ (4) 的有效性.通过模型验证, 可以将该SEA的一个重要性质归纳如下:
表 1 SEA机械参数Table 1 The mechanical parameters of the SEAParameter Value Ks 13 600 N/m c 0.018 m R 0.020 m r 0.005 m 性质1. SEA的输出力矩$\tau_{\rm SEA}$与电机和负载端的相对转角$\varphi$间呈连续的一一映射关系, 即$\tau_{\rm SEA}=f(\varphi)$, $f(\varphi)$为关于$\varphi$的连续可逆函数.且$f(\varphi)\in {\bf C}^2$.
将非线性SEA应用于单关节SEA交互机器人系统, 其示意图如图 3所示, 该机器人系统的动力学方程可描述如下:
$\left\{ \begin{array}{l} {\tau _M} = {J_M}{{\ddot \theta }_M} + {c_M}{{\dot \theta }_M} + {\tau _{{\rm{SEA}}}} + {d_M}\\ {\tau _{{\rm{SEA}}}} = f(\varphi ) = {J_L}({\theta _L}){{\ddot \theta }_L} + {C_L}({{\dot \theta }_L},{\theta _L}) + {d_L}\\ \varphi = {\theta _M} - {\theta _L} \end{array} \right.$
(5) 式(1) 中, $J_M$、$c_M$分别代表电机(将减速器和电机组成的驱动源整体简称为电机)的转动惯量和阻尼系数; $J_L(\theta_L)$代表负载的惯量; $C_L(\dot{ \theta }_L, \theta_L)$包括负载端的科氏力、重力等因素; $\theta_M(t)$代表电机端的转动角度; $\theta_L(t)$代表负载端的转动角度; $d_M(t)$、$d_L(t)$分别代表电机端和负载端可能存在的扰动, 包括模型误差、外界交互力等因素; $\tau_M(t)$表示电机的输出力矩; $\tau_{\rm SEA}(t)$代表SEA的输出力矩; $\varphi(t)$代表电机端与负载端的相对转角; $f(\varphi)$为描述SEA输出力矩与电机和负载间相对转角的函数.在这里值得指出的是, 区别于传统的线性SEA系统, $f(\varphi)$被视为一类更广义的函数.
基于性质1, SEA力矩控制问题可以转化为电机与负载端的相对位置控制问题, 即可得到如下控制目标的等价关系:
$\varphi (t) \to {\varphi _d}(t) \Leftrightarrow {\tau _{{\rm{SEA}}}}(t) \to {\tau _{{\rm{SEA}},d}}(t)$
(6) 其中, $\tau_{{\rm SEA}, d}(t)$表示SEA力矩参考轨迹, $\varphi_d(t)$表示电机与负载间相对转角的参考轨迹, $\tau_{{\rm SEA}, d}(t)$和$\varphi_d(t)$通过映射关系$f(\varphi)$ ($f^{(-1) }(\tau_{\rm SEA})$)可以相互转化, 考虑到驱动器的物理约束及跟踪力矩轨迹的平滑性, 期望轨迹$\tau_{{\rm SEA}, d}(t)$ ($\varphi_d(t)$)应足够平滑: $\tau_{{\rm SEA}, d}(t)\in{\bf C}^2$ ($\varphi_d(t)\in{\bf C}^2$).
通常情况下, 在交互应用中负载端的动力学模型往往完全未知且会发生剧烈的变化[2-5], 为了使控制器能够很好地处理这种情形, 在不考虑负载端动力学模型的情况下, 定义辅助变量如下: $x_1(t)=\varphi(t)$
${x_2}(t) = {{\dot \theta }_M}(t) = {{\dot x}_1}(t) + {{\dot \theta }_L}(t)$
将SEA动力学方程(5) 整理成如下形式:
$\left\{ \begin{array}{l} {{\dot x}_1} = {x_2} - {{\dot \theta }_L}\\ {J_M}{{\dot x}_2} = {\tau _M} - f({x_1}) - {c_M}{x_2} - {d_M}\\ y = {x_1} \end{array} \right.$
(7) 至此, 可以将SEA力矩控制问题描述为:针对系统(7) 设计控制器$\tau_M(t)$, 使得$\lim_{t\rightarrow\infty} x_1(t)=x_{1, d}(t)$.
为了后续的控制器分析, 考虑到实际情况和物理约束, 在缺乏电机端扰动和负载端动力学模型先验知识的前提下, 我们作出以下假设.
假设1. 负载运动速度$|\dot {\theta}_L|$, 加速度$|\ddot {\theta}_L|$, 加加速度$|\dddot {\theta}_L|$有上界, 即$|\dot {\theta}_L|, |\ddot {\theta}_L|, |\dddot {\theta}_L|\in\mathcal{L}_{\infty}$.
假设2. 扰动$|d_M(t)|$有上界, 即$|d_M(t)|\in\mathcal{L}_{\infty}$.
2. 控制器设计与分析
对于SEA交互系统的力矩控制而言, 主要考虑如下两个要求: 1) 要快速精确地达到期望力矩值/轨迹; 2) SEA系统往往需要与外界进行频繁的交互, 因此要求控制器对负载变化和外界扰动具有很强的鲁棒性, 使得系统在面对不同的交互情景下都能够正常工作.本文接下来所设计的自适应滑模控制器可实现以上目标.具体而言, 首先, 设计了负载运动观测器来观测负载的运动情况; 其次, 构建了滑模面并设计了自适应滑模控制器来引导系统状态收敛于滑模面; 最后, 在控制器中引入了抗饱和环节, 使得控制器满足输入饱和约束.借助Lyapunov方法和芭芭拉定理对闭环系统的稳定性和有界性进行了理论分析.
2.1 负载运动观测器设计(Payload observer)
根据上一节的描述可以知道, 控制目标是使电机和负载的相对转角达到期望值或期望轨迹, 为了实现这一目标, 需要将负载端的运动信号加入到反馈通道中, 在不考虑负载端的动力学模型的前提下, 就有必要构造运动观测器对负载的运动进行详尽的观测, 根据式(3), 设计观测器[36-38]如下:
$\left\{ \begin{array}{*{35}{l}} {{{\hat{\dot{\theta }}}}_{L}}={{z}_{1}}-{{l}_{1}}{{x}_{1}} \\ {{{\dot{z}}}_{1}}={{l}_{1}}({{x}_{2}}-{{{\hat{\dot{\theta }}}}_{L}})+{{{\hat{\ddot{\theta }}}}_{L}} \\ {{{\hat{\ddot{\theta }}}}_{L}}={{z}_{2}}-{{l}_{2}}{{x}_{1}} \\ {{{\dot{z}}}_{2}}={{l}_{2}}({{x}_{2}}-{{{\hat{\dot{\theta }}}}_{L}}) \\ \end{array} \right.$
(8) 其中, $l_1, l_2\in\mathbf{R}^+$为观测增益; $\hat {\dot \theta}_L, \hat {\ddot \theta}_L$为负载速度和加速度的观测信号; $z_1, z_2$为辅助变量.
定理1. 在假设2的前提下, 本文所设计的运动观测器(8) 可实现对负载速度$\dot \theta_L$和加速度$\ddot \theta_L$的有效观测, 即观测误差$e_d=[\dot \theta_L-\hat {\dot \theta}_L \quad \ddot \theta_L-\hat {\ddot \theta}_L]^{\rm T}$有界.
证明. 对$e_d(t)$求导并将式(7) 和(8) 代入整理可得:
$\begin{gathered} {{\dot e}_d} = A{e_d} + B{{\dddot \theta }_L} \hfill \\ A = \left[ {\begin{array}{*{20}{c}} { - {l_1}}&{\quad 1} \\ { - {l_2}}&{\quad 0} \end{array}} \right] \hfill \\ B = {\left[ {\begin{array}{*{20}{c}} 0&{\quad 1} \end{array}} \right]^{\text{T}}} \hfill \\ \end{gathered} $
(9) 基于假设1的条件$|\dddot {\theta_L}|\in\mathcal{L}_{\infty}$, 通过线性理论分析[39]可知, 通过选取增益$l_1, l_2$使得方程$s^2+l_1s+l_2=0$的根具有负实部, 即可保证观测误差$e_d(t)$有界.
2.2 自适应滑模控制器设计(Adaptive sliding-mode controller design)
首先定义误差变量$e_1=x_1-x_{1, d}=\varphi-\varphi_d$, 定义线性滤波器$r=\dot e_1+ \lambda e_1$, 其中$\lambda \in\mathbf{R}^+ $.基于定理1的结论和假设2的条件, 在此定义一正常数$D\in\mathbf{R}^+$满足$|d_M+J_M\tilde {\ddot \theta}_L|\leq D$, 其中$\tilde {\ddot \theta}_L=\ddot {\theta}_L-\hat {\ddot \theta}_L$为加速度观测误差.
结合上述定义变量, 设计自适应滑模控制器如下:
$\begin{align} & {{\tau }_{M}}=f({{x}_{1}})+{{{\hat{c}}}_{M}}{{x}_{2}}+{{{\hat{J}}}_{M}}({{{\ddot{x}}}_{1,d}}- \\ & \quad \quad \quad \lambda {{{\dot{e}}}_{1}}+{{{\hat{\ddot{\theta }}}}_{L}})-\hat{D}\text{sgn}(r)-kr \\ \end{align}$
(10) 其中, $k\in\mathbf{R}^+$为控制器控制增益, $\hat c_M, \hat J_M, \hat D$分别为$c_M, J_M, D$的估计值, ${\rm sgn}(\cdot)$为符号函数.参数的自适应律$\dot {\hat c}_M, \dot {\hat J}_M, \dot {\hat D}$分别为
${{{\dot{\hat{c}}}}_{M}}=-{{\Gamma }_{1}}{{x}_{2}}r$
(11) ${{{\dot{\hat{J}}}}_{M}}=-{{\Gamma }_{2}}({{{\ddot{x}}}_{1,d}}+{{{\hat{\ddot{\theta }}}}_{L}}-\lambda {{{\dot{e}}}_{1}})r$
(12) $\dot{\hat{D}}={{\Gamma }_{3}}|r|$
(13) 定理2. 考虑SEA系统(7) 满足假设1 ~ 3, 自适应滑模控制器(10) 能保证SEA的电机和负载间相对转角跟踪期望相对位置轨迹, 等价于SEA的输出力矩跟踪期望力矩轨迹, 即
$\mathop {\lim }\limits_{t \to \infty } \varphi (t) = {\varphi _d}(t) \Leftrightarrow \mathop {\lim }\limits_{t \to \infty } {\tau _{{\rm{SEA}}}}(t) = {\tau _{{\rm{SEA}},d}}(t)$
证明. 结合式(7) 得到如下误差系统:
$\left\{ \begin{array}{l} {{\dot e}_1} = {x_2} - {{\dot \theta }_L} - {{\dot x}_{1,d}}\\ {J_M}{{\dot x}_2} = {\tau _M} - f({x_1}) - {c_M}{x_2} - {d_M} \end{array} \right.$
(14) 选取如下的Lyapunov候选函数:
$V = \frac{1}{2}{J_M}{r^2} + \frac{1}{2}{\Gamma _1}^{ - 1}\tilde c_M^2 + \frac{1}{2}{\Gamma _2}^{ - 1}\tilde J_M^2 + \frac{1}{2}{\Gamma _3}^{ - 1}{\tilde D^2}$
(15) 其中, $\tilde {J}_M=J_M-\hat {J}_M, \tilde {c}_M=c_M-\hat c_M, \tilde D=D-\hat D$.
将式(15) 对时间求导, 可得:
$\begin{array}{*{35}{l}} \dot{V}={{J}_{M}}r\dot{r}+{{\Gamma }_{1}}^{-1}{{{\tilde{c}}}_{M}}{{{\dot{\tilde{c}}}}_{M}}+{{\Gamma }_{2}}^{-1}{{{\tilde{J}}}_{M}}{{{\dot{\tilde{J}}}}_{M}}+ \\ \quad {{\Gamma }_{3}}^{-1}\tilde{D}\dot{\tilde{D}}=r[{{\tau }_{M}}-f({{x}_{1}})-{{c}_{M}}{{x}_{2}}- \\ \quad {{J}_{M}}{{{\ddot{x}}}_{1,d}}+\lambda {{J}_{M}}{{{\dot{e}}}_{1}}-d-{{J}_{M}}({{{\tilde{\ddot{\theta }}}}_{L}}+{{{\hat{\ddot{\theta }}}}_{L}})]+ \\ \quad {{\Gamma }_{1}}^{-1}{{{\tilde{c}}}_{M}}{{{\dot{\tilde{c}}}}_{M}}+{{\Gamma }_{2}}^{-1}{{{\tilde{J}}}_{M}}{{{\dot{\tilde{J}}}}_{M}}+{{\Gamma }_{3}}^{-1}\tilde{D}\dot{\tilde{D}} \\ \end{array}$
(16) 将控制律(10) 代入上式, 可得:
$\begin{array}{*{35}{l}} \dot{V}=-k{{r}^{2}}-{{{\tilde{c}}}_{M}}{{x}_{2}}r+{{{\tilde{J}}}_{M}}(\lambda {{{\dot{e}}}_{1}}-{{{\ddot{x}}}_{1,d}}-{{{\hat{\ddot{\theta }}}}_{L}})- \\ \quad ({{d}_{M}}+{{J}_{M}}{{{\tilde{\ddot{\theta }}}}_{L}})r-\hat{D}|r|+{{\Gamma }_{1}}^{-1}{{{\tilde{c}}}_{M}}(-{{{\dot{\hat{c}}}}_{M}})+ \\ \quad {{\Gamma }_{2}}^{-1}{{{\tilde{J}}}_{M}}(-{{{\dot{\hat{J}}}}_{M}})+{{\Gamma }_{3}}^{-1}\tilde{D}(-\dot{\hat{D}}) \\ \end{array}$
(17) 注意到$|d+J_M \tilde {\ddot \theta}_L||r|-\hat D |r| \leq \tilde D |r|$, 则可推出
$\begin{array}{*{35}{l}} \dot{V}\le -k{{r}^{2}}-{{{\tilde{c}}}_{M}}({{x}_{2}}r+{{\Gamma }_{1}}^{-1}{{{\dot{\hat{c}}}}_{M}})+{{{\tilde{J}}}_{M}}(\lambda {{{\dot{e}}}_{1}}r- \\ \quad \quad {{{\ddot{x}}}_{1,d}}r-{{{\hat{\ddot{\theta }}}}_{L}}r-{{\Gamma }_{2}}^{-1}{{{\dot{\hat{J}}}}_{M}})+\tilde{D}(|r|-{{\Gamma }_{3}}^{-1}\hat{\dot{D}}) \\ \end{array}$
(18) 将自适应律(11) ~ (13) 代入可得:
$\dot V \le - k{r^2} \le 0$
(19) 由式(15) 和(19) 可知
$\begin{array}{l} V \in {{\cal L}_\infty }\\ r,{{\dot e}_1},{e_1},{{\tilde c}_M},{{\tilde J}_M},\tilde D \in {{\cal L}_\infty }\\ r,{{\dot e}_1},{e_1}, \in {{\cal L}_2}\\ {{\hat c}_M},{{\hat J}_M},\hat D \in {{\cal L}_\infty } \end{array}$
(20) 结合式(10) 和(14) 及假设2、假设3可得:
$\begin{array}{l} {x_1},{{\dot x}_1},{x_2} \in {{\cal L}_\infty }\\ {\tau _M} \in {{\cal L}_\infty } \end{array}$
(21) 最终结合芭芭拉定理有:
$\begin{array}{l} {e_1} \in {{\cal L}_2},{{\cal L}_\infty }\\ {{\dot e}_1} \in {{\cal L}_\infty }\\ \mathop {\lim }\limits_{t \to \infty } {e_1} = 0 \end{array}$
(22) 2.3 抗饱和系统设计(Anti-windup system design)
在设计控制器时, 需要根据电机特性考虑如下饱和情况: $\tau_{\min} \leq \tau_M\leq \tau_{\max}$, 其中$\tau_{\min}$和$\tau_{\max}$分别表示电机输出力矩的最小值和最大值:
${\tau _M} = sat(u) = \left\{ {\begin{array}{*{20}{l}} {{\tau _{\max }},}&{若u > {\tau _{\max }}}\\ {u,}&{若{\tau _{\min }} \le {\tau _M} \le {\tau _{\max }}}\\ {{\tau _{\min }},}&{若u < {\tau _{\min }}} \end{array}} \right.$
(23) 式中, $u(t)$为被设计的控制律.为了设计抗饱和控制律, 引入如下辅助系统[40-41]:
$\dot \nu = \left\{ {\begin{array}{*{20}{l}} { - \rho \nu - \frac{{g( \cdot )}}{\nu } + \Delta u,}&{|\nu | \ge \varepsilon }\\ {\Delta u,}&{|\nu | < \varepsilon } \end{array}} \right.$
(24) 式中, $\nu(t)$为辅助变量, $\rho \in{\bf R}^+$为一正常数, $g(\cdot)=|r\cdot\Delta u|+\frac{1}{2}\Delta u^2$, $\Delta u = u-\tau_M$, $\varepsilon\in{\bf R}^+$为正参数.
根据引入的辅助系统(24), 设计抗饱和控制律如下:
$\begin{array}{*{35}{l}} u=f({{x}_{1}})+{{{\hat{c}}}_{M}}{{x}_{2}}+{{{\hat{J}}}_{M}}({{{\ddot{x}}}_{1,d}}- \\ \quad \quad \lambda {{{\dot{e}}}_{1}}+{{{\hat{\ddot{\theta }}}}_{L}})-\hat{D}\text{sgn}(r)-kr-{{k}_{1}}\nu \\ \end{array}$
(25) 式中, $k_1\in\mathbf{R}^+$为正常数, 其余项与式(10) 均相同.参数的更新率如式(11) ~ (13).
定理3. 抗饱和自适应滑模控制律(25), 能够保证SEA的电机和负载间相对转角跟踪期望相对位置轨迹, 即SEA的输出力矩跟踪期望力矩轨迹.
证明. 当$|\nu|\geq\varepsilon$时, 选取如下Lyapunov函数:
$V = \frac{1}{2}{J_M}{r^2} + \frac{1}{2}{\nu ^2} + \frac{1}{2}\Gamma _1^{ - 1}\tilde c_M^2 + \frac{1}{2}\Gamma _2^{ - 1}\tilde J_M^2 + \frac{1}{2}\Gamma _3^{ - 1}{\tilde D^2}$
(26) 分析式(24) 和(25) 有:
$\begin{array}{l} \nu \dot \nu = - \rho {\nu ^2} - |r\Delta u| - \frac{1}{2}\Delta {u^2} + \Delta u\nu \le \\ \quad \quad \quad - (\rho - \frac{1}{2}){\nu ^2} - |r\Delta u|\\ r{\tau _M} \le ru + |r\Delta u| \end{array}$
(27) 参照定理2的证明过程, 将式(26) 两边对时间求导, 并代入式(7), (11) ~ (13) 和式(27) 可得:
$\dot V \le - \left( {k - \frac{1}{2}{k_1}} \right){r^2} - \left( {\rho - \frac{1}{2} - \frac{1}{2}{k_1}} \right){\nu ^2}$
(28) 取$k_1\leq\min \{2k, 2\rho - 1\}$, 则有:
$\dot V \le 0$
(29) 则与式(20) ~ (22) 类似, 即可得抗饱和控制器(25) 可使控制误差渐进收敛至零, 即SEA电机和负载间的相对转角会随时间收敛到期望轨迹.考虑另外一种情形, 当$|\nu|<\varepsilon$时, 意味着系统不存在输入饱和约束, 重新考虑式(15) ~ (22) 同样可以得到上述结论.
3. 仿真与实验验证
为了验证本文所提方法的有效性, 本节将利用数值仿真和硬件实验两种方式对其进行验证.
3.1 实验配置(Experimental configuration)
在给出仿真和实验结果之前, 首先简要介绍一下本文的实验平台.如图 4所示, 本文的单关节SEA机器人由机械主体、实时控制系统以及驱动装置三部分组成.在机械主体与驱动装置部分, 机器人可沿电机旋转轴进行平面旋转运动, 驱动源选用Maxon直流伺服电机驱动(减速比N = 1:100), 伺服电机端配有相对码盘(4 000 PPR), 负载端安装有绝对码盘(4 096 PPR), 弹性组件串联于电机和负载之间.对于实时控制部分, 采用运行在Linux-Xenomai实时操作环境下的PC104作为主控制器, 控制周期设定为1 ms.除此之外, 利用FPGA实现了CAN、SSI等总线接口来实现电机端码盘和负载端码盘信号的采集, 将信号处理后传送至PC104, 同时PC104将控制量通过FPGA下发至驱动器, 控制电机完成相应的指令.
3.2 数值仿真验证(Simulation verification)
首先利用仿真来测试本文控制器的控制性能.在仿真实验中, 通过动态调节负载端的参数和受力情况, 模拟了以下三种交互场景.具体而言, 第一组仿真实验为碰撞, 初始时负载端处于自由运动状态, 之后让负载端突然停止; 第二组仿真实验为释放, 初始时刻令负载端保持静止, 之后突然释放负载端让其进入自由运动状态; 第三组仿真为零阻抗控制, 在负载端加入周期的干扰力矩, 并且负载端的动力学参数呈现周期性的连续变化.这三组仿真实验模拟了人-机交互过程中常见的三种情形, 能够很好地验证出控制器面向交互应用的性能.为了体现本文方法的优势, 在此与文献[26-27]中所提的级联PID控制方法进行比较(级联PID控制方法被广泛用于SEA的力矩控制中[10-13, 26-27], 因此在此用作比较).经过调节, 本文设计的控制器相关参数如表 2所示, 级联PID控制器力矩环控制参数调节为$k_{op} = 10$, $k_{oi} = 3$, $k_{od} = 0.5$, 速度环控制参数为$k_{ip} = 3$, $k_{ii} = 1$, $k_{id} = 0$.
表 2 本文控制器的控制参数Table 2 The parameters of the proposed controllerParameter Value l1, l2 10, 50 Γ1, Γ2, Γ3 20, 20, 10 λ 15 k, k1 20, 5 ρ 5 ε 1 仿真实验1.碰撞
该情形具体描述为, 初始时刻负载端处于自由运动状态, 之后突然发生"碰撞", 负载端运动停止.该仿真的具体过程为, 期望力矩始终保持为$\tau_d=0.3$ N$\cdot$m, 初始时刻负载端的动力学参数设置为
${J_L} = 1{\rm{kg}} \cdot {{\rm{m}}^2},{C_L} = 0.05{\dot \theta _L}{\rm{N}} \cdot {\rm{m}}$
在$t=2$ s时突然将负载端的动力学参数改变为
${J_L} = 500{\rm{kg}} \cdot {{\rm{m}}^2},{C_L} = 10000{\dot \theta _L}{\rm{N}} \cdot {\rm{m}}$
仿真结果如图 5所示.通过图 5中的仿真结果可以看出, 本文控制方法在暂态响应和鲁棒性两方面具有明显的优势, 当"碰撞"发生后, 能够更有效地做出响应, 使SEA维持输出期望力矩.
仿真实验2.释放
该情形具体描述为, 初始时刻负载端处于静止状态, 之后负载端突然进入自由运动状态.该仿真实验的具体过程为, 期望力矩始终保持为$\tau_d=0.3$ N$\cdot$m, 初始时刻负载端的动力学参数设置为
$J_L=500$ kg$\cdot {\rm m}^2$, $C_L$=10 000 $\dot \theta_L$ N$\cdot$m
在$t $= 2 s时突然将负载端的动力学参数改变为
$J_L=1$ kg$\cdot {\rm m}^2$, $C_L$ = 0.05 $\dot \theta_L$ N$\cdot$m
仿真结果如图 6所示.从仿真结果可以看出, 初始阶段负载端处于静止状态, 两种控制方法都能够保证SEA输出力矩达到期望值, 本文方法收敛速度更快, 且无超调量, 具有更好的暂态响应性能.随着仿真中负载参数的突然改变, 负载开始进入加速运动阶段, 本文方法基本不受该变化带来的影响, 而级联PID方法无法在短时间内使输出力矩恢复到期望值, 体现了本文方法拥有更强的鲁棒性, 对交互场景拥有更强的适应能力.
仿真实验3.零阻抗控制
零阻抗控制经常被应用在人-机交互场合, 机器人系统保持零力矩输出以提高"透明度".该仿真实验的具体过程为, 期望力矩为$\tau_d=0$ N$\cdot$m, 负载端加入扰动信号(见式(30)), 且负载端的动力学参数随着负载运动呈现周期性变化, 以此来模拟人-机交互.仿真结果如图 7所示.根据仿真结果可以看出, 相比级联PID控制方法, 本文的控制方法使SEA输出力矩维持在更小的范围内, 能够更好地应对这种交互场景.
${\tau _L} = \left\{ {\begin{array}{*{20}{l}} {\sin (4\pi t),}&{若t \le 2{\rm{s}}}\\ {1 + \sin (8\pi t),}&{若2{\rm{s}} < t \le 6{\rm{s}}}\\ { - 1 + \sin (12\pi t),}&{若6{\rm{s}} < t \le 10{\rm{s}}} \end{array}} \right.$
(30) 3.3 实验验证(Experimental verification)
为进一步验证本文方法的有效性, 本节在如图 6所示的自主搭建的SEA实验平台上进行了实验验证.与仿真相对应, 分别在碰撞、释放和零阻抗控制的交互情景下进行了实验.为了体现本文方法的优越性, 使用级联PID控制方法和级联PID加前馈补偿的控制方法(Feedforward (FF) + PID)作对比.其中本文设计的控制器与级联PID控制器与仿真实验保持一致, 级联PID加前馈补偿控制器设计为: $\tau_M(t)=f(x_1(t))+c_{M0}x_2(t)+\text{PID}(\text{PID}(e_1(t))-x_2(t))$.其中, $c_{M0}$为电机标称阻尼系数, 经过辨识[42]设定为$c_{M0}=0.75$ N$\cdot$ms/rad, 该控制器的外环控制参数调节为$k_{op} = 5$, $k_{oi} = 1.5$, $k_{od} = 0.1$, 内环控制参数为$k_{ip} = 2$, $k_{ii} = 1$, $k_{id} = 0$.值得指出的是, 在实验中力矩值及力矩误差值均是通过式(1)~(4) 计算而得的.接下来, 通过具体的3组实验加以说明.
实验1.碰撞
第一组实验的实验结果如图 8所示, 对实验结果进行分析, 相比于级联PID控制方法和前馈结合级联PID的控制方法, 本文方法能使SEA输出力矩更快速地达到期望值, 且在碰撞发生后能够更快地恢复到期望值, 体现了本文方法在暂态响应和鲁棒性两方面的优势.需要指出的是, 在对比实验中, 两次碰撞发生的时刻并不完全相同, 因为在实际实验中很难保证严格的时间同步.
实验2.释放
第二组实验的实验结果如图 9所示, 实验结果表明, 初始时刻, 三种控制方法都能够使SEA的输出力矩收敛至期望值, 但本文方法能够更快地达到期望值且无超调.当负载端被释放后, 本文的方法能够很好地适应这种变化, SEA输出力矩基本没有受到影响.而级联PID控制器和结合前馈补偿的级联PID控制器的性能明显下降, SEA输出力矩无法快速地恢复到期望值.实验结果体现出了本文的控制方法具有更好的鲁棒性, 能够更好地适应交互场景中负载端动力学模型发生变化的情况.同第一组实验一样, 两次释放的时刻也存在着一些偏差.
实验3.零阻抗控制
实验3的实验结果如图 10所示, 可以看出, 在本文控制算法的作用下, SEA输出力矩能够保持在很小的范围内, 而在级联PID控制器和结合前馈补偿的级联PID控制器控制下, SEA的力矩输出相对较大, 充分表明了本文控制方法更好的鲁棒性和低阻抗特性.
通过实验结果可以看出, 在无交互的情况下, 本文设计的控制器和结合前馈补偿的级联PID控制器相比级联PID控制器都具有更快的收敛速度, 但本文方法无超调, 体现了更优的暂态响应性能; 在交互过程中, 由于本文的控制方法中加入了运动观测器的观测结果作为前馈项, 能够有效地应对交互过程中负载端发生的变化, 因此能够取得更好的控制效果且有着更优越的鲁棒性能.值得一提的是, 通过对图 5~7和图 8~10进行对比可以发现, 实验结果与仿真结果之间存在着一定的差异, 尤其是控制输入有着明显的不同, 这主要是因为在仿真中未考虑摩擦力的影响, 且实验平台中往往存在着一些建模中未考虑到的动力学因素.
4. 结论
为实现SEA的力矩控制, 本文基于SEA的动力学模型设计了运动观测器和自适应滑模控制器, 并引入了辅助的抗饱和系统来补偿系统饱和限制.与现有的SEA力/力矩控制方法相比, 本文控制方法适用于非线性SEA, 更加通用; 控制器能够很好地应对负载端的变化, 更加适用于交互系统.文中对控制器的性能进行了严格的理论分析, 并通过仿真和实验验证了所提方法的有效性.目前, 实验室正在搭建多关节SEA平台, 在接下来的工作中, 将对多关节SEA的控制问题进行进一步研究.
-
表 1 两种建模方法对比
Table 1 Comparison of the two modeling methods
自由栅格 障碍栅格 总数 四叉树法 1 219 1 377 2 596 均匀栅格法 11 839 9 071 20 910 表 2(a) 本文算法与基本MMAS在不同环境模型下对比(a)
Table 2(a) Comparison of the proposed algorithm with the MMAS in different environmental models (a)
序号 终点 改进算法+非均匀模型 改进算法+均匀栅格模型 Fw Lw Ew Bw Tw Fw Lw Ew Bw Tw 1 (−353, −7 548) 8.63 1 897 0.77 2 0.93 8.61 2 444 0.83 3.3 1.01 2 (4 572, −5 381) 8.63 22 717 0.54 6 1.21 8.04 15 082 1.03 24.7 2.99 3 (5 754, −259) 8.46 25 634 0.63 9.7 1.71 7.91 21 730 0.99 36.7 4.51 4 (5 557, 6 242) 8.15 41 289 1.00 16.8 4.32 7.50 44 339 1.26 89.3 13.15 5 (632, 10 182) 8.15 41 954 0.83 15.5 4.33 7.36 50 922 1.12 105.8 15.31 6 (2 602, 11 167) 8.26 43 852 0.82 16.7 4.70 7.33 51 068 1.26 108.3 14.69 7 (−3 899, 5 848) 8.26 53 701 0.79 18.4 5.34 6.98 65 284 1.24 147 2 185 8 (−6 066, 726) 8.18 65 815 0.78 24.8 6.53 6.75 74 028 1.12 175.1 32.27 9 (−10 203, −6 563) 8.21 70 886 0.77 22.8 7.73 6.32 93 157 1.27 240.2 49.75 10 (−5 278, −4 593) 8.14 71 386 0.80 27.1 7.72 6.40 90 830 1.44 227.4 51.42 表 2(b) 本文算法与基本MMAS在不同环境模型下对比(b)
Table 2(b) Comparison of the proposed algorithm with the MMAS in different environmental models (b)
序号 基本MMAS算法+非均匀模型 基本MMAS算法+均匀栅格模型 Fw Lw Ew Bw Tw Fw Lw Ew Bw Tw 1 8.63 1 897 0.77 2 0.57 8.61 2 411 0.90 2.3 0.31 2 8.56 20 341 0.55 6 1.66 7.65 12 711 1.67 27.5 2.09 3 8.46 25 320 0.65 9.2 1.77 7.23 23 016 1.70 61.2 3.93 4 8.04 38 956 1.15 18.4 6.62 6.14 50 406 2.09 152.5 13.14 5 8.13 46 588 0.98 19 7.47 5.36 85 489 2.01 266.8 20.03 6 8.17 44 814 0.93 17.8 6.53 5.67 65 877 2.21 204.4 16.22 7 7.76 66 983 1.26 36 10.46 4.02 200 854 2.38 672.7 38.22 8 7.36 83 167 1.58 56.1 12.96 3.82 1 151 948 2.36 3 925 51.51 9 7.03 116 137 1.63 83.05 15.72 3.88 1 304 770 2.49 4 450 61.40 10 6.95 113 584 1.71 87.1 16.40 3.87 1 418 617 2.61 4 835.9 60.16 表 3 次独立实验中本文算法和基本MMAS算法的对比
Table 3 Comparison of the planned results of the proposed algorithm and the MMAS algorithm in several independent experiments
本文算法 基本MMAS算法 适应度 8.21 7.33 路径长度(mm) 63453 86501 危险度 0.77 1.65 转弯次数(次) 23.1 56.3 耗时(s) 6.51 12.98 -
[1] Elbanhawi M, Simic M. Sampling-based robot motion planning: a review. IEEE Access, 2014, 2: 56-77 doi: 10.1109/ACCESS.2014.2302442 [2] Maekawa T, Noda T, Tamura S, Ozaki T, Machida K. Curvature continuous path generation for autonomous vehicle using B-spline curves. Computer-Aided Design, 2010, 42(4): 350-359 doi: 10.1016/j.cad.2009.12.007 [3] Jung D, Tsiotras P. On-line path generation for unmanned aerial vehicles using B-spline path templates. Journal of Guidance, Control, and Dynamics, 2013, 36(6): 1642-1653 doi: 10.2514/1.60780 [4] O'Rourke J. Computational Geometry in C. London: Cambridge University Press, 1997. [5] Neus M, Maouche S. Motion planning using the modified visibility graph. In: Proceedings of the 1999 IEEE International Conference on Systems, Man, and Cybernetics. Tokyo, Japan: IEEE, 1999. 651-655 [6] Brooks R A. Solving the find-path problem by good representation of free space. IEEE Transactions on Systems, Man, and Cybernetics, 1983, SMC-13(2): 190-197 [7] Alajlan M, Koubaa A, Chaari I, Bennaceur H, Ammar A. Global path planning for mobile robots in large-scale grid environments using genetic algorithms. In: Proceedings of the 2013 IEEE International Conference on Individual and Collective Behaviors in Robotics (ICBR). Sousse, Tunisia: IEEE, 2013. 1-8 [8] Takahashi O, Schilling R J. Motion planning in a plane using generalized voronoi diagrams. IEEE Transactions on Robotics and Automation, 1989, 5(2): 143-150 doi: 10.1109/70.88035 [9] Jan G E, Sun C C, Tsai W C, Lin T H. An O(n log n) shortest path algorithm based on Delaunay triangulation. IEEE/ASME Transactions on Mechatronics, 2014, 19(2): 660-666 doi: 10.1109/TMECH.2013.2252076 [10] Stentz A. Optimal and efficient path planning for unknown and dynamic environments. International Journal of Robotics and Automation, 1995, 10(3): 89-100 http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.15.3683 [11] Alexopoulos C, Griffin P M. Path planning for a mobile robot. IEEE Transactions on Systems, Man, and Cybernetics, 1992, 22(2): 318-322 doi: 10.1109/21.148404 [12] Fan D K, Shi P. Improvement of Dijkstra's algorithm and its application in route planning. In: Proceedings of the 7th International Conference on Fuzzy Systems and Knowledge Discovery (FSKD). Yantai, China: IEEE, 2010. 1901-1904 [13] Hu Y R, Yang S X, Xu L Z, Meng M Q H. A knowledge based genetic algorithm for path planning in unstructured mobile robot environments. In: Proceedings of the 2004 IEEE International Conference on Robotics and Biomimetics (ROBIO). Shenyang, China: IEEE, 2004. 767-772 [14] Dorigo M, Maniezzo V, Coloni A. Ant system: optimization by a colony of cooperating agents. IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics), 1996, 26(1): 29-41 doi: 10.1109/3477.484436 [15] Juang C F, Hsu C H. Reinforcement ant optimized fuzzy controller for mobile-robot wall-following control. IEEE Transactions on Industrial Electronics, 2009, 56(10): 3931-3940 doi: 10.1109/TIE.2009.2017557 [16] Dorigo M, Gambardella L M. Ant colony system: a cooperative learning approach to the traveling salesman problem. IEEE Transactions on Evolutionary Computation, 1997, 1(1): 53-66 doi: 10.1109/4235.585892 [17] Stützle T, Hoos H H. MAX-MIN ant system. Future Generation Computer Systems, 2000, 16(8): 889-914 doi: 10.1016/S0167-739X(00)00043-1 [18] Zeng D W, He Q, Leng B, Zheng W M, Xu H W, Wang Y Y, Guan G. An improved ant colony optimization algorithm based on dynamically adjusting ant number. In: Proceedings of the 2012 IEEE International Conference on Robotics and Biomimetics (ROBIO). Guangzhou, China: IEEE, 2012. 2039-2043 [19] Dubins L E. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of Mathematics, 1957, 79(3): 497-516 doi: 10.2307/2372560 [20] Reeds J A, Shepp L A. Optimal paths for a car that goes both forwards and backwards. Pacific Journal of Mathematics, 1990, 145(2): 367-393 doi: 10.2140/pjm [21] Boissonnat J D, Cerezo A, Leblond J. Shortest paths of bounded curvature in the plane. In: Proceedings of the 1992 IEEE International Conference on Robotics and Automation. Nice, France: IEEE, 1992. 2315-2320 [22] Scheuer A, Fraichard T. Collision-free and continuous-curvature path planning for car-like robots. In: Proceedings of the 1997 IEEE International Conference on Robotics and Automation. Albuquerque, New Mexico, USA: IEEE, 1997. 867-873 [23] Scheuer A, Fraichard T. Planning continuous-curvature paths for car-like robots. In: Proceedings of the 1996 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Osaka, Japan: IEEE, 1996. 1304-1311 [24] Scheuer A, Fraichard T. Continuous-curvature path planning for car-like vehicles. In: Proceedings of the 1997 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Grenoble, France: IEEE, 1997. 997-1003 [25] Fraichard T, Scheuer A. From Reeds and Shepp's to continuous-curvature paths. IEEE Transactions on Robotics, 2004, 20(6): 1025-1035 doi: 10.1109/TRO.2004.833789 [26] Yang K, Sukkarieh S. An analytical continuous-curvature path-smoothing algorithm. IEEE Transactions on Robotics, 2010, 26(3): 561-568 doi: 10.1109/TRO.2010.2042990 [27] Walton D J, Meek D S, Ali J M. Planar G2 transition curves composed of cubic Bézier spiral segments. Journal of Computational and Applied Mathematics, 2003, 157(2): 453-476 doi: 10.1016/S0377-0427(03)00435-7 [28] Škrjanc I, Klančar G. Optimal cooperative collision avoidance between multiple robots based on Bernstein-Bézier curves. Robotics and Autonomous Systems, 2010, 58(1): 1-9 doi: 10.1016/j.robot.2009.09.003 [29] Lo Bianco C G, Piazzi A, Romano M. Smooth motion generation for unicycle mobile robots via dynamic path inversion. IEEE Transactions on Robotics, 2004, 20(5): 884-891 doi: 10.1109/TRO.2004.832827 [30] Piazzi A, Lo Bianco C G, Romano M. η3-spline for the smooth path generation of wheeled mobile robots. IEEE Transactions on Robotics, 2007, 23(5): 1089-1095 doi: 10.1109/TRO.2007.903816 [31] Lai X C, Al Mamun A, Ge S S. Polar polynomial curve for smooth, collision-free path generation between two arbitrary configurations for nonholonomic robots. In: Proceedings of the 22nd International Symposium on Intelligent Control. Singapore, Singapore: IEEE, 2007. 59-64 [32] Sahni S. Data Structures, Algorithms, and Applications in C++. Boston: McGraw Hill, 1998. [33] Duan H B, Ma G J, Liu S Q. Experimental study of the adjustable parameters in basic ant colony optimization algorithm. In: Proceedings of the 2007 IEEE Congress on Evolutionary Computation. Singapore, Singapore: IEEE, 2007. 149-156 -