全方位移动机器人 Stanley 轨迹跟踪 Gazebo 仿真-编程知识

全方位移动机器人 Stanley 轨迹跟踪 Gazebo 仿真

本来打算今天出去跑一下 GPS，但是下雨，作罢

添加参考轨迹信息

以下三个功能包不需要修改：

mrobot：在 Rviz 和 Gazebo 中仿真机器人
cmd_to_mrobot：运动学解算，控制仿真机器人运动
mrobot_states_update：发布机器人状态信息

pure-pursuit 轨迹跟踪中只包含参考轨迹的 X、Y 信息，不包括参考航向角信息，而 stanley 轨迹跟踪需要参考航向角信息，因此修改 double_lane.cpp

主要有两种思路：

1、自定义参考轨迹消息，包括 ref_x，ref_y, ref_phi

float64[] ref_x
float64[] ref_y
float64[] ref_phi

但是参考轨迹要在 Rviz 中显示的话还是需要发布标准格式的消息，即 nav_msgs::Path

因此 double_lane 节点需要发布两个话题，一个给 stanley 控制器，一个给 Rviz，有点冗余

2、将参考航向角信息转化为 nav_msgs::Path 消息下的四元数格式

航向角相当于欧拉角中的偏航角（yaw），因此需要编写欧拉角到四元数的转换函数

array<float, 4> calEulerToQuaternion(const float roll, const float pitch, const float yaw) {array<float, 4> calQuaternion = {0.0f, 0.0f, 0.0f, 1.0f};  // 初始化四元数// 使用Eigen库来进行四元数计算Eigen::Quaternionf quat;quat = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) *Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());calQuaternion[0] = quat.x();calQuaternion[1] = quat.y();calQuaternion[2] = quat.z();calQuaternion[3] = quat.w();return calQuaternion;
}

这里做了一个小 check，用calEulerToQuaternion和calQuaternionToEuler两个函数验证了转化的正确性

stanley 控制器实现

整体框架和 pure-pursuit 控制器很像，控制算法依然在 poseCallback 回调函数中实现

在参考路径回调函数 pointCallback 中添加参考航向角信息

void pointCallback(const nav_msgs::Path &msg)
{// 避免参考点重复赋值if (pointNum != 0){return;}// geometry_msgs/PoseStamped[] posespointNum = msg.poses.size();// 提前开辟内存r_x_.resize(pointNum);r_y_.resize(pointNum);r_phi_.resize(pointNum);for (int i = 0; i < pointNum; i++){r_x_[i] = msg.poses[i].pose.position.x;r_y_[i] = msg.poses[i].pose.position.y;array<float, 3> rpy = calQuaternionToEuler(msg.poses[i].pose.orientation.x, msg.poses[i].pose.orientation.y,msg.poses[i].pose.orientation.z, msg.poses[i].pose.orientation.w);r_phi_[i] = rpy[2];// cout << r_x_[i] << "\t" << r_y_[i] << "\t" << r_phi_[i] << endl;}
}

C++ 中 stanley 算法的实现和 MATLAB 中很像

%==============================================================
% Calculate outputs
%==============================================================
function sys = mdlOutputs(t,x,u)global U; %store chi_tilde=[vel-vel_ref; delta - delta_ref]global cx;global cy;global phi_ref;pi = 3.1415926;ticfprintf('Update start, t=%6.3f\n',t);x = u(1);y = u(2);yaw_angle =u(3)*pi/180;%CarSim输出的Yaw angle为角度，角度转换为弧度v = u(4) / 3.6;gain_k = 5;L = 2.7;                                % [m] wheel base of vehicleN =  length(cx);Distance = zeros(N,1);x = x + L * cos(yaw_angle);y = y + L * sin(yaw_angle);for i = 1:NDistance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);end   %ind是最近点索引[value, location]= min(Distance);ind = location;%error是横向误差，根据实际位置与参考点的y值判断if y < cy(ind)error = value;elseerror = -value;endalpha = phi_ref(ind) - yaw_angle;if v < 1v = 1; endtheta = atan(gain_k * error/v);    delta = alpha + theta;
%     delta = delta * 180 / pi;U = delta;sys= U; % vel, steering, x, ytoc
% End of mdlOutputs.

// 计算发送给模型车的转角
void poseCallback(const geometry_msgs::PoseStamped &currentWaypoint)
{// 获取当前位姿auto currentPositionX = currentWaypoint.pose.position.x;auto currentPositionY = currentWaypoint.pose.position.y;auto currentPositionZ = 0.0;auto currentQuaternionX = currentWaypoint.pose.orientation.x;auto currentQuaternionY = currentWaypoint.pose.orientation.y;auto currentQuaternionZ = currentWaypoint.pose.orientation.z;auto currentQuaternionW = currentWaypoint.pose.orientation.w;std::array<float, 3> calRPY =calQuaternionToEuler(currentQuaternionX, currentQuaternionY,currentQuaternionZ, currentQuaternionW);float currentYaw = calRPY[2];cout << currentPositionX << "\t" << currentPositionY << "\t" << currentYaw << endl;// 计算前轴中心位置float front_x = currentPositionX + Ld * cos(currentYaw);float front_y = currentPositionY + Ld * sin(currentYaw);// 计算最近点距离及索引float cur_distance = 0.0;float min_distance = 1e9;for (int i = 0; i < pointNum; ++i){cur_distance = sqrt(pow(r_x_[i] - front_x, 2) + pow(r_y_[i] - front_y, 2));if (cur_distance < min_distance){min_distance = cur_distance;targetIndex = i;}}// 到达终点后结束控制，当两次以终点为目标点时结束if (targetIndex == pointNum - 1){++final_cnt;if (final_cnt >= 2){final_cnt = 2;}}// 计算横向误差，根据实际位置与参考点Y值判断float lateral_error = 0.0;if (currentPositionY < r_y_[targetIndex]){lateral_error = min_distance;}else{lateral_error = -min_distance;}// 计算航向偏差角和横向偏差角float alpha = r_phi_[targetIndex] - currentYaw;float theta = atan(Gain_K * lateral_error / currentVelocity);// cout << "targetIndex: " << targetIndex << endl;// cout << "ref_phi: " << r_phi_[targetIndex] << "\tcurrentYaw: " << currentYaw << endl;// cout << "currentVelocity: " << currentVelocity << "\tlateral_error: " << lateral_error << endl;// cout << "alpha: " << alpha << "\ttheta: " << theta << endl;// cout << "---------" << endl;// 发布小车运动指令及运动轨迹geometry_msgs::Twist vel_msg;vel_msg.linear.z = 1.0;if (targetIndex == pointNum - 1 && final_cnt >= 2){vel_msg.linear.x = 0;vel_msg.angular.z = 0;stanley_ctrl.publish(vel_msg);}else{float delta = alpha + theta;vel_msg.linear.x = 0.5;vel_msg.angular.z = delta;stanley_ctrl.publish(vel_msg);// 发布小车运动轨迹geometry_msgs::PoseStamped this_pose_stamped;this_pose_stamped.header.frame_id = "world";this_pose_stamped.header.stamp = ros::Time::now();this_pose_stamped.pose.position.x = currentPositionX;this_pose_stamped.pose.position.y = currentPositionY;this_pose_stamped.pose.orientation.x = currentQuaternionX;this_pose_stamped.pose.orientation.y = currentQuaternionY;this_pose_stamped.pose.orientation.z = currentQuaternionZ;this_pose_stamped.pose.orientation.w = currentQuaternionW;path.poses.push_back(this_pose_stamped);}actual_path.publish(path);
}