【智能车心得5】19届智能车竞赛独轮组·室外室内方案篇

Yair

已于 2025-04-08 21:36:07 修改

阅读量2k

点赞数 36

文章标签：人工智能算法经验分享 c语言嵌入式硬件

于 2024-10-15 23:38:15 首次发布

本文链接：https://blog.csdn.net/weixin_63742751/article/details/142966875

版权

1.室外定位方案

（1）GPS方案

下面的思路引用自@逐飞科技，请大家参考。

（2）RTK方案

在室外，我们需要使用卫星定位模块来确定和记录目标板所在的位置。这里我们用的不是普通的GPS模块，因为它的精度只有1m左右，误差竟然达到了场地的1/5，因此我们选择了更高精度的RTK模块，其精度在3cm左右。

下面将简要介绍一下RTK定位设备的原理:

RTK(Real-Time Kinematic)定位技术的原理基于载波相位差分方法，主要由以下几个环节构成:

• 基准站定位：首先，基准站固定在一个已知坐标点上，接收来自GNSS卫星（如北斗等）的信号，并计算出自身的定位信息，如图所示，这个是基站，最好用直播支架撑高离地1m的距离。并且需要保证附近无干扰。

• 误差计算：基准站将观测到的误差（包括大气延迟、卫星钟差、接收机钟差等）计算出来，并结合自身的精确坐标，形成差分改正数据。

• 数据传输：基准站通过无线电数据链（或其他通信方式）实时将差分改正数据发送给流动站，此处的流动站是指车模上的两个圆柱形卫星接收器。

• 流动站定位：流动站接收到基准站发送的差分改正数据后，同时采集自身的GNSS观测数据。流动站利用这些数据，通过差分处理，消除或减少共同的误差源，从而提高定位精度。

• 实时解算：流动站根据差分改正数据和自身的观测数据，实时解算出自身的精确三维坐标（经度、纬度、高程）。由于流动站和基准站之间的空间相关性，流动站可以利用基准站的数据来校正自身的观测误差，实现厘米级的定位精度。

经由RTK解算芯片，最终得到了带相应格式的帧头文本，通过串口传递给主控芯片。

如图所示，逐飞的RTK需要等两个绿灯都亮起才算进入RTK高精度固定解算模式,此时表示定位信号良好。这样，通过逐飞的例程，就可以直接获取车体所在的经度、纬度、高度、移动速度。精度都在小数点后八位，即double型。

Aurix Development Studio默认将数据处理为float型，必须要注意的是，如果按默认方式处理，获得的经纬度信息就是截断在小数点后六位的，此时调用逐飞库函数里的——两点间的距离&两点间的角度，所得到的值是不连贯的，断断续续的，这正是因为因为精度不够。所以必须改为double型，那么该如何将数据处理改为double型呢？

请参考逐飞科技的博文——极速越野组之逐飞演示车模浅析

其中，下图这两步是最为关键的。如果不更改fpu设置，即浮点运算单元的设置，只改变double型，实际是得不到有用的效果的，反而可能使得车原本的平衡失效。需要注意的是，改成double数之后，所有原来的float型函数都需要加上后缀f才对，如tan需要改为tanf。此后不能再混用double型和float型。

那么为了实现定位，然后循迹撞击目标板，应该如何操作呢？

从目标板的分布情况来看，我们理想的循迹方式应该是通过卫星定位模块，从出发区开始，在目标板前后进行打点，再最终回到起点，并注意整条路线上规避锥桶或者凸起即可。基于上述构思，我们进行了程序设计。

首先是RTK信息解算，调用库函数每时每刻获取该位置的经纬度。然后把打点数据存在数组里，当然也可以存到Flash里，这个时候由于是double型数据，所以存到Flash里是需要点小技巧的。需要将整数位和小数位分开处理，分别存放，之后提取出来时才作合并，相信大家可以自己摸索出来。

那么接下来，如果要从这个坐标点走到下一个坐标点，只需要考虑如何让车身朝向对齐到下一个点，而无需考虑两点之间的距离（因为车始终处于前行状态）。

那么该如何获取两个点之间与地理北极方向所形成的夹角呢？

latitude1 = ANGLE_TO_RAD(latitude1);
latitude2 = ANGLE_TO_RAD(latitude2);
longitude1 = ANGLE_TO_RAD(longitude1);
longitude2 = ANGLE_TO_RAD(longitude2);
double x = sin(longitude2 - longitude1) * cos(latitude2);
double y = cos(latitude1) * sin(latitude2) - sin(latitude1) * cos(latitude2) * cos(longitude2 - longitude1);
double angle = RAD_TO_ANGLE(atan2(x, y));
return ((0 < angle) ? angle : (angle + 360));

这个就是逐飞库里的get_two_points_azimuth函数，计算从第一个点到第二个点的方位角，以北极为参考正方向。这里采取的是一个Google Map实现的算法，可以很准确得到两点与北极之间的夹角。再次强调，必须把变量配置为double型变量进行运算，才能够得到准确的连续的值。

在知道了两点方位之后，我们还需要知道车头的朝向，这样才能求出需要转向的角度。这里可以采取两种方法实现。

其一是通过北极角来换算，因为车模上的两个卫星接收站之间的连线是指向南北极的，如图，假设是从左到右指向北极，那么车头的方向就是北极角减去90°。当然也可能从右指到左为北，与接线有关。

其二是通过陀螺仪来计算偏航Yaw角，要么使用四元数法计算出Yaw角，要么通过积分法。两种方法都需要指定初始方向（最好是地理北极）作为参考方向。

如图所示，车头和北极形成α的夹角，A到B点与北极方向形成β的夹角，那么应该转向的角度就可以知道为β-α，只需要让车模以这个实时变化的转角作为转向依据，将(β-α)作为转角丢进转向PID里形成所需要的飞轮差速即可实现转向，当然，这需要良好的PID参数。当到达下一个点附近几厘米后切点到下一坐标，然后再更新下一个转角即可。

除了用原始的经纬度这个方案，还可以把经纬度转换为RTK地心坐标系来处理，见CSDN博客：【经纬度和坐标之间怎么相互转换】经纬度和坐标之间怎么相互转换_坐标转换经纬度-CSDN博客

综上所述即为室外方案的寻点原理，它可以确保车模以较高的精度到达每个坐标点附近，由此实现循迹、撞击目标板和躲避障碍。

效果大致如下：

室外定位走S弯

室外定位走一圈

2.室内惯性导航方案

室内方案我们采取的是惯性导航技术。惯性导航（Inertial Navigation System, INS）是一种通过测量物体自身的加速度和旋转速度来推算其位置、速度和姿态的导航技术。惯性导航系统主要依赖惯性测量单元（IMU），其中包括加速度计和陀螺仪。加速度计用于测量物体在各个方向上的加速度。陀螺仪则用于测量物体的角速度，即物体在空间中的旋转运动。

通过对这些传感器数据进行积分，系统可以计算出物体的速度和位置。由于惯性导航不依赖于外部信号，它可以在没有GPS等外部参考的情况下工作，这使其在潜艇、航天器、导弹以及无人机等领域广泛应用。

不过，惯性导航系统存在累积误差（漂移），即由于测量误差和数值积分过程中的误差，随时间推移，计算出的位置信息会逐渐偏离实际位置。因此，惯性导航通常与其他导航系统（如GPS）结合使用，以校正这些误差。

那么回到具体的应用上来，该如何获取车模运行时的坐标信息呢？

通过每隔几毫秒读取一次陀螺仪的偏航角Yaw值和编码器测出的速度Speed值，对方向和位移的增量进行精确累加，即可获取车身当前坐标。参照如下伪代码：

Get_encoder();//20毫秒获取一次
Path = SystemData.Run_Speed * 0.020;
delta_X = Path * sin(posture.data.yaw);
delat_Y = Path * cos(posture.data.yaw);
X = X+delta_X;
Y = Y+delta_Y;

这样就可以随时得到车身所在的坐标位置。

我们在比赛前校准陀螺仪的初始朝向，认定为0度角。然后用手推车，绕着目标板摆放的顺序推车一周，如图所示从点O出发，每经过一个目标板就采集一个坐标，最后推车回库，即沿着路径O->A->B->C->D->O’，就实现了所有目标坐标的采集，记忆下了全部的路径信息。当然由于室内也会有凸起，所以可以适当地规避凸起来踩点。并且对于角度太刁钻的目标板，也可以通过规划合适的路线来避免车身转向倾倒。

由于该方案是记忆下之前人为规划的路径，然后让车自主复现，这就像有惯性记忆那样，故称之为惯性导航。（修改：此处之前定义有误，惯性导航是机器人学里的重要概念，定义为一种不依赖外部信息，仅通过惯性传感单元的测算获取自身位置姿态的定位方式。也就是说，之前通过陀螺仪+编码器得到坐标和朝向的过程就是惯导，而后续的路径复现不属于此范畴。）

那么如何根据坐标信息进行转向呢？编写函数计算两点之间与初始指定方向之间的夹角，然后用这个角度减去当前车身的偏航Yaw角即可。例如假设车辆运行在从O点到A点这段，距离坐标点A还有几厘米时，认为已经抵达，切换到新的目标坐标位置B点去。此时转向所用的角度即为Yaw角到B方向的角度差。之后以此类推。

与此同时，也可以实时计算车模当前所在位置与下一个目标点的距离，进而控制变速决策。

综上所述即为室内方案的寻点原理，它可以确保车模以较高的准确度到达每个坐标点附近，由此实现循迹、撞击目标板和躲避障碍。只要平衡性足够稳当，且对转向饱和问题处理得到位，就可以保证完赛。

当然，有时候我们发现，如果打点过多，往往越到后面越走不到对应的位点，这是什么原因呢？此时就要了解到误差因素：包括陀螺仪车头前倾而使得Yaw偏离Z轴积分方向、陀螺仪温漂等。

务必注意，发车前初始0°方向的校准非常重要。可以在发车区用胶带做个物理标记。如果要原地倒伏救援，一般也需要原地快速扶起来之后，对齐初始方向将陀螺仪Yaw角置为0°，再重新调整方向面向下一个目标板。因为倒地后Yaw角会歪。

最后，不论室外方案还是室内方案，车体实现拐弯，都是靠飞轮的差速。如何调出最优的PID参数，使得转弯快速进行，且车体不会因为飞轮饱和而倒地，是需要仔细摸索的。

**********************转向环中间环******************************

//纯RTK，没找到目标板的时候用RTK自己的转向参数
if(SystemData.Controller == RTK)
       {{Yaw_a = PID_Calculate(&RTK_DT.PID,  RTK_DT.expect_yaw.theta, 0);} }

//纯惯导，没找到目标板的时候用专属的惯导参数
if(SystemData.Controller == Navigation)//调用转向优化函数，将角度进行切片,但显示的是实际的角度偏差
        {Yaw_a = PID_Calculate(&NavigationDT.PID, NavigationDT.turn_angle, 0);} 

//RTK或惯导，且找到目标板的时候，可以用视觉偏差加以修正
if(SystemData.Controller == Navigation_Camera || SystemData.Controller == RTK_Camera)
       {Yaw_a = PID_Calculate(&Balance_Yaw.a, (float)SystemData.Visual_Offset, 0);} 
   
**********************转向环 内环******************************
Yaw_w = PID_Calculate(&Balance_Yaw.w, Yaw_a-((float)posture.data.gyro_origin.z), 0); 

**********************飞轮差速转向******************************
SystemData.Motor_A = (int)(Roll_w+Yaw_w);
SystemData.Motor_B =-(int)(Roll_w-Yaw_w);

效果大致如下：