一种考虑高阶约束的机械臂时间最优轨迹规划方法及设备

摘要: 本发明涉及属于工业机器人运动控制与轨迹规划,特别涉及一种考虑高阶约束的机械臂时间最优轨迹规划方法及设备。背景技术:1、目前,随着工业自动化程度的不断提升,串联机...
一种考虑高阶约束的机械臂时间最优轨迹规划方法及设备

本发明涉及属于工业机器人运动控制与轨迹规划,特别涉及一种考虑高阶约束的机械臂时间最优轨迹规划方法及设备。

背景技术:

1、目前,随着工业自动化程度的不断提升,串联机械臂已成为智能制造生产线中的核心执行单元。轨迹规划作为机械臂运动控制系统的核心环节,直接决定了机械臂的作业效率、运动平稳性与运行安全性。其中,时间最优轨迹规划旨在保证机械臂满足所有物理约束的前提下,使末端执行器沿指定路径的运动总时间最短,是提升生产线节拍、降低生产周期成本的关键技术。

2、在实际工业应用中,机械臂的运动受到多维度物理约束的限制:一是运动学约束,包括关节最大角速度、最大角加速度,以及影响运动冲击与振动的高阶约束 —— 关节jerk(角加加速度);二是动力学约束,即关节电机的输出力矩存在额定上限,由机械臂刚体动力学模型中的惯性力、科里奥利离心力与重力共同决定。如何在同时满足上述多层约束的前提下求解时间最优轨迹,是轨迹规划领域的核心研究问题。

3、现有时间最优轨迹规划技术主要分为三类:

4、第一类是基于砰砰原理的经典数值方法,其核心思想是时间最优解必然处于约束边界上,加速度在最大、最小值之间切换。这类方法早期仅考虑速度与加速度约束,求解效率较高,但无法处理复杂的动力学约束与高阶jerk约束,且易出现控制量高频切换的问题,加剧机械磨损。

5、第二类是基于参数化的凸优化方法,通过引入路径参数将时间维度与空间路径解耦,将轨迹规划问题转化为参数域上的优化问题。部分研究通过变量替换将速度、加速度与动力学力矩约束转化为凸约束,采用二阶锥规划或线性规划求解。这类方法保证了全局最优性,但普遍忽略jerk约束,导致实际运动中冲击大、振动明显,定位精度下降;若强行加入jerk约束,则会破坏问题的凸性,无法直接用凸优化求解器高效求解。

6、第三类是序列凸优化方法,针对非凸约束通过逐次线性近似或凸近似的方式迭代求解。这类方法理论上可以处理jerk等高阶非凸约束,但在实际应用中存在三个显著特点:

7、1、采样方式均匀化:普遍采用参数域均匀离散化构造离散采样点,对于曲率变化剧烈的路径段,均匀采样无法捕捉动力学特性的快速变化,易产生约束截断误差,导致实际轨迹超限;若全局加密采样点,则会大幅增加约束规模,求解效率急剧下降。

8、2、初始点构造困难:序列凸优化的收敛性高度依赖初始可行点,现有方法多采用保守的低速初始化或随机初始化,前者距离最优解过远、迭代次数多,后者无法保证可行性,易导致算法发散。

9、3、信赖域策略适配性差:传统信赖域多基于目标函数的下降率调整步长,但时间最优问题经等效转换后目标函数为纯线性形式,下降率准则失效;固定步长的信赖域则无法兼顾收敛速度与近似精度,步长过大易出现物理违规,步长过小收敛缓慢。

10、此外,现有方法对起止点零速度、零加速度的边界条件,多通过额外添加等式约束实现,增加了约束矩阵规模,进一步降低了求解效率,难以满足工业场景的实时性需求。

11、综上,现有机械臂时间最优轨迹规划技术存在以下核心缺陷:

12、均匀离散采样机制无法兼顾求解精度与计算效率,高曲率路径段约束校验精度不足,全局加密又导致计算量剧增;

13、高阶jerk约束的强非凸性难以融入凸优化框架,忽略该约束会导致运动冲击大,采用非线性求解则效率低、最优性无法保证;

14、序列优化方法缺乏高质量可行初始点构造手段,收敛稳定性差、迭代次数多;

15、传统信赖域策略不适用于线性目标的序列优化场景,无法在物理可行性与收敛速度之间实现自适应平衡;

16、边界条件处理方式冗余,增加了优化问题的规模与求解复杂度。

技术实现思路

1、本发明为解决公知技术中存在的技术问题而提供一种考虑高阶约束的机械臂时间最优轨迹规划方法及设备。

2、本发明为解决公知技术中存在的技术问题所采取的技术方案是:

3、一种考虑高阶约束的机械臂时间最优轨迹规划方法,基于路径曲率进行非均匀采样,得到自适应分布离散采样点;通过路径参数化解耦空间与时间维度,引入伪状态变量作为时间与空间的中介参数,将非线性的时间域多维物理约束整体映射为参数域内的线性包络线空间;将非凸约束通过局部泰勒展开逐次线性化;构建等效线性化的优化目标函数;结合基于物理违规量驱动的动态信赖域策略,构建序列线性规划求解闭环;基于解析缩放的方法生成序列线性规划迭代的初始控制点,最终收敛得到满足全维度约束的时间最优轨迹。

4、进一步地,该方法包括如下步骤:

5、步骤1,给定任务空间的目标几何路径后,通过机械臂逆运动学模型,将空间路径映射为关节空间的连续平滑曲线,该曲线以归一化参数表示,∈[0,1],分别对应路径的起点与终点;

6、步骤2,根据关节空间路径的曲率特性、任务空间路径曲率和/或动力学力矩变化率,通过逆变换采样生成自适应分布的离散采样点;基于离散采样点处的几何参数,分别构建原始的关节空间的速度、加速度、动力学力矩约束;

7、步骤3,定义参数速率为归一化参数对时间的导数,即,引入伪状态变量,将其定义为参数速率的平方:;伪状态变量采用满足线性叠加与可微特性的样条曲线进行参数化表示;

8、步骤4,基于复合函数链式求导法则,将原始的关节空间的速度、加速度、动力学力矩约束表达式均转换为伪状态变量及其导数的表达式,结合样条的线性特性,使所有约束线性化,并使边界条件降维;

9、步骤5,将最小化总时间作为路径优化目标,基于样条曲线将路径优化目标函数等价变换为最大化伪状态变量积分;将路径优化问题转化为线性松弛问题,求解松弛问题得到基准控制点向量;

10、步骤6,将基准控制点向量代入原始jerk公式,扫描所有离散采样点,计算得到jerk最大峰值;推导得到关于全局缩放因子的解析缩放公式,通过该解析缩放公式计算全局缩放因子,由全局缩放因子计算得到初始控制点;

11、步骤7,基于物理违规量自适应调整信赖域半径,从初始控制点开始,迭代求解全局最优解。

12、进一步地,步骤2包括如下分步骤

13、步骤2-1,按照如下方法提取关节空间路径的几何特性:

14、计算关节角度随参数的一阶几何导数与二阶几何导数,一阶导数反映关节角度沿路径的变化快慢,二阶导数反映变化率的弯曲程度;

15、步骤2-2,按照下式计算关节空间的广义标量曲率:

16、;

17、式中:

18、为关节空间的连续平滑曲线随参数的广义标量曲率;

19、为关节角度随参数的一阶几何导数;

20、为关节角度随参数的二阶几何导数;

21、步骤2-3,按照下式构造由基准项与曲率敏感项共同组成的采样密度函数:

22、;

23、式中:

24、为基准密度权重;;

25、为曲率敏感权重;;

26、为对应参数的采样密度;

27、步骤2-4,按照如下逆变换采样方法生成离散采样点:

28、对采样密度函数在整个参数域上进行积分,得到累积分布函数,反映从路径起点到任意位置的累积采样密度,累积采样密度函数的表达式如下:

29、;

30、式中:

31、为对应参数的累积采样密度;

32、为对应参数的采样密度;∈[0,];

33、将累积采样密度的分布归一化到 [0,1]区间后,在纵轴上均匀生成 n个采样刻度;,再通过累积采样密度函数的逆映射,将这些均匀分布的刻度反解回原参数域,最终得到非均匀分布的离散采样点序列,为离散采样点序号,为第个离散采样点对应的归一化路径参数值。

34、进一步地,步骤3包括如下分步骤:

35、步骤3-1,采用阶钳位 b样条曲线对伪状态变量在参数域上进行如下参数化表示:

36、;

37、;

38、式中:

39、为钳位 b样条曲线阶数;

40、为离散采样点数量;

41、为离散采样点序号;;

42、为第个待优化的控制点;

43、为第个b样条基函数;

44、为定义在节点向量上的 b样条基函数行向量;

45、为待优化的控制点列向量;

46、步骤3-2,利用b样条的微分特性,由伪状态变量对参数的一、二阶导数表示控制点向量的纯线性组合,按下式替换对应的基函数导数矩阵:

47、;

48、;

49、式中:

50、为的一阶导数;

51、为的二阶导数;

52、为 b样条基函数对参数的一阶导数行向量;

53、为 b样条基函数对参数的二阶导数行向量。

54、进一步地,步骤4包括如下分步骤:

55、步骤4-1,按照如下方法使关节速度约束线性化:

56、关节角速度等于关节路径的一阶几何导数乘以参数速率,即:

57、;

58、式中:

59、为关节序号,对应机械臂的第个自由度;

60、为参数速率;

61、为第个关节角速度;

62、为第个关节角位置随路径参数的一阶几何导数;

63、设关节角速度幅值约束如下:;为消除根号项,将速度幅值约束两边同时平方,代入伪状态变量的 b样条表达式后,速度约束直接转化为如下关于控制点的线性不等式:

64、;

65、式中:

66、为第个离散采样点对应的归一化路径参数值;

67、为第个离散采样点处的第个关节角度一阶几何导数;

68、为第个离散采样点处的b样条基函数行向量;

69、为第个关节角速度幅值上限限制;

70、关节角速度幅值约束每个离散采样点的关节角速度不超过电机最大转速限制;

71、步骤4-2,通过链式求导推导得到如下关节角加速度的表达式:

72、;

73、式中:

74、为第个关节角位置随路径参数的二阶几何导数;

75、为第个关节的角加速度;

76、将上式分解为两项:一项与伪状态变量本身成正比,由路径二阶几何导数决定;另一项与伪状态变量的一阶导数成正比,由路径一阶几何导数决定;两项均与控制点呈线性关系,因此将加速度约束直接转化为如下双侧线性不等式:

77、;

78、式中:

79、为第个离散采样点处的第个关节角度二阶几何导数;

80、为第个离散采样点处的b样条基函数一阶导数行向量;

81、为第个关节角加速度幅值上限限制;

82、步骤4-3,按照如下方法使动力学力矩约束线性化:

83、基于刚体动力学模型,机械臂关节力矩由三部分组成:惯性力矩、科里奥利与离心力矩、重力矩,机械臂关节力矩的表达式如下:

84、;

85、式中:

86、为关节驱动力矩向量;

87、为关节角位置向量;

88、为关节角速度向量;

89、为关节角加速度向量;

90、为关节空间惯性矩阵;

91、为科里奥利力和离心力耦合矩阵‌;

92、为重力矩向量;

93、其中重力矩仅与关节角度有关,是与优化变量无关的常数项;

94、将非线性速度耦合项‌表示为纯几何相关的系数向量与伪状态变量的乘积:

95、;

96、;

97、式中:

98、为科里奥利力与离心力项的纯几何系数向量;

99、为关节角位置随参数的一阶几何导数向量;

100、为以几何导数替换速度后的科里奥利力和离心力耦合矩阵;

101、其中,为纯几何系数;

102、将惯性项与非线性速度耦合项合并后,将整个力矩表达式转化为伪状态变量及其一阶导数的线性组合,最终转化为如下关于控制点的线性不等式约束:

103、;

104、式中:

105、为关节序号;

106、为第个关节额定驱动力矩上限;

107、为第个关节的重力矩分量;

108、为第个离散采样点处第个关节的动力学线性转换行向量;

109、其中动力学线性转换行向量由惯性矩阵、几何导数与 b样条基函数组合而成,保证关节驱动力矩不超出电机额定力矩上限;

110、步骤4-4,按如下方法对边界条件进行降维处理:

111、为保证机械臂在路径起点和终点处于绝对静止状态,必须满足起止点速度为零、加速度为零的边界条件;映射到伪状态变量上,等价于起止点处且;

112、利用钳位 b样条的端点凸包与导数特性,上述边界条件等价于强制控制点向量的首尾两个控制点严格为零,则有:;

113、在算法实现中,只需直接将这四个控制点的变量上下限设置为 0,无需添加任何额外约束行。

114、进一步地,步骤5中包括如下分步骤:

115、步骤5-1,时间最优轨迹规划的原始目标是最小化机械臂沿整条路径运动的总时间;总时间按下式表示为参数速率的倒数在参数域上的积分:

116、;

117、式中:

118、为机械臂沿整条路径运动的总时间;

119、为时间变量;

120、为参数速率;

121、按照下式将最小化参数速率倒数的积分等价转化为最大化伪状态变量的积分;;

122、令积分常数行向量,将非线性的时间最优目标,等效转换为关于控制点的纯线性目标函数:;

123、式中:

124、为b样条基函数行向量;

125、为待优化的控制点列向量;

126、为积分常数行向量;

127、步骤5-2,暂时剔除非凸的jerk约束,仅保留速度、加速度、动力学力矩这三类全局线性约束,构建一个线性的路径优化的松弛问题;求解该松弛问题,得到基准控制点向量。

128、进一步地,步骤6中,采用b 样条曲线对伪状态变量在参数域上进行参数化表示,基于 b 样条的线性叠加特性,当所有控制点按同一比例缩放时,伪状态变量及其各阶导数都会按相同比例同步缩放,将该关系代入jerk方程,jerk的数值会按缩放比例的 1.5 次方变化,如下式所示:

129、;

130、式中:

131、为关节序号;

132、为全局缩放因子;

133、为基准控制点向量;

134、表示第个关节的 jerk函数;

135、为控制点按全局缩放因子缩放后第个关节的 jerk值;

136、为基准控制点向量第个关节的 jerk值;

137、根据上式,为了让缩放后的jerk的最大峰值回落至物理限值以内,通过如下解析公式直接计算出理论最优的全局缩放因子:

138、;

139、式中:

140、为物理限值;

141、为jerk的最大峰值;

142、引入安全裕度系数对全局缩放因子进行幅度调整,按如下公式计算最优边界的初始控制点:

143、;

144、式中:

145、为安全裕度系数;取值范围为:0.90~0.95;

146、为初始控制点向量。

147、进一步地,步骤7包括如下分步骤:

148、步骤7-1,设:为初始控制点,为初始信赖域半径,ε为收敛容差,为物理违规容差,为迭代次数;

149、令:,γ为系数,取值范围为:0.01~0.1;其中为无穷范数;

150、载入、、ε、,初始化迭代次数,令 k=0;

151、步骤7-2,按如下方法构建线性子问题:

152、在当前迭代点处,计算最大化伪状态变量积分的系数向量;组装速度、加速度、动力学力矩的全局线性约束;计算jerk在当前点的基准值与雅可比梯度,生成jerk的局部线性近似约束;添加当前信赖域的边界约束,使待优化的控制点列向量相对当前迭代点的变化幅度不超过信赖域半径,如下式所示:

153、;

154、式中:

155、为待优化的控制点列向量;

156、为元素全为1的列向量;

157、为第次迭代的控制点;

158、为第次迭代的信赖域半径;

159、步骤7-3,按如下方法求解试探解:以最大化伪状态变量积分为优化目标,集结所有线性约束条件,调用线性规划求解器进行求解,得到本次迭代的试探解;

160、步骤7-4,按如下方法评估真实违规量:将试探解代入原始的非线性jerk方程,计算所有离散采样点的真实jerk值,统计全路径上的最大物理违规量,最大物理违规量反映真实jerk超出限值的最大幅度,如下式所示:

161、;

162、式中:

163、为关节序号;

164、为离散采样点序号;;为离散采样点数量;

165、为全路径上的最大物理违规量;

166、为本次迭代求解得到的试探解;

167、为第个离散采样点对应的归一化路径参数值;

168、为第个关节的物理jerk值上限;

169、表示将试探解代入后计算得到的真实非线性 jerk函数;

170、表示全局最大值函数;

171、步骤7-5,按如下方法动态更新信赖域半径及取舍试探解:

172、若,舍弃该试探解,保持,并收缩信赖域半径,令k=k+1,返回步骤7-2;

173、若:接受该试探解并使;进一步判断:若,取向量中所有元素绝对值的最大值;

λ为系数,取值范围为:0.90~0.99,扩大信赖域半径以加快收敛;否则保持信赖域半径不变;

174、步骤7-6,若接受该试探解,且控制点的最大更新幅度,判断已收敛至最优解,退出迭代运算并输出最优控制点;否则令k=k+1,返回步骤7-2。

175、进一步地,步骤7-2包括如下分步骤:

176、步骤a1,对jerk表达式继续求时间导数,如下式所示:

177、;

178、;

179、式中:

180、为第个关节的 jerk;

181、为jerk表达式中由几何导数与伪状态变量及其导数构成的中间组合函数;

182、为第个关节角度随参数的一阶几何导数;

183、为第个关节角度随参数的二阶几何导数;

184、为第个关节角度随参数的三阶几何导数;

185、为伪状态变量对参数的一阶几何导数;

186、为伪状态变量对参数的二阶几何导数;

187、步骤a2,采用序列近似优化方法,在每一轮迭代的当前控制点附近,对jerk的非线性表达式进行一阶泰勒展开,用线性函数局部近似非线性函数,如下式所示:

188、;

189、式中:

190、为控制点向量为时第个关节的 jerk函数值;

191、为关于控制点向量的梯度算子;

192、为第次迭代时第个关节的 jerk基准值;

193、为第次迭代时 jerk对控制点的雅可比梯度转置向量;

194、步骤a3,通过下式解析推导得到jerk对控制点的雅可比梯度向量,即jerk随控制点变化的灵敏度:

195、;

196、式中:

197、为第次迭代时当前控制点下的伪状态变量值;

198、为第次迭代时当前控制点下的中间组合函数值;

199、为b样条基函数行向量的转置向量;

200、其中中间线性项的梯度为常数矩阵,由几何导数与 b 样条基函数直接组合得到;

201、步骤a4,基于一阶泰勒展开,将线性近似后的jerk代入幅值约束,得到本轮迭代的局部线性不等式组:

202、;

203、式中:

204、为第个关节的物理 jerk限值上限;

205、为第次迭代时第个离散采样点下的jerk基准值;

206、为第次迭代时第个离散采样点的jerk雅可比梯度转置向量。

207、本发明还提供了一种实现考虑高阶约束的机械臂时间最优轨迹规划方法的设备,包括存储器和处理器,所述存储器用于存储计算机程序;所述处理器,用于执行所述计算机程序并在执行所述计算机程序时实现如上述的考虑高阶约束的机械臂时间最优轨迹规划方法步骤。

208、本发明具有的优点和积极效果是:与现有技术相比,本发明具有以下显著的技术进步与有益效果:

209、1、采样精度与效率的双重提升:曲率感知的非均匀采样机制实现了离散采样点的自适应优化分布,在高曲率关键区域保证约束校验精度,在平缓区域降低计算规模,解决了传统均匀采样 “精度与效率不可兼得” 的矛盾,同等精度下求解效率提升显著。

210、2、核心约束的全局精确性与凸性:速度、加速度、动力学力矩三类核心约束均实现了无损全局线性化,无任何近似误差,保证了求解结果的物理准确性,避免了实际运行中的约束超限风险;同时问题转化为标准线性规划,保证了全局最优性与求解高效性。

211、3、高阶约束的高效严格满足:通过序列泰勒展开与动态信赖域的结合,在保持线性规划求解效率的前提下,实现了对非凸jerk约束的严格满足,有效降低机械臂的运动冲击与振动,提升运动平稳性与定位精度,延长机械臂使用寿命。

212、4、 收敛稳定性与速度的大幅优化:解析缩放初始化策略提供了高质量的可行初始点,物理违规量驱动的动态信赖域实现了步长的自适应调整,两者协同作用使算法收敛速度快、鲁棒性强,不易发散,迭代次数显著减少,更适配工业场景的实时性需求。

213、5、轻量化的边界条件处理:利用 b 样条固有特性实现边界约束的降维处理,无需额外添加等式约束,缩减了优化问题规模,简化了实现逻辑,进一步提升了求解效率。