MPC路径跟踪Gazebo仿真

前言

由于工作的需要，要将ros_control默认的pid控制器换成mpc控制，且与网上许多博主实现不同的是，我们不是用MPC去跟踪我们自定义好的路径（比如自己提前踩点画一个圆，长方形或者其他形状的路径去进行跟踪），而是订阅规划器发布的plan话题，跟踪导航生成的路径。先附上实现效果吧（订阅的是teb发布的局部路径，参数调试还有待优化，由于采样频率以及prediction_horizon的参数设置问题导致跟踪的精度较高所以会出现抖动的现象）

MPC

正文

首先我们需要解决的是我们是如何知道当前控制小车运用的是PID控制还是我们自己的MPC控制器呢。我们都知道move_base导航架构中规划器会规划生成路径然后发布cmd_vel给控制器，然后小车就运动了，所以我们在运行move_base的那个launch文件里面修改发布的cmd_vel话题名为velocity<remap from = "cmd_vel" to = "velocity" />，这样小车如果是使用PID控制的话，那它就运动不了了。然后我们在MPC控制器里使得发布速度的话题名就叫cmd_vel。
经实践证明，当我们修改cmd_vel时不启动MPC控制的launch时，给小车一个目标点，小车可以进行路径规划但是不运动，然后我们再运行MPC控制的launch文件，小车对生成的路径进行跟踪开始运动了，所以我们可以确信当前的控制器就是我们自己的MPC控制器。
简单说一下我的运行架构，主要是三个launch文件：

roslaunch differentialAGV_197 gazebo.launch

第一个launch文件主要用于加载小车模型以及启动搭建好的gazebo世界环境

roslaunch navigation control.launch

第二个launch文件里面会启动amcl定位节点，运行map_server去加载设置的地图，启动move_base以及运行rviz

roslaunch mpc_track MPC_track.launch

最后一个launch文件当然就是启动MPC控制器了

代码实现

MPC_track()这个函数主要是实现路径跟踪控制（根据给定的路径对机器人进行路径跟踪控制，包括计算参考状态和控制量、执行优化算法、更新状态并发布消息等操作）

void MPC_track()
	{
		if (path->Size() != 0)
		{
			// 根据机器人当前位置和给定路径，找到最接近机器人的路径点，并返回该路径点的索引值。
			int target_index = path->Find_target_index(car);

			// 获取目标路径点附近的【1.参考状态】和【2.参考控制量】
			
			// 获取参考状态（修改后：为了可以后续输出参考状态，也就是要走的路径点信息）
			state_reference = get_state_reference(target_index);

			// 计算车辆位置与终点位置的距离
			double v_distance = abs(sqrt(pow(car.x - lastPoint.x, 2) + pow(car.y - lastPoint.y, 2)));
			
			// + 计算车辆位置与起点位置的距离
			double start_distance = abs(sqrt(pow(car.x - startPoint.x, 2) + pow(car.y - startPoint.y, 2)));

			// 获取参考控制量
			vector<mpc_track::control> control_reference = get_control_reference(target_index, v_distance);

			// 调用优化服务计算控制量
			mpc_optimize(state_reference, control_reference);

			// + 起点前0.25米减速
			if (start_distance >= 0 && start_distance< slow_LEVE2_DISTANCE)
			{
				control.v = control.v/3;
			}
			// + 起点前0.5米减速
			else if (start_distance >= slow_LEVE2_DISTANCE && start_distance <= slow_LEVE1_DISTANCE)
			{
				control.v = control.v*2/3;
				// control.kesi = control.kesi/3;
			}

			// + 到终点前0.5米减速(2/3)
			if (slow_judge(v_distance) == slow_LEVE1_V)
			{
				control.v = control.v*2/3;
				control.kesi = control.kesi*2/3;
				ROS_WARN("到终点前0.5米");
			}

			// + 到终点前0.25米减速(1/3)
			if (slow_judge(v_distance) == slow_LEVE2_V)
			{
				control.v = control.v/3;
				control.kesi = control.kesi/3;
				ROS_WARN("到终点前0.25米");
			}

			// 若到达终点，则使机器人停下
			if (slow_judge(v_distance) == 0)
			{
				control.v = 0;
				control.kesi = 0;
			}

			// + 偏离轨迹过大，则使机器人停下
			if (min_distance > 0.3)
			{
				control.v = 0;
				control.kesi = 0;
				ROS_WARN("偏离轨迹过大，机器人停下");
			}

			// 设置路径属性
			Prediction_Path_set();

			// 更新小车位姿状态_odom话题得到
			car = update_state_1(control, odom, car);

			// 发布消息
			PUB();

			ROS_INFO("--------------------------------------");
		}
	}

函数move_base_addpointcallback（）用于处理move_base全局路径消息，将接收到的路径提取需要的信息（ ID、x坐标、y坐标、航向角）保存到path对象中

void move_base_addpointcallback(const nav_msgs::Path::ConstPtr &msg)
	{
		// 创建一个存储路径点的向量
		vector<waypoint> waypoints;

		for (int i = 0; i < msg->poses.size(); i++)
		{
			waypoint waypoint;
			waypoint.ID = i;		// + 设置路径点的ID
			waypoint.x = msg->poses[i].pose.position.x; // 设置路径点的x坐标
			waypoint.y = msg->poses[i].pose.position.y; // 设置路径点的y坐标
			waypoints.push_back(waypoint); // 将路径点添加到存储路径点的向量中

		// + 获取角度，计算切线方向(move_base全局路径的点偏航角都是0)
		for (int j=0; j<waypoints.size(); j++)
		{
			double dx, dy, yaw;

			// 计算起始点的切线方向
			if (j == 0) 
			{
				dx = waypoints[1].x - waypoints[0].x;
				dy = waypoints[1].y - waypoints[0].y;
			}
			// 计算终点的切线方向
			else if (j == (waypoints.size() - 1)) 
			{
				dx = waypoints[j].x - waypoints[j - 1].x;
				dy = waypoints[j].y - waypoints[j - 1].y;
			}
			// 计算中间点的切线方向
			else 
			{
				dx = waypoints[j + 1].x - waypoints[j].x;
				dy = waypoints[j + 1].y - waypoints[j].y;
			}

			// 使用 atan2() 函数可以避免由于参数的正负号导致的计算错误以及分母为零的情况。
			// 通常，它比简单的 atan(y / x) 计算更加稳定和准确。
			yaw = atan2(dy, dx);    // 计算航向角
			waypoints[j].yaw = yaw; // 存储航向角
		}

		path->Add_new_point(waypoints);			   // 将路径点向量传递给path类中的Add_new_point函数
		lastIndex = path->Size() - 1;			   // 获取最后一个路径点的索引
		lastPoint = path->Get_waypoint(0);         // 获取第一个路径点
		lastPoint = path->Get_waypoint(lastIndex); // 获取最后一个路径点
	}