├── code ├── __pycache__ │ ├── MyCost.cpython-39.pyc │ ├── DirToMove.cpython-39.pyc │ ├── UpdateMap.cpython-39.pyc │ ├── CheckMotion.cpython-39.pyc │ ├── CreateModel.cpython-39.pyc │ ├── MotionDecode.cpython-39.pyc │ ├── PlotSolution.cpython-39.pyc │ ├── noncircshift.cpython-39.pyc │ ├── PathFromMotion.cpython-39.pyc │ └── CreateRandomSolution.cpython-39.pyc ├── DirToMove.py ├── MotionDecode.py ├── noncircshift.py ├── PlotModel.py ├── PathFromMotion.py ├── PlotSolution.py ├── CheckMotion.py ├── UpdateMap.py ├── CreateModel.py ├── MyCost.py ├── CreateRandomSolution.py └── MPSO_MAIN.py ├── .vscode └── settings.json └── README.md /code/__pycache__/MyCost.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangAn17/MPSO_CTS/HEAD/code/__pycache__/MyCost.cpython-39.pyc -------------------------------------------------------------------------------- /code/__pycache__/DirToMove.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangAn17/MPSO_CTS/HEAD/code/__pycache__/DirToMove.cpython-39.pyc -------------------------------------------------------------------------------- /code/__pycache__/UpdateMap.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangAn17/MPSO_CTS/HEAD/code/__pycache__/UpdateMap.cpython-39.pyc -------------------------------------------------------------------------------- /code/__pycache__/CheckMotion.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangAn17/MPSO_CTS/HEAD/code/__pycache__/CheckMotion.cpython-39.pyc -------------------------------------------------------------------------------- /code/__pycache__/CreateModel.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangAn17/MPSO_CTS/HEAD/code/__pycache__/CreateModel.cpython-39.pyc -------------------------------------------------------------------------------- /code/__pycache__/MotionDecode.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangAn17/MPSO_CTS/HEAD/code/__pycache__/MotionDecode.cpython-39.pyc -------------------------------------------------------------------------------- /code/__pycache__/PlotSolution.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangAn17/MPSO_CTS/HEAD/code/__pycache__/PlotSolution.cpython-39.pyc -------------------------------------------------------------------------------- /code/__pycache__/noncircshift.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangAn17/MPSO_CTS/HEAD/code/__pycache__/noncircshift.cpython-39.pyc -------------------------------------------------------------------------------- /code/__pycache__/PathFromMotion.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangAn17/MPSO_CTS/HEAD/code/__pycache__/PathFromMotion.cpython-39.pyc -------------------------------------------------------------------------------- /code/__pycache__/CreateRandomSolution.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangAn17/MPSO_CTS/HEAD/code/__pycache__/CreateRandomSolution.cpython-39.pyc -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.testing.unittestArgs": [ 3 | "-v", 4 | "-s", 5 | "./code", 6 | "-p", 7 | "*test.py" 8 | ], 9 | "python.testing.pytestEnabled": false, 10 | "python.testing.unittestEnabled": true 11 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MPSO-CTS 2 | ## 1. Introduction 3 | This project uses the MPSO method to achieve the task of searching for multiple targets in the region 4 | > Source Code Reference Journal 《Motion-encoded particle swarm optimization for moving target search using UAVs》,and use Python to rewrite the source code 5 | > 6 | > The link is https://doi.org/10.1016/j.asoc.2020.106705 7 | -------------------------------------------------------------------------------- /code/DirToMove.py: -------------------------------------------------------------------------------- 1 | # Convert from the target direction to a corresponding move in the map 2 | # 3 | 4 | 5 | def dir_to_move(dir): 6 | directions = { 7 | "N": [1, 0], 8 | "NE": [1, 1], 9 | "E": [0, 1], 10 | "SE": [-1, 1], 11 | "S": [-1, 0], 12 | "SW": [-1, -1], 13 | "W": [0, -1], 14 | "NW": [1, -1], 15 | } 16 | 17 | return directions.get(dir, [0, 0]) 18 | 19 | 20 | # Example usage 21 | if __name__ == "__main__": 22 | direction = "N" 23 | move_vector = dir_to_move(direction) 24 | print(move_vector) 25 | -------------------------------------------------------------------------------- /code/MotionDecode.py: -------------------------------------------------------------------------------- 1 | # Convert the encoded motion from an array of 8 numbers to a move 2 | # Eight numbers encode the magnitude of a vector with the angle of 3 | # NW, N, NE, W, E, SW, S, SE 4 | # 1 2 3 4 5 6 7 8 5 | # 6 | 7 | import numpy as np 8 | 9 | 10 | def motion_decode(motion): 11 | # Find the angle of the sum vector 12 | angle = np.arctan2(motion[1], motion[0]) 13 | 14 | # Map the angle to its corresponding octant (0, 45, 90, 135 ...) 15 | octant = (round(8 * angle / (2 * np.pi) + 8) % 8) + 1 16 | 17 | move_array = np.array( 18 | [[1, 0], [1, 1], [0, 1], [-1, 1], [-1, 0], [-1, -1], [0, -1], [1, -1]] 19 | ) 20 | 21 | # Map the octant to a move 22 | move = move_array[octant - 1] 23 | 24 | return move 25 | 26 | 27 | # Example usage 28 | if __name__ == "__main__": 29 | motion_vector = [1, 0] 30 | decoded_direction = motion_decode(motion_vector) 31 | print(decoded_direction) 32 | -------------------------------------------------------------------------------- /code/noncircshift.py: -------------------------------------------------------------------------------- 1 | # Utility function 2 | # Shift an array in 2D 3 | # Examples: 4 | # B = noncircshift(A,[0 1]) > Right 5 | # noncircshift(A,[0 -1]) > Left 6 | # noncircshift(A,[1 1]) > Right Down 7 | # 8 | 9 | import numpy as np 10 | 11 | 12 | def noncircshift(A, offsets): 13 | offsets = np.negative(offsets) 14 | siz = np.shape(A) 15 | N = len(siz) 16 | if len(offsets) < N: 17 | offsets.extend([0] * (N - len(offsets))) 18 | B = np.zeros(siz) 19 | indices = [] 20 | for i in range(N): 21 | idx = np.arange(siz[i]) + offsets[i] 22 | idx = np.clip(idx, 0, siz[i] - 1) # 确保索引值在有效范围内 23 | indices.append(idx) 24 | src_indices = np.meshgrid(*indices, indexing="ij") 25 | dest_indices = np.meshgrid( 26 | *[idx - off for idx, off in zip(indices, offsets)], indexing="ij" 27 | ) 28 | B[tuple(dest_indices)] = A[tuple(src_indices)] 29 | return B, src_indices, dest_indices 30 | 31 | 32 | # Example usage 33 | if __name__ == "__main__": 34 | A = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]]) 35 | offsets = [0, 1] 36 | B, src_indices, dest_indices = noncircshift(A, offsets) 37 | print("Shifted array:") 38 | print(B) 39 | print("Source indices:") 40 | print(src_indices) 41 | print("Destination indices:") 42 | print(dest_indices) 43 | -------------------------------------------------------------------------------- /code/PlotModel.py: -------------------------------------------------------------------------------- 1 | # Plot the search map 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | def plot_model(model): 8 | MAP_SIZE = model["MAPSIZE"] 9 | x = np.arange(1, MAP_SIZE + 1) 10 | y = np.arange(1, MAP_SIZE + 1) 11 | X, Y = np.meshgrid(x, y) 12 | 13 | plt.clf() 14 | h = plt.pcolormesh(X, Y, model["Pmap"]) 15 | 16 | # Start position 17 | plt.plot( 18 | model["xs"] + MAP_SIZE // 2 + 0.5, 19 | model["ys"] + MAP_SIZE // 2 + 0.5, 20 | "wo", 21 | markersize=3, 22 | markerfacecolor="w", 23 | linewidth=1, 24 | ) 25 | 26 | plt.xlabel("x (cell)") 27 | plt.ylabel("y (cell)") 28 | 29 | # Set properties 30 | h.set_linewidth(0.1) 31 | cb = plt.colorbar(h) 32 | cb.formatter.set_powerlimits((-3, -3)) 33 | cb.update_ticks() 34 | gcaP = plt.gca().get_position() 35 | cbP = cb.ax.get_position() 36 | cbP = (cbP.x0, cbP.y0, cbP.width / 2, cbP.height) # Change the colorbar width 37 | cb.ax.set_position(cbP) 38 | plt.gca().set_position(gcaP) 39 | plt.gcf().set_size_inches(3.5, 2.5) # Set the map size 40 | plt.show() 41 | 42 | 43 | # Example usage 44 | if __name__ == "__main__": 45 | model = {"MAPSIZE": 40, "Pmap": np.random.rand(40, 40), "xs": 0, "ys": 0} 46 | plot_model(model) 47 | -------------------------------------------------------------------------------- /code/PathFromMotion.py: -------------------------------------------------------------------------------- 1 | # Create a search path from the encoded motions 2 | # 3 | 4 | import numpy as np 5 | from MotionDecode import motion_decode 6 | 7 | 8 | def path_from_motion(position, model): 9 | n = model["n"] 10 | xs = model["xs"] 11 | ys = model["ys"] 12 | path = np.zeros((n, 2)) # path initialisation 13 | current_node = np.array([xs, ys]) 14 | for i in range(n): 15 | motion = position[i, :] 16 | next_move = motion_decode(motion) 17 | next_node = current_node + next_move 18 | 19 | # Limit the path to be within the map 20 | # x direction 21 | if next_node[0] > model["xmax"]: 22 | next_node[0] = model["xmax"] 23 | elif next_node[0] < model["xmin"]: 24 | next_node[0] = model["xmin"] 25 | # y direction 26 | if next_node[1] > model["ymax"]: 27 | next_node[1] = model["ymax"] 28 | elif next_node[1] < model["ymin"]: 29 | next_node[1] = model["ymin"] 30 | 31 | path[i, :] = current_node 32 | current_node = next_node 33 | 34 | return path 35 | 36 | 37 | # Example usage 38 | if __name__ == "__main__": 39 | 40 | from MotionDecode import motion_decode 41 | 42 | model = {"n": 4, "xs": 0, "ys": 0, "xmin": -10, "xmax": 10, "ymin": -10, "ymax": 10} 43 | position = np.array([[1, 0], [1, 0], [0, 1], [0, 1]]) 44 | 45 | print("Search path from encoded motions:") 46 | print(path_from_motion(position, model)) 47 | -------------------------------------------------------------------------------- /code/PlotSolution.py: -------------------------------------------------------------------------------- 1 | # Plot the search path 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | def plot_solution(tour, model): 8 | MAP_SIZE = model["MAPSIZE"] 9 | N = model["n"] # Path length 10 | x = np.arange(1, MAP_SIZE + 1) 11 | y = np.arange(1, MAP_SIZE + 1) 12 | X, Y = np.meshgrid(x, y) 13 | 14 | plt.clf() 15 | h = plt.pcolormesh(X, Y, model["Pmap"]) 16 | 17 | tourX = tour[:, 0] + model["xmax"] + 0.5 18 | tourY = tour[:, 1] + model["ymax"] + 0.5 19 | 20 | plt.plot(tourX, tourY, "wo-", markersize=3, markerfacecolor="w", linewidth=1) 21 | plt.plot(tourX[0], tourY[0], "ro-", markersize=2, markerfacecolor="r", linewidth=1) 22 | 23 | plt.xlabel("x (cell)") 24 | plt.ylabel("y (cell)") 25 | 26 | # Set properties 27 | h.set_linewidth(0.1) 28 | cb = plt.colorbar(h) 29 | cb.formatter.set_powerlimits((-3, -3)) 30 | cb.update_ticks() 31 | gcaP = plt.gca().get_position() 32 | cbP = cb.ax.get_position() 33 | cbP = (cbP.x0, cbP.y0, cbP.width / 2, cbP.height) # Change the colorbar width 34 | cb.ax.set_position(cbP) 35 | plt.gca().set_position(gcaP) 36 | plt.gcf().set_size_inches(3.5, 2.5) # Set the map size 37 | 38 | plt.show() 39 | 40 | 41 | # Example usage 42 | if __name__ == "__main__": 43 | tour = np.random.randint(0, 20, (10, 2)) # Example tour data 44 | model = { 45 | "MAPSIZE": 40, 46 | "Pmap": np.random.rand(40, 40), 47 | "xmax": 0, 48 | "ymax": 0, 49 | "n": 10, 50 | } 51 | plot_solution(tour, model) 52 | -------------------------------------------------------------------------------- /code/CheckMotion.py: -------------------------------------------------------------------------------- 1 | # Check if the encoded motion forms a valid path 2 | # 3 | 4 | import numpy as np 5 | from MotionDecode import motion_decode 6 | 7 | 8 | def check_motion(position, model): 9 | # Load model parameters 10 | n = model["n"] 11 | xs = model["xs"] 12 | ys = model["ys"] 13 | path = np.zeros((n, 2)) # The path consists of n nodes, each node is (x,y) 14 | current_node = np.array([xs, ys]) 15 | valid = True 16 | 17 | # Decode from the motions to a path 18 | for i in range(n): 19 | motion = position[i] 20 | next_move = motion_decode(motion) 21 | next_node = current_node + next_move 22 | 23 | # Out of map boundary 24 | if ( 25 | next_node[0] > model["xmax"] 26 | or next_node[0] < model["xmin"] 27 | or next_node[1] > model["ymax"] 28 | or next_node[1] < model["ymin"] 29 | ): 30 | valid = False 31 | return valid 32 | 33 | path[i] = next_node 34 | current_node = next_node 35 | 36 | # Check duplicate rows 37 | _, indices = np.unique(path, axis=0, return_index=True) 38 | if len(indices) < n: 39 | valid = False 40 | return valid 41 | 42 | 43 | # Example usage 44 | if __name__ == "__main__": 45 | 46 | from MotionDecode import motion_decode 47 | 48 | model = {"n": 4, "xs": 0, "ys": 0, "xmin": -2, "xmax": 2, "ymin": -2, "ymax": 2} 49 | position_valid = np.array([[1, 0], [1, 0], [0, 1], [0, 1]]) 50 | position_invalid1 = np.array([[1, 0], [1, 0], [0, 1], [1, 0]]) 51 | position_invalid2 = np.array([[1, 0], [0, 1], [-0.7071, -0.7071], [1, 0]]) 52 | 53 | print("Testing valid motion:") 54 | print(check_motion(position_valid, model)) # Should print True 55 | 56 | print("Testing invalid motion (out boundary node):") 57 | print(check_motion(position_invalid1, model)) # Should print False 58 | 59 | print("Testing invalid motion (duplicated node):") 60 | print(check_motion(position_invalid2, model)) # Should print False 61 | -------------------------------------------------------------------------------- /code/UpdateMap.py: -------------------------------------------------------------------------------- 1 | # Update the belief map given the current position of UAV and its 2 | # measurement 3 | 4 | import numpy as np 5 | from DirToMove import dir_to_move 6 | 7 | 8 | # input: 当前更新时刻, 路径长度, 目标运动长度, 目标运动方向, 当前无人机位置, 概率地图 9 | def update_map(current_step, path_length, total_moves, dir, location, map): 10 | map_size = np.shape(map) 11 | map_size_x = map_size[0] 12 | map_size_y = map_size[1] 13 | 14 | # Target move direction (totalMoves in the total path) 15 | move = dir_to_move(dir) 16 | 17 | if total_moves != 0: 18 | move_step = path_length / total_moves # 栅格运动特点，UAV与目标运动有比例关系 19 | if current_step % move_step == 0: # 整数倍关系时需要更新地图 20 | tmp_map = np.roll( 21 | map, move, axis=(0, 1) 22 | ) # 按照目标运动方向移动地图（目标转移概率地图） 23 | map = tmp_map / np.sum(tmp_map) # Scale it to 1 24 | 25 | p_sensor_no_detection = np.ones( 26 | (map_size_x, map_size_y) 27 | ) # Initialize the probability of no detection with ones 28 | p_sensor_no_detection[int(location["y"]), int(location["x"])] = ( 29 | 0 # For binary sensor model, the probability of no detection at UAV location is zero 30 | ) 31 | new_map = p_sensor_no_detection * map # Update the belief map 32 | scale_factor = np.sum(new_map) # Calculate the scaling factor 33 | new_map = new_map / scale_factor # Scale the updated belief map 34 | 35 | return scale_factor, new_map 36 | 37 | 38 | # Example usage 39 | if __name__ == "__main__": 40 | 41 | import matplotlib.pyplot as plt 42 | from DirToMove import dir_to_move 43 | 44 | # 定义模型参数 45 | current_step = 10 46 | path_length = 20 47 | total_moves = 4 48 | dir = "E" 49 | location = {"x": 2, "y": 2} 50 | map = np.ones((10, 10)) # 示例地图 51 | map[5, 5] = 5 # 目标位置 52 | map = map / np.sum(map) # 归一化 53 | 54 | # 更新地图 55 | scale_factor, new_map = update_map( 56 | current_step, path_length, total_moves, dir, location, map 57 | ) 58 | print("缩放因子:", scale_factor) 59 | print("更新后的地图:\n", new_map) 60 | 61 | # 可视化地图更新过程 62 | fig, axes = plt.subplots(1, 2, figsize=(10, 5)) # 创建子图 63 | 64 | # 绘制初始地图 65 | axes[0].imshow(map, cmap="viridis", origin="lower", vmin=0, vmax=1) 66 | axes[0].set_title("初始地图") 67 | 68 | # 绘制更新后的地图 69 | axes[1].imshow(new_map, cmap="viridis", origin="lower", vmin=0, vmax=1) 70 | axes[1].set_title("更新后的地图") 71 | 72 | plt.tight_layout() 73 | plt.show() 74 | -------------------------------------------------------------------------------- /code/CreateModel.py: -------------------------------------------------------------------------------- 1 | # Create the search map with initial belief(TPM) 2 | # 3 | 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | from scipy.stats import multivariate_normal 7 | 8 | 9 | def create_model(): 10 | # Create a grid map 11 | MAP_SIZE = 40 12 | x = np.arange(1, MAP_SIZE + 1) 13 | y = np.arange(1, MAP_SIZE + 1) 14 | X, Y = np.meshgrid( 15 | x, y 16 | ) # X row vectors are the same, Y column vectors are the same 17 | 18 | # Generate the probability map 19 | mu1 = np.array([10, 10]) # mean vector 20 | Sigma1 = MAP_SIZE * np.array( 21 | [[0.1, 0], [0, 0.1]] 22 | ) # covariance matrix includes four elements 23 | coords = np.column_stack( 24 | (X.flatten(), Y.flatten()) 25 | ) # matrix X and Y are flattened(降维) and stacked(堆叠合并) together 26 | F1 = multivariate_normal.pdf( 27 | coords, mean=mu1, cov=Sigma1 28 | ) # create a probability distribution 29 | F1 = np.reshape( 30 | F1, (len(y), len(x)) 31 | ) # reshape the distribution to the size of the map 32 | F1 = F1 / np.sum(F1) # normalize the distribution 33 | 34 | mu2 = np.array([5, 5]) 35 | Sigma2 = MAP_SIZE * np.array([[0.1, 0], [0, 0.1]]) 36 | F2 = multivariate_normal.pdf(coords, mean=mu2, cov=Sigma2) 37 | F2 = np.reshape(F2, (len(y), len(x))) 38 | F2 = F2 / np.sum(F2) 39 | 40 | Pmap = 0.5 * (F1 + F2) # normalize the probability map 41 | 42 | # Settings 43 | xmin = -MAP_SIZE // 2 44 | xmax = MAP_SIZE // 2 45 | ymin = -MAP_SIZE // 2 46 | ymax = MAP_SIZE // 2 47 | xs = 0 # Start search X position 48 | ys = 0 # Start search Y position 49 | n = 20 # Length of the search path?! 50 | MRANGE = 4 51 | 52 | # Model 53 | model = { 54 | "xs": xs, 55 | "ys": ys, 56 | "Pmap": Pmap, 57 | "n": n, 58 | "xmin": xmin, 59 | "xmax": xmax, 60 | "ymin": ymin, 61 | "ymax": ymax, 62 | "MRANGE": MRANGE, 63 | "MAPSIZE": MAP_SIZE, 64 | "X": X, 65 | "Y": Y, 66 | "targetMoves": 10, # The target moves setp that must be divisible by the path length (e.g, mod(N,move)=0) 67 | "targetDir": "E", # The target moves direction 68 | } 69 | 70 | return model 71 | 72 | 73 | # Example usage 74 | if __name__ == "__main__": 75 | model_instance = create_model() 76 | print(model_instance) 77 | 78 | # Visualization 79 | Pmap = model_instance["Pmap"] 80 | xmin = model_instance["xmin"] 81 | xmax = model_instance["xmax"] 82 | ymin = model_instance["ymin"] 83 | ymax = model_instance["ymax"] 84 | 85 | plt.figure(figsize=(8, 6)) 86 | plt.imshow(Pmap, cmap="viridis", extent=[xmin, xmax, ymin, ymax], origin="lower") 87 | plt.colorbar(label="Probability") 88 | plt.title("Transition Probability Map") 89 | plt.xlabel("X") 90 | plt.ylabel("Y") 91 | plt.grid(True) 92 | plt.show() 93 | -------------------------------------------------------------------------------- /code/MyCost.py: -------------------------------------------------------------------------------- 1 | # Calculate the cost associated to a search path 2 | # Return: costP - The cumulative probability of detection 3 | # 4 | 5 | import numpy as np 6 | from CheckMotion import check_motion 7 | from PathFromMotion import path_from_motion 8 | from UpdateMap import update_map 9 | 10 | 11 | def my_cost(position, model): 12 | if not check_motion(position, model): # Invalid path 13 | costP = 0 # Punish invalid paths 14 | return costP 15 | else: 16 | # Extract path nodes and convert to map coordinates 17 | path = path_from_motion(position, model) 18 | x = path[:, 0] 19 | y = path[:, 1] 20 | 21 | # Load map and path information/ 22 | Pmap = model["Pmap"] # Input map 23 | N = model["n"] # path length 24 | targetMoves = model["targetMoves"] # total moves of the target 25 | targetDir = model["targetDir"] # target movement direction 26 | 27 | pNodetectionAll = 1 # Initialize the Probability of No detection at all 28 | pDetection = np.zeros( 29 | N 30 | ) # Initialize the Probability of detection at each time step 31 | 32 | # Calculate the cost 33 | for i in range(N): 34 | location = { 35 | "x": x[i] + model["xmax"] + 1, 36 | "y": y[i] + model["ymax"] + 1, 37 | } # Shift location to [1, MAPSIZE] 38 | scaleFactor, Pmap = update_map( 39 | i, N, targetMoves, targetDir, location, Pmap 40 | ) # Update the probability map 41 | 42 | pNoDetection = scaleFactor # Probability of No Detection at time t is exactly the scaling factor 43 | pDetection[i] = pNodetectionAll * ( 44 | 1 - pNoDetection 45 | ) # Probability of Detection for the first time at time i 46 | pNodetectionAll *= pNoDetection 47 | 48 | costP = ( 49 | 1 - pNodetectionAll 50 | ) # Return the Cumulative Probability of detection up to now (P = 1 - R) 51 | return costP 52 | 53 | 54 | # Example usage and testing 55 | if __name__ == "__main__": 56 | 57 | from CheckMotion import check_motion 58 | from PathFromMotion import path_from_motion 59 | from UpdateMap import update_map 60 | from CreateRandomSolution import create_random_solution 61 | 62 | map = np.ones((10, 10)) # 示例地图 63 | map[5, 5] = 5 # 目标位置 64 | map = map / np.sum(map) # 归一化 65 | 66 | # Define model parameters 67 | model = { 68 | "Pmap": map, # Example probability map 69 | "n": 10, 70 | "xs": 0, 71 | "ys": 0, 72 | "xmin": -5, 73 | "xmax": 5, 74 | "ymin": -5, 75 | "ymax": 5, 76 | "targetMoves": 10, 77 | "targetDir": "E", 78 | } 79 | 80 | # Generate random position 81 | position = create_random_solution(model) 82 | 83 | # Calculate cost 84 | cost = my_cost(position, model) 85 | print("Cost of the path:", cost) 86 | -------------------------------------------------------------------------------- /code/CreateRandomSolution.py: -------------------------------------------------------------------------------- 1 | # Create random paths (solutions) 2 | # 3 | 4 | import numpy as np 5 | from MotionDecode import motion_decode 6 | 7 | 8 | def create_random_solution(model): 9 | n = model["n"] # Load the number of path nodes 10 | start_node = np.array([model["xs"], model["ys"]]) 11 | path = np.zeros((n, 2)) # path initialization 12 | max_it = 100 # Maximum number of trial iterations before resetting the path 13 | 14 | motions = np.array( 15 | [ 16 | [1, 0], 17 | [0.7071, 0.7071], 18 | [0, 1], 19 | [-0.7071, 0.7071], 20 | [-1, 0], 21 | [-0.7071, -0.7071], 22 | [0, -1], 23 | [0.7071, -0.7071], 24 | ] 25 | ) 26 | 27 | should_restart = True 28 | 29 | # Repeat until generating a valid path 30 | while should_restart: 31 | should_restart = False 32 | path = np.tile( 33 | start_node, (n, 1) 34 | ) # np.tile(A, reps)结果为重复A为reps(分行列)次 35 | position = np.zeros((n, 2)) # motion initialisation 36 | current_node = start_node 37 | for i in range(n): 38 | motion = motions[np.random.randint(0, len(motions)), :] 39 | invalid_flag = True 40 | it = 0 41 | while invalid_flag and it < max_it: 42 | next_move = motion_decode(motion) 43 | next_node = current_node + next_move 44 | invalid_flag = False 45 | 46 | # Limit the path to be within the map 47 | # Out of x direction -> Move it back 48 | if next_node[0] > model["xmax"]: 49 | motion = motions[4, :] # Move it [-1, 0] to back 50 | invalid_flag = True 51 | it += 1 52 | elif next_node[0] < model["xmin"]: 53 | motion = motions[0, :] # Move it [1, 0] to back 54 | invalid_flag = True 55 | it += 1 56 | 57 | # Out of y direction 58 | elif next_node[1] > model["ymax"]: 59 | motion = motions[6, :] 60 | invalid_flag = True 61 | it += 1 62 | elif next_node[1] < model["ymin"]: 63 | motion = motions[2, :] 64 | invalid_flag = True 65 | it += 1 66 | else: 67 | # Check duplicate nodes within a path 68 | for j in range(len(path)): 69 | if np.array_equal( 70 | next_node, path[j, :] 71 | ): # 判断两个数组是否相等 72 | motion = motions[np.random.randint(0, len(motions)), :] 73 | invalid_flag = True 74 | it += 1 75 | break 76 | 77 | # Restart the whole path 78 | if it >= max_it: 79 | should_restart = True 80 | break 81 | else: # Path ok 82 | path[i, :] = next_node 83 | current_node = next_node 84 | position[i, :] = motion 85 | 86 | return position 87 | 88 | 89 | # Example usage 90 | if __name__ == "__main__": 91 | 92 | from MotionDecode import motion_decode 93 | 94 | model = {"n": 4, "xs": 0, "ys": 0, "xmin": -20, "xmax": 20, "ymin": -20, "ymax": 20} 95 | 96 | print("Random solution:") 97 | print(create_random_solution(model)) 98 | -------------------------------------------------------------------------------- /code/MPSO_MAIN.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from CreateModel import create_model 3 | from MyCost import my_cost 4 | from PathFromMotion import path_from_motion 5 | from PlotSolution import plot_solution 6 | from CreateRandomSolution import create_random_solution 7 | from DirToMove import dir_to_move 8 | from noncircshift import noncircshift 9 | 10 | # Assume CreateModel, MyCost, PathFromMotion, PlotSolution are defined in other files 11 | 12 | 13 | def main(): 14 | # CreateModel() function is assumed to be defined elsewhere 15 | model = create_model() # Create search map and parameters 16 | 17 | CostFunction = lambda x: my_cost(x, model) # Cost Function 18 | 19 | nVar = model[ 20 | "n" 21 | ] # Number of Decision Variables = searching dimension of PSO = number of movements 22 | 23 | VarSize = (nVar, 2) # Size of Decision Variables Matrix 24 | 25 | VarMin = -model["MRANGE"] # Lower Bound of particles (Variables) 26 | VarMax = model["MRANGE"] # Upper Bound of particles 27 | 28 | # PSO Parameters 29 | MaxIt = 100 # Maximum Number of Iterations 30 | nPop = 1000 # Population Size (Swarm Size) 31 | w = 1 # Inertia Weight 32 | wdamp = 0.98 # Inertia Weight Damping Ratio 33 | c1 = 2.5 # Personal Learning Coefficient 34 | c2 = 2.5 # Global Learning Coefficient 35 | alpha = 2 36 | VelMax = alpha * (VarMax - VarMin) # Maximum Velocity 37 | VelMin = -VelMax # Minimum Velocity 38 | 39 | # Initialization 40 | empty_particle = { 41 | "Position": None, 42 | "Velocity": None, 43 | "Cost": None, 44 | "Best": {"Position": None, "Cost": None}, 45 | } 46 | GlobalBest = {"Cost": -1} # Maximization problem 47 | particles = [empty_particle.copy() for _ in range(nPop)] 48 | 49 | # Initialization Loop 50 | for i in range(nPop): 51 | # Initialize Position 52 | particles[i]["Position"] = create_random_solution(model) 53 | # Initialize Velocity 54 | particles[i]["Velocity"] = np.zeros(VarSize) 55 | # Evaluation 56 | costP = CostFunction(particles[i]["Position"]) 57 | particles[i]["Cost"] = costP 58 | # Update Personal Best 59 | particles[i]["Best"]["Position"] = particles[i]["Position"] 60 | particles[i]["Best"]["Cost"] = particles[i]["Cost"] 61 | # Update Global Best 62 | if particles[i]["Best"]["Cost"] > GlobalBest["Cost"]: 63 | GlobalBest = particles[i]["Best"].copy() 64 | 65 | # Array to Hold Best Cost Values at Each Iteration 66 | BestCost = np.zeros(MaxIt) 67 | 68 | # PSO Main Loop 69 | for it in range(MaxIt): 70 | for i in range(nPop): 71 | # Update Velocity 72 | particles[i]["Velocity"] = ( 73 | w * particles[i]["Velocity"] 74 | + c1 75 | * np.random.rand(*VarSize) 76 | * (particles[i]["Best"]["Position"] - particles[i]["Position"]) 77 | + c2 78 | * np.random.rand(*VarSize) 79 | * (GlobalBest["Position"] - particles[i]["Position"]) 80 | ) 81 | # Update Velocity Bounds 82 | particles[i]["Velocity"] = np.maximum(particles[i]["Velocity"], VelMin) 83 | particles[i]["Velocity"] = np.minimum(particles[i]["Velocity"], VelMax) 84 | # Update Position 85 | particles[i]["Position"] = ( 86 | particles[i]["Position"] + particles[i]["Velocity"] 87 | ) 88 | # Update Position Bounds 89 | particles[i]["Position"] = np.maximum(particles[i]["Position"], VarMin) 90 | particles[i]["Position"] = np.minimum(particles[i]["Position"], VarMax) 91 | # Evaluation 92 | costP = CostFunction(particles[i]["Position"]) 93 | particles[i]["Cost"] = costP 94 | # Update Personal Best 95 | if particles[i]["Cost"] > particles[i]["Best"]["Cost"]: 96 | particles[i]["Best"]["Position"] = particles[i]["Position"] 97 | particles[i]["Best"]["Cost"] = particles[i]["Cost"] 98 | # Update Global Best 99 | if particles[i]["Best"]["Cost"] > GlobalBest["Cost"]: 100 | GlobalBest = particles[i]["Best"].copy() 101 | 102 | # Update Best Cost Ever Found 103 | BestCost[it] = GlobalBest["Cost"] 104 | # Inertia Weight Damping 105 | w = w * wdamp 106 | 107 | # Show Iteration Information 108 | print("Iteration {}: Best Cost = {}".format(it + 1, BestCost[it])) 109 | 110 | # Results 111 | # Updade Map in Accordance to the Target Moves 112 | targetMoves = model["targetMoves"] # Number of Target Moves (Zero means static) 113 | moveDir = dir_to_move(model["targetDir"]) # Direction of the Target Movement 114 | moveArr = targetMoves * moveDir 115 | updatedMap, _, __ = noncircshift(model["Pmap"], moveArr) 116 | newModel = model 117 | newModel["Pmap"] = updatedMap 118 | 119 | # Plot Solution 120 | # PathFromMotion and PlotSolution functions are assumed to be defined elsewhere 121 | path = path_from_motion( 122 | GlobalBest["Position"], model 123 | ) # Convert from Motion to Cartesian Space 124 | plot_solution(path, newModel) 125 | 126 | # Plot Best Cost Over Iterations 127 | import matplotlib.pyplot as plt 128 | 129 | plt.figure() 130 | plt.plot(BestCost, linewidth=2) 131 | plt.xlabel("Iteration") 132 | plt.ylabel("Best Cost") 133 | plt.grid(True) 134 | plt.show() 135 | 136 | 137 | if __name__ == "__main__": 138 | main() 139 | --------------------------------------------------------------------------------