http://【2022数模国赛 B题【思路+完整代码】】 https://www.bilibili.com/video/BV1mT411K7mQ/?share_source=copy_web&vd_source=64ecf790ae9e305d0d5161aeed31a345 

第一问（1） 

import math
import cmath
import numpy as np
from itertools import combinationsacc = 10  # 计算结果保留五位小数
closest = 5  # 最多向心偏差五米
farthest = 20  # 最多离心偏差20米
dis_step = 0.1  # 距离步长
angle_max = 0.018  # 最大偏差角度（1°多一点）
ang_step = 0.1  # 角度步长F0 = [0, 0, 'F0']
F1 = [100, 0, 'F1']
F2 = [100, 2 / 9 * math.pi, 'F2']
F3 = [100, 4 / 9 * math.pi, 'F3']
F4 = [100, 6 / 9 * math.pi, 'F4']
F5 = [100, 8 / 9 * math.pi, 'F5']
F6 = [100, 10 / 9 * math.pi, 'F6']
F7 = [100, 12 / 9 * math.pi, 'F7']
F8 = [100, 14 / 9 * math.pi, 'F8']
F9 = [100, 16 / 9 * math.pi, 'F9']def polar_to_cartesian(p):x = p[0] * math.cos(p[1])y = p[0] * math.sin(p[1])return [x, y]def angle(ver, tra1, tra2):  # 角度计算函数a = distance_polar(tra1, tra2)b = distance_polar(ver, tra2)c = distance_polar(ver, tra1)n = math.pow(b, 2) + math.pow(c, 2) - math.pow(a, 2)d = 2 * b * creturn round(math.acos(abs(n) / d), acc)def distance_artesian(p1, p2):x = p1[0] - p2[0]y = p1[1] - p2[1]return math.pow(math.pow(x, 2) + math.pow(y, 2), 1 / 2)def distance_polar(p1, p2):  # 距离计算函数x = p1[0] * math.cos(p1[1]) - math.cos(p2[1]) * p2[0]y = p1[0] * math.sin(p1[1]) - math.sin(p2[1]) * p2[0]return math.pow(math.pow(x, 2) + math.pow(y, 2), 1 / 2)def var(li):  # 发射器组合生成器 li表示无人机列表k = list(combinations(li, 2))return kdef direction(ver, k):  # 信号接收器获取到的信号 ver代表接收器，k表示发射器组合res = []for i in k:res.append(angle(ver, i[0], i[1]))res.sort()return resdef tra(pre, key, li):  # 信号发射器列表 顺序生成器transmitter = []for i in combinations([x for x in li if x not in pre], key):transmitter.append(pre + list(i))return transmitterdef get_circle_r(p0, p1, r):if p1[0] == p0[0]:y0 = y1 = (p0[1] + p1[1]) / 2delta_y = (y0 - p0[1]) ** 2delta_x = math.sqrt(r ** 2 - delta_y)x0 = p1[0] - delta_xx1 = p1[0] + delta_xelse:C1 = (p1[0] ** 2 + p1[1] ** 2 - p0[0] ** 2 - p0[1] ** 2) / 2 / (p1[0] - p0[0])C2 = (p1[1] - p0[1]) / (p1[0] - p0[0])A = 1 + C2 ** 2B = 2 * (p0[0] - C1) * C2 - 2 * p0[1]C = (p0[0] - C1) ** 2 + p0[1] ** 2 - r ** 2y0 = (-B + math.sqrt(B * B - 4 * A * C)) / 2 / Ay1 = (-B - math.sqrt(B * B - 4 * A * C)) / 2 / Ax0 = C1 - C2 * y0x1 = C1 - C2 * y1return [x0, y0], [x1, y1]def get_circle_p(p1, p2, p3):x, y, z = p1[0] + p1[1] * 1j, p2[0] + p2[1] * 1j, p3[0] + p3[1] * 1jw = z - xw /= y - xc = (x - y) * (w - abs(w) ** 2) / 2j / w.imag - xreturn [-c.real, -c.imag]def insec(p1, r1, p2, r2):x = p1[0]y = p1[1]R = r1a = p2[0]b = p2[1]S = r2d = math.sqrt((abs(a - x)) ** 2 + (abs(b - y)) ** 2)if d > (R + S) or d < (abs(R - S)):# print("Two circles have no intersection")return None, Noneelif d == 0:# print("Two circles have same center!")return None, Noneelse:A = (R ** 2 - S ** 2 + d ** 2) / (2 * d)h = math.sqrt(R ** 2 - A ** 2)x2 = x + A * (a - x) / dy2 = y + A * (b - y) / dx3 = round(x2 - h * (b - y) / d, 2)y3 = round(y2 + h * (a - x) / d, 2)x4 = round(x2 + h * (b - y) / d, 2)y4 = round(y2 - h * (a - x) / d, 2)c1 = [x3, y3]c2 = [x4, y4]return c1, c2def location(ver, ang, tra0, tra1, tra2):ver = polar_to_cartesian(ver)tra0 = polar_to_cartesian(tra0)tra1 = polar_to_cartesian(tra1)tra2 = polar_to_cartesian(tra2)res = []r1 = abs(distance_artesian(tra1, tra0) / (2 * math.sin(ang[0])))r2 = abs(distance_artesian(tra2, tra0) / (2 * math.sin(ang[1])))o1_list = get_circle_r(tra1, tra0, r1)o1 = get_circle_p(tra1, tra0, ver)if distance_artesian(o1_list[0], o1) - r1 > distance_artesian(o1_list[1], o1) - r1:o1 = o1_list[1]else:o1 = o1_list[0]o2_list = get_circle_r(tra2, tra0, r2)o2 = get_circle_p(tra2, tra0, ver)if distance_artesian(o2_list[0], o2) - r2 > distance_artesian(o2_list[1], o2) - r2:o2 = o2_list[1]else:o2 = o2_list[0]p1, p2 = insec(o1, r1, o2, r2)if p1 is not None and p1 != [0.0, 0.0]:res.append(p1)if p2 is not None and p2 != [0.0, 0.0]:res.append(p2)result = res[0]for i in res:if distance_artesian(result, ver) > distance_artesian(i, ver):result = ireturn resultF3_v = [105, 119.75]
F3_ver = [F3_v[0], F3_v[1] * math.pi / 180]
print(math.degrees(angle(F3_ver, F1, F0)))
print(location(F4, [angle(F3_ver, F1, F0), angle(F3_ver, F2, F0)], F0, F1, F2), '预测的位置')
print(polar_to_cartesian(F3_ver), '实际位置')

第一问（2） 

import math
from itertools import combinationsacc = 5
F0 = [0, 0, 'F0']
F1 = [100, 0, 'F1']
F2 = [100, 2 / 9 * math.pi, 'F2']
F3 = [100, 4 / 9 * math.pi, 'F3']
F4 = [100, 6 / 9 * math.pi, 'F4']
F5 = [100, 8 / 9 * math.pi, 'F5']
F6 = [100, 10 / 9 * math.pi, 'F6']
F7 = [100, 12 / 9 * math.pi, 'F7']
F8 = [100, 14 / 9 * math.pi, 'F8']
F9 = [100, 16 / 9 * math.pi, 'F9']def angle(ver, tra1, tra2): # 角度计算函数a = distance(tra1, tra2)b = distance(ver, tra2)c = distance(ver, tra1)n = math.pow(b, 2) + math.pow(c, 2) - math.pow(a, 2)d = 2 * b * creturn round(math.degrees(math.acos(abs(n) / d)), acc)def distance(p1, p2): # 距离计算函数x = p1[0] * math.cos(p1[1]) - math.cos(p2[1]) * p2[0]y = p1[0] * math.sin(p1[1]) - math.sin(p2[1]) * p2[0]return math.pow(math.pow(x, 2) + math.pow(y, 2), 1 / 2)def var(li): # 发射器组合生成器 li表示无人机列表k = list(combinations(li, 2))return kdef direction(ver, k): # 信号接收器获取到的信号 ver代表接收器，k表示发射器组res = []for i in k:res.append(angle(ver, i[0], i[1]))res.sort()return resdef tra(pre, key, li): # 信号发射器列表 顺序生成器transmitter = []for i in combinations([x for x in li if x not in pre], key):transmitter.append(pre + list(i))return transmitterkey = int(input('额外信号发射器数量'))
pre = [F0, F1]
li = [F0, F1, F2, F3, F4, F5, F6, F7, F8, F9]
transmitter = tra(pre, key, li)
for i in transmitter:# 获取信号发射器列表print('当前发射器组合为' + str([x[2] for x in i]))k = var(i)result = []for j in li:# 获取信号接收器并判断是否满足条件if j in i:# 如果接收器本身是发射器则跳过continueelse: # 接收器不是发射器sign = direction(j, k)if sign in result:print(j[2] + '无法定位')breakelse:result.append(sign)else:print('当前发射器组合为' + str([x[2] for x in i]) + '测试通过')print(result)

第一问（3） 

import math
import cmath
import random
import numpy as np
from itertools import combinationsacc = 10  # 计算结果保留五位小数
closest = 5  # 最多向心偏差五米
farthest = 20  # 最多离心偏差20米
dis_step = 0.1  # 距离步长
angle_max = 0.018  # 最大偏差角度（1°多一点）
ang_step = 0.1  # 角度步长F0 = [0, 0, 'F0']
F1 = [100, 0, 'F1']
F2 = [100, 2 / 9 * math.pi, 'F2']
F3 = [100, 4 / 9 * math.pi, 'F3']
F4 = [100, 6 / 9 * math.pi, 'F4']
F5 = [100, 8 / 9 * math.pi, 'F5']
F6 = [100, 10 / 9 * math.pi, 'F6']
F7 = [100, 12 / 9 * math.pi, 'F7']
F8 = [100, 14 / 9 * math.pi, 'F8']
F9 = [100, 16 / 9 * math.pi, 'F9']FY0 = [0, 0]
FY1 = [100, 0]
FY2 = [98, 40.10 / 180 * math.pi]
FY3 = [112, 80.21 / 180 * math.pi]
FY4 = [105, 119.75/ 180 * math.pi]
FY5 = [98, 159.86/ 180 * math.pi]
FY6 = [112, 199.96/ 180 * math.pi]
FY7 = [105, 240.07/ 180 * math.pi]
FY8 = [98, 280.17/ 180 * math.pi]
FY9 = [112, 320.28/ 180 * math.pi]def angle_to_radian(p):return [p[0], p[1] * math.pi / 180]def polar_to_cartesian(p):x = p[0] * math.cos(p[1])y = p[0] * math.sin(p[1])return [x, y]def cartesian_to_polar(p):cn = complex(p[0], p[1])return cmath.polar(cn)def angle(ver, tra1, tra2):  # 角度计算函数a = distance_polar(tra1, tra2)b = distance_polar(ver, tra2)c = distance_polar(ver, tra1)n = math.pow(b, 2) + math.pow(c, 2) - math.pow(a, 2)d = 2 * b * creturn round(math.acos(abs(n) / d), acc)def distance_artesian(p1, p2):x = p1[0] - p2[0]y = p1[1] - p2[1]return math.pow(math.pow(x, 2) + math.pow(y, 2), 1 / 2)def distance_polar(p1, p2):  # 距离计算函数x = p1[0] * math.cos(p1[1]) - math.cos(p2[1]) * p2[0]y = p1[0] * math.sin(p1[1]) - math.sin(p2[1]) * p2[0]return math.pow(math.pow(x, 2) + math.pow(y, 2), 1 / 2)def var(li):  # 发射器组合生成器 li表示无人机列表k = list(combinations(li, 2))return kdef direction(ver, k):  # 信号接收器获取到的信号 ver代表接收器，k表示发射器组合res = []for i in k:res.append(angle(ver, i[0], i[1]))res.sort()return resdef tra(pre, key, li):  # 信号发射器列表 顺序生成器transmitter = []for i in combinations([x for x in li if x not in pre], key):transmitter.append(pre + list(i))return transmitterdef get_circle_r(p0, p1, r):if p1[0] == p0[0]:y0 = y1 = (p0[1] + p1[1]) / 2delta_y = (y0 - p0[1]) ** 2delta_x = math.sqrt(r ** 2 - delta_y)x0 = p1[0] - delta_xx1 = p1[0] + delta_xelse:C1 = (p1[0] ** 2 + p1[1] ** 2 - p0[0] ** 2 - p0[1] ** 2) / 2 / (p1[0] - p0[0])C2 = (p1[1] - p0[1]) / (p1[0] - p0[0])A = 1 + C2 ** 2B = 2 * (p0[0] - C1) * C2 - 2 * p0[1]C = (p0[0] - C1) ** 2 + p0[1] ** 2 - r ** 2y0 = (-B + math.sqrt(B * B - 4 * A * C)) / 2 / Ay1 = (-B - math.sqrt(B * B - 4 * A * C)) / 2 / Ax0 = C1 - C2 * y0x1 = C1 - C2 * y1return [x0, y0], [x1, y1]def get_circle_p(p1, p2, p3):x, y, z = p1[0] + p1[1] * 1j, p2[0] + p2[1] * 1j, p3[0] + p3[1] * 1jw = z - xw /= y - xc = (x - y) * (w - abs(w) ** 2) / 2j / w.imag - xreturn [-c.real, -c.imag]def insec(p1, r1, p2, r2):x = p1[0]y = p1[1]R = r1a = p2[0]b = p2[1]S = r2d = math.sqrt((abs(a - x)) ** 2 + (abs(b - y)) ** 2)if d > (R + S) or d < (abs(R - S)):# print("Two circles have no intersection")return None, Noneelif d == 0:# print("Two circles have same center!")return None, Noneelse:A = (R ** 2 - S ** 2 + d ** 2) / (2 * d)h = math.sqrt(R ** 2 - A ** 2)x2 = x + A * (a - x) / dy2 = y + A * (b - y) / dx3 = round(x2 - h * (b - y) / d, 2)y3 = round(y2 + h * (a - x) / d, 2)x4 = round(x2 + h * (b - y) / d, 2)y4 = round(y2 - h * (a - x) / d, 2)c1 = [x3, y3]c2 = [x4, y4]return c1, c2def location(ver, ang, tra0, tra1, tra2):ver = polar_to_cartesian(ver)tra0 = polar_to_cartesian(tra0)tra1 = polar_to_cartesian(tra1)tra2 = polar_to_cartesian(tra2)res = []r1 = abs(distance_artesian(tra1, tra0) / (2 * math.sin(ang[0])))r2 = abs(distance_artesian(tra2, tra0) / (2 * math.sin(ang[1])))o1_list = get_circle_r(tra1, tra0, r1)o1 = get_circle_p(tra1, tra0, ver)if distance_artesian(o1_list[0], o1) - r1 > distance_artesian(o1_list[1], o1) - r1:o1 = o1_list[1]else:o1 = o1_list[0]o2_list = get_circle_r(tra2, tra0, r2)o2 = get_circle_p(tra2, tra0, ver)if distance_artesian(o2_list[0], o2) - r2 > distance_artesian(o2_list[1], o2) - r2:o2 = o2_list[1]else:o2 = o2_list[0]p1, p2 = insec(o1, r1, o2, r2)if p1 is not None and p1 != [0.0, 0.0]:res.append(p1)if p2 is not None and p2 != [0.0, 0.0]:res.append(p2)result = res[0]for i in res:if distance_artesian(result, ver) > distance_artesian(i, ver):result = ireturn resultdef adjust(k1, k2, k3):par = random.uniform(0.05,0.1)kid = [par * angle(k1, F1, F0) + (1 - par) * angle(k3, F1, F0), par * angle(k1, F0, F2) + (1 - par) * angle(k3, F0, k2)]print(kid)print(location(k1, kid, F0, F1, k2))print(polar_to_cartesian(k1), '李响位置')print(polar_to_cartesian(k3), '实际位置')return location(k1, kid, F0, F1, k2)FY3 = adjust(F3,FY2,FY3)
FY4 = adjust(F4,FY2,FY4)
FY5 = adjust(F5,FY2,FY5)
FY6 = adjust(F6,FY2,FY6)
FY7 = adjust(F7,FY2,FY7)
FY8 = adjust(F8,FY2,FY8)
FY9 = adjust(F9,FY2,FY9)

第二问 （询问作者后编写，但应该没编对）

以FY15建立坐标轴
0：FY15
1:  FY14
2:  FY13
.
.
.
14:FY01
#无人机编队位置为无偏差直角
x = np.sqrt(2*2-1*2)
drone_ ideal_ location cartesian = [     
#第1列
[0,0],
[0,2],
[0,4],
[0,6],
[0,8],
#第2列
[X,1],
[X,3],
[X,5],
[X,7],
#第3列
[2*X,2],
[2*X,4],
[2*X,6],
#第4列
[3*X,3],
[3*X,5]
#第5列
[4*X,4]']
plot. location(drone_ ideal location cartesian)#除FY14,FY15,让其他无人机的位置进行一项随机偏移
drone_ ideal location cartesian=[ ]
for 1 in range(len(drone_ ideal _location .cartesian): 
location = drone_ ideal location cartesian[i]drone_ location cartesian.append(list(location))
if i in [0,1];continue
if random.random( )＞＝0.5drone_ location cartesian[i][0]=location[0]+random.random( )*8
else
drone_ location cartesian[i][0]=location[0]+random.random( )*8
if random.random( )＞＝0.5drone_ location cartesian[i][1]=location[1]+random.random( )*8
else
drone_ location cartesian[i][1]=location[1]+random.random( )*8
plot. location(drone_ ideal location cartesian)
if random.random( )<0.5#根据FY14,FY15，校准第一页x轴坐标，让第一列对齐
print（"根据FY14,FY15，先校准第一页x轴坐标，让第一列对齐"）
q3_adjust_ location（drone_ testing_location）
plot. location(drone_ testing_location)
#根据FY13,FY14,FY15，调整其他无人机
print（“根据FY13,FY14,FY15，调整其他无人机”）
q3_adjust_ location（drone_ testing_location）
plot. location(drone_ testing_location)
print