def compute_V0_screw_motion(x, R, xd, Rd, max_lin_vel=0.05, max_ang_vel=0.1, beta=1.0): """ 基于螺旋运动理论计算引导速度，更符合物理运动 参数： beta: 收敛速度系数 """ # 计算位置误差 pos_error = xd - x # 计算姿态误差（轴角表示） R_err = Rd.T @ R angle = np.arccos(np.clip((np.trace(R_err) - 1) / 2, -1, 1)) if angle < 1e-6: axis = np.array([0, 0, 1]) else: axis = np.array([ R_err[2, 1] - R_err[1, 2], R_err[0, 2] - R_err[2, 0], R_err[1, 0] - R_err[0, 1] ]) / (2 * np.sin(angle) + 1e-10) # 计算螺旋轴上的运动 if angle < 1e-6: # 纯平移运动 lin_vel = beta * pos_error ang_vel = np.zeros(3) else: # 计算瞬时螺旋轴 screw_axis = axis # 计算螺旋运动上的点（选择目标点） p = xd # 计算线速度和角速度 ang_vel = beta * angle * screw_axis # 计算伴随线速度 v_parallel = np.cross(ang_vel, p - x) v_perpendicular = beta * (pos_error - np.dot(pos_error, screw_axis) * screw_axis) lin_vel = v_parallel + v_perpendicular # 限幅 lin_vel_norm = np.linalg.norm(lin_vel) if lin_vel_norm > max_lin_vel: lin_vel = lin_vel * (max_lin_vel / lin_vel_norm) ang_vel_norm = np.linalg.norm(ang_vel) if ang_vel_norm > max_ang_vel: ang_vel = ang_vel * (max_ang_vel / ang_vel_norm) # 转换到体坐标系 V0_linear = R.T @ lin_vel V0_angular = R.T @ ang_vel return np.vstack([V0_linear.reshape(-1, 1), V0_angular.reshape(-1, 1)]) def compute_V0_time_optimal(x, R, xd, Rd, max_lin_vel=0.05, max_ang_vel=0.1, k_lin=0.8, k_ang=1.2): """ 基于距离加权的时间最优引导速度 参数： k_lin: 线速度的Sigmoid形状参数 k_ang: 角速度的Sigmoid形状参数 """ # 计算位置距离 pos_dist = np.linalg.norm(xd - x) # 计算姿态距离（旋转角度） R_err = Rd.T @ R cos_theta = (np.trace(R_err) - 1) / 2 cos_theta = np.clip(cos_theta, -1, 1) rot_dist = np.arccos(cos_theta) # 使用Sigmoid函数调整速度，接近目标时减速 def sigmoid_weight(dist, k): """距离越近，权重越小（减速）""" return 1 / (1 + np.exp(-k * (dist - 0.02))) # 0.02m或rad为切换点 # 计算线速度方向 if pos_dist > 1e-6: lin_dir = (xd - x) / pos_dist else: lin_dir = np.zeros(3) # 计算角速度方向（旋转轴） if rot_dist > 1e-6: R_err_skew = (R_err - R_err.T) / 2 ang_dir = np.array([R_err_skew[2, 1], R_err_skew[0, 2], R_err_skew[1, 0]]) ang_dir = ang_dir / (np.linalg.norm(ang_dir) + 1e-10) else: ang_dir = np.zeros(3) # 计算速度大小 lin_vel_mag = max_lin_vel * sigmoid_weight(pos_dist, k_lin) ang_vel_mag = max_ang_vel * sigmoid_weight(rot_dist, k_ang) # 组合速度 lin_vel = lin_vel_mag * lin_dir ang_vel = ang_vel_mag * ang_dir # 转换到体坐标系 V0_linear = R.T @ lin_vel V0_angular = R.T @ ang_vel return np.vstack([V0_linear.reshape(-1, 1), V0_angular.reshape(-1, 1)]) def compute_V0_interpolated(x, R, xd, Rd, max_lin_vel=0.05, max_ang_vel=0.1, interp_type='linear'): """ 基于插值的引导速度，特别适用于PiH任务 """ # 计算位置和姿态误差 pos_error = xd - x pos_error_norm = np.linalg.norm(pos_error) # 计算姿态误差 R_err = Rd.T @ R cos_theta = (np.trace(R_err) - 1) / 2 cos_theta = np.clip(cos_theta, -1, 1) rot_error = np.arccos(cos_theta) # 使用不同的插值策略 if interp_type == 'linear': # 线性插值 pos_weight = min(1.0, pos_error_norm / 0.1) # 0.1m为参考距离 rot_weight = min(1.0, rot_error / 0.5) # 0.5rad为参考角度 elif interp_type == 'smooth': # 平滑插值（三次多项式） pos_weight = 3 * (min(1.0, pos_error_norm / 0.1)) ** 2 \ - 2 * (min(1.0, pos_error_norm / 0.1)) ** 3 rot_weight = 3 * (min(1.0, rot_error / 0.5)) ** 2 \ - 2 * (min(1.0, rot_error / 0.5)) ** 3 elif interp_type == 'adaptive': # 自适应插值：根据误差比例调整 total_error = pos_error_norm + rot_error * 0.1 # 角度误差权重较低 if total_error > 0.01: pos_weight = pos_error_norm / total_error rot_weight = (rot_error * 0.1) / total_error else: pos_weight = rot_weight = 0.5 # 计算线速度 if pos_error_norm > 1e-6: lin_dir = pos_error / pos_error_norm lin_speed = max_lin_vel * pos_weight lin_vel = lin_speed * lin_dir else: lin_vel = np.zeros(3) # 计算角速度（使用SLERP思想） if rot_error > 1e-6: # 计算旋转轴 R_err_skew = (R_err - R_err.T) / 2 ang_axis = np.array([R_err_skew[2, 1], R_err_skew[0, 2], R_err_skew[1, 0]]) ang_axis = ang_axis / (np.linalg.norm(ang_axis) + 1e-10) ang_speed = max_ang_vel * rot_weight ang_vel = ang_speed * ang_axis else: ang_vel = np.zeros(3) # 转换到体坐标系 V0_linear = R.T @ lin_vel V0_angular = R.T @ ang_vel return np.vstack([V0_linear.reshape(-1, 1), V0_angular.reshape(-1, 1)])机器人操作空间速度计算python几种实现函数
)
