Tamura Coarseness for OTH Radar Image Evaluation and Its Application in Adaptive Optimization of Interference Suppression
-
摘要: 针对OTH (Over-the-horizon) 雷达距离−多普勒(Range-Doppler, RD)图, 本文首次提出采用纹理粗糙度作为RD图质量的评价指标, 即计算RD图所转化灰度图的Tamura纹理粗糙度. 分析表明, 粗糙度指标能准确反映RD图受干扰情况, 对于不同灰度转换函数具有稳健性. 作为应用举例, 本文将图像粗糙度用于改进射频干扰抑制算法, 使干扰抑制达到自适应优化. 实验结果表明, Tamura粗糙度能够正确反映RD图干扰抑制情况, 优化粗糙度指标能够使干扰抑制自适应达到最优.Abstract: Tamura coarseness is introduced as a reasonable index for evaluating the quality of range-Doppler (RD) map in OTH (Over-the-horizon) radar. Tamura coarseness is calculated as the texture coarseness of a gray image which is transformed from the RD map. The analysis shows that Tamura coarseness can correctly reflect the radio frequency interference in the RD image and is also robust when the gray transforming function changes. As an example of its application, Tamura coarseness is adopted to improve the interference suppression algorithm, so that the suppression can be optimized adaptively. Simulation results show that Tamura coarseness can be used as a quality index of the RD image, which helps to suppress the interference perfectly.
-
近年来, 轮式移动方式以其高效率、高能量利用率引起了腿足式机器人研究者的广泛关注[1]. 试图搭建腿轮构型机器人, 将腿式机器人的高越障能力与轮式运行方式的快速移动能力相结合. 因此, 双腿轮机器人、四腿轮机器人等平台应运而生[2]. 其中, 双腿轮机器人具有更加灵活的移动与转向性能, 在多种行业工况下具有广阔的应用前景[3].
为使双腿轮机器人的地形适应能力进一步提高, 面临较高垂直障碍时, 可采用跳跃形式快速跨越. 苏黎世理工大学研发的双腿轮机器人Accento[4-5]在基于线性二次调节器(Linear quadratic regulator, LQR)平衡控制器的基础上进行跳跃控制器的设计, 实现了跳跃运动, 收到了显著的效果. 美国波士顿动力公司研发的Handle双腿轮机器人[6]实现了快速加减速、跳跃、搬运重物等功能, 展现了良好的动态平衡能力. 如何构建一个可以将运动与平衡功能集于一体的具有扩展性的双腿轮控制框架, 成为一个亟待解决的问题. 但是, 双腿轮机器人系统为内在不稳定系统, 且具有欠驱动特性, 运动过程中不存在稳定平衡点, 因此需要借助复杂的控制算法使其保持动态平衡. 而针对该系统的高动态运动控制研究尚处于起步阶段.
移动机器人经过半个世纪的发展, 基于静态稳定的控制算法已成熟, 主要应用对象包括双足机器人[7]、四足机器人[8]、六足机器人[9]等, 该类算法在保证机器人静态稳定裕度的前提下进行位置控制, 实现期望运动, 其依据为ZMP (Zero moment point)[10]、ESM (Energy stability margin)[11]、NESM (Normalized ESM)[12]等静态稳定判据, 因此无法实现高动态运动控制. 而双腿轮机器人的支撑面为两轮与地面接触点的连线, 不存在静态稳定区域, 因此静态稳定算法对于双腿轮机器人的借鉴意义不大.
双轮机器人的控制方法已渐趋成熟, 作为一个欠驱动不稳定多耦合系统, 常用于控制算法的有效性验证[13-14], 日本庆应大学搭建了两轮移动机械臂平台, 验证了基于零空间的共振比率控制器[15]、滑膜控制器[16]、非线性反步法控制器等控制轮的移动与机械臂运动的有效性; 意大利技术研究院控制双足机器人站立于两轮移动平台Segway上[17], 采用二次优化方法以质心位置与角动量为调节目标, 生成全身关节力矩, 能够保持平衡并控制平台移动. 大连理工大学团队采用模型预测控制(Model predictive control, MPC)实现了两轮移动平台的运动轨迹跟踪[18]. 以上研究都具有借鉴意义, 但与两轮移动平台不同, 双腿轮平台不再存在两轮同轴被动约束, 这也使得控制器的构造难度大大增加.
近几年来, 腿足式机器人动态控制算法成为研究热点. 得益于各种实时优化解算库[19-20]的开发, 控制中的多解问题可转化为优化问题, 得到局部甚至全局最优解[21-22]. 如美国麻省理工学院的四足机器人Mini-cheetah[23]构造了全身控制与模型预测控制相结合的控制器, 实现了4 m/s的奔跑和碎石路面行走. 双足机器人Atlas[24]跟踪采用模型预测控制器优化的参考轨迹, 实现了左右脚交替三连跳40 cm台阶、前滚翻等高动态运动.
为提高双腿轮机器人的地形适应性, 实现稳定的速度跟随与跳跃, 本文在前期研究的基础上[25], 构造了基于最优力分配的全身力矩控制器并实现了跳跃运动, 其创新点如下:
1)实现了基于优化的自平衡全身力矩控制. 该方法可自行生成同时满足平衡与期望运动的全身力矩, 不需要设计平衡控制器. 受到跳跃着陆冲击干扰时, 能够有效规划期望状态轨迹, 并恢复有界稳定.
2)提出了适用于双腿轮机器人的连续跳跃规划. 可在保证高速运动的前提下, 实现垂直障碍的跨越, 并能够在腾空阶段对俯仰姿态进行有效调整, 保证着陆姿态.
1. 运动学与动力学建模
本文的控制对象以美国HEBI Robotics公司的双腿轮机器人HEBI IGOR[26]为原型, 其简化模型如图1所示. 该机器人具有一个单刚体躯干与两条结构相同相对于躯干左右对称布置的腿轮. 躯干尺寸为$200\;{\rm{ mm}}\times300\;{\rm{ mm}}\times200\; {\rm{mm}}\; (L\times w\times h)$, 质量为40 kg. 腿轮完全伸展长度为1150 mm, 其中大腿长度(L1)为500 mm, 小腿长度(L2)为500 mm, 轮的半径(R)为150 mm. 每条腿轮布置3 个旋转关节, 所有关节都可以实现独立的力矩控制. 髋部与膝部为俯仰旋转关节, 能够实现腿部的收缩与伸展, 轮通过一个俯仰旋转关节与腿部末端相连, 从而实现机器人在三维空间中的移动.
1.1 运动学建模
双腿轮机器人为典型的动基座系统, 浮动坐标系${\Sigma _B}$固连于躯干质心处, 其在惯性系中的线速度与角速度分别描述为${}^{\boldsymbol{O}}{{\boldsymbol{v}}_{\boldsymbol{B}}}$与${}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{\boldsymbol{B}}}$. 各肢体末端均由关节、连杆与躯干相连, 因此, 肢体末端广义速度同时受到躯干动基座广义速度与各关节角速度的影响.
$$\left[ {\begin{array}{*{20}{c}} {{}^{\boldsymbol{O}}{{\boldsymbol{v}}_{i}}} \\ {{}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{i}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\boldsymbol{E}}&{ - {}^{\boldsymbol{O}}{{\vec {\boldsymbol{r}}}_{\boldsymbol{B}i}}} \\ {\boldsymbol{0}}&{\boldsymbol{E}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{}^{\boldsymbol{O}}{{\boldsymbol{v}}_{\boldsymbol{B}}}} \\ {{}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{\boldsymbol{B}}}} \end{array}} \right] + {{\boldsymbol{J}}_{i}}{\dot {{\boldsymbol{q}}}_{i}}$$ (1) 其中, 本文中编号$i= 1, 2$分别代表右腿与左腿, ${}^{\boldsymbol{O}}{{\boldsymbol{v}}_{i}}$与${}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{i}}$分别为腿i末端相对于世界坐标系的速度与角速度. ${}^{\boldsymbol{O}}{\vec {\boldsymbol{r}}_{\boldsymbol{B}i}}$为由躯干坐标系到腿i末端坐标系的位矢在世界坐标系中的表示, ${{\boldsymbol{q}}_{i}}$为腿${i}$主动关节角度.
1.2 动力学建模
为便于构造分布式控制器, 采用了分布式动力学建模方法[25]. 分布式动力学模型由躯干子系统以及腿轮子系统组成. 腿轮子系统通过广义力旋量与躯干子系统形成迭代递推关系, 其广义力旋量的表示如图1所示. 躯干的动力学模型为
$${{\boldsymbol{M}}_{\boldsymbol{b}}}\left( {\boldsymbol{X}} \right)\ddot {\boldsymbol{X}}{\rm{\;+\;}}{{\boldsymbol{G}}_{\boldsymbol{b}}}\left( {\boldsymbol{X}} \right) = {{\boldsymbol{J}}^{{\rm{T}}}}{\boldsymbol{W}}$$ (2) 其中, ${\boldsymbol{X}} = {\left[ {{}^{O}{x_B}}\;\;{{}^{O}{y_B}} \;\; {{}^{O}{z_B}}\;\;\gamma \;\;\beta \;\;\alpha \right]^{\rm{T}}}$为躯干坐标系${\Sigma _B}$在大地坐标系${\Sigma _O}$中的位姿描述, ${{\boldsymbol{M}}_{\boldsymbol{b}}}$为惯性矩阵, ${{\boldsymbol{G}}_{\boldsymbol{b}}}$为重力补偿项. ${\boldsymbol{W}} = {\left[ {{\boldsymbol{W}}_{1}}\;\;{{{\boldsymbol{W}}_{2}}} \right]^{\rm{T}}}$为腿i末端施加于躯干的广义力旋量, 其中, ${{\boldsymbol{W}}_{i}} = {\left[{{f_{ix}}}\;\;{{f_{iz}}}\;\;{{n_{iy}}} \right]^{\rm{T}}}$. 支撑工况下, 腿轮施加于躯干的力旋量可作为躯干子系统的输入, 从而实现躯干子系统的控制. 由于轮式结构造成的非完整约束, 世界坐标系中躯干侧向运动(${}^{O}{\dot y_B}$)与前向运动(${}^{O}{\dot x_B}$)由躯干坐标系中x方向速度${}^B{{v}_{{B}}}$和航向角度($\alpha $)耦合而成, 基于非完整约束运动学构造矩阵${\boldsymbol{S}}$:
$${\boldsymbol{S}}\;{\rm{ = }}\left[ {\begin{array}{*{20}{c}} {\cos \alpha}&0&0&0&0 \\ {\sin \alpha }&0&0&0&0 \\ 0&1&0&0&0 \\ 0&0&1&0&0 \\ 0&0&0&1&0 \\ 0&0&0&0&1 \end{array}} \right]$$ (3) 使得${\dot{\boldsymbol X}}\;{\rm{=}}\;{\boldsymbol{S\eta}}$, 其中${\boldsymbol{\eta}}\; {\rm{=}}\;[{ {{}^B{{v}_{{B}}}}\;\;{{}^B{{\dot z}_B}}\;\;{\dot \gamma }\;\;{\dot \beta }\;\;{{\omega}}^{\rm{T}}}]$, ${{\omega}} $为航向角速度, 求导得$\ddot{\boldsymbol{X}}{\rm{ \;= \;}}{\boldsymbol{S}}\dot{\boldsymbol{\eta}} $, 因此, 式(2)中动力学模型可改写为
$${\boldsymbol{\Psi}} \dot {\boldsymbol{\eta}}\; {\rm{ + }}\;{{\boldsymbol{G}}_{\boldsymbol{b}}}\left( {\boldsymbol{X}} \right) = {{\boldsymbol{J}}^{{\rm{T}}}}{\boldsymbol{W}}$$ (4) 其中, ${\boldsymbol{\Psi}} ={{\boldsymbol{M}}_{\boldsymbol{b}}}{\boldsymbol{S}}$. 此时, 状态量${\boldsymbol{\eta}}$为躯干刚体在自然坐标系(始终与机体坐标系重合)中的运动描述.
与腿足式机器人不同, 双腿轮机器人处于支撑状态时, 腿轮不仅要提供躯干所需虚拟力, 还要在不同运动状态下保持唯一的平衡姿态. 因此, 以轮为动基座, 采用递推牛顿−欧拉方法建立支撑状态下的腿轮动力学方程如下所示:
$$\begin{split} &{{\boldsymbol{M}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\boldsymbol{q}}_{iw}}} \right){{\ddot {\boldsymbol{q}}}_{i}} + {{\boldsymbol{C}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\dot {\boldsymbol{q}}}_{i}},{{\boldsymbol{q}}_{iw}},{{\dot {\boldsymbol{q}}}_{iw}}} \right)+\\ & \quad{{\boldsymbol{G}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\boldsymbol{q}}_{iw}}} \right){\rm{ + }}\;{{\boldsymbol{I}}}\left( {{{q}_{iw}},{{\dot q}_{iw}}} \right){{\ddot q}_{iw}} + {\boldsymbol{J}}_{i}^{\rm{T}}{{\boldsymbol{W}}_{i}} = {{\boldsymbol{\tau}} _{i}} \end{split} $$ (5) 其中, ${{\boldsymbol{\tau}} _{i}} = {\left[ {{\tau _{iw}}}\;\;{{\tau _{ik}}}\;\;{{\tau _{ih}}} \right]^{\rm{T}}}$分别为腿i的轮关节、膝关节与髋关节力矩. ${{\boldsymbol{q}}_{i}}={\left[ {{{q}_{ic}}}\;\;{{{q}_{ik}}}\;\;{{{q}_{ih}}} \right]^{\rm{T}}}$分别为腿i 的小腿俯仰角度、膝关节与髋关节角度. ${{\boldsymbol{M}}_{i}}\left( {{{\boldsymbol{q}}_{i}}} \right)$、${{\boldsymbol{C}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\dot {\boldsymbol{q}}}_{i}}} \right)$、${{\boldsymbol{G}}_{i}}\left( {{{\boldsymbol{q}}_{i}}} \right)$分别为惯性矩阵、科氏力与离心力、重力补偿矩阵. 值得注意的是, 所提出的分布式动力学建模方法, 其关节角度变量中不包括轮式动基座的线运动与角运动, 但当双腿轮机器人进行高速运动时, 腿部连杆会产生较大的惯性力, 从而影响控制精确度与系统的平衡. 因此, ${\boldsymbol{I}}\left( {{{q}_{iw}},{{\dot q}_{iw}}} \right)$为动基座惯性力补偿矩阵, 以补偿高速运动时动基座传导给各腿轮的惯性力.
当双腿轮机器人处于腾空状态时, 将腿轮子系统分解为两个简化的模型 —— 腿部模型与轮式倒立摆模型, 腿部模型中髋关节坐标系与躯干坐标系方向相同, 以髋关节坐标系为基座, 建立动力学模型如下:
$${\boldsymbol{M}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s}} \right)\ddot {\boldsymbol{q}}_{i}^{s}\;+\; {\boldsymbol{C}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s},\dot {\boldsymbol{q}}_{i}^{s}} \right)\;+\;{\boldsymbol{G}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s}} \right) + {\boldsymbol{J}}_{i}^{{s}{\rm{T}}}{{\boldsymbol{F}}_{is}}{\rm{\;=\;}}{\boldsymbol{\tau}} _{i}^{s}$$ (6) 其中, ${\boldsymbol{\tau}} _{i}^{s} = \left[ {{\tau _{ih}}}\;\;{{\tau _{ik}}} \right]$, ${\boldsymbol{q}}_{i}^{s} = {\left[ {{{q}_{ih}}}\;\;{{{q}_{ik}}} \right]^{\rm{T}}}$.
为研究腾空阶段轮的转动对机器人姿态的影响, 将模型简化为一级轮式倒立摆, 即只关注矢状面内的运动, 将除轮以外的杆件质量集中于一个质点, 形成虚拟质心, 虚拟质心与轮轴的连线为虚拟倒立摆摆杆. 进行无外部接触力的拉格朗日动力学建模, 其动力学模型为
$$\begin{split} &\left( {{m_u} + {m_w} + \dfrac{{{I_w}}}{{{R^2}}}} \right)R{{\ddot \theta }_w} + L{m_u}\ddot \sigma {{\rm{c}} _\sigma } - L{m_u}{{\dot \sigma }^2}{{s} _\sigma } = 0 \\ &L{m_u}R{{{c}} _\sigma }{{\ddot \theta }_w} + \left( {{I_{b}} + {L^2}{m_u}} \right)\ddot \sigma - gL{m_u}{{s} _\sigma } = - {\tau _w} \\[-10pt] \end{split} $$ (7) 其中, ${m_u}$为除轮外其他杆件的质量总和, $\sigma $为倒立摆的虚拟俯仰角, L为虚拟摆杆长度, ${I_{b}}$与${I_w}$分别为摆杆与轮的转动惯量.
2. 轨迹生成器
构造轨迹生成器使机器人可根据障碍物的高度规划此次跳跃的躯干参考轨迹与足端参考轨迹. 本节中, 首先根据障碍物高度确定关键点处的位置与速度, 然后采用三次样条插值法得到躯干质心点与轮最小离地点的连续位置与速度. 简称轮最小离地点为足端. 相关参数的含义如表1所示.
表 1 跳跃参数设置Table 1 Parameters of the jump motion参数 参数含义 H0 正常行走时躯干站立高度在${\Sigma _O}$中的表示 H1 腾空瞬间躯干站立高度在${\Sigma _O}$中的表示 Hpt 足端最大离地距离在${\Sigma _O}$中的表示 Hs 足端最大收缩量在${\Sigma _B}$中的表示 图2展示了机器人跳跃时躯干与足端轨迹, 虚线表示躯干正常站立高度H0, 实线为跳跃过程中躯干的期望轨迹. 支撑相时, 机器人双轮着地; 腾空相时, 机器人双轮离地, 处于自由落体状态. 将跳跃动作分解为三阶段, 分别为起跳($p_0-p_1$)、腾空($p_1- p_3$)、缓冲($p_3-p_4$). 起跳阶段, 通过腿部快速伸展, 为躯干纵向加速, 当达到期望速度($p_1$)时, 腿部开始收缩, 进入腾空相. 腾空相躯干受到重力加速度, 纵向速度不断减小, 在此过程中, 腿部完成了收缩, 并伸展到可保持正常站高H0的长度, 准备落地. 缓冲阶段将纵向速度调整为0, 并保持期望站高.
图 2 躯干与足端矢状面轨迹示意图Fig. 2 The trajectory schematic of the torso and feet in the sagittal plane图2中, $ H_0 $、$ H_1 $以及腿部最大收缩量$H_s $ 为常量, 由机器人本体结构确定, $H_{pt} $由所要跨越的障碍高度确定. $p_2 $ 点的高度也随之确定: ${H_2}=H_s + {H_{pt}} $, 由于p2点的速度${{v}_2} = 0$, 根据能量守恒定理, 可得:
$$\left\{\begin{aligned} &{{v}_1} = \sqrt {2g\left( {{H_{pt}} + {H_s} - {H_1}} \right)} \\ &{{v}_{\rm{3}}} = \sqrt {2g\left( {{H_{pt}} + {H_s} - {H_{\rm{0}}}} \right)} \\ &{t_{01}} = \dfrac{{2\left( {{H_1} - {H_0}} \right)}}{{\sqrt {2g\left( {{H_{pt}} + {H_s} - {H_1}} \right)} }} \\ & {t_{12}} = \dfrac{{\sqrt {2g\left( {{H_{pt}} + {H_s} - {H_1}} \right)} }}{g} \\ & {t_{{\rm{23}}}} = \dfrac{{\sqrt {2g\left( {{H_{pt}} + {H_s} - {H_{\rm{0}}}} \right)} }}{g} \end{aligned}\right. $$ (8) 其中, ${{v}_1}$和${{v}_{\rm{3}}}$分别为点p1和p3处的速度. 足端离地时间即腾空时间为${t_{12}} + {t_{23}}$. 为同时保证每一段期望轨迹的平滑以及初始点与结束点的位置$\left( {{y_{{\rm{init}}}}},{{y_{{\rm{end}}}}}\right)$、速度$\left( {{{v}_{{\rm{init}}}}},{{{v}_{{\rm{end}}}}} \right)$要求, 采用三次样条函数生成连续轨迹. 起跳阶段的纵向位置期望轨迹、$p_1$-$p_2 $阶段、${p_2}$-${p_3}$ 阶段足端期望轨迹均由式(9)生成:
$$ \left\{\begin{aligned} &y=a{t}^{3}+b{t}^{2}+ct+d\\ &c={v}_{{\rm{init}}}\\ &d={y}_{{\rm{init}}}\\ &a=\dfrac{{v}_{{\rm{end}}}t-2{y}_{{\rm{end}}}+{v}_{{\rm{init}}}t+2{y}_{{\rm{init}}}}{T^{3}}\\ &b=\dfrac{{v}_{{\rm{end}}}-3a{T}^{2}-c}{2T}\end{aligned}\right.$$ (9) 其中, T为该段三次样条插值的总时长.
3. 全身力矩控制器
为实现机器人奔跑与跳跃的有效控制, 本节介绍了分布式控制器的设计方法. 首先基于躯干子系统的动力学模型, 采用二次规划 (Quadratic programming, QP)二次优化得到合理的虚拟作动力以使得躯干位姿满足控制要求. 采用逆动力学前馈补偿与逆运动学反馈补偿方法得到腿轮关节力矩.
3.1 基于最优虚拟力分配的躯干控制器
对于躯干单刚体进行静力分析, 假设躯干质量集中于几何中心处, 建立躯干质心合力与躯干受到的腿轮虚拟作动力的映射关系如下所示:
$$\left[ {\begin{array}{*{20}{c}} {\boldsymbol{F}} \\ {\boldsymbol{N}} \end{array}} \right]{\rm{\;+ \;}}{{\boldsymbol{G}}_{b}} = {\boldsymbol{A}}\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{W}}_{1}}} \\ {{{\boldsymbol{W}}_{2}}} \end{array}} \right]$$ (10) 其中, ${\boldsymbol{F}}{\rm{\;= [}}{F_x},{F_y},{F_z}{]^{\rm{T}}},\;{\boldsymbol{N}} = {[{N_x},{N_y},{N_z}]^{\rm{T}}}$分别为躯干质心合力与力矩. 受到关节配置的约束, 腿轮末端只能主动输出延机体坐标系x和z的力与绕y轴的力矩, 式(10)可展开为式(11)的形式.
$$\begin{split} \left[ {\begin{array}{*{20}{c}} {\boldsymbol{F}} \\ {\boldsymbol{N}} \end{array}} \right] + &\left[ {\begin{array}{*{20}{c}} {{m_{{b}}}g\sin \beta } \\ 0 \\ {{m_{{b}}}g\cos \beta } \\ {\rm{0}} \\ {\rm{0}} \\ {\rm{0}} \end{array}} \right] = \\ &\underbrace {\left[ {\begin{array}{*{20}{c}} 1&0&0&1&0&0 \\ 0&0&0&0&0&0 \\ 0&1&0&0&1&0 \\ 0&{\dfrac{w}{2}}&0&0&{ - \dfrac{w}{2}}&0 \\ { - \dfrac{h}{2}}&0&1&{ - \dfrac{h}{2}}&0&1 \\ { - \dfrac{w}{2}}&0&0&{\dfrac{w}{2}}&0&0 \end{array}} \right]}_{\boldsymbol{A}}\left[ {\begin{array}{*{20}{c}} {{f_{1x}}} \\ {{f_{1z}}} \\ {{n_{1y}}} \\ {{f_{2x}}} \\ {{f_{2z}}} \\ {{n_{2y}}} \end{array}} \right]\end{split}$$ (11) 其中, $g = 9.8 \; {\rm{m}}/{{\rm{s}}^2}$. 根据不同维度的特点构造躯干期望虚拟力${\boldsymbol{F}}$与${\boldsymbol{N}}$的反馈部分, 基于动力学模型进行必要的前馈补偿, 为保证速度跟踪性能, 虚拟力${F_x}$的反馈部分采用自适应滑膜方法构造, 其他维度采用比例−微分控制方法构造, 具体方法为
$$\left\{\begin{aligned} &{F_x} = s + sat\left( s \right)c \\ &s = \dot e + \lambda e \\ &e = {{\dot x}_d} - {}^B{{{v}}_{{B}}}, \dot e = {{\ddot x}_d} - {}^B{{\dot v}_B} \\ &{F_z} = {k_{pz}}\left( {{z_d} - {}^B{z_B}} \right) + {k_{dz}}\left( {{{\dot z}_d} - {}^B{{\dot z}_B}} \right)+{m_{{b}}}{{\ddot z}_d} \\ &{N_x} = {k_{p\gamma }}\left( {{\gamma _d} - \gamma } \right) + {k_{n\gamma }}\left( {{{\dot \gamma }_d} - \dot \gamma } \right) \\ & {N_y} = {k_{p\beta }}\left( {{\beta _d} - \beta } \right) + {k_{n\beta }}\left( {{{\dot \beta }_d} - \dot \beta } \right) \\ &{N_z} = {k_{{\omega}} }\left( {{{{\omega}} _d} - {{\omega}} } \right) \end{aligned}\right. $$ (12) 矩阵${\boldsymbol{A}}$为奇异矩阵, 因此无法直接解得与躯干虚拟广义力一一对应的腿轮末端作动力, 因此, 将式(11)变形为
$${\boldsymbol{A}}\underbrace {\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{W}}_{1}}} \\ {{{\boldsymbol{W}}_{2}}} \end{array}} \right]}_x=\underbrace {{{\left[ {\begin{array}{*{20}{c}} {\boldsymbol{F}} \\ {\boldsymbol{N}} \end{array}} \right]}^{{\rm{desire}}}}+{{\boldsymbol{G}}_{b}}}_b$$ (13) 通过QP优化器进行作动力求解
$$\begin{split} &\mathop {\min }\limits_{\boldsymbol{x}} \;\;\;f =\; {\left( {{\boldsymbol{Ax}} - {\boldsymbol{b}}} \right)^{\rm{T}}}{\boldsymbol{Q}}\left( {{\boldsymbol{Ax}} - {\boldsymbol{b}}} \right)\;+ \\ & \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{\boldsymbol{P}}{\left\| {{\boldsymbol{x}} - {{\boldsymbol{x}}_{{\rm{pre}}}}} \right\|^2} + {\boldsymbol{J}}{\left\| {\boldsymbol{x}} \right\|^2} \\ &{\rm{s.t}}\; \left| {{f_{ix}}} \right| \leq \mu {f_{iz}} \end{split} $$ (14) 其中, Q, P, J为正定对角权重矩阵, ${{\boldsymbol{x}}_{{\rm{pre}}}}$为上一时刻的解.
3.2 腿部控制器
当机器人处于支撑相时, 躯干控制器将躯干期望合力分配为各条腿轮的末端力旋量. 为实现力伺服, 基于轮式基座动力学模型前馈补偿的逆动力学力矩解算器如下所示
$$\begin{split} {{\boldsymbol{\tau}} _{i}} =\;& {\boldsymbol{J}}_{i}^{\rm{T}}{{\boldsymbol{W}}_{i}} + {{\boldsymbol{C}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\dot {\boldsymbol{q}}}_{i}},{{q}_{iw}},{{\dot q}_{iw}}} \right) + \\ &{{\boldsymbol{G}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{q}_{iw}}} \right) +{i}\left( {{{q}_{iw}},{{\dot q}_{iw}}} \right){{\ddot q}_{iw}} \end{split} $$ (15) 其中, ${q}_{iw}{\text{, }}{\dot{q}}_{iw}{\text{, }}{\ddot{q}}_{iw}$均由基于传感器的状态观测器得到. 基于动力学前馈补偿的控制方法可以使机器人自适应地保持在唯一的不稳定平衡点处.
当机器人处于腾空相时, 为保证腿轮末端与躯干的相对位置以及机器人在世界坐标系中姿态的稳定, 将腾空相腿部控制器分为两部分, 基于式(6)动力学模型, 对髋关节与膝关节进行力矩控制以实现腿轮末端轨迹跟踪.
$${\boldsymbol{\tau}} _{i}^{s} = {\boldsymbol{C}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s},\dot {\boldsymbol{q}}_{i}^{s}} \right) + {\boldsymbol{G}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s}} \right) + {\boldsymbol{J}}_{i}^{{s}{\rm{T}}}{{\boldsymbol{F}}_{is}}$$ (16) 其中, ${{\boldsymbol{F}}_{is}} = {\left[ {{f_{ipx}}}\;\;{{f_{ipz}}}\right]^{\rm{T}}}$.
$$\left\{\begin{aligned} &{f_{ipx}} = {k_{ipx}}\left( {{x_{pd}} - {x_{ip}}} \right) + {k_{idx}}\left( {{{\dot x}_{pd}} - {{\dot x}_{ip}}} \right) \\ &{f_{ipz}} = {k_{ipz}}\left( {{z_{pd}} - {z_{ip}}} \right) + {k_{idz}}\left( {{{\dot z}_{pd}} - {{\dot z}_{ip}}} \right) \end{aligned}\right. $$ (17) 其中, $\left( {{x_{ip}},{z_{ip}}} \right)$与$\left( {{x_{pd}},{z_{pd}}} \right)$分别为自然坐标系中的足端位置描述与期望足端位置描述.
在腾空阶段, 系统处于动量守恒状态, 因此腿部关节的运动会导致整体姿态的变化, 此时可通过控制飞轮转动对姿态的误差进行有效补偿. 将一级轮式倒立摆动力学模型式(7)变形, 得到:
$$\ddot \beta = - \frac{{L{m_{b}}R{{\rm{c}} _\beta }}}{{{I_{b}} + {L^2}{m_{b}}}}{\ddot \theta _w} - \frac{1}{{{I_{b}} + {L^2}{m_{b}}}}{\tau _w} + \frac{{gL{m_{b}}{{\rm{s}} _\beta }}}{{{I_{b}} + {L^2}{m_{b}}}}$$ (18) 此时轮不与地面接触, 可以看作调整平衡的飞轮, 通过式(18)可以看出, 飞轮的转动能够影响倒立摆的俯仰角运动状态, 当俯仰角逆时针偏离期望位置时, 飞轮应顺时针转动保证姿态平衡, 反之亦然. 基于这一结论, 可以根据躯干控制器中输出的俯仰姿态矫正力矩${N_y}$ (如式(12)所示)来规划轮的力矩, 从而得到轮关节的力矩为
$$ {\tau _{{\rm{1}}w}} = {\tau _{2w}} = - k{N_y} $$ (19) 其中, k为正比例增益.
4. 仿真实验
在Webots 8.4.0中建立与HEBI IGOR同尺寸3D模型, 与Visual Studio 2017、Gurobi 902优化解算库进行联合仿真. 为验证本文所提算法的有效性, 分别设置了连续跳跃实验与空中姿态调整对比实验, 其总体控制框图如图3所示. 仿真视频见
https://www.bilibili.com/video/BV1zz4y1X7jp/ .根据期望跳跃高度${H_{pt}}$的不同, 轨迹生成器会生成不同的起跳阶段躯干纵向期望轨迹. 分别针对${H_{pt}}\;{ = 0}.45\;{\rm{m}}$, ${H_{pt}}\;{ = 0}.35\;{\rm{m}}$, ${H_{pt}}\;{ = 0}.25\;{\rm{m}}$进行了弹跳实验, 机器人的躯干纵向轨迹${z_{b}}$以及足端位置轨迹${z_p}$如图4所示, 可以看出期望弹跳高度越大, 所规划的躯干纵向期望轨迹在起跳点处纵向速度越大, 从而腾空相时间越长.
图 4 不同离地高度下躯干与足端纵向位置曲线Fig. 4 Longitudinal position curves of the torso and feet at different ground clearance4.1 连续跳跃实验
本仿真实验主要验证: 1)该算法框架下的跳跃动作是否能够实现; 2)该算法框架是否能够对跳跃动作带来的强干扰进行抑制并保持有界稳定. 因此, 仿真实验设置如下: 机器人初始状态为站立静止, 在0 ~ 3 s内加速至2 m/s, 随后保持匀速运动. 当t = 3.35 s时, 通过外部遥控下达${{H}_{{pt}}}{\rm{\;= \;0}}.45\;{\rm{m}}$的跳跃命令, 机器人依次执行起跳−腾空−落地缓冲−调整; 当t = 5.85 s时, 再次下达${H_{pt}}=0.45\;{\rm{m}}$的跳跃命令, 机器人完成跳跃并最终将跳跃导致的状态偏移调整到有界稳定状态. 以首次跳跃为例, 其仿真截图如图5所示.
对于完全站立高度为1.35 m的腿轮机器人来说, 最大离地高度为0.45 m的跳跃会对其平衡状态产生较大冲击, 甚至失去平衡. 由图6和图7中曲线可看出, 每次跳跃结束后, 各状态量进行了及时调整. 0 ~ 3 s加速阶段躯干水平速度的最大跟踪误差不超过0.075 rad, 其他支撑阶段, 误差小于0.04 rad. 图7可见, 处于支撑相时, 加速度越大水平速度误差越大, 最大误差为0.06 m/s. 当机器人进入腾空相时, 与外界环境不再产生相互作用力, 此时, 机器人保持动量守恒状态, 因此腿部伸缩运动影响了躯干的水平速度, 最大误差不超过0.08 m/s. 由腾空相切换为支撑相的瞬间, 实际水平速度与期望速度(2 m/s)之间存在偏差, 控制器实时规划期望速度曲线, 并将速度调整为2 m/s. 此外, 如图6所示, 腾空相中机器人pitch与roll角波动较大, 但着陆后控制器可有效控制姿态角恢复到0 rad附近. 与之对应的, 如图8所示, 力与力矩在着陆后出现较明显波动, 用以调整状态量使其恢复临界稳定.
图8为躯干子系统所需的虚拟广义力曲线图, 可见, 前进方向虚拟作动力${F_x}$与躯干水平加速度相关, 0 ~ 3 s水平加速度始终为正, 因此, ${{F_x}> 0}$, 最大值约为46 N; 当站高保持常值时, 纵向虚拟作动力${F_z}$在388 N ~ 398 N (mb = 40 kg)区间内波动, 由于起跳瞬间躯干纵向期望轨迹的初始点处加速度不为零, 加速度跳变导致了${F_z}$的跳变.
躯干纵向位置跟踪曲线如图9所示, 最大误差为48 mm, 发生在起跳阶段, 当期望站高为常量${H_{\rm{0}}}$时, 最大误差为0.2 mm, 发生在落地缓冲阶段. 可见该控制框架具有对纵向位置跟踪的稳定性与抗扰能力. 腾空阶段, 足端位置基于躯干动基座计算得到, 从而躯干动基座的波动会影响足端轨迹跟随, 导致足端轨迹波动较大, 最大误差为50 mm.
通过最优力分配将躯干虚拟作动力分配为各腿轮末端期望的输出广义力如图10所示. 两腿轮的广义力输出基本保持一致, 在着陆后有明显的调整行为. 各关节力矩曲线如图11所示, 膝关节力矩输出较大, 髋关节与轮关节力矩均在0附近波动.
4.2 空中姿态调整对比实验
为验证轮轴力矩对俯仰姿态矫正的有效性, 设计了腾空姿态调整对比实验. 实验1中腾空相的轮轴力矩${\tau _w} = 0\;({\rm{N}}\cdot{\rm{m}})$, 实验2中腾空相的轮轴力矩${\tau _w} = - {N_y}$, 保证其他条件完全相同. 图12为对比实验的俯仰角曲线, 可见当${\tau _w} = 0\;({\rm{N}}\cdot{\rm{m}})$时, 俯仰角由腾空初始时刻到着陆瞬间, 一直处于单向递增状态, 若腾空时间过长会导致着陆姿态不稳定. 当${\tau _w} =- {N_y} $时, 以俯仰角状态量为反馈进行了有效的姿态调整, 使得腾空相俯仰角在 $(- 0.015\;{\rm {rad}} $, 0.02 rad)范围内波动. 证明了基于角动量守恒的轮的反向转动对腾空姿态的调整是有效的.
5. 结束语
为使双腿轮机器人实现高动态运动, 本文提出了基于最优力分配的全身力矩控制器, 该控制器基于动力学模型进行设计, 所生成的全身力矩可在不额外规划平衡控制器的基础上实现机器人的自平衡, 并有效控制躯干位姿. 仿真实验表明, 该控制器能够同时保证水平速度、站高等多个维度的状态量处于有界稳定. 为提高双腿轮机器人地形适应能力, 规划并实现了双腿轮机器人的跳跃运动, 使其能在不打断高速运动的同时跨越垂直障碍, 进行了连续跳跃实验, 验证了所提方法的有效性.
-
图 2 不同干扰情形下的RD灰度图: (a) ~ (c)分别表示无动态范围转换的无干扰、有窄带射频干扰、有宽带射频干扰的RD灰度图; (d) ~ (f)分别表示对应的动态范围为80 dB转换的RD灰度图
Fig. 2 RD gray-scale images: (a) ~ (c) are images without dynamic range conversion, for no RFI, narrowband RFI, and wideband RFI, respectively; (d) ~ (f) are corresponding images with dynamic range conversion for 80 dB
图 3 不同干扰抑制结果的RD灰度图: (a) ~ (e)分别表示无动态范围转换的无干扰、有宽带干扰、干扰未完全抑制、干扰完全抑制、干扰过度抑制的RD灰度图; (f) ~ (j)分别表示对应的动态范围为80 dB转换的RD灰度图
Fig. 3 RD gray-scale image for various interference suppression results: (a) ~ (e) are images without interference, broadband RFI, incomplete suppression, complete suppression, and excessive suppression, without dynamic range conversion; (f) ~ (j) are the corresponding images with dynamic range conversion for 80 dB
图 4 在无干扰的海杂波数据中加入目标回波后, RD图中20个目标的距离−多普勒单元位置
Fig. 4 The position of 20 targets in the RD map without any RFI
图 6 基于不同
$ \varepsilon$ 参数下的RD灰度图与粗糙度Fig. 6 RD gray-scale image and coarseness for varying
$ \varepsilon$ 
图 8 不同
$ K$ 值下$ \varepsilon$ 变化时的$k_{{\rm{best}}}$ 比例Fig. 8 The ratio of
$k_{{\rm{best}}}$ versus$ \varepsilon$ for varying$ K$ 表 1 不同干扰类型的RD图粗糙度
Table 1 Coarseness of RD images for various kinds of interference
无动态转换时灰度级 有动态转换时灰度级 256 64 32 256 64 32 无干扰 6.92 6.92 6.91 6.84 6.84 6.83 窄带RFI 7.37 7.37 7.36 7.31 7.31 7.30 宽带RFI 8.58 8.58 8.55 8.58 8.58 8.56 
下载: 导出CSV
表 2 不同干扰抑制程度的RD图粗糙度
Table 2 Coarseness of RD images for various levels of RFI suppression
无动态转换时灰度级 有动态转换时灰度级 256 64 32 256 64 32 无干扰 6.92 6.92 6.91 6.84 6.84 6.83 有干扰 8.58 8.58 8.55 8.58 8.58 8.56 未完全抑制 7.68 7.68 7.66 7.68 7.68 7.67 完全抑制 7.12 7.12 7.10 6.89 6.89 6.89 过度抑制 7.68 7.68 7.67 7.67 7.67 7.66
下载: 导出CSV
表 3 不同目标情况下RD图的粗糙度
Table 3 Coarseness of RD images of various targets
目标个数 0 1 5 10 15 20 无干扰 6.84 6.84 6.85 6.86 6.87 6.89 有干扰 8.58 8.58 8.58 8.58 8.58 8.58 未完全抑制 7.68 7.68 7.69 7.69 7.69 7.70 完全抑制 6.89 6.89 6.90 6.91 6.92 6.94 过度抑制 7.67 7.67 7.69 7.69 7.70 7.71
下载: 导出CSV
-
[1] 周文瑜, 焦培南. 超视距雷达技术. 北京:电子工业出版社, 2008. 132−134Zhou Wen-Yu, Jiao Pei-Nan. Over-the-horizon Radar Technology. Beijing: Publishing House of Electronics Industry, 2008. 132−134 [2] Wang W, Wyatt L R. Radio frequency interference cancellation for sea-state remote sensing by high-frequency radar. IET Radar, Sonar and Navigation, 2011, 5(4): 405-415 doi: 10.1049/iet-rsn.2010.0041 [3] 孔舒亚, 王亮. 压缩感知合成孔径雷达自适应射频干扰抑制方法. 电子测量技术, 2019, 42(13): 107-111Kong Shu-Ya, Wang Liang. Adaptive suppression method for SAR narrowband interference based on compressed sensing. Electronic Measurement Technology, 2019, 42(13): 107-111 [4] 文必洋, 韩金柱, 周企豪, 李艳. 高频地波雷达空域射频干扰抑制算法. 西安电子科技大学学报, 2017, 44(4): 132-137Wen Bi-Yang, Han Jin-Zhu, Zhou Qi-Hao, Li Yan. Radio frequency interference suppression algorithm in the spatial domain for high-frequency radars. Journal of Xidian University, 2017, 44(4): 132-137 [5] 罗忠涛. 新体制天波超视距雷达信号处理研究 [博士学位论文], 电子科技大学, 中国, 2015.Luo Zhong-Tao. Study on Signal Processing for Future Sky-wave Over-the-horizon Radar System [Ph.D. dissertation], University of Electronic Science and Technology of China, China, 2015. [6] 朱永恒, 黎明, 牛炯, 张玲, 纪永刚. 基于方向分析的高频地波雷达射频干扰抑制[J]. 中国海洋大学学报, 2018, 48(7): 142-148Zhu Yong-Heng, Li Ming, Niu Jiong, Zhang Ling, Ji Yong-Gang. Reasearch on radio frequency interference suppression for HFSWR[J]. Periodical Of Ocean University Of China, 2018, 48(7): 142-148 [7] 刘建成, 全厚德, 李召瑞, 刘东林, 赵宏志. 多路延迟正交合成的多径信道射频干扰对消. 电子与信息学报, 2017, 39(3): 654-661Liu Jian-Cheng, Quan Hou-De, Li Zhao-Rui, Liu Dong-Lin, Zhao Hong-Zhi. RF interference cancellation based on multi-tap delay and orthogonal combination in multipath channel. Journal of Electronics and Information Technology, 2017, 39(3): 654-661 [8] Luo Z T, He Z S, Li J. An effective scheme for radio frequency interference suppression in high-frequency radar. In: Proceedings of the 2015 IEEE Radar Conference. Arlington, VA, USA: IEEE, 2015. 539−544 [9] Luo Z T, Song T C, He Z S, Hu J F. Approach for transient interference detection based on straight line extraction for high-frequency sky-wave radar. Electronics Letters, 2017, 53(9): 618-620 doi: 10.1049/el.2016.4125 [10] 罗忠涛, 吴太锋, 何子述, 陈绪元. 基于图像分割的高频雷达射频干扰提取算法. 系统工程与电子技术, 2018, 40(4): 776-781 doi: 10.3969/j.issn.1001-506X.2018.04.10Luo Zhong-Tao, Wu Tai-Feng, He Zi-Shu, Chen Xu-Yuan. Extration of radio frequency interference based on image segmentation for high-frequency radar. Systems Engineering And Electronics, 2018, 40(4): 776-781 doi: 10.3969/j.issn.1001-506X.2018.04.10 [11] Tamura H, Mori S, Yamawaki T, Textural features corresponding to visual perception, IEEE Transactions on Systems, Man, and Cybernetics [12] Liu Y Q, Huang T Q. Exposing video inter-frame forgery by Zernike opponent chromaticity moments and coarseness analysis. Multimedia Systems, 2017, 23(2): 223-238 doi: 10.1007/s00530-015-0478-1 [13] 陈苏婷, 胡海锋, 张闯. 基于激光散斑成像的零件表面粗糙度建模. 物理学报, 2015, 64(23): 105-113Chen Su-Ting, Hu Hai-Feng, Zhang Chuang. Surface roughness modeling based on laser speckle imaging. Acta Physica Sinica, 2015, 64(23): 105-113 [14] 王顺杰, 齐春, 程玉胜. Tamura纹理特征在水下目标分类中的应用[J]. 应用声学, 2012, 31(2): 135-139 doi: 10.11684/j.issn.1000-310X.2012.02.013Wang Shun-Jie, Qi Chun, Cheng Yu-Sheng. Application of Tamura texture feature to classify underwater targets[J]. Applied Acoustics, 2012, 31(2): 135-139 doi: 10.11684/j.issn.1000-310X.2012.02.013 [15] Zhang X D, Shen P Y, Gao J R, X X, Qi D, Zhang L, et al. A license plate recognition system based on Tamura texture in complex conditions. In: Proceedings of the 2010 IEEE International Conference on Information and Automation. Harbin, China: IEEE, 2010. 1947−1952 [16] Gupta A, Garg M, Mittal A. A comparative performance evaluation of the 2010 segmented image with obstacle for textural coarseness. In: Proceedings of the 2010 IEEE International Conference on Computational Intelligence and Computing Research. Coimbatore, India: IEEE, 2010. 1−6 [17] Majtner T, Svoboda D. Extension of tamura texture features for 3D fluorescence microscopy. In: Proceedings of the 2nd International Conference on 3D Imaging, Modeling, Processing, Visualization and Transmission. Zurich, Switzerland: IEEE, 2012. 301−307 [18] Luo Z T, He Z S, Lu K, Chen X Y. Optimal receive filter design under similarity constraint in coloured noise. ET Radar, Sonar and Navigation, 2015, 9(7): 888-899 doi: 10.1049/iet-rsn.2014.0421 期刊类型引用(7)
1. 刘彦希，吴浩，蔡源，唐丹，宋弘. 基于改进EAST算法的电气设备铭牌文字检测. 四川轻化工大学学报(自然科学版). 2024(03): 42-50 . 
百度学术2. 常荣，唐力. 基于监督和卷积循环神经网络算法的电力设备铭牌识别技术. 电子器件. 2024(04): 1027-1032 . 百度学术3. 陈桃. AI视觉赋能工业质检的研究和实践. 江苏通信. 2024(04): 91-95 . 百度学术4. 石煌雄，胡洋，蒋作，潘文林，杨凡. 基于改进CTPN网络的电气铭牌文本区域检测. 云南民族大学学报(自然科学版). 2023(01): 96-102 . 百度学术5. 李雨，闫甜甜，周东生，魏小鹏. 基于注意力机制与深度多尺度特征融合的自然场景文本检测. 图学学报. 2023(03): 473-481 . 百度学术6. 董晨，郑禄，于舒，饶白云. 基于上下文感知与多尺度注意力的遥感变化检测. 软件导刊. 2023(11): 65-70 . 百度学术7. 姬壮伟. 轻量化双通道图像语义分割模型. 山西大同大学学报(自然科学版). 2022(05): 6-8 . 百度学术其他类型引用(7)
-
下载:
计量
- 文章访问数: 904
- HTML全文浏览量: 469
- PDF下载量: 90
- 被引次数: 14
