GNSS拒止无人机集群协同定位算法【附代码】

✨ 长期致力于无人机集群、GNSS拒止环境、机间测距、协同定位、测角定位研究工作擅长数据搜集与处理、建模仿真、程序编写、仿真设计。✅ 专业定制毕设、代码✅如需沟通交流点击《获取方式》1加权质心与最小二乘融合的相对定位解算框架针对GNSS拒止环境下无人机集群仅靠机间测距信息进行相对定位时存在野值干扰和噪声敏感问题提出一种加权质心与最小二乘融合的协同定位算法。首先构建集群分散式融合定位架构每架无人机作为独立节点通过机载超宽带模块获得与其他无人机的距离测量值。对于测量值中的野值设计基于滑动窗口的中值绝对偏差检测器窗口长度为5当新测量值偏离窗口内中值超过3倍标准差时视为野值并予以剔除代之以线性预测值。在剔除野值后采用加权质心算法获得初始位置估计权重取距离测量的倒数平方。然后以质心估计作为最小二乘迭代的初始值将定位问题转化为非线性优化函数使用高斯-牛顿法迭代求解。在仿真场景中设置6架无人机呈不规则立体分布测距噪声服从均值为0、标准差为0.3米的正态分布该融合算法将平均定位误差从纯最小二乘的1.2米降低至0.35米。2基于光学相位扫描的双站测角协同定位设计一种采用光学相位扫描技术的双站测角协同定位方法用于GNSS拒止环境下的无人机定位。每观测站配备一个四象限光电探测器当目标无人机发出的调制光信号到达时通过比较四个象限的光强差异计算方位角和俯仰角角度测量精度可达0.05度。双观测站之间通过微波链路同步时间戳利用基线长度已知的前提采用交叉定位原理解算目标三维坐标。为优化定位精度分布构建了精度衰减因子评估模型分析基线长度、角度测量误差、观测站自身定位误差三个因素对最终定位误差的传播特性。仿真结果表明当基线长度设置为目标距离的0.6至1.2倍时几何精度因子最优当角度测量误差从0.01度增加到0.1度时定位误差等比例从0.2米增长到1.9米。3分布式卡尔曼一致性滤波的协同状态估计为解决单次测距或测角定位结果的时间不连续性设计一种分布式卡尔曼一致性滤波算法用于无人机集群的连续状态估计。每架无人机维护自身的状态向量包括三维位置和三维速度通过机间通信网络与相邻无人机交换局部估计值。滤波算法包含两个阶段本地卡尔曼滤波阶段利用自身惯性测量单元数据进行时间更新并使用机间测距值作为量测更新一致性阶段将邻居无人机的状态估计值与本地估计值进行加权平均权重根据两无人机之间的测距噪声协方差自适应调整。在集群规模为8架无人机的仿真中通信拓扑为环形结构经过50次迭代后所有无人机的位置估计误差收敛到0.15米以内速度估计误差小于0.08米每秒相比不考虑一致性的独立滤波方案精度提升56%。import numpy as np from scipy.optimize import least_squares import networkx as nx class RangingOutlierDetector: def __init__(self, window_len5, threshold3.0): self.buffer [] self.window_len window_len self.thresh threshold def filter(self, new_range): self.buffer.append(new_range) if len(self.buffer) self.window_len: self.buffer.pop(0) if len(self.buffer) 3: return new_range median np.median(self.buffer) mad np.median(np.abs(self.buffer - median)) if mad 1e-6: mad 0.1 z_score np.abs(new_range - median) / (1.4826 * mad) if z_score self.thresh: return self.buffer[-2] (self.buffer[-2] - self.buffer[-3]) return new_range class WeightedCentroidLeastSquares: def __init__(self, noise_var0.09): self.noise_var noise_var def trilateration_residuals(self, x, anchors, ranges): res [] for i, anchor in enumerate(anchors): dist np.linalg.norm(x - anchor) res.append(dist - ranges[i]) return np.array(res) def locate(self, anchors, ranges): if len(anchors) 3: return None ranges np.array(ranges) # weighted centroid weights 1.0 / (ranges**2 1e-6) centroid np.sum(weights[:, None] * anchors, axis0) / np.sum(weights) # least squares refinement result least_squares(self.trilateration_residuals, centroid, args(anchors, ranges), methodlm, max_nfev30) return result.x class DistributedKalmanConsensus: def __init__(self, node_id, comm_graph): self.id node_id self.graph comm_graph self.x np.zeros(6) # pos(3), vel(3) self.P np.eye(6) * 0.1 self.Q np.eye(6) * 0.01 self.R 0.04 # ranging variance self.A np.array([[1,0,0,0.1,0,0], [0,1,0,0,0.1,0], [0,0,1,0,0,0.1], [0,0,0,1,0,0], [0,0,0,0,1,0], [0,0,0,0,0,1]]) self.H np.zeros((len(self.graph.nodes), 6)) for i, nb in enumerate(self.graph.nodes): if nb ! self.id: self.H[i, 0:3] np.array([1,0,0]) def predict(self): self.x self.A self.x self.P self.A self.P self.A.T self.Q def update_with_ranging(self, nb_positions, measured_ranges): for i, nb_id in enumerate(self.graph.nodes): if nb_id self.id: continue pred_range np.linalg.norm(self.x[0:3] - nb_positions[nb_id]) innovation measured_ranges[i] - pred_range S self.H[i:i1] self.P self.H[i:i1].T self.R K self.P self.H[i:i1].T / S self.x self.x K.flatten() * innovation self.P (np.eye(6) - np.outer(K.flatten(), self.H[i:i1])) self.P def consensus(self, neighbor_estimates, consensus_gain0.6): new_x self.x.copy() for nb_id, nb_x in neighbor_estimates.items(): new_x consensus_gain * (nb_x - self.x) self.x new_x