(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211116746.X (22)申请日 2022.09.14 (71)申请人 浙江智马达智能科技有限公司 地址 315336 浙江省宁波市杭州湾新区众 创二路7号 I栋B区 申请人 智马达 （上海） 机 器人科技有限公司 (72)发明人 黄斌　王小耘　蔡闻兵　陈大宇　 (74)专利代理 机构 北京智汇东方知识产权代理 事务所(普通 合伙) 11391 专利代理师 赵云秀 (51)Int.Cl. G06F 17/16(2006.01) G06F 17/18(2006.01) (54)发明名称 一种运动轨 迹拟合方法、 控制器及车辆 (57)摘要 本发明提供了一种运动轨迹拟合方法、 控制 器及车辆， 属于运动轨迹拟合技术领域。 该方法 包括： 获取m个数据点(Fi， ti)， 并将m个数据点分 为n段， ti为第i个时间点， Fi为目标参数； 用t的 多次多项式表 示第j段数据对应的拟合的目标参 数Fj(t)； 以Fj(ti)尽量接近Fi和多次多项式的多 次项系数尽量小为目标设定目标函数； 利用矩阵 变换的方式将目标函数转换成二次函数； 采用二 次规划算法求解二次函数； 判断Fj(ti)与Fi的误 差或误差的数学处理值是否小于相应的设定值； 若是， 拟合结束； 否则， 将n更新为n+1， 重新依次 对n段数据分别进行拟合， 直至误差满足条件。 本 发明的运动轨迹拟合方法能够提高拟合结果的 稳定性。 权利要求书2页 说明书5页 附图2页 CN 115374401 A 2022.11.22 CN 115374401 A 1.一种运动轨 迹拟合方法， 其特 征在于， 包括： 获取待拟合的m个数据点(Fi， ti)， 并将m个 数据点(Fi， ti)分为n段， 其中， i＝1， 2， …， m， n 的初始值为1， ti为第i个时间点， Fi为ti对应的目标参数， 所述目标参数为运动目标的运动 参数； 用t的多次多项式表示第j段数据对应的拟合的目标参数Fj(t)， 其中， t为时间， j＝1， 2，…， n； 以Fj(ti)尽量接近Fi和所述多次多 项式的多次项系数尽量小为目标设定目标函数； 利用矩阵变换的方式将所述目标函数转换成二次函数； 采用二次规划算法求 解所述二次函数， 以得到所述多次多 项式的各个项的系数； 判断Fj(ti)与Fi的误差或误差的数 学处理值是否小于相应的设定值； 若是， 拟合结束； 否则， 将n更新为n+1， 重新依次对n段数据分别进行拟合， 直至Fj(ti)与对应的Fi的误差 或误差的数 学处理值是否小于相应的设定值。 2.根据权利要求1所述的运动轨 迹拟合方法， 其特 征在于， 所述运动参数包括横向位移、 纵向位移、 转向角、 横向速度、 纵向速度、 横向加速度和纵 向加速度。 3.根据权利要求2所述的运动轨迹拟合方法， 其特征在于， 以Fj(ti)尽量接近Fi和所述 多次多项式的多次项系数尽量小为目标设定目标函数的步骤 包括： 将目标函数设定为以下公式： 其中， mu为 设定的系数值， ajp是第j段数据对应的多次多项式的第 p项的系数， P+1为第j 段数据对应的多次多 项式的项数。 4.根据权利要求3所述的运动轨迹拟合方法， 其特征在于， 利用矩阵变换的方式将所述 目标函数转换成二次函数的步骤 包括： 令矩阵B＝(bi， q)m， 2， 其中， q＝1或2， bi， 1＝ti2， bi， 2＝ti3； C＝(ci)n， 1， 其中， ci＝Fi‑F0‑v0*ti； z＝(aj2，…， ajP)T， 其中， T表示矩阵转置； 将B、 C和z代入所述目标函数 得到所述 二次函数为： min(||Bz‑C||2+mu*||z||2)。 5.根据权利要求4所述的运动轨迹拟合方法， 其特征在于， 采用二 次规划算法求解所述 二次函数， 以得到所述多次多 项式的各个项的系数的步骤之前还 包括： 以起始点的数据为约束计算所述多次多 项式的aj0和aj1。 6.根据权利要求5所述的运动轨迹拟合方法， 其特征在于， 采用二 次规划算法求解所述 二次函数， 以得到所述多次多 项式的各个项的系数的步骤之后还 包括： 在t0≠0时， 将求 解得到的多次多 项式平移t0。 7.根据权利要求6所述的运动轨迹拟合方法， 其特征在于， 判断F(ti)与Fi的误差或误差 的数学处理值是否小于相应的设定值的步骤 包括：权　利　要　求　书 1/2 页 2 CN 115374401 A 2计算F(ti)与Fi的均方根 误差； 判断所述均方根 误差是否小于第一设定值。 8.根据权利要求1 ‑7中任一项所述的运动轨 迹拟合方法， 其特 征在于， 所述多次多 项式为3次多 项式。 9.一种控制器， 其特征在于， 包括存储器和 处理器， 所述存储器内存储有控制程序， 所 述控制程序被所述处理器执行时用于实现根据权利要求 1‑8中任一项 所述的运动轨迹拟合 方法。 10.一种车辆， 其特 征在于， 包括权利要求9所述的控制器。权　利　要　求　书 2/2 页 3 CN 115374401 A 3