1.一种柔性体整体空间位姿检测方法,其特征在于:包括以下步骤:S1:沿柔性体表面分别布置旋向相反的光纤光栅传感器串,组成光纤光栅位姿感知网络;
S2:根据柔性体变曲率特性实时获取其变形数据:驱动柔性体运动,根据光纤光栅传感器串采集到的数据得出柔性体实时变形数据;
S3:通过S2中光纤光栅传感器串的波长数据获取柔性体各栅点的施加力;
S4:基于能量守恒方法构建变曲率柔性体空间位姿检测模型,建立施加力与末端位姿参数的映射关系;
S5:根据S3求得的施加力获取柔性体末端位姿。
2.根据权利要求1所述的柔性体整体空间位姿检测方法,其特征在于:所述步骤S3包括:S301:根据实时变形数据,利用准牛顿法和优化法构建多项式矩阵拟合模型,解耦各栅点处的多维耦合力;
S302:对相邻栅点间的节点施加力利用插值法进行分配,获取第i节的施加力Fi。
3.根据权利要求2所述的柔性体整体空间位姿检测方法,其特征在于:所述S301具体步骤为:由光纤光栅波长λ与光栅周期Λ和有效折射率neff之间的关系λ=2neffΛ可知,沿光纤光栅轴向施加力F1时光栅周期Λ随之变化,沿光纤光栅径向施加力F2时Λ和neff均相应变化,建立第i节第j(j=1,2,3,4)个光纤光栅传感器串的栅点波长与施加力之间的映射关系:其中,h为映射关系,△λ为波长偏移量; 为第i节第j个栅点的应变; 为第n次迭代的轴向力; 为第n次迭代轴向力的Hesse矩阵; 为第n次迭代的径向力;
为第n次迭代径向力的Hesse矩阵;m1和m2分别为轴向和横向的灵敏度系数,表示为:其中,p11和p12为弹光系数;E为光纤材料的弹性模量;μ为光纤材料的泊松比。
4.根据权利要求3所述的柔性体整体空间位姿检测方法,其特征在于:所述S302具体步骤为:两栅点之间的节点施加力利用插值法进行分配,根据同一位置处的两个旋向相反的栅点的径、轴向力相同,轴向力相同 使旋向相反的两栅点周期相同,径向力相同对旋向相反两栅点的有效折射率影响却不同,使得同一位置两栅点的应变不同,由实际波长变化可得,由下式可分别得出两个旋向相反的栅点的径、轴向力:由同一横截面上轴向力相同求解横向力确定各栅点的施加力,进而获得第i节的施加力
5.根据权利要求4所述的柔性体整体空间位姿检测方法,其特征在于:所述S4具体步骤为:S401:计算柔性体每节的拉伸应变能、弯曲应变能和扭转应变能;
S402:根据能量守恒定律构建变曲率柔性体空间位姿检测模型,建立施加力与末端位姿参数的映射关系。
6.根据权利要求5所述的柔性体整体空间位姿检测方法,其特征在于:所述S401具体步骤为:其中,Esi为第i节的拉伸应变能,EAi为第i节的拉伸刚度,esi为第i节s弧长上的拉伸应变;Ebi为第i节的弯曲应变能,EIi为第i节的弯曲刚度,ki为第i节的曲率;Eti为第i节的扭转应变能,GJi为第i节的扭转刚度,ti为第i节的剪切应力,s为弧长;
拉伸应变esi、扭转角 曲率ki和剪切应力ti可由第i节的n个栅点应变得到:柔性载体第i节的拉伸应变esi计算方法为:
柔性载体左右旋向FBG的应变由波长变化得出,同一位置处的扭转角相同,也作为该横截面的扭转角,扭转角和轴向应变在左右旋向中的表达式不同,联立得出扭转角 和轴向应变eij:m和n=1,2,3,且m≠n≠j
柔性载体第i节的曲率ki计算方法为:
m,n=1,2,3且n循环
柔性载体第i节的剪切应力ti计算方法为:
其中,eijFBG为第i节第j个栅点的应变,eij为第i节第j个栅点的轴向应变,ei左FBG为第i节重合栅点处左旋向栅点的应变,ei右FBG同理, 为第i节的扭转角,lij'为第i节第j个栅点所在母线变形后长度,li0为第i节未变形前的母线长度,ri0为第i节未变形前的初始半径,α为光纤光栅所在螺旋槽的螺旋角,μ为柔性载体材料的泊松比。
7.根据权利要求6所述的柔性体整体空间位姿检测方法,其特征在于:所述S402具体步骤为:其中,E为总弹性势能,Ey为总应变能,Ew为总外力势能,Fi表示第i个节点处所受外力,ei3表示第i个节点坐标系zi方向的单位向量;
柔性体在运动过程中处于动态平衡,因此,弹性势能E可得:其中,(αi,βi,γi)表示第i节末端的欧拉角。
8.根据权利要求7所述的柔性体整体空间位姿检测方法,其特征在于:所述S5具体步骤为:S501:采用人工蜂群优化法求解柔性体动态平衡时四元参数的最优解;
S5011:蜜源(四元数解)初始化设置:
蜜源i初始位置为
xid=Ld+rand(0,1)(Ud‑Ld),(i=1,2…N,xid∈(Ld,Ud))式中,xid表示第i组四元数解中第d(d=1,2,3,4)个四元数系数的值;(Ld,Ud)表示xid的上下限;采蜜蜂、观察蜂和蜜源的个数均为N,且采蜜蜂与蜜源一一对应;
S5012:蜜源(四元数解)更新:
根据新旧蜜源的适应度fit对比确定是否更新采蜜蜂搜索的蜜源,计算每个蜜源被观察蜂选择的概率,根据概率大小确定是否更新蜜源;
x′id=xid+rand(‑1,1)(xid‑xkd),(k≠i)式中,x'id表示采蜜蜂在蜜源i初始位置的空间邻域内搜索新的蜜源,若采集蜂对应的旧蜜源适应度fit小于新密源适应fit,被新密源替代;rand(‑1,1)表示[‑1,1]之间的随机数;pi表示轮盘赌模式下第i个蜜源被观察蜂选择的概率;
S5013:记录当前最好蜜源(四元数解)
当前蜜源记为最优蜜源,若当前蜜源的迭代次数大于阈值且其适应度并无增加,则舍去,用新的蜜源Xid替代作为初始值重新初始化,且相应的采集蜂转化为侦察蜂;
式中,k表示采集蜂和观察蜂对当前最好蜜源的搜索次数;limit表示搜索阈值; 和表示第d个四元数系数的上限和下限;
S5014:判断终止条件
当k≥max时,输出最优蜜源,即四元数的解;否则,重复步骤S5011;
S502:利用空间轴角的四元数表示法,根据各节欧拉角求解各节点的位置矢量,从而得到末端位置坐标和姿态角;
z
ri=cos(αi‑α0)r0+[1‑cos(αi‑α0)](uz·r0)r0+sin(αi‑α0)(uz×r0)y z z z zri=cosβiri+(1‑cosβi)(uy·ri)ri+sinβi(uy×ri)x y y y y
ri=cosγiri+(1‑cosγi)(ux·ri)ri+sinγi(ux×ri)式中,(αi,βi,γi)表示第i个节末端的欧拉角,r0表示基坐标原点与软体机械臂固定端的位置矢量,α0表示r0与基坐标XOY平面的夹角,(ux,uy,uz)表示基坐标沿坐标轴方向的单位向量。
9.一种实现柔性体整体空间位姿检测方法的系统,其特征在于,包括光纤光栅传感器串、处理模块、控制模块,所述控制模块控制柔性体的运动,所述光纤光栅传感器串与处理模块连接,所述光纤光栅传感器串安装于柔性体表面,所述处理模块处理光纤光栅传感器串采集到的柔性体数据。
10.根据权利要求9所述的实现柔性体整体空间位姿检测方法的系统,其特征在于,沿柔性体表面同向布置至少三条所述光纤光栅传感器串,并沿着圆周方向均布设置,柔性体的同一横截面上均布有三个栅点,沿任意栅点布置至少一条反向光纤光栅传感器串,共同组成光纤光栅位姿感知网络。