EEPW论坛

1、项目介绍：

设计一个高性能、高可靠性的机器人主控制器。利用Cortex-M7内核运行GUI和路径规划算法，提供人机交互界面；利用Cortex-M4内核运行实时运动控制算法（如步进电机控制、PID闭环控制）和通信协议，确保控制的精确性和实时性。

2、硬件介绍：

STM32H747I-DISCO探索套件

STM32H747I-DISCO Discovery套件是意法半导体STM32H747XIH6微控制器的完整演示和开发平台，旨在简化用户应用开发。

STM32H747XIH6是基于Arm®的微控制器，采用TFBGA240+25封装，具有2 Mbytes的闪存和1 Mbyte的RAM。

4英寸电容式触摸LCD显示模块，带MIPI® DSI接口，用于人机交互界面；

256 Mbit SDRAM 拓展内存

2片512-Mbit四路SPI接口的NOR闪存 拓展FLASH大小

带选择按钮的四向操纵杆，实现电机速度的调控

3、整体设计思路：

由于Cotrex-M7内核频率较高，性能很强，已经可以媲美Cortex-A7内核，十分适合运行计算密集型程序，故本项目中作为计算核心使用，而M4内核没有复杂的流水线和缓存，更适合高实时性的控制应用。故本项目的设计思路如下：

主处理器（Cortex-M7）:

负责非实时、计算密集型的上层应用。在本项目中，负责GUI的显示和路径规划算法的实现。

协处理器（Cortex-M4）:

负载高实时性的PID计算及电机控制。由于DISCO板上没有直接的CAN接口及PWM接口连接电机，所以这里使用函数模拟电机功能； M7内核软件流程图如下所示：

M4内核软件流程图如下所示：

4、效果展示：

开机后向串口发送模拟的路径规划路线，串口助手接收到数据如下图：

屏幕显示ST的logo及当前电机的设定速度：

可通过摇杆按键设定电机速度，同时可看到由M4内核控制的LED2灯闪烁：

5、关键代码展示：

其中最关键的代码就是路径规划算法,在固定轨道上，可以把轨道抽象成数据结构中的图，使用A*算法实现最短路径的查找：

// A*算法查找最短路径并生成TargetNode数组
 int get_shortest_path_astar(int start_node, int end_node, int *path, int max_path_length, TargetNode *target_nodes, int max_target_length)
 {
     int start_index = 0;
     int end_index = 0;
     if (find_node(start_node) == NULL)
     {
         //printf("错误: 起始节点 %d 不存在\n", start_node);
         return -1;
     }
 
     if (find_node(end_node) == NULL)
     {
         //printf("错误: 目标节点 %d 不存在\n", end_node);
         return -1;
     }
 
     if (start_node == end_node)
     {
         path[0] = start_node;
         return 1;
     }
 
     // 初始化A*节点数组（使用静态全局变量减少栈使用）
     for (int i = 0; i < rail_map.node_count; i++)
     {
         g_astar_nodes[i].node_id = rail_map.nodes[i].node_id;
         g_astar_nodes[i].g_cost = (rail_map.nodes[i].node_id == start_node) ? 0 : 999999;
         g_astar_nodes[i].h_cost = 0; // 不再计算启发式距离
         g_astar_nodes[i].f_cost = g_astar_nodes[i].g_cost + g_astar_nodes[i].h_cost;
         g_astar_nodes[i].visited = 0;
         g_astar_nodes[i].in_open_set = (rail_map.nodes[i].node_id == start_node) ? 1 : 0;
         g_astar_nodes[i].previous = -1;
         g_astar_nodes[i].path_type = STRAIGHT;
     }
 
     start_index = get_node_index(start_node);
     end_index = get_node_index(end_node);
 
     if (start_index == -1 || end_index == -1)
     {
         //printf("错误: 节点索引获取失败\n");
         return -1;
     }
 
     // A*算法主循环（简化为Dijkstra，因为h_cost为0）
     while (1)
     {
         // 找到开放集合中f_cost最小的节点
         int current_index = -1;
         int min_f_cost = 999999;
 
         for (int i = 0; i < rail_map.node_count; i++)
         {
             if (g_astar_nodes[i].in_open_set && !g_astar_nodes[i].visited &&
                 g_astar_nodes[i].f_cost < min_f_cost)
             {
                 min_f_cost = g_astar_nodes[i].f_cost;
                 current_index = i;
             }
         }
 
         if (current_index == -1)
         {
             // 开放集合为空，没有找到路径
             //printf("警告: 从节点 %d 到节点 %d 没有可达路径\n", start_node, end_node);
             return -1;
         }
 
         // 将当前节点从开放集合移到关闭集合
         g_astar_nodes[current_index].visited = 1;
         g_astar_nodes[current_index].in_open_set = 0;
 
         // 如果到达目标节点，停止搜索
         if (current_index == end_index)
         {
             break;
         }
 
         // 检查所有邻居节点
         MapNode *current_node = &rail_map.nodes[current_index];
         for (int i = 0; i < current_node->connection_count; i++)
         {
             Connection *conn = &current_node->connections[i];
             if (!conn->is_valid)
                 continue;
 
             int neighbor_index = get_node_index(conn->target_node_id);
             if (neighbor_index == -1 || g_astar_nodes[neighbor_index].visited)
                 continue;
 
             // 计算通过当前节点到邻居节点的g成本
             int tentative_g_cost = g_astar_nodes[current_index].g_cost + get_turn_weight(conn->type);
 
             // 如果邻居不在开放集合中，或者找到了更好的路径
             if (!g_astar_nodes[neighbor_index].in_open_set ||
                 tentative_g_cost < g_astar_nodes[neighbor_index].g_cost)
             {
 
                 g_astar_nodes[neighbor_index].g_cost = tentative_g_cost;
                 g_astar_nodes[neighbor_index].f_cost = tentative_g_cost + g_astar_nodes[neighbor_index].h_cost;
                 g_astar_nodes[neighbor_index].previous = current_node->node_id;
                 g_astar_nodes[neighbor_index].path_type = conn->type;
 
                 if (!g_astar_nodes[neighbor_index].in_open_set)
                 {
                     g_astar_nodes[neighbor_index].in_open_set = 1;
                 }
             }
         }
     }
 
     // 重构路径
     int path_length = 0;
     int current = end_node;
 
     // 从目标节点反向追踪到起始节点
     while (current != -1 && path_length < MAX_NODES)
     {
         g_temp_path[path_length++] = current;
 
         // 找到当前节点的前一个节点
         int current_idx = get_node_index(current);
         if (current_idx == -1)
             break;
 
         current = g_astar_nodes[current_idx].previous;
     }
 
     // 检查路径长度是否超出限制
     if (path_length > max_path_length)
     {
         //printf("警告: 路径长度 %d 超出最大限制 %d\n", path_length, max_path_length);
         return -1;
     }
 
     // 反转路径（因为是反向追踪的）
     for (int i = 0; i < path_length; i++)
     {
         path[i] = g_temp_path[path_length - 1 - i];
     }
 
     // 生成TargetNode数组
     if (target_nodes != NULL && max_target_length > 0)
     {
         int target_count = 0;
         
         // 遍历路径中的每一段，生成TargetNode
         for (int i = 0; i < path_length - 1 && target_count < max_target_length; i++)
         {
             MapNode *from_node = find_node(path[i]);
             MapNode *to_node = find_node(path[i + 1]);
 
             if (!from_node || !to_node)
             {
                 //printf("错误: 节点不存在 (%d -> %d)\n", path[i], path[i + 1]);
                 continue;
             }
 
             // 查找连接类型和速度
             ConnectionType conn_type = STRAIGHT;
             int conn_speed = 100; // 默认速度
             int found = 0;
 
             for (int j = 0; j < from_node->connection_count; j++)
             {
                 Connection *conn = &from_node->connections[j];
                 if (conn->is_valid && conn->target_node_id == path[i + 1])
                 {
                     conn_type = conn->type;
                     conn_speed = conn->speed;
                     found = 1;
                     break;
                 }
             }
 
             if (!found)
             {
                 //printf("警告: 未找到连接 (%d -> %d)\n", path[i], path[i + 1]);
             }
 
             // 创建目标节点
             target_nodes[target_count].type = connection_type_to_node_type(conn_type);
             target_nodes[target_count].speed = conn_speed; // 使用连接的速度
             target_nodes[target_count].id = path[i + 1]; // 目标节点ID
             target_count++;
         }
 
         // 添加停止指令
         if (target_count < max_target_length)
         {
             target_nodes[target_count].type = NODE_TYPE_IDLE;
             target_nodes[target_count].speed = 0;
             target_nodes[target_count].id = end_node; // 终点节点ID
             target_count++;
         }
 
         //printf("已生成 %d 个TargetNode指令 (路径长度: %d)\n", target_count, path_length);
         return target_count; // 返回生成的TargetNode数量
     }
 
     return path_length; // 如果没有传入TargetNode数组，返回路径长度
 }

6、心得体会：

ST推出的STM32H747在性能和实时性之间取得了很大的平衡，相比传统的MCU+MPU方案，其使用的异构双核架构，可以在单片MCU上实现高性能和高实时性，对于机器人而言降低了成本并提高了性能。但在开发过程中，异构架构的资料较少，开发方式也很复杂，且MDK对这样的程序支持也不是很好，希望未来有更适合于多核异构MCU的开发方式。