jasder
/
XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

修改编队算法

This commit is contained in:
Robin Shaun 2020-08-07 12:48:15 +08:00
parent 481d786671
commit 7db4fcdc39
4 changed files with 219 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
communication/multi_vehicle_communication.sh
View File

@ -1,5 +1,5 @@
#!/bin/bash
iris_num=9
iris_num=6
typhoon_h480_num=0
solo_num=0
plane_num=0

228
coordination/formation_demo/follower.py
View File

@ -35,8 +35,8 @@ class Follower:
self.formation_config = 'waiting'
self.following_count = 0
self.Kp = 1
self.velxy_max = 0.8
self.velz_max = 0.8
self.velxy_max = 0.6 #0.8
self.velz_max = 1
self.following_local_pose = [PoseStamped() for i in range(self.uav_num)]
self.following_local_pose_sub = [None]*self.uav_num
self.arrive_count = 0
@ -86,8 +86,30 @@ class Follower:
self.cmd_pub.publish(self.offboard)
self.info_pub.publish("Received")
print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config)
if not self.formation_config=='waiting':
self.L_matrix = self.get_L_central_matrix()
if self.formation_config=='waiting':
self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config]) #self.get_L_central_matrix()
else:
if self.first_formation:
self.first_formation=False
self.orig_formation=formation_dict[self.formation_config]
self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config])
#self.L_matrix = self.get_L_central_matrix()
else:
#self.new_formation=self.get_new_formation(self.orig_formation,formation_dict[self.formation_config])
self.adj_matrix = self.build_graph(self.orig_formation,formation_dict[self.formation_config])
self.label_left = numpy.max(self.adj_matrix, axis=1) # init label for the left
self.label_right = numpy.array([0]*(self.uav_num-1)) # init label for the right set
self.match_right = numpy.array([-1] *(self.uav_num-1))
self.visit_left = numpy.array([0]*(self.uav_num-1))
self.visit_right = numpy.array([0]*(self.uav_num-1))
self.slack_right = numpy.array([100]*(self.uav_num-1))
self.change_id = self.KM()
self.new_formation=self.get_new_formation(self.change_id,formation_dict[self.formation_config])
self.L_matrix = self.get_L_matrix(self.new_formation)
#self.L_matrix = self.get_L_central_matrix()
self.orig_formation=self.new_formation
self.following_ids = numpy.argwhere(self.L_matrix[self.id,:] == 1)
self.following_count = 0
for i in range(self.uav_num):
@ -96,19 +118,23 @@ class Follower:
for following_id in self.following_ids:
self.following_local_pose_sub[following_id[0]] = rospy.Subscriber(self.uav_type+'_'+str(following_id[0])+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0])
self.following_count += 1
if self.id==3:
print(self.L_matrix)
self.cmd_vel_enu.linear = Vector3(0, 0, 0)
for following_id in self.following_ids:
self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + formation_dict[self.formation_config][0, self.id-2]
self.cmd_vel_enu.linear.y += self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + formation_dict[self.formation_config][1, self.id-2]
self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + formation_dict[self.formation_config][2, self.id-2]
self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + formation_dict[self.formation_config][0, self.id-1]
self.cmd_vel_enu.linear.y += self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + formation_dict[self.formation_config][1, self.id-1]
self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + formation_dict[self.formation_config][2, self.id-1]
if not following_id[0] == 0:
self.cmd_vel_enu.linear.x -= formation_dict[self.formation_config][0, following_id[0]-1]
self.cmd_vel_enu.linear.y -= formation_dict[self.formation_config][1, following_id[0]-1]
self.cmd_vel_enu.linear.z -= formation_dict[self.formation_config][2, following_id[0]-1]
self.cmd_vel_enu.linear.x = self.Kp * self.cmd_vel_enu.linear.x + self.avoid_vel.x
self.cmd_vel_enu.linear.y = self.Kp * self.cmd_vel_enu.linear.y + self.avoid_vel.y
self.cmd_vel_enu.linear.z = self.Kp * self.cmd_vel_enu.linear.z + self.avoid_vel.z
self.cmd_vel_enu.linear.x = self.Kp * self.cmd_vel_enu.linear.x + self.avoid_vel.x
self.cmd_vel_enu.linear.y = self.Kp * self.cmd_vel_enu.linear.y + self.avoid_vel.y
self.cmd_vel_enu.linear.z = self.Kp * self.cmd_vel_enu.linear.z + self.avoid_vel.z
if self.cmd_vel_enu.linear.x > self.velxy_max:
self.cmd_vel_enu.linear.x = self.velxy_max
@ -131,13 +157,193 @@ class Follower:
self.arrive_count = 0
rate.sleep()
def build_graph(self,orig_formation,change_formation):
distance=[[0 for i in range(self.uav_num-1)]for j in range(self.uav_num-1)]
for i in range(self.uav_num-1):
for j in range(self.uav_num-1):
distance[i][j]=numpy.linalg.norm(orig_formation[:,i]-change_formation[:,j])
distance[i][j]=int(50-distance[i][j])
return distance
def find_path(self,i):
self.visit_left[i] = True
for j, match_weight in enumerate(self.adj_matrix[i],start=0):
if self.visit_right[j]:
continue
gap = self.label_left[i] + self.label_right[j] - match_weight
if gap == 0 :
self.visit_right[j] = True
if self.match_right[j]==-1 or self.find_path(self.match_right[j]):
self.match_right[j] = i
return True
else:
self.slack_right[j]=min(gap,self.slack_right[j])
#print('1',slack_right)
return False
def KM(self):
for i in range(self.uav_num-1):
#print(i)
self.slack_right = numpy.array([100]*(self.uav_num-1))
while True:
self.visit_left = numpy.array([0]*(self.uav_num-1))
self.visit_right = numpy.array([0]*(self.uav_num-1))
if self.find_path(i):
break
d = numpy.inf
#print ('2',slack_right)
for j, slack in enumerate(self.slack_right):
if not self.visit_right[j] :
d = min(d,slack)
#print(d)
for k in range(self.uav_num-1):
if self.visit_left[k]:
self.label_left[k] -= d
if self.visit_right[k]:
self.label_right[k] += d
else:
self.slack_right[k] -=d
return self.match_right
def get_new_formation(self,change_id,change_formation):
new_formation=numpy.zeros((3,self.uav_num-1))
position=numpy.zeros((3,self.uav_num-1))
change_id=[i + 1 for i in change_id]
#print (change_id)
for i in range(0,self.uav_num-1):
position[:,i]=change_formation[:,i]
for i in range(0,self.uav_num-1):
for j in range(0,self.uav_num-1):
if (j+1)==change_id[i]:
new_formation[:,i]=position[:,j]
return new_formation
#函数输入为相对leader的位置矩阵和无人机数量，输出为L矩阵
def get_L_matrix(self,rel_posi):
c_num=int((self.uav_num-1)/2)
min_num_index_list = [0]*c_num
comm=[[]for i in range (self.uav_num)]
w=numpy.ones((self.uav_num,self.uav_num))*0
nodes_next=[]
node_flag = [self.uav_num-1]
node_mid_flag=[]
rel_d=[0]*(self.uav_num-1)
for i in range(0,self.uav_num-1):
rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
c=numpy.copy(rel_d)
c.sort()
count=0
for j in range(0,c_num):
for i in range(0,self.uav_num-1):
if rel_d[i]==c[j]:
if not i in node_mid_flag:
min_num_index_list[count]=i
node_mid_flag.append(i)
count=count+1
if count==c_num:
break
if count==c_num:
break
for j in range(0,c_num):
nodes_next.append(min_num_index_list[j])
comm[self.uav_num-1].append(min_num_index_list[j])
size_=len(node_flag)
while (nodes_next!=[]) and (size_<(self.uav_num-1)):
next_node= nodes_next[0]
nodes_next=nodes_next[1:]
min_num_index_list = [0]*c_num
node_mid_flag=[]
rel_d=[0]*(self.uav_num-1)
for i in range(0,self.uav_num-1):
if i==next_node or i in node_flag:
rel_d[i]=2000
else:
rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2)
c=numpy.copy(rel_d)
c.sort()
count=0
for j in range(0,c_num):
for i in range(0,self.uav_num-1):
if rel_d[i]==c[j]:
if not i in node_mid_flag:
min_num_index_list[count]=i
node_mid_flag.append(i)
count=count+1
if count==c_num:
break
if count==c_num:
break
node_flag.append(next_node)
size_=len(node_flag)
for j in range(0,c_num):
if min_num_index_list[j] in node_flag:
nodes_next=nodes_next
else:
if min_num_index_list[j] in nodes_next:
nodes_next=nodes_next
else:
nodes_next.append(min_num_index_list[j])
comm[next_node].append(min_num_index_list[j])
for i in range (0,self.uav_num):
for j in range(0,self.uav_num-1):
if i==0:
if j in comm[self.uav_num-1]:
w[j+1][i]=1
else:
w[j+1][i]=0
else:
if j in comm[i-1]:
w[j+1][i]=1
else:
w[j+1][i]=0
L=w
for i in range (0,self.uav_num):
L[i][i]=-sum(w[i])
return L
#central-station control
def get_L_central_matrix(self):
L=numpy.zeros((self.uav_num,self.uav_num))
for i in range(1,self.uav_num):
L[i][0]=1
L[i][i]=-1
'''
L=numpy.array([[-0.,0. , 0., 0., 0., 0.],[ 1. ,-1. , 0. , 0. , 0. , 0.],[ 1., 2,-2. , 0. , 0. , 0.],[ 1., 0., 1 ,-2. , 0. , 0.],[ 1. , 0. , 0. , 1 ,-2. , 0.],[ 1. , 0. , 0. , 0. , 1 ,-2.]])
'''
return L
if __name__ == '__main__':

87
coordination/launch_generator/multi_vehicle.launch
View File

@ -192,92 +192,5 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_6 -->
<group ns="iris_6">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="7"/>
<arg name="ID_in_group" value="6"/>
<arg name="fcu_url" default="udp://:14547@127.0.0.1:34584"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="24574"/>
<arg name="mavlink_tcp_port" value="4567"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_7 -->
<group ns="iris_7">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="8"/>
<arg name="ID_in_group" value="7"/>
<arg name="fcu_url" default="udp://:14548@127.0.0.1:34586"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="3"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="24576"/>
<arg name="mavlink_tcp_port" value="4568"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_8 -->
<group ns="iris_8">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="9"/>
<arg name="ID_in_group" value="8"/>
<arg name="fcu_url" default="udp://:14549@127.0.0.1:34588"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="24578"/>
<arg name="mavlink_tcp_port" value="4569"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->

2
motion_planning/param/dwa_local_planner_params.yaml
View File

@ -26,7 +26,7 @@ DWAPlannerROS:
# Forward Simulation Parameters
sim_time: 2.0
vx_samples: 20
vy_samples: 0
vy_samples: 20
vth_samples: 40
controller_frequency: 10.0