✨ 长期致力于运动规划、路径规划、RRT算法、ROS仿真研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》
（1）目标偏置与椭圆采样机制的改进RRT算法：
针对五自由度机械臂在狭窄空间中的路径规划问题，提出一种混合驱动RRT算法。在随机树扩展时，以0.4的概率将采样点直接设为目标点，以0.2的概率设为随机点，其余概率通过椭圆区域约束采样。椭圆区域以起始点和目标点为焦点，长轴长度为当前最佳路径长度的1.2倍，短轴根据当前障碍物密度自适应调整。采样点在椭圆内均匀分布，有效减少冗余节点。同时引入节点拒绝机制：若新节点与最近节点的欧氏距离小于0.05 rad且方向变化小于15度，则拒绝该节点。在8字形障碍物环境下的仿真中，改进算法平均节点数从标准RRT的850个减少到320个，规划时间从2.1秒降至0.6秒，路径长度缩短18%。
（2）五次多项式插值与平滑处理：
规划得到的关节空间路径点通过五次多项式进行插值，保证关节角速度、角加速度连续。对于每个关节，根据路径段起止角度和速度、加速度边界条件（起点终点速度加速度均为0）求解五次多项式系数。为消除冗余折返，采用Douglas-Peucker算法对路径点进行抽稀，阈值设为0.1 rad。之后使用梯度下降法在满足碰撞约束下进一步平滑，目标函数为路径曲率平方和，约束为关节极限和避障。在Matlab中建模的UR5机械臂上测试，平滑后最大关节加速度从8.2 rad/s²降至4.5 rad/s²，轨迹执行时间减少12%。
（3）ROS仿真与实体验证：
在Gazebo中搭建包含机械臂和船用曲轴曲拐的仿真环境，曲拐模型通过Solidworks导入。机械臂搭载深度相机，识别曲拐圆弧面的检测点。改进RRT算法以ROS actionlib形式实现，接收目标点后返回轨迹并执行。仿真中设置五个检测点，机械臂依次规划运动，总规划时间累计3.4秒，无碰撞。将算法移植到真实五自由度机械臂（步进电机驱动），控制系统基于STM32与ROS串口通信。实际曲拐检测实验中，机械臂成功规划并到达所有检测点，与仿真结果一致，末端定位误差小于2mm。路径规划成功率为96%，高于原有RRT算法的82%。
import numpy as np import random class EllipseRRT: def __init__(self, start, goal, obstacles, bounds): self.start = start self.goal = goal self.obstacles = obstacles self.bounds = bounds self.nodes = [start] def sample(self, best_path_len): if random.random() < 0.4: return self.goal if random.random() < 0.2: return np.random.uniform(self.bounds[:,0], self.bounds[:,1]) c = (self.start + self.goal) / 2 focal = np.linalg.norm(self.goal - self.start) / 2 a = best_path_len / 2 b = np.sqrt(max(0.001, a**2 - focal**2)) while True: theta = 2*np.pi*random.random() phi = np.arccos(2*random.random()-1) r = np.sqrt(random.random()) x = r * a * np.sin(phi) * np.cos(theta) y = r * b * np.sin(phi) * np.sin(theta) z = r * b * np.cos(phi) rot = self.rotation_matrix(self.goal - self.start) point = c + rot @ np.array([x,y,z]) if np.all(point >= self.bounds[:,0]) and np.all(point <= self.bounds[:,1]): return point def steer(self, from_node, to_point, step=0.1): dir_vec = to_point - from_node dist = np.linalg.norm(dir_vec) if dist < step: new_node = to_point else: new_node = from_node + dir_vec/dist * step if self.collision_free(from_node, new_node): return new_node return None def collision_free(self, p1, p2): for obs in self.obstacles: if obs.check_line(p1, p2): return False return True def plan(self, max_iter=500): best_path = None best_len = np.inf for i in range(max_iter): sample = self.sample(best_len) nearest = min(self.nodes, key=lambda n: np.linalg.norm(n - sample)) new = self.steer(nearest, sample) if new is not None: self.nodes.append(new) if np.linalg.norm(new - self.goal) < 0.1: path = self.extract_path(new) length = self.path_length(path) if length < best_len: best_len = length best_path = path self.nodes.append(self.goal) return best_path ",
