C语言编程：坐标系的平移和旋转

本文总结博主在工作中遇到的坐标系转换相关问题，以及C语言编程实现。

1 问题场景

对于ADAS算法开发，在工作中遇到过很多需要坐标系转换的场景。例如，一辆车上有很多个传感器，包括摄像头、毫米波雷达和激光雷达。在同一时刻感知到外部环境信息后，由于不同传感器基于自身坐标系，在数据融合之前需要将目标信息转换到同一个坐标系之下（通常是汽车后轴中心点）。

例如，下图中的XOY坐标系是以车辆后轴中心点为原点，车头方向为X轴正方向，垂直车身向左是Y轴正方向。X’O’Y’是前视摄像头坐标系，以摄像头位置为坐标原点，坐标轴方向和XY相同。

X’O’Y’坐标系相对于XOY坐标系向前平移一个距离，左右也相应的有一个距离。已知一个点P在X’O’Y’坐标系中的坐标为(x’,y’），以及已知O’点在XOY坐标系的坐标(xo’,yo’)，就可以通过坐标平移算出点P在XOY坐标系中的坐标。这是坐标系平移的例子。

再举个例子，车辆在运动的过程中，方向盘打了一个角度，汽车就会做一个圆周运动。在某一时刻t0，经过Δt时间，车辆不仅产生一个位置上的平移，自身还有一个旋转。

结合上图，比如说在t0时刻一个目标点在XOY坐标系下的坐标是(x,y)，在t1时刻车辆运动到前面一点的位置，并且车头朝向偏左了一个角度。这时候，那个目标点和车的相对位置就改变了，需要重新计算它在X’O’Y’坐标系中的坐标。利用X’O’Y’坐标系相对于XOY坐标系的距离和角度，就可以求出来目标点在心得坐标系X’O’Y’坐标系中的坐标(x’,y’)。这个例子中包含了坐标系的平移和旋转。

2 公式推导

上面场景的问题总结如下：已知点P在XOY坐标系中的坐标为(px,py)，X’O’Y’坐标系的原点O’在XOY坐标系中的坐标为(ox’,oy’)，求点P在X’O’Y’坐标系中的坐标。这里坐标系采用了右手系，即X向右Y向上。旋转角度定义为逆时针为正角度，以便后面的推导。

两个坐标系之间通过平移和旋转两种运动转换得到。为简化推导，首先基于O’点做出一个中间坐标系X’’O’Y’’，先完成旋转运动。

这里问题就转换为先推导(px’,py’)和(px’’,py’’)的关系，再推导(px’’,py’’)和(px,py)的关系。

2.1 旋转坐标系推导

首先，过点P做垂直线PA⊥O’Y’，PB⊥O’X’，PC⊥O’Y’’，PD⊥O’X’’，那么很容易知道∠BPD = θ，如下图所示。

接着过点D做DE⊥O’X’，如下：

这样，就可以推导出px’：
$$p{x}^{\prime }=|O^{\prime }E|+|EB|=|O^{\prime }D|\cdot \cos \theta +|PD|\cdot \sin \theta =p{x}^{\prime \prime }\cdot \cos \theta +p{y}^{\prime \prime }\cdot \sin \theta$$

接着推导py’，过点D做DF⊥PF（PF是PB的延长线)，就可以推导出py’：

$$p{y}^{\prime }=|PF|-|BF|=|PD|\cdot \cos \theta -|OD|\cdot \sin \theta =p{y}^{\prime \prime }\cdot \cos \theta -p{x}^{\prime \prime }\cdot \sin \theta$$

将上面两个推导的结果写到一起，后面的章节需要用到：
$$\begin{gathered} p{x}^{\prime }=p{x}^{\prime \prime }\cdot \cos \theta +p{y}^{\prime \prime }\cdot \sin \theta \\ p{y}^{\prime }=-p{x}^{\prime \prime }\cdot \sin \theta +p{y}^{\prime \prime }\cdot \cos \theta \end{gathered}$$
这里也可以用矩阵变换的方式，一步就能得出这个结论。点P相当于绕着原点顺时针转了θ角度，所以通过旋转矩阵公式可以得出变换关系。
$$\left[\begin{array}{c}p{x}^{\prime }\\ p{y}^{\prime }\end{array}\right]=\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}p{x}^{\prime \prime }\\ p{y}^{\prime \prime }\end{array}\right]$$

2.2 平移坐标系推导

平移的推导过程就简单很多，见下图

就是简单的加减：

$$\begin{gathered} p{x}^{\prime \prime }=px-o^{\prime }x \\ p{y}^{\prime \prime }=py-o^{\prime }y \end{gathered}$$

2.3 完整公式

将上面两个小节的公式代入得出完整公式：
$$\begin{gathered} p{x}^{\prime }=(px-o^{\prime }x)\cdot \cos \theta +(py-o^{\prime }y)\cdot \sin \theta \\ p{y}^{\prime }=-(px-o^{\prime }x)\cdot \sin \theta +(py-o^{\prime }y)\cdot \cos \theta \end{gathered}$$
后面基于这个公式编写C语言程序

3 C语言编程

首先，分析一下这个程序的需求。输入是一个点的在旧坐标系下的坐标，以及新坐标系相对于旧坐标系的位置和旋转角度。输出是该点在新坐标系下的坐标。

这里，博主设计一个简单的函数来实现这个算法。

#include 
#include 

#define PI 3.1415926F

typedef struct Point_Tag
{
    float X;
    float Y;
} Point_Type;//点结构体

typedef struct CoordinatePosition_Tag
{
    float deltaX;
    float deltaY;
    float theta;
} CoordinatePosition_Type;//新坐标系相对旧坐标系位置结构体

//坐标系变换函数
void coordinate_transformation(const Point_Type*              const point_old_system_sp,
                               const CoordinatePosition_Type* const coordinate_position_sp,
                                     Point_Type*              const point_new_system_sp)
{
    point_new_system_sp->X =  (point_old_system_sp->X - coordinate_position_sp->deltaX) * cosf(coordinate_position_sp->theta) +
                              (point_old_system_sp->Y - coordinate_position_sp->deltaY) * sinf(coordinate_position_sp->theta);
    point_new_system_sp->Y = -(point_old_system_sp->X - coordinate_position_sp->deltaX) * sinf(coordinate_position_sp->theta) +
                              (point_old_system_sp->Y - coordinate_position_sp->deltaY) * cosf(coordinate_position_sp->theta);
}

//main函数中简单测试
int main()
{
    Point_Type point_old_system;
    CoordinatePosition_Type coordinate_position;
    Point_Type point_new_system;

    point_old_system.X = 2.0F;
    point_old_system.Y = 2.0F;
    coordinate_position.deltaX = 0.0F;
    coordinate_position.deltaY = 0.0F;
    coordinate_position.theta = (45.0F/180.0F*PI);

    coordinate_transformation(&point_old_system,&coordinate_position,&point_new_system);

    printf("NEW_X = %f,NEW_Y = %f", point_new_system.X,point_new_system.Y);

} 

代码实现是比较简单的，就是把公式翻译成C代码。功能函数中传了三个参数，前两个指针是用来输入点在就坐标系的位置，和新坐标系在旧坐标系的位置和角度。第三个指针用于获取输出结果。

>>返回个人博客总目录