传感器坐标系与车身坐标系的转换关系

1坐标系的定义

1.1车身坐标系定义如下:

车身坐标系

1.2传感器坐标系定义如下:

传感器坐标系

将车身坐标系定义为 O 2 X Y Z O_2XYZ O2XYZ,传感器的坐标系定义为 O 3 X Y Z O_3XYZ O3XYZ,绕 Z Z Z轴旋转的角度为 ψ \psi ψ,其中逆时针方向为正。

2坐标系旋转

Z Z Z时针旋转的旋转矩阵定义为
R = [ cos ⁡ ψ − sin ⁡ ψ 0 sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] R = \left[ \begin{matrix} \cos\psi & -\sin\psi & 0\\ \sin\psi&\cos\psi&0\\0&0&1 \end{matrix} \right] R=cosψsinψ0sinψcosψ0001
Z Z Z时针旋转的旋转矩阵定义为
R = [ cos ⁡ ψ sin ⁡ ψ 0 − sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] R = \left[ \begin{matrix} \cos\psi & \sin\psi & 0\\-\sin\psi&\cos\psi&0\\0&0&1 \end{matrix} \right] R=cosψsinψ0sinψcosψ0001

那么 O 3 X Y Z O_3XYZ O3XYZ O 2 X Y Z O_2XYZ O2XYZ坐标系之间的转换可以表示为
[ x 2 y 2 z 2 ] = [ cos ⁡ ψ − sin ⁡ ψ 0 sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] [ x 3 y 3 z 3 ] \left[ \begin{matrix} x_2\\y_2\\z_2 \end{matrix} \right]= \left[ \begin{matrix} \cos\psi & -\sin\psi & 0\\\sin\psi&\cos\psi&0\\0&0&1 \end{matrix} \right] \left[ \begin{matrix} x_3\\y_3\\z_3 \end{matrix} \right] x2y2z2=cosψsinψ0sinψcosψ0001x3y3z3
考虑到车体与传感器之间只存在绕 Z Z Z轴的旋转,故坐标系旋可以只考虑 O X Y OXY OXY平面内的旋转变换。假设按照下图方式旋转:
在这里插入图片描述
变换关系可以简化表示如下:
[ x 2 y 2 ] = [ cos ⁡ ψ − sin ⁡ ψ sin ⁡ ψ cos ⁡ ψ ] [ x 3 y 3 ] \left[ \begin{matrix} x_2\\y_2 \end{matrix} \right]= \left[ \begin{matrix} \cos\psi & -\sin\psi\\\sin\psi&\cos\psi \end{matrix} \right] \left[ \begin{matrix} x_3\\y_3 \end{matrix} \right] [x2y2]=[cosψsinψsinψcosψ][x3y3]

3坐标系平移

假设传感器相对车身坐标位置为 ( x 2 1 , y 2 1 ) (x^1_2,y^1_2) (x21,y21) 。所以最终传感器测量的目标位置在车体坐标系下的点表示如下:
[ x 2 y 2 ] = [ cos ⁡ ψ − sin ⁡ ψ sin ⁡ ψ cos ⁡ ψ ] [ x 3 y 3 ] + [ x 2 1 y 2 1 ] \left[ \begin{matrix} x_2\\y_2 \end{matrix} \right]= \left[ \begin{matrix} \cos\psi & -\sin\psi\\\sin\psi&\cos\psi \end{matrix} \right] \left[ \begin{matrix} x_3\\y_3 \end{matrix} \right] + \left[ \begin{matrix} x^1_2\\y^1_2 \end{matrix} \right] [x2y2]=[cosψsinψsinψcosψ][x3y3]+[x21y21]

### 智能车大赛中GPS坐标系IMU坐标系关系转换方法 #### GPS坐标系IMU坐标系概述 在智能车竞赛环境中,GPS提供的是基于WGS84的世界坐标系中的经纬度信息[^2]。而IMU则记录相对于其自身初始状态的姿态变化以及加速度数据,在车辆上的安装位置决定了这些数据所对应的局部坐标系。 为了使来自不同传感器的数据能够相互关联并用于精确定位和导航,必须建立从GPS到IMU再到车身坐标系的一系列变换关系。这不仅涉及到两个独立空间之间的映射,还包括时间同步等问题。 #### 坐标系间的转换过程 当考虑如何将GPS获取的位置信息转化为适用于IMU处理的形式时,通常先要完成以下几个步骤: 1. **从WGS84地理坐标转为本地笛卡尔坐标** 使用UTM投影或其他适合的方法把地球表面上的经纬度值转变为二维平面直角坐标(x,y),这样可以简化后续计算。此操作依赖于特定区域的地图投影参数设置。 2. **融合GNSS/INS组合导航系统得到高精度定位** 结合GNSS接收器提供的绝对地理位置信息惯性测量单元(IMU)给出的速度、方向等相对变动情况来提高整体定位准确性。这一阶段可能还会引入其他辅助手段如轮速计以增强鲁棒性和可靠性。 3. **定义统一的参考框架——车体坐标系** 设定一个固定不变的标准参照物作为所有子系统的共同起点,比如选取汽车质心处朝前的方向作为+x轴指向前进方向;左侧+y轴垂直地面指向左方;向下-z轴沿重力线负向延伸。这样的约定有助于保持一致性,并便于多源异构传感设备间的信息交互。 4. **执行刚体运动学模型下的坐标变换** 对于任意时刻t而言,假设已经获得了当前状态下IMU相对于起始点的变化矩阵T_imu(t), 同样也知道了经过上述一系列转化后的GPS读数对应于同一瞬间的空间矢量P_gps(t). 那么就可以利用齐次坐标的表示法来进行两者的匹配: \[ P_{gps\_in\_body}(t)= T^{-1}_{imu} * (R*P_{gps}(t)+ t) \] 其中\( R \)代表旋转部分,\( t \) 表示平移分量。\(^{-1}\)指代求逆运算符。这里需要注意的是,实际应用过程中往往还需要考虑到尺度因子的影响,因为不同的单位制可能导致数值差异较大。 5. **校准偏置误差** 实际部署之前应当仔细调整各个部件之间的相对位置关系,确保静态条件下二者输出一致。动态测试期间也要不断优化算法性能,减少累积漂移带来的影响。 ```python import numpy as np def gps_to_body_frame(gps_position, imu_orientation_matrix, translation_vector): """ Convert a point from the GPS coordinate system to the vehicle body frame. Parameters: gps_position : array_like The position vector in the GPS/WGS84-derived local Cartesian coordinates [x, y]. imu_orientation_matrix : ndarray A 3x3 rotation matrix representing orientation of IMU relative to initial state. translation_vector : array_like Translation between origin points expressed in terms of body-fixed axes. Returns: transformed_point : ndarray Position represented within the car's own reference frame. """ # Extend input into homogeneous form and apply transformation extended_input = np.array([*gps_position, 1]) combined_transform = np.hstack((imu_orientation_matrix.T, -np.dot(imu_orientation_matrix.T, translation_vector[:, None]))) result_homogeneous = np.vstack((combined_transform, [0., 0., 0., 1.])) return np.dot(result_homogeneous, extended_input)[:2] # Example usage with dummy data if __name__ == "__main__": example_gps_pos = [-79.995366, 40.440624] # Hypothetical UTM-like values identity_rotation = np.eye(3) zero_translation = np.zeros(3) converted_coords = gps_to_body_frame(example_gps_pos, identity_rotation, zero_translation) print(f"Converted Coordinates: {converted_coords}") ```
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

henry.zhu51

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值