完善编队

control/follower_accel_control.py

@@ -162,6 +162,38 @@ class Follower:
                 self.cmd_pub.publish(self.hover)
             rate.sleep()
 
+     def get_new_formation(self,orig_formation,change_formation):
+        new_formation=numpy.zeros((3,self.uav_num-1))
+        position=numpy.zeros((3,self.uav_num-1))
+
+        num_array=range(0,self.uav_num-1)
+        num_array_possi=list(permutations(num_array, self.uav_num-1))
+        possi_num=len(num_array_possi)
+
+        distance=[0 for i in range(self.uav_num-1)]
+        distance_max=100   # maybe larger
+        min_num=0
+
+        for k in range(possi_num): 
+            for i in range(self.uav_num-1):       
+                distance[i]=numpy.linalg.norm(orig_formation[:,i]-change_formation[:,num_array_possi[k][i]])
+            distance_mid_max=max(distance)
+            if distance_mid_max<distance_max:
+                distance_max=distance_mid_max
+                min_num=k
+
+        change_id=num_array_possi[min_num]
+        change_id=[i + 1 for i in change_id] 
+        #print (change_id)
+        for i in range(0,self.uav_num-1):
+            position[:,i]=orig_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):
 	
@@ -366,6 +398,4 @@ class Follower:
 
 if __name__ == '__main__':
     follower = Follower(int(sys.argv[1]),int(sys.argv[2]))
-  #follower = Follower(int(sys.argv[1]),9)
-    #follower = Follower(9,9)
     follower.loop()   

control/follower_accel_control_new.py

@@ -27,7 +27,7 @@ class Follower:
         self.following_ids = []
         self.formation_config = 'waiting'
         self.following_count = 0
-        self.Kp = 20
+        self.kp = 1
         #self.kr = (4/int((self.uav_num-1)/2))**0.5
         self.kr = 0.1
         self.f = 100
@@ -116,9 +116,9 @@ class Follower:
             for following_id in self.following_ids:
                 if self.following_local_pose[following_id[0]] == None and self.following_local_velocity[following_id[0]] == None:
                     print(following_id)     
-                self.cmd_accel_enu.x += self.following_local_pose[following_id[0]].pose.position.x + self.kr * self.following_local_velocity[following_id[0]].twist.linear.x - self.local_pose.pose.position.x - self.kr * self.local_velocity.twist.linear.x + formation_dict_6[self.formation_config][0, self.id-2]
-                self.cmd_accel_enu.y += self.following_local_pose[following_id[0]].pose.position.y + self.kr * self.following_local_velocity[following_id[0]].twist.linear.y - self.local_pose.pose.position.y - self.kr * self.local_velocity.twist.linear.y + formation_dict_6[self.formation_config][1, self.id-2]
-                self.cmd_accel_enu.z += self.following_local_pose[following_id[0]].pose.position.z + self.kr * self.following_local_velocity[following_id[0]].twist.linear.z - self.local_pose.pose.position.z - self.kr * self.local_velocity.twist.linear.z + formation_dict_6[self.formation_config][2, self.id-2]
+                self.cmd_accel_enu.x += self.kp * (self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + self.kr * (self.following_local_velocity[following_id[0]].twist.linear.x - self.local_velocity.twist.linear.x) + formation_dict_6[self.formation_config][0, self.id-2])
+                self.cmd_accel_enu.y += self.kp * (self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + self.kr * (self.following_local_velocity[following_id[0]].twist.linear.y - self.local_velocity.twist.linear.y) + formation_dict_6[self.formation_config][1, self.id-2])
+                self.cmd_accel_enu.z += self.kp * (self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + formation_dict_6[self.formation_config][2, self.id-2]+ self.kr * (self.following_local_velocity[following_id[0]].twist.linear.z - self.local_velocity.twist.linear.z))
             
                 if not following_id[0] == 0:
                     self.cmd_accel_enu.x -= formation_dict_6[self.formation_config][0, following_id[0]-1]

control/run.sh

@@ -3,7 +3,7 @@ python leader.py $1 &
 uav_num=2
 while(( $uav_num<= $1 )) 
 do
-    python follower_accel_control.py $uav_num $1&
+    python follower_accel_control_new.py $uav_num $1&
     #echo $uav_num
     let "uav_num++"
 done

control/stop.sh

@@ -1,2 +1,2 @@
-kill -9 $(ps -ef|grep follower_accel_control.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
+kill -9 $(ps -ef|grep follower_accel_control_new.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
 kill -9 $(ps -ef|grep leader.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')