CN103231379A - 一种2r欠驱动平面柔性机械臂的位置控制方法 - Google Patents

Abstract

一种2R欠驱动平面柔性机械臂的位置控制方法。本发明包括：采用假设模态方法，建立综合考虑关节被动、杆件柔性和关节摩擦力的2R欠驱动柔性机械臂的动力学模型。针对系统中同时存在的柔性和欠驱动特性，基于时间尺度和PID控制方法，设计分段控制策略，实现对主、被动关节的位置控制，进而完成整个机器人的末端操作。本发明所涉及的欠驱动柔性机械臂相对于传统的全驱动刚性系统具有重量轻、能耗低和结构紧凑的优点，通过分段控制设计，可以采用简单的方法实现此类机器人复杂的操作控制，避免了非线性理论和解决方法，达到方法简单、易于实现的目的。

Description

一种2R欠驱动平面柔性机械臂的位置控制方法

技术领域

本发明涉及2R欠驱动平面柔性机械臂的控制方法。

本控制方法主要涉及欠驱动柔性机械臂的位置控制方法，特别是对同时具有自由关节和柔性杆件的这类2R欠驱动平面系统，采用简单易行的控制方法可以实现两个关节同时定位。若通过合理分段并设计相应的控制器，则可实现欠驱动平面柔性机械臂末端复杂的操作任务。

背景技术

欠驱动平面柔性机器人是综合考虑关节自由运动和杆件结构优化两方面特点而产生的一个新兴的机器人研究领域，这类系统为现代机器人在轻质、低耗、高速和高精度等方面的发展提供了新的思路，在太空、深海及核工业环境等领域有重要的现实意义。

一.轻质、低耗和高速现代机器人发展的需要

为了满足现代机器人在高速、高精度、重载和轻量化等方面的要求，人们一般从两个途径加以解决，一方面，可以从减少驱动装置上着手，即设计具有自由关节的欠驱动系统；另一方面，可以从机器人的机构设计角度出发，尽量减少构件所用材料，采用以柔性构件为主要结构的柔性机械臂，因而，将欠驱动和柔性两种特性相结合的欠驱动柔性机器人运用而生。但是，目前的研究大都集中在欠驱动刚性机器人和全驱动柔性机器人各自领域，有关欠驱动柔性机器人这一新领域的研究还不多。如De luca应用路径规划算法将系统状态控制在原点附近。Imura通过坐标和输入反馈变换将系统转换为二阶链式形式，并提出了一种分段光滑镇定控制器。Huang和Wang等利用有限元方法分析了多柔性杆、多柔性关节的柔性机器人的动力学响应特性，验证了杆柔性、关节柔性和重力对动力学特性的影响。本发明研究具有柔性杆件和自由关节两方面特性的欠驱动平面柔性机器人，满足了现代机器人在轻质、低耗和高速等方面发展的需要。

二.开发简单可行欠驱动平面柔性机械臂位置控制方法的需要

欠驱动机器人系统的自由度多于驱动输入，它能以较少的控制输入得到在较多位形空间内的运动，但是采取何种有效控制方法，使系统获得类似于或优于全驱动机器人的运动，一直是这个研究领域的关键问题和难点，甚至成为研究的瓶颈。

针对这一问题，国内外学者尝试采用非完整和非线性控制方法，取得了一些初步成果。在非完整控制的研究中，平衡点反馈镇定、运动规划与轨迹跟踪是三个研究重点，其中只有轨迹跟踪问题一般可用标准的非线性控制技术来解决，而其他两个问题，则没有既定的理论和方法可以直接采用。针对欠驱动平面系统的平衡点镇定问题，著名的Brockett定理早已证明，不存在光滑的或连续的时不变状态反馈方法，必须考虑时变的和离散的反馈控制方法。但是，这些控制方法和控制理论通常复杂、深奥，一般不易掌握，尤其对欠驱动平面机械臂这类二阶非完整系统来说，其控制设计难度更大，到目前为止，尚没有一个代表性的模型及有价值的一般性结果。因此，如何采用简单可行的控制方法获得欠驱动系统的满意运动，是一个十分迫切的问题。

另外，对欠驱动柔性机械臂来说，由于同时考虑欠驱动和柔性两个特性，除了欠驱动会增加控制设计上的难度之外，在运动过程中杆件的柔性带来的问题同样不能忽视。当机器人运动时，系统中的各零部件会产生明显的弹性变形，尤其是柔性杆件的弹性振动和杆件末端不断累积放大的弹性转角，将直接影响各关节的位置跟踪，特别对于多柔性杆件情况，产生的误差更加需要重视。此时，如果照搬欠驱动刚性系统的控制方法，不但不能完成操作任务，甚至会产生误动作，因而，开发适用于欠驱动柔性机器人的控制策略是十分重要的环节。

发明内容

本发明的目的是针对同时具有欠驱动和柔性杆的2R欠驱动平面柔性机械臂，基于分段控制设计，采用简单易行的控制方法实现机械臂的位置控制。

本发明所采用的技术方案是：首先建立第二关节被动、第二杆柔性的2R欠驱动平面机械臂的实际系统动力学模型；其次，针对系统同时具有的欠驱动和柔性特性，采用时间尺度和PID控制方法，规划两个关节的运动轨迹；最后，基于上述思想，将控制过程划分五个阶段完成，实现具有自由被动关节的整个2R欠驱动平面柔性机械臂的末端操作任务。

具体技术方案如下：

（一）动力学模型的建立

本发明关于2R欠驱动平面柔性机械臂的位置控制方法是基于动力学模型的，因此，在设计控制策略之前，必须建立系统动力学模型。这里，第二关节是被动的，第二杆是柔性的，采用便于实时控制的假设模态方法建立系统的动力学模型为

$\left\{\begin{array}{c}{D}_{11}\·{\stackrel{\·\·}{q}}_1+D_{12}\·{\stackrel{\·\·}{q}}_2+D_{13}\·{\stackrel{\·\·}{q}}_f+h_1+B_1\·{\stackrel{\·}{q}}_1+\mathrm{sgn}\left({\stackrel{\·}{q}}_1\right)\·B_{1j}={\mathrm{\τ}}_1\\ D_{21}\·{\stackrel{\·\·}{q}}_1+D_{22}\·{\stackrel{\·\·}{q}}_2+D_{23}\·{\stackrel{\·\·}{q}}_f+h_2+B_2\·{\stackrel{\·}{q}}_2+\mathrm{sgn}\left({\stackrel{\·}{q}}_2\right){\·B}_{2j}=0\\ D_{31}\·{\stackrel{\·\·}{q}}_1+D_{32}\·{\stackrel{\·\·}{q}}_2+D_{33}\·{\stackrel{\·\·}{q}}_f+h_3+K_f\·q_f=0\end{array}\right.---\left(1\right)$

其中，q₁、

和q₂、

分别为第一关节、第二关节的位置、速度和加速度变量，q_f、分别为第二杆的弹性广义坐标的位移、速度和加速度；D₁₁、D₁₂、D₁₃、D₂₁、D₂₂、D₂₃、D₃₁、D₃₂、D₃₃是惯性矩阵项，h₁、h₂、h₃是科氏力、离心力和刚柔耦合项，它们各项都和杆长、质量、关节变量、弹性广义坐标实时值有关。B₁和B₂分别是第一关节、第二关节的粘性摩擦系数，B_1j和B_2j分别是第一关节、第二关节的库仑摩擦力，τ₁为第一关节的驱动力矩，K_f为刚度矩阵。

（二）位置控制具体包含如下步骤：

由于系统中柔性杆的出现，因此对欠驱动柔性机械臂来说，动力学模型需要作相应的变换。首先，将式(1)中的

消去，得到新的方程为

$\left[\begin{array}{cc}{N}_{11}& N_{12}\\ N_{21}& N_{22}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·\·}{q}}_1\\ {\stackrel{\·\·}{q}}_2\end{array}\right]+\left[\begin{array}{c}{b}_1\\ b_2\end{array}\right]=\left[\begin{array}{c}{\mathrm{\τ}}_1\\ 0\end{array}\right]---\left(2\right)$

这里限定柔性机器人两杆成一条直线的状态为初始位形，并设此时关节位置都为0rad。设计欠驱动柔性机械臂分五个阶段完成操作任务，各阶段的控制算法和关节运动情况分别为：

第一阶段：第一关节从初始位置q₁₀顺时针运动到q_1d，运动结束时速度接近0rad/s，其中关节运动规律为

$q_1=q_{10}+(q_{1d}-q_{10})(\frac{t}{T}-\frac{1}{2\mathrm{\π}}\sin \frac{2\mathrm{\πt}}{T})=q_{10}+(q_{1d}-q_{10})(\mathrm{kt}-\frac{1}{2\mathrm{\π}}\sin 2\mathrm{\πkt})---\left(3\right)$

式中，T是采样时间间隔，k为时间尺度参数。

此阶段，将第二关节的角度位置作为控制目标，第二关节利用与第一关节之间的动力学耦合产生相应的运动，其预定位置q_2d可以通过动力学方程(2)计算出的加速度

进行积分得到，

求解如下所示，

${\stackrel{\·\·}{q}}_{2d}=-\frac{1}{N_{22}}(N_{21}{\stackrel{\·\·}{q}}_1+b_2)---\left(4\right)$

值得注意的是，由于柔性杆件的存在，预定位置q_2d与刚性系统的不同。对第二关节施加PID控制进行位置跟踪，其控制输入为

$u={\stackrel{\·\·}{q}}_2={\stackrel{\·\·}{q}}_{2d}+K_{P2}(q_{2d}-q_2)+K_{D2}({\stackrel{\·}{q}}_{2d}-{\stackrel{\·}{q}}_2)+K_{I2}\∫(q_{2d}-q_2)\mathrm{dt}---\left(5\right)$

式中，

为第二关节的实际位置，K_P2、K_I2和K_D2分别为PID控制器位置、积分和微分增益。此时，第一关节的驱动力矩为

${\mathrm{\τ}}_1=(N_{12}-N_{11}{N}_{21}^{-1}{N}_{22}){\stackrel{\·\·}{q}}_2+b_1-N_{11}{N}_{21}^{-1}{b}_2---\left(6\right)$

因此，此阶段按照(6)式规律控制第一关节处的电机力矩，即可实现第二关节的位置跟踪。

第二阶段：将第一关节作为控制目标，并采用PID控制方法，使其稳定在预定位置q_1d附近，控制算法为

${\stackrel{\·\·}{q}}_1={\stackrel{\·\·}{q}}_{1d}+K_{P1}(q_{1d}-q_1)+K_{D1}({\stackrel{\·}{q}}_{1d}-{\stackrel{\·}{q}}_1)+K_{I1}\∫(q_{1d}-q_1)\mathrm{dt}---\left(7\right)$

式中，K_P1、K_I1和K_D1分别为PID控制器的位置、积分和微分增益。此时，第一关节电机的驱动力矩为

${\mathrm{\τ}}_1=N_{11}{\stackrel{\·\·}{q}}_1+N_{12}{\stackrel{\·\·}{q}}_2+b_1---\left(8\right)$

此阶段对第二关节不作控制，使其在惯性作用下继续运动，但由于第二杆件是柔性杆，运动过程产生的弹性振动会对第二关节的运动产生影响，使其速度很快衰减到0rad/s，进入关节的摩擦区，并退出此阶段。

第三阶段：由于在第二阶段末，两个关节的速度都接近0rad/s，此阶段将继续使第一关节和第二关节同时稳定在各自的位置不动，并持续一段时间。这里，控制目标为第一关节，对第一关节运用PID控制方法，其控制算法为式(7)。

第四阶段：第二关节位置为控制目标，使其运动返回到位置q₂₀=0rad，且在此阶段结束状态时速度刚好接近0rad/s，此时，第一杆和第二杆成一条直线。此阶段，对第二关节进行PID控制，控制算法见式(5)，此时第二关节的运动规律为

$q_2=q_{20}+(q_{2d}-q_{20})(\frac{t}{T}-\frac{1}{2\mathrm{\π}}\sin \frac{2\mathrm{\πt}}{T})---\left(9\right)$

由于动力学耦合关系，当第二关节作返回运动时，第一关节继续向正方向运动，由于对它不作任何控制，这样，当第二运动到位置q₂₀时，第一关节的速度不为零，还会继续运动。

(5)第五阶段：控制目标为第一关节，对其施加PID位置控制，控制算法如式(7)所示，要求它最后稳定在0rad。

上述控制方法的理论依据：

动力学方程从(1)式变换到(2)式，推导过程如下，从(1)式的第三行可得

${\stackrel{\·\·}{q}}_f=-\frac{1}{D_{33}}(D_{31}\·{\stackrel{\·\·}{q}}_1+D_{32}\·{\stackrel{\·\·}{q}}_2+h_3+K_f\·q_f)---\left(10\right)$

将(10)式带回(1)中的前两行，得

$\left\{\begin{array}{c}(D_{11}-D_{13}\frac{D_{31}}{D_{33}}){\stackrel{\·\·}{q}}_1+(D_{12}-D_{13}\frac{D_{32}}{D_{33}}){\stackrel{\·\·}{q}}_2+h_1-\frac{D_{13}}{D_{33}}(h_3+K_f{q}_f)={\mathrm{\τ}}_1\\ (D_{21}-D_{23}\frac{D_{31}}{D_{33}}){\stackrel{\·\·}{q}}_1+(D_{22}-D_{23}\frac{D_{32}}{D_{33}}){\stackrel{\·\·}{q}}_2+h_2-\frac{D_{23}}{D_{33}}(h_3+K_f{q}_f)=0\end{array}\right.---\left(11\right)$

将(11)式简化为(2)式所示。

本发明的优点和积极效果：

本发明主要涉及一种2R欠驱动平面柔性机械臂的位置控制方法，其优势在于：

（1）传统全驱动机器人的自由度等于关节驱动器的个数，后一个驱动器就成为前一个的负载，因而整个机器人不但体积庞大，而且质量大，无法完成高精度、高速的运动。本发明的控制对象是具有完全自由的关节和柔性杆的欠驱动平面机械臂，直接省略驱动装置，并采用结构优化的柔性杆件，因而，具有轻质、低耗和结构紧凑等优点，满足现代机器人在高速、高精度、重载和轻量化等方面的要求；

（2）根据系统同时存在欠驱动和柔性两方面的特点，并针对欠驱动柔性和刚性系统的在动力学特性上的本质区别，对第二杆柔性，第二关节被动的2R欠驱动平面机械臂进行了位置控制，不但控制两个关节实现了位置跟踪和定点控制等目标，同时使整个系统完成了类似于刚性系统的复杂操作任务。本发明没有从复杂、深奥、不易掌握的非线性控制理论和方法着手，而是采用时间尺度和PID控制等简单可行的控制方法，分若个阶段完成控制任务，达到了方法简单、思路清晰和易于实现的目的。将此方法应用于一个自制的2R欠驱动柔性机械臂上，控制效果比较理想（见实施例1）。

附图说明

图1是第一阶段两个关节运动示意图。

图2是第二阶段两个关节运动示意图。

图3是第四阶段两个关节运动示意图。

图4是第一阶段控制系统结构框图。

图5是第二阶段控制系统结构框图。

图6是2R欠驱动平面柔性机械臂示意图。

图7是2R欠驱动平面柔性机械臂控制流程图。

图8是第一阶段第二关节位置轨迹。

图9是第二阶段第二关节位置轨迹。

图10是第二阶段第二关节速度轨迹。

图11是第四阶段第二关节位置轨迹。

图12是第五阶段第一关节位置轨迹。

图13是第五阶段第二关节位置轨迹。

图14是整个过程柔性杆根部的弹性变形实测值。

具体实施方式

实施例1：

本发明提供的2R欠驱动平面柔性机械臂具体结构参照图6（图中1-电机；2-第一关节；3-刚性杆；4-柔性杆；5-第二关节；6-编码器），对其进行位置控制的具体步骤如下，其控制流程参照图7。

（一）动力学模型的建立

以2R欠驱动柔性机械臂为研究对象，其中第二关节是被动的，第二杆件是柔性的，采用假设模态方法建立系统的动力学模型为

$\left\{\begin{array}{c}{D}_{11}\·{\stackrel{\·\·}{q}}_1+D_{12}\·{\stackrel{\·\·}{q}}_2+D_{13}\·{\stackrel{\·\·}{q}}_f+h_1+B_1\·{\stackrel{\·}{q}}_1+\mathrm{sgn}\left({\stackrel{\·}{q}}_1\right)\·B_{1j}={\mathrm{\τ}}_1\\ D_{21}\·{\stackrel{\·\·}{q}}_1+D_{22}\·{\stackrel{\·\·}{q}}_2+D_{23}\·{\stackrel{\·\·}{q}}_f+h_2+B_2\·{\stackrel{\·}{q}}_2+\mathrm{sgn}\left({\stackrel{\·}{q}}_2\right){\·B}_{2j}=0\\ D_{31}\·{\stackrel{\·\·}{q}}_1+D_{32}\·{\stackrel{\·\·}{q}}_2+D_{33}\·{\stackrel{\·\·}{q}}_f+h_3+K_f\·q_f=0\end{array}\right.---\left(1\right)$

其中，q₁、

和q₂、

分别为第一关节、第二关节的位置、速度和加速度变量，q_f、

分别为第二杆的弹性广义坐标的位移、速度和加速度；D₁₁、D₁₂、D₁₃、D₂₁、D₂₂、D₂₃、D₃₁、D₃₂、D₃₃是惯性矩阵项，h₁、h₂、h₃是科氏力、离心力和刚柔耦合项，它们各项都和杆长、质量、关节变量、弹性广义坐标实时值有关。τ₁为第一关节的驱动力矩，K_f为刚度矩阵。

$D_{11}=m_1{r}_1^2+J_1+m_1{l}_1^2+m_2{l}_1{l}_2+\frac{1}{3}{m}_2{l}_2^2+m_2{l}_1{l}_2\cos \left(q_2\right)+J_m+D_{11}^{\′}$

$D_{12}=\frac{1}{3}{m}_2{l}_2^2+\frac{1}{2}{m}_2{l}_1{l}_2\cos \left(q_2\right)+D_{12}^{\′}$ D₂₁=D₁₂ $D_{22}=\frac{1}{3}{m}_2{l}_2^2+D_{22}^{\′}$ 这里，m₁和m₂分别为两杆的质量，m₁=3.384kg，m₂=0.0231kg；l₁和l₂分别为两杆的长度，l₁=0.26m，l₂=0.245m；J₁为关节1的转动惯量，J₁=0.00364kgm²；J_m为电机转动惯量，J_m=0.4×10^-4×i²kgm²，i=70为关节1减速器的传动比；r₁为杆1质心到第一关节之间的长度，r₁=0.1558m，D₁′₁、D₁′₂和D₂′₂为刚柔耦合项。第二柔性杆参数：弹性模量E=2.05×10⁵Mpa，惯性矩I=0.36mm⁴，截面高h=20mm，b=0.6mm。B₁和B₂分别是第一、第二关节的粘性摩擦系数，B_1j和B_2j是各自的库仑摩擦力，总是与关节运动方向相反，因此在其前面增加符号函数。在第一阶段：B₁=0.0325，B_1j=0.0325，B₂=0.038，B_1j=0.38；在第四阶段：B₁=0.027，B_1j=0.027，B₂=0.042，B_1j=0.42。sgn(x)为

$\mathrm{sgn}\left(x\right)=\left\{\begin{array}{cc}\left\{1\right\}& x>0\\ \{-1\}& x<0\\ \{-\mathrm{1,1}\}& x=0\end{array}\right.$

（二）位置控制具体包含如下步骤：

这里限定柔性机器人两杆成一条直线的状态为初始位形，并设此时关节位置都为0rad。设计欠驱动柔性械臂分5个阶段完成操作任务，各阶段的控制算法和关节运动情况分别为：

第一阶段(0～0.92s)：第一关节从初始位置q₁₀=0rad顺时针运动到q_1d=-0.7rad，运动结束时速度接近0rad/s，其中关节运动规律为

$q_1=q_{10}+(q_{1d}-q_{10})(\frac{t}{T}-\frac{1}{2\mathrm{\&pi;}}\sin \frac{2\mathrm{\&pi;t}}{T})=q_{10}+(q_{1d}-q_{10})(\mathrm{kt}-\frac{1}{2\mathrm{\&pi;}}\sin 2\mathrm{\&pi;kt})---\left(3\right)$

式中，T是采样时间间隔，此时T=4ms，k为时间尺度参数，调节k的大小可以调整第一关节的运动速度和加速度，但是不会改变关节所转过的角度，此时k=230。此阶段，第二关节为控制目标，它在PID控制器的控制作用下实现位置跟踪，控制结构如图7所示，其控制输入为

$u={\stackrel{\&CenterDot;\&CenterDot;}{q}}_2={\stackrel{\&CenterDot;\&CenterDot;}{q}}_{2d}+K_{P2}(q_{2d}-q_2)+K_{D2}({\stackrel{\&CenterDot;}{q}}_{2d}-{\stackrel{\&CenterDot;}{q}}_2)+K_{I2}\&Integral;(q_{2d}-q_2)\mathrm{dt}---\left(5\right)$ 式中，K_P2、K_I2和K_D2分别为控制关节2的PID控制器位置、积分和微分增益，K_P2=0.0075，K_I2=0.000125，K_D2=0.15，_q2d为关节2目标位置。

此阶段，第二关节利用与第一关节之间的动力学耦合产生相应的运动，此时欠驱动平面柔性机械臂第一关节电机的驱动力矩为

${\mathrm{\&tau;}}_1=(N_{12}-N_{11}{N}_{21}^{-1}{N}_{22}){\stackrel{\&CenterDot;\&CenterDot;}{q}}_2+b_1-N_{11}{N}_{21}^{-1}{b}_2---\left(6\right)$

第二阶段(0.92s～1.456s)：将第一关节作为控制目标，并采用PID控制方法，使其稳定在目标位置q_1d附近，控制算法为

${\stackrel{\&CenterDot;\&CenterDot;}{q}}_1={\stackrel{\&CenterDot;\&CenterDot;}{q}}_{1d}+K_{P1}(q_{1d}-q_1)+K_{D1}({\stackrel{\&CenterDot;}{q}}_{1d}-{\stackrel{\&CenterDot;}{q}}_1)+K_{I1}\&Integral;(q_{1d}-q_1)\mathrm{dt}---\left(7\right)$

式中，K_P1、K_I1和K_D1分别为PID控制器的位置、积分和微分增益，K_P1=0.0147，K_I2=0.000343，K_D2=0.21。此时，第一关节电机的驱动力矩为

${\mathrm{\&tau;}}_1=N_{11}{\stackrel{\&CenterDot;\&CenterDot;}{q}}_1+N_{12}{\stackrel{\&CenterDot;\&CenterDot;}{q}}_2+b_1---\left(8\right)$

此阶段对第二关节不作控制，使其在惯性作用下继续运动，但由于杆件2是柔性杆，运动过程产生的弹性振动会对关节2的运动产生影响，使其速度很快衰减到0rad/s，进入关节的摩擦区，并退出此阶段。

第三阶段(1.456s～13.456s)：第三阶段：由于在第二阶段末，两个关节的速度都接近0rad/s，此阶段将继续使第一关节和第二关节同时稳定在各自的位置不动，并持续一段时间。这里，控制目标为第一关节，对第一关节运用PID控制，其控制算法为式(5)，且K_P1=0.00003，K_I2=0.000001，K_D2=0.03。

第四阶段(13.456s～14.376s)：第二关节位置为控制目标，使其运动返回到位置q₂₀=0rad，且在此阶段结束状态时速度

刚好接近0rad/s，此时，第一杆和第二杆成一条直线。此阶段，对第二关节进行PID控制，控制算法见式(4)，K_P2=0.0027，K_I2=0.000027，K_D2=0.09。此时第二关节的运动规律为

$q_2=q_{20}+(q_{2d}-q_{20})(\frac{t}{T}-\frac{1}{2\mathrm{\&pi;}}\sin \frac{2\mathrm{\&pi;t}}{T})---\left(9\right)$

由于动力学耦合关系，当第二关节作返回运动时，第一关节继续向正方向运动，由于对它不作任何控制，这样，当第二运动到位置q₂₀时，第一关节的速度不为零，还会继续运动，此时k=230。

(5)第五阶段(14.376s～34.376s)：控制目标为第一关节，对其施加PID位置控制，控制算法如式(5)所示，要求它最后稳定在0rad，且K_P1=0.00003，K_I2=0.000001，K_D2=0.03。

采用说明书所述方法对2R欠驱动平面柔性机械臂进行控制，得到了图8～图14的控制效果。其中图8表明在第一阶段第二关节实现了对目标值q_2d的较好地跟踪；图9和图10分别为第二阶段第二关节的位置和速度轨迹，二曲线均呈波浪状，反映柔性杆的振动对关节运动的影响，表明第二关节在惯性作用下运动，当速度为零进入下一阶段，此时的位置为0.27rad；图11表明第四阶段第二关节能很好地跟踪目标位置；图12表明第五阶段第一关节在PID控制器的作用下继续缓慢逆时针运动，大约在1.0rad时开始顺时针运动，7s后接近0rad，并在此位置稳定不动，图13表明此阶段第二关节在摩擦力作用下始终保持在0rad附近；图14反映整个运动过程中第二杆根部弹性应变数值变化。

Claims (1)

1.一种2R欠驱动平面柔性机械臂的位置控制方法，其特征在于该方法包括：

（一）动力学模型的建立

在设计控制策略之前，必须建立系统动力学模型，这里，第二关节是被动的，第二杆是柔性的，采用便于实时控制的假设模态方法建立系统的动力学模型为

$\left\{\begin{array}{c}{D}_{11}\&CenterDot;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_1+D_{12}\&CenterDot;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_2+D_{13}\&CenterDot;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_f+h_1+B_1\&CenterDot;{\stackrel{\&CenterDot;}{q}}_1+\mathrm{sgn}\left({\stackrel{\&CenterDot;}{q}}_1\right)\&CenterDot;B_{1j}={\mathrm{\&tau;}}_1\\ D_{21}\&CenterDot;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_1+D_{22}\&CenterDot;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_2+D_{23}\&CenterDot;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_f+h_2+B_2\&CenterDot;{\stackrel{\&CenterDot;}{q}}_2+\mathrm{sgn}\left({\stackrel{\&CenterDot;}{q}}_2\right){\&CenterDot;B}_{2j}=0\\ D_{31}\&CenterDot;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_1+D_{32}\&CenterDot;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_2+D_{33}\&CenterDot;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_f+h_3+K_f\&CenterDot;q_f=0\end{array}\right.---\left(1\right)$ 其中，q₁、

和q₂、

分别为第一关节、第二关节的位置、速度和加速度变量，q_f、

分别为第二杆的弹性广义坐标的位移、速度和加速度；D₁₁、D₁₂、D₁₃、D₂₁、D₂₂、D₂₃、D₃₁、D₃₂、D₃₃是惯性矩阵项，h₁、h₂、h₃是科氏力、离心力和刚柔耦合项，它们各项都和杆长、质量、关节变量、弹性广义坐标实时值有关。B₁和B₂分别是第一关节、第二关节的粘性摩擦系数，B_1j和B_2j分别是第一关节、第二关节的库仑摩擦力，τ₁为第一关节的驱动力矩，K_f为刚度矩阵；

（二）位置控制，具体包含如下步骤：

由于系统中柔性杆的出现，因此对欠驱动柔性机械臂来说，动力学模型需要作相应的变换；首先，将式(1)中的

消去，得到新的方程为

$\left[\begin{array}{cc}{N}_{11}& N_{12}\\ N_{21}& N_{22}\end{array}\right]\left[\begin{array}{c}{\stackrel{\&CenterDot;\&CenterDot;}{q}}_1\\ {\stackrel{\&CenterDot;\&CenterDot;}{q}}_2\end{array}\right]+\left[\begin{array}{c}{b}_1\\ b_2\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&tau;}}_1\\ 0\end{array}\right]---\left(2\right)$

这里限定柔性机器人两杆成一条直线的状态为初始位形，并设此时关节位置都为0rad；设计欠驱动柔性械臂分五个阶段完成操作任务，各阶段的控制算法和关节运动情况分别为：

第一阶段：第一关节从初始位置q₁₀顺时针运动到q_1d，运动结束时速度接近0rad/s，其中关节运动规律为

$q_1=q_{10}+(q_{1d}-q_{10})(\frac{t}{T}-\frac{1}{2\mathrm{\&pi;}}\sin \frac{2\mathrm{\&pi;t}}{T})=q_{10}+(q_{1d}-q_{10})(\mathrm{kt}-\frac{1}{2\mathrm{\&pi;}}\sin 2\mathrm{\&pi;kt})---\left(3\right)$

式中，T是采样时间间隔，k为时间尺度参数；

此阶段，将第二关节的角度位置作为控制目标，第二关节利用与第一关节之间的动力学耦合产生相应的运动，预定位置q_2d通过动力学方程(2)计算出的加速度

进行积分得到，

求解如下所示，

${\stackrel{\&CenterDot;\&CenterDot;}{q}}_{2d}=-\frac{1}{N_{22}}(N_{21}{\stackrel{\&CenterDot;\&CenterDot;}{q}}_1+b_2)---\left(4\right)$

由于柔性杆件的存在，预定位置q_2d与刚性系统的不同；对第二关节施加PID控制进行位置跟踪，控制输入为

$u={\stackrel{\&CenterDot;\&CenterDot;}{q}}_2={\stackrel{\&CenterDot;\&CenterDot;}{q}}_{2d}+K_{P2}(q_{2d}-q_2)+K_{D2}({\stackrel{\&CenterDot;}{q}}_{2d}-{\stackrel{\&CenterDot;}{q}}_2)+K_{I2}\&Integral;(q_{2d}-q_2)\mathrm{dt}---\left(5\right)$

式中，

为第二关节的实际位置，K_P2、K_I2和K_D2分别为PID控制器位置、积分和微分增益；此时，第一关节的驱动力矩为

${\mathrm{\&tau;}}_1=(N_{12}-N_{11}{N}_{21}^{-1}{N}_{22}){\stackrel{\&CenterDot;\&CenterDot;}{q}}_2+b_1-N_{11}{N}_{21}^{-1}{b}_2---\left(6\right)$ 因此，此阶段按照(6)式规律控制第一关节处的电机力矩，实现第二关节的位置跟踪；

第二阶段：将第一关节作为控制目标，并采用PID控制方法，使第一关节稳定在目标位置q_1d附近，控制算法为

${\stackrel{\&CenterDot;\&CenterDot;}{q}}_1={\stackrel{\&CenterDot;\&CenterDot;}{q}}_{1d}+K_{P1}(q_{1d}-q_1)+K_{D1}({\stackrel{\&CenterDot;}{q}}_{1d}-{\stackrel{\&CenterDot;}{q}}_1)+K_{I1}\&Integral;(q_{1d}-q_1)\mathrm{dt}---\left(7\right)$ 式中，K_P1、K_I1和K_D1分别为PID控制器的位置、积分和微分增益；此时，第一关节电机的驱动力矩为

${\mathrm{\&tau;}}_1=N_{11}{\stackrel{\&CenterDot;\&CenterDot;}{q}}_1+N_{12}{\stackrel{\&CenterDot;\&CenterDot;}{q}}_2+b_1---\left(8\right)$ 此阶段对第二关节不作控制，在惯性作用下继续运动，但由于第二杆是柔性杆，运动过程产生的弹性振动会对第二关节的运动产生影响，使速度很快衰减到0rad/s，进入关节的摩擦区，并退出此阶段；

第三阶段：由于在第二阶段末，两个关节的速度都接近0rad/s，此阶段将继续使第一关节和第二关节同时稳定在各自的位置不动，并持续一段时间；这里，控制目标为第一关节，对第一关节运用PID控制方法，控制算法为式(7)；

第四阶段：第二关节位置为控制目标，使第二关节运动返回到位置q₂₀=0rad，且在此阶段结束状态时速度

刚好接近0rad/s，此时，第一杆和第二杆成一条直线；此阶段，对第二关节进行PID控制，控制算法见式(5)，此时第二关节的运动规律为

$q_2=q_{20}+(q_{2d}-q_{20})(\frac{t}{T}-\frac{1}{2\mathrm{\&pi;}}\sin \frac{2\mathrm{\&pi;t}}{T})---\left(9\right)$

由于动力学耦合关系，当第二关节作返回运动时，第一关节继续向正方向运动，由于对它不作任何控制，这样，当第二运动到位置q₂₀时，第一关节的速度不为零，还会继续运动；

第五阶段：控制目标为第一关节，施加PID位置控制，控制算法如式(7)所示，要求它最后稳定在0rad。

Images