CN113771046A - 一种最小化Jerk指标摆动轨迹规划方法 - Google Patents
一种最小化Jerk指标摆动轨迹规划方法 Download PDFInfo
- Publication number
- CN113771046A CN113771046A CN202111241375.3A CN202111241375A CN113771046A CN 113771046 A CN113771046 A CN 113771046A CN 202111241375 A CN202111241375 A CN 202111241375A CN 113771046 A CN113771046 A CN 113771046A
- Authority
- CN
- China
- Prior art keywords
- swing
- track
- leg
- robot
- speed
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
本发明提出一种最小化Jerk指标摆动轨迹规划方法,使用当前机器人估计质心速度与期望速度规划摆动落足点,摆动轨迹规划以最小化轨迹Jerk为目标,同时使用当前质心速度与期望速度作为轨迹始末的速度约束,从而规划出满足机器人本身速度状态且加速度平滑连续的摆动轨迹,基于最优Jerk目标保证轨迹总体冲击积分最小。
Description
技术领域
本发明属于机器人运动控制技术领域,具体涉及一种适用于机器人的最小化Jerk指标摆动轨迹规划方法。
背景技术
四足机器人摆动相控制决定机器人是否能够按期望速度进行移动,足端位置轨迹需要实时进行规划并能够基于机器人本体速度与期望速度进行在线调节。现有摆动轨迹规划方法仅能够保证轨迹始末点位置和摆动高度,以目前四足机器人常用摆线轨迹为例,其速度和加速度往往不连续,存在大角度拐点。以力控四足机器人为例,如果加速度存在拐点会对执行器带来较大冲击;以电驱动四足机器人为例,其加速度方向切换时电机也需要换向,高频换向往往会对减速与传动机构带来损坏。因此,需要提出一种新的摆动轨迹规划方法,以在保证加速度平滑时满足位置约束。
发明内容
(一)要解决的技术问题
本发明的目的是提出一种最小化Jerk指标摆动轨迹规划方法,以解决如何在保证加速度平滑时满足位置约束的技术问题。
(二)技术方案
为解决上述技术问题,本发明提出一种最小化Jerk指标摆动轨迹规划方法,其特征在于,该摆动轨迹规划方法包括如下步骤:
S1.已知四足机器人当前质心高度、质心速度,结合给定的期望质心速度,根据以下公式规划在全局坐标系{N}下的落足点位置:
其中,为机器人第i条腿在{N}系下的落足点位置,为机器人第i个胯关节在{N}系下的位置,pcog,z为机器人当前质心高度,vcog为机器人当前质心速度,vcog,d为期望质心速度,Ts为支撑相时间,g为重力加速度;
S2.基于摆动控制坐标系描述对机器人第i条腿的足端位置进行坐标系转换
S2-1.基于机器人结构参数与单腿构型,使用关节角度反馈值,采用运动学正解计算第i条腿足端在胯关节坐标系{H}下的三维位置:
其中,为第i条腿足端在{H}系下的三维位置,f(c1,c2,c3)为对应单腿构型的运动学正解计算函数,根据以下公式进行运动学正解计算,其中c1为机器人大腿角度,c2为小腿角度,c3为胯关节角度,L1为大腿长度,L2为小腿长度,L3为侧摆电机相对胯关节的偏差:
S2-2.基于机器人左右两侧腿胯关节间距和前后两侧腿胯关节间距,将第i条腿足端在{H}系下的三维位置转化为机体坐标系{B}下的三维位置:
S2-3.基于机器人机载IMU测量得到的姿态四元数,获取当前{N}系与{B}系间的转换矩阵:
根据以下公式,将第i条腿足端在{B}系下的三维位置转换为{N}系下的三维位置:
S2-4.设定期望摆动高度Hsw,根据以下公式计算得到第i条腿在{N}系下的摆动中点位置:
S3.基于摆动起始位置、摆动中点位置、落足点位置,构建第i条腿的摆动轨迹位置约束:
其中,Pst为摆动轨迹起始点的位置约束,Pmid为摆动轨迹起中点的位置约束,Pend为摆动轨迹结束点的位置约束;
基于当前质心速度vcog与期望质心速度vcog,d,根据以下公式构建摆动轨迹速度约束;在摆动轨迹轨迹中点不设具体速度值的约束,以保证摆动轨迹中点前后两段轨迹的速度连续,即:
其中,vst为摆动轨迹起始点的速度约束,vend为摆动轨迹结束点的速度约束;
S4.在确定摆动轨迹起点、中点和结束点的位置约束与速度约束后,以最小轨迹Jerk求解作为轨迹曲线函数的三次多项式的最优化多项式系数,保证轨迹加速度连续,构建连续前馈力矩,减小机器人摆动控制中的电机冲击;
S5.摆动轨迹由前后两段组成,采用步骤S3中的摆动轨迹位置约束条件和速度约束条件构建如下关系式:
其中,n为轨迹函数阶次,t0为摆动轨迹前半段时间,t1为摆动轨迹中点时间,t2为摆动轨迹后半段时间;
S6.求解步骤S5中关系式的未知项X,X作为轨迹对应的多项式系数,以最小化Jerk为优化目标求取最优的轨迹参数
根据以下公式,对摆动轨迹前后两段的Jerk值分别进行积分,作为优化目标:
其中,T为该段轨迹的时间,k为轨迹分段数量;
S7.基于优化目标,采用极小值原理进行参数求解,最优参数对应的轨迹与时间函数S*(t)如下:
其中,p0、v0、a0为该段轨迹起始点的位置、速度与加速度;
S8.基于步骤S6-S7,根据以下公式求解出摆动轨迹前后两段曲线对应的多项式系数[αst-midβst-midγst-mid]和[αmid-endβmid-endγmid-end]:
结合轨迹多项式函数,得到摆动轨迹前半段和后半段任意时刻t的位置、速度和加速度:
其中,[pst vst ast]为摆动轨迹开始时的位置、速度和加速度,[pmid vmid amid]为摆动轨迹中点的位置、速度和加速度,[pend vend aend]为摆动轨迹终点的位置、速度和加速度,Ts为轨迹总时间。
(三)技术效果
本发明提出一种最小化Jerk指标摆动轨迹规划方法,使用当前机器人估计质心速度与期望速度规划摆动落足点,摆动轨迹规划以最小化轨迹Jerk为目标,同时使用当前质心速度与期望速度作为轨迹始末的速度约束,从而规划出满足机器人本身速度状态且加速度平滑连续的摆动轨迹,基于最优Jerk目标保证轨迹总体冲击积分最小。
附图说明
图1为本发明实施例中摆动轨迹规划方法流程图;
图2为本发明实施例中机器人坐标系描述:(a)机器人右视图,(b)机器人俯视图({H}为胯关节坐标系,{B}为机体坐标系);
图3(a)为本发明实施例中规划的摆动轨迹结果,图3(b)为摆线轨迹与规划的X加速度曲线,图3(c)为Z加速度曲线。
具体实施方式
为使本发明的目的、内容和优点更加清楚,下面结合附图和实施例,对本发明的具体实施方式作进一步详细描述。
本实施例提出一种最小化Jerk指标摆动轨迹规划方法,主要流程如图1所示,具体包括如下步骤:
S1.已知四足机器人当前质心高度、质心速度,结合遥控器给定的期望质心速度,根据以下公式规划在全局坐标系{N}(简称“{N}系”)下的落足点位置:
其中,为机器人第i条腿在{N}系下的落足点位置,为机器人第i个胯关节在{N}系下的位置,pcog,z为机器人当前质心高度,vcog为机器人当前质心速度,vcog,d为期望质心速度,Ts为支撑相时间,g为重力加速度。
S2.基于摆动控制坐标系描述对机器人第i条腿的足端位置进行坐标系转换。
S2-1.基于机器人结构参数与单腿构型,使用关节角度反馈值,采用运动学正解计算第i条腿足端在胯关节坐标系{H}(简称“{H}系”)下的三维位置:
其中,为第i条腿足端在{H}系下的三维位置,f(c1,c2,c3)为对应单腿构型的运动学正解计算函数,参考图2给出的机器人坐标系描述其计算方法,根据以下公式进行运动学正解计算,其中c1为机器人大腿角度,c2为小腿角度,c3为胯关节角度,L1为大腿长度,L2为小腿长度,L3为侧摆电机相对胯关节的偏差:
S2-2.基于机器人左右两侧腿胯关节间距和前后两侧腿胯关节间距,将第i条腿足端在{H}系下的三维位置转化为机体坐标系{B}(简称“{B}系”)下的三维位置:
S2-3.基于机器人机载IMU测量得到的姿态四元数,获取当前{N}系与{B}系间的转换矩阵:
根据以下公式,将第i条腿足端在{B}系下的三维位置转换为{N}系下的三维位置:
S2-4.设定期望摆动高度Hsw,根据以下公式计算得到第i条腿在{N}系下的摆动中点位置:
S3.基于摆动起始位置、摆动中点位置、落足点位置,构建第i条腿的摆动轨迹位置约束:
其中,Pst为摆动轨迹起始点的位置约束,Pmid为摆动轨迹起中点的位置约束,Pend为摆动轨迹结束点的位置约束。
基于当前质心速度vcog与期望质心速度vcog,d,根据以下公式构建摆动轨迹速度约束;在摆动轨迹轨迹中点不设具体速度值的约束,以保证摆动轨迹中点前后两段轨迹的速度连续,即:
其中,vst为摆动轨迹起始点的速度约束,vend为摆动轨迹结束点的速度约束。
S4.在确定摆动轨迹起点、中点和结束点的位置约束与速度约束后,以最小轨迹Jerk求解作为轨迹曲线函数的三次多项式的最优化多项式系数,保证轨迹加速度连续,构建连续前馈力矩,减小机器人摆动控制中的电机冲击。
S5.摆动轨迹由前后两段组成,采用步骤S3中的摆动轨迹位置约束条件和速度约束条件构建如下关系式:
其中,n为轨迹函数阶次,t0为摆动轨迹前半段时间,t1为摆动轨迹中点时间,t2为摆动轨迹后半段时间。
S6.求解步骤S5中关系式的未知项X,X作为轨迹对应的多项式系数,以最小化Jerk为优化目标求取最优的轨迹参数
根据以下公式,对摆动轨迹前后两段的Jerk值分别进行积分,作为优化目标:
其中,T为该段轨迹的时间,k为轨迹分段数量。
S7.基于优化目标,采用极小值原理进行参数求解,最优参数对应的轨迹与时间函数S*(t)如下:
其中,p0、v0、a0为该段轨迹起始点的位置、速度与加速度。
S8.基于步骤S6-S7,根据以下公式求解出摆动轨迹前后两段曲线对应的多项式系数[αst-mid βst-mid γst-mid]和[αmid-end βmid-end γmid-end]:
结合轨迹多项式函数,得到摆动轨迹前半段和后半段任意时刻t的位置、速度和加速度:
其中,[pst vst ast]为摆动轨迹开始时的位置、速度和加速度,[pmid vmid amid]为摆动轨迹中点的位置、速度和加速度,[pend vend aend]为摆动轨迹终点的位置、速度和加速度,Ts为轨迹总时间。
基于本发明进行摆动轨迹规划的结果如图3所示。其中,图3(a)给出本发明与传统摆线轨迹规划的三维曲线结果,本发明和传统方法均能满足起点、中点、终点的轨迹位置约束;图3(b)和3(c)分别为X轴和Z轴轨迹加速度规划结果,可以看出本发明在轨迹中点时加速度能实现连续过度,而传统摆线轨迹会出现加速度突变。
上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明技术原理的前提下,还可以做出若干改进和变形,这些改进和变形也应视为本发明的保护范围。
Claims (1)
1.一种最小化Jerk指标摆动轨迹规划方法,其特征在于,所述摆动轨迹规划方法包括如下步骤:
S1.已知四足机器人当前质心高度、质心速度,结合给定的期望质心速度,根据以下公式规划在全局坐标系{N}下的落足点位置:
其中,为机器人第i条腿在{N}系下的落足点位置,为机器人第i个胯关节在{N}系下的位置,pcog,z为机器人当前质心高度,vcog为机器人当前质心速度,vcog,d为期望质心速度,Ts为支撑相时间,g为重力加速度;
S2.基于摆动控制坐标系描述对机器人第i条腿的足端位置进行坐标系转换
S2-1.基于机器人结构参数与单腿构型,使用关节角度反馈值,采用运动学正解计算第i条腿足端在胯关节坐标系{H}下的三维位置:
其中,为第i条腿足端在{H}系下的三维位置,f(c1,c2,c3)为对应单腿构型的运动学正解计算函数,根据以下公式进行运动学正解计算,其中c1为机器人大腿角度,c2为小腿角度,c3为胯关节角度,L1为大腿长度,L2为小腿长度,L3为侧摆电机相对胯关节的偏差:
S2-2.基于机器人左右两侧腿胯关节间距和前后两侧腿胯关节间距,将第i条腿足端在{H}系下的三维位置转化为机体坐标系{B}下的三维位置:
S2-3.基于机器人机载IMU测量得到的姿态四元数,获取当前{N}系与{B}系间的转换矩阵:
根据以下公式,将第i条腿足端在{B}系下的三维位置转换为{N}系下的三维位置:
S2-4.设定期望摆动高度Hsw,根据以下公式计算得到第i条腿在{N}系下的摆动中点位置:
S3.基于摆动起始位置、摆动中点位置、落足点位置,构建第i条腿的摆动轨迹位置约束:
其中,Pst为摆动轨迹起始点的位置约束,Pmid为摆动轨迹起中点的位置约束,Pend为摆动轨迹结束点的位置约束;
基于当前质心速度vcog与期望质心速度vcog,d,根据以下公式构建摆动轨迹速度约束;在摆动轨迹轨迹中点不设具体速度值的约束,以保证摆动轨迹中点前后两段轨迹的速度连续,即:
其中,vst为摆动轨迹起始点的速度约束,vend为摆动轨迹结束点的速度约束;
S4.在确定摆动轨迹起点、中点和结束点的位置约束与速度约束后,以最小轨迹Jerk求解作为轨迹曲线函数的三次多项式的最优化多项式系数,保证轨迹加速度连续,构建连续前馈力矩,减小机器人摆动控制中的电机冲击;
S5.摆动轨迹由前后两段组成,采用步骤S3中的摆动轨迹位置约束条件和速度约束条件构建如下关系式:
其中,n为轨迹函数阶次,t0为摆动轨迹前半段时间,t1为摆动轨迹中点时间,t2为摆动轨迹后半段时间;
S6.求解步骤S5中关系式的未知项X,X作为轨迹对应的多项式系数,以最小化Jerk为优化目标求取最优的轨迹参数
根据以下公式,对摆动轨迹前后两段的Jerk值分别进行积分,作为优化目标:
其中,T为该段轨迹的时间,k为轨迹分段数量;
S7.基于优化目标,采用极小值原理进行参数求解,最优参数对应的轨迹与时间函数S*(t)如下:
其中,p0、v0、a0为该段轨迹起始点的位置、速度与加速度;
S8.基于步骤S6-S7,根据以下公式求解出摆动轨迹前后两段曲线对应的多项式系数[αst-mid βst-mid γst-mid]和[αmid-end βmid-end γmid-end]:
结合轨迹多项式函数,得到摆动轨迹前半段和后半段任意时刻t的位置、速度和加速度:
其中,[pst vst ast]为摆动轨迹开始时的位置、速度和加速度,[pmid vmid amid]为摆动轨迹中点的位置、速度和加速度,[pend vend aend]为摆动轨迹终点的位置、速度和加速度,Ts为轨迹总时间。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111241375.3A CN113771046B (zh) | 2021-10-25 | 2021-10-25 | 一种最小化Jerk指标摆动轨迹规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111241375.3A CN113771046B (zh) | 2021-10-25 | 2021-10-25 | 一种最小化Jerk指标摆动轨迹规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113771046A true CN113771046A (zh) | 2021-12-10 |
CN113771046B CN113771046B (zh) | 2023-06-30 |
Family
ID=78873498
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111241375.3A Active CN113771046B (zh) | 2021-10-25 | 2021-10-25 | 一种最小化Jerk指标摆动轨迹规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113771046B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114734445A (zh) * | 2022-05-09 | 2022-07-12 | 中兵智能创新研究院有限公司 | 一种四足机器人动态适应负载的控制方法 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120290131A1 (en) * | 2011-05-09 | 2012-11-15 | King Fahd University Of Petroleum And Minerals | Parallel kinematic machine trajectory planning method |
CN109991847A (zh) * | 2019-01-11 | 2019-07-09 | 天津理工大学 | 一种柔性多体机器人近似时间最优轨迹规划方法 |
CN111766885A (zh) * | 2020-07-08 | 2020-10-13 | 清华大学深圳国际研究生院 | 一种四足机器人的静步态规划方法 |
CN111896007A (zh) * | 2020-08-12 | 2020-11-06 | 智能移动机器人(中山)研究院 | 一种补偿足地冲击的四足机器人姿态解算方法 |
CN112147889A (zh) * | 2020-08-31 | 2020-12-29 | 南京理工大学 | 一种四足机器人复合式越障轨迹规划方法 |
CN112757306A (zh) * | 2021-01-25 | 2021-05-07 | 北京交通大学 | 一种机械臂逆解多解选择和时间最优轨迹规划算法 |
CN113246120A (zh) * | 2021-03-08 | 2021-08-13 | 山东大学 | 一种液压四足双臂机器人的遥操作控制方法 |
CN113524177A (zh) * | 2021-06-25 | 2021-10-22 | 南方科技大学 | 一种足式机器人的控制方法 |
-
2021
- 2021-10-25 CN CN202111241375.3A patent/CN113771046B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120290131A1 (en) * | 2011-05-09 | 2012-11-15 | King Fahd University Of Petroleum And Minerals | Parallel kinematic machine trajectory planning method |
CN109991847A (zh) * | 2019-01-11 | 2019-07-09 | 天津理工大学 | 一种柔性多体机器人近似时间最优轨迹规划方法 |
CN111766885A (zh) * | 2020-07-08 | 2020-10-13 | 清华大学深圳国际研究生院 | 一种四足机器人的静步态规划方法 |
CN111896007A (zh) * | 2020-08-12 | 2020-11-06 | 智能移动机器人(中山)研究院 | 一种补偿足地冲击的四足机器人姿态解算方法 |
CN112147889A (zh) * | 2020-08-31 | 2020-12-29 | 南京理工大学 | 一种四足机器人复合式越障轨迹规划方法 |
CN112757306A (zh) * | 2021-01-25 | 2021-05-07 | 北京交通大学 | 一种机械臂逆解多解选择和时间最优轨迹规划算法 |
CN113246120A (zh) * | 2021-03-08 | 2021-08-13 | 山东大学 | 一种液压四足双臂机器人的遥操作控制方法 |
CN113524177A (zh) * | 2021-06-25 | 2021-10-22 | 南方科技大学 | 一种足式机器人的控制方法 |
Non-Patent Citations (1)
Title |
---|
张浩杰;苏治宝;HERNANDEZ D E;苏波;: "移动机器人低能耗最优路径规划方法", 农业机械学报, no. 09 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114734445A (zh) * | 2022-05-09 | 2022-07-12 | 中兵智能创新研究院有限公司 | 一种四足机器人动态适应负载的控制方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113771046B (zh) | 2023-06-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111913490B (zh) | 基于落足调整的四足机器人动步态稳定控制方法及系统 | |
Park et al. | Variable-speed quadrupedal bounding using impulse planning: Untethered high-speed 3d running of mit cheetah 2 | |
Girard | Interactive design of 3-D computer-animated legged animal motion | |
EP1559519B1 (en) | Robot device, motion control device for robot device and motion control method | |
KR101029228B1 (ko) | 로봇과 그 제어 방법 | |
US8868239B2 (en) | Walking control apparatus of robot and method of controlling the same | |
US20050107916A1 (en) | Robot device and control method of robot device | |
CN103149933A (zh) | 基于闭环控制的仿人机器人全向行走方法 | |
JPWO2003011535A1 (ja) | 脚式移動ロボット及びその動作制御方法 | |
CN110405769B (zh) | 一种启发式仿人机器人摔倒保护方法 | |
CN111240339A (zh) | 一种双足机器人的仿人步态规划方法 | |
WO2003068454A1 (fr) | Robot androide marchant sur deux jambes | |
Verrelst et al. | Dynamically stepping over obstacles by the humanoid robot HRP-2 | |
CN101068662B (zh) | 腿式移动机器人及其控制方法 | |
CN113771046A (zh) | 一种最小化Jerk指标摆动轨迹规划方法 | |
JP2011161524A (ja) | 2脚歩行ロボット及びそのための歩容データ生成方法 | |
CN109739094A (zh) | 一种基于自适应滑模控制的移动机器人轨迹跟踪新方法 | |
CN114384918A (zh) | 基于落足点调整的四足机器人对角步态柔顺控制方法 | |
JP4483254B2 (ja) | ロボット装置及びロボット装置の制御方法 | |
US20160129591A1 (en) | Controller for mobile robot | |
Vanderborght et al. | Objective locomotion parameters based inverted pendulum trajectory generator | |
JP5035005B2 (ja) | 脚式ロボット、その制御方法、その制御システム | |
JP2004237403A (ja) | 歩行ロボットの制御用データ作成装置とzmp位置の計算方法 | |
CN115167389A (zh) | 行为驱动的四足机器人中枢层细粒度演进优化模型 | |
CN112256028A (zh) | 一种双足机器人柔顺步态控制方法、系统、设备及介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |