[发明专利]一种用于飞行拍摄效率最优的柔性速度规划方法

权利要求书

1.一种用于飞行拍摄效率最优的柔性速度规划方法，其特征在于，该方法首先根据电机性能约束设置运行加加速度、加速度以及运行中最大速度等信息，基于电机参数约束，求解速度区间可行域，以便于速度曲线的设计；然后根据边界速度约束，计算S型速度曲线加/减速段运行时间与所需位移；其次基于给定位移约束，以寻求最大运行速度为目标，更新S型速度曲线加、减速段运行时间，并计算匀速段时间，从而得到整个柔性速度曲线的时间分配关系；然后依据速度时间关系，规划点到点的运行速度；最后基于视觉检测的二维位置偏差，主运动方向采用效率最优的速度规划方法补偿，次运动方向以最优平稳性为目标设计速度曲线进行位置补偿；方法的具体步骤如下：

步骤1：基于电机参数约束的速度区间可行域求解

在高速贴片机的运行过程中，可将其视为二维平面内多次点到点运动；以其中一次点到点运动为例，起点时间定义为t_s，终点时间定义为t_e，从起始点到终点运行过程中任意时间t∈[t_s,t_e]，运行轨迹可以定义为p(t)＝[x(t),y(t)]；起始点与终点的运行受到速度与加速度的约束，假定点到点运行的起始位置A＝p(t_s)，速度与加速度约束为E_con＝[v(t_s),a(t_s)]＝[v_a,a_a]，终点位置B＝p(t_e)，速度与加速度约束为E_con＝[v(t_e),a(t_e)]＝[v_b,a_b]，两点之间的位移为x_ab；为了更加柔顺的从点A快速运动到点B，极大的发挥电机的运行性能，合理规划点A到点B之间的速度尤为重要；电机的加加速度为J_a、减加速度J_d，最大加速度为a_a,max，最大减速度a_d,max，运行过程中电机的最大速度v_max，点到点运动时的运动学参数需满足电机性能要求；

式中，J_lim、a_lim、v_lim表示电机的极限性能参数，由电机出厂参数决定；则运行速度与加速度满足约束max(v_a,v_b)∈[0,v_max]，max(a_a,max,a_b,max)∈[0,a_lim]；

通过计算起始点与结束点的边界速度约束，进而得到速度可行域；对于加速度过程，速度时间之间的关系满足：

式中，v(t)表示在t时刻时的速度；为了求出速度的上、下限值，需要将最大加、减速度代入方程(2)，得到速度的不等式关系：

v(t)≤v_a+a_a,maxt (3)

v(t)≥v_a-a_d,maxt (4)

在减速段运行过程中，通过逆向求解，得到减速过程中速度与时间的关系：

式中，表示从终点位置向前的逆向计时；因此，通过逆向求解得到减速段时速度的不等式关系：

基于此，计算出起始和结束位置的速度约束包络曲线；考虑电机的最大运行速度与给定位移的关系，在给定位移约束下，可能存在达到最大速度和未达到最大速度两种情况；由此，根据直线电机的参数得到速度可行域，所设计的速度曲线应满足在可行区间范围内；

步骤2：边界速度约束的加/减速段时间精确求解方法；

起始点A的速度是v_a，结束点B的速度是v_b，当v_a＜v_b时，从广义上将AB段视为加速度段；为使运行过程平稳，电机在A点以初速度v_a按S型速度曲线加速运行到B点时，此时速度为v_b；将S型速度曲线将其分为三个阶段，即增加加速度阶段t₁、恒定加速度阶段t₂、减小加速度阶段t₃，且有T_i＝∑(t_i)；根据电机性能约束，计算得到三个阶段的加速度a、速度v、位移x与时间t的表达式；

当t∈[0,T₁]时，

a(t)＝J_at (8)

由此，对于给定的加速段的加加速度J_a和最大加速度a_a,max，可以根据公式(8)计算得到t₁，进而可根据公式(9)-(10)得到v(t₁)和x(t₁)；

当t∈[T₁,T₂]时，

a(t)＝a_a,max (11)

v(t)＝v(T₁)+a_a,max(t-T₁) (12)

当t∈[T₂,T₃]时，

a(t)＝a(T₂)-J_a(t-T₂) (14)

由公式(8)-(16)，可计算得到S型速度曲线加速段的时间满足如下公式

当时，

当时，

由此，计算出从点A到点B加速过程中的时间序列t_i，i＝1,2,3，根据此时间序列，计算出加速段所需的位移：

式中，v_a、v_b表示加速段的起始点与结束点的速度，t₁、t₂和t₃表示加速段各段时间；x_amin表示加速区域从A点到B点的最小位移，也就是说，一旦运行距离小于x_amin，直线电机就不能以S型速度曲线完成加速过程；

同理，计算得到S型速度曲线减速段的时间满足公式为：

当时，

当时，

根据此时间段，逆向求解计算出减速段从A点到B点所需的最小位移：

步骤3：基于位移约束的效率最优S型速度曲线时间段更新；

为了充分缩短点A到点B运行时间，提高运行效率，在运行过程中应达到最大速度；假设给定的位移x_ab足以先从A点以v_a柔性加速到最大速度v_max，然后减速到B点，此时速度v_b；根据步骤2中公式(17)-(18)计算从点A到最大速度v_max的加速时间段t₁、t₂和t₃，以及从最大速度v_max到点B的减速时间段t₅、t₆和t₇，根据公式(19)、(22)计算出此时加速段以及减速段所需要的位移为：

式中，x_amax和x_dmax分别为达到最大速度时加、减速段的最大运行位移；

考虑到给定的实际距离可能不足以加速到最大速度；因此，根据A、B两点给定的位移x_ab，将其分为三种情况：

情况一：给定位移x_ab＜x_amin

考虑起始点速度v_a和结束点速度v_b的边界速度约束，若v_a＜v_b，直线电机以最大加加速度启动，仍无法在当前位移约束下从点A加速运行到点B；若v_a＞v_b，直线电机以最大减加速度启动，仍无法在当前位移约束下从点A减速运行到点B；所以当x_ab＜x_amin，直线电机不能以S型速度曲线完成加减速过程，这需要在实际运动控制中加以考虑；

情况二：位移x_amin＜x_ab＜x_amax+x_dmax

从点A运行到点B时，无法先达到最大速度，此时加速度没有达到电机设定的最大加速度；为使效率最优为目标，设运行可达到的最大速度为v′_max；根据公式(17)-(18)可计算从点A加速到最大速度v′_max的加速时间段t₁、t₂和t₃，以及从最大速度v′_max减速到点B的减速时间段t₅、t₆和t₇；进而利用公式(19)计算加速段所需位移x_a以及减速段所需位移x_d为：

根据给定位移约束x_ab，得到加减速段位移满足的公式为：

x_ab＝x_a+x_d (27)

利用二分法求解上式精确得到运行中达到的最大速度v′_max，然后根据公式(17)-(18)计算出加速时间(t₁,t₂,t₃)和减速时间(t₅,t₆,t₇)；

情况三：位移x_ab＞x_amax+x_dmax

从点A运行到点B时，可达到最大速度v_max；根据公式(17)-(18)计算从点A到最大速度v_max的加速时间段t₁、t₂和t₃，以及从最大速度v_max到点B的减速时间段t₅、t₆和t₇，根据公式(23)-(24)计算加速段的位移x_amax与减速段位移x_dmax，由此计算得到匀速段运行时间：

根据线性电机的性能参数，考虑边界速度和位移约束，合理地得到时间序列t_i，i＝1,...,7；

步骤4：基于S型速度曲线的点到点速度规划与位置补偿方法；

通过步骤3中基于位移约束下的S型速度曲线时间段求解方法计算出S型速度曲线各段时间，然后根据S型速度曲线的加加速度与时间之间的关系为：

对加加速度J进行一次积分、二次积分和三次积分，分别得到兼顾效率与平稳性的点到点运动的加速度-时间、速度-时间、位移-时间表达式为：

基于以上分析，通过公式(30)-(32)可以确定任意时刻t_i电机的位置x_i、实时速度v_i以及此时加速度a_i；据此，实现直线电机在点对点运行过程中的位置实时控制；对于在相机位姿检测过程中获得的位置偏差Δx、Δy，在设定位移的基础上予以补偿；在主运动方向上根据步骤2，步骤3的方法计算S型速度曲线各段时间，然后利用步骤4的点到点速度规划方法进行位置补偿；在次运动方向上，为了避免补偿时引起该方向的速度波动，更新S型速度曲线各段时间与主运动方向上一致，以最优平稳性进行位置偏差的补偿。