【技术实现步骤摘要】
轨迹多项式生成装置及其轨迹序列控制器
[0001]本专利技术涉及一种电子电路,且特别涉及一种轨迹多项式生成装置及其轨迹序列控制器。
技术介绍
[0002]在机器人导航过程中,如何控制机器人从A点移动到B点,通常称之为运动规划。运动规划一般又分为两步:路径规划和轨迹规划。所谓路径规划是指,在地图中搜索一条从A点到B点的运动路径,其中运动路径由一系列离散的路径点(waypoint)组成。运动路径的路径点可能比较稀疏(不平滑)。为了能更好的控制机器人运动,需要将稀疏的路径点变成平滑的曲线或稠密的轨迹点,也就是轨迹规划。如何实现硬件去加速生成多个路径点中任意两个相邻路径点之间的曲线的系数(coefficient),从而减少轨迹规划中的实时计算延迟,是机器人
的诸多重要课题之一。
技术实现思路
[0003]本专利技术提供一种轨迹多项式生成(trajectory polynomial generation)装置及其轨迹序列控制器(trajectory sequential controller),以调度轨迹浮点计算电路去计算运动路径的多个路径点(waypoint)中任意两个相邻路径点之间的曲线的系数(coefficient)。
[0004]在根据本专利技术的实施例中,所述轨迹序列控制器包括轨迹序列生成器(trajectory sequential generator)、准备(prepare)电路、前进(forward)电路、后进(backward)电路以及生成(generation)电路。准备电路耦 ...
【技术保护点】
【技术特征摘要】
1.一种轨迹序列控制器,用以调度轨迹浮点计算电路去计算运动路径的多个路径点中任意两个相邻路径点之间的曲线的系数,其特征在于,所述轨迹序列控制器包括:轨迹序列生成器;准备电路,耦接至所述轨迹序列生成器,用以基于所述轨迹序列生成器的激活去调度所述轨迹浮点计算电路计算所述多个路径点中任意两个相邻路径点之间的x轴步长,以及调度所述轨迹浮点计算电路使用所述x轴步长与所述多个路径点的坐标准备三次样条插值算法的线性方程组中的系数矩阵与常数矢量;前进电路,耦接至所述轨迹序列生成器,用以基于所述轨迹序列生成器的激活去调度所述轨迹浮点计算电路使用所述系数矩阵与所述常数矢量计算所述线性方程组中的二次差分值矢量的至少一个元素;后进电路,耦接至所述轨迹序列生成器,用以基于所述轨迹序列生成器的激活去调度所述轨迹浮点计算电路使用所述系数矩阵与所述常数矢量计算所述二次差分值矢量中的其余元素;以及生成电路,耦接至所述轨迹序列生成器,用以基于所述轨迹序列生成器的激活去调度所述轨迹浮点计算电路使用所述x轴步长、所述多个路径点的所述坐标与所述二次差分值矢量计算出所述曲线的所述系数。2.根据权利要求1所述的轨迹序列控制器,其特征在于,当所述三次样条插值算法完成一次计算时,所述轨迹序列生成器发出完成中断信号给外部装置。3.根据权利要求1所述的轨迹序列控制器,其特征在于,所述准备电路从轨迹缓存提取所述多个路径点的所述坐标。4.根据权利要求1所述的轨迹序列控制器,其特征在于,所述轨迹序列生成器依据所述轨迹浮点计算电路的并行度将所述多个路径点分为至少一个批次去进行所述三次样条插值算法。5.根据权利要求4所述的轨迹序列控制器,其特征在于,在所述至少一个批次的任一个中所述准备电路进行多个步骤以计算所述x轴步长、所述系数矩阵与所述常数矢量,其中,在所述多个步骤中的第一步骤,所述准备电路调度所述轨迹浮点计算电路计算属于目前批次的所述多个路径点的多个对应x轴步长,其中所述多个对应x轴步长作为所述系数矩阵的第一部分元素;在所述多个步骤中的第二步骤,所述准备电路调度所述轨迹浮点计算电路计算属于所述目前批次的所述多个路径点中任意两个相邻路径点之间的多个y轴步长;在所述多个步骤中的第三步骤,所述准备电路调度所述轨迹浮点计算电路计算所述多个对应x轴步长的每一个的第一倍数值,以及所述准备电路调度所述轨迹浮点计算电路计算所述多个y轴步长与所述多个对应x轴步长的多个商值;在所述多个步骤中的第四步骤,所述准备电路调度所述轨迹浮点计算电路计算所述第一倍数值中任意两个相邻倍数值的和值,其中所述和值作为所述系数矩阵的第二部分元素;在所述多个步骤中的第五步骤,所述准备电路调度所述轨迹浮点计算电路计算所述多个商值中任意两个相邻商值的差值,其中所述多个商值被所述生成电路所复用;以及在所述多个步骤中的第六步骤,所述准备电路调度所述轨迹浮点计算电路计算所述差
值的每一个的第二倍数值,其中所述第二倍数值作为所述常数矢量的元素。6.根据权利要求4所述的轨迹序列控制器,其特征在于,所述前进电路进行多个迭代以求得所述二次差分值矢量的所述至少一个元素,其中在所述多个迭代中的第i个迭代中,所述前进电路调度所述轨迹浮点计算电路计算tmp=b
i
–
a
i
*c
i
‑1,其中tmp为所述第i个迭代中的中间数,a
i
为所述系数矩阵的第一对角线的第i个元素,b
i
为所述系数矩阵的第二对角线的第i个元素,以及c
i
‑1为所述系数矩阵的第三对角线的第i
‑
1个元素;所述前进电路调度所述轨迹浮点计算电路计算c
i
=c
i
/tmp,其中c
i
为所述系数矩阵的第三对角线的第i个元素;以及所述前进电路调度所述轨迹浮点计算电路计算d
i
=[d
i
–
a
i
*d
i
‑1]/tmp,其中d
i
‑1为所述常数矢量的第i
‑
1个元素,以及d
i
为所述常数矢量的第i个元素。7.根据权利要求4所述的轨迹序列控制器,其特征在于,所述后进电路进行多个迭代以计算所述二次差分值矢量中的所述其余元素,其中在所述多个迭代中的第i个迭代中,所述后进电路调度所述轨迹浮点计算电路计算f
i
=d
i
–
c
i
*f
i+1
,其中f
i
为所述二次差分值矢量的第i个元素,f
i+1
为所述二次差分值矢量的第i+1个元素,d
i
为所述常数矢量的第i个元素,以及c
i
为所述系数矩阵的第三对角线的第i个元素。8.根据权利要求4所述的轨迹序列控制器,其特征在于,所述生成电路进行多个迭代以计算所述曲线的所述系数,其中在所述多个迭代中的第i个迭代中,所述生成电路以第i个路径点的y轴坐标作为所述曲线的常数项系数;所述生成电路调度所述轨迹浮点计算电路计算[(y
i+1
–
y
i
)/h
i
]
–
[(h
i
/2)*f
i
]
–
[(h
i
/6)*(f
i+1
‑
f
i
)]作为所述曲线的一次项系数,其中y
i+1
为第i+1个路径点的y轴坐标,y
i
为第i个路径点的y轴坐标,h
i
为第i+1个路径点与第i个路径点之间的所述x轴步长,f
i
为所述二次差分值矢量的第i个元素,以及f
i+1
为所述二次差分值矢量的第i+1个元素;所述生成电路调度所述轨迹浮点计算电路计算f
i
/2作为所述曲线的二次项系数;以及所述生成电路调度所述轨迹浮点计算电路计算(f
i+1
‑
f
i
)/(6*h
i
)作为所述曲线的三次项系数。9.根据权利要求1所述的轨迹序列控制器,其特征在于,所述系数矩阵为对角矩阵。10.一种轨迹多项式生成装置,用以计算运动路径的多个路径点中任意两个相邻路径点之间的曲线的系数,其特征在于,所述轨迹多项式生成装置包括:轨迹浮点计算电路;轨迹缓存,用以提供所述多个路径点的坐标;以及轨迹序列控制器,耦接至所述轨迹缓存与所述轨迹浮点计算电路,用以调度所述轨迹浮点计算电路去进行三次样条插值算法,以将所述多个路径点的所述坐标转换为所述曲线的所述系数,其中所述轨迹序列控制器调度所述轨迹浮点计算电路计算所述多个路径点中任意两个相邻路径点之间的x轴步长,调度所述轨迹浮点计算电路使用所述x轴步长与所述多个路径点的坐标准备三次样条插值算法的线性方程组中的系数矩阵与常数矢量,调度所述轨迹浮点计算电路使用所述系数矩阵与所述常数矢量计算所述线性方程组中的二次差分值矢量的元素,以及调度所述轨迹浮点计算电路使用所述x轴步长、所述多个路径点的所述坐标与所述二次差分值矢量计算出所述曲线的所述系数。11.根据权利要求10所述的轨迹多项式生成装置,其特征在于,所述轨迹序列控制器包
括:轨迹序列生成器;准备电路,耦接至所述轨迹序列生成器与所述轨迹缓存,用以基于所述轨迹序列生成器的激活去调度所述轨迹浮点计算电路计算所述x轴步长,以及调度所述轨迹浮点计算电路使用所述x轴步长与所述多个路径点的坐标准备所述系数矩阵与所述常数矢量;前进电路,耦接至所述轨迹序列生成器与所述轨迹缓存,用以基于所述轨迹序列生成器的激活去调度所述轨迹浮点计算电路使用所述系数矩阵与所述常数矢量计算所述二次差分值矢量的至少一个元素;后进电路,耦接至所述轨迹序列生成器与所述轨迹缓存,用以基于所述轨迹序列生成器的激活去调度所述轨迹浮点计算电路使用所述系数矩阵与所述常数矢量计算所述二次差分值矢量中的其余元素;以及生成电路,耦接至所述轨迹序列生成器与所述轨迹缓存,用以基于所述轨迹序列生成器的激活去调度所述轨迹浮点计算电路使用所述x轴步长、所述多个路径点的所述坐标与所述二次差分值矢量计算出所述曲线的所述系数。12.根据权利要求11所述的轨迹多项式生成装置,其特征在于,当所述三次样条插值算法完成一次计算时,所述轨迹序列生成器发出完成中断信号给外部装置。13.根据权利要求11所述的轨迹多项式生...
【专利技术属性】
技术研发人员:ꢀ七四专利代理机构,
申请(专利权)人:上海壁仞智能科技有限公司,
类型:发明
国别省市:
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。