#基于ROS的编队控制

前言

研究生新生，着手研究编队控制。本文为一个记录：在ROS自带的小乌龟环境下实现基于领航者的三角形编队控制。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、基于领航者-跟随者的编队控制方法实现

步骤1.根据领航者的位姿信息和目标队形，计算跟随平衡车的理想坐标和方位角：

在本例中方位角设定相等于领航者的方位角，理想坐标为领航者左右两侧靠后

步骤2,由跟随者当前实际位姿信息和所求理想位姿信息，计算其坐标误差和方位角误差：

如图所示，在全局坐标系中$$p_{Di}={\left[\begin{array}{lll}{x}_{Di}& y_{Di}& {\phi }_{Di}\end{array}\right]}^T$$表示i号跟随者的期望位姿信息
$$p_{Fi}={\left[\begin{array}{lll}{x}_{Fi}& y_{Fi}& {\phi }_{Fi}\end{array}\right]}^T$$表示i号跟随机器人的实际位姿信息；
由平面几何关系可得
$$\begin{array}{rl}{x}_e& =\Delta x\cos \left({\phi }_{Fi}\right)+\Delta y\sin \left({\phi }_{Fi}\right)\\ y_e& =\Delta y\cos \left({\phi }_{Fi}\right)-\Delta x\sin \left({\phi }_{Fi}\right)\\ {\phi }_e& ={\phi }_{Di}-{\phi }_{Fi}\end{array}$$
通过设计合适的控制律使得误差为0，即可实现编队控制。
步骤3.构建运动学控制器，前人已实现$$\begin{array}{c}{v}_{Fi}=v_{Di}\cos \left({\phi }_e\right)+k_2{x}_e\\ w_{Fi}=w_{Di}+k_1{v}_{Di}{y}_e+k_3\sin \left({\phi }_e\right)\end{array}$$

二、在小乌龟仿真环境中实现三角形编队控制

代码实现

将乌龟节点启动自带的小龟视为领航者，设定有两个跟随者，以三角形编队运动

新建两个乌龟

代码如下（示例）：
python创建龟2

#! /usr/bin/env python 


import rospy
from rospy import client
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse

if __name__ == "__main__":
    rospy.init_node("newturtle_p")
    client = rospy.ServiceProxy("/spawn",Spawn)
    request = SpawnRequest()
    request.x = 4.5
    request.y = 2.0
    request.theta = 0
    request.name = "turtle2"

    client.wait_for_service()
    try:
        response = client.call(request)
        rospy.loginfo("生成成功")
    except Exception as e:
        rospy.loginfo("wrong")

C++创建龟3

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{   
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"my_turtle");
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 1.0;
    spawn.request.theta = 0.0;
    spawn.request.name = "turtle3";

    client.waitForExistence();
    bool flag = client.call(spawn);
    if (flag)
    {
        ROS_INFO("乌龟成功，新乌龟叫%s",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("失败！");
    }
    
    return 0;
}

配置Cmakelist

add_executable(test_01_newturtle src/test_01_newturtle.cpp)
add_dependencies(test_01_newturtle ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_01_newturtle
  ${catkin_LIBRARIES}
)
catkin_install_python(PROGRAMS
  scripts/test01_newtutle_p.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

由控制律计算小龟2、3速度信息并发布

只显示控制龟2，龟3同理：

#! /usr/bin/env python

import rospy
import math
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist

def turtle1callback(pose):
    ideal_pose.x = pose.x
    ideal_pose.y = pose.y
    ideal_pose.theta = pose.theta
    ideal_pose.linear_velocity = pose.linear_velocity
    ideal_pose.angular_velocity = pose.angular_velocity
   
def turtle2callback(pose):
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1)
    error_pose.x = (ideal_pose.x-pose.x)*math.cos(pose.theta) + (ideal_pose.y-pose.y)*math.sin(pose.theta) - 1
    error_pose.y = (ideal_pose.y-pose.y)*math.cos(pose.theta) - (ideal_pose.x-pose.x)*math.sin(pose.theta) - 1
    error_pose.theta = math.degrees(ideal_pose.theta) - math.degrees(pose.theta)
    error_pose.theta = math.radians(error_pose.theta)
    error_pose.linear_velocity = ideal_pose.linear_velocity
    error_pose.angular_velocity = ideal_pose.angular_velocity    error_pose.linear_velocity*math.cos(error_pose.theta)+1*error_pose.x
    twist.linear.x = 2*math.cos(error_pose.theta)+ 4 * error_pose.x
    twist.angular.z = 1 * ideal_pose.linear_velocity * error_pose.y + 1* error_pose.linear_velocity * math.sin(error_pose.theta)

    if(abs(twist.linear.x)>4):
       twist.linear.x = 4
    pub.publish(twist)

if __name__ == "__main__":
    rospy.init_node("ss")
    ideal_pose = Pose()
    error_pose = Pose()
    sub1 = rospy.Subscriber("/turtle1/pose",Pose,turtle1callback,queue_size=10)
    sub2 = rospy.Subscriber("/turtle2/pose",Pose,turtle2callback,queue_size=10)
    rospy.spin()

代码编译通过后，启动小乌龟节点以及以上节点，再给领航龟发布速度信息或使用键盘控制节点。

结果展示

ROS小龟编队控制

---

总结

迈出了第一步，本文主要特点在于系统是闭环。
在小乌龟界面实现后，会在gazebo环境下的小车仿真实现。
一起进步吧