[发明专利]基于非线性优化方法实现的移动基座轨迹规划器

摘要

一种基于非线性优化方法实现的移动基座轨迹规划器，属于通信技术领域。本发明的目的是针对移动基座在任务规划过程中对速度提出的需求，采用非线性优化的方法设计了满足需求的基于非线性优化方法实现的移动基座轨迹规划器。本发明采用三次b样条曲线对轨迹进行参数化，且对整个运动时间进行线性缩放以满足对速度、加速度的约束，然后，通过非线性优化求解满足约束的轨迹。本发明讲整个轨迹规划算法封装成标准的输入输出函数，下载到FPGA中运行，构成轨迹规划器，方便了用户的使用。

主权项

一种基于非线性优化方法实现的移动基座轨迹规划器，其特征在于：采用三次B样条曲线对轨迹进行参数化，且对整个运动时间进行线性缩放以满足对速度、加速度的约束，然后，通过非线性优化求解满足约束的轨迹；设任务输出为路径点{(xk，yk，vk)}：①确定速度v的方向：将所有路径点用线段连接起来，各点速度分别在x轴和y轴方向投影分解为(vx，vy)，这样就将路径点(x，y，v)变换为(x，y，vx，vy)；②利用B样条对轨迹进行参数化：将路径点(xk，yk)由Qk表示,对应的速度(vxk，vvk)由Dk表示；其中k＝0，1，......n‑1，n代表路径点的个数；设待求的控制点由Pk(k＝0，1，......2(n+1))表示，a、确定各路径点对应时间参数值首先将全部运动时间归一化到[0,1]，即u‾0=0,u‾n=1]]>d=Σk=1n|Qk-Qk-1|---(1)]]>u‾k=u‾k-1+|Qk-Qk-1|d,k=1,...n-1---(2)]]>b、确定节点向量uk(k＝0，1，......2(n+1)+p):U={0,0,0,0,u‾12,2u‾1+u‾23,u‾1+2u‾23,......u‾n-2+2u‾n-13,u‾n-1+12,1,1,1,1}]]>uk代表节点向量，p代表样条基函数；c、对(Q，D)进行参数化:利用节点向量U＝{u0，u1，......um},和控制点(P0，P1......P2n+1)生成参数化表达式C(u)C(u)=Σi=0nNi,p(u)Pi---(3)]]>u为运动时间t归一化后的参数值；Pi是轨迹的控制点；Ni，p是p次B样条基函数,其具体表达式由下式给出：Ni,0(u)=1ifui≤u<ui+10otherwise---(4)]]>Ni,p=u-uiui+p-uiNi,p-1(u)-ui+p+1-uui+p+1-ui+1Ni+1,p-1(u)---(5)]]>N′i,p=pui+p-uiNi,p-1(u)-pui+p+1-ui+1Ni+1,p-1(u)---(6)]]>其中m＝n+p+1；d、根据给定的路径点(Q，D)可列以下方程组：C(u‾0)=Q0V(u‾0)=D0C(u‾1)=Q1V(u‾1)=D1...C(u‾n)=QnC(u‾n)=Dn---(7)]]>其中即：N0,3(u‾0)P0+...+N2n+1,3(u‾0)P2n+1=Q0N0,3′(u‾0)P0+...+N2n+1,3′(u‾0)P2n+1=D0N0,3(u‾1)P1+...+N2n+1,3(u‾1)P2n+1=Q1N0,3′(u‾1)P0+...+N2n+1,3′(u‾1)P2n+1=D1...N0,3(u‾n)P0+...+N2n+1,3(u‾n)P2n+1=QnN0,3′(u‾n)P0+...+N2n+1,3′(u‾n)P2n+1=Dn---(8)]]>令：则：[N；N′]*P＝[Q；D]     (10)N代表基函数在各路径点的取值；N`代表基函数导数在各路径点的取值；由于对参数值进行了归一化，对应其参数点实际速度为C·(u‾k)=Dkτ---(11)]]>则：[N；N′]*P＝[Q；Dτ]     (12)即：P＝inv([N；N′])*[Q；Dτ]     (13)由此可得控制点P可以由参数τ表示，即：P＝fp(τ)         (14)fp(τ)是在时间参数τ下对应的控制点P的取值；将该控制点P代入公式(3)，则x方向和y方向的轨迹均可由参数τ和归一化后的时间参数值u表示，即：x=Cx(τ,u)y=Cy(τ,u)---(15);]]>③约束运动基座存在以下物理约束：速度约束：v≤vmax    (16)其中：加速度约束：a≤amax    (18)其中④非线性优化求解满足约束的轨迹：将轨迹规划问题转化为以下非线性优化问题，选择最小能量优化问题:、min∫01(v(τ,u)*a(τ,u))2du---(20)]]>v(τ，u)代表时间参数τ和归一化时间参数值u下的速度；a(τ，u)代表时间参数τ和归一化时间参数值u下的加速度；subject tov≤vmaxa≤amax---(21)]]>将τ做为优化变量求解该非线性优化问题。