[发明专利]联动轨迹捕获实验中主体和分离体解耦机构运动分配方法

权利要求书

1.联动轨迹捕获实验中主体和分离体解耦机构运动分配方法，其特征在于，所述的运动分配方法使用的解耦机构包括串联或并联的主体机构和分离体机构；

所述的主体机构具有六个自由度，分别由串联的六部分机构来控制，包括主体X向直线移动机构、主体Y向直线移动机构、主体Z向直线移动机构、主体α俯仰转动机构、主体β偏航转动机构和主体γ滚转机构；主体X向直线移动机构、主体Z向直线移动机构、主体Y向直线移动机构均由电机与丝杠配合驱动，主体α俯仰转动机构、主体β偏航转动机构通过电机与直线变圆弧机构实现，主体γ滚转机构由滚转电机配合减速器驱动；主体模型固定在主体机构执行末端的主体支杆上，主体模型的质心位于主体机构的解耦点，主体机构状态为主体解耦机构状态；

所述的分离体机构具有六个自由度，分别由串联的六部分机构来控制，包括分离体X向直线移动机构、分离体Y向直线移动机构、分离体Z向直线移动机构、分离体α俯仰转动机构、分离体β偏航转动机构和分离体γ滚转机构；分离体X向直线移动机构、分离体Z向直线移动机构、分离体Y向直线移动机构均由电机与丝杠配合驱动，分离体α俯仰转动机构、分离体β偏航转动机构通过电机与直线变圆弧机构实现，分离体γ滚转机构由滚转电机配合减速器驱动；分离体模型固定在分离体机构执行末端的分离体支杆上，分离体模型的质心位于分离体机构的解耦点，分离体机构状态为分离体解耦机构状态；

所述的运动分配方法包括以下步骤：

a.采集主体模型和分离体模型当前时刻的当前位姿；

主体模型当前位姿B_Zo＝(x_Zo，y_Zo，z_Zo，α_Zo，β_Zo，θ_Zo)，分离体模型当前位姿B_Fo＝(x_Fo，y_Fo，z_Fo，α_Fo，β_Fo，θ_Fo)；

b.通过当前时刻的当前位姿的主体模型和分离体模型的气动力数据，计算主体模型和分离体模型下一时间步长的理论位姿；

主体模型理论位姿B_Zn＝(x_Zn，y_Zn，z_Zn，α_Zn，β_Zn，θ_Zn)，分离体模型理论位姿B_Fn＝(x_Fn，y_Fn，z_Fn，α_Fn，β_Fn，θ_Fn)；

c.从当前位姿到达理论位姿，主体机构的三个角度自由度机构的运动角度分别为α_Zn-α_Z0，β_Zn-β_Z0，θ_Zn-θ_Z0；分离体机构的三个角度自由度机构的运动角度分别为α_Fn-α_F0，β_Fn-β_F0，θ_Fn-θ_F0；

d.计算当前位姿和理论位姿状态下，主体模型和分离体模型的X向、Y向、Z向相对距离和相对距离变动量；

X向，主体模型当前位姿和分离体模型当前位姿相对距离x_Fo-x_Zo、主体模型理论位姿和分离体模型理论位姿相对距离x_Fn-x_Zn，相对距离变动量Δx＝x_Fn-x_Zn-x_Fo+x_Zo；

Y向，主体模型当前位姿和分离体模型当前位姿相对距离y_Fo-y_Zo、主体模型理论位姿和分离体模型理论位姿相对距离y_Fn-y_Zn，相对距离变动量Δy＝y_Fn-y_Zn-y_Fo+y_Zo；

Z向，主体模型当前位姿和分离体模型当前位姿相对距离z_Fo-z_Zo、主体模型理论位姿和分离体模型理论位姿相对距离z_Fn-z_Zn，相对距离变动量Δz＝z_Fn-z_Zn-z_Fo+z_Zo；

e.判定从当前位姿到达理论位姿，Δx、Δy、Δz的正负；

若Δx为正，表示分离体模型相对主体模型往X正方向计划运动的距离为|Δx|，反之，表示分离体模型相对主体模型往X负方向计划运动|Δx|；

若Δy为正，表示分离体模型相对主体模型往Y正方向计划运动的距离为|Δy|，反之，表示分离体模型相对主体模型往Y负方向计划运动|Δy|；

若Δ_z为正，表示分离体模型相对主体模型往Z正方向计划运动的距离为|Δz|，反之，表示分离体模型相对主体模型往Z负方向计划运动|Δz|；

f.计算主体模型当前位姿和分离体模型当前位姿在X向、Y向、Z向的负端余量和正端余量；

将主体机构的X向、Y向、Z向的运动行程分别表示为[x_Zmin，x_Zmax]、[y_Zmin，y_Zmax]、[z_Zmin，z_Zmax]，则主体机构X向、Y向、Z向的负端余量分别为x_Zo-x_Zmin、y_Zo-y_Zmin、z_Zo-z_Zmin，正端余量分别为x_Zmax-x_Zo、y_Zmax-y_Zo、z_Zmax-z_Zo；

将分离体机构的X向、Y向、Z向的运动行程分别表示为[x_Fmin，x_Fmax]、[y_Fmin，y_Fmax]、[z_Fmin，z_Fmax]，则分离体机构X向、Y向、Z向的负端余量分别为x_Fo-x_Fmin、y_Fo-y_Fmin、z_Fo-z_Fmin，正端余量分别为x_Fmax-x_Fo、y_Fmax-y_Fo、z_Fmax-z_Fo；

g.根据Δx、Δy、Δz的正负，分别计算从当前位姿到达理论位姿，主体模型和分离体模型在X向、Y向、Z向的对比余量；

若Δx为正，对比余量x_Fmax-x_Fo≥x_Zo-x_Zmin成立，则分离体模型正向移动|Δx|；否则，主体模型负向移动|Δx|；

若Δx为负，对比余量x_Fo-x_Fmin≥x_Zmax-x_Zo成立，则分离体模型负向移动|Δx|；否则，主体模型正向移动|Δx|；

若Δy为正，对比余量y_Fmax-y_Fo≥y_Zo-y_Zmin成立，则分离体模型正向移动|Δy|；否则，主体模型负向移动|Δy|；

若Δy为负，对比余量y_Fo-y_Fmin≥y_Zmax-y_Zo成立，则分离体模型负向移动|Δy|；否则，主体模型正向移动|Δy|；

若Δz为正，对比余量z_Fmax-z_Fo≥z_Zo-z_Zmin成立，则分离体模型正向移动|Δz|；否则，主体模型负向移动|Δz|；

若Δz为负，对比余量z_Fo-z_Fmin≥z_Zmax-z_Zo成立，则分离体模型负向移动|Δz|；否则，主体模型正向移动|Δz|；

h.控制主体机构和分离体机构分别完成步骤c的角度运动和步骤g的X向、Y向、Z向运动，到达下一时间步长的主体模型理论位姿和分离体模型理论位姿。