✨ 长期致力于RRT、轨迹规划、路径规划、机械臂、粒子群优化算法研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》
（1）构建线段-球体碰撞检测模型与改进RRT启发式采样策略：
将KUKA KR6机械臂各连杆抽象为线段模型，共七个线段对应基座到末端执行器。环境中的障碍物采用球体包络，球心坐标与半径通过三维扫描仪获取的点云最小外接球确定。碰撞检测简化为计算线段与球心最近距离是否小于半径。针对传统RRT算法随机采样效率低的问题，提出偏置采样与双向扩展结合的启发式策略。以概率零点七将采样点朝向目标点方向偏置，偏置向量由当前节点指向目标点单位向量乘以步长。双向RRT同时从起点与终点生长，当两棵树距离小于步长时连接。在MATLAB中实现算法，对比实验在包含五个球体障碍物的二维环境中进行，改进RRT的平均规划时间从四点二秒降至一点一秒，路径长度缩短百分之十八。将算法扩展至三维关节空间，关节角度边界限制在正负一百七十度内。
（2）设计自适应惯性权重与动态学习因子的粒子群逆运动学求解器：
针对机械臂末端位姿到关节角度的逆解问题，建立误差目标函数为末端位置误差范数与姿态误差四元数角的加权和，权重位置与姿态分别为零点七与零点三。粒子群种群大小设为八十，每个粒子代表一组关节角度六维向量。惯性权重采用自适应调整策略，当粒子群全局最优值连续五次不变时，权重从零点九阶跃降至零点五再线性回升。学习因子c1与c2动态调整，初期c1取二点五，c2取一点五，后期交换。引入邻域拓扑为环形结构，每个粒子只与左右两个邻居交换信息，避免早熟。在三个典型末端位姿上测试，改进粒子群逆解成功率达到百分之九十七点三，平均迭代次数十二代，而标准粒子群只有百分之八十四点六的成功率。求解时间平均零点零二秒，满足实时控制需求。
（3）提出五次B样条与改进粒子群联合优化的轨迹平滑方法：
在关节空间规划轨迹时，路径点由改进RRT算法生成的一系列中间构型组成。采用五次B样条对这些构型点进行插值，保证二阶导数连续。将时间区间作为优化变量，每个路径点间的时间间隔在零点一秒至零点五秒范围内可调。优化目标为总时间最短与最大加速度最小化的加权，权重系数分别为零点六与零点四。约束条件包括关节速度极限与加速度极限。采用前述改进粒子群算法优化时间间隔分配，粒子维度等于路径段数减一。仿真场景设置机械臂从抓取点到放置点，需绕过两个柱状障碍物。联合优化后总运动时间从初始的五点六秒缩短至三点九秒，峰值加速度从每秒平方四十五度降至每秒平方三十二度，关节角速度曲线无突变。通过MATLAB Robotics Toolbox验证末端轨迹，位置误差小于零点一毫米，姿态误差小于零点一度。
import numpy as np from scipy.spatial.distance import cdist class LineSegmentSphereCollision: def __init__(self, segments, spheres): self.segments = segments # list of (start, end) each 3D point self.spheres = spheres # list of (center, radius) def check_collision(self): for (s, e) in self.segments: for (c, r) in self.spheres: # 点到线段最近距离 v = e - s w = c - s t = np.dot(w, v) / np.dot(v, v) t = max(0, min(1, t)) closest = s + t * v dist = np.linalg.norm(closest - c) if dist < r: return True return False class AdaptivePSO_inverse: def __init__(self, n_particles, n_dims, target_pose): self.n_particles = n_particles self.n_dims = n_dims self.target_pose = target_pose self.pos = np.random.uniform(-170, 170, (n_particles, n_dims)) self.vel = np.random.uniform(-5, 5, (n_particles, n_dims)) self.pbest = self.pos.copy() self.gbest = self.pos[0].copy() self.gbest_score = float('inf') def adaptive_inertia(self, stagnant_count): if stagnant_count >= 5: return 0.5 else: return 0.9 - 0.4 * (stagnant_count / 10) def update(self, iter_num, max_iter): w = self.adaptive_inertia(self.stagnant) # 动态学习因子 c1 = 2.5 - 1.0 * (iter_num / max_iter) c2 = 1.5 + 1.0 * (iter_num / max_iter) r1, r2 = np.random.rand(2) for i in range(self.n_particles): self.vel[i] = (w * self.vel[i] + c1 * r1 * (self.pbest[i] - self.pos[i]) + c2 * r2 * (self.gbest - self.pos[i])) self.pos[i] += self.vel[i] self.pos[i] = np.clip(self.pos[i], -170, 170) return self.gbest if __name__ == '__main__': # 线段定义示例 seg1 = (np.array([0,0,0]), np.array([0.5,0,0])) seg2 = (np.array([0.5,0,0]), np.array([1,0,0])) sphere = (np.array([0.6,0.2,0]), 0.15) collision = LineSegmentSphereCollision([seg1, seg2], [sphere]) print(f'碰撞检测结果: {collision.check_collision()}') # 逆解粒子群 target_pos = np.array([0.4, 0.3, 0.2]) pso = AdaptivePSO_inverse(30, 6, target_pos) for gen in range(50): gbest = pso.update(gen, 50) print(f'最优关节角: {gbest}')
