小程序开发公司网站源码下载,苏州网站建设用哪种好,iis wordpress,电商网站建设考试题基于 GPS 定位信息的 Pure-Pursuit 轨迹跟踪实车测试#xff08;1#xff09;
进行了多组实验#xff0c;包括顺逆时针转向#xff0c;直线圆弧轨迹行驶#xff0c;以及Pure-Pursuit 轨迹跟踪测试
代码修改
需要修改的代码并不多#xff0c;主要对 gps_sensor 功能包和…基于 GPS 定位信息的 Pure-Pursuit 轨迹跟踪实车测试1
进行了多组实验包括顺逆时针转向直线圆弧轨迹行驶以及Pure-Pursuit 轨迹跟踪测试
代码修改
需要修改的代码并不多主要对 gps_sensor 功能包和 purepursuit 代码的修改
gps_sensor 发布机器人实际运动轨迹而不是在 purepursuit 中发布gps_sensor 同时实现 mrobot_states_update 功能包的功能发布机器人状态信息purepursuit 将最终的控制指令发送给实际机器人cmd_to_robot 代替 cmd_to_mrobot #include ros/ros.h
#include sensor_msgs/NavSatFix.h
#include geometry_msgs/PoseStamped.h
#include geometry_msgs/TwistStamped.h
#include nav_msgs/Path.h#include math.h
#include iostream
#include Eigen/Geometry
#include chronousing namespace std;struct my_pose
{double latitude;double longitude;double altitude;
};struct my_location
{double x;double y;
};// 角度制转弧度制
double rad(double d)
{return d * M_PI / 180.0;
}arrayfloat, 4 calEulerToQuaternion(const float roll, const float pitch, const float yaw)
{arrayfloat, 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;
}// 全局变量
ros::Publisher state_pub_;
nav_msgs::Path ros_path_;
ros::Publisher vel_pub;
ros::Publisher pose_pub;const double EARTH_RADIUS 6371.393; // 6378.137;地球半径bool init false;
my_pose init_pose;// 计算速度
my_location pre_location;
my_location curr_location;
chrono::_V2::system_clock::time_point pre_time;
chrono::_V2::system_clock::time_point curr_time;void gpsCallback(const sensor_msgs::NavSatFixConstPtr gps_msg_ptr)
{// 初始化if (!init){init_pose.latitude gps_msg_ptr-latitude;init_pose.longitude gps_msg_ptr-longitude;init_pose.altitude gps_msg_ptr-altitude;init true;}else{// 计算相对位置double radLat1, radLat2, radLong1, radLong2, delta_lat, delta_long;radLat1 rad(init_pose.latitude);radLong1 rad(init_pose.longitude);radLat2 rad(gps_msg_ptr-latitude);radLong2 rad(gps_msg_ptr-longitude);// 计算xdelta_lat radLat2 - radLat1;delta_long 0;double x 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) cos(radLat1) * cos(radLat2) * pow(sin(delta_long / 2), 2)));x x * EARTH_RADIUS * 1000;// 计算ydelta_lat 0;delta_long radLong1 - radLong2;double y 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) cos(radLat2) * cos(radLat2) * pow(sin(delta_long / 2), 2)));y y * EARTH_RADIUS * 1000;// 计算yawarrayfloat, 4 calQuaternion calEulerToQuaternion(0.0, 0.0, gps_msg_ptr-altitude);// 发布轨迹ros_path_.header.frame_id world;ros_path_.header.stamp ros::Time::now();geometry_msgs::PoseStamped pose;pose.header ros_path_.header;pose.pose.position.x x;pose.pose.position.y y;pose.pose.orientation.x calQuaternion[0];pose.pose.orientation.y calQuaternion[1];pose.pose.orientation.z calQuaternion[2];pose.pose.orientation.w calQuaternion[3]; ros_path_.poses.push_back(pose); // 计算速度curr_location.x x;curr_location.y y;curr_time chrono::system_clock::now();auto timeDifference curr_time - pre_time;auto durationInMilliseconds chrono::duration_castchrono::milliseconds(timeDifference);double displacement sqrt(pow(curr_location.x - pre_location.x, 2) pow(curr_location.y - pre_location.y, 2));geometry_msgs::TwistStamped vel;// v s / tvel.twist.linear.x displacement / durationInMilliseconds.count() * 1000;// 更新信息pre_location curr_location;pre_time curr_time;cout 位姿信息 endl;cout x , y , gps_msg_ptr-altitude endl;cout 速度信息 endl;cout vel.twist.linear.x endl;cout --------- endl;// 信息发布vel_pub.publish(vel);pose_pub.publish(pose); state_pub_.publish(ros_path_);}
}int main(int argc, char **argv)
{init false;ros::init(argc, argv, gps_subscriber);ros::NodeHandle n;ros::Subscriber pose_sub n.subscribe(/robot_gps, 10, gpsCallback);state_pub_ n.advertisenav_msgs::Path(/gps_path, 10);vel_pub n.advertisegeometry_msgs::TwistStamped(/center_velocity, 10);pose_pub n.advertisegeometry_msgs::PoseStamped(/center_pose, 10);ros::spin();return 0;
}#include geometry_msgs/Twist.h
#include geometry_msgs/PoseStamped.h
#include nav_msgs/Path.h
#include ros/ros.h
#include tf/tf.h
#include tf/transform_broadcaster.h#include algorithm
#include cassert
#include cmath
#include fstream
#include iostream
#include string
#include vector
#include cpprobotics_types.h#define PREVIEW_DIS 1.0 // 预瞄距离
#define Ld 0.55 // 轴距using namespace std;
using namespace cpprobotics;ros::Publisher purepersuit_;const float target_vel 0.2;
float carVelocity 0;
float preview_dis 0;
float k 0.1;// 计算四元数转换到欧拉角
std::arrayfloat, 3 calQuaternionToEuler(const float x, const float y,const float z, const float w)
{std::arrayfloat, 3 calRPY {(0, 0, 0)};// roll atan2(2(wxyz),1-2(x*xy*y))calRPY[0] atan2(2 * (w * x y * z), 1 - 2 * (x * x y * y));// pitch arcsin(2(wy-zx))calRPY[1] asin(2 * (w * y - z * x));// yaw atan2(2(wxyz),1-2(y*yz*z))calRPY[2] atan2(2 * (w * z x * y), 1 - 2 * (y * y z * z));return calRPY;
}cpprobotics::Vec_f r_x_;
cpprobotics::Vec_f r_y_;int pointNum 0; // 保存路径点的个数
int targetIndex pointNum - 1;// 计算发送给模型车的转角
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::arrayfloat, 3 calRPY calQuaternionToEuler(currentQuaternionX, currentQuaternionY,currentQuaternionZ, currentQuaternionW);cout currentPositionX , currentPositionY , calRPY[2] endl;for (int i pointNum - 1; i 0; --i){float distance sqrt(pow((r_x_[i] - currentPositionX), 2) pow((r_y_[i] - currentPositionY), 2));if (distance preview_dis){targetIndex i 1;break;}}if (targetIndex pointNum){targetIndex pointNum - 1;}float alpha atan2(r_y_[targetIndex] - currentPositionY, r_x_[targetIndex] - currentPositionX) -calRPY[2];// 当前点和目标点的距离Idfloat dl sqrt(pow(r_y_[targetIndex] - currentPositionY, 2) pow(r_x_[targetIndex] - currentPositionX, 2));// 发布小车运动指令及运动轨迹geometry_msgs::Twist vel_msg;vel_msg.linear.z 1.0;if (dl 0.2) // 离终点很近时停止运动{vel_msg.linear.x 0;vel_msg.angular.z 0;purepersuit_.publish(vel_msg);}else{float theta atan(2 * Ld * sin(alpha) / dl);vel_msg.linear.x target_vel;vel_msg.angular.z theta;purepersuit_.publish(vel_msg);}
}void velocityCall(const geometry_msgs::TwistStamped carWaypoint)
{carVelocity carWaypoint.twist.linear.x;// 预瞄距离计算preview_dis k * carVelocity PREVIEW_DIS;
}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);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;}
}int main(int argc, char **argv)
{// 创建节点ros::init(argc, argv, pure_pursuit);// 创建节点句柄ros::NodeHandle n;// 创建Publisher发送经过pure_pursuit计算后的转角及速度purepersuit_ n.advertisegeometry_msgs::Twist(/cmd_vel, 20);ros::Subscriber splinePath n.subscribe(/ref_path, 20, pointCallback);ros::Subscriber carVel n.subscribe(/center_velocity, 20, velocityCall);ros::Subscriber carPose n.subscribe(/center_pose, 20, poseCallback);ros::spin();return 0;
}数据处理
原始的数据便于查阅但不便于 MATLAB 进行读取分析主要包括 ***_pub.txt 和 ***_path.txt 两种待处理数据分别如下
$GPRMC,085750.20,A,3150.93719306,N,11717.59499143,E,0.071,252.6,161123,5.7,W,D*26数据长度83
latitude: 31.848953217667
longitude: 117.293249857167
heading_angle_deg: 252.600000000000
heading_angle_rad: 4.408701690538
heading_angle_rad: -1.874483616642
---------
$GPRMC,085750.30,A,3150.93719219,N,11717.59498178,E,0.297,264.0,161123,5.7,W,D*28数据长度83
latitude: 31.848953203167
longitude: 117.293249696333
heading_angle_deg: 264.000000000000
heading_angle_rad: 4.607669225265
heading_angle_rad: -1.675516081915
---------位姿信息
0.0169953,0.0210645,-2.24798
速度信息
1.59198e-11
---------
位姿信息
0.0183483,0.0200412,2.49058
速度信息
0.0180464
---------下面的命令首先使用**grep过滤包含heading_angle_deg: 的行然后使用awk**提取每行的第二个字段即数字部分最后将结果写入output.txt文件 grep heading_angle_deg: input.txt | awk {print $2} output.txt下面的命令使用**awk匹配包含位姿信息:的行然后使用getline**读取下一行数据并打印出来最后将结果写入output.txt文件
awk /位姿信息:/ {getline; print} input.txt output.txt处理前和处理后的文件目录如下
redwallredwall-G3-3500:~/log/2023--11-16/raw$ tree
.
├── actual_path.txt
├── anticlock_path.txt
├── anticlock_pub.txt
├── clock_path.txt
├── clock_pub.txt
├── gps_path.txt
├── gps_pub.txt
├── lane_path.txt
└── lane_pub.txtredwallredwall-G3-3500:~/log/2023--11-16/processed$ tree
.
├── anticlock_path_pose.txt
├── anticlock_path_vel.txt
├── anticlock_pub_sift.txt
├── clock_path_pose.txt
├── clock_path_vel.txt
├── clock_pub_sift.txt
├── gps_path_pose.txt
├── gps_path_vel.txt
├── gps_pub_sift.txt
├── lane_path_pose.txt
├── lane_path_vel.txt
└── lane_pub_sift.txt数据分析
本次有“卫星”标签的连接车后的蘑菇头以 10 Hz 的频率获取定位数据
分析 lane_pub 和 lane_path直线圆弧轨迹运动
由实际轨迹可知纬度差应对应 X经度差应对应 Y应该修改代码 由实际航向角信息结合实际轨迹本次天线的基线向量应该是与正北方向重合顺时针转向时航向角增大说明天线安装的方式应该是正确的 分析 clock_pub 和 clock_path 顺时针原地转向
主要是分析航向角信息由于提高采样频率因此存在很大的噪声可以对信号进行去噪处理
本次已经尽量控制两个蘑菇头的安装安装前用卷尺进行了安装测量但仍存在误差
结合之前的分析记录顺时针转向时先增大至 360再从 0 开始继续增大下面符合该规律 人为判断的正北朝向并不和实际正北朝向重合因此起始和终止位置并不为 0 度
分析 gps_pub、gps_path 和 actual_path
首先机器人是由北向南开即初始朝向是向南由 lane 数据可知初始朝向是有问题的
其实 Pure-Pursuit 算法并不需要航向角信息初始位姿是一个比较大的影响
实际速度会影响预瞄距离实际速度较小会导致预瞄距离较小导致控制的不稳定 存在问题
1、和 GPS 串口通信存在问题读取配置以及信息长度方面存在问题需要修改
2、GPS 航向角信息的获取仍存在较大问题准确性比较低
3、GPS 对于高速移动的车辆厘米级别的定位精度还可以目前对于低速机器人可能不太适用
