与传统工业机器人[1-2]相比，连续体机器人通过模仿蛇、章鱼触手、象鼻等生物特性，具有无限自由度，对于非结构环境的适应性更强，且质地柔软，在灾害救援、微创手术及脆弱目标抓取等方面应用广泛．连续体机器人按驱动方式的不同可分为：柔性流体驱动型(FFA)[3-4]，形状记忆合金型(SMA)[5-7]，电活性聚合物型(EAP)[8]，绳驱动型[9-10]．与其他驱动方式相比，绳驱动的连续体机器人具有自重轻、转动惯量小和易于实现变刚度控制等优点，但是绳驱动连续体机器人的连续弯曲、无限自由度的非线性结构使得常规控制方法难以提供稳定的控制效果．由于绳驱动机械手具有典型的非线性特性，因此PID等常规控制算法用于绳驱动机械手轨迹跟踪控制精度较差．模糊控制(Fuzzy)对于非线性系统具有较好的控制效果[11-12]，但由于绳驱动机械手是典型的欠驱动系统，随着关节增加，模糊规则库难以建立，从而影响控制效果．滑模控制(SMC)具有结构简单、鲁棒性强的优点，对具有不确定参数的被控对象有着良好的控制效果[13-14]，但是SMC的抖振问题容易造成系统误差无法在有限时间内收敛到零，并且电机控制量的跳变也容易干扰跟踪精度．模型预测控制(MPC)是一种应用广泛的基于模型的控制技术．文献[15]利用DNN-MPC模型为具有柔性塑料关节和刚性连杆的六自由度气动机器人建模，使所有关节上的平均稳态误差减少到1°~2°之间．文献[16]提出非线性演化MPC(NEMPC)模型并应用在一个24种状态的气动连续体柔性机器人实验平台上，但在性能方面并未有出色表现．文献[17]在考虑到机器人在与外部环境相互作用而产生构型变化的情况下，为柔性连续体机器人提出了一种具有指数加权预测层的预测控制算法，并实验验证其有效性．文献[18]提出一种基于库普曼的MPC模型(KL-MPC)，实现了气动柔性连续介质机械臂的自主控制．现阶段MPC控制建模的研究对象多是刚性或柔性流体驱动型机器人，对于基于绳驱动的连续体机器人的MPC控制建模研究较少．本研究设计了一种基于量子粒子群优化(QPSO)算法全局搜索策略的MPC控制器，并成功应用于绳驱动连续体机器人的轨迹跟踪控制．该QPSO-MPC控制器建立在连续体机器人的运动学分析基础上，仿真及机器人平台实验验证了本方法具有精确的轨迹跟踪能力．1 绳驱动机器人结构及运动学模型1.1　机器人坐标建立本研究设计的绳驱动连续体机器人结构如图1所示．该机器人由1根柔性支撑杆，4条均匀分布，间隔为π/2的驱动绳索及6个关节盘组成．10.13245/j.hust.229388.F001图1绳驱动连续体机器人结构示意图建立如图2所示的连续型机器人单关节几何模型，将4个驱动绳穿过的孔从1~4标号．分别以关节盘基座圆心o0，关节盘末端圆心o1为坐标原点，以o0到孔1方向为x0轴，o0到孔2方向为y0轴，z0轴垂直基座向上，以o1到孔1方向为x1轴，o1到孔2方向为y1轴，z1轴垂直关节盘末端向上，建立如图2所示的坐标系．o为关节弯曲后形成的圆弧的圆心，旋转角α为平面x0-y0-z0与平面o-o0-o1的夹角，θ为柔性支撑弯曲角．10.13245/j.hust.229388.F002图2绳驱动连续体机器人单关节模型及坐标系在分析单关节机器人运动学模型之前，须要做以下关键假设：a．连续体机器人以常曲率弯曲；b．机器人的柔性橡胶支撑不可压缩；c．机器人由4条均匀分布的牵引绳驱动．该机器人是通过绳索长度变化量Δli(i=1，2，3，4)控制，Δli即为驱动空间变量．定义机器人的关节空间Θ=[α，θ]T，表示弯曲角θ∈(0，π)与旋转角α∈(0，2π)的集合，通过控制绳索长度就可以操控机器人的弯曲和旋转角度．定义机器人的工作空间为P，表示末端位置向量，通过关节空间可以推导出工作空间．如图3所示，从驱动空间到工作空间的正向映射即为机器人的正运动学分析．正运动学建模分为两个步骤：首先通过分段常曲率法推导驱动空间与关节空间的映射关系；然后根据连续体机器人的齐次坐标变换矩阵，得到关节空间与工作空间的映射关系．与之相反，从工作空间到驱动空间的反向映射即为绳驱动连续体机器人的逆运动学分析．10.13245/j.hust.229388.F003图3连续体机器人各空间变量映射关系1.2　驱动空间与关节空间映射图4为机器人单关节模型的俯视图．如图4中的几何关系所示，在恒定曲率假设下，四根驱动绳的曲率半径可表示为ζi=ζ-rcos αi，(1)式中：ζ和ζi分别为机器人柔性支撑和第i根绳索的弯曲曲率半径；r为关节盘绳孔所在分度圆半径；αi为驱动绳i产生的扭转角，有αi=α-(i-1)π/2．由于柔性支撑与驱动绳所形成的弯曲角θ相同，将式(1)与弯曲角θ相乘，并根据驱动绳索的平行特性可推导出四根驱动绳长度变化Δli(i=1，2，3，4)为Δli=l-li=(ζ-ζi)θ=rθcos αi，(2)式中l和li分别为机器人单关节长度和第i根驱动绳的绳长．10.13245/j.hust.229388.F004图4连续体机器人俯视图根据式(2)，驱动空间到关节空间的映射关系可表示为：αi=arctan (Δli+1/Δli)；(3)θ=Δli/(rcos α)．(4)1.3　关节空间与工作空间映射关节空间与工作空间的映射关系可以通过求解基座坐标到末端坐标的变换矩阵T∈R4×4获得，T由旋转矩阵R∈R3×3和平移矩阵(机器人末端坐标)P∈R3组成．基于几何分析法，该连续体机器人基坐标到末端坐标的旋转矩阵R可通过绕y轴和z轴两次旋转得到R=c2αc θ+s2αc αs αc θ-c αs αc αs θc αs αc θ-c αs αs2αc θ+c2αs αs θ-c αs θ-s αs θc θ，(5)式中：c表示cos；s表示sin．位置向量为P=lθc α(1-c θ)lθs α(1-c θ)lθs θT．(6)基座坐标到末端坐标的位姿变换矩阵为Τ=RP01．(7)式(7)描述了关节空间到工作空间的正运动学模型．为了求解工作空间到关节空间的逆运动学变换，将T表示为T=nxoxaxpxnyoyaypynzozazpz0001，(8)式中：(nx，ny，nz)，(ox，oy，oz)和(ax，ay，az)分别为末端坐标系x1，y1和z1轴所对应的单位矢量的各分量；px，py和pz为末端坐标系原点在基座坐标系中的对应位置分量．当机器人末端位置已知时，连续体机器人的弯曲角θ和旋转角α为：θ=arccos az；(9)α=arctan (py/px)．(10)2 控制策略传统MPC须要在每个采样瞬间解决一个有限时间范围的开环优化问题，非常耗时．为了提高模型预测控制最优解的速度，本研究设计了一种基于QPSO-MPC控制器的连续体机器人轨迹跟踪算法．如图5所示，通过工作空间到关节空间的逆运动学方程将参考轨迹坐标和实测的轨迹坐标转换为关节空间变量并传入QPSO-MPC控制器．该控制器由系统约束、线性误差模型和用于目标函数求解的QPSO优化模块组成．图中：Pd为机器人末端期望坐标；连续体机器人控制系统的状态量x=[x，y，z，θ，α]T；控制量u=[θ˙，α˙]T；xd为其期望状态量；uc为QPSO输出的未来控制量；Δl˙i为绳长变化量的一阶导数．则连续体机器人的状态空间表达式可写为x˙=f(x,u)．(11)10.13245/j.hust.229388.F005图5基于QPSO-MPC的连续体机器人轨迹跟踪算法流程框图假设系统质量、惯性及有效的伺服驱动器的瞬态响应可以忽略．连续体机器人关节空间速度Θ˙∈R2×1与末端速度P˙∈R3×1之间的正运动学可用其雅可比矩阵表示，对式(6)求导得到P˙=J(Θ)Θ˙=-lc α1-θs θ-c θθ2ls αc θ-1θ-ls α1-θs θ-c θθ2-lc αc θ-1θ-lθc θ-s θθ20θ˙α˙, (12)式中J(Θ)∈R3×2为机器人关节角的雅可比矩阵．根据式(12)，将关节状态引入系统状态空间得到P˙Θ˙=J(Θ)Θ˙Θ˙．(13)将式(11)在参考轨迹xd处泰勒展开，并经过离散化处理后得x˜(k+1)=Atx˜(k)+Btu˜(k)，(14)式中：x˜=x-xd;u˜=u-ud;At=δfδxx=xdu=ud;Bt=δfδux=xdu=ud．设预测控制器模型的目标函数为F(k)=∑i=1Npx(k+i|t)-xd(k+i|t)Q2+∑i=1Nc-1Δu(k+i|t)R2+ρε2,(15)式中：Np和Nc分别为预测时域和控制时域；Q和R分别为状态增量加权矩阵和控制增量加权矩阵；ρ为权重系数；ε为松弛因子．ρ和ε可保证系统每个时刻的优化目标都有可行解．此时，可以推导出系统的预测输出为Y(t)=Ψtξ(t|t)+ΓtΔU(t)．(16)基于对机器人运行稳定性及跟踪精确度的考虑，当伺服控制器拉拽绳索时，要避免出现转速跳变，所以须增加控制量与控制增量约束条件，即：umin(t+k)≤u(t+k)≤umax(t+k)；(17)Δumin(t+k)≤Δu(t+k)≤Δumax(t+k)，(18)式中k=0，1，⋯，Nc-1．此外，机器人末端执行器也必须在其可行域内运动，即xmin(t+k)≤x(t+k)≤xmax(t+k)，(19)式中k=0，1，⋯，Np-1．当跟踪参考轨迹时，系统的期望被控输出与参考轨迹的误差x(k+i|t)-xd(k+i|t)Q2应尽可能接近零．此时目标函数就可简化为求解控制增量的表达式，即F(ξ(t),u(t-1),ΔU(t))=ΔU(t)HtΔU(t)T+GtΔU(t)T, (20)式中：Ht=ΓtTQΓt+R00ρ为正定矩阵用来惩罚控制变化率；Gt=2E(t)TQΓt0;E(t)为预测域中的跟踪误差．Ht和Gt将作为QPSO优化流程的输入．整理式(17)~(20)，求解MPC的优化约束问题可以表示为以下QPSO优化问题：minΔU(t) ΔU(t)HtΔU(t)T+GtΔU(t)T;s.t. ΔUmin≤ΔUt≤ΔUmax,Umin≤ΔUt+Ut≤Umax,Ymin≤Yt≤Ymax. (21)QPSO的粒子演化表达式为mbest(t)=∑i=1MPiM=∑i=1MPi1M,∑i=1MPi2M,…,∑i=1MPiDM；(22)Pm,ij(t+1)=φj(t)Pij(t)+(1-φj(t))Pgj(t)；(23)Xij(t+1)=Pm,ij(t)±βmbest(t)-Xij(t)ln(ur(t))-1, (24)式中：mbest(t)为所有粒子最优位置平均值；Pi(t)=(Pi1(t)，Pi2(t)，…，PiD(t))为粒子i(i∈[1，M])第t次迭代最优位置；Pg(t)=(Pg1(t)，Pg2(t)，…，PgD(t))为第t次迭代全局最优位置；Xij(t)=(Xi1(t)，Xi2(t)，…，XiD(t))为粒子位置；Pm,ij(t)为Pi(t)与Pg(t)之间的随机点；M和D分别为粒子总数和粒子群维度；φ(t)和ur(t)为0~1的随机参数；β为收缩-膨胀系数，用来调整控制算法的收敛速度．QPSO算法的基本逻辑如下．步骤1 随机初始化粒子群位置Xi(0)．令Pi(0)=Xi(0)，再将Pi(0)代入式(21)找到全局最优位置Pg(0)．步骤2 通过式(22)计算mbest(t)．步骤3 遍历所有粒子，根据式(21)计算每个粒子的位置损失函数值F(Xij(t+1))；根据式(25)更新个体最优位置Pi(t)，即Pi(t)=Xi(t)F(Xi(t))≤F(Pi(t-1));Pi(t-1)F(Xi(t))F(Pi(t-1)). (25)根据式(26)更新全局最优位置Pg(t)，即Pg(t)=Pg(t-1)F(Pi(t))≤F(Pg(t-1));Pi(t)F(Pi(t))F(Pg(t-1)). (26)将Pi(t)，Pg(t)代入式(23)，求出随机点Pm,ij(t+1)，然后由式(24)更新所有粒子的新位置Xij(t+1)．步骤4 令t=t+1，返回步骤2，直到等于最大迭代数，算法结束．将最终的Pg(t)作为控制增量返回到连续体机器人的控制系统中．在经过QPSO优化处理后，可以得到一系列控制输入增量，即ΔUt*=[Δut*, Δut+1*, ⋯, Δut+Nc-1*]T．(27)整个运动学控制器的输出是式(27)的第一个元素，这样就可以求出该动态控制器的期望控制速度为uc=Δut*+ud．(28)对式(2)求导可以得到4根绳索驱动速度，即Δl˙i=r[θ˙cos(γ-α)-θα˙sin(γ-α)]，(29)式中：Δl˙i为第i根驱动绳的期望速度；γ为分位角，有γ=(n-1)π/2．将Δl˙i输入到伺服控制器，即可操控伺服电机按照期望速度转动．3 仿真与实验结果3.1　仿真结果首先通过定点跟踪(标称模型)验证QPSO-MPC控制器静态仿真性能的有效性，然后通过预设空间轨迹来验证控制器的动态轨迹跟踪效果．仿真过程中将MPC控制器的性能与QPSO-MPC控制器进行比较．初始向量x=[0，0，l，0，0]T，即以上实验连续体机器人初始位置都设定为4根绳索自然松弛的状态下，其关节长度l=30 cm．本实验设定连续体机器人目标位置为xd=[12.405，7.162，24.810，π/3，π/6]T，采样周期为0.005 s，MPC预测时域为10，控制时域为1，最大迭代次数为50，QPSO粒子群规模为50．两种控制器对定点跟踪效果如图6，结果表明：QPSO-MPC与MPC控制器都可追踪固定空间点，平均跟踪误差分别为2.528 8 cm和2.598 1 cm．在接近目标点过程中，QPSO-MPC控制器能更快更稳定地跟踪到目标点．10.13245/j.hust.229388.F006图6定点跟踪仿真实验结果然后通过跟踪预设轨迹测试QPSO-MPC控制器的轨迹跟踪效果．空间轨迹跟踪实验结果如图7所示，在轨迹跟踪初期QPSO-MPC与MPC基本重合．这是因为初期跟踪误差较大，在系统约束相同的情况下，最优的控制输入即为系统约束的上下限．当跟踪误差缩小到一定范围时，QPSO-MPC在系统约束范围内优化控制输入序列，抑制了系统超调，使得机器人更快、更准确稳定地跟踪目标轨迹．空间轨迹跟踪实验验证了QPSO-MPC优化过程的有效性．10.13245/j.hust.229388.F007图7空间轨迹跟踪仿真实验结果3.2　实验结果为验证本研究设计的QPSO-MPC控制器的实际控制效果，搭建了如图8所示的实验平台．机器人系统由1根柔性橡胶软杆，1个固定在机器人末端的MTI-630姿态传感器，4根抗绕驱动绳，4个伺服驱动器，6个关节盘及20个不锈钢弹簧组成．柔性橡胶软杆长为30 cm，直径为1 cm，为机器人提供弯曲刚度；关节盘直径为30 cm，厚度为1 cm，相邻关节盘距离为6 cm，每个关节盘有8个均匀分布的直径为0.5 cm的圆孔作为驱动绳的拉伸通道；不锈钢弹簧直径为1 cm，用以维持机器人外部轮廓；伺服驱动器均匀分布在操作底盘上；MTI-630姿态传感器方向误差低于0.5°，经过滤波处理后得到机器人实时运动轨迹数据．10.13245/j.hust.229388.F008图8绳驱动连续体机器人实验平台本研究通过三个轨迹的跟踪任务，即圆、空间折线和螺旋轨迹来验证QPSO-MPC控制器的性能，并与MPC控制器进行比较．每组任务都是以机器人不受驱动力作用(四根牵引绳索无牵引力)为运动起点，在一定时间内沿参考轨迹运动，结果如图9所示．图9表明三组实验中QPSO-MPC控制器具有更好的轨迹跟踪能力．10.13245/j.hust.229388.F009图9QPSO-MPC和MPC控制器轨迹跟踪实验结果将三组实验误差量化为机器人末端执行器在每个采样点的实际位置与参考轨迹的欧氏距离的均值，结果如表1所示，QPSO-MPC，MPC控制器平均误差分别为2.004 cm和3.059 cm，方差分别为0.007和0.272．与MPC控制器相比，前者平均误差减少了52.64%，实验结果表明：QPSO-MPC控制器不仅具有更好的轨迹跟踪性能，而且控制稳定性也更突出．10.13245/j.hust.229388.T001表1轨迹跟踪误差及方差控制器不同轨迹误差/cm误差均值/cm方差圆空间折线螺旋轨迹QPSO-MPC2.1001.9711.9432.0040.007MPC3.5162.4913.1693.0590.2724 结论a．基于分段常曲率法建立了绳驱动连续体机器人的运动学模型．b．将MPC算法与QPSO优化方法相结合，用QPSO代替MPC的二次规划求解器，设计了基于QPSO-MPC的绳驱动连续体机器人的控制系统，提高了系统稳定性和控制精度．c．通过搭建机器人实验平台进行实验验证，QPSO-MPC控制器比传统MPC控制器具有更好的轨迹跟踪控制效果，平均误差减少了52.64%，实验方差更小．证明了本研究提出的基于QPSO-MPC控制器跟踪精度更高，稳定性更好．