AGV平面坐标系变换公式及实例

1、AGV坐标系简介

如上图，小车前后对角是有激光雷达的，其坐标系称为激光坐标系，采用极坐标系体现。中间为车体坐标系，激光坐标系相对于车体坐标系关系不变；左下角是地图坐标系，小车扫图后，建立的坐标系即为地图坐标系，小车在运动过程中，车体坐标系相对于地图坐标系是变化的。

2、坐标系旋转与点旋转的变换公式

坐标系旋转：

直角坐标系 XOY逆时针旋转 θ角后变成 X′OY′，原坐标系内点的坐标变化为：

如果是顺时针旋转，则将 𝜃 改为 −𝜃 带入公式。

另一种表达方式：

即X′OY′坐标系相对于“静止”XOY坐标系顺时针旋转：

x1=xcos(β)+ysin(β); y1=ycos(β)-xsin(β);

X′OY′坐标系相对于“静止”XOY坐标系逆时针旋转：

x1=xcos(β)-ysin(β); y1=ycos(β)+xsin(β);

坐标系旋转公式推导图1

点的旋转：

直角坐标系 XOY内的点 A(x,y)绕原点逆时针旋转 α 角后得到点 B(x′,y′)，两点坐标关系为：

3、小车中激光雷达点云转换例子实战

再次温习这个图。

①激光的点云由极坐标点转直角坐标系点：

x=r⋅cos(θ)

y=r⋅sin(θ)

直角坐标转极坐标系：

②激光坐标系转小车坐标系

这两个坐标系相对关系固定，激光坐标系相对于小车坐标系的变换可以认为是小车坐标系旋转到激光坐标系，这样激光坐标系的点就映射到了小车坐标系。实际中，除了旋转还有原点位置的偏差，这块直接加上差值即可，差值是激光坐标系原点相对于小车坐标系坐标系原点的参数，接口可以获取。

灵魂代码如下：

cartesian_x = dist*cos(angle+install_info_yaw) + install_info_x;
cartesian_y = dist*sin(angle+install_info_yaw) + install_info_y;

展开后发现是激光雷达坐标的install_info_yaw相对于小车坐标系都是逆时针数据。

③小车坐标系下的雷达点云数据相对于地图坐标系的转换

此时地图坐标系是“静止”的，那么接口给出的小车实时位置x,y和yaw信息是相对于

地图坐标系的。根据坐标系旋转公式推导图1，已知的是红色坐标系下的点(小车坐标系)，求蓝色坐标系下的点(地图坐标系)，即已知x’，y’，θ求x，y。我的处理方式是求旋转矩阵的逆，然后再求出x，y，用的eigen库。坐标系旋转都是相对的，后面想起更好的方式再记录。

if(robot_push_data_.angle > 0){
    float agv_yal = -robot_push_data_.angle;
    Eigen::Matrix2d r;
    r << cos(agv_yal), sin(agv_yal),
         -sin(agv_yal), cos(agv_yal);
         Eigen::MatrixXd pos(2,1);
         pos(0,0) = cartesian_x;
         pos(1,0) = cartesian_y;

    Eigen::MatrixXd posnew(2,1);
    posnew = r.inverse()*pos;

    cartesian_x = posnew(0,0) + robot_push_data_.x;
    cartesian_y = posnew(1,0) + robot_push_data_.y;
}