CN1885217A - 欠驱动双摆系统摆杆杂技动作控制方法 - Google Patents

Abstract

一种欠驱动双摆系统摆杆杂技动作控制方法，属于欠驱动系统控制技术。本发明利用欠驱动双摆系统，根据杂技演员的动作特点，模拟人体的动觉智能，利用摆杆的运动惯性，对由两摆杆的平衡位置和运动姿态组成的成套杂技动作进行分阶段控制。成套杂技动作的总控制器，由各阶段控制目标对应的子控制器，经关联图式连接而成。总控制器被编制成实时运行的程序控制代码，输入到相应的计算机控制系统中，输出的控制信号经伺服电机驱动器功率放大后，实现伺服电机的运动控制，进而驱动小车，带动两摆杆运动，使两摆杆完成各种成套杂技动作。本发明可广泛应用于欠驱动关节型机械臂、杂技机器人、天空飞行器和智能玩具等装置的控制。

Description

欠驱动双摆系统摆杆杂技动作控制方法

一、技术领域

本发明属于欠驱动系统的控制技术领域，特别涉及欠驱动双摆系统双摆杆的运动规划与控制方法。

二、背景技术

在控制领域中，倒立摆是公认的检验控制理论的实验装置。如申请号为02257287.2公开的“四级倒立摆实验仪”，主要由四级摆杆、基座、传动带、小车、滑轨、编码器、伺服电机等组成。该实验仪从初始状态(即人为地将四级摆杆置于垂直向上的状态)开始工作后，与各级摆杆连接的编码器检测各摆杆与垂直方向的夹角，并将其测量结果传给控制器(即《数学通报》2003年第6期“模糊控制理论及其在倒立摆控制中的应用”论文公开的“变论域的自适应模糊控制器”)。控制器对测得的信号进行处理，产生控制信号输出，由伺服驱动器驱动伺服电机。伺服电机通过传动带使小车运动，实现对处在倒立状态四级摆杆的稳定控制。该实验仪为控制理论的研究提供了检验手段，相应的控制方法提供了多级摆系统稳摆控制的解决策略，但无明显的工程意义。

在自动控制领域中，作为控制对象的倒立摆系统，是一个欠驱动、多变量、非线性、强耦合的复杂系统。对倒立摆的控制是一个十分经典而具有挑战性的研究课题。在现有欠驱动倒立摆系统的控制中，如前述“变论域的自适应模糊控制器”对四级倒立摆的实时稳定控制中，论域随着误差变小而收缩(亦可随着误差增大而扩展)。变论域的自适应模糊控制是一种近似线形化为基础的控制方法。在规则形式不变的前提下，论域收缩相当于增加规则，也即插值结点加密，从而提高了精度，是一种动态逐点收敛的插值器。所以，基于变论域的模糊控制器提高了模糊控制器的控制精度。由于在倒立状态下，欠驱动倒立摆杆只在倒立点附近的小范围运动，提高精度是有意义的。但是，对于摆杆从下垂点摆起至倒立点的运动控制，因其具有欠驱动、大范围非线性以及控制目标任务复杂等特点，应用模糊控制等现有的控制方法是无法实现的。对欠驱动倒立摆系统的大范围运动控制，一直是自动控制领域中的难题。

三、发明内容

本发明的目的是针对现有倒立摆系统控制方法的不足之处，提供一种欠驱动双摆系统摆杆杂技动作的控制方法。该方法能解决欠驱动双摆系统大范围运动控制的难题，将倒立摆系统从实验装置推向实际工程应用。

本发明方法是这样实现的：一种欠驱动双摆系统摆杆杂技动作控制方法，利用现有的小车双摆系统(如图2所示)，该系统包括：内摆杆(内杆)、外摆杆(外杆)、小车、角度传感器、交流/直流电机和控制器等。小车双摆系统的外杆与内杆、内杆与小车之间，均通过带滚动轴承的旋转轴活动连接，在旋转轴上分别装设有角度编码器或旋转电位器，以便分别监测内、外杆与垂直方向的转角。在伺服电机的控制程序中，装载有依据本方法编制的控制程序。两杆的杂技动作，根据杂技演员的动作特点，模拟人体的动觉智能，利用摆杆的运动惯性，对由两摆杆的平衡位置和运动姿态组成的成套杂技动作进行分阶段控制。成套杂技动作的总控制器，由各阶段控制目标对应的子控制器，经关联图式连接而成。总控制器被编制成实时运行的程序控制代码，输入到相应的计算机控制系统中，输出的控制信号经伺服电机驱动器功率放大后，实现伺服电机的运动控制，进而驱动小车，带动两摆杆运动，使两摆杆完成各种成套杂技动作，实现了对欠驱动双摆系统大范围运动精确控制的目的。

一种欠驱动双摆系统摆杆杂技动作控制方法，利用欠驱动双摆系统及控制器，通过计算机程序进行控制，实现控制的具体方法步骤如下(如图1所示)：

(1)两杆成套杂技动作控制目标的确定

1)两杆稳定平衡位置组合状态及其控制方法

利用欠驱动双摆系统，依据杂技演员的动作特点，通过近似线形化为基础的控制方法，对两杆各稳定平衡位置组合状态进行控制。其四个基本组合状态及控制方法如下：(如图3、图4所示)

①内杆向下-外杆向下(down-down)的自稳定平衡状态(如图3-1所示)；

这种状态作为双摆系统开始进行杂技动作的初始状态，通过程序依据该初始状态对控制系统进行初始化标定。在编排的成套杂技动作中当以这种状态为动作序列的目标时：在偏差大时，对内、外杆的摆角和小车的位移采用开环的磅磅控制；在偏差小时，对内、外杆的摆角和小车的位移分别采用闭环的正反馈或负反馈的比例微分，并通过它们之间的线性叠加对小车的运动进行控制，使两杆尽快的达到这种状态。

②内杆向下-外杆向上(down-up)的不自稳定平衡状态(如图3-2所示)

在编排的成套杂技动作中，当以这种状态为动作序列的目标时，内杆与内杆垂直向下的夹角、外杆与外杆垂直向上的夹角均在α以内，对内、外杆的摆角和小车的位移分别采用闭环的正反馈或负反馈比例微分，并通过它们之间的线性叠加对小车的运动进行控制，从而完成对内杆垂直向下、外杆垂直向上的平衡状态的控制，其控制过程如图4-1、4-2、4-3所示。

③内杆向上-外杆向上(up-up)的不自稳定平衡状态(如图3-3所示)

在编排的成套杂技动作中，当以这种状态为动作序列的目标时，内、外杆与内、外杆均垂直向上的夹角均在α以内，对内、外杆的摆角和小车的位移分别采用闭环的正反馈或负反馈比例微分，并通过它们之间的线性叠加对小车的运动进行控制，从而完成对内杆垂直向上、外杆也垂直向上的平衡状态的控制，其控制过程如图4-7、4-8、4-9所示。

④内杆向上-外杆向下(up-down)的不自稳定平衡状态(如图3-4所示)

在编排的成套杂技动作中，当以这种状态为动作序列的目标时，内杆与内杆垂直向上的夹角、外杆与外杆垂直向下的夹角均在α以内，对内、外杆的摆角和小车的位移分别采用闭环的正负反馈比例微分，并通过它们之间的线性叠加对小车的运动进行控制，从而完成对内杆垂直向上，外杆垂直向下的平衡状态的控制，其控制过程如图4-10、4-11、4-12所示。

2)两杆动作基本运动姿态及其控制方法

利用欠驱动双摆系统，依据杂技演员的动作特点，模拟人体的动觉智能，通过程序对两杆的各种运动姿态进行控制。其十二个基本运动姿态及控制方法如下：(如图4所示)

①内杆向下-外杆摆起(down-swing-up)

对图4中的第1个运动姿态控制目标，双摆的控制系统通过正反馈比例控制，控制外杆的振荡摆起。同时，双摆的控制系统通过正或负反馈的比例微分控制使内杆保持垂直向下的姿态，使小车的位移保持在初始位置附近。通过上述三个控制作用的线性叠加控制小车的位移，完成该运动姿态的控制。

②内杆向下-外杆顺时针旋转(down-R-rotate)

对图4中的第2个运动姿态控制目标，在内杆向下-外杆向上(down-up)时，小车左移，使外杆从右方自由下跌，启动外杆顺时针旋转过程，当外杆质心低于其旋转轴时，用对外杆摆角的正反馈控制，向外杆注入能量，以补充外杆旋转中因为摩擦造成的能量损失，外杆旋转的次数决定于注入能量的大小。当外杆旋转回到外杆向上时，双摆的控制系统通过正反馈或负反馈的比例微分控制，使内杆保持垂直向下的姿态，并使小车的位移保持在初始位置附近。

③内杆向下-外杆逆时针旋转(down-L-rotate)

对图4中的第3个运动姿态控制目标，当内杆向下-外杆向上(down-up)时，小车右移，使外杆从左方自由下跌，启动外杆逆时针旋转控制，当外杆质心低于其转轴时，用对外杆摆角的正反馈控制，向外杆注入能量，以补充外杆旋转中因为摩擦造成的能量损失，外杆旋转的次数决定于注入能量的大小。当外杆旋转回到外杆向上时，双摆的控制系统通过正反馈或负反馈的比例微分控制，使内杆保持垂直向下的姿态，并使小车的位移保持在初始位置附近。

④内杆向上-外杆摆起(up-swing-up)

对图4中的第4个运动姿态控制目标，双摆的控制系统通过正反馈比例控制，控制外杆的振荡摆起。同时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制使内杆保持垂直向上的姿态，使小车的位移保持在初始位置附近。通过上述三个控制作用的线性叠加控制小车的位移，完成该运动姿态的控制。

⑤内杆向上-外杆顺时针旋转(up-R-rotate)

对图4中的第5个运动姿态控制目标，当内杆向上-外杆向上(up-up)时，小车左移，使外杆从右方自由下跌，启动外杆顺时针旋转控制，当外杆质心低于其旋转轴时，用对外杆摆角的正反馈控制，向外杆注入能量，以补充外杆旋转中因为摩擦造成的能量损失，外杆旋转的次数决定于注入能量的大小。当外杆旋转回到外杆向上时，双摆的控制系统通过正反馈或负反馈的比例微分控制，使内杆保持垂直向下的姿态，并使小车的位移保持在初始位置附近。

⑥内杆向上-外杆逆时针旋转(up-L-rotate)

对图4中的第6个运动姿态控制目标，当内杆向上-外杆向上(up-up)时，小车右移，使外杆从左方自由下跌，启动外杆逆时针旋转控制，当外杆质心低于其旋转轴时，用对外杆摆角的正反馈控制，向外杆注入能量，以补充外杆旋转中因为摩擦造成的能量损失，外杆旋转的次数决定于注入能量的大小。当外杆旋转回到外杆向上时，双摆的控制系统通过正反馈或负反馈的比例微分控制，使内杆保持垂直向下的姿态，并使小车的位移保持在初始位置附近。

⑦内杆向下-外杆向下-两杆同时摆起(down-down-swing-up)

对图4中的第7个运动姿态控制目标，当内杆向下-外杆向下(down-down)时，双摆的控制系统通过正反馈比例控制，控制内杆和外杆同时的振荡摆起。同时，通过闭环的正反馈或负反馈的比例微分，对两杆的夹角和小车的位置进行控制，使两摆杆成近似直线的姿态振荡摆起。这时三个控制作用的线性叠加构成了对小车运动控制。

⑧内杆向上-外杆向上-两杆同时顺时针旋转(up-up-R-rotate)

对图4中的第8个运动姿态控制目标，当内杆向上-外杆向上(up-up)时，小车左移，使两杆同时从右方自由下跌，启动两杆顺时针旋转控制，当两杆质心低于其旋转轴时，用对两杆摆角的正反馈控制，向两杆同时注入能量，以补充两杆旋转中因为摩擦造成的能量损失，两杆同时旋转的次数决定于注入能量的大小。当内杆和外杆旋转同时回到垂直向上时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆、外杆同时保持垂直向上的姿态，并使小车的位移保持在初始位置附近。

⑨内杆向上-外杆向上-两杆同时逆时针旋转(up-up-L-rotate)

对图4中的第9个运动姿态控制目标，当内杆向上-外杆向上(up-up)时，小车右移，使两杆同时从左方自由下跌，启动两杆逆时针旋转控制，当两杆质心低于其旋转轴时，用对两杆摆角的正反馈控制，向两杆同时注入能量，以补充两杆旋转中因为摩擦造成的能量损失，两杆同时旋转的次数决定于注入能量的大小。当内杆和外杆旋转同时回到垂直向上时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆、外杆同时保持垂直向上的姿态，并使小车的位移保持在初始位置附近。

⑩内杆向下-外杆向上-两杆同时摆起(down-up-swing-up)

对图4中的第10个动作姿态控制目标，当内杆向下-外杆向上(down-up)时，首先用较小的小车运动使双摆杆偏离(down-up)位置，然后根据两摆杆的位置与运动姿态特征，通过正反馈的比例控制方法，使内杆振荡摆起，同时，采用用负反馈的比例微分控制方法，保持外杆与内杆呈一定的夹角和相对运动的姿态并同时振荡摆起。小车的运动控制始终由上述正负反馈控制的线性叠加构成。

_内杆向上-外杆向下-两杆同时顺时针旋转(up-down-R-rotate)

对图4中的第11个动作姿态控制目标，当内杆向上-外杆向下(up-down)时，通过负反馈控制内杆和外杆的夹角，同时小车左移，使两杆从右方自由下跌，启动两杆顺时针旋转控制，当两杆质心低于内杆旋转轴时，采用正反馈比例控制向两杆注入能量，补充旋转中因为摩擦造成的能量损失，两杆同时旋转的次数决定于注入能量的大小。当内杆和外杆旋转回到内杆向上-外杆向下时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆保持向上-外杆保持向下的姿态，并使小车的位移保持在初始位置附近。

_内杆向上-外杆向下-两杆同时逆时针旋转(up-down-L-rotate)

对图4中的第12个动作姿态控制目标，当内杆向上-外杆向下(up-down)时，通过负反馈控制内杆和外杆的夹角，同时小车右移，使两杆从左方自由下跌，启动两杆逆时针旋转控制，当两杆质心低于内杆旋转轴时，采用正反馈比例控制向两杆注入能量，补充旋转中因为摩擦造成的能量损失，两杆同时旋转的次数决定于注入能量的大小。当内杆和外杆旋转回到内杆向上-外杆向下时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆保持向上-外杆保持向下的姿态，并使小车的位移保持在初始位置附近。

上述四个基本位置组合状态和十二个基本运动姿态的控制方法，保证了成套杂技动作实现的可行性。同时，用简捷的正负反馈相结合的控制方法，保证了控制的实时性与可靠性。

3)两杆成套杂技动作控制目标的确定及控制任务分解

①两杆成套杂技动作控制目标的确定

利用欠驱动双摆系统，模拟杂技演员的动作，依据成套杂技动作的特点，按照杂技艺术的综合性、创造性和愉悦性的要求，结合第(1)-1)步的平衡位置组合状态和(1)-2)步的十二个基本运动姿态及其相互转换与衔接关系，通过计算机辅助程序，编排各种成套的杂技动作，并确定相应的成套杂技动作控制目标。成套杂技动作控制目标的数量，由相应的成套杂技动作的动作数决定。其中每一个杂技动作表现了从某一种平衡位置组合状态，经历某一种基本运动姿态的运动过程，到达另一种平衡位置组合状态或者回到原来的平衡位置组合状态(具体详见实施实例)。

②两杆成套杂技动作控制目标的控制任务分解

根据第(1)-3)-①步确定的两杆成套杂技动作控制目标，按照各成套杂技动作控制目标的动作顺序，通过程序将各动作分别分解成各阶段的子控制目标，并设置各个阶段的子控制目标相应的子控制器。各个阶段的控制目标通过相应的子控制器的控制予以实现。同时，还要确定按动作顺序，实现各动作之间平滑切换和衔接的控制目标及设置相应的总顺序控制器(即关联图式)，从而完成双摆系统在大范围内的非线性控制和成套杂技动作的自主控制。

(2)成套杂技动作控制目标的控制

1)被控对象双摆系统的建模与参数辨识

①欠驱动双摆系统数学模型结构的建立

用牛顿-欧拉法或分析力学中的拉格朗日方程建立欠驱动双摆系统的数学模型结构。建立的模型结构形式为非线性二阶微分方程：

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=F$ 式中：q＝(x，θ₁，θ₂)^T， $\stackrel{\·}{q}={(\stackrel{\·}{x},{\stackrel{\·}{\mathrm{\θ}}}_1,{\stackrel{\·}{\mathrm{\θ}}}_2)}^T$

②在计算机中构建立欠驱动双摆系统数学模型相应的数值模型

用数值分析方法，依据第(2)-1)-①步建立的数学模型在计算机中建立相应的数值模型，并通过初步物理测量确定数学模型中的动力学参数。在计算机仿真中，用固定步长的四阶龙格库塔法求解微分方程。

③双摆系统等效模型参数的辨识

为使第(2)-1)-②步在计算机中建立的数值仿真模型与实际对象做到基本一致，以便加速完成从计算机仿真到实物控制实验的过渡，采用遗传进化算法对数值仿真模型的中的动力学参数进行整定，使其能与实际对象等效。其步骤是：首先建立反映实际测量的欠驱动双摆系统的输入输出响应和第(2)-1)-②步在计算机中建立的数值模型的输入输出响应差别的遗传进化算法的适应度函数，然后应用遗传进化算法对这些动力学参数进行优化，直至数值仿真模型的输入输出响应与实际对象的输入输出响应做到基本一致。模型中不便于用测量法准确得到的摩擦系数，也在以上遗传进化计算的过程中被确定，从而得到的是能与实际对象基本一致的等效数值模型，因此控制器的有效控制参数的确定、系统工作品质的评价等工作均可由数值仿真实验完成，使得后面的仿真研究对实时控制有很强的指导意义。

2)成套杂技动作总控制器及各杂技动作相应子控制器控制参数的确定与优化

对第(1)-3)步确定出的成套杂技动作控制目标、分解得到的各分阶段控制目标及设置对应的各子控制器以及保证各动作之间平滑切换和衔接的关联图式，保证了整个成套杂技动作控制器结构的合理性和成套杂技动作实现的可行性。但是，其中有大量的切换特征参数和控制参数要确定和优化。本发明通过遗传进化算法实现上述各子控制器和关联图式参数的确定与优化，以快速、准确实现成套杂技动作为目标建立遗传进化算法的适应度函数，然后应用遗传进化算法对这些特征及控制参数进行优化，直至成套杂技动作的精确实现。这种方法能方便地由计算机可靠地得到多组完成各控制任务的可行的控制参数，使仿真模型与实际对象基本等效，从而大大地降低实物控制实验的调试工作量。

3)实物控制实验及实验结果的评估与判断

对第(1)-3)步设置的各分阶段控制目标的各子控制器和总顺序控制器，编制完成相应的分阶段控制目标的，能实时运行的控制程序后，输入到由PC系统组成的控制器或由嵌入式系统组成的控制器或由PC系统和嵌入式系统组成的控制器中，在其控制器中设置有运动控制卡，对系统中伺服电机的旋转进行控制，通过伺服电机的旋转驱动小车的位移，带动与小车活动连接的两个摆杆的位置状态和运动姿态。在控制欠驱动双摆系统实物进行成套杂技动作的运动实验中，实时地记录环境干扰的情况和控制的效果。在实验过程中，随时将仿真实验结果与实物实验结果进行分析比较及评估判断：当还没有完成成套杂技动作的控制目标时，重复进行第(2)步中的各个步骤，对控制器参数或控制程序进行再调整；当完成成套杂技动作的控制目标时，结束实物控制实验。从而完成欠驱动双摆系统成套杂技动作的控制。

本发明采用上述多控制目标及多模态控制的分解控制实现复杂控制目标的控制方法，具有简洁可靠，便于模拟杂技演员的身体动觉智能，并便于实现非线性大范围运动控制和其它复杂系统控制等的突出优点。本发明可广泛应用于欠驱动关节型机械臂、杂技机器人、天空飞行器和智能玩具等装置的控制。

四、附图说明

图1本发明的程序流程框图；

图2现有欠驱动小车双摆系统结构示意图；

图中：1.交流/直流伺服电机，2.基座(小车)，3.传动带，4.滑轨，5.角度编码器，6.内摆杆，7.外摆杆。

图3本发明两摆杆平衡位置组合状态控制目标示意图；

图中：1.内杆向下-外杆向下(down-down)，2.内杆向下-外杆向上(down-up)，3.内杆向上-外杆向上(up-up)，4.内杆向上-外杆向下(up-down)。

图4本发明两摆杆十二个基本运动姿态控制目标示意图；

图中：1.内杆向下-外杆摆起(down-swing-up)，2.内杆向下-外杆顺时针旋转(down-R-rotate)，3.内杆向下-外杆逆时针旋转(down-L-rotate)，4.内杆向上-外杆摆起(up-swing-up)，5.内杆向上-外杆顺时针旋转(up-R-rotate)，6.内杆向上-外杆逆时针旋转(up-L-rotate)，7.内杆向下-外杆向下-两杆同时摆起(down-down-swing-up)，8.内杆向上-外杆向上-两杆同时顺时针旋转(up-up-R-rotate)，9.内杆向上-外杆向上-两杆同时逆时针旋转(up-up-L-rotate)，10.内杆向下-外杆向上-两杆同时摆起(down-up-swing-up)，11.内杆向上-外杆向下-两杆同时顺时针旋转(up-down-R-rotate)，12.内杆向上-外杆向下-两杆同时逆时针旋转(up-down-L-rotate)。

图5本发明PC系统控制器的硬件系统组成框图；

图中：8.PC系统，9.运动控制卡，10.电机伺服控制器，11.交流/直流伺服电机，12.双摆系统，14.PC机。

图6本发明嵌入式系统的控制器硬件系统组成框图；

图中：10.电机伺服控制器，11.交流/直流伺服电机，12.双摆系统，13.嵌入式系统。

图7本发明PC机和嵌入式系统构成的控制器硬件系统组成框图；

图中：10.电机伺服控制器，11.交流/直流伺服电机，12.双摆系统，13.嵌入式系统，14.PC机。

图8为本实施例1的分解成套杂技动作控制目标的程序流程框图；

图9为本实施例1的分解成套杂技动作分阶段控制目标示意图；

图10为本实施例2的分解成套杂技动作控制目标的程序流程框图；

图11为本实施例2的分解成套杂技动作分阶段控制目标示意图。

五、具体实施方式

下面结合具体实施方式，进一步说明本发明。

实施例1

如图8～9所示，一种欠驱动双摆系统摆杆杂技动作控制方法，利用欠驱动双摆系统和控制器，通过程序对已编排出的第一套成套杂技动作的分解及控制的方法步骤如下：

1)对成套杂技动作的编排与分阶段控制目标的分解

对通过计算机已编排出的第一套成套杂技动作控制目标，按其动作顺序(如图8所示)，采用开闭环相结合及正负反馈结合的控制方式通过程序将该套杂技动作控制目标分解为十五个阶段控制目标，如图9所示，并设置十五个分阶段控制目标对应的十五个子控制器，其分解成套杂技动作控制目标的方法步骤如下：

①第一个阶段控制目标，从系统第1个位置状态，即内杆向下-外杆向下(down-down)的自然稳定的位置状态开始，经内杆向下-外杆摆起(down-swing-up)的第1种基本运动姿态到达内杆向下-外杆向上(down-up)的第2个位置状态并稳定；

②第二个阶段控制目标，经内杆向下-外杆顺时针旋转(down-R-rotate)720°(两周)的第2种基本运动姿态，回到内杆向下-外杆向上(down-up)的第2个位置状态并稳定；

③第三个阶段控制目标，经内杆向下-外杆逆时针旋转(down-L-rotate)720°(两周)的第3种基本运动姿态，回到内杆向下-外杆向上(down-up)的第2个位置状态并稳定；

④第四个阶段控制目标，经内杆向下-外杆向上-两杆同时摆起(down-up-swing-up)的第10种基本运动姿态到第内杆向上-外杆向下(up-down)的4个位置状态并稳定；

⑤第五个阶段控制目标，经内杆向上-外杆摆起(up-swing-up)的第4种基本运动姿态到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

⑥第六个阶段控制目标，经内杆向上-外杆顺时针旋转(up-R-rotate)1080°(三周)的第5种基本运动姿态，回到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

⑦第七个阶段控制目标，经内杆向上-外杆逆时针旋转(up-L-rotate)1080°(三周)的第6种基本运动姿态，回到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

⑧第八个阶段控制目标，经内杆向上-外杆顺时针旋转(up-R-rotate)720°(两周)的第5种基本运动姿态，回到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

⑨第九个阶段控制目标，经内杆向上-外杆向上-两杆同时顺时针旋转(up-up-R-rotate)180°(半周)的第8种基本运动姿态，到内杆向下-外杆向下(down-down)的第1个位置状态并稳定；

⑩第十个阶段控制目标，经内杆向下-外杆向下-两杆同时摆起(down-down-swing-up)的第7种基本运动姿态到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

_第十一个阶段控制目标，经内杆向上-外杆向上-两杆同时逆时针旋转(up-up-L-rotate)360°(一周)的第9种基本运动姿态回到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

_第十二个阶段控制目标，经内杆向上-外杆顺时针旋转(up-R-rotate)540°(一周半)的第5种基本运动姿态，到内杆向上-外杆向下(up-down)的第4个位置状态并稳定；

_第十三个阶段控制目标，经内杆向上-外杆向下-两杆同时逆时针旋转(up-down-R-rotate)720°(二周)的第12种基本运动姿态，回到内杆向上-外杆向下(up-down)的第4个位置状态并稳定；

_第十四个阶段控制目标，经内杆向上-外杆向下-两杆同时顺时针旋转(up-down-R-rotate)540°(一周半)的第11种基本运动姿态，到内杆向下-外杆向上(down-up)的第2个位置状态并稳定；

_第十五个阶段控制目标，经内杆向下-外杆顺时针旋转(down-R-rotate)900°(二周半)的第2种基本运动姿态，到内杆向下-外杆向下(down-down)的第1个位置状态并稳定，结束该套动作。

2)成套杂技动作控制目标的控制

①被控对象双摆系统的建模与参数辨识

设状态变量为：q＝[x θ₁ θ₂]^T，(其中：x为小车位移，θ₁为内摆杆角度，θ₂为外摆杆角度)。应用拉格朗日方程得到小车双摆系统的运动模型为：

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=F$

式中： $M\left(q\right)=\left[\begin{array}{ccc}M+m_1+m_2+m_0& (m_1{l}_1+m_{2}L+m_{0}L)\cos \left({\mathrm{\θ}}_1\right)& m_2{l}_2\cos \left({\mathrm{\θ}}_2\right)\\ (m_1{l}_1+m_{2}L+m_{0}L)\cos \left({\mathrm{\θ}}_1\right)& (m_1{l}_1^2+J_1+m_2{L}^2+m_0{L}^2)& m_{2}L{l}_2\cos ({\mathrm{\θ}}_1-{\mathrm{\θ}}_2)\\ m_2{l}_2\cos \left({\mathrm{\θ}}_2\right)& m_{2}L{l}_2\cos ({\mathrm{\θ}}_1-{\mathrm{\θ}}_2)& (m_2{l}_2^2+J_2)\end{array}\right]$

$C(q,\stackrel{\·}{q})=\left[\begin{array}{ccc}f& -(m_1{l}_1+m_{2}L+m_{0}L){\stackrel{\·}{\mathrm{\θ}}}_1\sin \left({\mathrm{\θ}}_1\right)& -m_2{l}_2{\stackrel{\·}{\mathrm{\θ}}}_2\sin \left({\mathrm{\θ}}_2\right)\\ 0& c_1+c_2& m_{2}L{l}_2{\stackrel{\·}{\mathrm{\θ}}}_2\sin ({\mathrm{\θ}}_1-{\mathrm{\θ}}_2)-c_2\\ 0& -m_{2}L{l}_2{\stackrel{\·}{\mathrm{\θ}}}_1\sin ({\mathrm{\θ}}_1-{\mathrm{\θ}}_2)-c_2& c_2\end{array}\right]$

$g\left(q\right)=\left[\begin{array}{c}0\\ -(m_{0}L+m_1{l}_1+m_{2}L)g\left(\sin {\mathrm{\θ}}_1\right)\\ -m_{2}g{l}_2\sin \left({\mathrm{\θ}}_2\right)\end{array}\right]$

F＝[u 0 0]^T

上式中各符号的物理意义及实施中的具体参数见下表：

小车质量(mcar)	1.32kg	内摆杆质量(m1)：	0.108kg
				外摆杆质量(m2)	0.108kg	编码器质量(M1)	0.208kg
内摆杆质心到转轴距离(l1)	0.20m	外摆杆质心到转轴距离(l2)	0.20m
				小车水平位移(x)	(-0.5，0.5)m	内、外摆杆长度L	0.40m
小车-轨道摩擦系数(f0)		22.915
				摆杆转动摩擦系数(f1、f2)		0.7756
内、外摆杆转动惯量(J1、J2)		0.00144kg.m2
				内摆杆摆动角度θ1(rad)		外摆杆摆动角度θ2(rad)
水平施加控制力u(N)

依据上式在计算机中用C语言编程实现快速运算的双摆系统数值模型，用固定步长的四阶龙格库塔法解二阶微分方程，仿真时间间隔与实时控制一致，取5ms。

进一步，让摆杆从上方自由跌落，记录内、外摆杆的运动数据。向伺服电机输出给定的加速度信号，记录下摆杆与小车的运动数据。

利用遗传进化算法随机组合搜索模型参数，使仿真中同样条件的数值模型自由运动与实物运动数据接近，其中最接近的一组模型参数就是模型参数辨识的结果。使仿真研究有了与实物一致的控制对象，因此控制方法的调试先在计算机中仿真进行实现，然后再移植到实物控制系统中去，使控制效率大大提高。

②控制器参数的确定与优化

对第1)步分解得到的十五个分阶段控制目标，设置对应的十五个子控制器，保证了控制器结构的合理性。但其中有大量的控制参数需要确定。依据所分解出的十五个阶段控制目标，并分别编制十五个相应的多模态控制程序，通过对计算机中数值模型的仿真控制效果与预期控制目标之间的比较，利用遗传进化算法确定并优化这十五个控制器的控制参数。

该方法不仅能方便地由计算机方便地得到多组可完成各个控制任务的可行的控制参数，而且可以大大地降低调试工作量。因为仿真计算中允许失败，且执行速度比实物实验快很多，所以这一步骤与传统方法相比，可以节省大量的手工劳动。

3)实物控制实验及实验结果的评估与判断

将第1)步分解得到的十五个阶段控制目标的十五个子控制器和由第2)-②步优化得到的十五个子控制器的参数，置入有PCI接口的PC系统中，由安装在PC系统中的GT-400-SV运动控制卡(深圳固高科技公司产品)输出控制信号，经伺服电机驱动器功率放大后，实现伺服电机的运动控制。同时，运动控制卡也能接收小车位置、内外摆杆摆角等双摆系统的各种状态信息，反馈给控制器。

在此基础上，欠驱动双摆系统摆杆第一套杂技动作的控制方法，按图8所示的程序流程，编制成实时运行的控制程序，并置入到PC系统中，完成对双摆杆实物系统第一套成套杂技动作的运动控制。在实物控制实验中，加入环境干扰实时地记录有关的控制效果，对控制结果评估与判断：当未完成成套动作的控制目标时，重复进行第2)步，对控制器参数或控制程序进行再调整；当完成成套杂技动作的控制目标时，结束调整，从而完成欠驱动双摆实物系统第一套杂技动作的仿人运动控制。

实施例2：

如图10～11所示，一种欠驱动双摆系统摆杆杂技动作控制方法，利用欠驱动双摆系统和控制器，通过程序对已编排出的第二套成套杂技动作的分解及控制的方法步骤如下：

1)对成套杂技动作控制目标的分解

对通过计算机编排出的第二套套成套杂技动作控制目标，按其动作顺序(如图10所示)，采用开闭环相结合及正负反馈结合的控制方式通过程序将该套杂技动作控制目标分解为十五个阶段控制目标，如图11所示，并设置十五个分阶段控制目标对应的十五个子控制器，其分解成套杂技动作控制目标的方法步骤如下：

①第一个阶段控制目标，从系统第1个位置状态，即内杆向下-外杆向下(down-down)的自然稳定的位置状态开始，经内杆向下-外杆向下-两杆同时摆起(down-down-swing-up)的第7种基本运动姿态到达内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

②第二个阶段控制目标，经内杆向上-外杆向上-两杆同时顺时针旋转(up-up-R-rotate)720°(两周)的第8种基本运动姿态，回到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

③第三个阶段控制目标，经内杆向上-外杆向上-两杆同时逆时针旋转(up-up-L-rotate)1080°(三周)的第9种基本运动姿态，回到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

④第四个阶段控制目标，经内杆向上-外杆顺时针旋转(up-R-rotate)720°(两周)的第5种基本运动姿态，回到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

⑤第五个阶段控制目标，经内杆向上-外杆逆时针旋转(up-L-rotate)900°(两周半)的第6种基本运动姿态，到内杆向上-外杆向下(up-down)的第4个位置状态并稳定；

⑥第六个阶段控制目标，经内杆向上-外杆向下-两杆同时顺时针旋转(up-down-R-rotate)1080°(三周)的第11种基本运动姿态，回到内杆向上-外杆向下(up-down)的第4个位置状态并稳定；

⑦第七个阶段控制目标，经内杆向上-外杆向下-两杆同时逆时针旋转(up-down-L-rotate)1080°(三周)的第12种基本运动姿态，回到内杆向上-外杆向下(up-down)的第4个位置状态并稳定；

⑧第八个阶段控制目标，经内杆向上-外杆向下-两杆同时顺时针旋转(up-down-R-rotate)900°(两周半)的第11种基本运动姿态，到内杆向下-外杆向上(down-up)的第2个位置状态并稳定；

⑨第九个阶段控制目标，经内杆向下-外杆顺时针旋转(down-R-rotate)720°(两周)的第2种基本运动姿态，回到内杆向下-外杆向上(down-up)的第2个位置状态并稳定；

⑩第十个阶段控制目标，经内杆向下-外杆逆时针旋转(down-L-rotate)1260°(三周半)的第3种基本运动姿态，到内杆向下-外杆向下(down-down)的第1个位置状态并稳定；

_第十一个阶段控制目标，经内杆向下-外杆摆起(down-swing-up)的第1种基本运动姿态，到内杆向下-外杆向上(down-up)的第2个位置状态并稳定；

_第十二个阶段控制目标，经内杆向下-外杆向上-两杆同时摆起(down-up-swing-up)的第10种基本运动姿态，到内杆向上-外杆向下(up-down)的第4个位置状态并稳定；

_第十三个阶段控制目标，经内杆向上-外杆摆起(up-swing-up)的第4种基本运动姿态，到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

_第十四个阶段控制目标，经内杆向上-外杆向上-两杆同时顺时针旋转(up-up-R-rotate)1080°(三周)的第8种基本运动姿态，回到内杆向上-外杆向上(up-up)的第3个位置状态并稳定；

_第十五个阶段控制目标，经内杆向上-外杆向上-两杆同时逆时针旋转(up-up-L-rotate)1260°(三周半)的第9种基本动作，回到内杆向下-外杆向下(down-down)的第1个位置状态并稳定，结束该套动作。

2)成套杂技动作控制目标的控制

①被控对象双摆系统的建模与参数辨识

机理建模、数值仿真模型的建立与模型参数辨识，与实施例1的2)-①步相同。

②控制器参数的确定与优化

控制器参数的确定与优化方法，与实施例1的第2)-②步相同，但因为控制目标不同，遗传进化算法中的适应度函数和所求得的最佳控制参数不同。

3)实物控制实验及实验结果的评估与判断

将第1)步分解得到的十五个阶段控制目标的各子控制器和由第2)-②步优化得到的十五个子控制器的参数，置入PC系统和嵌入式系统的控制器硬件系统(如图7)，在嵌入式系统中设置了底层的运动控制算法模块及与PC机通信的程序模块，嵌入式系统输出的控制信号，经伺服电机驱动器功率放大后，实现伺服电机的运动控制，同时，嵌入式系统也接收小车位置、内外摆杆摆角等双摆系统的状态信息，反馈给控制器。

在此基础上，欠驱动双摆系统第二套杂技动作控制方法，按图10所示的程序流程，编制成实时运行的控制程序，并置入到嵌入式系统中，完成对双摆杆的杂技动作控制，PC系统与嵌入式系统通信，实现对双摆系统运行状态监视和设置控制任务等上层管理功能；在实物控制实验中，加入环境干扰实时地记录有关的控制效果，对控制结果评估与判断：当未完成成套动作的控制目标时，重复进行第2)步，对控制器参数或控制程序进行再调整；当完成成套杂技动作的控制目标时，结束调整，从而完成欠驱动双摆实物系统第二套杂技动作的仿人运动控制。

Claims (3)

1.一种欠驱动双摆系统摆杆杂技动作控制方法，利用欠驱动双摆系统及控制器，通过计算机程序进行控制，其特征在于具体的方法步骤如下：

(1)两杆成套杂技动作控制目标的确定

1)两杆稳定平衡位置组合状态及控制方法

利用欠驱动双摆系统，依据杂技演员的动作特点，通过近似线形化为基础的控制方法，对两杆各稳定平衡位置组合状态进行控制，其四个基本稳定平衡位置组合状态及控制方法如下：

①内杆向下-外杆向下的自稳定平衡状态

这种状态作为双摆系统开始进行杂技动作的初始状态，通过程序根据该初始状态对控制系统进行初始化标定；在编排的成套杂技动作中，当以这种状态为动作序列的目标时：在偏差大时，对内、外杆的摆角和小车的位移采用开环的磅磅控制；在偏差小时，对内、外杆的摆角和小车的位移分别采用闭环的正反馈或负反馈的比例微分，并通过它们之间的线性叠加对小车的运动进行控制；

②内杆向下-外杆向上的不自稳定平衡状态

在编排的成套杂技动作中，当以这种状态为动作序列的目标时，内杆与内杆垂直向下的夹角、外杆与外杆垂直向上的夹角均在α以内，对内、外杆的摆角和小车的位移分别采用闭环的正反馈或负反馈比例微分，并通过它们之间的线性叠加对小车的运动进行控制，从而完成对内杆垂直向下、外杆垂直向上的平衡状态的控制；

③内杆向上-外杆向上的不自稳定平衡状态

在编排的成套杂技动作中，当以这种状态为动作序列的目标时，内、外杆与内、外杆均垂直向上的夹角均在α以内，对内、外杆的摆角和小车的位移分别采用闭环的正反馈或负反馈比例微分，并通过它们之间的线性叠加对小车的运动进行控制，从而完成对内杆垂直向上、外杆也垂直向上的平衡状态的控制；

④内杆向上-外杆向下的不自稳定平衡状态

在编排的成套杂技动作中，当以这种状态为动作序列的目标时，内杆与内杆垂直向上的夹角、外杆与外杆垂直向下的夹角均在α以内，对内、外杆的摆角和小车的位移分别采用闭环的正负反馈比例微分，并通过它们之间的线性叠加对小车的运动进行控制，从而完成对内杆垂直向上，外杆垂直向下的平衡状态的控制；

2)两杆动作基本运动姿态及其控制方法

利用欠驱动双摆系统，依据杂技演员的动作特点，模拟人体的动觉智能，通过程序对两杆的各种运动姿态进行控制，其十二个基本运动姿态及控制方法如下：

①内杆向下-外杆摆起

对图4中的第1个运动姿态控制目标，双摆的控制系统通过正反馈比例控制，控制外杆的振荡摆起，同时，双摆的控制系统通过闭环的正反馈或负反馈比例微分控制使内杆保持垂直向下的姿态，使小车的位移保持在初始位置附近，通过上述三个控制作用的线性叠加，控制小车的位移，完成该运动姿态的控制；

②内杆向下-外杆顺时针旋转

对图4中的第2个运动姿态控制目标，在内杆向下-外杆向上时，小车左移，使外杆从右方自由下跌，启动外杆顺时针旋转控制，当外杆质心低于其旋转轴时，通过对外杆摆角的正反馈控制，向外杆注入能量，以补充外杆旋转中因摩擦造成的能量损失，外杆旋转的次数决定于注入能量的大小；当外杆旋转回到外杆向上时，双摆的控制系统通过正反馈或负反馈的比例微分控制，使内杆保持垂直向下的姿态，并使小车的位移保持在初始位置附近；

③内杆向下-外杆逆时针旋转

对图4中的第3个运动姿态控制目标，当内杆向下-外杆向上时，小车右移，使外杆从左方自由下跌，启动外杆逆时针旋转控制，当外杆质心低于其旋转轴时，通过对外杆摆角的正反馈控制，向外杆注入能量，以补充外杆旋转中因摩擦造成的能量损失，外杆旋转的次数决定于注入能量的大小，当外杆旋转回到外杆向上时，双摆的控制系统通过正反馈或负反馈的比例微分控制，使内杆保持垂直向下的姿态，并使小车的位移保持在初始位置附近；

④内杆向上-外杆摆起

对图4中的第4个运动姿态控制目标，双摆的控制系统通过正反馈比例控制，控制外杆的振荡摆起，同时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆保持垂直向上的姿态，使小车的位移保持在初始位置附近，通过上述三个控制作用的线性叠加，控制小车的位移，完成该运动姿态的控制；

⑤内杆向上-外杆顺时针旋转

对图4中的第5个运动姿态控制目标，当内杆向上-外杆向上时，小车左移，使外杆从右方自由下跌，启动外杆顺时针旋转控制，当外杆质心低于其旋转轴时，通过对外杆摆角的正反馈控制，向外杆注入能量，以补充外杆旋转中因摩擦造成的能量损失，外杆旋转的次数决定于注入能量的大小，当外杆旋转回到外杆向上时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆保持垂直向上的姿态，并使小车的位移保持在初始位置附近；

⑥内杆向上-外杆逆时针旋转

对图4中的第6个运动姿态控制目标，当内杆向上-外杆向上时，小车右移，使外杆从左方自由下跌，启动外杆逆时针旋转控制，当外杆质心低于其旋转轴时，通过对外杆摆角的正反馈控制，向外杆注入能量，以补充外杆旋转中因摩擦造成的能量损失，外杆旋转的次数决定于注入能量的大小，当外杆旋转回到外杆向上时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆保持垂直向上的姿态，并使小车的位移保持在初始位置附近；

⑦内杆向下-外杆向下-两杆同时摆起

对图4中的第7个运动姿态控制目标，当内杆向下-外杆向下时，双摆的控制系统通过正反馈比例控制，控制内杆和外杆同时的振荡摆起，同时，通过闭环的正反馈或负反馈的比例微分，对两杆的夹角和小车的位置进行控制，使两摆杆成近似直线的姿态振荡摆起，这时三个控制作用的线性叠加构成了对小车的运动控制；

⑧内杆向上-外杆向上-两杆同时顺时针旋转

对图4中的第8个运动姿态控制目标，当内杆向上-外杆向上时，小车左移，使两杆同时从右方自由下跌，启动两杆同时顺时针旋转控制，当两杆质心低于其旋转轴时，通过对两杆摆角的正反馈控制，向两杆同时注入能量，以补充两杆旋转中因摩擦造成的能量损失，两杆同时旋转的次数决定于注入能量的大小，当内杆和外杆旋转同时回到垂直向上时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆、外杆同时保持垂直向上的姿态，并使小车的位移保持在初始位置附近；

⑨内杆向上-外杆向上-两杆同时逆时针旋转

对图4中的第9个运动姿态控制目标，当内杆向上-外杆向上时，小车右移，使两杆同时从左方自由下跌，启动两杆同时逆时针旋转控制，当两杆质心低于其旋转轴时，通过对两杆摆角的正反馈控制，向两杆同时注入能量，以补充两杆旋转中因摩擦造成的能量损失，两杆同时旋转的次数决定于注入能量的大小，当内杆和外杆旋转同时回到垂直向上时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆、外杆同时保持垂直向上的姿态，并使小车的位移保持在初始位置附近；

⑩内杆向下-外杆向上-两杆同时摆起

对图4中的第10个动作姿态控制目标，当内杆向下-外杆向上时，首先用较小的小车运动使双摆杆偏离位置，然后根据两摆杆的位置与运动姿态特征，通过正反馈的比例控制，使内杆振荡摆起，同时，通过闭环的负反馈的比例微分控制，保持外杆与内杆呈一定的夹角和相对运动的姿态，并同时振荡摆起，小车的运动控制始终由上述正负反馈控制的线性叠加构成；

_内杆向上-外杆向下-两杆同时顺时针旋转

对图4中的第11个动作姿态控制目标，当内杆向上-外杆向下时，通过负反馈控制内杆和外杆的夹角，同时小车左移，使两杆从右方自由下跌，启动两杆顺时针旋转控制，当两杆质心低于内杆旋转轴时，通过正反馈比例控制向两杆注入能量，补充旋转中因摩擦造成的能量损失，两杆同时旋转的次数决定于注入能量的大小，当内杆和外杆旋转回到内杆向上-外杆向下时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆保持向上-外杆保持向下的姿态，并使小车的位移保持在初始位置附近；

_内杆向上-外杆向下-两杆同时逆时针旋转

对图4中的第12个动作姿态控制目标，当内杆向上-外杆向下时，通过负反馈控制内杆和外杆的夹角，同时小车右移，使两杆从左方自由下跌，启动两杆逆时针旋转控制，当两杆质心低于内杆旋转轴时，通过正反馈比例控制向两杆注入能量，补充旋转中因摩擦造成的能量损失，两杆同时旋转的次数决定于注入能量的大小，当内杆和外杆旋转回到内杆向上-外杆向下时，双摆的控制系统通过闭环的正反馈或负反馈的比例微分控制，使内杆保持向上-外杆保持向下的姿态，并使小车的位移保持在初始位置附近；

3)两杆成套杂技动作控制目标的确定及控制任务分解

①两杆成套杂技动作控制目标的确定

利用欠驱动双摆系统，模拟杂技演员的动作，依据成套杂技动作的特点，按照杂技艺术的综合性、创造性和愉悦性的要求，结合第(1)-1)步的平衡位置组合状态和(1)-2)步的十二个基本运动姿态及其相互转换与衔接关系，通过计算机辅助程序，编排各种成套的杂技动作，并确定相应的成套杂技动作控制目标，成套杂技动作控制目标的数量，由相应的成套杂技动作的动作数决定，其中每一个杂技动作表现了从某一种平衡位置组合状态，经历某一种基本运动姿态的运动过程，到达另一种平衡位置组合状态或者回到原来的平衡位置组合状态；

②两杆成套杂技动作控制目标的控制任务分解

根据第(1)-3)-①步确定的两杆成套杂技动作控制目标，按照各成套杂技动作控制目标的动作顺序，通过程序将各动作分别分解成各阶段的子控制目标，并设置各个阶段子控制目标相应的子控制器，各个阶段的子控制目标通过相应的子控制器的控制予以实现，还要确定按动作顺序，实现各动作之间平滑切换和衔接的控制目标及设置相应的关联图式，从而实现双摆系统在大范围内的非线性控制和成套杂技动作的自主控制；

(2)成套杂技动作控制目标的控制

1)被控对象双摆系统的建模与参数辨识

①欠驱动双摆系统数学模型结构的建立

用牛顿-欧拉法或分析力学中的拉格朗日方程建立欠驱动双摆系统的数学模型结构，建立的模型结构形式为非线性二阶微分方程：

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=F$ 式中：q＝(x，θ₁，θ₂)^T， $\stackrel{\·}{q}={(\stackrel{\·}{x},{\stackrel{\·}{\mathrm{\θ}}}_1,{\stackrel{\·}{\mathrm{\θ}}}_2)}^T$

②在计算机中构建欠驱动双摆系统数学模型相应的数值模型

用数值分析方法，依据第(2)-1)-①步建立的数学模型，在计算机中建立相应的数值模型，并通过初步物理测量确定数学模型中的动力学参数，在计算机仿真中，用固定步长的四阶龙格库塔法求解微分方程；

③双摆系统等效模型参数的辨识

首先依据反映实际测量的欠驱动双摆系统的输入输出响应和第(2)-1)-②步在计算机中建立的数值模型的输入输出响应之间的差别，建立相应遗传进化算法的适应度函数，然后应用遗传进化算法对模型中的动力学参数进行整定，直至数值仿真模型的输入输出响应与实际对象的输入输出响应做到基本一致，模型中不便于用测量法准确得到的摩擦系数，也在以上遗传进化计算的过程中被确定，从而得到的是能与实际对象基本一致的等效数值模型，因此控制器的有效控制参数的确定、系统工作品质的评价等工作均可由数值仿真实验完成；

2)成套杂技动作各子控制器和关联图式参数的确定与优化

对第(1)-3)步确定出的成套杂技动作控制目标、分解得到的各分阶段控制目标及设置对应的各子控制器和保证各动作之间平滑切换和衔接的关联图式，通过遗传进化算法实现上述各子控制器和关联图式参数的确定与优化；

3)实物控制实验及实验结果的评估与判断

对第(1)-3)步设置的各子控制器和关联图式，编制能实时运行的控制程序之后，输入到由PC系统组成的控制器或由嵌入式系统组成的控制器或由PC系统和嵌入式系统组成的控制器中，在其控制器中设置有运动控制卡，对系统中伺服电机的旋转进行控制，通过伺服电机的旋转驱动小车的位移，控制与小车活动连接的两个摆杆的位置状态和运动姿态，在控制欠驱动双摆系统实物进行成套杂技动作的实验中，实时地记录环境干扰的情况和控制的效果，在实验过程中，随时将仿真实验结果与实物实验结果进行分析比较及评估判断：当没有完成成套杂技动作的控制目标时，重复进行第(2)步中的各个步骤，对控制器参数或控制程序进行再调整；当完成成套杂技动作的控制目标时，结束实物控制实验，从而完成欠驱动双摆系统成套杂技动作的控制。

2.按照权利要求1所述的欠驱动双摆系统摆杆杂技动作控制方法，其特征在于一种欠驱动双摆系统摆杆杂技动作控制方法，利用欠驱动双摆系统和控制器，通过程序对已编排出的第一套成套杂技动作的分解及控制的方法步骤如下：

1)对成套杂技动作控制目标的分解

对通过计算机已编排出的第一套成套杂技动作控制目标，按其动作顺序，采用开闭环相结合及正负反馈结合的控制方式，通过程序将该套杂技动作控制目标分解为十五个阶段控制目标，并设置十五个阶段控制目标对应的十五个子控制器，其分解及控制方法步骤如下：

①第一个阶段控制目标，从双摆系统第1个位置状态，即内杆向下-外杆向下的自然稳定的位置状态开始，经内杆向下-外杆摆起的第1种基本运动姿态，到达内杆向下-外杆向上的第2个位置状态并稳定；

②第二个阶段控制目标，经内杆向下-外杆顺时针旋转720°的第2种基本运动姿态，回到内杆向下-外杆向上的第2个位置状态并稳定；

③第三个阶段控制目标，经内杆向下-外杆逆时针旋转720°的第3种基本运动姿态，回到内杆向下-外杆向上的第2个位置状态并稳定；

④第四个阶段控制目标，经内杆向下-外杆向上-两杆同时摆起的第10种基本运动姿态到第内杆向上-外杆向下的4个位置状态并稳定；

⑤第五个阶段控制目标，经内杆向上-外杆摆起的第4种基本运动姿态到内杆向上-外杆向上的第3个位置状态并稳定；

⑥第六个阶段控制目标，经内杆向上-外杆顺时针旋转1080°的第5种基本运动姿态，回到内杆向上-外杆向上的第3个位置状态并稳定；

⑦第七个阶段控制目标，经内杆向上-外杆逆时针旋转1080°的第6种基本运动姿态，回到内杆向上-外杆向上的第3个位置状态并稳定；

⑧第八个阶段控制目标，经内杆向上-外杆顺时针旋转720°的第5种基本运动姿态，回到内杆向上-外杆向上的第3个位置状态并稳定；

⑨第九个阶段控制目标，经内杆向上-外杆向上-两杆同时顺时针旋转180°的第8种基本运动姿态，到内杆向下-外杆向下的第1个位置状态并稳定；

⑩第十个阶段控制目标，经内杆向下-外杆向下-两杆同时摆起的第7种基本运动姿态到内杆向上-外杆向上的第3个位置状态并稳定；

_第十一个阶段控制目标，经内杆向上-外杆向上-两杆同时逆时针旋转360°的第9种基本运动姿态回到内杆向上-外杆向上的第3个位置状态并稳定；

_第十二个阶段控制目标，经内杆向上-外杆顺时针旋转40°的第5种基本运动姿态，到内杆向上-外杆向下的第4个位置状态并稳定；

_第十三个阶段控制目标，经内杆向上-外杆向下-两杆同时逆时针旋转20°的第12种基本运动姿态，回到内杆向上-外杆向下的第4个位置状态并稳定；

_第十四个阶段控制目标，经内杆向上-外杆向下-两杆同时顺时针旋转540°的第11种基本运动姿态，到内杆向下-外杆向上的第2个位置状态并稳定；

_第十五个阶段控制目标，经内杆向下-外杆顺时针旋转900°的第2种基本运动姿态，到内杆向下-外杆向下的第1个位置状态并稳定，结束该套动作；

2)成套杂技动作控制目标的控制

①被控对象双摆系统的建模与参数辨识

设状态变量为：q＝[x θ₁ θ₂]^T，式中：x为小车位移，θ₁为内摆杆角度，θ₂为外摆杆角度，应用拉格朗日方程得小车双摆系统的运动模型为：

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=F$

式中： $M\left(q\right)=\left[\begin{array}{ccc}M+m_1+m_2+m_0& (m_1{l}_1+m_{2}L+m_{0}L)\cos \left({\mathrm{\θ}}_1\right)& m_2{l}_2\cos \left({\mathrm{\θ}}_2\right)\\ (m_1{l}_1+m_{2}L+m_{0}L)\cos \left({\mathrm{\θ}}_1\right)& (m_1{l}_1^2+J_1+m_2{L}^2+m_0{L}^2)& m_2{\mathrm{Ll}}_2\cos ({\mathrm{\θ}}_1-{\mathrm{\θ}}_2)\\ m_2{l}_2\cos \left({\mathrm{\θ}}_2\right)& m_2{\mathrm{Ll}}_2\cos ({\mathrm{\θ}}_1-{\mathrm{\θ}}_2)& (m_2{l}_2^2+J_2)\end{array}\right]$

$C(q,\stackrel{\·}{q})=\left[\begin{array}{ccc}f& -(m_1{l}_1+m_{2}L+m_{0}L){\stackrel{\·}{\mathrm{\θ}}}_1\sin \left({\mathrm{\θ}}_1\right)& -m_2{l}_2{\stackrel{\·}{\mathrm{\θ}}}_2\sin \left({\mathrm{\θ}}_2\right)\\ 0& c_1+c_2& m_2{\mathrm{Ll}}_2{\stackrel{\·}{\mathrm{\θ}}}_2\sin ({\mathrm{\θ}}_1-{\mathrm{\θ}}_2)-c_2\\ 0& -m_2{\mathrm{Ll}}_2{\stackrel{\·}{\mathrm{\θ}}}_1\sin ({\mathrm{\θ}}_1-{\mathrm{\θ}}_2)-c_2& c_2\end{array}\right]$

$g\left(q\right)=\left[\begin{array}{c}0\\ -(m_{0}L+m_1{l}_1+m_{2}L)g\left(\sin {\mathrm{\θ}}_1\right)\\ -m_2{\mathrm{gl}}_2\sin \left({\mathrm{\θ}}_2\right)\end{array}\right]$

F＝[u 0 0]^T

上式中各符号的物理意义及实施中的具体参数见下表：   小车质量m_car   1.32kg   内摆杆质量m₁   0.108kg   外摆杆质量m₂   0.108kg   编码器质量M₁   0.208kg   内摆杆质心到转轴距离l₁   0.20m   外摆杆质心到转轴距离l₂   0.20m   小车水平位移x   (-0.5，0.5)m   内、外摆杆长度L   0.40m   小车-轨道摩擦系数f₀   22.915   摆杆转动摩擦系数f₁、f₂   0.7756   内、外摆杆转动惯量J₁、J₂   0.00144kg.m²   内摆杆摆动角度θ₁rad   外摆杆摆动角度θ₂rad   水平施加控制力uN

依据上式，在计算机中用C语言编程实现快速运算的双摆系统数值模型，用固定步长的四阶龙格库塔法解上述二阶微分方程，仿真时间间隔与实时控制一致，取5ms；

进一步，让摆杆从上方自由跌落，记录内、外摆杆的运动数据，向伺服电机输出给定的加速度信号，记录下摆杆与小车的运动数据；

利用遗传进化算法随机组合搜索模型参数，使仿真中同样条件的数值模型自由运动与实物自由运动的数据接近，其中最接近的一组模型参数就是模型参数辨识的结果；

②控制器参数的确定与优化

对第1)步分解得到的十五个阶段控制目标，设置对应的十五个子控制器，并分别编制十五个相应的多模态控制程序，通过对计算机中数值模型的仿真控制效果与预期控制目标之间的比较，利用遗传进化算法确定并优化这十五个控制器的控制参数；

3)实物控制实验及实验结果的评估与判断

将第1)步分解得到的十五个阶段控制目标的十五个子控制器和由第2)-②步优化得到的十五个子控制器的参数，置入有PCI接口的PC系统中，由安装在PC系统中的GT-400-SV运动控制卡输出控制信号，经伺服电机驱动器功率放大后，实现伺服电机的运动控制，同时，运动控制卡也能接收小车位置、内外摆杆摆角等双摆系统的各种状态信息，反馈给控制器；

在此基础上，欠驱动双摆系统第一套杂技动作的控制方法，按图8所示的程序流程，编制成实时运行的控制程序，并置入到PC系统中，完成对欠驱动双摆实物系统第一套成套杂技动作的运动控制；在实物控制实验中，加入环境干扰实时地记录有关的控制效果，对控制结果评估与判断：当未完成成套动作的控制目标时，重复进行第2)步，对控制器参数或控制程序进行再调整；当完成成套杂技动作的控制目标时，结束调整，从而完成欠驱动双摆实物系统第一套杂技动作的仿人运动控制。

3.按照权利要求1所述的欠驱动双摆系统摆杆杂技动作控制方法，其特征在于一种欠驱动双摆系统摆杆杂技动作控制方法，利用欠驱动双摆系统和控制器，通过程序对已编排出的第二套成套杂技动作的分解及控制的方法步骤如下：

1)对成套杂技动作控制目标的分解

对通过计算机已编排出的第二套成套杂技动作控制目标，按其动作顺序，采用开闭环相结合及正负反馈结合的控制方式，通过程序将该套杂技动作控制目标分解为十五个阶段控制目标，并设置十五个阶段控制目标对应的十五个子控制器，其分解及控制方法步骤如下：

①第一个阶段控制目标，从系统第1个位置状态，即内杆向下-外杆向下的自然稳定的位置状态开始，经内杆向下-外杆向下-两杆同时摆起的第7种基本运动姿态到达内杆向上-外杆向上的第3个位置状态并稳定；

②第二个阶段控制目标，经内杆向上-外杆向上-两杆同时顺时针旋转720°的第8种基本运动姿态，回到内杆向上-外杆向上的第3个位置状态并稳定；

③第三个阶段控制目标，经内杆向上-外杆向上-两杆同时逆时针旋转1080°的第9种基本运动姿态，回到内杆向上-外杆向上的第3个位置状态并稳定；

④第四个阶段控制目标，经内杆向上-外杆顺时针旋转720°的第5种基本运动姿态，回到内杆向上-外杆向上的第3个位置状态并稳定；

⑤第五个阶段控制目标，经内杆向上-外杆逆时针旋转900°的第6种基本运动姿态，到内杆向上-外杆向下的第4个位置状态并稳定；

⑥第六个阶段控制目标，经内杆向上-外杆向下-两杆同时顺时针旋转1080°的第11种基本运动姿态，回到内杆向上-外杆向下的第4个位置状态并稳定；

⑦第七个阶段控制目标，经内杆向上-外杆向下-两杆同时逆时针旋转1080°的第12种基本运动姿态，回到内杆向上-外杆向下的第4个位置状态并稳定；

⑧第八个阶段控制目标，经内杆向上-外杆向下-两杆同时顺时针旋转900°的第11种基本运动姿态，到内杆向下-外杆向上的第2个位置状态并稳定；

⑨第九个阶段控制目标，经内杆向下-外杆顺时针旋转720°的第2种基本运动姿态，回到内杆向下-外杆向上的第2个位置状态并稳定；

⑩第十个阶段控制目标，经内杆向下-外杆逆时针旋转1260°的第3种基本运动姿态，到内杆向下-外杆向下的第1个位置状态并稳定；

_第十一个阶段控制目标，经内杆向下-外杆摆起的第1种基本运动姿态，到内杆向下-外杆向上的第2个位置状态并稳定；

_第十二个阶段控制目标，经内杆向下-外杆向上-两杆同时摆起的第10种基本运动姿态，到内杆向上-外杆向下的第4个位置状态并稳定；

_第十三个阶段控制目标，经内杆向上-外杆摆起的第4种基本运动姿态，到内杆向上-外杆向上的第3个位置状态并稳定；

_第十四个阶段控制目标，经内杆向上-外杆向上-两杆同时顺时针旋转1080°的第8种基本运动姿态，回到内杆向上-外杆向上的第3个位置状态并稳定；

_第十五个阶段控制目标，经内杆向上-外杆向上-两杆同时逆时针旋转1260°的第9种基本动作，回到内杆向下-外杆向下的第1个位置状态并稳定，结束该套动作；

2)成套杂技动作控制目标的控制

①被控对象双摆系统的建模与参数辨识

机理建模、数值仿真模型的建立与模型参数辨识，与权利要求2的2)-①步相同；

②控制器参数的确定与优化

控制器参数的确定与优化方法，与权利要求2的第2)-②步相同，但因为控制目标同，遗传进化算法中的适应度函数和所求得的最佳控制参数不同；

3)实物控制实验及实验结果的评估与判断

将第1)步分解得到的十五个阶段控制目标的十五子控制器和由第2)-②步优化得到的十五个子控制器的参数，置入PC系统和嵌入式系统组成的控制器中，在嵌入式系统中设置了底层的运动控制算法模块及与PC系统通信的程序模块，嵌入式系统输出的控制信号，经伺服电机驱动器功率放大后，实现伺服电机的运动控制，同时，嵌入式系统也接收小车位置、内外摆杆摆角等双摆系统的状态信息，反馈给控制器；

在此基础上，欠驱动双摆系统第二套杂技动作控制方法，按图10所示的程序流程，编制成实时运行的控制程序，并置入到嵌入式系统中，完成对双摆杆的杂技动作控制，PC系统与嵌入式系统通信，实现对双摆系统运行状态监视和设置控制任务等上层管理功能；在实物控制实验中，加入环境干扰实时地记录有关的控制效果，对控制结果评估与判断：当未完成成套动作的控制目标时，重复进行第2)步，对控制器参数或控制程序进行再调整；当完成成套杂技动作的控制目标时，结束调整，从而完成欠驱动双摆实物系统第二套杂技动作的仿人运动控制。

Images