[发明专利]一种冗余机器人多约束逆解方法

权利要求书

1.一种冗余机器人多约束逆解方法，其特征在于：包括以下步骤：

Step1:建立机器人关节角度相关的连续加权矩阵；

Step2:构建推离关节远离极限角度的排斥速度势场；

Step3:建立加权梯度投影法冗余机器人逆解；

Step4:定义优化准则函数正则化连续变量系数；

Step5:设计优化准则函数正则化处理原则；

Step6:求解冗余机器人多约束逆运动学；

所述Step1中，连续加权矩阵W_c与机器人避关节角度极限相关，连续加权矩阵W_c定义为：

W_c＝diag(w_c(q_i))，i＝1,…,n；

其中，n为机器人关节自由度；q_i为第i个关节角度；

连续加权矩阵因子w_c(q_i)为：

其中，q_imin和q_imax分别为关节角度最小极限和最大极限；q_itmax＝(1-Ω)q_imax+Ωq_imin、q_itmin＝(1-Ω)q_imin+Ωq_imax分别为正负极限的阈值；Ω为阻尼区域宽度；g_Ω(·)是三次函数，g_Ω(d)＝-2d³+3d²；w_c(q_i)的引入将关节角度范围分成了三部分：阻尼区域-灵活区域-阻尼区域；

所述Step1中，连续加权矩阵W_c用于构建加权矩阵W_b和冗余机器人加权雅可比矩阵J_wb，描述为：

W_b＝I_n-W_c；

J_wb＝JW_b；

基于加权雅可比矩阵J_wb建立冗余机器人加权零空间矩阵为：

其中，为加权雅可比矩阵J_wb的伪逆；

作用于关节角度阻尼区域内的排斥速度势场T_r用于推离关节角度远离极限角度，描述为：

T_r＝diag(t_r(q_i))，i＝1,…,n；

其中，t_rmax为关节最大排斥角速度；

排斥速度势场函数T_r作用于冗余机器人末端m维度主任务的所述加权零空间矩阵内，建立加权梯度投影法冗余机器人逆解，为：

其中为机器人关节速度；

基于阻尼最小二乘法，对所述加权梯度投影法冗余机器人逆解重新定义，即：

其中，ρ_max是最大阻尼因子；ε是奇异区域大小阈值；σ_min是J_wb最小奇异值；ρ_wb是J_wb阻尼因子；是含有阻尼因子ρ_wb的加权雅可比伪逆；

所述冗余机器人基于正则化处理的连续变量系数k_j(H_nj)根据当前机器人构型配置，在线自适应连续调整优化准则函数变量系数，描述为：

k_j(H_nj)＝±f_norm(H_nj)，j＝1,…,s

其中，H_nj是正则化处理后的优化准则函数H_j；f_norm(·)是连续变量系数函数，若H_nj欲最大化，取正号，若H_nj欲最小化，取负号；s为总约束任务的数目；

所述连续变量系数函数f_norm(H_nj)定义为：

其中，和分别是H_nj的最大值和最小值；分别是的阈值；a，b，c，d分别是连续变量系数函数f_norm(H_nj)的系数；λ是与之间的带宽；

所述Step5中，优化准则函数正则化处理原则用于对不同的优化准则函数进行正则化处理使得优化准则函数具有相同的量纲和相同的幅值，优化准则函数正则化处理原则合理分配变量系数并平衡不同优化准则函数在冗余机器人逆解中的作用，所述优化准则函数正则化处理原则包括两种类型原则，包括：

A.避障优化准则函数，其正则化处理原则定义为：

N_∞(H_j)＝1-exp(a_c-H_j)；

其中，N_∞(∞)＝1表示最优的正则化结果，N_∞(a_c)＝0为相应的最差正则化结果；在避障情况下，N_∞(∞)表示机器人与障碍距离无穷远，N_∞(a_c)表示机器人与障碍相碰撞，a_c为碰撞距离阈值；

B.；避关节角度极限优化准则函数，其正则化处理原则定义为：

N_c(H_j)＝exp(b_c-H_j)；

其中，N_c(b_c)＝1表示最优正则化结果，N_c(∞)＝0表示最差正则化结果；在避关节角度极限情况下，N_c(b_c)表示机器人关节处于角度中间值，b_c为最优关节角度位置，N_c(∞)表示关节处于正负极限处；

所述冗余机器人多约束逆运动学求解基于连续加权矩阵和优化准则函数正则化处理原则，定义为：

其中，