下述的C++代码以及matlab代码实现了相同的作用,C++代码是matlab代码通过matlab coder工具转换而来,对比两种代码,详细解释C++代码的具体实现过程。…
- User
下述的C++代码以及matlab代码实现了相同的作用,C++代码是matlab代码通过matlab coder工具转换而来,对比两种代码,详细解释C++代码的具体实现过程。 C++代码:
d = b_uavD3DConn.AirSpeed; // UAV俯仰角限制下界 // UAV俯仰角限制上界 // 俯仰角计算 // 未归一化的方向向量 // 高度差 // 负高度为正方向 // 待跟踪航迹俯仰角不能超过UAV性能限制 // validateattributes(pitch, 'double', {">=", pitchLimit_1, "<=", // pitchLimit_2}); 航向角计算 目标轨线长度 方向向量 航迹采样 if (std::isnan(d)) { y.set_size(1, 1); y[0] = rtNaN; } else if ((d == 0.0) || (d < 0.0)) { y.set_size(1, 0); } else if (std::isinf(d)) { y.set_size(1, 1); y[0] = 0.0; } else if (std::floor(d) == d) { loop_ub = static_cast(4000.0 / d); y.set_size(1, loop_ub + 1); for (i = 0; i <= loop_ub; i++) { y[i] = d * static_cast(i); } } else { double apnd; ndbl = std::floor(4000.0 / d + 0.5); apnd = ndbl * d; if (d > 0.0) { cdiff = apnd - 4000.0; } else { cdiff = 4000.0 - apnd; } if (std::abs(cdiff) < 1.7763568394002505E-12) { ndbl++; apnd = 4000.0; } else if (cdiff > 0.0) { apnd = (ndbl - 1.0) * d; } else { ndbl++; } if (ndbl >= 0.0) { n = static_cast(ndbl); } else { n = 0; } y.set_size(1, n); if (n > 0) { y[0] = 0.0; if (n > 1) { y[n - 1] = apnd; nm1d2 = (n - 1) / 2; for (k = 0; k <= nm1d2 - 2; k++) { cdiff = (static_cast(k) + 1.0) * d; y[k + 1] = cdiff; y[(n - k) - 2] = apnd - cdiff; } if (nm1d2 << 1 == n - 1) { y[nm1d2] = apnd / 2.0; } else { cdiff = static_cast(nm1d2) * d; y[nm1d2] = cdiff; y[nm1d2 + 1] = apnd - cdiff; } } } } loop_ub = y.size(1); sample_LenSeq.set_size(y.size(1)); for (i = 0; i < loop_ub; i++) { sample_LenSeq[i] = y[i]; } // 目标航迹采样长度索引序列 // 目标航迹采样数量 // 航迹采样点不能超过10个 if (sample_LenSeq.size(0) > maxSampleNum) { unsigned int u; cdiff = std::floor(static_cast(sample_LenSeq.size(0)) / maxSampleNum); if (cdiff < 4.294967296E+9) { if (cdiff >= 0.0) { u = static_cast(cdiff); } else { u = 0U; } } else if (cdiff >= 4.294967296E+9) { u = MAX_uint32_T; } else { u = 0U; } if ((u == 0U) || ((u > 0U) && (static_cast(sample_LenSeq.size(0)) < 1U))) { i = 1; k = -1; } else { i = static_cast(u); k = sample_LenSeq.size(0) - 1; } loop_ub = div_s32(k, i); n = loop_ub + 1; b_sample_LenSeq.set_size(loop_ub + 1); for (k = 0; k <= loop_ub; k++) { b_sample_LenSeq[k] = sample_LenSeq[i * k]; } sample_LenSeq.set_size(loop_ub + 1); for (i = 0; i < n; i++) { sample_LenSeq[i] = b_sample_LenSeq[i]; } } loop_ub = sample_LenSeq.size(0); sample_Poses.set_size(sample_LenSeq.size(0), 4); n = sample_LenSeq.size(0) << 2; for (i = 0; i < n; i++) { sample_Poses[i] = 0.0; } // 显式定义为四列矩阵 n = (loop_ub / 2) << 1; nm1d2 = n - 2; for (i = 0; i < 3; i++) { for (k = 0; k <= nm1d2; k += 2) { __m128d r; r = _mm_loadu_pd(&sample_LenSeq[k]); _mm_storeu_pd( &sample_Poses[k + sample_Poses.size(0) * i], _mm_add_pd( _mm_mul_pd(_mm_set1_pd(static_cast(dir_vec[i])), r), mm_set1_pd(static_cast(startPos[i])))); } for (k = n; k < loop_ub; k++) { sample_Poses[k + sample_Poses.size(0) * i] = static_cast(dir_vec[i]) * sample_LenSeq[k] + static_cast(startPos[i]); } } // 前三列为位置线性采样 // sample_Poses = startPos + dir_vec .* sample_LenSeq; % // 目标轨线位置线性采样 保证最后一个线性插值点为终点 x_data[0] = (sample_Poses[sample_Poses.size(0) - 1] != 0.0); x_data[1] = (sample_Poses[(sample_Poses.size(0) + sample_Poses.size(0)) - 1] != 4000.0); x_data[2] = (sample_Poses[(sample_Poses.size(0) + sample_Poses.size(0) * 2) - 1] != -1000.0); b_y = false; k = 0; exitg1 = false; while ((!exitg1) && (k <= 2)) { if (x_data[k]) { b_y = true; exitg1 = true; } else { k++; } } if (b_y) { sample_Poses[sample_Poses.size(0)] = 0.0; sample_Poses[sample_Poses.size(0) + sample_Poses.size(0)] = 4000.0; sample_Poses[sample_Poses.size(0) + sample_Poses.size(0) * 2] = -1000.0; } n = sample_Poses.size(0); iobj_1.sumple_num = n; for (i = 0; i < n; i++) { sample_Poses[i + sample_Poses.size(0) * 3] = 0.0; } // 初始化第四列 for (i = 0; i < n; i++) { sample_Poses[i + sample_Poses.size(0) * 3] = 1.5707963267948966; } matlab代码: % 用直线构造目标航迹并采样航迹点位姿 function [sample_Poses, sample_Num, target_PathSeg] = Get_TargetPathSeg_Line(~, uavD3DConn, target_PathPose, maxSampleNum) % 说明: % 所有Get_TargetPathSeg*方法拥有相同的输入输出接口 % 输入: % 输出:% 输入获取 airSpeed = uavD3DConn.AirSpeed; pitchLimit_1 = uavD3DConn.FlightPathAngleLimit(1); % UAV俯仰角限制下界 pitchLimit_2 = uavD3DConn.FlightPathAngleLimit(2); % UAV俯仰角限制上界 startPos = target_PathPose(1, 1:3); goalPos = target_PathPose(2, 1:3); % 俯仰角计算 delta_XYZ = goalPos - startPos; % 未归一化的方向向量 delta_Z = delta_XYZ(3); % 高度差 pitch = atan(-delta_Z / norm(delta_XYZ(1:2))); % 负高度为正方向 % 待跟踪航迹俯仰角不能超过UAV性能限制 % validateattributes(pitch, 'double', {">=", pitchLimit_1, "<=", pitchLimit_2}); % 航向角计算 target_PathLength = norm(delta_XYZ); % 目标轨线长度 dir_vec = (delta_XYZ) / target_PathLength; % 方向向量 heading = CalConvergencePt.Get_NED_Heading(dir_vec(1:2)); % 航迹采样 sample_LenSeq = (0 : airSpeed : target_PathLength)'; % 目标航迹采样长度索引序列 sample_Num = length(sample_LenSeq); % 目标航迹采样数量 % 航迹采样点不能超过10个 if sample_Num > maxSampleNum sampleLenInterval = uint32(floor(sample_Num / maxSampleNum)); sample_LenSeq = sample_LenSeq(1 : sampleLenInterval : sample_Num); end sample_Poses = zeros(length(sample_LenSeq), 4); % 显式定义为四列矩阵 sample_Poses(:, 1:3) = startPos + dir_vec .* sample_LenSeq; % 前三列为位置线性采样 % sample_Poses = startPos + dir_vec .* sample_LenSeq; % 目标轨线位置线性采样 numRows = size(sample_Poses, 1); % 保证最后一个线性插值点为终点 if any(sample_Poses(numRows, 1:3) ~= goalPos) sample_Poses(numRows+1, 1:3) = goalPos; end sample_Poses(:, 4) = 0; % 初始化第四列 sample_Poses(:, 4) = heading; % 线性采样点加入姿态 sample_Num = size(sample_Poses, 1); % 线性采样点数量 target_PathSeg = [startPos, heading; goalPos, heading]; % 起止目标航迹点位姿 - Assistant
本文提供的C++代码是通过MATLAB Coder工具将MATLAB代码转换而来,两个代码实现了相同的功能,即用直线构造目标航迹并采样航迹点位姿。下面将对C++代码进行详细的解释,并对照MATLAB代码,说明每一步的具体实现过程。
1. 读取UAV的空速(AirSpeed)
C++代码:
d = b_uavD3DConn.AirSpeed;MATLAB代码:
airSpeed = uavD3DConn.AirSpeed;这里的
d对应于MATLAB代码中的airSpeed,表示无人机的空速。
2. 生成航迹采样长度序列
sample_LenSeqC++代码:
if (std::isnan(d)) { y.set_size(1, 1); y[0] = rtNaN; } else if ((d == 0.0) || (d < 0.0)) { y.set_size(1, 0); } else if (std::isinf(d)) { y.set_size(1, 1); y[0] = 0.0; } else if (std::floor(d) == d) { loop_ub = static_cast<int>(4000.0 / d); y.set_size(1, loop_ub + 1); for (i = 0; i <= loop_ub; i++) { y[i] = d * static_cast<double>(i); } } else { // 处理步长为非整数的情况 // ...(后续代码) }MATLAB代码:
sample_LenSeq = (0 : airSpeed : target_PathLength)';在MATLAB中,使用冒号操作符
:直接生成从0到目标路径长度target_PathLength,以airSpeed为步长的序列。由于C++语言中没有直接的冒号操作符,需要通过编程实现相同的功能。C++代码中,对
d(即airSpeed)进行了多种情况的判断:- NaN、零、负数、无穷大的情况,分别进行特殊处理。
- 整数步长的情况,直接计算序列的长度
loop_ub = static_cast<int>(4000.0 / d);,然后生成序列。 - 非整数步长的情况,进行精确的浮点数计算,避免累积误差。
这里的
4000.0对应于target_PathLength,即目标路径的长度。
3. 处理非整数步长的情况
当步长
d为非整数时,需要特别处理,以确保生成的序列与MATLAB中的结果一致。C++代码:
double apnd; ndbl = std::floor(4000.0 / d + 0.5); apnd = ndbl * d; if (d > 0.0) { cdiff = apnd - 4000.0; } else { cdiff = 4000.0 - apnd; } if (std::abs(cdiff) < 1.7763568394002505E-12) { ndbl++; apnd = 4000.0; } else if (cdiff > 0.0) { apnd = (ndbl - 1.0) * d; } else { ndbl++; } if (ndbl >= 0.0) { n = static_cast<int>(ndbl); } else { n = 0; } y.set_size(1, n); if (n > 0) { y[0] = 0.0; if (n > 1) { y[n - 1] = apnd; nm1d2 = (n - 1) / 2; for (k = 0; k <= nm1d2 - 2; k++) { cdiff = (static_cast<double>(k) + 1.0) * d; y[k + 1] = cdiff; y[(n - k) - 2] = apnd - cdiff; } if (nm1d2 << 1 == n - 1) { y[nm1d2] = apnd / 2.0; } else { cdiff = static_cast<double>(nm1d2) * d; y[nm1d2] = cdiff; y[nm1d2 + 1] = apnd - cdiff; } } }这部分代码的作用是精确地计算非整数步长情况下的采样序列,避免浮点数累积误差,与MATLAB行为保持一致。
4. 将生成的序列赋值给
sample_LenSeqC++代码:
loop_ub = y.size(1); sample_LenSeq.set_size(y.size(1)); for (i = 0; i < loop_ub; i++) { sample_LenSeq[i] = y[i]; }相当于MATLAB中的:
sample_LenSeq = y'; % 转置为列向量
5. 限制采样点数量
为防止采样点过多,代码限制了采样点的数量,确保不超过
maxSampleNum。C++代码:
if (sample_LenSeq.size(0) > maxSampleNum) { cdiff = std::floor(static_cast<double>(sample_LenSeq.size(0)) / maxSampleNum); unsigned int u; if (cdiff < 4294967296.0) { if (cdiff >= 0.0) { u = static_cast<unsigned int>(cdiff); } else { u = 0U; } } else { u = 4294967295U; } if ((u == 0U) || ((u > 0U) && (static_cast<unsigned int>(sample_LenSeq.size(0)) < 1U))) { i = 1; k = -1; } else { i = static_cast<int>(u); k = sample_LenSeq.size(0) - 1; } loop_ub = div_s32(k, i); n = loop_ub + 1; b_sample_LenSeq.set_size(loop_ub + 1); for (k = 0; k <= loop_ub; k++) { b_sample_LenSeq[k] = sample_LenSeq[i * k]; } sample_LenSeq.set_size(loop_ub + 1); for (i = 0; i < n; i++) { sample_LenSeq[i] = b_sample_LenSeq[i]; } }对应的MATLAB代码:
if sample_Num > maxSampleNum sampleLenInterval = uint32(floor(sample_Num / maxSampleNum)); sample_LenSeq = sample_LenSeq(1 : sampleLenInterval : sample_Num); end这里,C++代码通过计算采样间隔
u,重新采样sample_LenSeq,以控制采样点数量。
6. 初始化并计算
sample_PosesC++代码:
loop_ub = sample_LenSeq.size(0); sample_Poses.set_size(sample_LenSeq.size(0), 4); n = sample_LenSeq.size(0) << 2; for (i = 0; i < n; i++) { sample_Poses[i] = 0.0; } // 位置线性采样 n = (loop_ub / 2) << 1; nm1d2 = n - 2; for (i = 0; i < 3; i++) { for (k = 0; k <= nm1d2; k += 2) { __m128d r; r = _mm_loadu_pd(&sample_LenSeq[k]); _mm_storeu_pd( &sample_Poses[k + sample_Poses.size(0) * i], _mm_add_pd( _mm_mul_pd(_mm_set1_pd(static_cast<double>(dir_vec[i])), r), _mm_set1_pd(static_cast<double>(startPos[i])))); } for (k = n; k < loop_ub; k++) { sample_Poses[k + sample_Poses.size(0) * i] = static_cast<double>(dir_vec[i]) * sample_LenSeq[k] + static_cast<double>(startPos[i]); } }对应的MATLAB代码:
sample_Poses = zeros(length(sample_LenSeq), 4); % 显式定义为四列矩阵 sample_Poses(:, 1:3) = startPos + dir_vec .* sample_LenSeq; % 前三列为位置线性采样C++代码中,使用了SIMD指令(如
__m128d)对sample_Poses的前三列进行计算,以加快处理速度。
7. 确保最后一个采样点为终点
C++代码:
x_data[0] = (sample_Poses[sample_Poses.size(0) - 1] != 0.0); x_data[1] = (sample_Poses[(sample_Poses.size(0) + sample_Poses.size(0)) - 1] != 4000.0); x_data[2] = (sample_Poses[(sample_Poses.size(0) + sample_Poses.size(0) * 2) - 1] != -1000.0); b_y = false; k = 0; exitg1 = false; while ((!exitg1) && (k <= 2)) { if (x_data[k]) { b_y = true; exitg1 = true; } else { k++; } } if (b_y) { sample_Poses[sample_Poses.size(0)] = 0.0; sample_Poses[sample_Poses.size(0) + sample_Poses.size(0)] = 4000.0; sample_Poses[sample_Poses.size(0) + sample_Poses.size(0) * 2] = -1000.0; }对应的MATLAB代码:
numRows = size(sample_Poses, 1); if any(sample_Poses(numRows, 1:3) ~= goalPos) sample_Poses(numRows+1, 1:3) = goalPos; endC++代码通过检查
sample_Poses最后一行的前三个元素是否等于终点坐标(在此假设为(0, 4000, -1000)),如果不等,则在sample_Poses中追加终点坐标,确保最后一个采样点为终点。
8. 初始化并赋值第四列(航向角)
C++代码:
n = sample_Poses.size(0); for (i = 0; i < n; i++) { sample_Poses[i + sample_Poses.size(0) * 3] = 0.0; } // 初始化第四列 for (i = 0; i < n; i++) { sample_Poses[i + sample_Poses.size(0) * 3] = 1.5707963267948966; }对应的MATLAB代码:
sample_Poses(:, 4) = 0; % 初始化第四列 sample_Poses(:, 4) = heading; % 线性采样点加入姿态在C++代码中,使用了
1.5707963267948966,即 π/2,表示航向角为90度(弧度制)。在MATLAB代码中,heading是通过计算得到的航向角。
9. 总结
通过对比分析,可以看出C++代码严格按照MATLAB代码的逻辑进行了实现,主要包括以下步骤:
- 读取输入参数:如空速、起始和结束位置等。
- 生成采样长度序列:考虑了特殊情况,模拟了MATLAB的冒号操作符。
- 限制采样点数量:确保采样点数量不超过最大值。
- 计算采样点位姿:使用向量化计算方法,高效地计算采样点的位置和姿态。
- 确保采样点包含终点:检查最后一个采样点是否为终点,如果不是,则追加终点。
- 赋值航向角:初始化并设置每个采样点的航向角。
需要注意的是,由于语言特性的差异,MATLAB中的一些简洁操作在C++中需要更复杂的实现。此外,C++代码中使用了SIMD指令进行优化,提高了计算效率。
总体而言,C++代码实现了MATLAB代码的所有功能,对每一步骤都进行了详细的处理,确保在各种情况下都能得到与MATLAB相同的结果。